Mercurial > projects > openmelee
annotate ai/steer.d @ 23:e79347dd38a3
steering
author | zzzzrrr <mason.green@gmail.com> |
---|---|
date | Thu, 26 Mar 2009 20:35:58 -0400 |
parents | 4fce5596d1f6 |
children | 441eb7672404 |
rev | line source |
---|---|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
1 /* |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
2 * Copyright (c) 2009, Mason Green (zzzzrrr) |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
3 * http://www.dsource.org/projects/openmelee |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
4 * Based on OpenSteer, Copyright (c) 2002-2003, Sony Computer Entertainment America |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
5 * Original author: Craig Reynolds |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
6 * |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
7 * All rights reserved. |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
8 * |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
9 * Redistribution and use in source and binary forms, with or without modification, |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
10 * are permitted provided that the following conditions are met: |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
11 * |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
12 * * Redistributions of source code must retain the above copyright notice, |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
13 * this list of conditions and the following disclaimer. |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
14 * * Redistributions in binary form must reproduce the above copyright notice, |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
15 * this list of conditions and the following disclaimer in the documentation |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
16 * and/or other materials provided with the distribution. |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
17 * * Neither the name of the polygonal nor the names of its contributors may be |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
18 * used to endorse or promote products derived from this software without specific |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
19 * prior written permission. |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
20 * |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
24 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
25 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
26 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
27 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
28 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
29 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
30 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
31 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
32 */ |
18 | 33 module openmelee.ai.steer; |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
34 |
19 | 35 import tango.io.Stdout : Stdout; |
36 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
37 import blaze.common.bzMath: bzDot, bzClamp, bzVec2; |
19 | 38 import blaze.collision.shapes.bzShape : bzShape; |
39 import tango.math.Math : sqrt; | |
15 | 40 import blaze.dynamics.bzBody: bzBody; |
41 | |
18 | 42 import openmelee.ships.ship : Ship, State; |
43 import openmelee.ai.utilities; | |
0 | 44 |
9 | 45 class Steer |
46 { | |
47 // Constructor: initializes state | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
48 this (Ship ship) |
0 | 49 { |
19 | 50 m_ship = ship; |
51 m_body = ship.rBody; | |
15 | 52 } |
53 | |
54 struct PathIntersection | |
55 { | |
56 bool intersect; | |
57 float distance; | |
58 bzVec2 surfacePoint; | |
59 bzVec2 surfaceNormal; | |
60 bzBody obstacle; | |
9 | 61 } |
0 | 62 |
9 | 63 // reset state |
64 void reset () { | |
65 // initial state of wander behavior | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
66 m_wanderSide = 0; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
67 m_wanderUp = 0; |
9 | 68 } |
69 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
70 void update() { |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
71 m_position = m_ship.state.position; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
72 m_velocity = m_ship.state.velocity; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
73 m_speed = m_ship.state.speed; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
74 m_maxForce = m_ship.state.maxForce; |
12 | 75 m_forward = m_ship.state.forward; |
22 | 76 m_radius = m_ship.state.radius; |
9 | 77 } |
0 | 78 |
9 | 79 // -------------------------------------------------- steering behaviors |
0 | 80 |
9 | 81 bzVec2 steerForWander (float dt) { |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
82 // random walk m_wanderSide and m_wanderUp between -1 and +1 |
9 | 83 float speed = 12 * dt; // maybe this (12) should be an argument? |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
84 m_wanderSide = scalarRandomWalk (m_wanderSide, speed, -1, +1); |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
85 m_wanderUp = scalarRandomWalk (m_wanderUp, speed, -1, +1); |
0 | 86 |
9 | 87 // return a pure lateral steering vector: (+/-Side) + (+/-Up) |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
88 return (m_side * m_wanderSide) + (m_up * m_wanderUp); |
9 | 89 } |
0 | 90 |
9 | 91 // Seek behavior |
92 bzVec2 steerForSeek (bzVec2 target) { | |
93 bzVec2 desiredVelocity = target - m_position; | |
94 return desiredVelocity - m_velocity; | |
95 } | |
0 | 96 |
9 | 97 // Flee behavior |
98 bzVec2 steerForFlee (bzVec2 target) { | |
99 bzVec2 desiredVelocity = m_position - target; | |
100 return desiredVelocity - m_velocity; | |
101 } | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
102 |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
103 /* |
9 | 104 // xxx proposed, experimental new seek/flee [cwr 9-16-02] |
105 bzVec2 xxxsteerForFlee (bzVec2 target) { | |
106 bzVec2 offset = m_position - target; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
107 bzVec2 desiredVelocity = bzClamp(offset.truncateLength (maxSpeed ()); |
9 | 108 return desiredVelocity - m_velocity; |
109 } | |
0 | 110 |
9 | 111 bzVec2 xxxsteerForSeek (bzVec2 target) { |
112 // bzVec2 offset = target - position; | |
113 bzVec2 offset = target - m_position; | |
114 bzVec2 desiredVelocity = offset.truncateLength (maxSpeed ()); //xxxnew | |
115 return desiredVelocity - m_velocity; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
116 } |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
117 */ |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
118 |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
119 /* |
9 | 120 // ------------------------------------------------------------------------ |
121 // Obstacle Avoidance behavior | |
122 // | |
123 // Returns a steering force to avoid a given obstacle. The purely | |
124 // lateral steering force will turn our vehicle towards a silhouette edge | |
125 // of the obstacle. Avoidance is required when (1) the obstacle | |
126 // intersects the vehicle's current path, (2) it is in front of the | |
127 // vehicle, and (3) is within minTimeToCollision seconds of travel at the | |
128 // vehicle's current velocity. Returns a zero vector value (bzVec2::zero) | |
129 // when no avoidance is required. | |
130 bzVec2 steerToAvoidObstacle (float minTimeToCollision, Obstacle obstacle) { | |
0 | 131 |
9 | 132 bzVec2 avoidance = obstacle.steerToAvoid (this, minTimeToCollision); |
133 return avoidance; | |
134 } | |
135 | |
15 | 136 // avoids all obstacles in an ObstacleGroup |
137 */ | |
138 | |
139 bzVec2 steerToAvoidObstacles (float minTimeToCollision, bzBody obstacles) { | |
0 | 140 |
9 | 141 bzVec2 avoidance; |
142 PathIntersection nearest, next; | |
22 | 143 float minDistanceToCollision = 10; //minTimeToCollision * m_speed; |
19 | 144 |
9 | 145 next.intersect = false; |
146 nearest.intersect = false; | |
0 | 147 |
9 | 148 // test all obstacles for intersection with my forward axis, |
149 // select the one whose point of intersection is nearest | |
19 | 150 for (bzBody o = obstacles; o; o = o.next) { |
151 | |
152 if(o is m_body) continue; | |
153 | |
15 | 154 // This code which presumes the obstacle is spherical |
22 | 155 findNextIntersectionWithSphere(o, next); |
0 | 156 |
15 | 157 if (!nearest.intersect || (next.intersect && next.distance < nearest.distance)) { |
158 nearest = next; | |
159 } | |
9 | 160 } |
19 | 161 |
9 | 162 // when a nearest intersection was found |
15 | 163 if (nearest.intersect && (nearest.distance < minDistanceToCollision)) { |
9 | 164 // compute avoidance steering force: take offset from obstacle to me, |
165 // take the component of that which is lateral (perpendicular to my | |
166 // forward direction), set length to maxForce, add a bit of forward | |
167 // component (in capture the flag, we never want to slow down) | |
15 | 168 bzVec2 offset = m_position - nearest.obstacle.position; |
19 | 169 avoidance = perpendicularComponent(offset, m_forward); |
15 | 170 avoidance.normalize; |
22 | 171 avoidance = m_body.localPoint(avoidance); |
172 ///avoidance *= m_maxForce; | |
19 | 173 //avoidance += m_forward * m_maxForce * 0.75f; |
0 | 174 } |
175 | |
22 | 176 auto state = cast(State) m_body.userData; |
177 state.avoid = avoidance; | |
9 | 178 return avoidance; |
179 } | |
21 | 180 |
23 | 181 bzVec2 getSteering(bzBody obstacles) { |
182 | |
183 // 1. Find the target that’s closest to collision | |
184 | |
185 float radius = m_radius; | |
186 float rad; | |
187 // Store the first collision time | |
188 float shortestTime = 0.5; | |
189 | |
190 // Store the target that collides then, and other data | |
191 // that we will need and can avoid recalculating | |
192 bzBody firstTarget = null; | |
193 float firstMinSeparation = 0; | |
194 float firstDistance = 0; | |
195 bzVec2 firstRelativePos; | |
196 bzVec2 firstRelativeVel; | |
197 bzVec2 relativePos; | |
198 | |
199 // Loop through each target | |
200 for (bzBody target = obstacles; target; target = target.next) { | |
201 | |
202 if(target is m_body) continue; | |
203 | |
204 // Calculate the time to collision | |
205 relativePos = m_body.localPoint(target.position); // - m_position; | |
206 bzVec2 relativeVel = target.linearVelocity - m_velocity; | |
207 float relativeSpeed = relativeVel.length; | |
208 float timeToCollision = bzDot(relativePos, relativeVel) / | |
209 (relativeSpeed * relativeSpeed); | |
210 | |
211 // Check if it is going to be a collision at all | |
212 float distance = relativePos.length; | |
213 float minSeparation = distance - relativeSpeed * shortestTime; | |
214 | |
215 float eRad; | |
216 for (bzShape s = target.shapeList; s; s = s.next) { | |
217 if(s.sweepRadius > eRad) { | |
218 eRad = s.sweepRadius; | |
219 } | |
220 } | |
221 | |
222 if (minSeparation > radius + eRad) continue; | |
223 | |
224 // Check if it is the shortest | |
225 if (timeToCollision > 0 && timeToCollision < shortestTime) { | |
226 | |
227 // Store the time, target and other data | |
228 shortestTime = timeToCollision; | |
229 firstTarget = target; | |
230 firstMinSeparation = minSeparation; | |
231 firstDistance = distance; | |
232 firstRelativePos = relativePos; | |
233 firstRelativeVel = relativeVel; | |
234 rad = eRad; | |
235 } | |
236 } | |
237 | |
238 // 2. Calculate the steering | |
239 | |
240 // If we have no target, then exit | |
241 if(!firstTarget) return bzVec2.zeroVect; | |
242 | |
243 Stdout(shortestTime).newline; | |
244 | |
245 // If we’re going to hit exactly, or if we’re already | |
246 // colliding, then do the steering based on current | |
247 // position. | |
248 if(firstMinSeparation <= 0 || firstDistance < radius + rad) { | |
249 relativePos = firstTarget.position - m_position; | |
250 } else { | |
251 // Otherwise calculate the future relative position: | |
252 relativePos = firstRelativePos + | |
253 firstRelativeVel * shortestTime; | |
254 } | |
255 | |
256 // Avoid the target | |
257 bzVec2 steering = relativePos; | |
258 steering.normalize(); | |
259 //steering.linear = relativePos * maxAcceleration | |
260 | |
261 return steering; | |
262 } | |
263 | |
264 bzVec2 avoid(float maxLookAhead, bzBody obstacles) { | |
21 | 265 |
266 float avoidMargin = 1.0f; | |
23 | 267 float maxLookahead = maxLookAhead * m_speed; |
21 | 268 |
269 // Make sure we're moving | |
270 if (m_velocity.length > 0) | |
271 { | |
272 for (bzBody o = obstacles; o; o = o.next) { | |
273 | |
274 if(o is m_body) continue; | |
275 | |
276 // Find the distance from the line we're moving along to the obstacle. | |
277 bzVec2 movementNormal = m_velocity; | |
278 movementNormal.normalize(); | |
279 bzVec2 characterToObstacle = o.position - m_position; | |
280 | |
281 real distanceSquared = bzDot(characterToObstacle, movementNormal); | |
22 | 282 distanceSquared = characterToObstacle.lengthSquared - distanceSquared*distanceSquared; |
21 | 283 |
284 // Check for collision | |
285 float oRad = 0; | |
22 | 286 for (bzShape s = o.shapeList; s; s = s.next) { |
287 if(s.sweepRadius > oRad) { | |
288 oRad = s.sweepRadius; | |
21 | 289 } |
290 } | |
291 | |
292 real radius = oRad + avoidMargin; | |
293 if (distanceSquared < radius*radius) | |
294 { | |
295 // Find how far along our movement vector the closest pass is | |
296 real distanceToClosest = bzDot(characterToObstacle, movementNormal); | |
297 | |
298 // Make sure this isn't behind us and is closer than our lookahead. | |
299 if (distanceToClosest > 0 && distanceToClosest < maxLookahead) | |
300 { | |
301 // Find the closest point | |
22 | 302 bzVec2 closestPoint = o.position + movementNormal * distanceToClosest; |
21 | 303 |
304 // Find the point of avoidance | |
22 | 305 bzVec2 target = o.position + (closestPoint - o.position).length * radius; |
21 | 306 |
23 | 307 target -= m_position; |
308 | |
22 | 309 auto state = cast(State) m_body.userData; |
23 | 310 state.avoid = m_body.localPoint(target); |
311 | |
21 | 312 return target; |
313 } | |
314 } | |
315 } | |
316 } | |
22 | 317 |
318 auto state = cast(State) m_body.userData; | |
319 state.avoid = bzVec2.zeroVect; | |
320 | |
21 | 321 return bzVec2.zeroVect; |
322 | |
323 } | |
15 | 324 |
19 | 325 void findNextIntersectionWithSphere(bzBody obs, |
22 | 326 inout PathIntersection intersection) { |
19 | 327 |
328 // This routine is based on the Paul Bourke's derivation in: | |
329 // Intersection of a Line and a Sphere (or circle) | |
22 | 330 // http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/raysphere.c |
331 | |
19 | 332 float b, c, d, p, q, s; |
333 bzVec2 lc; | |
334 | |
335 // initialize pathIntersection object | |
336 intersection.intersect = false; | |
337 intersection.obstacle = obs; | |
338 | |
22 | 339 // find "local center" (lc) of sphere in coordinate space |
19 | 340 lc = m_body.localPoint(obs.position); |
341 | |
342 // computer line-sphere intersection parameters | |
343 b = 0; | |
344 | |
345 // Find radius of obstacle | |
346 float obsRadius = 0; | |
347 for (bzShape shape = obs.shapeList; shape; shape = shape.next) { | |
348 if(shape.sweepRadius > obsRadius) { | |
349 obsRadius = shape.sweepRadius; | |
350 } | |
351 } | |
352 | |
22 | 353 c = square(lc.x) + square(lc.y) - square(obsRadius + m_radius); |
19 | 354 d = (b * b) - (4 * c); |
355 | |
356 // when the path does not intersect the sphere | |
357 if (d < 0) return; | |
358 | |
359 // otherwise, the path intersects the sphere in two points with | |
360 // parametric coordinates of "p" and "q". | |
361 // (If "d" is zero the two points are coincident, the path is tangent) | |
362 s = sqrt(d); | |
363 p = (-b + s) * 0.5f; | |
364 q = (-b - s) * 0.5f; | |
365 | |
366 // both intersections are behind us, so no potential collisions | |
367 if ((p < 0) && (q < 0)) return; | |
368 | |
369 // at least one intersection is in front of us | |
370 intersection.intersect = true; | |
371 intersection.distance = | |
372 ((p > 0) && (q > 0)) ? | |
373 // both intersections are in front of us, find nearest one | |
374 ((p < q) ? p : q) : | |
375 // otherwise only one intersections is in front, select it | |
376 ((p > 0) ? p : q); | |
377 } | |
378 | |
15 | 379 /* |
9 | 380 // ------------------------------------------------------------------------ |
381 // Unaligned collision avoidance behavior: avoid colliding with other | |
382 // nearby vehicles moving in unconstrained directions. Determine which | |
383 // (if any) other other vehicle we would collide with first, then steers | |
384 // to avoid the site of that potential collision. Returns a steering | |
385 // force vector, which is zero length if there is no impending collision. | |
386 | |
387 bzVec2 steerToAvoidNeighbors (float minTimeToCollision, AVGroup others) { | |
0 | 388 |
9 | 389 // first priority is to prevent immediate interpenetration |
390 bzVec2 separation = steerToAvoidCloseNeighbors (0, others); | |
391 if (separation != bzVec2::zero) return separation; | |
0 | 392 |
9 | 393 // otherwise, go on to consider potential future collisions |
394 float steer = 0; | |
15 | 395 Ship threat; |
9 | 396 |
397 // Time (in seconds) until the most immediate collision threat found | |
398 // so far. Initial value is a threshold: don't look more than this | |
399 // many frames into the future. | |
400 float minTime = minTimeToCollision; | |
0 | 401 |
9 | 402 // xxx solely for annotation |
403 bzVec2 xxxThreatPositionAtNearestApproach; | |
404 bzVec2 xxxOurPositionAtNearestApproach; | |
0 | 405 |
9 | 406 // for each of the other vehicles, determine which (if any) |
407 // pose the most immediate threat of collision. | |
408 for (AVIterator i = others.begin(); i != others.end(); i++) | |
409 { | |
15 | 410 Ship other = i; |
411 if (other !is this) | |
0 | 412 { |
9 | 413 // avoid when future positions are this close (or less) |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
414 float collisionDangerThreshold = radius * 2; |
9 | 415 |
416 // predicted time until nearest approach of "this" and "other" | |
417 float time = predictNearestApproachTime (other); | |
0 | 418 |
9 | 419 // If the time is in the future, sooner than any other |
420 // threatened collision... | |
421 if ((time >= 0) (time < minTime)) | |
422 { | |
423 // if the two will be close enough to collide, | |
424 // make a note of it | |
425 if (computeNearestApproachPositions (other, time) | |
426 < collisionDangerThreshold) | |
0 | 427 { |
9 | 428 minTime = time; |
429 threat = other; | |
430 xxxThreatPositionAtNearestApproach | |
431 = hisPositionAtNearestApproach; | |
432 xxxOurPositionAtNearestApproach | |
433 = ourPositionAtNearestApproach; | |
0 | 434 } |
435 } | |
436 } | |
9 | 437 } |
0 | 438 |
9 | 439 // if a potential collision was found, compute steering to avoid |
440 if (threat) | |
441 { | |
442 // parallel: +1, perpendicular: 0, anti-parallel: -1 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
443 float parallelness = m_forward.dot(threat.forward); |
9 | 444 float angle = 0.707f; |
445 | |
446 if (parallelness < -angle) | |
0 | 447 { |
9 | 448 // anti-parallel "head on" paths: |
449 // steer away from future threat position | |
450 bzVec2 offset = xxxThreatPositionAtNearestApproach - m_position; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
451 float sideDot = offset.dot(m_side()); |
9 | 452 steer = (sideDot > 0) ? -1.0f : 1.0f; |
453 } | |
454 else | |
455 { | |
456 if (parallelness > angle) | |
0 | 457 { |
9 | 458 // parallel paths: steer away from threat |
459 bzVec2 offset = threat.position - m_position; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
460 float sideDot = bzDot(offset, m_side); |
0 | 461 steer = (sideDot > 0) ? -1.0f : 1.0f; |
462 } | |
463 else | |
464 { | |
9 | 465 // perpendicular paths: steer behind threat |
466 // (only the slower of the two does this) | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
467 if (threat.speed() <= speed) |
0 | 468 { |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
469 float sideDot = bzDot(m_side, threat.velocity); |
0 | 470 steer = (sideDot > 0) ? -1.0f : 1.0f; |
471 } | |
472 } | |
473 } | |
474 } | |
475 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
476 return m_side() * steer; |
9 | 477 } |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
478 */ |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
479 |
9 | 480 // Given two vehicles, based on their current positions and velocities, |
481 // determine the time until nearest approach | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
482 float predictNearestApproachTime (State other) { |
9 | 483 |
484 // imagine we are at the origin with no velocity, | |
485 // compute the relative velocity of the other vehicle | |
486 bzVec2 myVelocity = m_velocity; | |
487 bzVec2 otherVelocity = other.velocity; | |
488 bzVec2 relVelocity = otherVelocity - myVelocity; | |
489 float relSpeed = relVelocity.length; | |
490 | |
491 // for parallel paths, the vehicles will always be at the same distance, | |
492 // so return 0 (aka "now") since "there is no time like the present" | |
493 if (relSpeed == 0) return 0; | |
494 | |
495 // Now consider the path of the other vehicle in this relative | |
496 // space, a line defined by the relative position and velocity. | |
497 // The distance from the origin (our vehicle) to that line is | |
498 // the nearest approach. | |
499 | |
500 // Take the unit tangent along the other vehicle's path | |
501 bzVec2 relTangent = relVelocity / relSpeed; | |
502 | |
503 // find distance from its path to origin (compute offset from | |
504 // other to us, find length of projection onto path) | |
505 bzVec2 relPosition = m_position - other.position; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
506 float projection = bzDot(relTangent, relPosition); |
9 | 507 |
508 return projection / relSpeed; | |
509 } | |
0 | 510 |
9 | 511 // Given the time until nearest approach (predictNearestApproachTime) |
512 // determine position of each vehicle at that time, and the distance | |
513 // between them | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
514 float computeNearestApproachPositions (State other, float time) { |
0 | 515 |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
516 bzVec2 myTravel = m_forward * m_speed * time; |
9 | 517 bzVec2 otherTravel = other.forward * other.speed * time; |
518 | |
519 bzVec2 myFinal = m_position + myTravel; | |
520 bzVec2 otherFinal = other.position + otherTravel; | |
0 | 521 |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
522 return (myFinal - otherFinal).length; |
9 | 523 } |
524 | |
525 // ------------------------------------------------------------------------ | |
526 // pursuit of another vehicle ( version with ceiling on prediction time) | |
527 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
528 bzVec2 steerForPursuit (State quarry) { |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
529 return steerForPursuit (quarry, float.max); |
9 | 530 } |
531 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
532 bzVec2 steerForPursuit (State quarry, float maxPredictionTime) { |
0 | 533 |
9 | 534 // offset from this to quarry, that distance, unit vector toward quarry |
535 bzVec2 offset = quarry.position - m_position; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
536 float distance = offset.length; |
9 | 537 bzVec2 unitOffset = offset / distance; |
538 | |
539 // how parallel are the paths of "this" and the quarry | |
540 // (1 means parallel, 0 is pependicular, -1 is anti-parallel) | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
541 float parallelness = bzDot(m_forward , quarry.forward); |
9 | 542 |
543 // how "forward" is the direction to the quarry | |
544 // (1 means dead ahead, 0 is directly to the side, -1 is straight back) | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
545 float forwardness = bzDot(m_forward , unitOffset); |
0 | 546 |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
547 float directTravelTime = distance / m_speed; |
9 | 548 int f = intervalComparison (forwardness, -0.707f, 0.707f); |
549 int p = intervalComparison (parallelness, -0.707f, 0.707f); | |
550 | |
551 float timeFactor = 0; // to be filled in below | |
0 | 552 |
9 | 553 // Break the pursuit into nine cases, the cross product of the |
554 // quarry being [ahead, aside, or behind] us and heading | |
555 // [parallel, perpendicular, or anti-parallel] to us. | |
556 switch (f) | |
557 { | |
558 case +1: | |
559 switch (p) | |
0 | 560 { |
9 | 561 case +1: // ahead, parallel |
562 timeFactor = 4; | |
0 | 563 break; |
9 | 564 case 0: // ahead, perpendicular |
565 timeFactor = 1.8f; | |
0 | 566 break; |
9 | 567 case -1: // ahead, anti-parallel |
568 timeFactor = 0.85f; | |
0 | 569 break; |
570 } | |
9 | 571 break; |
572 case 0: | |
573 switch (p) | |
574 { | |
575 case +1: // aside, parallel | |
576 timeFactor = 1; | |
577 break; | |
578 case 0: // aside, perpendicular | |
579 timeFactor = 0.8f; | |
580 break; | |
581 case -1: // aside, anti-parallel | |
582 timeFactor = 4; | |
583 break; | |
584 } | |
585 break; | |
586 case -1: | |
587 switch (p) | |
588 { | |
589 case +1: // behind, parallel | |
590 timeFactor = 0.5f; | |
591 break; | |
592 case 0: // behind, perpendicular | |
593 timeFactor = 2; | |
594 break; | |
595 case -1: // behind, anti-parallel | |
596 timeFactor = 2; | |
597 break; | |
598 } | |
599 break; | |
600 } | |
0 | 601 |
9 | 602 // estimated time until intercept of quarry |
603 float et = directTravelTime * timeFactor; | |
604 | |
605 // xxx experiment, if kept, this limit should be an argument | |
606 float etl = (et > maxPredictionTime) ? maxPredictionTime : et; | |
607 | |
608 // estimated position of quarry at intercept | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
609 bzVec2 target = quarry.predictFuturePosition(etl); |
0 | 610 |
12 | 611 return target; //steerForSeek (target); |
9 | 612 } |
613 | |
614 // ------------------------------------------------------------------------ | |
615 // evasion of another vehicle | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
616 bzVec2 steerForEvasion (State menace, float maxPredictionTime) { |
0 | 617 |
9 | 618 // offset from this to menace, that distance, unit vector toward menace |
619 bzVec2 offset = menace.position - m_position; | |
620 float distance = offset.length; | |
0 | 621 |
9 | 622 float roughTime = distance / menace.speed; |
623 float predictionTime = ((roughTime > maxPredictionTime) ? maxPredictionTime : roughTime); | |
624 bzVec2 target = menace.predictFuturePosition (predictionTime); | |
625 | |
626 return steerForFlee (target); | |
627 } | |
0 | 628 |
629 | |
9 | 630 // ------------------------------------------------------------------------ |
631 // tries to maintain a given speed, returns a maxForce-clipped steering | |
632 // force along the forward/backward axis | |
633 bzVec2 steerForTargetSpeed (float targetSpeed) { | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
634 float mf = m_maxForce; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
635 float speedError = targetSpeed - m_speed; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
636 return m_forward * bzClamp(speedError, -mf, +mf); |
9 | 637 } |
0 | 638 |
639 | |
9 | 640 // ----------------------------------------------------------- utilities |
641 bool isAhead (bzVec2 target) {return isAhead (target, 0.707f);}; | |
642 bool isAside (bzVec2 target) {return isAside (target, 0.707f);}; | |
643 bool isBehind (bzVec2 target) {return isBehind (target, -0.707f);}; | |
0 | 644 |
9 | 645 bool isAhead (bzVec2 target, float cosThreshold) |
646 { | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
647 bzVec2 targetDirection = target - m_position; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
648 targetDirection.normalize(); |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
649 return bzDot(m_forward, targetDirection) > cosThreshold; |
9 | 650 } |
0 | 651 |
9 | 652 bool isAside (bzVec2 target, float cosThreshold) |
653 { | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
654 bzVec2 targetDirection = target - m_position; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
655 targetDirection.normalize(); |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
656 float dp = bzDot(m_forward, targetDirection); |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
657 return (dp < cosThreshold) && (dp > -cosThreshold); |
9 | 658 } |
0 | 659 |
9 | 660 bool isBehind (bzVec2 target, float cosThreshold) |
661 { | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
662 bzVec2 targetDirection = target - m_position; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
663 targetDirection.normalize(); |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
664 return bzDot(m_forward, targetDirection) < cosThreshold; |
9 | 665 } |
666 | |
667 private: | |
668 | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
669 Ship m_ship; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
670 |
9 | 671 bzVec2 m_position; |
672 bzVec2 m_velocity; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
673 bzVec2 m_up; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
674 bzVec2 m_side; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
675 bzVec2 m_forward; |
19 | 676 float m_radius; |
677 bzBody m_body; | |
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
678 |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
679 float m_speed = 0; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
680 float m_maxForce = 0; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
681 |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
682 // Wander behavior |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
683 float m_wanderSide; |
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
684 float m_wanderUp; |
9 | 685 } |