Beispiel #1
0
        public Quaternion(CFrame cf)
        {
            CFrame matrix = (cf - cf.Position);

            float[] ac = cf.GetComponents();

            float m11 = ac[3], m12 = ac[4], m13 = ac[5],
                  m21 = ac[6], m22 = ac[7], m23 = ac[8],
                  m31 = ac[9], m32 = ac[10], m33 = ac[11];

            float trace = m11 + m22 + m33;

            if (trace > 0)
            {
                float s = (float)Math.Sqrt(1 + trace);
                float r = 0.5f / s;

                W = s * 0.5f;
                X = (m32 - m23) * r;
                Y = (m13 - m31) * r;
                Z = (m21 - m12) * r;
            }
            else
            {
                float big = Math.Max(Math.Max(m11, m22), m33);

                if (big == m11)
                {
                    float s = (float)Math.Sqrt(1 + m11 - m22 - m33);
                    float r = 0.5f / s;

                    W = (m32 - m23) * r;
                    X = 0.5f * s;
                    Y = (m21 + m12) * r;
                    Z = (m13 + m31) * r;
                }
                else if (big == m22)
                {
                    float s = (float)Math.Sqrt(1 - m11 + m22 - m33);
                    float r = 0.5f / s;

                    W = (m13 - m31) * r;
                    X = (m21 + m12) * r;
                    Y = 0.5f * s;
                    Z = (m32 + m23) * r;
                }
                else if (big == m33)
                {
                    float s = (float)Math.Sqrt(1 - m11 - m22 + m33);
                    float r = 0.5f / s;

                    W = (m21 - m12) * r;
                    X = (m13 + m31) * r;
                    Y = (m32 + m23) * r;
                    Z = 0.5f * s;
                }
            }
        }