root/src/BRAINSFramework/Locomotion/LocomtionSteering.cs

Show revision log
 
113
1
//****************************************************************//
1
using System;
2
//********************copyright Rik Dodsworth 2009****************//



3
//****************************************************************//



4
//***************************free to use**************************//



5
//****************************************************************//



6
//*******************please just leave this header****************//



7
//*************************for recognition************************//



8
//****************************************************************//



9
//****************************************************************//



10
using System;



11
using System.Collections.Generic;
2
using System.Collections.Generic;



12
using System.Linq;
3
using System.Linq;



13
using System.Text;
4
using System.Text;



14
using Microsoft.Xna.Framework;
5
using Microsoft.Xna.Framework;



6
using Brains.Framework.Utility;



15
7



16
namespace Brains.Framework.Locomotion
8
namespace Brains.Framework.Locomotion



17
{
9
{



10
    /// <summary>



11
    /// This class is currently W.I.P



12
    /// </summary>



13
    /// <remarks></remarks>



18
    public class LocomtionSteering:LocomotionController
14
    public class LocomtionSteering:LocomotionController



19
    {
15
    {



16
        private static Random rand = new Random();



17
        private Vector2 _wanderTarget;



20
18



21
        Vector2 _steeringForce;
19
        public const float WanderRadius = 1.2f;



22
        private float ObstacleAvoidanceWeight=10;
20
        public const float WanderDistance = 2.0f;



23
        public float SeekWeight=1;
21
        public const float WanderJitter = 80;



22
         



23
        public enum Deceleration



24
        {



25
            Fast,



26
            Normal,



27
            Slow



28
        }



24
29



25
        public Vector2 SeekPos { get; set; }



26
        [Flags()]
30
        [Flags()]



27
        public enum BehaviorType
31
        public enum BehaviorType



28
        {
32
        {



29
            none = 0x00000,
33
            None = 0x00000,



30
            seek = 0x00002,
34
            Seek = 0x00002,



31
            flee = 0x00004,
35
            Flee= 0x00004,



32
            arrive = 0x00008,
36
            Arrive= 0x00008,



33
            wander = 0x00010,
37
            Wander = 0x00010,



34
            cohesion = 0x00020,
38
            //Cohesion= 0x00020,



35
            separation = 0x00040,
39
            //Seperation= 0x00040,



36
            allignment = 0x00080,
40
            //Alignment = 0x00080,



37
            obstacle_avoidance = 0x00100,
41
            //ObstacleAvoidance= 0x00100,



38
            wall_avoidance = 0x00200,
42
            //WallAvoidance = 0x00200,



39
            follow_path = 0x00400,
43
            //FollowPath = 0x00400,



40
            pursuit = 0x00800,
44
            Pursuit= 0x00800,



41
            evade = 0x01000,
45
            Evade = 0x01000,



42
            interpose = 0x02000,
46
            //Interpose= 0x02000,



43
            hide = 0x04000,
47
            //Hide= 0x04000,



44
            flock = 0x08000,
48
            //Flock = 0x08000,



45
            offset_pursuit = 0x10000,
49
            OffsetPursuit= 0x10000,



46
        }
50
        }



47
        Vector2 SteeringForce = Vector2.Zero;



48
        Vector2 Heading;



49
51



50
        float MaxForce=4;
52
        public void TurnAllOff()



51
        Vector2 Velocity;



52
        private BehaviorType _behaviorFlags;



53
        public LocomtionSteering()



54
        {
53
        {



55
            
54
            _behaviorFlags = BehaviorType.None;



56
        }
55
        }



57
        public void SeekOn(Vector2 target)
56
        



57
        public void TurnOnArrive(Vector2 targetPosition)



58
        {
58
        {



59
            TurnOn(BehaviorType.Arrive);



60
            SeekPos = targetPosition;



61
        }



62
        



63
        public void TurnOffArrive()



64
        {



65
            TurnOff(BehaviorType.Arrive);



66
        }



59
67



60
            if (!On(BehaviorType.seek))
68
        public void TurnOnSeek(Vector2 targetPosition)



61
                _behaviorFlags |= BehaviorType.seek;
69
        {



70
            SeekPos = targetPosition;



71
            TurnOn(BehaviorType.Seek);



72
        }



73
        



74
        public void TurnOffSeek()



75
        {



76
            TurnOff(BehaviorType.Seek);



77
        }



78
        



79
        public void TurnOnWander()



80
        {



81
            TurnOn(BehaviorType.Wander);



82
        }



83
        



84
        public void TurnOffWander()



85
        {



86
            TurnOff(BehaviorType.Wander);



87
        }



88
        



89
        public void TurnOnOffsetPursuit(Vector2 seek)



90
        {



62
91



63
            SeekPos= target;



64
        }
92
        }



93
        



94
        public void TurnOffOffsetPursuit()



95
        {



96



97
        }



98
        



99
        public void TurnOnPursuit(Vector2 seek)



100
        {



101



102
        }



103
        



104
        public void TurnOffPursuit()



105
        {



106



107
        }



108
        



109
        public void TurnOnFlee(Vector2 seek)



110
        {



111



112
        }



113
        



114
        public void TurnOffFlee()



115
        {



116



117
        }



118
      



119
        private Vector2 _steeringForce;



120
        private float ObstacleAvoidanceWeight=10;



121
        private Vector2 SteeringForce = Vector2.Zero;



122
        private Vector2 Heading;



123
        private float MaxForce = 4;



124
        private Vector2 Velocity;



125
        private BehaviorType _behaviorFlags;



126



127



128
        public float SeekWeight = 1;



129
        public float ArriveWeight= 1;



130
        public float WanderWeight= 1;



131



132
        public Vector2 SeekPos { get; set; }



133
        public Deceleration DecelerationSpeed { get; set; }



134
        public LocomtionSteering()



135
        {



136
            DecelerationSpeed = Deceleration.Normal;



137
        }



138
        float timeElapsed;



65
        public override void Update(Microsoft.Xna.Framework.GameTime gameTime)
139
        public override void Update(Microsoft.Xna.Framework.GameTime gameTime)



66
        {
140
        {



141
            timeElapsed = gameTime.GetElapsed();



67
            SteeringUpdate(gameTime.GetElapsed());
142
            SteeringUpdate(gameTime.GetElapsed());



68
        }
143
        }



144



69
        
145
        



70
        public void ObstacleAvoidanceOn()
146



71
        {



72
            _behaviorFlags |= BehaviorType.obstacle_avoidance;



73
        }



74
        private void SteeringUpdate(float elapsed)
147
        private void SteeringUpdate(float elapsed)



75
        {
148
        {



76
            if(!On(BehaviorType.seek))



77
                SeekOn(Owner.DesiredPosition);



78
            if (!On(BehaviorType.obstacle_avoidance))



79
                ObstacleAvoidanceOn();



80



81
            Vector2 OldPos = Owner.Position;
149
            Vector2 OldPos = Owner.Position;



82
            SeekPos = Owner.DesiredPosition;



83
            SteeringForce = Vector2.Zero;
150
            SteeringForce = Vector2.Zero;



84
151



85
            SteeringForce = Calculate();
152
            SteeringForce = Calculate();


...
...
94
            Owner.Position += Velocity * elapsed;
161
            Owner.Position += Velocity * elapsed;



95
            Owner.Orientation = Heading;// (float)Math.Atan2(Heading.Y, Heading.X);
162
            Owner.Orientation = Heading;// (float)Math.Atan2(Heading.Y, Heading.X);



96
        }
163
        }



97
        public void SetHeading(Vector2 new_heading)
164



165
        private void SetHeading(Vector2 new_heading)



98
        {
166
        {



99
            Heading= new_heading;
167
            Heading= new_heading;



100
            //Right = Perp(_heading);
168
            //Right = Perp(_heading);



101
        }
169
        }



102
        
170
        



103
        public Vector2 Truncate(Vector2 vec, float max)
171
        private Vector2 Truncate(Vector2 vec, float max)



104
        {
172
        {



105
            Vector2 __v = vec;
173
            Vector2 __v = vec;



106
            if (vec.Length() > max)
174
            if (vec.Length() > max)


...
...
114
        private Vector2 Calculate()
182
        private Vector2 Calculate()



115
        {
183
        {



116
            _steeringForce = Vector2.Zero;
184
            _steeringForce = Vector2.Zero;



117
            //if (isObstacleAvoidanceOn())
185
            



118
            //    CalculateNearbyObjects();



119



120
           



121
            _steeringForce = CalculatePrioritized();
186
            _steeringForce = CalculatePrioritized();



122
             
187
             



123
            return _steeringForce;
188
            return _steeringForce;



124
        }
189
        }



125
        private bool On(BehaviorType bt)
190



191
        private bool On(BehaviorType type)



126
        {
192
        {



127
            return (_behaviorFlags & bt) == bt;
193
            return (_behaviorFlags & type) == type;



128
        }
194
        }



195
        



196
        private void TurnOn(BehaviorType bt)



197
        {



198
            if(!On(bt))



199
                _behaviorFlags |= bt;



200
        }



201



202
        private void TurnOff(BehaviorType bt)



203
        {



204
            if(On(bt))



205
                _behaviorFlags ^= bt;



206
        }



207
        



129
        private Vector2 CalculatePrioritized()
208
        private Vector2 CalculatePrioritized()



130
        {
209
        {



131
210



132
            Vector2 force = Vector2.Zero;
211
            Vector2 force = Vector2.Zero;



133
212



134
            if (On(BehaviorType.obstacle_avoidance))
213
            if (On(BehaviorType.Seek))



135
            {
214
            {



136
                Vector2 __oa = ObstacleAvoidance();
215
                force = Seek(SeekPos);



137
                force = Vector2.Multiply(__oa, ObstacleAvoidanceWeight);
216
                force = Vector2.Multiply(force, SeekWeight);



217



138
                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
218
                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;



139
            }
219
            }



140
220



141
            if (On(BehaviorType.seek))
221



222



223
            if (On(BehaviorType.Arrive))



142
            {
224
            {



143
                force = Seek(SeekPos);
225
                force = Arrive(SeekPos, DecelerationSpeed) * ArriveWeight;



144
                force = Vector2.Multiply(force, SeekWeight);



145
226



146
                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
227
                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;



147
            }
228
            }



229
            



230
            if (On(BehaviorType.Wander))



231
            {



232
                force = Wander() * WanderWeight;



148
233



149
234
                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;



235
            }



150
         
236
         



151
            return _steeringForce;
237
            return _steeringForce;



152
        }
238
        }



153
239



154
        private Vector2 Seek(Vector2 seek)
240
        



155
        {
241
        



156
            Vector2 tpos = seek - Owner.Position;



157
            Vector2 DesiredVelocity = Vector2.Zero;



158
            if (tpos != Vector2.Zero)



159
                DesiredVelocity = Vector2.Normalize(seek- Owner.Position) * MaxSpeed;



160



161
            return (DesiredVelocity - Velocity);



162
        }



163
        private bool AccumulateForce(ref Vector2 sf, Vector2 ForceToAdd)
242
        private bool AccumulateForce(ref Vector2 sf, Vector2 ForceToAdd)



164
        {
243
        {



165
            //first calculate how much steering force we have left to use
244
            //first calculate how much steering force we have left to use


...
...
186
            return true;
265
            return true;



187
        }
266
        }



188
      
267
      



189



190
        private Vector2 ObstacleAvoidance()
268
        private Vector2 ObstacleAvoidance()



191
        {
269
        {



192
            Vector2 _steer=Vector2.Zero;
270
            Vector2 _steer=Vector2.Zero;



193
            foreach (var item in Owner.ParentWorld.Map.Cluster[0].Cells)
271
            foreach (var item in Owner.ParentWorld.Map.ClusterGrid.Grids[0].Cells)



194
            {
272
            {



195
                if (item.Type == 0)
273
                if (item.Type == 0)



196
                {
274
                {



197
                    //Check feeler 1
275
                    //Check feeler 1



198
                    Ray _ra = new Ray(Owner.Position.ToVector3(), Owner.Feelers[1].WorldDirection.ToVector3());
276
                    Ray _ra = new Ray(Owner.Position.ToVector3(), Owner.Feelers[1].WorldDirection.ToVector3());



199
                    Vector3 tl = (item.Position - new Vector2(Owner.ParentWorld.Map.Cluster[0].CellSize / 2)).ToVector3();
277
                    Vector3 tl = (item.Position - new Vector2(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize / 2)).ToVector3();



200
                    BoundingBox _box = new BoundingBox(
278
                    BoundingBox _box = new BoundingBox(



201
                        tl,
279
                        tl,



202
                        tl + new Vector3(Owner.ParentWorld.Map.Cluster[0].CellSize,
280
                        tl + new Vector3(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize,



203
                            Owner.ParentWorld.Map.Cluster[0].CellSize, 0));
281
                            Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize, 0));



204
                    float? inte = _ra.Intersects(_box);
282
                    float? inte = _ra.Intersects(_box);



205
                    if (inte.HasValue)
283
                    if (inte.HasValue)



206
                    {
284
                    {


...
...
214
                    //Check feeler2
292
                    //Check feeler2



215
293



216
                     _ra = new Ray(Owner.Position.ToVector3(), Owner.Feelers[2].WorldDirection.ToVector3());
294
                     _ra = new Ray(Owner.Position.ToVector3(), Owner.Feelers[2].WorldDirection.ToVector3());



217
                     tl = (item.Position - new Vector2(Owner.ParentWorld.Map.Cluster[0].CellSize / 2)).ToVector3();
295
                     tl = (item.Position - new Vector2(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize / 2)).ToVector3();



218
                     _box = new BoundingBox(
296
                     _box = new BoundingBox(



219
                        tl,
297
                        tl,



220
                        tl + new Vector3(Owner.ParentWorld.Map.Cluster[0].CellSize,
298
                        tl + new Vector3(Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize,



221
                            Owner.ParentWorld.Map.Cluster[0].CellSize, 0));
299
                            Owner.ParentWorld.Map.ClusterGrid.Grids[0].CellSize, 0));



222
                    inte = _ra.Intersects(_box);
300
                    inte = _ra.Intersects(_box);



223
                    if (inte.HasValue)
301
                    if (inte.HasValue)



224
                    {
302
                    {


...
...
232
310



233
            return _steer;
311
            return _steer;



234
        }
312
        }



313
       



314
        public Vector2 Wander()



315
        {



316
            double JitterThisTimeSlice = WanderJitter * timeElapsed;



317



318
            //first, add a small random vector to the target's position (RandomClamped



319
            //returns a value between -1 and 1)



320
            _wanderTarget+= new Vector2(RandomClamped() * WanderJitter,



321
                                  RandomClamped() * WanderJitter);



322



323
            if (_wanderTarget != Vector2.Zero) //reproject this new vector back on to a unit circle



324
                _wanderTarget.Normalize();



325



326
            //increase the length of the vector to the same as the radius



327
            //of the wander circle



328
            _wanderTarget *= WanderRadius;



329



330
            //move the target into a position WanderDist in front of the agent



331
            Vector2 _newtarget = _wanderTarget + new Vector2(WanderDistance, 0);



332



333
            //project the target into world space



334
            Vector2 _target = VectorUtil.PointToWorldSpace(_newtarget,



335
                                                 Owner.Orientation,



336
                                                 Owner.Position);



337



338
            return _target - Owner.Position;



339
            



340
        }



341



342
        public Vector2 Arrive(Vector2 targetPosition,Deceleration decelspeed)



343
        {



344
            Vector2 toTarget = targetPosition - Owner.Position;



345



346
            //calculate the distance to the target



347
            float dist = toTarget.Length();



348



349
            if (dist > 0)



350
            {



351
                //because Deceleration is enumerated as an int, this value is required



352
                //to provide fine tweaking of the deceleration..



353
                const float DecelerationTweaker = 0.3f;



354



355
                //calculate the speed required to reach the target given the desired



356
                //deceleration



357
                float speed = dist / ((float)decelspeed * DecelerationTweaker);



358



359
                //make sure the velocity does not exceed the max



360
                speed = Math.Min(speed, MaxSpeed);



361



362
                //from here proceed just like Seek except we don't need to normalize 



363
                //the ToTarget vector because we have already gone to the trouble



364
                //of calculating its length: dist. 



365
                Vector2 DesiredVelocity = toTarget * speed / dist;



366



367
                return (DesiredVelocity - Velocity);



368
            }



369



370
            return Vector2.Zero;



371
        }



372
        private Vector2 Seek(Vector2 seek)



373
        {



374
            Vector2 tpos = seek - Owner.Position;



375
            Vector2 DesiredVelocity = Vector2.Zero;



376
            if (tpos != Vector2.Zero)



377
                DesiredVelocity = Vector2.Normalize(seek - Owner.Position) * MaxSpeed;



378



379
            return (DesiredVelocity - Velocity);



380
        }



381



382
        float RandomClamped()



383
        {



384
            return rand.Next() - rand.Next();



385
        } 



235
    }
386
    }



236
}
387
}