/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="obs">
/// A spherical obstacle to check against <see cref="SphericalObstacle"/>
/// </param>
/// <param name="line">
/// Line that we expect we'll follow to our future destination
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
public PathIntersection FindNextIntersectionWithSphere (SphericalObstacle obs, Vector3 line)
{
/*
* 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/
*
* Retaining the same variable values used in that description.
*
*/
float a, b, c, bb4ac;
var toCenter = Vehicle.Position - obs.center;
// initialize pathIntersection object
var intersection = new PathIntersection(obs);
#if ANNOTATE_AVOIDOBSTACLES
obs.annotatePosition();
Debug.DrawLine(Vehicle.Position, Vehicle.Position + line, Color.cyan);
#endif
// computer line-sphere intersection parameters
a = line.sqrMagnitude;
b = 2 * Vector3.Dot(line, toCenter);
c = obs.center.sqrMagnitude;
c += Vehicle.Position.sqrMagnitude;
c -= 2 * Vector3.Dot(obs.center, Vehicle.Position);
c -= Mathf.Pow(obs.radius + Vehicle.ScaledRadius, 2);
bb4ac = b * b - 4 * a * c;
if (bb4ac >= 0) {
intersection.intersect = true;
Vector3 closest = Vector3.zero;
if (bb4ac == 0) {
// Only one intersection
var mu = -b / (2*a);
closest = mu * line;
}
else {
// More than one intersection
var mu1 = (-b + Mathf.Sqrt(bb4ac)) / (2*a);
var mu2 = (-b - Mathf.Sqrt(bb4ac)) / (2*a);
/*
* If the results are negative, the obstacle is behind us.
*
* If one result is negative and the other one positive,
* that would indicate that one intersection is behind us while
* the other one ahead of us, which would mean that we're
* just overlapping the obstacle, so we should still avoid.
*/
if (mu1 < 0 && mu2 < 0)
intersection.intersect = false;
else
closest = (Mathf.Abs(mu1) < Mathf.Abs (mu2)) ? mu1 * line : mu2 * line;
}
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawRay(Vehicle.Position, closest, Color.red);
#endif
intersection.distance = closest.magnitude;
}
return intersection;
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="obs">
/// A spherical obstacle to check against <see cref="SphericalObstacle"/>
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
public PathIntersection FindNextIntersectionWithSphere (SphericalObstacle obs)
{
// 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
PathIntersection intersection = new PathIntersection(obs);
// find "local center" (lc) of sphere in the vehicle's coordinate space
lc = transform.InverseTransformPoint(obs.center);
#if ANNOTATE_AVOIDOBSTACLES
obs.annotatePosition();
#endif
// computer line-sphere intersection parameters
b = -2 * lc.z;
c = Mathf.Pow(lc.x, 2) + Mathf.Pow(lc.y, 2) + Mathf.Pow(lc.z, 2) -
Mathf.Pow(obs.radius + Vehicle.Radius, 2);
d = (b * b) - (4 * c);
// when the path does not intersect the sphere
if (d < 0) return intersection;
// 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 intersection;
// 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 intersection;
}