public Vector3 SteerToFollowPathLinear(int direction, float predictionTime, GCRoute path)
{
// our goal will be offset from our path distance by this amount
float pathDistanceOffset = direction * predictionTime * Speed;
// predict our future position
Vector3 futurePosition = PredictFuturePosition(predictionTime);
// measure distance along path of our current and predicted positions
float nowPathDistance =
path.MapPointToPathDistance(Position);
// are we facing in the correction direction?
Vector3 pathHeading = path.TangentAt(Position) * (float)direction;
bool correctDirection = Vector3.Dot(pathHeading, Forward) > 0;
// find the point on the path nearest the predicted future position
// XXX need to improve calling sequence, maybe change to return a
// XXX special path-defined object which includes two Vector3s and a
// XXX bool (onPath,tangent (ignored), withinPath)
float futureOutside;
Vector3 onPath = path.MapPointToPath(futurePosition, out futureOutside);
// determine if we are currently inside the path tube
float nowOutside;
Vector3 nowOnPath = path.MapPointToPath(Position, out nowOutside);
// no steering is required if our present and future positions are
// inside the path tube and we are facing in the correct direction
float m = -Radius;
bool whollyInside = (futureOutside < m) && (nowOutside < m);
if (whollyInside && correctDirection)
{
// all is well, return zero steering
return Vector3.Zero;
}
else
{
// otherwise we need to steer towards a target point obtained
// by adding pathDistanceOffset to our current path position
// (reduce the offset if facing in the wrong direction)
float targetPathDistance = (nowPathDistance +
(pathDistanceOffset *
(correctDirection ? 1 : 0.1f)));
Vector3 target = path.MapPathDistanceToPoint(targetPathDistance);
// if we are on one segment and target is on the next segment and
// the dot of the tangents of the two segments is negative --
// increase the target offset to compensate the fold back
int ip = path.IndexOfNearestSegment(Position);
int it = path.IndexOfNearestSegment(target);
if (((ip + direction) == it) &&
(path.DotSegmentUnitTangents(it, ip) < -0.1f))
{
float newTargetPathDistance =
nowPathDistance + (pathDistanceOffset * 2);
target = path.MapPathDistanceToPoint(newTargetPathDistance);
}
AnnotatePathFollowing(futurePosition, onPath, target, futureOutside);
// if we are currently outside head directly in
// (QQQ new, experimental, makes it turn in more sharply)
if (nowOutside > 0) return SteerForSeek(nowOnPath);
// steering to seek target on path
Vector3 seek = Vector3Helpers.TruncateLength(SteerForSeek(target), MaxForce);
// return that seek steering -- except when we are heading off
// the path (currently on path and future position is off path)
// in which case we put on the brakes.
if ((nowOutside < 0) && (futureOutside > 0))
return (Vector3Helpers.PerpendicularComponent(seek, Forward) - (Forward * MaxForce));
else
return seek;
}
}
// Path following case for curved prediction and incremental steering
// (called from steerToFollowPath for the curvedSteering case)
//
// QQQ this does not handle the case when we AND futurePosition
// QQQ are outside, say when approach the path from far away
//
public Vector3 SteerToFollowPathCurve(int direction, float predictionTime, GCRoute path)
{
// predict our future position (based on current curvature and speed)
Vector3 futurePosition = PredictFuturePosition(predictionTime);
// find the point on the path nearest the predicted future position
float futureOutside;
Vector3 onPath = path.MapPointToPath(futurePosition, out futureOutside);
Vector3 pathHeading = path.TangentAt(onPath, direction);
Vector3 rawBraking = Forward * MaxForce * -1;
Vector3 braking = ((futureOutside < 0) ? Vector3.Zero : rawBraking);
//qqq experimental wrong-way-fixer
float nowOutside;
Vector3 nowTangent = Vector3.Zero;
Vector3 p = Position;
Vector3 nowOnPath = path.MapPointToPath(p, out nowTangent, out nowOutside);
nowTangent *= (float)direction;
float alignedness = Vector3.Dot(nowTangent, Forward);
// facing the wrong way?
if (alignedness < 0)
{
annotation.Line(p, p + (nowTangent * 10), Color.Cyan);
// if nearly anti-parallel
if (alignedness < -0.707f)
{
Vector3 towardCenter = nowOnPath - p;
Vector3 turn = (Vector3.Dot(towardCenter, Side) > 0 ?
Side * MaxForce :
Side * MaxForce * -1);
return (turn + rawBraking);
}
else
{
return (Vector3Helpers.PerpendicularComponent(SteerTowardHeading(pathHeading), Forward) + braking);
}
}
// is the predicted future position(+radius+margin) inside the path?
if (futureOutside < -(Radius + 1.0f)) //QQQ
{
// then no steering is required
return Vector3.Zero;
}
else
{
// otherwise determine corrective steering (including braking)
annotation.Line(futurePosition, futurePosition + pathHeading, Color.Red);
AnnotatePathFollowing(futurePosition, onPath,
Position, futureOutside);
// two cases, if entering a turn (a waypoint between path segments)
if (path.NearWaypoint(onPath) && (futureOutside > 0))
{
// steer to align with next path segment
annotation.Circle3D(0.5f, futurePosition, Up, Color.Red, 8);
return SteerTowardHeading(pathHeading) + braking;
}
else
{
// otherwise steer away from the side of the path we
// are heading for
Vector3 pathSide = LocalRotateForwardToSide(pathHeading);
Vector3 towardFP = futurePosition - onPath;
float whichSide = (Vector3.Dot(pathSide, towardFP) < 0) ? 1.0f : -1.0f;
return (Side * MaxForce * whichSide) + braking;
}
}
}