Beispiel #1
0
        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;
            }
        }
Beispiel #2
0
        // 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;
                }
            }
        }