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