Esempio n. 1
0
        /// <summary>
        /// Scale the given quaternion to unit length
        /// </summary>
        /// <param name="q">The quaternion to normalize</param>
        /// <returns>The normalized quaternion</returns>
        static QuaternionRightHand Normalize(QuaternionRightHand q)
        {
            float scale = 1.0f / q.Length;
            QuaternionRightHand result = new QuaternionRightHand(q.xyz * scale, q.w * scale);

            return(result);
        }
        /// <summary>
        /// 可以用来计算ojbtoworld矩阵
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="q"></param>
        /// <param name="s"></param>
        /// <returns></returns>
        public static Matrix4x4RightHand TRS(Vector3RightHand pos, QuaternionRightHand q, Vector3RightHand s)
        {
            Matrix4x4RightHand posMatrix = Matrix4x4RightHand.identity;

            posMatrix.m03 = pos.x;
            posMatrix.m13 = pos.y;
            posMatrix.m23 = pos.z;

            Matrix4x4RightHand rotateMatrix = Matrix4x4RightHand.identity;

            rotateMatrix.m00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
            rotateMatrix.m10 = 2 * q.x * q.y + 2 * q.w * q.z;
            rotateMatrix.m20 = 2 * q.x * q.z - 2 * q.w * q.y;

            rotateMatrix.m01 = 2 * q.x * q.y - 2 * q.w * q.z;
            rotateMatrix.m11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
            rotateMatrix.m21 = 2 * q.y * q.z + 2 * q.w * q.x;

            rotateMatrix.m02 = 2 * q.x * q.z + 2 * q.w * q.y;
            rotateMatrix.m12 = 2 * q.y * q.z - 2 * q.w * q.x;
            rotateMatrix.m22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;

            Matrix4x4RightHand scaleMatrix = Scale(s);

            Matrix4x4RightHand ret = posMatrix * rotateMatrix * scaleMatrix;

            return(ret);
        }
Esempio n. 3
0
        public static QuaternionRightHand LookRotation(Vector3RightHand forward, Vector3RightHand up)
        {
            forward = Vector3RightHand.Normalize(forward);
            Vector3RightHand right = Vector3RightHand.Normalize(Vector3RightHand.Cross(up, forward));

            up = Vector3RightHand.Cross(forward, right);
            var m00 = right.x;
            var m01 = right.y;
            var m02 = right.z;
            var m10 = up.x;
            var m11 = up.y;
            var m12 = up.z;
            var m20 = forward.x;
            var m21 = forward.y;
            var m22 = forward.z;


            float num8       = (m00 + m11) + m22;
            var   quaternion = new QuaternionRightHand();

            if (num8 > 0f)
            {
                var num = (float)Mathf.Sqrt(num8 + 1f);
                quaternion.w = num * 0.5f;
                num          = 0.5f / 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 = (float)Mathf.Sqrt(((1f + m00) - m11) - m22);
                var num4 = 0.5f / num7;
                quaternion.x = 0.5f * num7;
                quaternion.y = (m01 + m10) * num4;
                quaternion.z = (m02 + m20) * num4;
                quaternion.w = (m12 - m21) * num4;
                return(quaternion);
            }
            if (m11 > m22)
            {
                var num6 = (float)Mathf.Sqrt(((1f + m11) - m00) - m22);
                var num3 = 0.5f / num6;
                quaternion.x = (m10 + m01) * num3;
                quaternion.y = 0.5f * num6;
                quaternion.z = (m21 + m12) * num3;
                quaternion.w = (m20 - m02) * num3;
                return(quaternion);
            }
            var num5 = (float)Mathf.Sqrt(((1f + m22) - m00) - m11);
            var num2 = 0.5f / num5;

            quaternion.x = (m20 + m02) * num2;
            quaternion.y = (m21 + m12) * num2;
            quaternion.z = 0.5f * num5;
            quaternion.w = (m01 - m10) * num2;
            return(quaternion);
        }
Esempio n. 4
0
        public override bool Equals(object other)
        {
            if (!(other is QuaternionRightHand))
            {
                return(false);
            }
            QuaternionRightHand quaternion = (QuaternionRightHand)other;

            return(this.x.Equals(quaternion.x) && this.y.Equals(quaternion.y) && this.z.Equals(quaternion.z) && this.w.Equals(quaternion.w));
        }
Esempio n. 5
0
        public static QuaternionRightHand Inverse(QuaternionRightHand rotation)
        {
            float lengthSq = rotation.LengthSquared;

            if (lengthSq != 0.0)
            {
                float i = 1.0f / lengthSq;
                return(new QuaternionRightHand(rotation.xyz * -i, rotation.w * i));
            }
            return(rotation);
        }
