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