public List <IMotion> CreatePathTo(Vector destination, Vector initialVelocity, Vector initialPosition, ulong startTime)
        {
            // get initial velocity
            //determine turning circles
            Vector circleOnePosition, circleTwoPosition;

            double circleRadius = CalcRadius(initialVelocity.Length, m_Acceleration);

            DetermineTurningCircles(initialVelocity, circleRadius, out circleOnePosition, out circleTwoPosition);

            //select a turning circle

            Vector destinationRelativeToInitialPosition = destination - initialPosition;

            Vector selectedTuringCicle = SelectTuriningCircle(circleOnePosition, circleTwoPosition, destinationRelativeToInitialPosition, circleRadius);

            //determine turn direction
            TurnDirection turnDirection = DetermineTurnDirection(initialVelocity, selectedTuringCicle);

            //determine turn end
            Angle turnStart = new Angle(-selectedTuringCicle);
            // zero the destination around the turning circle
            var destinationRelativeToTurningCircle = destination - (initialPosition + selectedTuringCicle);

            //use the relative destination to pick an end point for the turn
            Angle turnEnd = DetermineTurnEnd(destinationRelativeToTurningCircle, circleRadius, turnDirection);

            Angle turnRate = DetermineTurnRate(initialVelocity.Length, circleRadius, turnDirection);

            var turnDuration = DetermineDurationOfTurn(turnStart, turnEnd, turnRate, turnDirection);

            turnDuration += startTime;
            // create circular motion

            var circle = new CircularMotion(startTime, circleRadius, turnStart, turnRate, initialVelocity.Length, initialPosition);

            Vector startOfLine = initialPosition + selectedTuringCicle + CoordinateConversions.RadialToVector(turnEnd, circleRadius);
            //Vector startOfLine = circle.GetCurrentPosition(turnDuration);
            // create linear motion

            var   velocity        = (destination - startOfLine);
            ulong destinationTime = (ulong)((velocity.Length / initialVelocity.Length) * 1000.0);

            velocity.Normalise();
            velocity = velocity * initialVelocity.Length;
            var linear  = new LinearMotion(turnDuration, velocity, startOfLine);
            var linear2 = new LinearMotion(turnDuration + destinationTime, velocity, destination);

            //Assert.AreEqual(circle.GetVelocity(turnDuration), linear.GetVelocity(turnDuration));
            List <IMotion> path = new List <IMotion>
            {
                circle,
                linear,
                linear2,
            };

            return(path);
        }
 public bool Equals(LinearMotion other)
 {
     if (ReferenceEquals(null, other))
     {
         return false;
     }
     if (ReferenceEquals(this, other))
     {
         return true;
     }
     return other.m_Velocity.Equals(m_Velocity)
             && other.m_StartTime == m_StartTime
             && other.m_InitialPosition.Equals(m_InitialPosition);
 }
Exemple #3
0
 public bool Equals(LinearMotion other)
 {
     if (ReferenceEquals(null, other))
     {
         return(false);
     }
     if (ReferenceEquals(this, other))
     {
         return(true);
     }
     return(other.m_Velocity.Equals(m_Velocity) &&
            other.m_StartTime == m_StartTime &&
            other.m_InitialPosition.Equals(m_InitialPosition));
 }
        public List<IMotion> CreatePathTo(Vector destination, Vector initialVelocity, Vector initialPosition, ulong startTime)
        {
            // get initial velocity
            //determine turning circles
            Vector circleOnePosition, circleTwoPosition;

            double circleRadius = CalcRadius(initialVelocity.Length, m_Acceleration);

            DetermineTurningCircles(initialVelocity, circleRadius, out circleOnePosition, out circleTwoPosition);

            //select a turning circle

            Vector destinationRelativeToInitialPosition = destination - initialPosition;

            Vector selectedTuringCicle = SelectTuriningCircle(circleOnePosition, circleTwoPosition, destinationRelativeToInitialPosition, circleRadius);

            //determine turn direction
            TurnDirection turnDirection = DetermineTurnDirection(initialVelocity, selectedTuringCicle);

            //determine turn end
            Angle turnStart = new Angle(-selectedTuringCicle);
            // zero the destination around the turning circle
            var destinationRelativeToTurningCircle = destination - (initialPosition + selectedTuringCicle);

            //use the relative destination to pick an end point for the turn
            Angle turnEnd = DetermineTurnEnd(destinationRelativeToTurningCircle, circleRadius, turnDirection);

            Angle turnRate = DetermineTurnRate(initialVelocity.Length, circleRadius, turnDirection);

            var turnDuration = DetermineDurationOfTurn(turnStart, turnEnd, turnRate, turnDirection);
            turnDuration += startTime;
            // create circular motion

            var circle = new CircularMotion(startTime, circleRadius, turnStart, turnRate, initialVelocity.Length, initialPosition);

            Vector startOfLine = initialPosition + selectedTuringCicle + CoordinateConversions.RadialToVector(turnEnd, circleRadius);
            //Vector startOfLine = circle.GetCurrentPosition(turnDuration);
            // create linear motion

            var velocity = (destination - startOfLine);
            ulong destinationTime = (ulong)((velocity.Length / initialVelocity.Length) * 1000.0);
            velocity = velocity.Normalise();
            velocity = velocity * initialVelocity.Length;
            var linear = new LinearMotion(turnDuration, velocity, startOfLine);
            var linear2 = new LinearMotion(turnDuration + destinationTime, velocity, destination);

            //Assert.AreEqual(circle.GetVelocity(turnDuration), linear.GetVelocity(turnDuration));
            List<IMotion> path = new List<IMotion>
            {
                circle,
                linear,
                linear2,
            };
            return path;
        }