Ejemplo n.º 1
0
        /// <summary>
        /// 矩阵转四元数
        /// </summary>
        /// <param name="q">输出</param>
        /// <param name="m">输入矩阵</param>
        /// <returns></returns>
        public static FixedQuaternion MatrixQuat(this FixedMatrix4x4 m)
        {
            FixedQuaternion q = new FixedQuaternion();
            Fixed           s;
            Fixed           F25 = new Fixed(25, 100);

            if (m.m00 > m.m11 && m.m00 > m.m22)
            {
                s    = 2 * FixedMathf.Sqrt(1f + m.m00 - m.m11 - m.m22);
                q[0] = F25 * s;
                q[1] = (m.m10 + m.m01) / s;
                q[2] = (m.m02 + m.m20) / s;
                q[3] = (m.m21 - m.m12) / s;
            }
            else if (m.m11 > m.m22)
            {
                s    = 2 * FixedMathf.Sqrt(1f + m.m11 - m.m00 - m.m22);
                q[0] = (m.m10 + m.m01) / s;
                q[1] = F25 * s;
                q[2] = (m.m21 + m.m12) / s;
                q[3] = (m.m02 - m.m20) / s;
            }
            else
            {
                s    = 2 * FixedMathf.Sqrt(1f + m.m22 - m.m00 - m.m11);
                q[0] = (m.m02 + m.m20) / s;
                q[1] = (m.m21 + m.m12) / s;
                q[2] = F25 * s;
                q[3] = (m.m10 - m.m01) / s;
            }

            return(q);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// 四元数转矩阵
        /// </summary>
        /// <param name="m">输出矩阵</param>
        /// <param name="q">输入四元数</param>
        /// <returns></returns>
        public static FixedMatrix4x4 QuatMatrix(this FixedMatrix4x4 m, FixedQuaternion q)
        {
            Fixed xx, xy, xz, xw;
            Fixed yy, yz, yw;
            Fixed zz, zw;

            xx = q[0] * q[0];
            xy = q[0] * q[1];
            xz = q[0] * q[2];
            xw = q[0] * q[3];
            yy = q[1] * q[1];
            yz = q[1] * q[2];
            yw = q[1] * q[3];
            zz = q[2] * q[2];
            zw = q[2] * q[3];

            m.m00 = 1 - 2 * (yy + zz);
            m.m01 = 2 * (xy - zw);
            m.m02 = 2 * (xz + yw);
            m.m03 = 0;
            m.m10 = 2 * (xy + zw);
            m.m11 = 1 - 2 * (xx + zz);
            m.m12 = 2 * (yz - xw);
            m.m13 = 0;
            m.m20 = 2 * (xz - yw);
            m.m21 = 2 * (yz + xw);
            m.m22 = 1 - 2 * (xx + yy);
            m.m23 = 0;
            m.m30 = 0;
            m.m31 = 0;
            m.m32 = 0;
            m.m33 = 1;

            return(m);
        }
Ejemplo n.º 3
0
        public static FixedQuaternion Lerp(FixedQuaternion a, FixedQuaternion b, Fixed f)
        {
            Fixed           tempF  = f;
            Fixed           temp1  = Fixed.One - tempF;
            FixedQuaternion result = new FixedQuaternion();
            Fixed           temp2  = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;

            if (temp2 >= Fixed.Zero)
            {
                result.x = temp1 * a.x + tempF * b.x;
                result.y = temp1 * a.y + tempF * b.y;
                result.z = temp1 * a.z + tempF * b.z;
                result.w = temp1 * a.w + tempF * b.w;
            }
            else
            {
                result.x = temp1 * a.x - tempF * b.x;
                result.y = temp1 * a.y - tempF * b.y;
                result.z = temp1 * a.z - tempF * b.z;
                result.w = temp1 * a.w - tempF * b.w;
            }
            Fixed temp3 = Fixed.One / result.magnitude;

            result.x *= temp3;
            result.y *= temp3;
            result.z *= temp3;
            result.w *= temp3;
            return(result);
        }
Ejemplo n.º 4
0
        public static FixedQuaternion LookRotation(FixedVector3 forward, FixedVector3 upwards)
        {
            forward = forward.normalized;
            FixedVector3 right = FixedVector3.Cross(upwards, forward).normalized;

            upwards = FixedVector3.Cross(forward, right);
            var rightX   = right.x;
            var rightY   = right.y;
            var rightZ   = right.z;
            var upwardsX = upwards.x;
            var upwardsY = upwards.y;
            var upwardsZ = upwards.z;
            var forwardX = forward.x;
            var forwardY = forward.y;
            var forwardZ = forward.z;


            Fixed temp1      = rightX + upwardsY + forwardZ;
            var   quaternion = new FixedQuaternion();

            if (temp1 > Fixed.Zero)
            {
                var num = FixedMathf.Sqrt(temp1 + Fixed.One);
                quaternion.w = num * Fixed.Half;
                num          = Fixed.Half / num;
                quaternion.x = (upwardsZ - forwardY) * num;
                quaternion.y = (forwardX - rightZ) * num;
                quaternion.z = (rightY - upwardsX) * num;
                return(quaternion);
            }
            if (rightX >= upwardsY && rightX >= forwardZ)
            {
                var temp2 = FixedMathf.Sqrt(Fixed.One + rightX - upwardsY - forwardZ);
                var temp3 = Fixed.Half / temp2;
                quaternion.x = Fixed.Half * temp2;
                quaternion.y = (rightY + upwardsX) * temp3;
                quaternion.z = (rightZ + forwardX) * temp3;
                quaternion.w = (upwardsZ - forwardY) * temp3;
                return(quaternion);
            }
            if (upwardsY > forwardZ)
            {
                var temp4 = FixedMathf.Sqrt(Fixed.One + upwardsY - rightX - forwardZ);
                var temp5 = Fixed.Half / temp4;
                quaternion.x = (upwardsX + rightY) * temp5;
                quaternion.y = Fixed.Half * temp4;
                quaternion.z = (forwardY + upwardsZ) * temp5;
                quaternion.w = (forwardX - rightZ) * temp5;
                return(quaternion);
            }
            var temp6 = FixedMathf.Sqrt(Fixed.One + forwardZ - rightX - upwardsY);
            var temp7 = Fixed.Half / temp6;

            quaternion.x = (forwardX + rightZ) * temp7;
            quaternion.y = (forwardY + upwardsZ) * temp7;
            quaternion.z = Fixed.Half * temp6;
            quaternion.w = (rightY - upwardsX) * temp7;
            return(quaternion);
        }
Ejemplo n.º 5
0
 public FixedMatrix4x4(FixedQuaternion column0, FixedQuaternion column1, FixedQuaternion column2, FixedQuaternion column3)
 {
     this.m00 = column0.x;
     this.m01 = column1.x;
     this.m02 = column2.x;
     this.m03 = column3.x;
     this.m10 = column0.y;
     this.m11 = column1.y;
     this.m12 = column2.y;
     this.m13 = column3.y;
     this.m20 = column0.z;
     this.m21 = column1.z;
     this.m22 = column2.z;
     this.m23 = column3.z;
     this.m30 = column0.w;
     this.m31 = column1.w;
     this.m32 = column2.w;
     this.m33 = column3.w;
 }
Ejemplo n.º 6
0
 public bool Equals(FixedQuaternion other)
 {
     return(x == other.x && y == other.y && z == other.z && w == other.w);
 }
Ejemplo n.º 7
0
        public static Fixed Angle(FixedQuaternion a, FixedQuaternion b)
        {
            Fixed f = Dot(a, b);

            return(FixedMathf.Acos(FixedMathf.Min(FixedMathf.Abs(f), Fixed.One).AsFloat()) * new Fixed(2) * Fixed.Rad2Deg);
        }
Ejemplo n.º 8
0
        public static FixedQuaternion Slerp(FixedQuaternion a, FixedQuaternion b, Fixed f)
        {
            if (a.sqrMagnitude == Fixed.Zero)
            {
                if (b.sqrMagnitude == Fixed.Zero)
                {
                    return(Identity);
                }
                return(b);
            }
            if (b.sqrMagnitude == Fixed.Zero)
            {
                return(a);
            }

            if (f > Fixed.One)
            {
                f = Fixed.One;
            }
            if (f < Fixed.Zero)
            {
                f = Fixed.Zero;
            }

            Fixed cosHalfAngle = a.w * b.w + FixedVector3.Dot(new FixedVector3(a.x, a.y, a.z), new FixedVector3(b.x, b.y, b.z));

            if (cosHalfAngle >= Fixed.One || cosHalfAngle <= -Fixed.One)
            {
                return(a);
            }
            if (cosHalfAngle < Fixed.Zero)
            {
                b.x          = -b.x;
                b.y          = -b.y;
                b.z          = -b.z;
                b.w          = -b.w;
                cosHalfAngle = -cosHalfAngle;
            }

            Fixed blendA;
            Fixed blendB;

            if (cosHalfAngle < 0.999999)
            {
                Fixed halfAngle           = FixedMathf.Acos(cosHalfAngle);
                Fixed sinHalfAngle        = FixedMathf.Sin(halfAngle);
                Fixed oneOverSinHalfAngle = Fixed.One / sinHalfAngle;
                blendA = FixedMathf.Sin(halfAngle * (Fixed.One - f)) * oneOverSinHalfAngle;
                blendB = FixedMathf.Sin(halfAngle * f) * oneOverSinHalfAngle;
            }
            else
            {
                blendA = Fixed.One - f;
                blendB = f;
            }
            FixedQuaternion result =
                new FixedQuaternion(blendA * new FixedVector3(a.x, a.y, a.z) + blendB * new FixedVector3(b.x, b.y, b.z),
                                    blendA * a.w + blendB * b.w);

            if (result.sqrMagnitude > Fixed.Zero)
            {
                return(Normalize(result));
            }
            return(Identity);
        }
Ejemplo n.º 9
0
 public static Fixed Dot(FixedQuaternion a, FixedQuaternion b)
 {
     return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
 }
Ejemplo n.º 10
0
        public static FixedQuaternion Normalize(FixedQuaternion c)
        {
            Fixed scale = Fixed.One / c.magnitude;

            return(new FixedQuaternion(c.x * scale, c.y * scale, c.z * scale, c.w * scale));
        }