/// <summary>
/// Return a premultiplication matrix required to place the input system of axes in the same coordinate
/// system as the base coordinate system from which all subunit coordinate systems derive.
/// </summary>
/// <param name="move1"></param>
/// <param name="move2"></param>
/// <param name="stay1"></param>
/// <param name="stay2"></param>
/// <returns></returns>
public static bool TryGetTwoAxisAlignmentMatrix(Line move1, Line move2, Line stay1, Line stay2, out Matrix matrix, float maxError = 0.2f)
{
if (Line.GetDistance(move1, move2) > maxError)
{
Console.WriteLine("Failed to align axes within {0:F4} angstroms, distance was {1:F4}).", maxError, Line.GetDistance(move1, move2));
matrix = Matrix.Identity;
return(false);
}
// Create working copies of the lines to be moved and make sure they have the same relative directionality as stationary axes
// by switching directions if necessary
LineTrackingCoordinateSystem axisCopy1 = LineTrackingCoordinateSystem.CreateFromPointDirection(move1.Point, move1.Direction);
LineTrackingCoordinateSystem axisCopy2 = LineTrackingCoordinateSystem.CreateFromPointDirection(move2.Point, move2.Direction);
if (Vector3.Dot(stay1.Direction, stay2.Direction) * Vector3.Dot(axisCopy1.Direction, axisCopy2.Direction) < 0)
{
axisCopy2.OriginalDirection *= -1;
}
// Track rotations and translations to align axes to principal axes
Quaternion alignAxis1 = VectorMath.GetRotationQuaternion(axisCopy1.Direction, stay1.Direction);
axisCopy1.ApplyRotation(alignAxis1);
axisCopy2.ApplyRotation(alignAxis1);
float angleError2 = VectorMath.GetDihedralAngleRadians(axisCopy2.Direction, axisCopy1.Direction, stay2.Direction);
Quaternion alignAxis2 = Quaternion.CreateFromAxisAngle(axisCopy1.Direction, -angleError2);
axisCopy1.ApplyRotation(alignAxis2);
axisCopy2.ApplyRotation(alignAxis2);
Vector3 intersect = Line.GetNearestPointOnLine(Line.CreateFrom(axisCopy1), Line.CreateFrom(axisCopy2));
axisCopy1.ApplyTranslation(-intersect);
axisCopy2.ApplyTranslation(-intersect);
matrix = axisCopy1.Transform;
//Debug.Assert((stay1.Direction - Vector3.Transform(move1.Direction, matrix.Rotation)).Length() < 0.1f);
//Debug.Assert((stay2.Direction - Vector3.Transform(move2.Direction, matrix.Rotation)).Length() < 0.1f);
return(true);
}