void DrawPathFencesOnMap(TerrainMap map, GCRoute path)
{
float xs = map.xSize / (float)map.resolution;
float zs = map.zSize / (float)map.resolution;
Vector3 alongRow = new Vector3(xs, 0, 0);
Vector3 nextRow = new Vector3(-map.xSize, 0, zs);
Vector3 g = new Vector3((map.xSize - xs) / -2, 0, (map.zSize - zs) / -2);
for (int j = 0; j < map.resolution; j++)
{
for (int i = 0; i < map.resolution; i++)
{
float outside = path.HowFarOutsidePath(g);
float wallThickness = 1.0f;
// set map cells adjacent to the outside edge of the path
if ((outside > 0) && (outside < wallThickness))
{
map.SetMapBit(i, j, true);
}
// clear all other off-path map cells
if (outside > wallThickness)
{
map.SetMapBit(i, j, false);
}
g += alongRow;
}
g += nextRow;
}
}
public override void HandleFunctionKeys(Keys key)
{
switch (key)
{
case Keys.F1: SelectNextDemo(); break;
case Keys.F2: ReversePathFollowDirection(); break;
case Keys.F3: TogglePathFences(); break;
case Keys.F4: ToggleRandomRocks(); break;
case Keys.F5: ToggleCurvedSteering(); break;
case Keys.F6: // QQQ draw an enclosed "pen" of obstacles to test cycle-stuck
{
float m = MapDriver.worldSize * 0.4f; // main diamond size
float n = MapDriver.worldSize / 8; // notch size
Vector3 q = new Vector3(0, 0, m - n);
Vector3 s = new Vector3(2 * n, 0, 0);
Vector3 c = s - q;
Vector3 d = s + q;
int pathPointCount = 2;
float[] pathRadii = new float[] { 10, 10 };
Vector3[] pathPoints = new Vector3[] { c, d };
GCRoute r = new GCRoute(pathPointCount, pathPoints, pathRadii, false);
DrawPathFencesOnMap(vehicle.map, r);
break;
}
}
}
public override void HandleFunctionKeys(Keys key)
{
switch (key)
{
case Keys.F1: SelectNextDemo(); break;
case Keys.F2: ReversePathFollowDirection(); break;
case Keys.F3: TogglePathFences(); break;
case Keys.F4: ToggleRandomRocks(); break;
case Keys.F5: ToggleCurvedSteering(); break;
case Keys.F6: // QQQ draw an enclosed "pen" of obstacles to test cycle-stuck
{
float m = MapDriver.worldSize * 0.4f; // main diamond size
float n = MapDriver.worldSize / 8; // notch size
Vector3 q = new Vector3(0, 0, m - n);
Vector3 s = new Vector3(2 * n, 0, 0);
Vector3 c = s - q;
Vector3 d =s + q;
int pathPointCount = 2;
float[] pathRadii = new float[] { 10, 10 };
Vector3[] pathPoints = new Vector3[] { c, d };
GCRoute r = new GCRoute(pathPointCount, pathPoints, pathRadii, false);
DrawPathFencesOnMap(vehicle.map, r);
break;
}
}
}
void DrawPathFencesOnMap(TerrainMap map, GCRoute path)
{
float xs = map.xSize / (float)map.resolution;
float zs = map.zSize / (float)map.resolution;
Vector3 alongRow = new Vector3(xs, 0, 0);
Vector3 nextRow = new Vector3(-map.xSize, 0, zs);
Vector3 g = new Vector3((map.xSize - xs) / -2, 0, (map.zSize - zs) / -2);
for (int j = 0; j < map.resolution; j++)
{
for (int i = 0; i < map.resolution; i++)
{
float outside = path.HowFarOutsidePath(g);
float wallThickness = 1.0f;
// set map cells adjacent to the outside edge of the path
if ((outside > 0) && (outside < wallThickness))
map.SetMapBit(i, j, true);
// clear all other off-path map cells
if (outside > wallThickness) map.SetMapBit(i, j, false);
g += alongRow;
}
g += nextRow;
}
}
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;
}
}
}
// this is a version of the one in SteerLibrary.h modified for "slow when
// heading off path". I put it here because the changes were not
// compatible with Pedestrians.cpp. It needs to be merged back after
// things settle down.
//
// its been modified in other ways too (such as "reduce the offset if
// facing in the wrong direction" and "increase the target offset to
// compensate the fold back") plus I changed the type of "path" from
// Pathway to GCRoute to use methods like indexOfNearestSegment and
// dotSegmentUnitTangents
//
// and now its been modified again for curvature-based prediction
//
public Vector3 SteerToFollowPath(int direction, float predictionTime, GCRoute path)
{
if (curvedSteering)
return SteerToFollowPathCurve(direction, predictionTime, path);
else
return SteerToFollowPathLinear(direction, predictionTime, path);
}
// constructor
public MapDriver()
{
map = MakeMap();
path = MakePath();
Reset();
// to compute mean time between collisions
sumOfCollisionFreeTimes = 0;
countOfCollisionFreeTimes = 0;
// keep track for reliability statistics
collisionLastTime = false;
timeOfLastCollision = Demo.Clock.TotalSimulationTime;
// keep track of average speed
totalDistance = 0;
totalTime = 0;
// keep track of path following failure rate
pathFollowTime = 0;
pathFollowOffTime = 0;
// innitialize counters for various performance data
stuckCount = 0;
stuckCycleCount = 0;
stuckOffPathCount = 0;
lapsStarted = 0;
lapsFinished = 0;
hintGivenCount = 0;
hintTakenCount = 0;
// follow the path "upstream or downstream" (+1/-1)
pathFollowDirection = 1;
// use curved prediction and incremental steering:
curvedSteering = true;
incrementalSteering = true;
}