Beispiel #1
0
 /// <summary>
 /// Quaternions are added.
 /// </summary>
 /// <param name="quaternion1">The first quaternion.</param>
 /// <param name="quaternion2">The second quaternion.</param>
 /// <param name="result">The sum of both quaternions.</param>
 public static void Add(ref Fixed64Quaternion quaternion1, ref Fixed64Quaternion quaternion2, out Fixed64Quaternion 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;
 }
Beispiel #2
0
 /// <summary>
 /// Quaternions are subtracted.
 /// </summary>
 /// <param name="quaternion1">The first quaternion.</param>
 /// <param name="quaternion2">The second quaternion.</param>
 /// <param name="result">The difference of both quaternions.</param>
 public static void Subtract(ref Fixed64Quaternion quaternion1, ref Fixed64Quaternion quaternion2, out Fixed64Quaternion 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;
 }
Beispiel #3
0
 /// <summary>
 /// Scale a quaternion
 /// </summary>
 /// <param name="quaternion1">The quaternion to scale.</param>
 /// <param name="scaleFactor">Scale factor.</param>
 /// <param name="result">The scaled quaternion.</param>
 public static void Multiply(ref Fixed64Quaternion quaternion1, Fixed64 scaleFactor, out Fixed64Quaternion result)
 {
     result.x = quaternion1.x * scaleFactor;
     result.y = quaternion1.y * scaleFactor;
     result.z = quaternion1.z * scaleFactor;
     result.w = quaternion1.w * scaleFactor;
 }
Beispiel #4
0
        public static Fixed64Quaternion LerpUnclamped(Fixed64Quaternion a, Fixed64Quaternion b, Fixed64 t)
        {
            Fixed64Quaternion result = Multiply(a, (1 - t)) + Multiply(b, t);

            result.Normalize();

            return(result);
        }
Beispiel #5
0
        public static Fixed64Quaternion Conjugate(Fixed64Quaternion value)
        {
            Fixed64Quaternion quaternion;

            quaternion.x = -value.x;
            quaternion.y = -value.y;
            quaternion.z = -value.z;
            quaternion.w = value.w;
            return(quaternion);
        }
Beispiel #6
0
        public static Fixed64Quaternion FromToRotation(Fixed64Vector3 fromVector, Fixed64Vector3 toVector)
        {
            Fixed64Vector3    w = Fixed64Vector3.Cross(fromVector, toVector);
            Fixed64Quaternion q = new Fixed64Quaternion(w.x, w.y, w.z, Fixed64Vector3.Dot(fromVector, toVector));

            q.w += Fixed64.Sqrt(fromVector.SqrMagnitude * toVector.SqrMagnitude);
            q.Normalize();

            return(q);
        }
Beispiel #7
0
        public static Fixed64 Angle(Fixed64Quaternion a, Fixed64Quaternion b)
        {
            Fixed64Quaternion aInv = Inverse(a);
            Fixed64Quaternion f    = b * aInv;

            Fixed64 angle = Fixed64.Acos(f.w) * 2 * Fixed64.Rad2Deg;

            if (angle > 180)
            {
                angle = 360 - angle;
            }

            return(angle);
        }
Beispiel #8
0
        public static Fixed64Quaternion Slerp(Fixed64Quaternion from, Fixed64Quaternion to, Fixed64 t)
        {
            t = FixedMath.Clamp(t, 0, 1);

            Fixed64 dot = Dot(from, to);

            if (dot < 0.0f)
            {
                to  = Multiply(to, -1);
                dot = -dot;
            }

            Fixed64 halfTheta = Fixed64.Acos(dot);

            return(Multiply(Multiply(from, Fixed64.Sin((1 - t) * halfTheta)) + Multiply(to, Fixed64.Sin(t * halfTheta)), 1 / Fixed64.Sin(halfTheta)));
        }
