Exemple #1
0
        public void FromToRotation(Vector3 from, Vector3 to, ref Matrix33 mtx)
        {
            Vector3 v = Vector3.Cross(from, to);
            tfloat  e = Vector3.Dot(from, to);

            if (e > (Math.ONE - Math.Epsilon))
            {
                mtx[0, 0] = Math.ONE; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ZERO;
                mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ONE; mtx[0, 2] = Math.ZERO;
                mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ONE;
            }
            else if (e < (new tfloat(-1) * Math.ONE + Math.Epsilon))
            {
                Vector3 up, left = Vector3.ZERO;
                tfloat  invlen;
                tfloat  fxx, fyy, fzz, fxy, fxz, fyz;
                tfloat  uxx, uyy, uzz, uxy, uxz, uyz;
                tfloat  lxx, lyy, lzz, lxy, lxz, lyz;

                left[0] = Math.ZERO; left[1] = from[2]; left[2] = -from[1];
                if (Vector3.Dot(left, left) < Math.Epsilon)
                {
                    left[0] = -from[2]; left[1] = Math.ZERO; left[2] = from[0];
                }

                invlen   = Math.ONE / Math.Sqrt(Vector3.Dot(left, left));
                left[0] *= invlen;
                left[1] *= invlen;
                left[2] *= invlen;
                up       = Vector3.Cross(left, from);

                fxx = -from[0] * from[0]; fyy = -from[1] * from[1]; fzz = -from[2] * from[2];
                fxy = -from[0] * from[1]; fxz = -from[0] * from[2]; fyz = -from[1] * from[2];

                uxx = up[0] * up[0]; uyy = up[1] * up[1]; uzz = up[2] * up[2];
                uxy = up[0] * up[1]; uxz = up[0] * up[2]; uyz = up[1] * up[2];

                lxx = -left[0] * left[0]; lyy = -left[1] * left[1]; lzz = -left[2] * left[2];
                lxy = -left[0] * left[1]; lxz = -left[0] * left[2]; lyz = -left[1] * left[2];
                /* symmetric matrix */
                mtx[0, 0] = fxx + uxx + lxx; mtx[0, 1] = fxy + uxy + lxy; mtx[0, 2] = fxz + uxz + lxz;
                mtx[1, 0] = mtx[0, 1]; mtx[1, 1] = fyy + uyy + lyy; mtx[1, 2] = fyz + uyz + lyz;
                mtx[2, 0] = mtx[0, 2]; mtx[2, 1] = mtx[1, 2]; mtx[2, 2] = fzz + uzz + lzz;
            }
            else
            {
                tfloat hvx, hvz, hvxy, hvxz, hvyz;
                tfloat h = (Math.ONE - e) / Vector3.Dot(v, v);
                hvx       = h * v[0];
                hvz       = h * v[2];
                hvxy      = hvx * v[1];
                hvxz      = hvx * v[2];
                hvyz      = hvz * v[1];
                mtx[0, 0] = e + hvx * v[0]; mtx[0, 1] = hvxy - v[2]; mtx[0, 2] = hvxz + v[1];
                mtx[1, 0] = hvxy + v[2]; mtx[1, 1] = e + h * v[1] * v[1]; mtx[1, 2] = hvyz - v[0];
                mtx[2, 0] = hvxz - v[1]; mtx[2, 1] = hvyz + v[0]; mtx[2, 2] = e + hvz * v[2];
            }
        }
Exemple #2
0
        public Matrix33 SetFromToRotation(Vector3 from, Vector3 to)
        {
            Matrix33 mtx = new Matrix33();

            this.FromToRotation(from, to, ref mtx);
            this[0, 0] = mtx[0, 0]; this[0, 1] = mtx[0, 1]; this[0, 2] = mtx[0, 2];
            this[1, 0] = mtx[1, 0]; this[1, 1] = mtx[1, 1]; this[1, 2] = mtx[1, 2];
            this[2, 0] = mtx[2, 0]; this[2, 1] = mtx[2, 1]; this[2, 2] = mtx[2, 2];
            return(mtx);
        }
Exemple #3
0
        public static void MatrixToQuaternion(Matrix33 mtx, ref Quaternion q)
        {
            tfloat fTrace = mtx[0, 0] + mtx[1, 1] + mtx[2, 2];
            tfloat fRoot;

            if (fTrace > Math.ZERO)
            {
                fRoot = Math.Sqrt(fTrace + Math.ONE); // 2w
                q.w   = 0.5f * fRoot;
                fRoot = 0.5f / fRoot;                 // 1/(4w)
                q.x   = (mtx[2, 1] - mtx[1, 2]) * fRoot;
                q.y   = (mtx[0, 2] - mtx[2, 0]) * fRoot;
                q.z   = (mtx[1, 0] - mtx[0, 1]) * fRoot;
            }
            else
            {
                int[] next = new int[3] {
                    1, 2, 0
                };

                int i = 0;
                if (mtx[1, 1] > mtx[0, 0])
                {
                    i = 1;
                }

                if (mtx[2, 2] > mtx[i, i])
                {
                    i = 2;
                }

                int j = next[i];
                int k = next[j];

                fRoot = Math.Sqrt(mtx[i, i] - mtx[j, j] - mtx[k, k] + Math.ONE);
                q[i]  = 0.5f / fRoot;
                q.w   = (mtx[k, j] - mtx[j, k]) * fRoot;
                q[j]  = (mtx[j, i] + mtx[i, j]) * fRoot;
                q[k]  = (mtx[k, i] + mtx[i, k]) * fRoot;
            }

            q = Normalize(q);
        }
Exemple #4
0
        public static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection)
        {
            tfloat lhsMag = fromDirection.magnitude;
            tfloat rhsMag = toDirection.magnitude;

            if (lhsMag < Math.Epsilon || rhsMag < Math.Epsilon)
            {
                return(identity);
            }

            //nomarl///
            Vector3 lhs = fromDirection / lhsMag;
            Vector3 rhs = toDirection / rhsMag;

            Matrix33 mtx = new Matrix33();

            mtx = mtx.SetFromToRotation(lhs, rhs);

            Quaternion q = new Quaternion();

            MatrixToQuaternion(mtx, ref q);
            return(q);
        }