public static bool GetAxesAfterAlignmentToAngleDegrees(LineTrackingCoordinateSystem axis1, Vector3 rotationCenter, LineTrackingCoordinateSystem axis2, float angleDegrees, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float angleErrorDegrees)
{
float angleRadians = (float)(angleDegrees * Math.PI / 180);
float angleErrorRadians = 0;
bool result = GetAxesAfterAlignmentToAngleRadians(axis1, rotationCenter, axis2, rotationCenter, angleRadians, out realignedAxis1, out realignedAxis2, out angleErrorRadians);
angleErrorDegrees = (float)(angleErrorRadians * 180 / Math.PI);
return(result);
}
public static LineTrackingCoordinateSystem CreateFromPointDirection(Vector3 point, Vector3 direction)
{
LineTrackingCoordinateSystem line = new LineTrackingCoordinateSystem();
line.OriginalPoint = point;
line.OriginalDirection = direction;
line.OriginalDirection.Normalize();
return(line);
}
public static bool Equals(LineTrackingCoordinateSystem one, LineTrackingCoordinateSystem two, float rotationDegreesMax, float translationMax)
{
float degrees = CoordinateSystem.GetRotationAngleViaQuaternionsInDegrees(one, two);
if (degrees < rotationDegreesMax && Vector3.DistanceSquared(one.Point, two.Point) < translationMax * translationMax)
{
return(true);
}
return(false);
}
/// <summary>
///
/// </summary>
/// <param name="other"></param>
/// <param name="identity">Whether the transform matrix should be identity, vs copied from the other</param>
public LineTrackingCoordinateSystem(LineTrackingCoordinateSystem other, bool identity)
{
if (identity)
{
OriginalDirection = other.Direction;
OriginalPoint = other.Point;
}
else
{
OriginalDirection = other.OriginalDirection;
OriginalPoint = other.OriginalPoint;
base.Copy((CoordinateSystem)other);
}
}
/// <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);
}
public static Line CreateFrom(LineTrackingCoordinateSystem other)
{
return(Line.CreateFromPointAndDirection(other.Point, other.Direction));
}
public static bool GetAxesAfterAlignmentToAngleRadians(LineTrackingCoordinateSystem axis1, Vector3 rotationCenter1, LineTrackingCoordinateSystem axis2, Vector3 rotationCenter2, float angleRadians, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float angleErrorRadians)
{
realignedAxis1 = null;
realignedAxis2 = null;
angleErrorRadians = float.NaN;
// Find the midpoint of where the lines are nearest to one another
// Rotate the lines such that the point at the same distance from the rotation center on each line is now coincident with the midpoint
// Rotate the second line about the normal from the center to plane formed by both lines by the desired number of degrees (i think that works and is def. equivalent to rotating both separately)
// midpoint
Line line1 = Line.CreateFrom(axis1);
Line line2 = Line.CreateFrom(axis2);
Vector3 midpoint = Line.GetMidpoint(line1, line2);
// Find points on each line at the same distance from the rotation center as the rotation center is from the midpoint
float distance1 = Vector3.Distance(rotationCenter1, midpoint);
float distance2 = Vector3.Distance(rotationCenter2, midpoint);
Vector3 neighbor11;
Vector3 neighbor12;
Line.GetPointsOnLineAtDistance(line1, rotationCenter1, distance1, out neighbor11, out neighbor12);
Vector3 point1 = VectorMath.GetNearestPoint(midpoint, neighbor11, neighbor12);
Vector3 neighbor21;
Vector3 neighbor22;
Line.GetPointsOnLineAtDistance(line2, rotationCenter2, distance2, out neighbor21, out neighbor22);
Vector3 point2 = VectorMath.GetNearestPoint(midpoint, neighbor21, neighbor22);
if (VectorMath.IsNaN(point1) || VectorMath.IsNaN(point2))
{
return(false);
}
// Rotate each line1 and line2 s.t. point1 and point2 are coincident with the midpoint
realignedAxis1 = new LineTrackingCoordinateSystem(axis1);//.CreateFromPointDirection(line1.Point, line1.Direction);
realignedAxis1.ApplyTranslation(-rotationCenter1);
realignedAxis1.ApplyRotation(VectorMath.GetRotationQuaternion(Vector3.Normalize(point1 - rotationCenter1), Vector3.Normalize(midpoint - rotationCenter1)));
realignedAxis1.ApplyTranslation(rotationCenter1);
realignedAxis2 = new LineTrackingCoordinateSystem(axis2);//.CreateFromPointDirection(line2.Point, line2.Direction);
realignedAxis2.ApplyTranslation(-rotationCenter2);
realignedAxis2.ApplyRotation(VectorMath.GetRotationQuaternion(Vector3.Normalize(point2 - rotationCenter2), Vector3.Normalize(midpoint - rotationCenter2)));
realignedAxis2.ApplyTranslation(rotationCenter2);
if (Line.GetDistance(Line.CreateFrom(realignedAxis1), Line.CreateFrom(realignedAxis2)) > 0.5f)
{
// floating point error
return(false);
}
// Rotate lines about the plane normal to achieve the desired angle between
float currentAngleRadians = (float)VectorMath.GetAngleRadians(realignedAxis1.Point, midpoint, realignedAxis2.Point);
float requiredDelta = (angleRadians - currentAngleRadians) / 2;
Vector3 rotationAxisDirection = Vector3.Cross(Vector3.Normalize(realignedAxis1.Point - midpoint), Vector3.Normalize(realignedAxis2.Point - midpoint));
Line rotationAxis1 = Line.CreateFromPointAndDirection(rotationCenter1, -rotationAxisDirection);
Line rotationAxis2 = Line.CreateFromPointAndDirection(rotationCenter2, -rotationAxisDirection);
realignedAxis1.ApplyRotationRadians(rotationAxis1, requiredDelta);
realignedAxis2.ApplyRotationRadians(rotationAxis2, -requiredDelta);
float newAngleRadians = (float)VectorMath.GetAngleRadians(realignedAxis1.Direction, realignedAxis2.Direction);
if (Line.GetDistance(Line.CreateFrom(realignedAxis1), Line.CreateFrom(realignedAxis2)) >= 1)
{
// TODO: This should never happen - debug why it is
return(false);
}
angleErrorRadians = Math.Abs(requiredDelta);
return(true);
}
public bool Equals(LineTrackingCoordinateSystem other, float rotationDegreesMax, float translationMax)
{
return(LineTrackingCoordinateSystem.Equals(this, other, rotationDegreesMax, translationMax));
}
public LineTrackingCoordinateSystem(LineTrackingCoordinateSystem other)
: base(other)
{
OriginalDirection = other.OriginalDirection;
OriginalPoint = other.OriginalPoint;
}
