// Find the AbstractVehicle whose screen position is nearest the given window
// coordinates, typically the mouse position. Returns NULL if there are no
// AbstractVehicles.
//
// This works by constructing a line in 3d space between the camera location
// and the "mouse point". Then it measures the distance from that line to the
// centers of each AbstractVehicle. It returns the AbstractVehicle whose
// distance is smallest.
//
// xxx Issues: Should the distanceFromLine test happen in "perspective space"
// xxx or in "screen space"? Also: I think this would be happy to select a
// xxx vehicle BEHIND the camera location.
internal static IVehicle findVehicleNearestScreenPosition(int x, int y)
{
// find the direction from the camera position to the given pixel
Vector3 direction = DirectionFromCameraToScreenPosition(x, y);
// iterate over all vehicles to find the one whose center is nearest the
// "eye-mouse" selection line
float minDistance = float.MaxValue; // smallest distance found so far
IVehicle nearest = null; // vehicle whose distance is smallest
List <IVehicle> vehicles = AllVehiclesOfSelectedPlugIn();
foreach (IVehicle vehicle in vehicles)
{
// distance from this vehicle's center to the selection line:
float d = Vector3Helpers.DistanceFromLine(vehicle.Position, Camera.Position, direction);
// if this vehicle-to-line distance is the smallest so far,
// store it and this vehicle in the selection registers.
if (d < minDistance)
{
minDistance = d;
nearest = vehicle;
}
}
return(nearest);
}
// ----------------------------------------------------------------------------
// 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);
}
public Vector3 xxxSteerForSeek(Vector3 target)
{
// const Vector3 offset = target - position;
Vector3 offset = target - this.Position;
Vector3 desiredVelocity = Vector3Helpers.TruncateLength(offset, this.MaxSpeed); //xxxnew
return(desiredVelocity - this.Velocity);
}
// 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;
}
}
// adjust the steering force passed to applySteeringForce.
// allows a specific vehicle class to redefine this adjustment.
// default is to disallow backward-facing steering at low speed.
// xxx experimental 8-20-02
public virtual Vector3 AdjustRawSteeringForce(Vector3 force, float deltaTime)
{
float maxAdjustedSpeed = 0.2f * MaxSpeed;
if ((Speed > maxAdjustedSpeed) || (force == Vector3.Zero))
{
return(force);
}
else
{
float range = Speed / maxAdjustedSpeed;
float cosine = Utilities.Interpolate((float)Math.Pow(range, 20), 1.0f, -1.0f);
return(Vector3Helpers.LimitMaxDeviationAngle(force, cosine, Forward));
}
}
// apply a given steering force to our momentum,
// adjusting our orientation to maintain velocity-alignment.
public void ApplySteeringForce(Vector3 force, float elapsedTime)
{
Vector3 adjustedForce = AdjustRawSteeringForce(force, elapsedTime);
// enforce limit on magnitude of steering force
Vector3 clippedForce = Vector3Helpers.TruncateLength(adjustedForce, MaxForce);
// compute acceleration and velocity
Vector3 newAcceleration = (clippedForce / Mass);
Vector3 newVelocity = Velocity;
// damp out abrupt changes and oscillations in steering acceleration
// (rate is proportional to time step, then clipped into useful range)
if (elapsedTime > 0)
{
float smoothRate = Utilities.Clip(9 * elapsedTime, 0.15f, 0.4f);
Utilities.BlendIntoAccumulator(smoothRate, newAcceleration, ref acceleration);
}
// Euler integrate (per frame) acceleration into velocity
newVelocity += acceleration * elapsedTime;
// enforce speed limit
newVelocity = Vector3Helpers.TruncateLength(newVelocity, MaxSpeed);
// update Speed
Speed = (newVelocity.Length);
// Euler integrate (per frame) velocity into position
Position = (Position + (newVelocity * elapsedTime));
// regenerate local space (by default: align vehicle's forward axis with
// new velocity, but this behavior may be overridden by derived classes.)
RegenerateLocalSpace(newVelocity, elapsedTime);
// maintain path curvature information
MeasurePathCurvature(elapsedTime);
// running average of recent positions
Utilities.BlendIntoAccumulator(elapsedTime * 0.06f, // QQQ
Position,
ref smoothedPosition);
}
// 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);
}
// set a random "2D" heading: set local Up to global Y, then effectively
// rotate about it by a random angle (pick random forward, derive side).
public void RandomizeHeadingOnXZPlane()
{
Up = Vector3.UnitY;
Forward = Vector3Helpers.RandomUnitVectorOnXZPlane();
Side = LocalRotateForwardToSide(Forward);
}
// General purpose circle/disk drawing routine. Draws circles or disks (as
// specified by "filled" argument) and handles both special case 2d circles
// on the XZ plane or arbitrary circles in 3d space (as specified by "in3d"
// argument)
public static void DrawCircleOrDisk(float radius, Vector3 axis, Vector3 center, Color color, int segments, bool filled, bool in3d)
{
if (Demo.IsDrawPhase == true)
{
LocalSpace ls = new LocalSpace();
if (in3d)
{
// define a local space with "axis" as the Y/up direction
// (XXX should this be a method on LocalSpace?)
Vector3 unitAxis = axis;
unitAxis.Normalize();
Vector3 unitPerp = Vector3Helpers.FindPerpendicularIn3d(axis);
unitPerp.Normalize();
ls.Up = unitAxis;
ls.Forward = unitPerp;
ls.Position = (center);
ls.SetUnitSideFromForwardAndUp();
}
// make disks visible (not culled) from both sides
if (filled)
{
BeginDoubleSidedDrawing();
}
// point to be rotated about the (local) Y axis, angular step size
Vector3 pointOnCircle = new Vector3(radius, 0, 0);
float step = (float)(2 * Math.PI) / (float)segments;
// set drawing color
SetColor(color);
// begin drawing a triangle fan (for disk) or line loop (for circle)
drawBegin(filled ? PrimitiveType.TriangleFan : PrimitiveType.LineStrip);
// for the filled case, first emit the center point
if (filled)
{
AddVertex(in3d ? ls.Position : center);
}
// rotate p around the circle in "segments" steps
float sin = 0, cos = 0;
int vertexCount = filled ? segments + 1 : segments;
for (int i = 0; i < vertexCount; i++)
{
// emit next point on circle, either in 3d (globalized out
// of the local space), or in 2d (offset from the center)
AddVertex(in3d ? ls.GlobalizePosition(pointOnCircle) : pointOnCircle + center);
// rotate point one more step around circle
pointOnCircle = Vector3Helpers.RotateAboutGlobalY(pointOnCircle, step, ref sin, ref cos);
}
// close drawing operation
drawEnd();
if (filled)
{
EndDoubleSidedDrawing();
}
}
else
{
DeferredCircle.AddToBuffer(radius, axis, center, color, segments, filled, in3d);
}
}