Beispiel #9
0
        /// <summary>
        /// Multiply two quaternions.
        /// </summary>
        /// <param name="quaternion1">The first quaternion.</param>
        /// <param name="quaternion2">The second quaternion.</param>
        /// <param name="result">The product of both quaternions.</param>
        public static void Multiply(ref Fixed64Quaternion quaternion1, ref Fixed64Quaternion quaternion2, out Fixed64Quaternion result)
        {
            Fixed64 x     = quaternion1.x;
            Fixed64 y     = quaternion1.y;
            Fixed64 z     = quaternion1.z;
            Fixed64 w     = quaternion1.w;
            Fixed64 num4  = quaternion2.x;
            Fixed64 num3  = quaternion2.y;
            Fixed64 num2  = quaternion2.z;
            Fixed64 num   = quaternion2.w;
            Fixed64 num12 = (y * num2) - (z * num3);
            Fixed64 num11 = (z * num4) - (x * num2);
            Fixed64 num10 = (x * num3) - (y * num4);
            Fixed64 num9  = ((x * num4) + (y * num3)) + (z * num2);

            result.x = ((x * num) + (num4 * w)) + num12;
            result.y = ((y * num) + (num3 * w)) + num11;
            result.z = ((z * num) + (num2 * w)) + num10;
            result.w = (w * num) - num9;
        }
Beispiel #10
0
        /// <summary>
        /// Creates a quaternion from a matrix.
        /// </summary>
        /// <param name="matrix">A matrix representing an orientation.</param>
        /// <param name="result">JQuaternion representing an orientation.</param>
        public static void CreateFromMatrix(ref Fixed64Matrix matrix, out Fixed64Quaternion result)
        {
            Fixed64 num8 = (matrix.M11 + matrix.M22) + matrix.M33;

            if (num8 > Fixed64.Zero)
            {
                Fixed64 num = Fixed64.Sqrt((num8 + Fixed64.One));
                result.w = num * Fixed64.Half;
                num      = Fixed64.Half / num;
                result.x = (matrix.M23 - matrix.M32) * num;
                result.y = (matrix.M31 - matrix.M13) * num;
                result.z = (matrix.M12 - matrix.M21) * num;
            }
            else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
            {
                Fixed64 num7 = Fixed64.Sqrt((((Fixed64.One + matrix.M11) - matrix.M22) - matrix.M33));
                Fixed64 num4 = Fixed64.Half / num7;
                result.x = Fixed64.Half * num7;
                result.y = (matrix.M12 + matrix.M21) * num4;
                result.z = (matrix.M13 + matrix.M31) * num4;
                result.w = (matrix.M23 - matrix.M32) * num4;
            }
            else if (matrix.M22 > matrix.M33)
            {
                Fixed64 num6 = Fixed64.Sqrt((((Fixed64.One + matrix.M22) - matrix.M11) - matrix.M33));
                Fixed64 num3 = Fixed64.Half / num6;
                result.x = (matrix.M21 + matrix.M12) * num3;
                result.y = Fixed64.Half * num6;
                result.z = (matrix.M32 + matrix.M23) * num3;
                result.w = (matrix.M31 - matrix.M13) * num3;
            }
            else
            {
                Fixed64 num5 = Fixed64.Sqrt((((Fixed64.One + matrix.M33) - matrix.M11) - matrix.M22));
                Fixed64 num2 = Fixed64.Half / num5;
                result.x = (matrix.M31 + matrix.M13) * num2;
                result.y = (matrix.M32 + matrix.M23) * num2;
                result.z = Fixed64.Half * num5;
                result.w = (matrix.M12 - matrix.M21) * num2;
            }
        }
Beispiel #11
0
        public static Fixed64Quaternion RotateTowards(Fixed64Quaternion from, Fixed64Quaternion to, Fixed64 maxDegreesDelta)
        {
            Fixed64 dot = Dot(from, to);

            if (dot < 0.0f)
            {
                to  = Multiply(to, -1);
                dot = -dot;
            }

            Fixed64 halfTheta = Fixed64.Acos(dot);
            Fixed64 theta     = halfTheta * 2;

            maxDegreesDelta *= Fixed64.Deg2Rad;

            if (maxDegreesDelta >= theta)
            {
                return(to);
            }

            maxDegreesDelta /= theta;

            return(Multiply(Multiply(from, Fixed64.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, Fixed64.Sin(maxDegreesDelta * halfTheta)), 1 / Fixed64.Sin(halfTheta)));
        }
Beispiel #12
0
 static Fixed64Quaternion()
 {
     identity = new Fixed64Quaternion(0, 0, 0, 1);
 }
