public static Rotation3 SphericalLinear(double t, double t1, Rotation3 r1, double t2, Rotation3 r2)
{
var q1 = r1.Quaternion;
var q2 = r2.Quaternion;
var dot = Quaternion.Dot(q1, q2);
if (Quaternion.Dot(q1, q2) < 0)
{
q2 = -q2;
dot = -dot;
}
var dt = t2 - t1;
var angle = BasicMath.Acos(dot);
Quaternion q;
if (angle > BasicMath.Epsilon)
{
q = Math.Sin(angle * (t2 - t) / dt) * q1 + Math.Sin(angle * (t - t1) / dt) * q2;
}
else
{
q = ((t2 - t) * q1 + (t - t1) * q2) / dt;
}
return((Rotation3)q);
}
public static Rotation3 GramSchmidt(Vector3 x, Vector3 y)
{
x = x.Normalized;
y = (y - (x * y) * x).Normalized;
var z = x.Cross(y);
var cosAngle = (x.X + y.Y + z.Z - 1) / 2;
var angle = BasicMath.Acos(cosAngle);
var axis0 = new Vector3(y.Z - z.Y, z.X - x.Z, x.Y - y.X); // vanishes at angle pi
if (cosAngle > -0.9) // optimization
{
return(new Rotation3(angle, axis0));
}
var xy = x.Y + y.X;
var xz = x.Z + z.X;
var yz = y.Z + z.Y;
var axis1 = new Vector3(2 * (x.X - cosAngle), xy, xz);
var axis2 = new Vector3(xy, 2 * (y.Y - cosAngle), yz);
var axis3 = new Vector3(xz, yz, 2 * (z.Z - cosAngle));
var norm1 = axis1.Norm2;
var norm2 = axis2.Norm2;
var norm3 = axis3.Norm2;
Vector3 axis;
if (norm1 > norm2 && norm1 > norm3)
{
axis = (axis1 * axis0 > 0 ? axis1 : -axis1);
}
else if (norm2 > norm1 && norm2 > norm3)
{
axis = (axis2 * axis0 > 0 ? axis2 : -axis2);
}
else
{
axis = (axis3 * axis0 > 0 ? axis3 : -axis3);
}
return(new Rotation3(angle, axis));
}
public static double Angle(Vector3i v, Vector3i w)
{
var normprod = Math.Sqrt(v.Norm2 * w.Norm2);
return(BasicMath.Acos(v * w / normprod));
}