void
OpenSteer::SteerLibrary::
findNextIntersectionWithSphere (const AbstractVehicle& v,
SphericalObstacleData& 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;
float3 lc;
// initialize pathIntersection object
intersection.intersect = false;
intersection.obstacle = &obs;
// find "local center" (lc) of sphere in boid's coordinate space
lc = v.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 + v.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 = sqrtXXX (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 = true;
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;
}
void
OpenSteer::
SphereObstacle::
findIntersectionWithVehiclePath (const AbstractVehicle& vehicle,
PathIntersection& pi) const
{
// 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/
// But the computation is done in the vehicle's local space, so
// the line in question is the Z (Forward) axis of the space which
// simplifies some of the calculations.
float b, c, d, p, q, s;
Vec3 lc;
// initialize pathIntersection object to "no intersection found"
pi.intersect = false;
// find sphere's "local center" (lc) in the vehicle's coordinate space
lc = vehicle.localizePosition (center);
pi.vehicleOutside = lc.length () > radius;
// if obstacle is seen from inside, but vehicle is outside, must avoid
// (noticed once a vehicle got outside it ignored the obstacle 2008-5-20)
if (pi.vehicleOutside && (seenFrom () == inside))
{
pi.intersect = true;
pi.distance = 0.0f;
pi.steerHint = (center - vehicle.position()).normalize();
return;
}
// compute line-sphere intersection parameters
const float r = radius + vehicle.radius();
b = -2 * lc.z;
c = square (lc.x) + square (lc.y) + square (lc.z) - square (r);
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 = sqrtXXX (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, so intersects our forward
// path
pi.intersect = true;
pi.obstacle = this;
pi.distance =
((p > 0) && (q > 0)) ?
// both intersections are in front of us, find nearest one
((p < q) ? p : q) :
// otherwise one is ahead and one is behind: we are INSIDE obstacle
(seenFrom () == outside ?
// inside a solid obstacle, so distance to obstacle is zero
0.0f :
// hollow obstacle (or "both"), pick point that is in front
((p > 0) ? p : q));
pi.surfacePoint =
vehicle.position() + (vehicle.forward() * pi.distance);
pi.surfaceNormal = (pi.surfacePoint-center).normalize();
switch (seenFrom ())
{
case outside:
pi.steerHint = pi.surfaceNormal;
break;
case inside:
pi.steerHint = -pi.surfaceNormal;
break;
case both:
pi.steerHint = pi.surfaceNormal * (pi.vehicleOutside ? 1.0f : -1.0f);
break;
}
}