public static Vector3 SteerToAvoidObstacles(this IVehicle vehicle, float minTimeToCollision, IEnumerable <IObstacle> obstacles, IAnnotationService annotation = null)
{
PathIntersection?nearest = null;
float minDistanceToCollision = minTimeToCollision * vehicle.Speed;
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
foreach (var o in obstacles)
{
var next = o.NextIntersection(vehicle);
if (!next.HasValue)
{
continue;
}
if (!nearest.HasValue || (next.Value < nearest.Value.Distance))
{
nearest = new PathIntersection {
Distance = next.Value, Obstacle = o
}
}
;
}
if (nearest.HasValue)
{
if (annotation != null)
{
annotation.AvoidObstacle(minDistanceToCollision);
}
return(nearest.Value.Obstacle.SteerToAvoid(vehicle, minTimeToCollision));
}
return(Vector3.Zero);
}
/// <summary>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and steer to avoid the closest one to the
/// vehicle. It's not ideal, as that means the vehicle might crash into
/// another obstacle while avoiding the closest one, but it'll do.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
{
return(avoidance);
}
PathIntersection nearest = new PathIntersection(null);
/*
* While we could just calculate line as (Velocity * predictionTime)
* and save ourselves the substraction, this allows other vehicles to
* override PredictFuturePosition for their own ends.
*/
Vector3 futurePosition = Vehicle.PredictFuturePosition(_minTimeToCollision);
Vector3 line = (futurePosition - Vehicle.Position);
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
Profiler.BeginSample("Find nearest intersection");
foreach (var o in Vehicle.Radar.Obstacles)
{
SphericalObstacle sphere = o as SphericalObstacle;
PathIntersection next = FindNextIntersectionWithSphere(sphere, line);
if (!nearest.intersect ||
(next.intersect &&
next.distance < nearest.distance))
{
nearest = next;
}
}
Profiler.EndSample();
// when a nearest intersection was found
Profiler.BeginSample("Calculate avoidance");
if (nearest.intersect &&
nearest.distance < line.magnitude)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, nearest.obstacle.center, Color.red);
#endif
// 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 = Vehicle.Position - nearest.obstacle.center;
avoidance = OpenSteerUtility.perpendicularComponent(offset, transform.forward);
avoidance.Normalize();
avoidance *= Vehicle.MaxForce;
avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
}
Profiler.EndSample();
return(avoidance);
}
/// <summary>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and calculate a repulsion vector based
/// on them.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || !Vehicle.Radar.Obstacles.Any())
{
return(avoidance);
}
/*
* While we could just calculate movement as (Velocity * predictionTime)
* and save ourselves the substraction, this allows other vehicles to
* override PredictFuturePosition for their own ends.
*/
Vector3 futurePosition = Vehicle.PredictFutureDesiredPosition(_estimationTime);
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, futurePosition, Color.cyan);
#endif
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
Profiler.BeginSample("Accumulate spherical obstacle influences");
for (int i = 0; i < Vehicle.Radar.Obstacles.Count; i++)
{
var sphere = Vehicle.Radar.Obstacles[i];
if (sphere == null || sphere.Equals(null))
{
continue; // In case the object was destroyed since we cached it
}
PathIntersection next = FindNextIntersectionWithSphere(Vehicle.Position, futurePosition, sphere);
float avoidanceMultiplier = 1;
if (next.Intersect)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawRay(Vehicle.Position, Vehicle.DesiredVelocity.normalized * next.Distance, Color.yellow);
#endif
avoidanceMultiplier = Vehicle.Radar.Obstacles.Count;
}
var distanceCurrent = Vehicle.Position - sphere.Position;
var distanceFuture = futurePosition - sphere.Position;
avoidance += avoidanceMultiplier * distanceCurrent / distanceFuture.sqrMagnitude;
}
Profiler.EndSample();
avoidance /= Vehicle.Radar.Obstacles.Count;
var newDesired = Vector3.Reflect(Vehicle.DesiredVelocity, avoidance);
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.green);
Debug.DrawLine(Vehicle.Position, futurePosition, Color.blue);
Debug.DrawLine(Vehicle.Position, Vehicle.Position + newDesired, Color.white);
#endif
return(newDesired);
}
// 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
protected void FindNextIntersectionWithSphere(SphericalObstacle obs, ref PathIntersection intersection)
{
// 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 = false;
intersection.obstacle = obs;
// find "local center" (lc) of sphere in boid's coordinate space
lc = this.LocalizePosition(obs.Center);
// computer line-sphere intersection parameters
b = -2 * lc.Z;
c = lc.X * lc.X + lc.Y * lc.Y + lc.Z * lc.Z -
(obs.Radius + this.Radius) * (obs.Radius + this.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)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 = 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);
}
// avoids all obstacles in an ObstacleGroup
public Vector3 SteerToAvoidObstacles <Obstacle>(float minTimeToCollision, List <Obstacle> obstacles)
where Obstacle : IObstacle
{
Vector3 avoidance = Vector3.Zero;
PathIntersection nearest = new PathIntersection();
PathIntersection next = new PathIntersection();
float minDistanceToCollision = minTimeToCollision * this.Speed;
next.intersect = false;
nearest.intersect = false;
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
foreach (Obstacle o in obstacles)
{
//FIXME: this should be a generic call on Obstacle, rather than this code which presumes the obstacle is spherical
FindNextIntersectionWithSphere(o as SphericalObstacle, ref next);
if (nearest.intersect == false || (next.intersect != false && next.distance < nearest.distance))
{
nearest = next;
}
}
// when a nearest intersection was found
if ((nearest.intersect != false) && (nearest.distance < minDistanceToCollision))
{
// show the corridor that was checked for collisions
annotation.AvoidObstacle(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 = this.Position - nearest.obstacle.Center;
avoidance = VectorUtils.PerpendicularComponent(offset, this.Forward);
avoidance.Normalize();
avoidance *= this.MaxForce;
avoidance += this.Forward * this.MaxForce * 0.75f;
}
return(avoidance);
}
/// <summary>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and steer to avoid the closest one to the
/// vehicle. It's not ideal, as that means the vehicle might crash into
/// another obstacle while avoiding the closest one, but it'll do.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
{
return avoidance;
}
PathIntersection nearest = new PathIntersection(null);
/*
* While we could just calculate line as (Velocity * predictionTime)
* and save ourselves the substraction, this allows other vehicles to
* override PredictFuturePosition for their own ends.
*/
Vector3 futurePosition = Vehicle.PredictFuturePosition(_minTimeToCollision);
Vector3 line = (futurePosition - Vehicle.Position);
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
Profiler.BeginSample("Find nearest intersection");
foreach (var o in Vehicle.Radar.Obstacles)
{
SphericalObstacle sphere = o as SphericalObstacle;
PathIntersection next = FindNextIntersectionWithSphere (sphere, line);
if (!nearest.intersect ||
(next.intersect &&
next.distance < nearest.distance))
{
nearest = next;
}
}
Profiler.EndSample();
// when a nearest intersection was found
Profiler.BeginSample("Calculate avoidance");
if (nearest.intersect &&
nearest.distance < line.magnitude)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, nearest.obstacle.center, Color.red);
#endif
// 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 = Vehicle.Position - nearest.obstacle.center;
avoidance = OpenSteerUtility.perpendicularComponent(offset, transform.forward);
avoidance.Normalize();
avoidance *= Vehicle.MaxForce;
avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
}
Profiler.EndSample();
return avoidance;
}
/// <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>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and steer to avoid the closest one to the
/// vehicle. It's not ideal, as that means the vehicle might crash into
/// another obstacle while avoiding the closest one, but it'll do.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || !Vehicle.Radar.Obstacles.Any())
{
return(avoidance);
}
PathIntersection nearest = new PathIntersection(null);
/*
* While we could just calculate movement as (Velocity * predictionTime)
* and save ourselves the substraction, this allows other vehicles to
* override PredictFuturePosition for their own ends.
*/
Vector3 futurePosition = Vehicle.PredictFutureDesiredPosition(_minTimeToCollision);
Vector3 movement = futurePosition - Vehicle.Position;
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, futurePosition, Color.cyan);
#endif
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
Profiler.BeginSample("Find nearest intersection");
foreach (var o in Vehicle.Radar.Obstacles)
{
var sphere = o as DetectableObject;
PathIntersection next = FindNextIntersectionWithSphere(Vehicle.Position, futurePosition, sphere);
if (!nearest.Intersect ||
(next.Intersect &&
next.Distance < nearest.Distance))
{
nearest = next;
}
}
Profiler.EndSample();
// when a nearest intersection was found
Profiler.BeginSample("Calculate avoidance");
if (nearest.Intersect &&
nearest.Distance < movement.magnitude)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, nearest.Obstacle.Position, Color.red);
#endif
// compute avoidance steering force: take offset from obstacle to me,
// take the component of that which is lateral (perpendicular to my
// movement direction), add a bit of forward component
Vector3 offset = Vehicle.Position - nearest.Obstacle.Position;
Vector3 moveDirection = movement.normalized;
avoidance = OpenSteerUtility.perpendicularComponent(offset, moveDirection);
avoidance.Normalize();
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.white);
#endif
avoidance += moveDirection * Vehicle.MaxForce * _avoidanceForceFactor;
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.yellow);
#endif
}
Profiler.EndSample();
return(avoidance);
}
// 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 = Vector3.zero;
if (obstacles == null || obstacles.Count == 0)
{
return avoidance;
}
PathIntersection nearest = new PathIntersection(null);
float minDistanceToCollision = minTimeToCollision * Speed;
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
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
PathIntersection next = findNextIntersectionWithSphere (o);
if (!nearest.intersect ||
(next.intersect &&
next.distance < nearest.distance))
{
nearest = next;
}
}
// when a nearest intersection was found
if (nearest.intersect &&
nearest.distance < minDistanceToCollision)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Position, nearest.obstacle.center, Color.red);
#endif
// 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();
avoidance *= MaxForce;
avoidance += Forward * MaxForce * 0.75f;
}
return avoidance;
}
/// <summary>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and steer to avoid the closest one to the
/// vehicle. It's not ideal, as that means the vehicle might crash into
/// another obstacle while avoiding the closest one, but it'll do.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || !Vehicle.Radar.Obstacles.Any())
{
return avoidance;
}
PathIntersection nearest = new PathIntersection(null);
/*
* While we could just calculate movement as (Velocity * predictionTime)
* and save ourselves the substraction, this allows other vehicles to
* override PredictFuturePosition for their own ends.
*/
Vector3 futurePosition = Vehicle.PredictFutureDesiredPosition(_minTimeToCollision);
Vector3 movement = futurePosition - Vehicle.Position;
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, futurePosition, Color.cyan);
#endif
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
Profiler.BeginSample("Find nearest intersection");
foreach (var o in Vehicle.Radar.Obstacles)
{
var sphere = o as DetectableObject;
PathIntersection next = FindNextIntersectionWithSphere(Vehicle.Position, futurePosition, sphere);
if (!nearest.Intersect ||
(next.Intersect &&
next.Distance < nearest.Distance))
{
nearest = next;
}
}
Profiler.EndSample();
// when a nearest intersection was found
Profiler.BeginSample("Calculate avoidance");
if (nearest.Intersect &&
nearest.Distance < movement.magnitude)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, nearest.Obstacle.Position, Color.red);
#endif
// compute avoidance steering force: take offset from obstacle to me,
// take the component of that which is lateral (perpendicular to my
// movement direction), add a bit of forward component
Vector3 offset = Vehicle.Position - nearest.Obstacle.Position;
Vector3 moveDirection = movement.normalized;
avoidance = OpenSteerUtility.perpendicularComponent(offset, moveDirection);
avoidance.Normalize();
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.white);
#endif
avoidance += moveDirection * Vehicle.MaxForce * _avoidanceForceFactor;
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + avoidance, Color.yellow);
#endif
}
Profiler.EndSample();
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;
}
// 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.Normalise();//.normalize ();
avoidance *= maxForce ();
avoidance += forward() * maxForce () * 0.75f;
}
return avoidance;
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="obs">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </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(DetectableObject 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.Position;
// initialize pathIntersection object
var intersection = new PathIntersection(obs);
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(Vehicle.Position, Vehicle.Position + line, Color.cyan);
#endif
// computer line-sphere intersection parameters
a = line.magnitude;
b = 2 * Vector3.Dot(line, toCenter);
c = obs.Position.magnitude;
c += Vehicle.Position.magnitude;
c -= 2 * Vector3.Dot(obs.Position, Vehicle.Position);
c -= Mathf.Pow(obs.ScaledRadius + 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 a vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="vehicle">
/// The vehicle to evaluate.
/// </param>
/// <param name="futureVehiclePosition">
/// The position where we expect the vehicle to be soon
/// </param>
/// <param name="obstacle">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
/// <remarks>We could probably spin out this function to an independent tool class</remarks>
public static PathIntersection FindNextIntersectionWithSphere(Vehicle vehicle, Vector3 futureVehiclePosition,
DetectableObject obstacle)
{
// this mainly follows http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
var intersection = new PathIntersection(obstacle);
var combinedRadius = vehicle.Radius + obstacle.Radius;
var movement = futureVehiclePosition - vehicle.Position;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehicle.Position;
// this is the length of vehicleToObstacle projected onto direction
var projectionLength = Vector3.Dot(direction, vehicleToObstacle);
// if the projected obstacle center lies further away than our movement + both radius, we're not going to collide
if (projectionLength > movement.magnitude + combinedRadius)
{
//print("no collision - 1");
return intersection;
}
// the foot of the perpendicular
var projectedObstacleCenter = vehicle.Position + projectionLength * direction;
// distance of the obstacle to the pathe the vehicle is going to take
var obstacleDistanceToPath = (obstacle.Position - projectedObstacleCenter).magnitude;
//print("obstacleDistanceToPath: " + obstacleDistanceToPath);
// if the obstacle is further away from the movement, than both radius, there's no collision
if (obstacleDistanceToPath > combinedRadius)
{
//print("no collision - 2");
return intersection;
}
// use pythagorean theorem to calculate distance out of the sphere (if you do it 2D, the line through the circle would be a chord and we need half of its length)
var halfChord = Mathf.Sqrt(combinedRadius * combinedRadius + obstacleDistanceToPath * obstacleDistanceToPath);
// if the projected obstacle center lies opposite to the movement direction (aka "behind")
if (projectionLength < 0)
{
// behind and further away than both radius -> no collision (we already passed)
if (vehicleToObstacle.magnitude > combinedRadius)
return intersection;
var intersectionPoint = projectedObstacleCenter - direction * halfChord;
intersection.Intersect = true;
intersection.Distance = (intersectionPoint - vehicle.Position).magnitude;
return intersection;
}
// calculate both intersection points
var intersectionPoint1 = projectedObstacleCenter - direction * halfChord;
var intersectionPoint2 = projectedObstacleCenter + direction * halfChord;
// pick the closest one
var intersectionPoint1Distance = (intersectionPoint1 - vehicle.Position).magnitude;
var intersectionPoint2Distance = (intersectionPoint2 - vehicle.Position).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
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;
}
/// <summary>
/// Finds the vehicle's next intersection with a spherical obstacle
/// </summary>
/// <param name="vehiclePosition">
/// The current position of the vehicle
/// </param>
/// <param name="futureVehiclePosition">
/// The position where we expect the vehicle to be soon
/// </param>
/// <param name="obstacle">
/// A spherical obstacle to check against <see cref="DetectableObject"/>
/// </param>
/// <returns>
/// A PathIntersection with the intersection details <see cref="PathIntersection"/>
/// </returns>
public PathIntersection FindNextIntersectionWithSphere(Vector3 vehiclePosition, Vector3 futureVehiclePosition, DetectableObject obstacle)
{
// this mainly follows http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
var intersection = new PathIntersection(obstacle);
float combinedRadius = Vehicle.ScaledRadius + obstacle.ScaledRadius;
var movement = futureVehiclePosition - vehiclePosition;
var direction = movement.normalized;
var vehicleToObstacle = obstacle.Position - vehiclePosition;
// this is the length of vehicleToObstacle projected onto direction
float projectionLength = Vector3.Dot(direction, vehicleToObstacle);
// if the projected obstacle center lies further away than our movement + both radius, we're not going to collide
if (projectionLength > movement.magnitude + combinedRadius)
{
//print("no collision - 1");
return(intersection);
}
// the foot of the perpendicular
var projectedObstacleCenter = vehiclePosition + projectionLength * direction;
// distance of the obstacle to the pathe the vehicle is going to take
float obstacleDistanceToPath = (obstacle.Position - projectedObstacleCenter).magnitude;
//print("obstacleDistanceToPath: " + obstacleDistanceToPath);
// if the obstacle is further away from the movement, than both radius, there's no collision
if (obstacleDistanceToPath > combinedRadius)
{
//print("no collision - 2");
return(intersection);
}
// use pythagorean theorem to calculate distance out of the sphere (if you do it 2D, the line through the circle would be a chord and we need half of its length)
float halfChord = Mathf.Sqrt(combinedRadius * combinedRadius + obstacleDistanceToPath * obstacleDistanceToPath);
// if the projected obstacle center lies opposite to the movement direction (aka "behind")
if (projectionLength < 0)
{
// behind and further away than both radius -> no collision (we already passed)
if (vehicleToObstacle.magnitude > combinedRadius)
{
return(intersection);
}
var intersectionPoint = projectedObstacleCenter - direction * halfChord;
intersection.Intersect = true;
intersection.Distance = (intersectionPoint - vehiclePosition).magnitude;
return(intersection);
}
// calculate both intersection points
var intersectionPoint1 = projectedObstacleCenter - direction * halfChord;
var intersectionPoint2 = projectedObstacleCenter + direction * halfChord;
// pick the closest one
float intersectionPoint1Distance = (intersectionPoint1 - vehiclePosition).magnitude;
float intersectionPoint2Distance = (intersectionPoint2 - vehiclePosition).magnitude;
intersection.Intersect = true;
intersection.Distance = Mathf.Min(intersectionPoint1Distance, intersectionPoint2Distance);
return(intersection);
}
/// <summary>
/// Calculates the force necessary to avoid the closest spherical obstacle
/// </summary>
/// <returns>
/// Force necessary to avoid an obstacle, or Vector3.zero
/// </returns>
/// <remarks>
/// This method will iterate through all detected spherical obstacles that
/// are within MinTimeToCollision, and steer to avoid the closest one to the
/// vehicle. It's not ideal, as that means the vehicle might crash into
/// another obstacle while avoiding the closest one, but it'll do.
/// </remarks>
protected override Vector3 CalculateForce()
{
Vector3 avoidance = Vector3.zero;
if (Vehicle.Radar.Obstacles == null || Vehicle.Radar.Obstacles.Count == 0)
{
return avoidance;
}
PathIntersection nearest = new PathIntersection(null);
float minDistanceToCollision = _minTimeToCollision * Vehicle.Speed;
// test all obstacles for intersection with my forward axis,
// select the one whose point of intersection is nearest
foreach (var o in Vehicle.Radar.Obstacles)
{
SphericalObstacle sphere = o as SphericalObstacle;
PathIntersection next = FindNextIntersectionWithSphere (sphere);
if (!nearest.intersect ||
(next.intersect &&
next.distance < nearest.distance))
{
nearest = next;
}
}
// when a nearest intersection was found
if (nearest.intersect &&
nearest.distance < minDistanceToCollision)
{
#if ANNOTATE_AVOIDOBSTACLES
Debug.DrawLine(transform.position, nearest.obstacle.center, Color.red);
#endif
// 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 = transform.position - nearest.obstacle.center;
avoidance = OpenSteerUtility.perpendicularComponent(offset, transform.forward);
avoidance.Normalize();
avoidance *= Vehicle.MaxForce;
avoidance += transform.forward * Vehicle.MaxForce * _avoidanceForceFactor;
}
return avoidance;
}
