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