Exemple #1
0
        public static Matrix4d operator *(Matrix4d m1, Matrix4d m2)
        {
            Matrix4d ret = new Matrix4d();

            for (int i = 0; i < row_size; i++)
            {
                for (int j = 0; j < row_size; j++)
                {
                    ret[i, j] = 0.0f;
                    for (int k = 0; k < row_size; k++)
                    {
                        ret[i, j] += m1[i, k] * m2[k, j];
                    }
                }
            }
            return(ret);
        }
Exemple #2
0
        public static void FindRotAxisAngle(Matrix4d rotMat, out Vector3 rotAxis, out float angle)
        {
            float trace = rotMat[0, 0] + rotMat[1, 1] + rotMat[2, 2];

            angle = Mathf.Acos((trace - 1) / 2);

            if (rotMat[0, 1] > 0)
            {
                angle = -angle;                   /// this may be violated...
            }
            float sin2 = 2 * Mathf.Sin(angle);
            float x    = (rotMat[2, 1] - rotMat[1, 2]) / sin2;
            float y    = (rotMat[0, 2] - rotMat[2, 0]) / sin2;
            float z    = (rotMat[1, 0] - rotMat[0, 1]) / sin2;

            rotAxis = new Vector3(x, y, z).normalized;
        }
        public Matrix4d OuterCross(Vector4d v)
        {
            Matrix4d m = new Matrix4d();

            m[0, 0] = x * v.x;
            m[0, 1] = x * v.y;
            m[0, 2] = x * v.z;
            m[0, 3] = x * v.w;
            m[1, 0] = y * v.x;
            m[1, 1] = y * v.y;
            m[1, 2] = y * v.z;
            m[1, 3] = y * v.w;
            m[2, 0] = z * v.x;
            m[2, 1] = z * v.y;
            m[2, 2] = z * v.z;
            m[2, 3] = z * v.w;
            m[3, 0] = w * v.x;
            m[3, 1] = w * v.y;
            m[3, 2] = w * v.z;
            m[3, 3] = w * v.w;
            return(m);
        }
Exemple #4
0
        public static Matrix4d OuterCross(Vector4 v1, Vector4 v2)
        {
            Matrix4d m = new Matrix4d();

            m[0, 0] = v1.x * v2.x;
            m[0, 1] = v1.x * v2.y;
            m[0, 2] = v1.x * v2.z;
            m[0, 3] = v1.x * v2.w;
            m[1, 0] = v1.y * v2.x;
            m[1, 1] = v1.y * v2.y;
            m[1, 2] = v1.y * v2.z;
            m[1, 3] = v1.y * v2.w;
            m[2, 0] = v1.z * v2.x;
            m[2, 1] = v1.z * v2.y;
            m[2, 2] = v1.z * v2.z;
            m[2, 3] = v1.z * v2.w;
            m[3, 0] = v1.w * v2.x;
            m[3, 1] = v1.w * v2.y;
            m[3, 2] = v1.w * v2.z;
            m[3, 3] = v1.w * v2.w;
            return(m);
        }
Exemple #5
0
        private Matrix4d QuatToMatrix4d(Vector4d q)
        {
            double n = q.Dot(q);
            double s = (n > 0.0) ? (2.0 / n) : 0.0f;

            double xs, ys, zs;
            double wx, wy, wz;
            double xx, xy, xz;
            double yy, yz, zz;

            xs = q.x * s; ys = q.y * s; zs = q.z * s;
            wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
            xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
            yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;

            Matrix4d m = new Matrix4d();

            m[0, 0] = 1.0 - (yy + zz); m[1, 0] = xy - wz; m[2, 0] = xz + wy;
            m[0, 1] = xy + wz; m[1, 1] = 1.0 - (xx + zz); m[2, 1] = yz - wx;
            m[0, 2] = xz - wy; m[1, 2] = yz + wx; m[2, 2] = 1.0 - (xx + yy);
            m[3, 3] = 1.0;
            return(m);
        }
Exemple #6
0
        public Matrix4d GetMatrix()
        {
            if (type == MotionType.Rotation)
            {
                return(QuatToMatrix4d(quat));
            }

            if (type == MotionType.Scale)
            {
                Matrix4d m = Matrix4d.IdentityMatrix();
                m[0, 0] = m[1, 1] = m[2, 2] = 1.0 + (edPt.x - stPt.x) * adjustWidth;
                return(m);
            }

            if (type == MotionType.Pan)
            {
                Matrix4d m = Matrix4d.IdentityMatrix();
                m[0, 3] = edPt.x - stPt.x;
                m[1, 3] = edPt.y - stPt.y;
                return(m);
            }

            return(Matrix4d.IdentityMatrix());
        }
Exemple #7
0
 public static Vector3 GetTranslation(Matrix4d T)
 {
     return(new Vector3(T[0, 3], T[1, 3], T[2, 3]));
 }
Exemple #8
0
 public Matrix4d(Matrix4d m) : this(m.e)
 {
 }