Mercurial > projects > openmelee
annotate ai/steer.d @ 22:4fce5596d1f6
ai work
| author | zzzzrrr <mason.green@gmail.com> |
|---|---|
| date | Thu, 26 Mar 2009 16:56:30 -0400 |
| parents | cad384ad349e |
| children | e79347dd38a3 |
| 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 |
| 181 bzVec2 avoid(float minTimeToCollision, bzBody obstacles) { | |
| 182 | |
| 183 float avoidMargin = 1.0f; | |
| 184 float maxLookahead = minTimeToCollision * m_speed; | |
| 185 | |
| 186 // Make sure we're moving | |
| 187 if (m_velocity.length > 0) | |
| 188 { | |
| 189 for (bzBody o = obstacles; o; o = o.next) { | |
| 190 | |
| 191 if(o is m_body) continue; | |
| 192 | |
| 193 // Find the distance from the line we're moving along to the obstacle. | |
| 194 bzVec2 movementNormal = m_velocity; | |
| 195 movementNormal.normalize(); | |
| 196 bzVec2 characterToObstacle = o.position - m_position; | |
| 197 | |
| 198 real distanceSquared = bzDot(characterToObstacle, movementNormal); | |
| 22 | 199 distanceSquared = characterToObstacle.lengthSquared - distanceSquared*distanceSquared; |
| 21 | 200 |
| 201 // Check for collision | |
| 202 float oRad = 0; | |
| 22 | 203 for (bzShape s = o.shapeList; s; s = s.next) { |
| 204 if(s.sweepRadius > oRad) { | |
| 205 oRad = s.sweepRadius; | |
| 21 | 206 } |
| 207 } | |
| 208 | |
| 209 real radius = oRad + avoidMargin; | |
| 210 if (distanceSquared < radius*radius) | |
| 211 { | |
| 212 // Find how far along our movement vector the closest pass is | |
| 213 real distanceToClosest = bzDot(characterToObstacle, movementNormal); | |
| 214 | |
| 215 // Make sure this isn't behind us and is closer than our lookahead. | |
| 216 if (distanceToClosest > 0 && distanceToClosest < maxLookahead) | |
| 217 { | |
| 218 // Find the closest point | |
| 22 | 219 bzVec2 closestPoint = o.position + movementNormal * distanceToClosest; |
| 21 | 220 |
| 221 // Find the point of avoidance | |
| 22 | 222 bzVec2 target = o.position + (closestPoint - o.position).length * radius; |
| 21 | 223 |
| 22 | 224 auto state = cast(State) m_body.userData; |
| 225 state.avoid = target; | |
| 226 | |
| 21 | 227 return target; |
| 228 } | |
| 229 } | |
| 230 } | |
| 231 } | |
| 22 | 232 |
| 233 auto state = cast(State) m_body.userData; | |
| 234 state.avoid = bzVec2.zeroVect; | |
| 235 | |
| 21 | 236 return bzVec2.zeroVect; |
| 237 | |
| 238 } | |
| 15 | 239 |
| 19 | 240 void findNextIntersectionWithSphere(bzBody obs, |
| 22 | 241 inout PathIntersection intersection) { |
| 19 | 242 |
| 243 // This routine is based on the Paul Bourke's derivation in: | |
| 244 // Intersection of a Line and a Sphere (or circle) | |
| 22 | 245 // http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/raysphere.c |
| 246 | |
| 19 | 247 float b, c, d, p, q, s; |
| 248 bzVec2 lc; | |
| 249 | |
| 250 // initialize pathIntersection object | |
| 251 intersection.intersect = false; | |
| 252 intersection.obstacle = obs; | |
| 253 | |
| 22 | 254 // find "local center" (lc) of sphere in coordinate space |
| 19 | 255 lc = m_body.localPoint(obs.position); |
| 256 | |
| 257 // computer line-sphere intersection parameters | |
| 258 b = 0; | |
| 259 | |
| 260 // Find radius of obstacle | |
| 261 float obsRadius = 0; | |
| 262 for (bzShape shape = obs.shapeList; shape; shape = shape.next) { | |
| 263 if(shape.sweepRadius > obsRadius) { | |
| 264 obsRadius = shape.sweepRadius; | |
| 265 } | |
| 266 } | |
| 267 | |
| 22 | 268 c = square(lc.x) + square(lc.y) - square(obsRadius + m_radius); |
| 19 | 269 d = (b * b) - (4 * c); |
| 270 | |
| 271 // when the path does not intersect the sphere | |
| 272 if (d < 0) return; | |
| 273 | |
| 274 // otherwise, the path intersects the sphere in two points with | |
| 275 // parametric coordinates of "p" and "q". | |
| 276 // (If "d" is zero the two points are coincident, the path is tangent) | |
| 277 s = sqrt(d); | |
| 278 p = (-b + s) * 0.5f; | |
| 279 q = (-b - s) * 0.5f; | |
| 280 | |
| 281 // both intersections are behind us, so no potential collisions | |
| 282 if ((p < 0) && (q < 0)) return; | |
| 283 | |
| 284 // at least one intersection is in front of us | |
| 285 intersection.intersect = true; | |
| 286 intersection.distance = | |
| 287 ((p > 0) && (q > 0)) ? | |
| 288 // both intersections are in front of us, find nearest one | |
| 289 ((p < q) ? p : q) : | |
| 290 // otherwise only one intersections is in front, select it | |
| 291 ((p > 0) ? p : q); | |
| 292 } | |
| 293 | |
| 15 | 294 /* |
| 9 | 295 // ------------------------------------------------------------------------ |
| 296 // Unaligned collision avoidance behavior: avoid colliding with other | |
| 297 // nearby vehicles moving in unconstrained directions. Determine which | |
| 298 // (if any) other other vehicle we would collide with first, then steers | |
| 299 // to avoid the site of that potential collision. Returns a steering | |
| 300 // force vector, which is zero length if there is no impending collision. | |
| 301 | |
| 302 bzVec2 steerToAvoidNeighbors (float minTimeToCollision, AVGroup others) { | |
| 0 | 303 |
| 9 | 304 // first priority is to prevent immediate interpenetration |
| 305 bzVec2 separation = steerToAvoidCloseNeighbors (0, others); | |
| 306 if (separation != bzVec2::zero) return separation; | |
| 0 | 307 |
| 9 | 308 // otherwise, go on to consider potential future collisions |
| 309 float steer = 0; | |
| 15 | 310 Ship threat; |
| 9 | 311 |
| 312 // Time (in seconds) until the most immediate collision threat found | |
| 313 // so far. Initial value is a threshold: don't look more than this | |
| 314 // many frames into the future. | |
| 315 float minTime = minTimeToCollision; | |
| 0 | 316 |
| 9 | 317 // xxx solely for annotation |
| 318 bzVec2 xxxThreatPositionAtNearestApproach; | |
| 319 bzVec2 xxxOurPositionAtNearestApproach; | |
| 0 | 320 |
| 9 | 321 // for each of the other vehicles, determine which (if any) |
| 322 // pose the most immediate threat of collision. | |
| 323 for (AVIterator i = others.begin(); i != others.end(); i++) | |
| 324 { | |
| 15 | 325 Ship other = i; |
| 326 if (other !is this) | |
| 0 | 327 { |
| 9 | 328 // 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
|
329 float collisionDangerThreshold = radius * 2; |
| 9 | 330 |
| 331 // predicted time until nearest approach of "this" and "other" | |
| 332 float time = predictNearestApproachTime (other); | |
| 0 | 333 |
| 9 | 334 // If the time is in the future, sooner than any other |
| 335 // threatened collision... | |
| 336 if ((time >= 0) (time < minTime)) | |
| 337 { | |
| 338 // if the two will be close enough to collide, | |
| 339 // make a note of it | |
| 340 if (computeNearestApproachPositions (other, time) | |
| 341 < collisionDangerThreshold) | |
| 0 | 342 { |
| 9 | 343 minTime = time; |
| 344 threat = other; | |
| 345 xxxThreatPositionAtNearestApproach | |
| 346 = hisPositionAtNearestApproach; | |
| 347 xxxOurPositionAtNearestApproach | |
| 348 = ourPositionAtNearestApproach; | |
| 0 | 349 } |
| 350 } | |
| 351 } | |
| 9 | 352 } |
| 0 | 353 |
| 9 | 354 // if a potential collision was found, compute steering to avoid |
| 355 if (threat) | |
| 356 { | |
| 357 // 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
|
358 float parallelness = m_forward.dot(threat.forward); |
| 9 | 359 float angle = 0.707f; |
| 360 | |
| 361 if (parallelness < -angle) | |
| 0 | 362 { |
| 9 | 363 // anti-parallel "head on" paths: |
| 364 // steer away from future threat position | |
| 365 bzVec2 offset = xxxThreatPositionAtNearestApproach - m_position; | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
366 float sideDot = offset.dot(m_side()); |
| 9 | 367 steer = (sideDot > 0) ? -1.0f : 1.0f; |
| 368 } | |
| 369 else | |
| 370 { | |
| 371 if (parallelness > angle) | |
| 0 | 372 { |
| 9 | 373 // parallel paths: steer away from threat |
| 374 bzVec2 offset = threat.position - m_position; | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
375 float sideDot = bzDot(offset, m_side); |
| 0 | 376 steer = (sideDot > 0) ? -1.0f : 1.0f; |
| 377 } | |
| 378 else | |
| 379 { | |
| 9 | 380 // perpendicular paths: steer behind threat |
| 381 // (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
|
382 if (threat.speed() <= speed) |
| 0 | 383 { |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
384 float sideDot = bzDot(m_side, threat.velocity); |
| 0 | 385 steer = (sideDot > 0) ? -1.0f : 1.0f; |
| 386 } | |
| 387 } | |
| 388 } | |
| 389 } | |
| 390 | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
391 return m_side() * steer; |
| 9 | 392 } |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
393 */ |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
394 |
| 9 | 395 // Given two vehicles, based on their current positions and velocities, |
| 396 // determine the time until nearest approach | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
397 float predictNearestApproachTime (State other) { |
| 9 | 398 |
| 399 // imagine we are at the origin with no velocity, | |
| 400 // compute the relative velocity of the other vehicle | |
| 401 bzVec2 myVelocity = m_velocity; | |
| 402 bzVec2 otherVelocity = other.velocity; | |
| 403 bzVec2 relVelocity = otherVelocity - myVelocity; | |
| 404 float relSpeed = relVelocity.length; | |
| 405 | |
| 406 // for parallel paths, the vehicles will always be at the same distance, | |
| 407 // so return 0 (aka "now") since "there is no time like the present" | |
| 408 if (relSpeed == 0) return 0; | |
| 409 | |
| 410 // Now consider the path of the other vehicle in this relative | |
| 411 // space, a line defined by the relative position and velocity. | |
| 412 // The distance from the origin (our vehicle) to that line is | |
| 413 // the nearest approach. | |
| 414 | |
| 415 // Take the unit tangent along the other vehicle's path | |
| 416 bzVec2 relTangent = relVelocity / relSpeed; | |
| 417 | |
| 418 // find distance from its path to origin (compute offset from | |
| 419 // other to us, find length of projection onto path) | |
| 420 bzVec2 relPosition = m_position - other.position; | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
421 float projection = bzDot(relTangent, relPosition); |
| 9 | 422 |
| 423 return projection / relSpeed; | |
| 424 } | |
| 0 | 425 |
| 9 | 426 // Given the time until nearest approach (predictNearestApproachTime) |
| 427 // determine position of each vehicle at that time, and the distance | |
| 428 // between them | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
429 float computeNearestApproachPositions (State other, float time) { |
| 0 | 430 |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
431 bzVec2 myTravel = m_forward * m_speed * time; |
| 9 | 432 bzVec2 otherTravel = other.forward * other.speed * time; |
| 433 | |
| 434 bzVec2 myFinal = m_position + myTravel; | |
| 435 bzVec2 otherFinal = other.position + otherTravel; | |
| 0 | 436 |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
437 return (myFinal - otherFinal).length; |
| 9 | 438 } |
| 439 | |
| 440 // ------------------------------------------------------------------------ | |
| 441 // pursuit of another vehicle ( version with ceiling on prediction time) | |
| 442 | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
443 bzVec2 steerForPursuit (State quarry) { |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
444 return steerForPursuit (quarry, float.max); |
| 9 | 445 } |
| 446 | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
447 bzVec2 steerForPursuit (State quarry, float maxPredictionTime) { |
| 0 | 448 |
| 9 | 449 // offset from this to quarry, that distance, unit vector toward quarry |
| 450 bzVec2 offset = quarry.position - m_position; | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
451 float distance = offset.length; |
| 9 | 452 bzVec2 unitOffset = offset / distance; |
| 453 | |
| 454 // how parallel are the paths of "this" and the quarry | |
| 455 // (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
|
456 float parallelness = bzDot(m_forward , quarry.forward); |
| 9 | 457 |
| 458 // how "forward" is the direction to the quarry | |
| 459 // (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
|
460 float forwardness = bzDot(m_forward , unitOffset); |
| 0 | 461 |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
462 float directTravelTime = distance / m_speed; |
| 9 | 463 int f = intervalComparison (forwardness, -0.707f, 0.707f); |
| 464 int p = intervalComparison (parallelness, -0.707f, 0.707f); | |
| 465 | |
| 466 float timeFactor = 0; // to be filled in below | |
| 0 | 467 |
| 9 | 468 // Break the pursuit into nine cases, the cross product of the |
| 469 // quarry being [ahead, aside, or behind] us and heading | |
| 470 // [parallel, perpendicular, or anti-parallel] to us. | |
| 471 switch (f) | |
| 472 { | |
| 473 case +1: | |
| 474 switch (p) | |
| 0 | 475 { |
| 9 | 476 case +1: // ahead, parallel |
| 477 timeFactor = 4; | |
| 0 | 478 break; |
| 9 | 479 case 0: // ahead, perpendicular |
| 480 timeFactor = 1.8f; | |
| 0 | 481 break; |
| 9 | 482 case -1: // ahead, anti-parallel |
| 483 timeFactor = 0.85f; | |
| 0 | 484 break; |
| 485 } | |
| 9 | 486 break; |
| 487 case 0: | |
| 488 switch (p) | |
| 489 { | |
| 490 case +1: // aside, parallel | |
| 491 timeFactor = 1; | |
| 492 break; | |
| 493 case 0: // aside, perpendicular | |
| 494 timeFactor = 0.8f; | |
| 495 break; | |
| 496 case -1: // aside, anti-parallel | |
| 497 timeFactor = 4; | |
| 498 break; | |
| 499 } | |
| 500 break; | |
| 501 case -1: | |
| 502 switch (p) | |
| 503 { | |
| 504 case +1: // behind, parallel | |
| 505 timeFactor = 0.5f; | |
| 506 break; | |
| 507 case 0: // behind, perpendicular | |
| 508 timeFactor = 2; | |
| 509 break; | |
| 510 case -1: // behind, anti-parallel | |
| 511 timeFactor = 2; | |
| 512 break; | |
| 513 } | |
| 514 break; | |
| 515 } | |
| 0 | 516 |
| 9 | 517 // estimated time until intercept of quarry |
| 518 float et = directTravelTime * timeFactor; | |
| 519 | |
| 520 // xxx experiment, if kept, this limit should be an argument | |
| 521 float etl = (et > maxPredictionTime) ? maxPredictionTime : et; | |
| 522 | |
| 523 // estimated position of quarry at intercept | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
524 bzVec2 target = quarry.predictFuturePosition(etl); |
| 0 | 525 |
| 12 | 526 return target; //steerForSeek (target); |
| 9 | 527 } |
| 528 | |
| 529 // ------------------------------------------------------------------------ | |
| 530 // evasion of another vehicle | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
531 bzVec2 steerForEvasion (State menace, float maxPredictionTime) { |
| 0 | 532 |
| 9 | 533 // offset from this to menace, that distance, unit vector toward menace |
| 534 bzVec2 offset = menace.position - m_position; | |
| 535 float distance = offset.length; | |
| 0 | 536 |
| 9 | 537 float roughTime = distance / menace.speed; |
| 538 float predictionTime = ((roughTime > maxPredictionTime) ? maxPredictionTime : roughTime); | |
| 539 bzVec2 target = menace.predictFuturePosition (predictionTime); | |
| 540 | |
| 541 return steerForFlee (target); | |
| 542 } | |
| 0 | 543 |
| 544 | |
| 9 | 545 // ------------------------------------------------------------------------ |
| 546 // tries to maintain a given speed, returns a maxForce-clipped steering | |
| 547 // force along the forward/backward axis | |
| 548 bzVec2 steerForTargetSpeed (float targetSpeed) { | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
549 float mf = m_maxForce; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
550 float speedError = targetSpeed - m_speed; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
551 return m_forward * bzClamp(speedError, -mf, +mf); |
| 9 | 552 } |
| 0 | 553 |
| 554 | |
| 9 | 555 // ----------------------------------------------------------- utilities |
| 556 bool isAhead (bzVec2 target) {return isAhead (target, 0.707f);}; | |
| 557 bool isAside (bzVec2 target) {return isAside (target, 0.707f);}; | |
| 558 bool isBehind (bzVec2 target) {return isBehind (target, -0.707f);}; | |
| 0 | 559 |
| 9 | 560 bool isAhead (bzVec2 target, float cosThreshold) |
| 561 { | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
562 bzVec2 targetDirection = target - m_position; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
563 targetDirection.normalize(); |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
564 return bzDot(m_forward, targetDirection) > cosThreshold; |
| 9 | 565 } |
| 0 | 566 |
| 9 | 567 bool isAside (bzVec2 target, float cosThreshold) |
| 568 { | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
569 bzVec2 targetDirection = target - m_position; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
570 targetDirection.normalize(); |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
571 float dp = bzDot(m_forward, targetDirection); |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
572 return (dp < cosThreshold) && (dp > -cosThreshold); |
| 9 | 573 } |
| 0 | 574 |
| 9 | 575 bool isBehind (bzVec2 target, float cosThreshold) |
| 576 { | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
577 bzVec2 targetDirection = target - m_position; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
578 targetDirection.normalize(); |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
579 return bzDot(m_forward, targetDirection) < cosThreshold; |
| 9 | 580 } |
| 581 | |
| 582 private: | |
| 583 | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
584 Ship m_ship; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
585 |
| 9 | 586 bzVec2 m_position; |
| 587 bzVec2 m_velocity; | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
588 bzVec2 m_up; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
589 bzVec2 m_side; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
590 bzVec2 m_forward; |
| 19 | 591 float m_radius; |
| 592 bzBody m_body; | |
|
11
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
593 |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
594 float m_speed = 0; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
595 float m_maxForce = 0; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
596 |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
597 // Wander behavior |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
598 float m_wanderSide; |
|
d998bf1b0654
Added utilities and AI; fixed steer
Mason Green <mason.green@gmail.com>
parents:
9
diff
changeset
|
599 float m_wanderUp; |
| 9 | 600 } |