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