Beispiel #1
0
        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);
        }
Beispiel #2
0
        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));
        }
Beispiel #3
0
        public static double Angle(Vector3i v, Vector3i w)
        {
            var normprod = Math.Sqrt(v.Norm2 * w.Norm2);

            return(BasicMath.Acos(v * w / normprod));
        }