Beispiel #13
0
 /// <summary>
 /// Scale a quaternion
 /// </summary>
 /// <param name="quaternion1">The quaternion to scale.</param>
 /// <param name="scaleFactor">Scale factor.</param>
 /// <returns>The scaled quaternion.</returns>
 #region public static JQuaternion Multiply(JQuaternion quaternion1, FP scaleFactor)
 public static Fixed64Quaternion Multiply(Fixed64Quaternion quaternion1, Fixed64 scaleFactor)
 {
     Multiply(ref quaternion1, scaleFactor, out Fixed64Quaternion result);
     return(result);
 }
Beispiel #14
0
 /// <summary>
 /// Multiply two quaternions.
 /// </summary>
 /// <param name="quaternion1">The first quaternion.</param>
 /// <param name="quaternion2">The second quaternion.</param>
 /// <returns>The product of both quaternions.</returns>
 #region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
 public static Fixed64Quaternion Multiply(Fixed64Quaternion quaternion1, Fixed64Quaternion quaternion2)
 {
     Multiply(ref quaternion1, ref quaternion2, out Fixed64Quaternion result);
     return(result);
 }
Beispiel #15
0
 /// <summary>
 /// Quaternions are subtracted.
 /// </summary>
 /// <param name="quaternion1">The first quaternion.</param>
 /// <param name="quaternion2">The second quaternion.</param>
 /// <returns>The difference of both quaternions.</returns>
 #region public static JQuaternion Subtract(JQuaternion quaternion1, JQuaternion quaternion2)
 public static Fixed64Quaternion Subtract(Fixed64Quaternion quaternion1, Fixed64Quaternion quaternion2)
 {
     Subtract(ref quaternion1, ref quaternion2, out Fixed64Quaternion result);
     return(result);
 }
Beispiel #16
0
        public static void CreateFromYawPitchRoll(Fixed64 yaw, Fixed64 pitch, Fixed64 roll, out Fixed64Quaternion result)
        {
            Fixed64 num9 = roll * Fixed64.Half;
            Fixed64 num6 = Fixed64.Sin(num9);
            Fixed64 num5 = Fixed64.Cos(num9);
            Fixed64 num8 = pitch * Fixed64.Half;
            Fixed64 num4 = Fixed64.Sin(num8);
            Fixed64 num3 = Fixed64.Cos(num8);
            Fixed64 num7 = yaw * Fixed64.Half;
            Fixed64 num2 = Fixed64.Sin(num7);
            Fixed64 num  = Fixed64.Cos(num7);

            result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
            result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
            result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
            result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
        }
Beispiel #17
0
 /// <summary>
 /// Quaternions are added.
 /// </summary>
 /// <param name="quaternion1">The first quaternion.</param>
 /// <param name="quaternion2">The second quaternion.</param>
 /// <returns>The sum of both quaternions.</returns>
 #region public static JQuaternion Add(JQuaternion quaternion1, JQuaternion quaternion2)
 public static Fixed64Quaternion Add(Fixed64Quaternion quaternion1, Fixed64Quaternion quaternion2)
 {
     Add(ref quaternion1, ref quaternion2, out Fixed64Quaternion result);
     return(result);
 }
Beispiel #18
0
        public static Fixed64Quaternion Inverse(Fixed64Quaternion rotation)
        {
            Fixed64 invNorm = Fixed64.One / ((rotation.x * rotation.x) + (rotation.y * rotation.y) + (rotation.z * rotation.z) + (rotation.w * rotation.w));

            return(Multiply(Conjugate(rotation), invNorm));
        }
Beispiel #19
0
 public static Fixed64 Dot(Fixed64Quaternion a, Fixed64Quaternion b)
 {
     return(a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
 }
Beispiel #20
0
        public void SetFromToRotation(Fixed64Vector3 fromDirection, Fixed64Vector3 toDirection)
        {
            Fixed64Quaternion targetRotation = FromToRotation(fromDirection, toDirection);

            Set(targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w);
        }
Beispiel #21
0
        public static Fixed64Quaternion Lerp(Fixed64Quaternion a, Fixed64Quaternion b, Fixed64 t)
        {
            t = FixedMath.Clamp(t, Fixed64.Zero, Fixed64.One);

            return(LerpUnclamped(a, b, t));
        }