public void AlignAxis(int nAxis, Vector3f vTo)
{
Debug.Assert(rotation.w != 0);
Quaternionf rot = Quaternionf.FromTo(GetAxis(nAxis), vTo);
Rotate(rot);
}
// 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 Frame3f SolveMinRotation(Frame3f source, Frame3f 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;
}
}
}
Frame3f R1 = source.Rotated(
Quaternionf.FromTo(source.GetAxis(best_i), (float)fMaxSign * target.GetAxis(best_j)));
Vector3f 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, (float)fSecondSign * target.GetAxis(second_j), vAround);
return(R1);
}
public Frame3f(Vector3f origin, Vector3f setAxis, int nAxis)
{
if (nAxis == 0)
{
rotation = Quaternionf.FromTo(Vector3f.AxisX, setAxis);
}
else if (nAxis == 1)
{
rotation = Quaternionf.FromTo(Vector3f.AxisY, setAxis);
}
else
{
rotation = Quaternionf.FromTo(Vector3f.AxisZ, setAxis);
}
this.origin = origin;
}
public Frame3f(Vector3d origin, Vector3d setZ)
{
rotation = Quaternionf.FromTo(Vector3f.AxisZ, (Vector3f)setZ);
this.origin = (Vector3f)origin;
}
public Frame3f(Vector3f origin, Vector3f setZ)
{
rotation = Quaternionf.FromTo(Vector3f.AxisZ, setZ);
this.origin = origin;
}
// M.Baske
public void AlignAxis(Vector3f axis, Vector3f vTo)
{
Quaternionf rot = Quaternionf.FromTo(axis, vTo);
Rotate(rot);
}
public void AlignAxis(int nAxis, Vector3f vTo)
{
Quaternionf rot = Quaternionf.FromTo(GetAxis(nAxis), vTo);
Rotate(rot);
}