Esempio n. 6
0
 public static QuaternionRightHand Slerp(QuaternionRightHand a, QuaternionRightHand b, float t)
 {
     if (t > 1)
     {
         t = 1;
     }
     if (t < 0)
     {
         t = 0;
     }
     return(SlerpUnclamped(a, b, t));
 }
Esempio n. 7
0
        public static QuaternionRightHand RotateTowards(QuaternionRightHand from, QuaternionRightHand to, float maxDegreesDelta)
        {
            float num = QuaternionRightHand.Angle(from, to);

            if (num == 0f)
            {
                return(to);
            }
            float t = Mathf.Min(1f, maxDegreesDelta / num);

            return(QuaternionRightHand.SlerpUnclamped(from, to, t));
        }
Esempio n. 8
0
 public static QuaternionRightHand Lerp(QuaternionRightHand from, QuaternionRightHand to, float t)
 {
     if (t > 1)
     {
         t = 1;
     }
     if (t < 0)
     {
         t = 0;
     }
     return(Slerp(from, to, t));
 }
Esempio n. 9
0
        //static Quaternion FromToRotation(Vector3 fromDiection, Vector3 toDirection)
        //根据两个向量计算出旋转量,计算出来的旋转量为从fromDiection,旋转到toDirection的旋转量。
        //这句话意思很明显了。就是计算旋转量。
        //那么LookRotation(Vector3 forward)计算的是,Z轴旋转到forward的旋转量。
        //推出:Quaternion.LookRotation(new Vector3(1,0,0)) == Quaternion.FromToRotation(Vector3.forward, new Vector3(1,0,0));
        //因为前者就是计算向前向量到当前向量(1,0,0)的旋转量的,其实现过程就是后者喽。

        public static QuaternionRightHand FromToRotation(Vector3RightHand v1, Vector3RightHand v2)
        {
            //         fromDirection.Normalize();
            //         toDirection.Normalize();
            //         Quaternion fromQua = LookRotation(fromDirection);
            //         Quaternion toQua = LookRotation(toDirection);
            //         fromQua = Normalize(fromQua);
            //         toQua = Normalize(toQua);

            //Vector3 fromDir = fromQua * Vector3.forward;
            //Vector3 toDir = toQua * Vector3.forward;

            //Quaternion ret = RotateTowards(fromQua, toQua, float.MaxValue);
            //return ret;
            return(QuaternionRightHand.AngleAxis(Vector3RightHand.Angle(v1, v2), Vector3RightHand.Cross(v1, v2)));
        }
Esempio n. 10
0
        static void ToAxisAngleRad(QuaternionRightHand q, out Vector3RightHand axis, out float angle)
        {
            if (Mathf.Abs(q.w) > 1.0f)
            {
                q.Normalize();
            }
            angle = 2.0f * (float)Mathf.Acos(q.w); // angle
            float den = (float)Mathf.Sqrt(1.0f - q.w * q.w);

            if (den > 0.0001f)
            {
                axis = q.xyz / den;
            }
            else
            {
                // This occurs when the angle is zero.
                // Not a problem: just set an arbitrary normalized axis.
                axis = new Vector3RightHand(1, 0, 0);
            }
        }
Esempio n. 11
0
        public static QuaternionRightHand AngleAxis(float degress, Vector3RightHand axis)
        {
            if (axis.sqrMagnitude == 0.0f)
            {
                return(identity);
            }

            QuaternionRightHand result = identity;
            var radians = degress * Mathf.Deg2Rad;

            radians *= 0.5f;
            axis.Normalize();
            axis     = axis * (float)Mathf.Sin(radians);
            result.x = axis.x;
            result.y = axis.y;
            result.z = axis.z;
            result.w = (float)Mathf.Cos(radians);

            return(Normalize(result));
        }
Esempio n. 12
0
        public static void Test()
        {
            Vector3RightHand    pos    = new Vector3RightHand(100, 200, 300);
            QuaternionRightHand rotate = QuaternionRightHand.identity;

            rotate.eulerAngles = new Vector3RightHand(40, 50, 60);
            //Console.WriteLine("测试Quaternion " + rotate);

            Vector3RightHand   scale = new Vector3RightHand(7, 8, 9);
            Matrix4x4RightHand trs   = TRS(pos, rotate, scale);

            Console.WriteLine("测试trs\n" + trs.ToString());

            Matrix4x4RightHand transpose = Transpose(trs);

            Console.WriteLine("测试转置矩阵\n" + transpose.ToString());

            Matrix4x4RightHand inverse = Inverse(trs);

            Console.WriteLine("测试逆矩阵\n" + inverse.ToString());

            Matrix4x4RightHand posMatrix = Matrix4x4RightHand.identity;

            posMatrix.m03 = pos.x;
            posMatrix.m13 = pos.y;
            posMatrix.m23 = pos.z;
            Vector4 point    = new Vector4(0, 0, 0, 1);
            Vector4 newPoint = posMatrix * point;

            float fov    = 60;
            float aspect = (float)16 / 9;
            float zNear  = 1;
            float zFar   = 1000;
            Matrix4x4RightHand perspectiveMat = Matrix4x4RightHand.Perspective(fov, aspect, zNear, zFar);

            Console.WriteLine("测试透视矩阵\n" + perspectiveMat.ToString());

            //Console.ReadLine();
        }
