public SteerForSphericalObstacleAvoidance.PathIntersection FindNextIntersectionWithSphere(SphericalObstacle obs, Vector3 line)
{
Vector3 vector = base.Vehicle.Position - obs.center;
SteerForSphericalObstacleAvoidance.PathIntersection result = new SteerForSphericalObstacleAvoidance.PathIntersection(obs);
obs.annotatePosition();
Debug.DrawLine(base.Vehicle.Position, base.Vehicle.Position + line, Color.get_cyan());
float sqrMagnitude = line.get_sqrMagnitude();
float num = 2f * Vector3.Dot(line, vector);
float num2 = obs.center.get_sqrMagnitude();
num2 += base.Vehicle.Position.get_sqrMagnitude();
num2 -= 2f * Vector3.Dot(obs.center, base.Vehicle.Position);
num2 -= Mathf.Pow(obs.radius + base.Vehicle.ScaledRadius, 2f);
float num3 = num * num - 4f * sqrMagnitude * num2;
if (num3 >= 0f)
{
result.intersect = true;
Vector3 vector2 = Vector3.get_zero();
if (num3 == 0f)
{
float num4 = -num / (2f * sqrMagnitude);
vector2 = num4 * line;
}
else
{
float num5 = (-num + Mathf.Sqrt(num3)) / (2f * sqrMagnitude);
float num6 = (-num - Mathf.Sqrt(num3)) / (2f * sqrMagnitude);
if (num5 < 0f && num6 < 0f)
{
result.intersect = false;
}
else
{
vector2 = ((Mathf.Abs(num5) >= Mathf.Abs(num6)) ? (num6 * line) : (num5 * line));
}
}
Debug.DrawRay(base.Vehicle.Position, vector2, Color.get_red());
result.distance = vector2.get_magnitude();
}
return(result);
}
/// <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);
}
