// ----------------------------------------------------------------------------
// Returns the distance between a point and a line. The line is defined in
// terms of a point on the line ("lineOrigin") and a UNIT vector parallel to
// the line ("lineUnitTangent")
public static float DistanceFromLine(Vector3 point, Vector3 lineOrigin, Vector3 lineUnitTangent)
{
Vector3 offset = point - lineOrigin;
Vector3 perp = Vector3Helpers.PerpendicularComponent(offset, lineUnitTangent);
return(perp.Length());
}
// ------------------------------------------------------------------------
// avoidance of "close neighbors" -- used only by steerToAvoidNeighbors
//
// XXX Does a hard steer away from any other agent who comes withing a
// XXX critical distance. Ideally this should be replaced with a call
// XXX to steerForSeparation.
public Vector3 SteerToAvoidCloseNeighbors <TVehicle>(float minSeparationDistance, IEnumerable <TVehicle> others)
where TVehicle : IVehicle
{
// for each of the other vehicles...
foreach (IVehicle other in others)
{
if (other != this /*this*/)
{
float sumOfRadii = this.Radius + other.Radius;
float minCenterToCenter = minSeparationDistance + sumOfRadii;
Vector3 offset = other.Position - this.Position;
float currentDistance = offset.Length;
//If we're exactly on top of each other, something's gotta give
if (offset == Vector3.Zero)
{
Random rnd = new Random();
return(new Vector3(rnd.NextDouble(), rnd.NextDouble(), 0));
}
if (currentDistance < minCenterToCenter)
{
return(Vector3Helpers.PerpendicularComponent(-offset, this.Forward));
}
}
}
// otherwise return zero
return(Vector3.Zero);
}
// measure path curvature (1/turning-radius), maintain smoothed version
void MeasurePathCurvature(float elapsedTime)
{
if (elapsedTime > 0)
{
Vector3 dP = lastPosition - Position;
Vector3 dF = (lastForward - Forward) / dP.Length;
Vector3 lateral = Vector3Helpers.PerpendicularComponent(dF, Forward);
float sign = (Vector3.Dot(lateral, Side) < 0) ? 1.0f : -1.0f;
curvature = lateral.Length * sign;
Utilities.BlendIntoAccumulator(elapsedTime * 4.0f, curvature, ref smoothedCurvature);
lastForward = Forward;
lastPosition = Position;
}
}
// 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 = Vector3Helpers.PerpendicularComponent(offset, this.Forward);
avoidance.Normalize();
avoidance *= this.MaxForce;
avoidance += this.Forward * this.MaxForce * 0.75f;
}
return(avoidance);
}
// ------------------------------------------------------------------------
// avoidance of "close neighbors" -- used only by steerToAvoidNeighbors
//
// XXX Does a hard steer away from any other agent who comes withing a
// XXX critical distance. Ideally this should be replaced with a call
// XXX to steerForSeparation.
public Vector3 SteerToAvoidCloseNeighbors <TVehicle>(float minSeparationDistance, List <TVehicle> others)
where TVehicle : IVehicle
{
// for each of the other vehicles...
foreach (IVehicle other in others)
{
if (other != this /*this*/)
{
float sumOfRadii = this.Radius + other.Radius;
float minCenterToCenter = minSeparationDistance + sumOfRadii;
Vector3 offset = other.Position - this.Position;
float currentDistance = offset.Length();
if (currentDistance < minCenterToCenter)
{
annotation.AvoidCloseNeighbor(other, minSeparationDistance);
return(Vector3Helpers.PerpendicularComponent(-offset, this.Forward));
}
}
}
// otherwise return zero
return(Vector3.Zero);
}