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