// this version avoids all of the obstacles in an ObstacleGroup // // XXX 9-12-03: note this does NOT use the Obstacle::steerToAvoid protocol // XXX like the older steerToAvoidObstacle does/did. It needs to be fixed public Vector3 steerToAvoidObstacles(float minTimeToCollision, ArrayList obstacles) { Vector3 avoidance = new Vector3(); PathIntersection nearest, next; nearest = new PathIntersection(); next = new PathIntersection(); float minDistanceToCollision = minTimeToCollision * speed(); next.intersect = 0; // false; nearest.intersect = 0; // false; // test all obstacles for intersection with my forward axis, // select the one whose point of intersection is nearest //for (ObstacleIterator o = obstacles.begin(); o != obstacles.end(); o++) for (int i = 0; i < obstacles.Count; i++) { SphericalObstacle o = (SphericalObstacle)obstacles[i]; // xxx this should be a generic call on Obstacle, rather than // xxx this code which presumes the obstacle is spherical findNextIntersectionWithSphere(o, next); if ((nearest.intersect == 0) || ((next.intersect != 0) && (next.distance < nearest.distance))) { nearest = next; } } // when a nearest intersection was found if ((nearest.intersect != 0) && (nearest.distance < minDistanceToCollision)) { // show the corridor that was checked for collisions annotateAvoidObstacle(minDistanceToCollision); // compute avoidance steering force: take offset from obstacle to me, // take the component of that which is lateral (perpendicular to my // forward direction), set length to maxForce, add a bit of forward // component (in capture the flag, we never want to slow down) Vector3 offset = Position - nearest.obstacle.center; //avoidance = offset.perpendicularComponent (forward()); avoidance = OpenSteerUtility.perpendicularComponent(offset, forward()); avoidance.Normalize();//.normalize (); avoidance *= maxForce(); avoidance += forward() * maxForce() * 0.75f; } return(avoidance); }
// ---------------------------------------------------------------------------- // xxx experiment cwr 9-6-02 public void findNextIntersectionWithSphere(SphericalObstacle obs, PathIntersection intersection) { // xxx"SphericalObstacle& obs" should be "const SphericalObstacle& // obs" but then it won't let me store a pointer to in inside the // PathIntersection // This routine is based on the Paul Bourke's derivation in: // Intersection of a Line and a Sphere (or circle) // http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/ float b, c, d, p, q, s; Vector3 lc; // initialize pathIntersection object intersection.intersect = 0; intersection.obstacle = obs; // find "local center" (lc) of sphere in boid's coordinate space lc = localizePosition(obs.center); // computer line-sphere intersection parameters b = -2 * lc.z; c = square(lc.x) + square(lc.y) + square(lc.z) - square(obs.radius + radius()); d = (b * b) - (4 * c); // when the path does not intersect the sphere if (d < 0) { return; } // otherwise, the path intersects the sphere in two points with // parametric coordinates of "p" and "q". // (If "d" is zero the two points are coincident, the path is tangent) s = (float)System.Math.Sqrt(d); p = (-b + s) / 2; q = (-b - s) / 2; // both intersections are behind us, so no potential collisions if ((p < 0) && (q < 0)) { return; } // at least one intersection is in front of us intersection.intersect = 0; intersection.distance = ((p > 0) && (q > 0)) ? // both intersections are in front of us, find nearest one ((p < q) ? p : q) : // otherwise only one intersections is in front, select it ((p > 0) ? p : q); return; }
// ---------------------------------------------------------------------------- // xxx experiment cwr 9-6-02 public void findNextIntersectionWithSphere(SphericalObstacle obs, PathIntersection intersection) { // xxx"SphericalObstacle& obs" should be "const SphericalObstacle& // obs" but then it won't let me store a pointer to in inside the // PathIntersection // This routine is based on the Paul Bourke's derivation in: // Intersection of a Line and a Sphere (or circle) // http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/ float b, c, d, p, q, s; Vector3 lc; // initialize pathIntersection object intersection.intersect = 0; intersection.obstacle = obs; // find "local center" (lc) of sphere in boid's coordinate space lc = localizePosition (obs.center); // computer line-sphere intersection parameters b = -2 * lc.z; c = square (lc.x) + square (lc.y) + square (lc.z) - square (obs.radius + radius()); d = (b * b) - (4 * c); // when the path does not intersect the sphere if (d < 0) return; // otherwise, the path intersects the sphere in two points with // parametric coordinates of "p" and "q". // (If "d" is zero the two points are coincident, the path is tangent) s = (float) System.Math.Sqrt(d); p = (-b + s) / 2; q = (-b - s) / 2; // both intersections are behind us, so no potential collisions if ((p < 0) && (q < 0)) return; // at least one intersection is in front of us intersection.intersect = 0; intersection.distance = ((p > 0) && (q > 0)) ? // both intersections are in front of us, find nearest one ((p < q) ? p : q) : // otherwise only one intersections is in front, select it ((p > 0) ? p : q); return; }