private static void ValidateGeometryUtilities()
{
for (int i = 0; i < 10; i++)
{
// Check the proper quaternion is generated to go 'from'->'to'
Random r = new Random();
Vector3 v1 = Vector3.Normalize(new Vector3(r.Next(), r.Next(), r.Next()));
Vector3 v2 = Vector3.Normalize(new Vector3(r.Next(), r.Next(), r.Next()));
float angle1 = r.Next() % 360;
float angle2 = r.Next() % 360;
if (angle1 == 0 || angle2 == 0)
{
continue;
}
Quaternion from = Quaternion.CreateFromAxisAngle(v1, (float)VectorMath.DegreesToRad(angle1));
Quaternion to = Quaternion.CreateFromAxisAngle(v2, (float)VectorMath.DegreesToRad(angle2));
Quaternion transformQuat = VectorMath.GetLocalRotation(from, to);
Quaternion fromTo = from * transformQuat;
//Quaternion fromTo = from;
float newAngleBetween = VectorMath.GetAngleDegrees(to, fromTo);
Debug.Assert(Math.Abs(newAngleBetween) < 5);
float verifyAngle1;
Vector3 verifyAxis1;
VectorMath.GetRotationVectorAndAngleDegrees(from, out verifyAxis1, out verifyAngle1);
Debug.Assert(Vector3.Dot(verifyAxis1, v1) > 0.99 && Math.Abs(verifyAngle1 - angle1) < 1 || Vector3.Dot(verifyAxis1, v1) < -0.99 && Math.Abs(360 - verifyAngle1 - angle1) < 1);
//Debug.Assert( <= 2);
Quaternion verifyFrom = VectorMath.GetLocalRotation(Quaternion.Identity, from);
VectorMath.GetRotationVectorAndAngleDegrees(verifyFrom, out verifyAxis1, out verifyAngle1);
Debug.Assert(Vector3.Dot(verifyAxis1, v1) > 0.99 && Math.Abs(verifyAngle1 - angle1) < 1 || Vector3.Dot(verifyAxis1, v1) < -0.99 && Math.Abs(360 - verifyAngle1 - angle1) < 1);
}
{
Quaternion rotation1 = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)Math.PI / 2);
CoordinateSystem cs = new CoordinateSystem();
cs.Translation = new Vector3(1, 2, 3);
Console.WriteLine("PRE = " + cs.Translation.ToString());
cs.RotateLocal(new Vector3(-1, -2, -3), rotation1);
Console.WriteLine("POST = " + cs.Translation.ToString());
}
{
for (int i = 0; i < 10; i++)
{
Random r = new Random();
Vector3 v1 = Vector3.Normalize(new Vector3(r.Next(), r.Next(), r.Next()));
Vector3 v2 = Vector3.Normalize(new Vector3(r.Next(), r.Next(), r.Next()));
Quaternion quat = VectorMath.GetRotationQuaternion(v1, v2);
Debug.Assert(Vector3.Dot(Vector3.Transform(v1, quat), v2) > 0.99);
}
}
}
public bool GetGlobalRealignmentToZ(CoordinateSystem cs1, CoordinateSystem cs2, out Matrix matrix)
{
Vector3 axis;
float angleDegrees;
//Quaternion rotation = VectorMath.GetLocalRotation(cs1.Rotation, cs2.Rotation);
Quaternion rotation = VectorMath.GetGlobalRotation(cs1.Rotation, cs2.Rotation);
VectorMath.GetRotationVectorAndAngleDegrees(rotation, out axis, out angleDegrees);
Vector3 v12 = cs2.Translation - cs1.Translation;
Vector3 u12 = Vector3.Normalize(v12);
Vector3 xMidpoint = (cs1.Translation + cs2.Translation) / 2;
float midpointOffset = (float)(v12.Length() / 2 / Math.Tan(VectorMath.DegreesToRad(Angle) / 2));
Vector3 center = Vector3.Normalize(Vector3.Cross(axis, u12)) * midpointOffset + xMidpoint;
LineTrackingCoordinateSystem line = LineTrackingCoordinateSystem.CreateFromPointDirection(center, axis);
line.ApplyTranslation(-center);
Debug.Assert(Math.Abs(Vector3.Dot(cs2.Translation, axis) - Vector3.Dot(cs1.Translation, axis)) < 0.1);
//float rad = (float) VectorMath.GetAngleRadians(line.Direction, Vector3.UnitZ);
//Vector3 rotationAxis = Vector3.Cross(line.Direction, Vector3.UnitZ);
//line.Rotate(Vector3.Zero, Quaternion.CreateFromAxisAngle(rotationAxis, -rad));
line.ApplyRotation(VectorMath.GetRotationQuaternion(axis, Vector3.UnitZ));
matrix = line.Transform;
Debug.Assert(Math.Abs(angleDegrees - Angle) < 0.1);
Debug.Assert(Math.Abs(Vector3.Transform(cs1.Translation, matrix).Length() - Vector3.Transform(cs2.Translation, matrix).Length()) < 0.1);
//Debug.Assert((Vector3.Transform(cs2.Translation, matrix) - Vector3.Transform(cs1.Translation, matrix)).Length() < 0.1);
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);
}
public void RotateDegrees(Vector3 origin, Vector3 axis, float radians)
{
RotateRadians(origin, axis, (float)VectorMath.DegreesToRad(radians));
}