Esempio n. 13
0
 public static bool operator !=(QuaternionRightHand lhs, QuaternionRightHand rhs)
 {
     return(QuaternionRightHand.Dot(lhs, rhs) <= 0.999999f);
 }
Esempio n. 14
0
 public void ToAngleAxis(out float angle, out Vector3RightHand axis)
 {
     QuaternionRightHand.ToAxisAngleRad(this, out axis, out angle);
     angle *= Mathf.Rad2Deg;
 }
Esempio n. 15
0
        public static float Angle(QuaternionRightHand a, QuaternionRightHand b)
        {
            float f = QuaternionRightHand.Dot(a, b);

            return((float)Mathf.Acos(Mathf.Min(Mathf.Abs(f), 1f)) * 2f * 57.29578f);
        }
Esempio n. 16
0
        private static QuaternionRightHand SlerpUnclamped(QuaternionRightHand a, QuaternionRightHand b, float t)
        {
            // if either input is zero, return the other.
            if (a.LengthSquared == 0.0f)
            {
                if (b.LengthSquared == 0.0f)
                {
                    return(identity);
                }
                return(b);
            }
            else if (b.LengthSquared == 0.0f)
            {
                return(a);
            }


            float cosHalfAngle = a.w * b.w + Vector3RightHand.Dot(a.xyz, b.xyz);

            if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
            {
                // angle = 0.0f, so just return one input.
                return(a);
            }
            else if (cosHalfAngle < 0.0f)
            {
                b.xyz        = -b.xyz;
                b.w          = -b.w;
                cosHalfAngle = -cosHalfAngle;
            }

            float blendA;
            float blendB;

            if (cosHalfAngle < 0.99f)
            {
                // do proper slerp for big angles
                float halfAngle           = (float)Mathf.Acos(cosHalfAngle);
                float sinHalfAngle        = (float)Mathf.Sin(halfAngle);
                float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
                blendA = (float)Mathf.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
                blendB = (float)Mathf.Sin(halfAngle * t) * oneOverSinHalfAngle;
            }
            else
            {
                // do lerp if angle is really small.
                blendA = 1.0f - t;
                blendB = t;
            }

            QuaternionRightHand result = new QuaternionRightHand(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w);

            if (result.LengthSquared > 0.0f)
            {
                return(Normalize(result));
            }
            else
            {
                return(identity);
            }
        }
Esempio n. 17
0
        public static void Test()
        {
            QuaternionRightHand rotate = QuaternionRightHand.identity;

            rotate.eulerAngles = new Vector3RightHand(40, 50, 60);
            Debug.Log("测试euler to Quaternion \n" + rotate);

            Vector3RightHand euler = rotate.eulerAngles;

            Debug.Log("测试quaternion to euler \n" + euler);

            Vector3RightHand dir = new Vector3RightHand(0.3f, 0.4f, 0.5f);

            dir.Normalize();
            QuaternionRightHand lookRotation = QuaternionRightHand.LookRotation(dir, Vector3RightHand.up);

            Debug.Log("测试look rotation \n" + lookRotation);

            QuaternionRightHand rotate2 = QuaternionRightHand.identity;

            rotate2.eulerAngles = new Vector3RightHand(70, 80, 90);

            QuaternionRightHand slerp = QuaternionRightHand.Slerp(rotate, rotate2, 0.5f);

            Debug.Log("测试slerp \n" + slerp);

            //Console.ReadLine();

            float angle = Angle(rotate, rotate2);

            Debug.Log("测试angle " + angle);

            QuaternionRightHand angleAxis = AngleAxis(10, dir);

            Debug.Log("测试angleAxis " + angleAxis);

            Vector3RightHand dir2 = new Vector3RightHand(0.6f, 0.7f, 0.8f);

            dir2.Normalize();

            QuaternionRightHand rotateTowards = QuaternionRightHand.RotateTowards(QuaternionRightHand.LookRotation(dir), QuaternionRightHand.LookRotation(dir2), float.MaxValue);

            Debug.Log("测试RotateTowards " + rotateTowards);

            Vector3RightHand    dir3           = new Vector3RightHand(1, 0, 0);
            Vector3RightHand    dir4           = new Vector3RightHand(0, 0, 1);
            QuaternionRightHand fromToRotation = FromToRotation(dir, dir2);

            //Quaternion fromToRotation = FromToRotation(dir4, dir3);
            Debug.Log("测试FromToRotation " + fromToRotation);

            QuaternionRightHand inverstQua = QuaternionRightHand.Inverse(fromToRotation);

            Debug.Log("测试inverst " + inverstQua);

            QuaternionRightHand lerpQua = QuaternionRightHand.Lerp(rotate, rotate2, 0.5f);

            Debug.Log("测试lerp " + lerpQua);

            float            angle2 = 0;
            Vector3RightHand axis   = Vector3RightHand.zero;

            lerpQua.ToAngleAxis(out angle2, out axis);
            Debug.Log("测试ToAngleAxis " + angle2 + " " + axis);

            //Console.ReadLine();
        }
