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 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); }
public void DetermineDurationOfTurnClockwiseComplex() { Angle start = new Angle(Math.PI - 0.1); Angle end = new Angle((Math.PI * 2) - 0.1); Angle rate = new Angle(-Math.PI / 10); TurnDirection turnDirection = TurnDirection.Clockwise; var path = new Path(); var turnDuration = path.DetermineDurationOfTurn(start, end, rate, turnDirection); Assert.AreEqual(10000, turnDuration); }
public void DetermineDurationOfTurnAntiClockwiseSimple() { Angle end = new Angle(Math.PI / 2); Angle start = new Angle((Math.PI * 2)); Angle rate = new Angle(Math.PI / 10); TurnDirection turnDirection = TurnDirection.AntiClockwise; var path = new Path(); var turnDuration = path.DetermineDurationOfTurn(start, end, rate, turnDirection); Assert.AreEqual(5000, turnDuration); }
public void DetermineTurnEndWestAntiClockwise() { Vector turningCircle = new Vector(2, 0); Vector destination = new Vector(-3, 0); double turningCircleRadius = 2; Vector turnPointToDestinationOffset = destination - turningCircle; Angle tangle = new Angle(turnPointToDestinationOffset); Angle cosangle = new Angle(Math.Acos(2.0 / 5.0)); Angle expectedAngle = tangle - cosangle; expectedAngle.ReduceAngle(); var path = new Path(); Assert.AreEqual(expectedAngle, path.DetermineTurnEnd(turnPointToDestinationOffset, turningCircleRadius, TurnDirection.AntiClockwise)); }
/// <summary> /// Determines if a turn is clockwise or anti clockwise /// </summary> /// <param name="velocity">The current velocity of the object</param> /// <param name="turiningCircleOffset">The position of the turning circle relative to the initial position</param> /// <returns>Clockwise or anti clockwise</returns> public TurnDirection DetermineTurnDirection(Vector velocity, Vector turiningCircleOffset) { int velocityFacing = Angle.FacingNumber(velocity); int offsetFacing = Angle.FacingNumber(turiningCircleOffset); TurnDirection turnDirection = TurnDirection.Unknown; if ((velocityFacing == (offsetFacing + 3)) || (velocityFacing == (offsetFacing - 1))) { turnDirection = TurnDirection.AntiClockwise; } else if ((velocityFacing == (offsetFacing - 3)) || (velocityFacing == (offsetFacing + 1))) { turnDirection = TurnDirection.Clockwise; } return(turnDirection); }
/// <summary> /// /// </summary> /// <param name="destination">The position of the destination relative to the turning point</param> /// <param name="radius">The radius of the turning circle</param> /// <param name="turnDirection">If the turn is clockwise or anti clockwise</param> /// <returns></returns> public Angle DetermineTurnEnd(Vector destination, double radius, TurnDirection turnDirection) { Angle angleTowardsDestination = new Angle(destination); Angle tangentAngles = new Angle(Math.Acos(radius / destination.Length)); Angle desiredEndPoint; if (turnDirection == TurnDirection.Clockwise) { desiredEndPoint = angleTowardsDestination + tangentAngles; } else { desiredEndPoint = angleTowardsDestination - tangentAngles; } desiredEndPoint.ReduceAngle(); return(desiredEndPoint); }
/// <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 ulong DetermineDurationOfTurn(Angle start, Angle end, Angle turnRate, TurnDirection turnDirection) { if (turnDirection == TurnDirection.Clockwise) { if (end > start) { end = new Angle(end.Value - (Math.PI * 2)); } } if (turnDirection == TurnDirection.AntiClockwise) { if (end < start) { end = new Angle(end.Value + (Math.PI * 2)); } } var turnAngle = start - end; var stuff = (turnAngle.Value / turnRate.Value) * -1000.0; return((ulong)(Math.Round(stuff))); }
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)); }
/// <summary> /// /// </summary> /// <param name="destination">The position of the destination relative to the turning point</param> /// <param name="radius">The radius of the turning circle</param> /// <param name="turnDirection">If the turn is clockwise or anti clockwise</param> /// <returns></returns> public Angle DetermineTurnEnd(Vector destination, double radius, TurnDirection turnDirection) { Angle angleTowardsDestination = new Angle(destination); Angle tangentAngles = new Angle(Math.Acos(radius / destination.Length)); Angle desiredEndPoint; if (turnDirection == TurnDirection.Clockwise) { desiredEndPoint = angleTowardsDestination + tangentAngles; } else { desiredEndPoint = angleTowardsDestination - tangentAngles; } desiredEndPoint.ReduceAngle(); return desiredEndPoint; }
public void DetermineDurationOfTurnAntiClockwiseComplex() { Angle end = new Angle(Math.PI + 0.1); Angle start = new Angle((Math.PI * 2) + 0.1); Angle rate = new Angle(Math.PI / 10); TurnDirection turnDirection = TurnDirection.AntiClockwise; var path = new Path(); var turnDuration = path.DetermineDurationOfTurn(start, end, rate, turnDirection); Assert.AreEqual(10000, turnDuration); }
public ulong DetermineDurationOfTurn(Angle start, Angle end, Angle turnRate, TurnDirection turnDirection) { if (turnDirection == TurnDirection.Clockwise) { if (end > start) { end = new Angle(end.Value - (Math.PI * 2)); } } if (turnDirection == TurnDirection.AntiClockwise) { if (end < start) { end = new Angle(end.Value + (Math.PI * 2)); } } var turnAngle = start - end; var stuff = (turnAngle.Value / turnRate.Value) * -1000.0; return (ulong)(Math.Round(stuff)); }
public Vector GetVelocity(ulong currentTime) { double vectorOffset; // 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 double 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); }
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; }
public void DetermineDurationOfTurnClockwiseSimple() { Angle start = new Angle(Math.PI / 2); Angle end = new Angle(); Angle rate = new Angle(-Math.PI / 10); TurnDirection turnDirection = TurnDirection.Clockwise; var path = new Path(); var turnDuration = path.DetermineDurationOfTurn(start, end, rate, turnDirection); Assert.AreEqual(5000, turnDuration); }
public void DetermineTurnEndWestClockwise() { Vector turningCircle = new Vector(2, 0); Vector destination = new Vector(-3, 0); double turningCircleRadius = 2; Vector turnPointToDestinationOffset = destination - turningCircle; Angle tangle = new Angle(turnPointToDestinationOffset); Angle cosangle = new Angle(Math.Acos(2.0 / 5.0)); Angle expectedAngle = tangle + cosangle; expectedAngle.ReduceAngle(); var path = new Path(); Assert.AreEqual(expectedAngle, path.DetermineTurnEnd(turnPointToDestinationOffset, turningCircleRadius, TurnDirection.Clockwise)); }
public static Vector2 RadialToVector2(Angle angle, double radius) { return(new Vector2((radius * Math.Cos(angle.Value)), (radius * Math.Sin(angle.Value)))); }