Beispiel #1
0
 public static Quaternion4d Squad(
     Quaternion4d q1,
     Quaternion4d a,
     Quaternion4d b,
     Quaternion4d c,
     double t)
 {
     return(Slerp(
                Slerp(q1, c, t), Slerp(a, b, t), 2 * t * (1.0 - t)));
 }
Beispiel #2
0
 public override bool Equals(object obj)
 {
     if (obj is Quaternion4d)
     {
         Quaternion4d q = (Quaternion4d)obj;
         return(q == this);
     }
     else
     {
         return(false);
     }
 }
Beispiel #3
0
        /// <summary>
        /// Transforms a rotation in quaternion form to a set of Euler angles
        /// </summary>
        /// <returns>The rotation transformed to Euler angles, X=Yaw, Y=Pitch, Z=Roll (radians)</returns>
        public static Point3d QuaternionToEuler(Quaternion4d q)
        {
            double q0 = q.W;
            double q1 = q.X;
            double q2 = q.Y;
            double q3 = q.Z;

            double x = Math.Atan2(2 * (q2 * q3 + q0 * q1), (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
            double y = Math.Asin(-2 * (q1 * q3 - q0 * q2));
            double z = Math.Atan2(2 * (q1 * q2 + q0 * q3), (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));

            return(new Point3d(x, y, z));
        }
Beispiel #4
0
        public static Quaternion4d Slerp(Quaternion4d q0, Quaternion4d q1, double t)
        {
            double cosom = q0.X * q1.X + q0.Y * q1.Y + q0.Z * q1.Z + q0.W * q1.W;
            double tmp0, tmp1, tmp2, tmp3;

            if (cosom < 0.0)
            {
                cosom = -cosom;
                tmp0  = -q1.X;
                tmp1  = -q1.Y;
                tmp2  = -q1.Z;
                tmp3  = -q1.W;
            }
            else
            {
                tmp0 = q1.X;
                tmp1 = q1.Y;
                tmp2 = q1.Z;
                tmp3 = q1.W;
            }

            /* calc coeffs */
            double scale0, scale1;

            if ((1.0 - cosom) > double.Epsilon)
            {
                // standard case (slerp)
                double omega = Math.Acos(cosom);
                double sinom = Math.Sin(omega);
                scale0 = Math.Sin((1.0 - t) * omega) / sinom;
                scale1 = Math.Sin(t * omega) / sinom;
            }
            else
            {
                /* just lerp */
                scale0 = 1.0 - t;
                scale1 = t;
            }

            Quaternion4d q = new Quaternion4d();

            q.X = scale0 * q0.X + scale1 * tmp0;
            q.Y = scale0 * q0.Y + scale1 * tmp1;
            q.Z = scale0 * q0.Z + scale1 * tmp2;
            q.W = scale0 * q0.W + scale1 * tmp3;

            return(q);
        }
Beispiel #5
0
        //the below functions have not been certified to work properly
        public static Quaternion4d Exp(Quaternion4d q)
        {
            double sinom;
            double om = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);

            if (Math.Abs(om) < double.Epsilon)
            {
                sinom = 1.0;
            }
            else
            {
                sinom = Math.Sin(om) / om;
            }

            q.X = q.X * sinom;
            q.Y = q.Y * sinom;
            q.Z = q.Z * sinom;
            q.W = Math.Cos(om);

            return(q);
        }
Beispiel #6
0
        public static Quaternion4d Ln(Quaternion4d q)
        {
            double t = 0;

            double s  = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);
            double om = Math.Atan2(s, q.W);

            if (Math.Abs(s) < double.Epsilon)
            {
                t = 0.0f;
            }
            else
            {
                t = om / s;
            }

            q.X = q.X * t;
            q.Y = q.Y * t;
            q.Z = q.Z * t;
            q.W = 0.0f;

            return(q);
        }
Beispiel #7
0
        public static void SquadSetup(
            ref Quaternion4d outA,
            ref Quaternion4d outB,
            ref Quaternion4d outC,
            Quaternion4d q0,
            Quaternion4d q1,
            Quaternion4d q2,
            Quaternion4d q3)
        {
            q0 = q0 + q1;
            q0.Normalize();

            q2 = q2 + q1;
            q2.Normalize();

            q3 = q3 + q1;
            q3.Normalize();

            q1.Normalize();

            outA = q1 * Exp(-0.25 * (Ln(Exp(q1) * q2) + Ln(Exp(q1) * q0)));
            outB = q2 * Exp(-0.25 * (Ln(Exp(q2) * q3) + Ln(Exp(q2) * q1)));
            outC = q2;
        }
Beispiel #8
0
 public static double Dot(Quaternion4d a, Quaternion4d b)
 {
     return(a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W);
 }
Beispiel #9
0
 public static double Abs(Quaternion4d q)
 {
     return(Math.Sqrt(Norm2(q)));
 }
Beispiel #10
0
 public static double Norm2(Quaternion4d q)
 {
     return(q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W);
 }