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; }