/// <summary>
///Returns a SphericalObstacle from the current gameObject
/// </summary>
/// <param name="gameObject">
/// A game object to create the obstacle from<see cref="GameObject"/>
/// </param>
/// <returns>
/// A SphericalObstacle encompassing the game object<see cref="Obstacle"/>
/// </returns>
public static Obstacle GetObstacle(GameObject gameObject)
{
SphericalObstacle obstacle;
int id = gameObject.GetInstanceID();
Component[] colliders;
float radius = 0.0f, currentRadius;
if (!ObstacleCache.ContainsKey(id))
{
var obstacleData = gameObject.GetComponent <SphericalObstacleData>();
// If the object provides his own spherical obstacle information,
// use it instead of calculating a sphere that encompasses the
// whole collider.
if (obstacleData != null)
{
ObstacleCache[id] = new SphericalObstacle(obstacleData.Radius, gameObject.transform.position + obstacleData.Center);
}
else
{
colliders = gameObject.GetComponentsInChildren <Collider>();
if (colliders == null)
{
Debug.LogError("Obstacle '" + gameObject.name + "' has no colliders");
return(null);
}
foreach (Collider collider in colliders)
{
if (collider.isTrigger)
{
continue;
}
// Get the maximum extent to create a sphere that encompasses the whole obstacle
float maxExtents = Mathf.Max(Mathf.Max(collider.bounds.extents.x, collider.bounds.extents.y),
collider.bounds.extents.z);
/*
* Calculate the displacement from the object center to the
* collider, and add in the maximum extents of the bounds.
* Notice that we don't need to multiply by the object's
* local scale, since that is already considered in the
* bounding rectangle.
*/
float distanceToCollider = Vector3.Distance(gameObject.transform.position, collider.bounds.center);
currentRadius = distanceToCollider + maxExtents;
if (currentRadius > radius)
{
radius = currentRadius;
}
}
ObstacleCache[id] = new SphericalObstacle(radius, gameObject.transform.position);
}
}
obstacle = ObstacleCache[id] as SphericalObstacle;
return(obstacle);
}
/// <summary>
///Returns a SphericalObstacle from the current gameObject
/// </summary>
/// <param name="gameObject">
/// A game object to create the obstacle from<see cref="GameObject"/>
/// </param>
/// <returns>
/// A SphericalObstacle encompassing the game object<see cref="Obstacle"/>
/// </returns>
public static Obstacle GetObstacle( GameObject gameObject )
{
SphericalObstacle obstacle;
int id = gameObject.GetInstanceID();
Component[] colliders;
float radius = 0.0f, currentRadius;
if(!ObstacleCache.ContainsKey( id ))
{
var obstacleData = gameObject.GetComponent<SphericalObstacleData>();
// If the object provides his own spherical obstacle information,
// use it instead of calculating a sphere that encompasses the
// whole collider.
if (obstacleData != null)
{
ObstacleCache[id] = new SphericalObstacle(obstacleData.Radius, gameObject.transform.position + obstacleData.Center);
}
else
{
colliders = gameObject.GetComponentsInChildren<Collider>();
if( colliders == null )
{
Debug.LogError( "Obstacle '" + gameObject.name + "' has no colliders" );
return null;
}
foreach( Collider collider in colliders )
{
if( collider.isTrigger )
{
continue;
}
// Get the maximum extent to create a sphere that encompasses the whole obstacle
float maxExtents = Mathf.Max(Mathf.Max(collider.bounds.extents.x, collider.bounds.extents.y),
collider.bounds.extents.z);
/*
* Calculate the displacement from the object center to the
* collider, and add in the maximum extents of the bounds.
* Notice that we don't need to multiply by the object's
* local scale, since that is already considered in the
* bounding rectangle.
*/
float distanceToCollider = Vector3.Distance(gameObject.transform.position, collider.bounds.center);
currentRadius = distanceToCollider + maxExtents;
if( currentRadius > radius )
{
radius = currentRadius;
}
}
ObstacleCache[id] = new SphericalObstacle( radius, gameObject.transform.position );
}
}
obstacle = ObstacleCache[ id ] as SphericalObstacle;
return obstacle;
}
public PathIntersection(SphericalObstacle obstacle)
{
this.obstacle = obstacle;
intersect = false;
distance = float.MaxValue;
}
/// <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;
}