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)); }