public override bool GetLocalRealignmentWithTwoRotationCenters(LineTrackingCoordinateSystem axis1, Vector3 center1, LineTrackingCoordinateSystem axis2, Vector3 center2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees)
{
Vector3 dir1 = axis1.Direction;
Vector3 dir2 = axis2.Direction;
if (VectorMath.GetAngleRadians(dir1, dir2) > Math.PI)
{
dir1 = -dir1;
}
Vector3 average = Vector3.Normalize(dir1 + dir2);
Matrix mat1 = VectorMath.GetRotationMatrix(dir1, average);
mat1.Translation += center1 - Vector3.Transform(center1, mat1);
Matrix mat2 = VectorMath.GetRotationMatrix(dir2, average);
mat2.Translation += center2 - Vector3.Transform(center2, mat2);
realignedAxis1 = new LineTrackingCoordinateSystem(axis1);
realignedAxis2 = new LineTrackingCoordinateSystem(axis2);
realignedAxis1.ApplyTransform(mat1);
realignedAxis2.ApplyTransform(mat2);
errorDegrees = (float)VectorMath.GetAngleDegrees(dir1, average);
return(true);
}
public override bool GetGlobalRealignment(Line local1, Line local2, out Matrix matrix)
{
LineTrackingCoordinateSystem cs1 = new LineTrackingCoordinateSystem(local1);
LineTrackingCoordinateSystem cs2 = new LineTrackingCoordinateSystem(local2);
if (VectorMath.GetAngleRadians(local1.Direction, local2.Direction) > 0.001)
{
matrix = Matrix.Identity;
return(false);
}
// Position the assembly so that
// 1) the axes point in the same direction as Axis1
Quaternion rotation1 = VectorMath.GetRotationQuaternion(local1.Direction, Axis1.Direction);
cs1.ApplyRotation(rotation1);
cs2.ApplyRotation(rotation1);
// 2) axis1 local and desired overlap
Vector3 translateToAxis1 = -cs1.Point;
cs1.Translation += translateToAxis1;
cs2.Translation += translateToAxis1;
// 3) axis2 local is offset from Axis1 in the same direction as Axis2. The scaling is taken care of later.
float angleDegrees = (float)VectorMath.GetDihedralAngleDegrees(cs2.Point, Axis1.Point, Axis1.Point + 50 * Axis1.Direction, Axis2.Point);
cs2.ApplyRotationDegrees(Line.CreateFrom(cs1), angleDegrees);
matrix = cs2.Transform;
return(true);
}
public override bool GetGlobalRealignment(Line local1, Line local2, out Matrix matrix)
{
// Verify that local1 and local2 overlap
if (VectorMath.GetAngleRadians(local1.Direction, local2.Direction) > 0.001)
{
matrix = Matrix.Identity;
return(false);
}
// Move the line so that it intersects the origin and then rotate it such that it points in the Z direction
LineTrackingCoordinateSystem cs1 = new LineTrackingCoordinateSystem(local1);
cs1.Translation -= cs1.Origin;
cs1.ApplyRotation(VectorMath.GetRotationQuaternion(cs1.Direction, Vector3.UnitZ));
matrix = cs1.Transform;
return(true);
}
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 static bool GetCoordinateSystemsAfterAlignmentToAngleDegrees2(CoordinateSystem unalignedSystem1, Vector3 rotationCenter1, CoordinateSystem unalignedSystem2, Vector3 rotationCenter2, float degrees, out CoordinateSystem alignedSystem1, out CoordinateSystem alignedSystem2, out float errorDegrees)
{
#if DEBUG
//ValidateGeometryUtilities();
#endif
// Copy the original coordinate systems as a starting point for the aligned systems
alignedSystem1 = new CoordinateSystem(unalignedSystem1);
alignedSystem2 = new CoordinateSystem(unalignedSystem2);
// Track the lever lengths throughout to ensure coordinate system origin w.r.t. rotation coordinates remains the same
float lengthORC1 = (rotationCenter1 - alignedSystem1.Translation).Length();
float lengthORC2 = (rotationCenter2 - alignedSystem2.Translation).Length();
// Fix the rotation angle
{
Quaternion localRotation = VectorMath.GetLocalRotation(alignedSystem1.Rotation, alignedSystem2.Rotation);
float desiredRad = (float)VectorMath.DegreesToRad(degrees);
float currentRad;
Vector3 uaxis = VectorMath.GetRotationVector(localRotation); // axis of rotation unit vector
Vector3 uAxis;
VectorMath.GetRotationVectorAndAngleRadians(localRotation, out uAxis, out currentRad);
//uAxis = Vector3.Transform(localRotation, alignedSystem1.Rotation);
float delta = (desiredRad - currentRad) / 2; // divide by two because both systems will have the same rotation applied
Quaternion fix1 = Quaternion.CreateFromAxisAngle(uAxis, -delta);
Quaternion fix2 = Quaternion.CreateFromAxisAngle(uAxis, delta);
alignedSystem1.RotateLocal(rotationCenter1, fix1);
alignedSystem2.RotateLocal(rotationCenter2, fix2);
//Debug.Assert(Vector3.Dot(uAxis, VectorMath.GetRotationVector(Quaternion.Normalize(VectorMath.GetQuaternion(alignedSystem1.Rotation, alignedSystem2.Rotation)))) > 0.99);
Debug.Assert(Math.Abs(Vector3.Dot(uAxis, VectorMath.GetRotationVector(Quaternion.Normalize(VectorMath.GetLocalRotation(alignedSystem1.Rotation, alignedSystem2.Rotation))))) > 0.99);
Debug.Assert(Math.Abs(lengthORC1 - (rotationCenter1 - alignedSystem1.Translation).Length()) < 0.1);
Debug.Assert(Math.Abs(lengthORC2 - (rotationCenter2 - alignedSystem2.Translation).Length()) < 0.1);
#if DEBUG
float newDegrees = (float)VectorMath.GetRotationAngleDegrees(alignedSystem1.Rotation, alignedSystem2.Rotation);
Debug.Assert(float.IsNaN(newDegrees) || Math.Abs(newDegrees - degrees) < 1 || Math.Abs(360 - newDegrees - degrees) < 1);
float deltaDegrees = (float)VectorMath.RadToDegrees(delta);
#endif
}
// Apply a second rotation - **the same rotation** - to both coordinate systems s.t. their origins project onto the same
// point on the new axis. This eliminates translation and allows a straight up measurement of rotation angle delta and also eliminates
// the need for moving the spacer via RMSD-minimization alignment with the moved other two systems.
{
Quaternion rotation = VectorMath.GetLocalRotation(alignedSystem1.Rotation, alignedSystem2.Rotation);
Vector3 uAxis;
float rad;
VectorMath.GetRotationVectorAndAngleRadians(rotation, out uAxis, out rad);
Vector3 vX12 = alignedSystem2.Translation - alignedSystem1.Translation;
Vector3 vRC12 = rotationCenter2 - rotationCenter1;
Vector3 uRC12 = Vector3.Normalize(vRC12);
float lRC12 = vRC12.Length();
float xHeightAlongAxis = Vector3.Dot(vX12, uAxis);
float rcHeightAlongAxis = Vector3.Dot(vRC12, uAxis);
float leverHeightAlongAxis = xHeightAlongAxis - rcHeightAlongAxis;
Debug.Assert(Math.Abs(Vector3.Dot((alignedSystem2.Translation - rotationCenter2) - (alignedSystem1.Translation - rotationCenter1), uAxis) - leverHeightAlongAxis) < 0.1);
Debug.Assert(VectorMath.GetRotationVector(VectorMath.GetLocalRotation(alignedSystem1.Rotation, alignedSystem2.Rotation)) == uAxis);
if (lRC12 < Math.Abs(leverHeightAlongAxis))
{
// No change in direction of the rotation axis can place the two aligned systems at the same height, because one lever arm
// is so much longer than the other
errorDegrees = -1;
return(false);
}
// Figure out the angle between axis and rc1->rc2 that will make the rc1->rc2 projection cancel out the lever arm projection
float desiredRcHeightAlongAxis = -leverHeightAlongAxis;
float radDesired = (float)Math.Acos(desiredRcHeightAlongAxis / lRC12);
Quaternion rotationOption1 = Quaternion.CreateFromAxisAngle(Vector3.Normalize(Vector3.Cross(uRC12, uAxis)), radDesired);
Quaternion rotationOption2 = Quaternion.CreateFromAxisAngle(Vector3.Normalize(Vector3.Cross(uRC12, uAxis)), -radDesired);
Vector3 axisOption1 = Vector3.Transform(uRC12, rotationOption1);
Vector3 axisOption2 = Vector3.Transform(uRC12, rotationOption2);
Vector3 uAxisFinal = Vector3.Distance(uAxis, axisOption1) < Vector3.Distance(uAxis, axisOption2)? axisOption1 : axisOption2;
float angle = (float)VectorMath.GetAngleRadians(uAxis, uAxisFinal);
Quaternion quat = Quaternion.CreateFromAxisAngle(Vector3.Normalize(Vector3.Cross(uAxis, uAxisFinal)), angle);
if (Vector3.Dot(Vector3.Transform(uAxis, quat), uAxisFinal) < 0.99)
{
throw new Exception();
}
//Quaternion alternateRotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(Vector3.Cross(uAxis, uRC12)), angle);
Quaternion rotationToMakePlanar = quat; // VectorMath.GetRotationQuaternion(uAxis, uAxisFinal);
if (Vector3.Dot(Vector3.Transform(uAxis, rotationToMakePlanar), uAxisFinal) < 0.99)
{
throw new Exception();
}
Vector3 rotationVector = VectorMath.GetRotationVector(rotationToMakePlanar);
if (Math.Abs(VectorMath.GetRotationAngleDegrees(rotationToMakePlanar)) < 0.1)
{
errorDegrees = (float)Math.Max(VectorMath.GetRotationAngleDegrees(unalignedSystem1.Rotation, alignedSystem1.Rotation), VectorMath.GetRotationAngleDegrees(unalignedSystem2.Rotation, alignedSystem2.Rotation));
Debug.Assert(Math.Abs(Vector3.Dot((alignedSystem2.Translation - rotationCenter2) - (alignedSystem1.Translation - rotationCenter1), uAxis) - leverHeightAlongAxis) < 0.5); // any rotation should preserve lever height along the axis
return(true);
}
// Do it
Debug.Assert(Vector3.Cross(rotationVector, uAxis).Length() > 0.99);
Debug.Assert(Math.Abs(Vector3.Dot(uAxis, VectorMath.GetRotationVector(Quaternion.Normalize(VectorMath.GetLocalRotation(alignedSystem1.Rotation, alignedSystem2.Rotation))))) > 0.99);
rotationToMakePlanar.W = -rotationToMakePlanar.W;
alignedSystem1.RotateLocal(rotationCenter1, rotationToMakePlanar);
alignedSystem2.RotateLocal(rotationCenter2, rotationToMakePlanar);
Vector3 uAxisQuat = Vector3.Transform(uAxis, rotationToMakePlanar);
uAxis = VectorMath.GetRotationVector(VectorMath.GetLocalRotation(alignedSystem1.Rotation, alignedSystem2.Rotation));
float newRadAxisRC12 = (float)VectorMath.GetAngleRadians(uRC12, uAxis);
//Debug.Assert(Math.Abs(newRadAxisRC12 - radDesired2) < 0.001);
//Debug.Assert(Math.Abs(Vector3.Dot((alignedSystem2.Translation - rotationCenter2) - (alignedSystem1.Translation - rotationCenter1), uAxis) - leverHeightAlongAxis) < 0.1); // any rotation should preserve lever height along the axis
#if DEBUG && BROKEN
uAxis = new Vector3(1, 2, 3);
uAxis.Normalize();
Quaternion testRotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(uAxis), (float)Math.PI / 4);
Console.WriteLine("PRE rc1->x1 dot uaxis = " + Vector3.Dot((alignedSystem1.Translation - rotationCenter1), uAxis));
Console.WriteLine("PRE rc2->x2 dot uaxis = " + Vector3.Dot((alignedSystem2.Translation - rotationCenter2), uAxis));
Console.WriteLine("PRE rc1->x1 distance = " + (alignedSystem1.Translation - rotationCenter1).Length());
Console.WriteLine("PRE rc2->x2 distance = " + (alignedSystem2.Translation - rotationCenter2).Length());
alignedSystem1.RotateRadians(rotationCenter1, uAxis, (float)Math.PI / 4);
alignedSystem2.RotateRadians(rotationCenter2, uAxis, (float)Math.PI / 4);
//alignedSystem1.Rotate(rotationCenter1, testRotation);
//alignedSystem2.Rotate(rotationCenter2, testRotation);
//uAxis = Vector3.Transform(uAxis, testRotation);
Console.WriteLine("POST rc1->x1 dot uaxis = " + Vector3.Dot((alignedSystem1.Translation - rotationCenter1), uAxis));
Console.WriteLine("POST rc2->x2 dot uaxis = " + Vector3.Dot((alignedSystem2.Translation - rotationCenter2), uAxis));
Console.WriteLine("POST rc1->x1 distance = " + (alignedSystem1.Translation - rotationCenter1).Length());
Console.WriteLine("POST rc2->x2 distance = " + (alignedSystem2.Translation - rotationCenter2).Length());
#endif
#if DEBUG
float newDegrees = (float)VectorMath.GetRotationAngleDegrees(alignedSystem1.Rotation, alignedSystem2.Rotation);
Debug.Assert(float.IsNaN(newDegrees) || Math.Abs(newDegrees - degrees) < 1 || Math.Abs(360 - newDegrees - degrees) < 1);
//float deltaDegrees = (float)VectorMath.RadToDegrees(delta);
#endif
Debug.Assert(Math.Abs(uAxis.Length() - 1) < 0.001);
Debug.Assert(Math.Abs(lengthORC1 - (rotationCenter1 - alignedSystem1.Translation).Length()) < 0.1);
Debug.Assert(Math.Abs(lengthORC2 - (rotationCenter2 - alignedSystem2.Translation).Length()) < 0.1);
//Debug.Assert(Math.Abs(Vector3.Dot((alignedSystem2.Translation - rotationCenter2) - (alignedSystem1.Translation - rotationCenter1), uAxis) - leverHeightAlongAxis) < 0.1); // any rotation should preserve lever height along the axis
if (Vector3.Dot(uAxis, uAxisFinal) < 0.99)
{
errorDegrees = -1;
return(false);
}
if (Math.Abs(Vector3.Dot(uAxis, alignedSystem2.Translation - alignedSystem1.Translation)) > 0.1)
{
errorDegrees = -1;
return(false);
}
Debug.Assert(Math.Abs(Vector3.Dot(uAxis, alignedSystem2.Translation) - Vector3.Dot(uAxis, alignedSystem1.Translation)) < 0.1);
errorDegrees = (float)Math.Max(VectorMath.GetRotationAngleDegrees(unalignedSystem1.Rotation, alignedSystem1.Rotation), VectorMath.GetRotationAngleDegrees(unalignedSystem2.Rotation, alignedSystem2.Rotation));
if (errorDegrees < 5)
{
return(true);
}
}
errorDegrees = (float)Math.Max(VectorMath.GetRotationAngleDegrees(unalignedSystem1.Rotation, alignedSystem1.Rotation), VectorMath.GetRotationAngleDegrees(unalignedSystem2.Rotation, alignedSystem2.Rotation));
return(true);
}