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 AxisPattern(Vector3 origin, Vector3 axis, int multiplicity, T item = null, IEnumerable <int> skipIndices = null)
{
axis.Normalize();
Trace.Assert(VectorMath.IsValid(axis));
int[] skip = skipIndices == null ? new int[] { } : skipIndices.Distinct().ToArray();
_coordinateSystems = new CoordinateSystem[multiplicity - skip.Length];
for (int i = 0, coordinateSystemIndex = 0; i < multiplicity; i++)
{
if (skip.Contains(i))
{
continue;
}
LineTrackingCoordinateSystem rotated = new LineTrackingCoordinateSystem(origin, axis);
rotated.RotateDegrees(origin, axis, i * 360 / multiplicity);
_coordinateSystems[coordinateSystemIndex++] = rotated;
}
base.CoordinateSystems = _coordinateSystems;
if (item != null)
{
base.Add(item);
}
}
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 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);
}
// Returns the two coordinate system transformations necessary to place two C-symmetry axes at the desired angle from one another.
public override bool GetLocalRealignmentWithOneRotationCenter(LineTrackingCoordinateSystem unalignedAxis1, Vector3 rotationCenter, LineTrackingCoordinateSystem unalignedAxis2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees)
{
bool success = Geometry.GetAxesAfterAlignmentToAngleDegrees(unalignedAxis1, rotationCenter, unalignedAxis2, AngleDegrees, out realignedAxis1, out realignedAxis2, out errorDegrees);
return(success);
}
// Returns the two coordinate system transformations necessary to place two C-symmetry axes at the desired angle from one another.
public override bool GetLocalRealignmentWithOneRotationCenter(LineTrackingCoordinateSystem unalignedAxis1, Vector3 rotationCenter, LineTrackingCoordinateSystem unalignedAxis2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees)
{
throw new InvalidOperationException("One joint is insufficient to align two lines");
}
public override bool GetLocalRealignmentWithTwoRotationCenters(LineTrackingCoordinateSystem axis1, Vector3 center1, LineTrackingCoordinateSystem axis2, Vector3 center2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees)
{
// Set out parameter default values
realignedAxis1 = new LineTrackingCoordinateSystem(axis1);
realignedAxis2 = new LineTrackingCoordinateSystem(axis2);
errorDegrees = float.NaN;
// Find the points (p1 and p2) where axes (l1 and l2) are nearest and perpendicular to the rotation center
// Find the plane normals (r1 and r2) defined by each center the new axis (p1->p2) that isn't yet tangent both spheres
Vector3 p1 = Line.GetNearestPointOnLine(Line.CreateFrom(axis1), center1);
Vector3 p2 = Line.GetNearestPointOnLine(Line.CreateFrom(axis2), center2);
Line p12 = Line.CreateFromTwoPoints(p1, p2);
Vector3 r1 = Vector3.Normalize(Vector3.Cross(p1 - center1, p12.Direction)); // Plane normals
Vector3 r2 = Vector3.Normalize(Vector3.Cross(p2 - center2, p12.Direction));
Vector3 vC12 = center2 - center1;
Vector3 uC12 = Vector3.Normalize(vC12);
// Get the "average" plane that is created by rotating each plane about (p1->p2) until they are coincident
Vector3 r = Vector3.Dot(r1, r2) > 0? Vector3.Normalize(r1 + r2) : Vector3.Normalize(r1 - r2); // This gives the normal for the nearest of two possible mid-planes
Debug.Assert(Math.Abs(Vector3.Dot(r, p12.Direction)) < 0.001);
// Project center1 and center2 onto the plane to determine circle centers and their radii
Vector3 c1projection = center1 + Vector3.Dot(p1 - center1, r) * r;
Vector3 c2projection = center2 + Vector3.Dot(p2 - center2, r) * r;
float c1radius = Vector3.Distance(p1, c1projection);
float c2radius = Vector3.Distance(p2, c2projection);
Vector3 vProjectionC1C2 = c2projection - c1projection;
Vector3 uProjectionC1C2 = Vector3.Normalize(vProjectionC1C2);
float distanceProjectionC12 = vProjectionC1C2.Length();
Debug.Assert(Math.Abs(Vector3.Dot(p1 - c1projection, r)) < 0.001);
Debug.Assert(Math.Abs(Vector3.Dot(p2 - c2projection, r)) < 0.001);
// If one spherical surface fully contains the other, there are no solutions
if (c1radius + distanceProjectionC12 < c2radius || c2radius + distanceProjectionC12 < c1radius)
{
return(false);
}
// Compute theta (angle between c1->c2 and the p1 on c1 or p2 on c2)
//float originalTheta1 = (float) Math.Acos(p2 - c1proj)
float solutionTheta1 = (float)Math.Acos((c1radius - c2radius) / distanceProjectionC12);
float solutionTheta2 = c1radius + c2radius < distanceProjectionC12 ? (float)Math.Acos((c2radius + c2radius) / distanceProjectionC12) : float.NaN; // theta2 is only defined for spheres that don't overlap
// Solutions exist at +/- theta1
Vector3 positiveTheta1P1 = c1projection + Vector3.Transform(uProjectionC1C2, Quaternion.CreateFromAxisAngle(r, solutionTheta1));
Vector3 positiveTheta1P2 = c2projection + Vector3.Transform(uProjectionC1C2, Quaternion.CreateFromAxisAngle(r, solutionTheta1));
Vector3 negativeTheta1P1 = c1projection + Vector3.Transform(uProjectionC1C2, Quaternion.CreateFromAxisAngle(r, -solutionTheta1));
Vector3 negativeTheta1P2 = c2projection + Vector3.Transform(uProjectionC1C2, Quaternion.CreateFromAxisAngle(r, -solutionTheta1));
Vector3 uPositiveTheta1 = positiveTheta1P2 - positiveTheta1P1;
Vector3 uNegativeTheta1 = negativeTheta1P1 - negativeTheta1P2;
float radErrorPositiveTheta1 = (float)(VectorMath.GetAngleDegrees(positiveTheta1P1 - c1projection, p1 - c1projection) + VectorMath.GetAngleDegrees(positiveTheta1P2 - c2projection, p2 - c2projection));
float radErrorNegativeTheta1 = (float)(VectorMath.GetAngleDegrees(negativeTheta1P1 - c1projection, p1 - c1projection) + VectorMath.GetAngleDegrees(negativeTheta1P2 - c2projection, p2 - c2projection));
if (radErrorPositiveTheta1 < radErrorNegativeTheta1)
{
errorDegrees = radErrorPositiveTheta1;
// Move the realigned axes to reflect rotation onto the p1->p2 line segment
Quaternion rotation1 = VectorMath.GetRotationQuaternion(p1 - center1, positiveTheta1P1 - center1);
Quaternion rotation2 = VectorMath.GetRotationQuaternion(p2 - center2, positiveTheta1P2 - center2);
realignedAxis1.Translation -= center1;
realignedAxis1.ApplyRotation(rotation1);
realignedAxis1.Translation += center1;
realignedAxis2.Translation -= center2;
realignedAxis2.ApplyRotation(rotation2);
realignedAxis2.Translation += center2;
rotation1 = Vector3.Dot(realignedAxis1.Direction, uPositiveTheta1) > 0 ? VectorMath.GetRotationQuaternion(realignedAxis1.Direction, uPositiveTheta1) : VectorMath.GetRotationQuaternion(realignedAxis1.Direction, -uPositiveTheta1);
rotation2 = Vector3.Dot(realignedAxis2.Direction, uPositiveTheta1) > 0 ? VectorMath.GetRotationQuaternion(realignedAxis2.Direction, uPositiveTheta1) : VectorMath.GetRotationQuaternion(realignedAxis2.Direction, -uPositiveTheta1);
realignedAxis1.Translation -= center1;
realignedAxis1.ApplyRotation(rotation1);
realignedAxis1.Translation += center1;
realignedAxis2.Translation -= center2;
realignedAxis2.ApplyRotation(rotation2);
realignedAxis2.Translation += center2;
}
else
{
errorDegrees = radErrorNegativeTheta1;
return(false);
}
// TODO: negative theta1, theta2 positive and negative
return(true);
}
// Returns the two coordinate system transformations necessary to place two C-symmetry axes at the desired angle from one another.
public override bool GetLocalRealignmentWithOneRotationCenter(LineTrackingCoordinateSystem unalignedAxis1, Vector3 rotationCenter, LineTrackingCoordinateSystem unalignedAxis2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees)
{
throw new NotImplementedException();
//bool success = Geometry.GetAxesAfterAlignmentToAngleDegrees(unalignedAxis1, rotationCenter, unalignedAxis2, Angle, out realignedAxis1, out realignedAxis2, out errorDegrees);
//return success;
}
public abstract bool GetLocalRealignmentWithOneRotationCenter(LineTrackingCoordinateSystem unalignedAxis1, Vector3 rotationCenter1, LineTrackingCoordinateSystem unalignedAxis2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees);
public abstract bool GetLocalRealignmentWithTwoRotationCenters(LineTrackingCoordinateSystem axis1, Vector3 center1, LineTrackingCoordinateSystem axis2, Vector3 center2, out LineTrackingCoordinateSystem realignedAxis1, out LineTrackingCoordinateSystem realignedAxis2, out float errorDegrees);
