|
0
|
1 // ----------------------------------------------------------------------------
|
|
|
2 //
|
|
|
3 //
|
|
|
4 // OpenSteer -- Steering Behaviors for Autonomous Characters
|
|
|
5 //
|
|
|
6 // Copyright (c) 2002-2003, Sony Computer Entertainment America
|
|
|
7 // Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>
|
|
|
8 //
|
|
|
9 // Permission is hereby granted, free of charge, to any person obtaining a
|
|
|
10 // copy of this software and associated documentation files (the "Software"),
|
|
|
11 // to deal in the Software without restriction, including without limitation
|
|
|
12 // the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
|
|
13 // and/or sell copies of the Software, and to permit persons to whom the
|
|
|
14 // Software is furnished to do so, subject to the following conditions:
|
|
|
15 //
|
|
|
16 // The above copyright notice and this permission notice shall be included in
|
|
|
17 // all copies or substantial portions of the Software.
|
|
|
18 //
|
|
|
19 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
|
20 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
|
21 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
|
|
22 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
|
23 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
|
24 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
|
|
|
25 // DEALINGS IN THE SOFTWARE.
|
|
|
26 //
|
|
|
27 //
|
|
|
28 // ----------------------------------------------------------------------------
|
|
|
29 //
|
|
|
30 //
|
|
|
31 // SteerLibraryMixin
|
|
|
32 //
|
|
|
33 // This mixin (class with templated superclass) adds the "steering library"
|
|
|
34 // functionality to a given base class. SteerLibraryMixin assumes its base
|
|
|
35 // class supports the Ship interface.
|
|
|
36 //
|
|
|
37 // 10-04-04 bk: put everything into the OpenSteer namespace
|
|
|
38 // 02-06-03 cwr: create mixin (from "SteerMass")
|
|
|
39 // 06-03-02 cwr: removed TS dependencies
|
|
|
40 // 11-21-01 cwr: created
|
|
|
41 //
|
|
|
42 //
|
|
|
43 // ----------------------------------------------------------------------------
|
|
2
|
44 module openmelee.steer;
|
|
0
|
45
|
|
9
|
46 class Steer
|
|
|
47 {
|
|
|
48 // Constructor: initializes state
|
|
|
49 this ()
|
|
0
|
50 {
|
|
9
|
51 // set inital state
|
|
|
52 reset ();
|
|
|
53 }
|
|
0
|
54
|
|
9
|
55 // reset state
|
|
|
56 void reset () {
|
|
|
57 // initial state of wander behavior
|
|
|
58 wanderSide = 0;
|
|
|
59 wanderUp = 0;
|
|
|
60 }
|
|
|
61
|
|
|
62 void update(bzBody rBody) {
|
|
|
63
|
|
|
64 m_position = rBody.position;
|
|
|
65 m_velocity = rBody.linearVelocity;
|
|
|
66 }
|
|
0
|
67
|
|
9
|
68 // -------------------------------------------------- steering behaviors
|
|
0
|
69
|
|
9
|
70 // Wander behavior
|
|
|
71 float wanderSide;
|
|
|
72 float wanderUp;
|
|
0
|
73
|
|
9
|
74 bzVec2 steerForWander (float dt) {
|
|
|
75 // random walk wanderSide and wanderUp between -1 and +1
|
|
|
76 float speed = 12 * dt; // maybe this (12) should be an argument?
|
|
|
77 wanderSide = scalarRandomWalk (wanderSide, speed, -1, +1);
|
|
|
78 wanderUp = scalarRandomWalk (wanderUp, speed, -1, +1);
|
|
0
|
79
|
|
9
|
80 // return a pure lateral steering vector: (+/-Side) + (+/-Up)
|
|
|
81 return (side() * wanderSide) + (up() * wanderUp);
|
|
|
82 }
|
|
0
|
83
|
|
9
|
84 // Seek behavior
|
|
|
85 bzVec2 steerForSeek (bzVec2 target) {
|
|
|
86 bzVec2 desiredVelocity = target - m_position;
|
|
|
87 return desiredVelocity - m_velocity;
|
|
|
88 }
|
|
0
|
89
|
|
9
|
90 // Flee behavior
|
|
|
91 bzVec2 steerForFlee (bzVec2 target) {
|
|
|
92 bzVec2 desiredVelocity = m_position - target;
|
|
|
93 return desiredVelocity - m_velocity;
|
|
|
94 }
|
|
0
|
95
|
|
9
|
96 // xxx proposed, experimental new seek/flee [cwr 9-16-02]
|
|
|
97 bzVec2 xxxsteerForFlee (bzVec2 target) {
|
|
|
98 bzVec2 offset = m_position - target;
|
|
|
99 bzVec2 desiredVelocity = offset.truncateLength (maxSpeed ());
|
|
|
100 return desiredVelocity - m_velocity;
|
|
|
101 }
|
|
0
|
102
|
|
9
|
103 bzVec2 xxxsteerForSeek (bzVec2 target) {
|
|
|
104 // bzVec2 offset = target - position;
|
|
|
105 bzVec2 offset = target - m_position;
|
|
|
106 bzVec2 desiredVelocity = offset.truncateLength (maxSpeed ()); //xxxnew
|
|
|
107 return desiredVelocity - m_velocity;
|
|
|
108 }
|
|
0
|
109
|
|
9
|
110 // ------------------------------------------------------------------------
|
|
|
111 // Obstacle Avoidance behavior
|
|
|
112 //
|
|
|
113 // Returns a steering force to avoid a given obstacle. The purely
|
|
|
114 // lateral steering force will turn our vehicle towards a silhouette edge
|
|
|
115 // of the obstacle. Avoidance is required when (1) the obstacle
|
|
|
116 // intersects the vehicle's current path, (2) it is in front of the
|
|
|
117 // vehicle, and (3) is within minTimeToCollision seconds of travel at the
|
|
|
118 // vehicle's current velocity. Returns a zero vector value (bzVec2::zero)
|
|
|
119 // when no avoidance is required.
|
|
|
120 bzVec2 steerToAvoidObstacle (float minTimeToCollision, Obstacle obstacle) {
|
|
0
|
121
|
|
9
|
122 bzVec2 avoidance = obstacle.steerToAvoid (this, minTimeToCollision);
|
|
|
123 return avoidance;
|
|
|
124 }
|
|
|
125
|
|
|
126 // avoids all obstacles in an ObstacleGroup
|
|
0
|
127
|
|
9
|
128 bzVec2 steerToAvoidObstacles (float minTimeToCollision,
|
|
|
129 ObstacleGroup obstacles) {
|
|
0
|
130
|
|
9
|
131 bzVec2 avoidance;
|
|
|
132 PathIntersection nearest, next;
|
|
|
133 float minDistanceToCollision = minTimeToCollision * speed();
|
|
0
|
134
|
|
9
|
135 next.intersect = false;
|
|
|
136 nearest.intersect = false;
|
|
0
|
137
|
|
9
|
138 // test all obstacles for intersection with my forward axis,
|
|
|
139 // select the one whose point of intersection is nearest
|
|
|
140 for (ObstacleIterator o = obstacles.begin(); o != obstacles.end(); o++)
|
|
|
141 {
|
|
|
142 // xxx this should be a generic call on Obstacle, rather than
|
|
|
143 // xxx this code which presumes the obstacle is spherical
|
|
|
144 findNextIntersectionWithSphere ((SphericalObstacle)**o, next);
|
|
0
|
145
|
|
9
|
146 if ((nearest.intersect == false) ||
|
|
|
147 ((next.intersect != false)
|
|
|
148 (next.distance < nearest.distance)))
|
|
|
149 nearest = next;
|
|
|
150 }
|
|
0
|
151
|
|
9
|
152 // when a nearest intersection was found
|
|
|
153 if ((nearest.intersect != false)
|
|
|
154 (nearest.distance < minDistanceToCollision))
|
|
|
155 {
|
|
|
156 // show the corridor that was checked for collisions
|
|
|
157 annotateAvoidObstacle (minDistanceToCollision);
|
|
0
|
158
|
|
9
|
159 // compute avoidance steering force: take offset from obstacle to me,
|
|
|
160 // take the component of that which is lateral (perpendicular to my
|
|
|
161 // forward direction), set length to maxForce, add a bit of forward
|
|
|
162 // component (in capture the flag, we never want to slow down)
|
|
|
163 bzVec2 offset = m_position - nearest.obstacle.center;
|
|
|
164 avoidance = offset.perpendicularComponent (forward());
|
|
|
165 avoidance = avoidance.normalize ();
|
|
|
166 avoidance *= maxForce ();
|
|
|
167 avoidance += forward() * maxForce () * 0.75;
|
|
0
|
168 }
|
|
|
169
|
|
9
|
170 return avoidance;
|
|
|
171 }
|
|
|
172
|
|
|
173 // ------------------------------------------------------------------------
|
|
|
174 // Unaligned collision avoidance behavior: avoid colliding with other
|
|
|
175 // nearby vehicles moving in unconstrained directions. Determine which
|
|
|
176 // (if any) other other vehicle we would collide with first, then steers
|
|
|
177 // to avoid the site of that potential collision. Returns a steering
|
|
|
178 // force vector, which is zero length if there is no impending collision.
|
|
|
179
|
|
|
180 bzVec2 steerToAvoidNeighbors (float minTimeToCollision, AVGroup others) {
|
|
0
|
181
|
|
9
|
182 // first priority is to prevent immediate interpenetration
|
|
|
183 bzVec2 separation = steerToAvoidCloseNeighbors (0, others);
|
|
|
184 if (separation != bzVec2::zero) return separation;
|
|
0
|
185
|
|
9
|
186 // otherwise, go on to consider potential future collisions
|
|
|
187 float steer = 0;
|
|
|
188 Ship* threat = NULL;
|
|
|
189
|
|
|
190 // Time (in seconds) until the most immediate collision threat found
|
|
|
191 // so far. Initial value is a threshold: don't look more than this
|
|
|
192 // many frames into the future.
|
|
|
193 float minTime = minTimeToCollision;
|
|
0
|
194
|
|
9
|
195 // xxx solely for annotation
|
|
|
196 bzVec2 xxxThreatPositionAtNearestApproach;
|
|
|
197 bzVec2 xxxOurPositionAtNearestApproach;
|
|
0
|
198
|
|
9
|
199 // for each of the other vehicles, determine which (if any)
|
|
|
200 // pose the most immediate threat of collision.
|
|
|
201 for (AVIterator i = others.begin(); i != others.end(); i++)
|
|
|
202 {
|
|
|
203 Ship other = **i;
|
|
|
204 if (other != this)
|
|
0
|
205 {
|
|
9
|
206 // avoid when future positions are this close (or less)
|
|
|
207 float collisionDangerThreshold = radius() * 2;
|
|
|
208
|
|
|
209 // predicted time until nearest approach of "this" and "other"
|
|
|
210 float time = predictNearestApproachTime (other);
|
|
0
|
211
|
|
9
|
212 // If the time is in the future, sooner than any other
|
|
|
213 // threatened collision...
|
|
|
214 if ((time >= 0) (time < minTime))
|
|
|
215 {
|
|
|
216 // if the two will be close enough to collide,
|
|
|
217 // make a note of it
|
|
|
218 if (computeNearestApproachPositions (other, time)
|
|
|
219 < collisionDangerThreshold)
|
|
0
|
220 {
|
|
9
|
221 minTime = time;
|
|
|
222 threat = other;
|
|
|
223 xxxThreatPositionAtNearestApproach
|
|
|
224 = hisPositionAtNearestApproach;
|
|
|
225 xxxOurPositionAtNearestApproach
|
|
|
226 = ourPositionAtNearestApproach;
|
|
0
|
227 }
|
|
|
228 }
|
|
|
229 }
|
|
9
|
230 }
|
|
0
|
231
|
|
9
|
232 // if a potential collision was found, compute steering to avoid
|
|
|
233 if (threat)
|
|
|
234 {
|
|
|
235 // parallel: +1, perpendicular: 0, anti-parallel: -1
|
|
|
236 float parallelness = forward.dot(threat.forward);
|
|
|
237 float angle = 0.707f;
|
|
|
238
|
|
|
239 if (parallelness < -angle)
|
|
0
|
240 {
|
|
9
|
241 // anti-parallel "head on" paths:
|
|
|
242 // steer away from future threat position
|
|
|
243 bzVec2 offset = xxxThreatPositionAtNearestApproach - m_position;
|
|
|
244 float sideDot = offset.dot(side());
|
|
|
245 steer = (sideDot > 0) ? -1.0f : 1.0f;
|
|
|
246 }
|
|
|
247 else
|
|
|
248 {
|
|
|
249 if (parallelness > angle)
|
|
0
|
250 {
|
|
9
|
251 // parallel paths: steer away from threat
|
|
|
252 bzVec2 offset = threat.position - m_position;
|
|
0
|
253 float sideDot = offset.dot(side());
|
|
|
254 steer = (sideDot > 0) ? -1.0f : 1.0f;
|
|
|
255 }
|
|
|
256 else
|
|
|
257 {
|
|
9
|
258 // perpendicular paths: steer behind threat
|
|
|
259 // (only the slower of the two does this)
|
|
|
260 if (threat.speed() <= speed())
|
|
0
|
261 {
|
|
9
|
262 float sideDot = side().dot(threat.velocity);
|
|
0
|
263 steer = (sideDot > 0) ? -1.0f : 1.0f;
|
|
|
264 }
|
|
|
265 }
|
|
|
266 }
|
|
|
267 }
|
|
|
268
|
|
9
|
269 return side() * steer;
|
|
|
270 }
|
|
0
|
271
|
|
9
|
272 // Given two vehicles, based on their current positions and velocities,
|
|
|
273 // determine the time until nearest approach
|
|
|
274 float predictNearestApproachTime (Ship other) {
|
|
|
275
|
|
|
276 // imagine we are at the origin with no velocity,
|
|
|
277 // compute the relative velocity of the other vehicle
|
|
|
278 bzVec2 myVelocity = m_velocity;
|
|
|
279 bzVec2 otherVelocity = other.velocity;
|
|
|
280 bzVec2 relVelocity = otherVelocity - myVelocity;
|
|
|
281 float relSpeed = relVelocity.length;
|
|
|
282
|
|
|
283 // for parallel paths, the vehicles will always be at the same distance,
|
|
|
284 // so return 0 (aka "now") since "there is no time like the present"
|
|
|
285 if (relSpeed == 0) return 0;
|
|
|
286
|
|
|
287 // Now consider the path of the other vehicle in this relative
|
|
|
288 // space, a line defined by the relative position and velocity.
|
|
|
289 // The distance from the origin (our vehicle) to that line is
|
|
|
290 // the nearest approach.
|
|
|
291
|
|
|
292 // Take the unit tangent along the other vehicle's path
|
|
|
293 bzVec2 relTangent = relVelocity / relSpeed;
|
|
|
294
|
|
|
295 // find distance from its path to origin (compute offset from
|
|
|
296 // other to us, find length of projection onto path)
|
|
|
297 bzVec2 relPosition = m_position - other.position;
|
|
|
298 float projection = relTangent.dot(relPosition);
|
|
|
299
|
|
|
300 return projection / relSpeed;
|
|
|
301 }
|
|
0
|
302
|
|
9
|
303 // Given the time until nearest approach (predictNearestApproachTime)
|
|
|
304 // determine position of each vehicle at that time, and the distance
|
|
|
305 // between them
|
|
|
306 float computeNearestApproachPositions (Ship other, float time) {
|
|
0
|
307
|
|
9
|
308 bzVec2 myTravel = forward * speed * time;
|
|
|
309 bzVec2 otherTravel = other.forward * other.speed * time;
|
|
|
310
|
|
|
311 bzVec2 myFinal = m_position + myTravel;
|
|
|
312 bzVec2 otherFinal = other.position + otherTravel;
|
|
0
|
313
|
|
9
|
314 // xxx for annotation
|
|
|
315 ourPositionAtNearestApproach = myFinal;
|
|
|
316 hisPositionAtNearestApproach = otherFinal;
|
|
|
317
|
|
|
318 return bzVec2::distance (myFinal, otherFinal);
|
|
|
319 }
|
|
0
|
320
|
|
9
|
321 // otherwise return zero
|
|
|
322 return bzVec2.zeroVect;
|
|
|
323 }
|
|
|
324
|
|
|
325 // ------------------------------------------------------------------------
|
|
|
326 // pursuit of another vehicle ( version with ceiling on prediction time)
|
|
|
327
|
|
|
328 bzVec2 steerForPursuit (Ship quarry) {
|
|
|
329 return steerForPursuit (quarry, FLT_MAX);
|
|
|
330 }
|
|
|
331
|
|
|
332 bzVec2 steerForPursuit (Ship quarry, float maxPredictionTime) {
|
|
0
|
333
|
|
9
|
334 // offset from this to quarry, that distance, unit vector toward quarry
|
|
|
335 bzVec2 offset = quarry.position - m_position;
|
|
|
336 float distance = offset.length ();
|
|
|
337 bzVec2 unitOffset = offset / distance;
|
|
|
338
|
|
|
339 // how parallel are the paths of "this" and the quarry
|
|
|
340 // (1 means parallel, 0 is pependicular, -1 is anti-parallel)
|
|
|
341 float parallelness = forward.dot(quarry.forward());
|
|
|
342
|
|
|
343 // how "forward" is the direction to the quarry
|
|
|
344 // (1 means dead ahead, 0 is directly to the side, -1 is straight back)
|
|
|
345 float forwardness = forward.dot(unitOffset);
|
|
0
|
346
|
|
9
|
347 float directTravelTime = distance / speed;
|
|
|
348 int f = intervalComparison (forwardness, -0.707f, 0.707f);
|
|
|
349 int p = intervalComparison (parallelness, -0.707f, 0.707f);
|
|
|
350
|
|
|
351 float timeFactor = 0; // to be filled in below
|
|
0
|
352
|
|
9
|
353 // Break the pursuit into nine cases, the cross product of the
|
|
|
354 // quarry being [ahead, aside, or behind] us and heading
|
|
|
355 // [parallel, perpendicular, or anti-parallel] to us.
|
|
|
356 switch (f)
|
|
|
357 {
|
|
|
358 case +1:
|
|
|
359 switch (p)
|
|
0
|
360 {
|
|
9
|
361 case +1: // ahead, parallel
|
|
|
362 timeFactor = 4;
|
|
|
363 color = gBlack;
|
|
0
|
364 break;
|
|
9
|
365 case 0: // ahead, perpendicular
|
|
|
366 timeFactor = 1.8f;
|
|
0
|
367 break;
|
|
9
|
368 case -1: // ahead, anti-parallel
|
|
|
369 timeFactor = 0.85f;
|
|
0
|
370 break;
|
|
|
371 }
|
|
9
|
372 break;
|
|
|
373 case 0:
|
|
|
374 switch (p)
|
|
|
375 {
|
|
|
376 case +1: // aside, parallel
|
|
|
377 timeFactor = 1;
|
|
|
378 break;
|
|
|
379 case 0: // aside, perpendicular
|
|
|
380 timeFactor = 0.8f;
|
|
|
381 break;
|
|
|
382 case -1: // aside, anti-parallel
|
|
|
383 timeFactor = 4;
|
|
|
384 break;
|
|
|
385 }
|
|
|
386 break;
|
|
|
387 case -1:
|
|
|
388 switch (p)
|
|
|
389 {
|
|
|
390 case +1: // behind, parallel
|
|
|
391 timeFactor = 0.5f;
|
|
|
392 break;
|
|
|
393 case 0: // behind, perpendicular
|
|
|
394 timeFactor = 2;
|
|
|
395 break;
|
|
|
396 case -1: // behind, anti-parallel
|
|
|
397 timeFactor = 2;
|
|
|
398 break;
|
|
|
399 }
|
|
|
400 break;
|
|
|
401 }
|
|
0
|
402
|
|
9
|
403 // estimated time until intercept of quarry
|
|
|
404 float et = directTravelTime * timeFactor;
|
|
|
405
|
|
|
406 // xxx experiment, if kept, this limit should be an argument
|
|
|
407 float etl = (et > maxPredictionTime) ? maxPredictionTime : et;
|
|
|
408
|
|
|
409 // estimated position of quarry at intercept
|
|
|
410 bzVec2 target = quarry.predictFuturePosition (etl);
|
|
0
|
411
|
|
9
|
412 return steerForSeek (target);
|
|
|
413 }
|
|
|
414
|
|
|
415 // ------------------------------------------------------------------------
|
|
|
416 // evasion of another vehicle
|
|
|
417 bzVec2 steerForEvasion (Ship menace, float maxPredictionTime) {
|
|
0
|
418
|
|
9
|
419 // offset from this to menace, that distance, unit vector toward menace
|
|
|
420 bzVec2 offset = menace.position - m_position;
|
|
|
421 float distance = offset.length;
|
|
0
|
422
|
|
9
|
423 float roughTime = distance / menace.speed;
|
|
|
424 float predictionTime = ((roughTime > maxPredictionTime) ? maxPredictionTime : roughTime);
|
|
|
425 bzVec2 target = menace.predictFuturePosition (predictionTime);
|
|
|
426
|
|
|
427 return steerForFlee (target);
|
|
|
428 }
|
|
0
|
429
|
|
|
430
|
|
9
|
431 // ------------------------------------------------------------------------
|
|
|
432 // tries to maintain a given speed, returns a maxForce-clipped steering
|
|
|
433 // force along the forward/backward axis
|
|
|
434 bzVec2 steerForTargetSpeed (float targetSpeed) {
|
|
|
435 float mf = maxForce();
|
|
|
436 float speedError = targetSpeed - speed ();
|
|
|
437 return forward () * clip (speedError, -mf, +mf);
|
|
|
438 }
|
|
0
|
439
|
|
|
440
|
|
9
|
441 // ----------------------------------------------------------- utilities
|
|
|
442 bool isAhead (bzVec2 target) {return isAhead (target, 0.707f);};
|
|
|
443 bool isAside (bzVec2 target) {return isAside (target, 0.707f);};
|
|
|
444 bool isBehind (bzVec2 target) {return isBehind (target, -0.707f);};
|
|
0
|
445
|
|
9
|
446 bool isAhead (bzVec2 target, float cosThreshold)
|
|
|
447 {
|
|
|
448 bzVec2 targetDirection = (target - m_position ()).normalize ();
|
|
|
449 return forward().dot(targetDirection) > cosThreshold;
|
|
|
450 }
|
|
0
|
451
|
|
9
|
452 bool isAside (bzVec2 target, float cosThreshold)
|
|
|
453 {
|
|
|
454 bzVec2 targetDirection = (target - m_position ()).normalize ();
|
|
|
455 float dp = forward().dot(targetDirection);
|
|
|
456 return (dp < cosThreshold) (dp > -cosThreshold);
|
|
|
457 }
|
|
0
|
458
|
|
9
|
459 bool isBehind (bzVec2 target, float cosThreshold)
|
|
|
460 {
|
|
|
461 bzVec2 targetDirection = (target - m_position).normalize ();
|
|
|
462 return forward().dot(targetDirection) < cosThreshold;
|
|
|
463 }
|
|
|
464
|
|
|
465 private:
|
|
|
466
|
|
|
467 bzVec2 m_position;
|
|
|
468 bzVec2 m_velocity;
|
|
|
469
|
|
|
470 }
|
