Example #1
0
 /// <summary>
 /// Calculates the dot product of two Quaternions.
 /// </summary>
 /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param>
 public static double Dot(QuaternionD quaternion1, QuaternionD quaternion2)
 {
     return (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
 }
Example #2
0
        private Vector3D GetAngleVelocity(QuaternionD q1, QuaternionD q2)
        {
            q1.Conjugate();
            QuaternionD r = q2 * q1;

            double angle = 2 * System.Math.Acos(MathHelper.Clamp(r.W, -1, 1));
            if (angle > Math.PI)
            {
                angle -= 2.0 * Math.PI;
            }

            Vector3D velocity = angle * new Vector3D(r.X, r.Y, r.Z) /
                System.Math.Sqrt(r.X * r.X + r.Y * r.Y + r.Z * r.Z);

            return velocity;
        }
Example #3
0
 /// <summary>
 /// Creates a QuaternionD from a rotation MatrixD.
 /// </summary>
 /// <param name="matrix">The rotation MatrixD to create the QuaternionD from.</param>
 public static QuaternionD CreateFromRotationMatrix(MatrixD matrix)
 {
     double num1 = matrix.M11 + matrix.M22 + matrix.M33;
     QuaternionD quaternion = new QuaternionD();
     if (num1 > 0.0)
     {
         double num2 = Math.Sqrt(num1 + 1.0);
         quaternion.W = num2 * 0.5;
         double num3 = 0.5 / num2;
         quaternion.X = (matrix.M23 - matrix.M32) * num3;
         quaternion.Y = (matrix.M31 - matrix.M13) * num3;
         quaternion.Z = (matrix.M12 - matrix.M21) * num3;
     }
     else if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
     {
         double num2 = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
         double num3 = 0.5 / num2;
         quaternion.X = 0.5 * num2;
         quaternion.Y = (matrix.M12 + matrix.M21) * num3;
         quaternion.Z = (matrix.M13 + matrix.M31) * num3;
         quaternion.W = (matrix.M23 - matrix.M32) * num3;
     }
     else if (matrix.M22 > matrix.M33)
     {
         double num2 = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
         double num3 = 0.5 / num2;
         quaternion.X = (matrix.M21 + matrix.M12) * num3;
         quaternion.Y = 0.5 * num2;
         quaternion.Z = (matrix.M32 + matrix.M23) * num3;
         quaternion.W = (matrix.M31 - matrix.M13) * num3;
     }
     else
     {
         double num2 = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
         double num3 = 0.5 / num2;
         quaternion.X = (matrix.M31 + matrix.M13) * num3;
         quaternion.Y = (matrix.M32 + matrix.M23) * num3;
         quaternion.Z = 0.5 * num2;
         quaternion.W = (matrix.M12 - matrix.M21) * num3;
     }
     return quaternion;
 }
Example #4
0
 /// <summary>
 /// Multiplies two quaternions.
 /// </summary>
 /// <param name="quaternion1">The quaternion on the left of the multiplication.</param><param name="quaternion2">The quaternion on the right of the multiplication.</param><param name="result">[OutAttribute] The result of the multiplication.</param>
 public static void Multiply(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
 {
     double num1 = quaternion1.X;
     double num2 = quaternion1.Y;
     double num3 = quaternion1.Z;
     double num4 = quaternion1.W;
     double num5 = quaternion2.X;
     double num6 = quaternion2.Y;
     double num7 = quaternion2.Z;
     double num8 = quaternion2.W;
     double num9 = (num2 * num7 - num3 * num6);
     double num10 = (num3 * num5 - num1 * num7);
     double num11 = (num1 * num6 - num2 * num5);
     double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
     result.X = (num1 * num8 + num5 * num4) + num9;
     result.Y = (num2 * num8 + num6 * num4) + num10;
     result.Z = (num3 * num8 + num7 * num4) + num11;
     result.W = num4 * num8 - num12;
 }
Example #5
0
 /// <summary>
 /// Divides a QuaternionD by another QuaternionD.
 /// </summary>
 /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">The divisor.</param><param name="result">[OutAttribute] Result of the division.</param>
 public static void Divide(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
 {
     double num1 = quaternion1.X;
     double num2 = quaternion1.Y;
     double num3 = quaternion1.Z;
     double num4 = quaternion1.W;
     double num5 = 1.0 / (quaternion2.X * quaternion2.X + quaternion2.Y * quaternion2.Y + quaternion2.Z * quaternion2.Z + quaternion2.W * quaternion2.W);
     double num6 = -quaternion2.X * num5;
     double num7 = -quaternion2.Y * num5;
     double num8 = -quaternion2.Z * num5;
     double num9 = quaternion2.W * num5;
     double num10 = (num2 * num8 - num3 * num7);
     double num11 = (num3 * num6 - num1 * num8);
     double num12 = (num1 * num7 - num2 * num6);
     double num13 = (num1 * num6 + num2 * num7 + num3 * num8);
     result.X = (num1 * num9 + num6 * num4) + num10;
     result.Y = (num2 * num9 + num7 * num4) + num11;
     result.Z = (num3 * num9 + num8 * num4) + num12;
     result.W = num4 * num9 - num13;
 }
Example #6
0
 /// <summary>
 /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
 /// </summary>
 /// <param name="value1">The first QuaternionD rotation in the series.</param><param name="value2">The second QuaternionD rotation in the series.</param><param name="result">[OutAttribute] The QuaternionD rotation representing the concatenation of value1 followed by value2.</param>
 public static void Concatenate(ref QuaternionD value1, ref QuaternionD value2, out QuaternionD result)
 {
     double num1 = value2.X;
     double num2 = value2.Y;
     double num3 = value2.Z;
     double num4 = value2.W;
     double num5 = value1.X;
     double num6 = value1.Y;
     double num7 = value1.Z;
     double num8 = value1.W;
     double num9 = (num2 * num7 - num3 * num6);
     double num10 = (num3 * num5 - num1 * num7);
     double num11 = (num1 * num6 - num2 * num5);
     double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
     result.X = (num1 * num8 + num5 * num4) + num9;
     result.Y = (num2 * num8 + num6 * num4) + num10;
     result.Z = (num3 * num8 + num7 * num4) + num11;
     result.W = num4 * num8 - num12;
 }
Example #7
0
 /// <summary>
 /// Adds two Quaternions.
 /// </summary>
 /// <param name="quaternion1">QuaternionD to add.</param><param name="quaternion2">QuaternionD to add.</param><param name="result">[OutAttribute] Result of adding the Quaternions.</param>
 public static void Add(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
 {
     result.X = quaternion1.X + quaternion2.X;
     result.Y = quaternion1.Y + quaternion2.Y;
     result.Z = quaternion1.Z + quaternion2.Z;
     result.W = quaternion1.W + quaternion2.W;
 }
        public static void Multiply(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
        {
            double x     = quaternion1.X;
            double y     = quaternion1.Y;
            double z     = quaternion1.Z;
            double w     = quaternion1.W;
            double num5  = quaternion2.X;
            double num6  = quaternion2.Y;
            double num7  = quaternion2.Z;
            double num8  = quaternion2.W;
            double num9  = (y * num7) - (z * num6);
            double num10 = (z * num5) - (x * num7);
            double num11 = (x * num6) - (y * num5);
            double num12 = ((x * num5) + (y * num6)) + (z * num7);

            result.X = ((x * num8) + (num5 * w)) + num9;
            result.Y = ((y * num8) + (num6 * w)) + num10;
            result.Z = ((z * num8) + (num7 * w)) + num11;
            result.W = (w * num8) - num12;
        }
        public static void Divide(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
        {
            double x     = quaternion1.X;
            double y     = quaternion1.Y;
            double z     = quaternion1.Z;
            double w     = quaternion1.W;
            double num5  = 1.0 / ((((quaternion2.X * quaternion2.X) + (quaternion2.Y * quaternion2.Y)) + (quaternion2.Z * quaternion2.Z)) + (quaternion2.W * quaternion2.W));
            double num6  = -quaternion2.X * num5;
            double num7  = -quaternion2.Y * num5;
            double num8  = -quaternion2.Z * num5;
            double num9  = quaternion2.W * num5;
            double num10 = (y * num8) - (z * num7);
            double num11 = (z * num6) - (x * num8);
            double num12 = (x * num7) - (y * num6);
            double num13 = ((x * num6) + (y * num7)) + (z * num8);

            result.X = ((x * num9) + (num6 * w)) + num10;
            result.Y = ((y * num9) + (num7 * w)) + num11;
            result.Z = ((z * num9) + (num8 * w)) + num12;
            result.W = (w * num9) - num13;
        }
        public static void Lerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
        {
            double num  = amount;
            double num2 = 1.0 - num;

            if (((((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W)) >= 0.0)
            {
                result.X = (num2 * quaternion1.X) + (num * quaternion2.X);
                result.Y = (num2 * quaternion1.Y) + (num * quaternion2.Y);
                result.Z = (num2 * quaternion1.Z) + (num * quaternion2.Z);
                result.W = (num2 * quaternion1.W) + (num * quaternion2.W);
            }
            else
            {
                result.X = (num2 * quaternion1.X) - (num * quaternion2.X);
                result.Y = (num2 * quaternion1.Y) - (num * quaternion2.Y);
                result.Z = (num2 * quaternion1.Z) - (num * quaternion2.Z);
                result.W = (num2 * quaternion1.W) - (num * quaternion2.W);
            }
            double num3 = 1.0 / Math.Sqrt((((result.X * result.X) + (result.Y * result.Y)) + (result.Z * result.Z)) + (result.W * result.W));

            result.X *= num3;
            result.Y *= num3;
            result.Z *= num3;
            result.W *= num3;
        }
        public static void Concatenate(ref QuaternionD value1, ref QuaternionD value2, out QuaternionD result)
        {
            double x     = value2.X;
            double y     = value2.Y;
            double z     = value2.Z;
            double w     = value2.W;
            double num5  = value1.X;
            double num6  = value1.Y;
            double num7  = value1.Z;
            double num8  = value1.W;
            double num9  = (y * num7) - (z * num6);
            double num10 = (z * num5) - (x * num7);
            double num11 = (x * num6) - (y * num5);
            double num12 = ((x * num5) + (y * num6)) + (z * num7);

            result.X = ((x * num8) + (num5 * w)) + num9;
            result.Y = ((y * num8) + (num6 * w)) + num10;
            result.Z = ((z * num8) + (num7 * w)) + num11;
            result.W = (w * num8) - num12;
        }
        public static void Slerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
        {
            double num3;
            double num4;
            double num  = amount;
            double d    = (((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W);
            bool   flag = false;

            if (d < 0.0)
            {
                flag = true;
                d    = -d;
            }
            if (d > 0.999998986721039)
            {
                num3 = 1.0 - num;
                num4 = flag ? -num : num;
            }
            else
            {
                double a    = Math.Acos(d);
                double num6 = 1.0 / Math.Sin(a);
                num3 = Math.Sin((1.0 - num) * a) * num6;
                num4 = flag ? (-Math.Sin(num * a) * num6) : (Math.Sin(num * a) * num6);
            }
            result.X = (num3 * quaternion1.X) + (num4 * quaternion2.X);
            result.Y = (num3 * quaternion1.Y) + (num4 * quaternion2.Y);
            result.Z = (num3 * quaternion1.Z) + (num4 * quaternion2.Z);
            result.W = (num3 * quaternion1.W) + (num4 * quaternion2.W);
        }
 public static double Dot(QuaternionD quaternion1, QuaternionD quaternion2) =>
 ((((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W));
 static QuaternionD()
 {
     Identity = new QuaternionD(0.0, 0.0, 0.0, 1.0);
 }
Example #15
0
 /// <summary>
 /// Interpolates between two quaternions, using spherical linear interpolation.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value that indicates how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] Result of the interpolation.</param>
 public static void Slerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
 {
     double num1 = amount;
     double num2 = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
     bool flag = false;
     if (num2 < 0.0)
     {
         flag = true;
         num2 = -num2;
     }
     double num3;
     double num4;
     if (num2 > 0.999998986721039)
     {
         num3 = 1.0 - num1;
         num4 = flag ? -num1 : num1;
     }
     else
     {
         double num5 = Math.Acos(num2);
         double num6 = (1.0 / Math.Sin(num5));
         num3 = Math.Sin((1.0 - num1) * num5) * num6;
         num4 = flag ? -Math.Sin(num1 * num5) * num6 : Math.Sin(num1 * num5) * num6;
     }
     result.X = (num3 * quaternion1.X + num4 * quaternion2.X);
     result.Y = (num3 * quaternion1.Y + num4 * quaternion2.Y);
     result.Z = (num3 * quaternion1.Z + num4 * quaternion2.Z);
     result.W = (num3 * quaternion1.W + num4 * quaternion2.W);
 }
 public static bool IsZero(QuaternionD value) =>
 IsZero(value, 0.0001);
Example #17
0
 /// <summary>
 /// Linearly interpolates between two quaternions.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value indicating how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] The resulting quaternion.</param>
 public static void Lerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
 {
     double num1 = amount;
     double num2 = 1.0 - num1;
     if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
     {
         result.X = (num2 * quaternion1.X + num1 * quaternion2.X);
         result.Y = (num2 * quaternion1.Y + num1 * quaternion2.Y);
         result.Z = (num2 * quaternion1.Z + num1 * quaternion2.Z);
         result.W = (num2 * quaternion1.W + num1 * quaternion2.W);
     }
     else
     {
         result.X = (num2 * quaternion1.X - num1 * quaternion2.X);
         result.Y = (num2 * quaternion1.Y - num1 * quaternion2.Y);
         result.Z = (num2 * quaternion1.Z - num1 * quaternion2.Z);
         result.W = (num2 * quaternion1.W - num1 * quaternion2.W);
     }
     double num3 = 1.0 / Math.Sqrt(result.X * result.X + result.Y * result.Y + result.Z * result.Z + result.W * result.W);
     result.X *= num3;
     result.Y *= num3;
     result.Z *= num3;
     result.W *= num3;
 }
 public static bool IsZero(QuaternionD value, double epsilon) =>
 ((((Math.Abs(value.X) < epsilon) && (Math.Abs(value.Y) < epsilon)) && (Math.Abs(value.Z) < epsilon)) && (Math.Abs(value.W) < epsilon));
Example #19
0
 /// <summary>
 /// Flips the sign of each component of the quaternion.
 /// </summary>
 /// <param name="quaternion">Source quaternion.</param><param name="result">[OutAttribute] Negated quaternion.</param>
 public static void Negate(ref QuaternionD quaternion, out QuaternionD result)
 {
     result.X = -quaternion.X;
     result.Y = -quaternion.Y;
     result.Z = -quaternion.Z;
     result.W = -quaternion.W;
 }
        public static void CreateFromTwoVectors(ref Vector3D firstVector, ref Vector3D secondVector, out QuaternionD result)
        {
            Vector3D vectord;

            Vector3D.Cross(ref firstVector, ref secondVector, out vectord);
            result    = new QuaternionD(vectord.X, vectord.Y, vectord.Z, (double)Vector3.Dot((Vector3)firstVector, (Vector3)secondVector));
            result.W += result.Length();
            result.Normalize();
        }
Example #21
0
 /// <summary>
 /// Subtracts a quaternion from another quaternion.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="result">[OutAttribute] Result of the subtraction.</param>
 public static void Subtract(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
 {
     result.X = quaternion1.X - quaternion2.X;
     result.Y = quaternion1.Y - quaternion2.Y;
     result.Z = quaternion1.Z - quaternion2.Z;
     result.W = quaternion1.W - quaternion2.W;
 }
Example #22
0
 // Per component IsZero, returns 1 for each component which equals to 0
 public static bool IsZero(QuaternionD value, double epsilon)
 {
     return(Math.Abs(value.X) < epsilon && Math.Abs(value.Y) < epsilon && Math.Abs(value.Z) < epsilon && Math.Abs(value.W) < epsilon);
 }
Example #23
0
 /// <summary>
 /// Multiplies a quaternion by a scalar value.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="scaleFactor">Scalar value.</param><param name="result">[OutAttribute] The result of the multiplication.</param>
 public static void Multiply(ref QuaternionD quaternion1, double scaleFactor, out QuaternionD result)
 {
     result.X = quaternion1.X * scaleFactor;
     result.Y = quaternion1.Y * scaleFactor;
     result.Z = quaternion1.Z * scaleFactor;
     result.W = quaternion1.W * scaleFactor;
 }
Example #24
0
        /// <summary>
        /// Creates a new QuaternionD from specified yaw, pitch, and roll angles.
        /// </summary>
        /// <param name="yaw">The yaw angle, in radians, around the y-axis.</param><param name="pitch">The pitch angle, in radians, around the x-axis.</param><param name="roll">The roll angle, in radians, around the z-axis.</param><param name="result">[OutAttribute] An existing QuaternionD filled in to express the specified yaw, pitch, and roll angles.</param>
        public static void CreateFromYawPitchRoll(double yaw, double pitch, double roll, out QuaternionD result)
        {
            double num1 = roll * 0.5;
            double num2 = Math.Sin(num1);
            double num3 = Math.Cos(num1);
            double num4 = pitch * 0.5;
            double num5 = Math.Sin(num4);
            double num6 = Math.Cos(num4);
            double num7 = yaw * 0.5;
            double num8 = Math.Sin(num7);
            double num9 = Math.Cos(num7);

            result.X = (num9 * num5 * num3 + num8 * num6 * num2);
            result.Y = (num8 * num6 * num3 - num9 * num5 * num2);
            result.Z = (num9 * num6 * num2 - num8 * num5 * num3);
            result.W = (num9 * num6 * num3 + num8 * num5 * num2);
        }
 public static bool IsZero(QuaternionD value)
 {
     return(IsZero(value, 0.0001));
 }
Example #26
0
 /// <summary>
 /// Calculates the dot product of two Quaternions.
 /// </summary>
 /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param>
 public static double Dot(QuaternionD quaternion1, QuaternionD quaternion2)
 {
     return(quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
 }
 public bool Equals(QuaternionD other) =>
 ((((this.X == other.X) && (this.Y == other.Y)) && (this.Z == other.Z)) && (this.W == other.W));
Example #28
0
 /// <summary>
 /// Calculates the dot product of two Quaternions.
 /// </summary>
 /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param><param name="result">[OutAttribute] Dot product of the Quaternions.</param>
 public static void Dot(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out double result)
 {
     result = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
 }
Example #29
0
 /// <summary>
 /// Creates a QuaternionD from a rotation MatrixD.
 /// </summary>
 /// <param name="matrix">The rotation MatrixD to create the QuaternionD from.</param><param name="result">[OutAttribute] The created QuaternionD.</param>
 public static void CreateFromRotationMatrix(ref MatrixD matrix, out QuaternionD result)
 {
     double num1 = matrix.M11 + matrix.M22 + matrix.M33;
     if (num1 > 0.0)
     {
         double num2 = Math.Sqrt(num1 + 1.0);
         result.W = num2 * 0.5;
         double num3 = 0.5 / num2;
         result.X = (matrix.M23 - matrix.M32) * num3;
         result.Y = (matrix.M31 - matrix.M13) * num3;
         result.Z = (matrix.M12 - matrix.M21) * num3;
     }
     else if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
     {
         double num2 = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
         double num3 = 0.5 / num2;
         result.X = 0.5 * num2;
         result.Y = (matrix.M12 + matrix.M21) * num3;
         result.Z = (matrix.M13 + matrix.M31) * num3;
         result.W = (matrix.M23 - matrix.M32) * num3;
     }
     else if (matrix.M22 > matrix.M33)
     {
         double num2 = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
         double num3 = 0.5 / num2;
         result.X = (matrix.M21 + matrix.M12) * num3;
         result.Y = 0.5 * num2;
         result.Z = (matrix.M32 + matrix.M23) * num3;
         result.W = (matrix.M31 - matrix.M13) * num3;
     }
     else
     {
         double num2 = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
         double num3 = 0.5 / num2;
         result.X = (matrix.M31 + matrix.M13) * num3;
         result.Y = (matrix.M32 + matrix.M23) * num3;
         result.Z = 0.5 * num2;
         result.W = (matrix.M12 - matrix.M21) * num3;
     }
 }
Example #30
0
        /// <summary>
        /// Interpolates between two quaternions, using spherical linear interpolation.
        /// </summary>
        /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value that indicates how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] Result of the interpolation.</param>
        public static void Slerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
        {
            double num1 = amount;
            double num2 = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
            bool   flag = false;

            if (num2 < 0.0)
            {
                flag = true;
                num2 = -num2;
            }
            double num3;
            double num4;

            if (num2 > 0.999998986721039)
            {
                num3 = 1.0 - num1;
                num4 = flag ? -num1 : num1;
            }
            else
            {
                double num5 = Math.Acos(num2);
                double num6 = (1.0 / Math.Sin(num5));
                num3 = Math.Sin((1.0 - num1) * num5) * num6;
                num4 = flag ? -Math.Sin(num1 * num5) * num6 : Math.Sin(num1 * num5) * num6;
            }
            result.X = (num3 * quaternion1.X + num4 * quaternion2.X);
            result.Y = (num3 * quaternion1.Y + num4 * quaternion2.Y);
            result.Z = (num3 * quaternion1.Z + num4 * quaternion2.Z);
            result.W = (num3 * quaternion1.W + num4 * quaternion2.W);
        }
Example #31
0
 /// <summary>
 /// Calculates the dot product of two Quaternions.
 /// </summary>
 /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param><param name="result">[OutAttribute] Dot product of the Quaternions.</param>
 public static void Dot(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out double result)
 {
     result = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
 }
Example #32
0
        /// <summary>
        /// Linearly interpolates between two quaternions.
        /// </summary>
        /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value indicating how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] The resulting quaternion.</param>
        public static void Lerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
        {
            double num1 = amount;
            double num2 = 1.0 - num1;

            if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
            {
                result.X = (num2 * quaternion1.X + num1 * quaternion2.X);
                result.Y = (num2 * quaternion1.Y + num1 * quaternion2.Y);
                result.Z = (num2 * quaternion1.Z + num1 * quaternion2.Z);
                result.W = (num2 * quaternion1.W + num1 * quaternion2.W);
            }
            else
            {
                result.X = (num2 * quaternion1.X - num1 * quaternion2.X);
                result.Y = (num2 * quaternion1.Y - num1 * quaternion2.Y);
                result.Z = (num2 * quaternion1.Z - num1 * quaternion2.Z);
                result.W = (num2 * quaternion1.W - num1 * quaternion2.W);
            }
            double num3 = 1.0 / Math.Sqrt(result.X * result.X + result.Y * result.Y + result.Z * result.Z + result.W * result.W);

            result.X *= num3;
            result.Y *= num3;
            result.Z *= num3;
            result.W *= num3;
        }
Example #33
0
 /// <summary>
 /// Linearly interpolates between two quaternions.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value indicating how far to interpolate between the quaternions.</param>
 public static QuaternionD Lerp(QuaternionD quaternion1, QuaternionD quaternion2, double amount)
 {
     double num1 = amount;
     double num2 = 1.0 - num1;
     QuaternionD quaternion = new QuaternionD();
     if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
     {
         quaternion.X = (num2 * quaternion1.X + num1 * quaternion2.X);
         quaternion.Y = (num2 * quaternion1.Y + num1 * quaternion2.Y);
         quaternion.Z = (num2 * quaternion1.Z + num1 * quaternion2.Z);
         quaternion.W = (num2 * quaternion1.W + num1 * quaternion2.W);
     }
     else
     {
         quaternion.X = (num2 * quaternion1.X - num1 * quaternion2.X);
         quaternion.Y = (num2 * quaternion1.Y - num1 * quaternion2.Y);
         quaternion.Z = (num2 * quaternion1.Z - num1 * quaternion2.Z);
         quaternion.W = (num2 * quaternion1.W - num1 * quaternion2.W);
     }
     double num3 = 1.0 / Math.Sqrt(quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
     quaternion.X *= num3;
     quaternion.Y *= num3;
     quaternion.Z *= num3;
     quaternion.W *= num3;
     return quaternion;
 }
Example #34
0
        /// <summary>
        /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
        /// </summary>
        /// <param name="value1">The first QuaternionD rotation in the series.</param><param name="value2">The second QuaternionD rotation in the series.</param><param name="result">[OutAttribute] The QuaternionD rotation representing the concatenation of value1 followed by value2.</param>
        public static void Concatenate(ref QuaternionD value1, ref QuaternionD value2, out QuaternionD result)
        {
            double num1  = value2.X;
            double num2  = value2.Y;
            double num3  = value2.Z;
            double num4  = value2.W;
            double num5  = value1.X;
            double num6  = value1.Y;
            double num7  = value1.Z;
            double num8  = value1.W;
            double num9  = (num2 * num7 - num3 * num6);
            double num10 = (num3 * num5 - num1 * num7);
            double num11 = (num1 * num6 - num2 * num5);
            double num12 = (num1 * num5 + num2 * num6 + num3 * num7);

            result.X = (num1 * num8 + num5 * num4) + num9;
            result.Y = (num2 * num8 + num6 * num4) + num10;
            result.Z = (num3 * num8 + num7 * num4) + num11;
            result.W = num4 * num8 - num12;
        }
Example #35
0
 /// <summary>
 /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
 /// </summary>
 /// <param name="value1">The first QuaternionD rotation in the series.</param><param name="value2">The second QuaternionD rotation in the series.</param>
 public static QuaternionD Concatenate(QuaternionD value1, QuaternionD value2)
 {
     double num1 = value2.X;
     double num2 = value2.Y;
     double num3 = value2.Z;
     double num4 = value2.W;
     double num5 = value1.X;
     double num6 = value1.Y;
     double num7 = value1.Z;
     double num8 = value1.W;
     double num9 = (num2 * num7 - num3 * num6);
     double num10 = (num3 * num5 - num1 * num7);
     double num11 = (num1 * num6 - num2 * num5);
     double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
     QuaternionD quaternion;
     quaternion.X = (num1 * num8 + num5 * num4) + num9;
     quaternion.Y = (num2 * num8 + num6 * num4) + num10;
     quaternion.Z = (num3 * num8 + num7 * num4) + num11;
     quaternion.W = num4 * num8 - num12;
     return quaternion;
 }
Example #36
0
 /// <summary>
 /// Adds two Quaternions.
 /// </summary>
 /// <param name="quaternion1">QuaternionD to add.</param><param name="quaternion2">QuaternionD to add.</param><param name="result">[OutAttribute] Result of adding the Quaternions.</param>
 public static void Add(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
 {
     result.X = quaternion1.X + quaternion2.X;
     result.Y = quaternion1.Y + quaternion2.Y;
     result.Z = quaternion1.Z + quaternion2.Z;
     result.W = quaternion1.W + quaternion2.W;
 }
Example #37
0
 /// <summary>
 /// Flips the sign of each component of the quaternion.
 /// </summary>
 /// <param name="quaternion">Source quaternion.</param>
 public static QuaternionD Negate(QuaternionD quaternion)
 {
     QuaternionD quaternion1;
     quaternion1.X = -quaternion.X;
     quaternion1.Y = -quaternion.Y;
     quaternion1.Z = -quaternion.Z;
     quaternion1.W = -quaternion.W;
     return quaternion1;
 }
Example #38
0
 /// <summary>
 /// Subtracts a quaternion from another quaternion.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="result">[OutAttribute] Result of the subtraction.</param>
 public static void Subtract(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
 {
     result.X = quaternion1.X - quaternion2.X;
     result.Y = quaternion1.Y - quaternion2.Y;
     result.Z = quaternion1.Z - quaternion2.Z;
     result.W = quaternion1.W - quaternion2.W;
 }
Example #39
0
 /// <summary>
 /// Adds two Quaternions.
 /// </summary>
 /// <param name="quaternion1">QuaternionD to add.</param><param name="quaternion2">QuaternionD to add.</param>
 public static QuaternionD Add(QuaternionD quaternion1, QuaternionD quaternion2)
 {
     QuaternionD quaternion;
     quaternion.X = quaternion1.X + quaternion2.X;
     quaternion.Y = quaternion1.Y + quaternion2.Y;
     quaternion.Z = quaternion1.Z + quaternion2.Z;
     quaternion.W = quaternion1.W + quaternion2.W;
     return quaternion;
 }
Example #40
0
        /// <summary>
        /// Multiplies two quaternions.
        /// </summary>
        /// <param name="quaternion1">The quaternion on the left of the multiplication.</param><param name="quaternion2">The quaternion on the right of the multiplication.</param><param name="result">[OutAttribute] The result of the multiplication.</param>
        public static void Multiply(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
        {
            double num1  = quaternion1.X;
            double num2  = quaternion1.Y;
            double num3  = quaternion1.Z;
            double num4  = quaternion1.W;
            double num5  = quaternion2.X;
            double num6  = quaternion2.Y;
            double num7  = quaternion2.Z;
            double num8  = quaternion2.W;
            double num9  = (num2 * num7 - num3 * num6);
            double num10 = (num3 * num5 - num1 * num7);
            double num11 = (num1 * num6 - num2 * num5);
            double num12 = (num1 * num5 + num2 * num6 + num3 * num7);

            result.X = (num1 * num8 + num5 * num4) + num9;
            result.Y = (num2 * num8 + num6 * num4) + num10;
            result.Z = (num3 * num8 + num7 * num4) + num11;
            result.W = num4 * num8 - num12;
        }
Example #41
0
 /// <summary>
 /// Subtracts a quaternion from another quaternion.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param>
 public static QuaternionD Subtract(QuaternionD quaternion1, QuaternionD quaternion2)
 {
     QuaternionD quaternion;
     quaternion.X = quaternion1.X - quaternion2.X;
     quaternion.Y = quaternion1.Y - quaternion2.Y;
     quaternion.Z = quaternion1.Z - quaternion2.Z;
     quaternion.W = quaternion1.W - quaternion2.W;
     return quaternion;
 }
Example #42
0
 /// <summary>
 /// Returns the conjugate of a specified QuaternionD.
 /// </summary>
 /// <param name="value">The QuaternionD of which to return the conjugate.</param><param name="result">[OutAttribute] An existing QuaternionD filled in to be the conjugate of the specified one.</param>
 public static void Conjugate(ref QuaternionD value, out QuaternionD result)
 {
     result.X = -value.X;
     result.Y = -value.Y;
     result.Z = -value.Z;
     result.W = value.W;
 }
Example #43
0
 /// <summary>
 /// Multiplies two quaternions.
 /// </summary>
 /// <param name="quaternion1">The quaternion on the left of the multiplication.</param><param name="quaternion2">The quaternion on the right of the multiplication.</param>
 public static QuaternionD Multiply(QuaternionD quaternion1, QuaternionD quaternion2)
 {
     double num1 = quaternion1.X;
     double num2 = quaternion1.Y;
     double num3 = quaternion1.Z;
     double num4 = quaternion1.W;
     double num5 = quaternion2.X;
     double num6 = quaternion2.Y;
     double num7 = quaternion2.Z;
     double num8 = quaternion2.W;
     double num9 = (num2 * num7 - num3 * num6);
     double num10 = (num3 * num5 - num1 * num7);
     double num11 = (num1 * num6 - num2 * num5);
     double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
     QuaternionD quaternion;
     quaternion.X = (num1 * num8 + num5 * num4) + num9;
     quaternion.Y = (num2 * num8 + num6 * num4) + num10;
     quaternion.Z = (num3 * num8 + num7 * num4) + num11;
     quaternion.W = num4 * num8 - num12;
     return quaternion;
 }
Example #44
0
 /// <summary>
 /// Returns the inverse of a QuaternionD.
 /// </summary>
 /// <param name="quaternion">Source QuaternionD.</param>
 public static QuaternionD Inverse(QuaternionD quaternion)
 {
     double num = 1.0 / (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
     QuaternionD quaternion1;
     quaternion1.X = -quaternion.X * num;
     quaternion1.Y = -quaternion.Y * num;
     quaternion1.Z = -quaternion.Z * num;
     quaternion1.W = quaternion.W * num;
     return quaternion1;
 }
Example #45
0
 /// <summary>
 /// Multiplies a quaternion by a scalar value.
 /// </summary>
 /// <param name="quaternion1">Source quaternion.</param><param name="scaleFactor">Scalar value.</param>
 public static QuaternionD Multiply(QuaternionD quaternion1, double scaleFactor)
 {
     QuaternionD quaternion;
     quaternion.X = quaternion1.X * scaleFactor;
     quaternion.Y = quaternion1.Y * scaleFactor;
     quaternion.Z = quaternion1.Z * scaleFactor;
     quaternion.W = quaternion1.W * scaleFactor;
     return quaternion;
 }
Example #46
0
 /// <summary>
 /// Returns the inverse of a QuaternionD.
 /// </summary>
 /// <param name="quaternion">Source QuaternionD.</param><param name="result">[OutAttribute] The inverse of the QuaternionD.</param>
 public static void Inverse(ref QuaternionD quaternion, out QuaternionD result)
 {
     double num = 1.0 / (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
     result.X = -quaternion.X * num;
     result.Y = -quaternion.Y * num;
     result.Z = -quaternion.Z * num;
     result.W = quaternion.W * num;
 }
Example #47
0
 /// <summary>
 /// Divides a QuaternionD by another QuaternionD.
 /// </summary>
 /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">The divisor.</param>
 public static QuaternionD Divide(QuaternionD quaternion1, QuaternionD quaternion2)
 {
     double num1 = quaternion1.X;
     double num2 = quaternion1.Y;
     double num3 = quaternion1.Z;
     double num4 = quaternion1.W;
     double num5 = 1.0 / (quaternion2.X * quaternion2.X + quaternion2.Y * quaternion2.Y + quaternion2.Z * quaternion2.Z + quaternion2.W * quaternion2.W);
     double num6 = -quaternion2.X * num5;
     double num7 = -quaternion2.Y * num5;
     double num8 = -quaternion2.Z * num5;
     double num9 = quaternion2.W * num5;
     double num10 = (num2 * num8 - num3 * num7);
     double num11 = (num3 * num6 - num1 * num8);
     double num12 = (num1 * num7 - num2 * num6);
     double num13 = (num1 * num6 + num2 * num7 + num3 * num8);
     QuaternionD quaternion;
     quaternion.X = (num1 * num9 + num6 * num4) + num10;
     quaternion.Y = (num2 * num9 + num7 * num4) + num11;
     quaternion.Z = (num3 * num9 + num8 * num4) + num12;
     quaternion.W = num4 * num9 - num13;
     return quaternion;
 }
Example #48
0
 /// <summary>
 /// Creates a QuaternionD from a vector and an angle to rotate about the vector.
 /// </summary>
 /// <param name="axis">The vector to rotate around.</param><param name="angle">The angle to rotate around the vector.</param><param name="result">[OutAttribute] The created QuaternionD.</param>
 public static void CreateFromAxisAngle(ref Vector3D axis, double angle, out QuaternionD result)
 {
     double num1 = angle * 0.5;
     double num2 = Math.Sin(num1);
     double num3 = Math.Cos(num1);
     result.X = axis.X * num2;
     result.Y = axis.Y * num2;
     result.Z = axis.Z * num2;
     result.W = num3;
 }
        /// <summary>
        /// Divides a QuaternionD by another QuaternionD.
        /// </summary>
        /// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">The divisor.</param><param name="result">[OutAttribute] Result of the division.</param>
        public static void Divide(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
        {
            double num1  = quaternion1.X;
            double num2  = quaternion1.Y;
            double num3  = quaternion1.Z;
            double num4  = quaternion1.W;
            double num5  = 1.0 / (quaternion2.X * quaternion2.X + quaternion2.Y * quaternion2.Y + quaternion2.Z * quaternion2.Z + quaternion2.W * quaternion2.W);
            double num6  = -quaternion2.X * num5;
            double num7  = -quaternion2.Y * num5;
            double num8  = -quaternion2.Z * num5;
            double num9  = quaternion2.W * num5;
            double num10 = (num2 * num8 - num3 * num7);
            double num11 = (num3 * num6 - num1 * num8);
            double num12 = (num1 * num7 - num2 * num6);
            double num13 = (num1 * num6 + num2 * num7 + num3 * num8);

            result.X = (num1 * num9 + num6 * num4) + num10;
            result.Y = (num2 * num9 + num7 * num4) + num11;
            result.Z = (num3 * num9 + num8 * num4) + num12;
            result.W = num4 * num9 - num13;
        }
Example #50
0
 /// <summary>
 /// Creates a new QuaternionD from specified yaw, pitch, and roll angles.
 /// </summary>
 /// <param name="yaw">The yaw angle, in radians, around the y-axis.</param><param name="pitch">The pitch angle, in radians, around the x-axis.</param><param name="roll">The roll angle, in radians, around the z-axis.</param><param name="result">[OutAttribute] An existing QuaternionD filled in to express the specified yaw, pitch, and roll angles.</param>
 public static void CreateFromYawPitchRoll(double yaw, double pitch, double roll, out QuaternionD result)
 {
     double num1 = roll * 0.5;
     double num2 = Math.Sin(num1);
     double num3 = Math.Cos(num1);
     double num4 = pitch * 0.5;
     double num5 = Math.Sin(num4);
     double num6 = Math.Cos(num4);
     double num7 = yaw * 0.5;
     double num8 = Math.Sin(num7);
     double num9 = Math.Cos(num7);
     result.X = (num9 * num5 * num3 + num8 * num6 * num2);
     result.Y = (num8 * num6 * num3 - num9 * num5 * num2);
     result.Z = (num9 * num6 * num2 - num8 * num5 * num3);
     result.W = (num9 * num6 * num3 + num8 * num5 * num2);
 }
            private void UpdateViewRange(MyPlanet planet)
            {
                var pos = MySector.MainCamera.Position;

                double dist = double.MaxValue;

                foreach (var p in MyPlanets.GetPlanets())
                {
                    double dsq = Vector3D.DistanceSquared(pos, p.WorldMatrix.Translation);
                    if (dsq < dist)
                    {
                        planet = p;
                        dist = dsq;
                    }
                }

                var radius = planet.MinimumRadius;
                double altitude;

                m_height = planet.MaximumRadius - radius;

                var center = planet.WorldMatrix.Translation;


                m_radius = HyperSphereHelpers.DistanceToTangentProjected(ref center, ref pos, radius, out altitude);

                var up = center - pos;
                up.Normalize();

                m_center = pos + up * altitude;

                var forward = Vector3D.CalculatePerpendicularVector(up);

                m_orientation = QuaternionD.CreateFromForwardUp(forward, up);
            }
Example #52
0
        /// <summary>
        /// Works for normalized vectors only
        /// </summary>
        public static QuaternionD CreateFromForwardUp(Vector3D forward, Vector3D up)
        {
            Vector3D vector = -forward;
            Vector3D vector2 = Vector3D.Cross(up, vector);
            Vector3D vector3 = Vector3D.Cross(vector, vector2);
            var m00 = vector2.X;
            var m01 = vector2.Y;
            var m02 = vector2.Z;
            var m10 = vector3.X;
            var m11 = vector3.Y;
            var m12 = vector3.Z;
            var m20 = vector.X;
            var m21 = vector.Y;
            var m22 = vector.Z;

            double num8 = (m00 + m11) + m22;
            var quaternion = new QuaternionD();
            if (num8 > 0)
            {
                var num = Math.Sqrt(num8 + 1.0);
                quaternion.W = num * 0.5;
                num = 0.5 / num;
                quaternion.X = (m12 - m21) * num;
                quaternion.Y = (m20 - m02) * num;
                quaternion.Z = (m01 - m10) * num;
                return quaternion;
            }
            if ((m00 >= m11) && (m00 >= m22))
            {
                var num7 = Math.Sqrt(((1.0 + m00) - m11) - m22);
                var num4 = 0.5 / num7;
                quaternion.X = 0.5 * num7;
                quaternion.Y = (m01 + m10) * num4;
                quaternion.Z = (m02 + m20) * num4;
                quaternion.W = (m12 - m21) * num4;
                return quaternion;
            }
            if (m11 > m22)
            {
                var num6 = Math.Sqrt(((1.0 + m11) - m00) - m22);
                var num3 = 0.5 / num6;
                quaternion.X = (m10 + m01) * num3;
                quaternion.Y = 0.5 * num6;
                quaternion.Z = (m21 + m12) * num3;
                quaternion.W = (m20 - m02) * num3;
                return quaternion;
            }
            var num5 = Math.Sqrt(((1.0 + m22) - m00) - m11);
            var num2 = 0.5 / num5;
            quaternion.X = (m20 + m02) * num2;
            quaternion.Y = (m21 + m12) * num2;
            quaternion.Z = 0.5 * num5;
            quaternion.W = (m01 - m10) * num2;
            return quaternion;
        }
 public static Vector3D Transform(this Vector3D value, QuaternionD rotation)
 {
     double num = (double)(rotation.X + rotation.X);
     double num2 = (double)(rotation.Y + rotation.Y);
     double num3 = (double)(rotation.Z + rotation.Z);
     double num4 = (double)rotation.W * num;
     double num5 = (double)rotation.W * num2;
     double num6 = (double)rotation.W * num3;
     double num7 = (double)rotation.X * num;
     double num8 = (double)rotation.X * num2;
     double num9 = (double)rotation.X * num3;
     double num10 = (double)rotation.Y * num2;
     double num11 = (double)rotation.Y * num3;
     double num12 = (double)rotation.Z * num3;
     double x = value.X * (1.0 - num10 - num12) + value.Y * (num8 - num6) + value.Z * (num9 + num5);
     double y = value.X * (num8 + num6) + value.Y * (1.0 - num7 - num12) + value.Z * (num11 - num4);
     double z = value.X * (num9 - num5) + value.Y * (num11 + num4) + value.Z * (1.0 - num7 - num10);
     Vector3D result;
     result.X = x;
     result.Y = y;
     result.Z = z;
     return result;
 }
Example #54
0
 public static bool IsEqual(QuaternionD value1, QuaternionD value2) =>
 (((IsZero((double)(value1.X - value2.X), 1E-05f) && IsZero((double)(value1.Y - value2.Y), 1E-05f)) && IsZero((double)(value1.Z - value2.Z), 1E-05f)) && IsZero((double)(value1.W - value2.W), 1E-05f));