public NuGenRot3F(NuGenVec3F from, NuGenVec3F to)
{
from.Normalize();
to.Normalize();
float cost =
NuGenVec3F.Dot(from, to) /
(float)Math.Sqrt(NuGenVec3F.Dot(from, to) * NuGenVec3F.Dot(to, to));
if (cost > 0.99999)
{
r = 1;
v = new NuGenVec3F(0, 0, 0);
}
else if (cost < -0.99999)
{
NuGenVec3F frm = from.Normalized;
v = NuGenVec3F.Cross(frm, NuGenVec3F.UnitX);
if (v.Length < 0.00001)
{
v = NuGenVec3F.Cross(frm, NuGenVec3F.UnitY);
}
r = 0;
v.Normalize();
}
else
{
r = (float)Math.Sqrt(0.5 * (1.0 + cost));
v = NuGenVec3F.Cross(from, to);
v *= (float)Math.Sqrt((0.5 * (1.0 - cost)) / NuGenVec3F.Dot(v, v));
}
}