Esempio n. 18
0
        // 四元数转欧拉角
        static Vector3RightHand ToEulerRad(QuaternionRightHand q)
        {
            //             float sqw = rotation.w * rotation.w;
            //             float sqx = rotation.x * rotation.x;
            //             float sqy = rotation.y * rotation.y;
            //             float sqz = rotation.z * rotation.z;
            //             float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
            //             float test = rotation.x * rotation.w - rotation.y * rotation.z;
            //             Vector3 v;
            //
            //             if (test > 0.4995f * unit)
            //             { // singularity at north pole
            //                 v.y = 2f * (float)Mathf.Atan2(rotation.y, rotation.x);
            //                 v.x = (float)Mathf.PI / 2;
            //                 v.z = 0;
            //                 return NormalizeAngles(v * Mathf.Rad2Deg);
            //             }
            //             if (test < -0.4995f * unit)
            //             { // singularity at south pole
            //                 v.y = -2f * (float)Mathf.Atan2(rotation.y, rotation.x);
            //                 v.x = -(float)Mathf.PI / 2;
            //                 v.z = 0;
            //                 return NormalizeAngles(v * Mathf.Rad2Deg);
            //             }
            //             Quaternion q = new Quaternion(rotation.w, rotation.z, rotation.x, rotation.y);
            //             v.y = (float)Mathf.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w));     // Yaw
            //             v.x = (float)Mathf.Asin(2f * (q.x * q.z - q.w * q.y));                             // Pitch
            //             v.z = (float)Mathf.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z));      // Roll
            //             return NormalizeAngles(v * Mathf.Rad2Deg) * Mathf.Deg2Rad;
            float sqw = q.w * q.w;
            float sqx = q.x * q.x;
            float sqy = q.y * q.y;
            float sqz = q.z * q.z;

            float unit = sqx + sqy + sqz + sqw;
            float test = q.x * q.y + q.z * q.w;

            float yaw   = 0.0f;
            float pitch = 0.0f;
            float roll  = 0.0f;

            // North pole singularity
            if (test > 0.499f * unit)
            {
                yaw   = 2.0f * Mathf.Atan2(q.x, q.w);
                pitch = Mathf.PI * 0.5f;
                roll  = 0.0f;
            }

            // South pole singularity
            else if (test < -0.499f * unit)
            {
                yaw   = -2.0f * Mathf.Atan2(q.x, q.w);
                pitch = -Mathf.PI * 0.5f;
                roll  = 0.0f;
            }

            else
            {
                yaw   = Mathf.Atan2(2.0f * q.y * q.w - 2.0f * q.x * q.z, sqx - sqy - sqz + sqw);
                pitch = Mathf.Asin(2.0f * test / unit);
                roll  = Mathf.Atan2(2.0f * q.x * q.w - 2.0f * q.y * q.z, -sqx + sqy - sqz + sqw);
            }

            // Keep angles [0..360].
            if (Mathf.Sign(yaw) < 0)
            {
                yaw = Mathf.Deg2Rad * (360) + yaw;
            }
            if (Mathf.Sign(pitch) < 0)
            {
                pitch = Mathf.Deg2Rad * (360) + pitch;
            }
            if (Mathf.Sign(roll) < 0)
            {
                roll = Mathf.Deg2Rad * (360) + roll;
            }

            return(new Vector3RightHand(roll, yaw, pitch));
        }
Esempio n. 19
0
 public static float Dot(QuaternionRightHand a, QuaternionRightHand b)
 {
     return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
 }