private static MathVector ClosestPointOnAxis(ITriadManipulator m, IMathPoint p, IMathVector cameraVector, MathVector axis)
{
double pT;
double qT;
if (ClosestPointBetweenLines(m.Origin, axis, p, cameraVector, out pT, out qT))
{
return((MathVector)axis.Scale(pT));
}
else
{
return(null);
}
}
private static MathVector ClosestPointOnAxis(ITriadManipulator m, IMathPoint p, IMathVector cameraVector, MathVector axis)
{
double pT;
double qT;
if (ClosestPointBetweenLines(m.Origin, axis, p, cameraVector, out pT, out qT))
{
return (MathVector) axis.Scale(pT);
}
else
{
return null;
}
}
/// <summary>
/// Projects the vector between the inputed point and a point on the axis line and then scales
/// The axis to the projected length.
/// </summary>
/// <param name="axis">The axis vector that will be projected onto</param>
/// <param name="point">the point on the other axis</param>
/// <returns>A vector scaled to the projection</returns>
public static MathVector GetScaledVector(MathVector axis, double[] point1, double[] point2)
{
double[] tempArray = { point1[0] - point2[0], point1[1] - point2[1], point1[2] - point2[2] };
MathVector pointVector = RobotInfo.mathUtil.CreateVector(tempArray);//creates a vector between the point on the line and the origin of the joint
return axis.Scale(pointVector.Dot(axis));
}
