/// <summary>
/// Gets the middle two control points of an intersection given the starting and ending vectors
/// </summary>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <param name="startVec"></param>
/// <param name="endVec"></param>
/// <returns></returns>
/// <remarks>Start point in and End should also pouint it</remarks>
public static MiddleControlPoints MiddleIntersectionControlPoints(Coordinates exit,
Coordinates entry, Coordinates exitInVector, Coordinates entryInVector)
{
// equally normalize the heading vectos
entryInVector.Normalize();
exitInVector.Normalize();
// get the vector from the exit point to the entry point
Coordinates p = exit - entry;
// get the difference in heading vectors
Coordinates w = exitInVector - entryInVector;
// get the magnitude of the difference in headings
double d = w.Length;
// the time given that the headings represent velocities
double time2cpa = 0;
// do something
if (Math.Abs(d) > 1e-6)
{
time2cpa = -p.Dot(w) / Math.Pow(d, 2);
}
// get the middle control points
MiddleControlPoints mcps = new MiddleControlPoints(exit + exitInVector.Normalize(time2cpa),
entry + entryInVector.Normalize(time2cpa));
return(mcps);
}
/// <summary>
/// Gets the middle two control points of an intersection given the starting and ending vectors
/// </summary>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <param name="startVec"></param>
/// <param name="endVec"></param>
/// <returns></returns>
/// <remarks>Start point in and End should also pouint it</remarks>
public static MiddleControlPoints MiddleIntersectionControlPoints(Coordinates exit,
Coordinates entry, Coordinates exitInVector, Coordinates entryInVector)
{
// equally normalize the heading vectos
entryInVector.Normalize();
exitInVector.Normalize();
// get the vector from the exit point to the entry point
Coordinates p = exit - entry;
// get the difference in heading vectors
Coordinates w = exitInVector - entryInVector;
// get the magnitude of the difference in headings
double d = w.Length;
// the time given that the headings represent velocities
double time2cpa = 0;
// do something
if (Math.Abs(d) > 1e-6)
time2cpa = -p.Dot(w) / Math.Pow(d, 2);
// get the middle control points
MiddleControlPoints mcps = new MiddleControlPoints(exit + exitInVector.Normalize(time2cpa),
entry + entryInVector.Normalize(time2cpa));
return mcps;
}