// finds minimal rotation that aligns source frame with axes of target frame.
// considers all signs
// 1) find smallest angle(axis_source, axis_target), considering all sign permutations
// 2) rotate source to align axis_source with sign*axis_target
// 3) now rotate around alined_axis_source to align second-best pair of axes
public static Frame3d SolveMinRotation(Frame3d source, Frame3d target)
{
int best_i = -1, best_j = -1;
double fMaxAbsDot = 0, fMaxSign = 0;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
double d = source.GetAxis(i).Dot(target.GetAxis(j));
double a = Math.Abs(d);
if (a > fMaxAbsDot)
{
fMaxAbsDot = a;
fMaxSign = Math.Sign(d);
best_i = i;
best_j = j;
}
}
}
Frame3d R1 = source.Rotated(
Quaterniond.FromTo(source.GetAxis(best_i), fMaxSign * target.GetAxis(best_j)));
Vector3d vAround = R1.GetAxis(best_i);
int second_i = -1, second_j = -1;
double fSecondDot = 0, fSecondSign = 0;
for (int i = 0; i < 3; ++i)
{
if (i == best_i)
{
continue;
}
for (int j = 0; j < 3; ++j)
{
if (j == best_j)
{
continue;
}
double d = R1.GetAxis(i).Dot(target.GetAxis(j));
double a = Math.Abs(d);
if (a > fSecondDot)
{
fSecondDot = a;
fSecondSign = Math.Sign(d);
second_i = i;
second_j = j;
}
}
}
R1.ConstrainedAlignAxis(second_i, fSecondSign * target.GetAxis(second_j), vAround);
return(R1);
}
public bool EpsilonEqual(Frame3d f2, double epsilon)
{
return(origin.EpsilonEqual(f2.origin, epsilon) &&
rotation.EpsilonEqual(f2.rotation, epsilon));
}
/// <summary>
/// Interpolate between two frames - Lerp for origin, Slerp for rotation
/// </summary>
public static Frame3d Interpolate(Frame3d f1, Frame3d f2, double t)
{
return(new Frame3d(
Vector3d.Lerp(f1.origin, f2.origin, t),
Quaterniond.Slerp(f1.rotation, f2.rotation, t)));
}
/// <summary> Map frame *from* local frame coordinates into "world" coordinates </summary>
public Frame3d FromFrame(ref Frame3d f)
{
return(new Frame3d(FromFrameP(ref f.origin), FromFrame(ref f.rotation)));
}
///<summary> Map frame *into* local coordinates of Frame </summary>
public Frame3d ToFrame(ref Frame3d f)
{
return(new Frame3d(ToFrameP(ref f.origin), ToFrame(ref f.rotation)));
}
public Frame3d(Frame3d copy)
{
this.rotation = copy.rotation;
this.origin = copy.origin;
}
/// <summary>
/// Gets the xy component of this vector projected into a plane defined by a normal.
/// </summary>
/// <param name="planeNormal"></param>
/// <returns></returns>
public Vector2d ProjectIntoPlane(Vector3d planeNormal)
{
var frame = new Frame3d(Zero, planeNormal);
return(frame.FromFrameV(this).xy);
}