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); }
/// <summary> /// primary constructor for circular motion objects /// </summary> /// <param name="startTime">The begining time for the motion</param> /// <param name="radius">The radius of the turning circle</param> /// <param name="startAngle">The angle to the start point on the circle</param> /// <param name="turnRate">The turn rate in radians</param> /// <param name="initialSpeed">Speed used to determine the current velocity</param> /// <param name="initialPosition">The position when this motion started</param> public CircularMotion(ulong startTime, double radius, Angle startAngle, Angle turnRate, double initialSpeed, Vector initialPosition) { m_StartTime = startTime; m_Radius = radius; m_StartAngle = startAngle; m_TurnRate = turnRate; m_InitialSpeed = initialSpeed; m_CircleOffset = CoordinateConversions.RadialToVector(startAngle, m_Radius); m_InitialPosition = initialPosition; }
public Vector GetMotion(ulong currentTime) { double timeElapsed = (currentTime - m_StartTime); timeElapsed = timeElapsed / 1000.0; Angle angle = new Angle(m_StartAngle.Value + (m_TurnRate.Value * timeElapsed)); var positionOnCirlce = CoordinateConversions.RadialToVector(angle, m_Radius); return(positionOnCirlce - m_CircleOffset); }
/// <summary> /// Determines the positions of the two possible turning circles /// Relative to the initial position which is not passed into the function /// </summary> /// <param name="initialVelocity">The current velocity of the object</param> /// <param name="radius">The desired radius of the object</param> /// <param name="circleOne">The first of the two possible turning circles</param> /// <param name="circleTwo">The second of the two possible turning circles</param> public void DetermineTurningCircles(Vector initialVelocity, double radius, out Vector circleOne, out Vector circleTwo) { Angle velocityAngle; // calculate the angle of the current velocity velocityAngle = new Angle(initialVelocity); velocityAngle.ReduceAngle(); //double vesselSpeed = initialVelocity.Length; // calculate the radius if the turning circle based on the acceleration and current speed //double radius = CalcRadius(vesselSpeed, acceleration); // calcualte both of the turning circles Angle angleOne = new Angle(velocityAngle.Value + (Math.PI / 2)); circleOne = CoordinateConversions.RadialToVector(angleOne, radius); Angle angleTwo = new Angle(velocityAngle.Value - (Math.PI / 2)); circleTwo = CoordinateConversions.RadialToVector(angleTwo, radius); }
public Vector GetVelocity(ulong currentTime) { double vectorOffset; double timeElapsed = 0; // the vector is 90degrees from the angle of acceleration // the angle to the current position if (m_TurnRate >= new Angle(0.0)) { vectorOffset = Math.PI / 2; } else { vectorOffset = (-1 * (Math.PI / 2)); } // set the elapsed time to get an accurate resutlt timeElapsed = (currentTime - m_StartTime); timeElapsed = timeElapsed / 1000.0; Angle angle = new Angle(m_StartAngle.Value + (m_TurnRate.Value * timeElapsed) + vectorOffset); return(CoordinateConversions.RadialToVector(angle, m_InitialSpeed)); }