public static Matrix3d QuatToMatrix3d(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;

            Matrix3d m = new Matrix3d();
            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);
            return m;
        }
        public Quaternion(Matrix3d m)
        {
            double T = 1.0 + m.Trace();
            double S, W, X, Y, Z;

            if (T > eps)
            {
                S = Math.Sqrt(T) * 2.0;
                X = (m[5] - m[7]) / S;
                Y = (m[6] - m[2]) / S;
                Z = (m[1] - m[3]) / S;
                W = 0.25 * S;
            }
            else if (m[0] > m[4] && m[0] > m[8])
            {
                S = Math.Sqrt(1.0 + m[0] - m[4] - m[8]) * 2.0;
                X = 0.25 * S;
                Y = (m[1] + m[3]) / S;
                Z = (m[6] + m[2]) / S;
                W = (m[5] - m[7]) / S;
            }
            else if (m[4] > m[8])
            {
                S = Math.Sqrt(1.0 + m[4] - m[0] - m[8]) * 2;
                X = (m[1] + m[3]) / S;
                Y = 0.25 * S;
                Z = (m[5] + m[7]) / S;
                W = (m[6] - m[2]) / S;
            }
            else
            {
                S = Math.Sqrt(1.0 + m[8] - m[0] - m[4]) * 2;
                X = (m[6] + m[2]) / S;
                Y = (m[5] + m[7]) / S;
                Z = 0.25 * S;
                W = (m[1] - m[3]) / S;
            }

            this.s = W;
            this.v = new Vector3d(X, Y, Z);
        }
 public Matrix3d ToMatrix3d()
 {
     double xx = v.x * v.x;
     double xy = v.x * v.y;
     double xz = v.x * v.z;
     double xw = v.x * s;
     double yy = v.y * v.y;
     double yz = v.y * v.z;
     double yw = v.y * s;
     double zz = v.z * v.z;
     double zw = v.z * s;
     Matrix3d m = new Matrix3d();
     m[0] = 1 - 2 * (yy + zz);
     m[1] = 2 * (xy + zw);
     m[2] = 2 * (xz - yw);
     m[3] = 2 * (xy - zw);
     m[4] = 1 - 2 * (xx + zz);
     m[5] = 2 * (yz + xw);
     m[6] = 2 * (xz + yw);
     m[7] = 2 * (yz - xw);
     m[8] = 1 - 2 * (xx + yy);
     return m;
 }
 public Matrix3d ToMatrix3D()
 {
     Matrix3d ret = new Matrix3d();
     ret[0] = this[0, 0];
     ret[1] = this[0, 1];
     ret[2] = this[0, 2];
     ret[3] = this[1, 0];
     ret[4] = this[1, 1];
     ret[5] = this[1, 2];
     ret[6] = this[2, 0];
     ret[7] = this[2, 1];
     ret[8] = this[2, 2];
     return ret;
 }
 public Matrix4d(Matrix3d m)
 {
     for (int i=0; i<3; i++)
         for (int j=0; j<3; j++)
             this[i,j] = m[i,j];
 }
 public Matrix3d ToMatrix3d()
 {
     Matrix3d ret = new Matrix3d();
     ret[0] = e[0];
     ret[1] = e[1];
     ret[2] = e[2];
     ret[3] = e[4];
     ret[4] = e[5];
     ret[5] = e[6];
     ret[6] = e[8];
     ret[7] = e[9];
     ret[8] = e[10];
     return ret;
 }
        public Matrix3d OrthogonalFactor(double eps)
        {
            Matrix3d Q = new Matrix3d(this);
            Matrix3d Q2 = new Matrix3d();
            double err = 0;
            do
            {
                Q2 = (Q + Q.InverseTranspose()) / 2.0;
                err = (Q2 - Q).SqNorm();
                Q = Q2;
            } while (err > eps);

            return Q2;
        }
 public static Matrix3d operator *(Matrix3d m1, Matrix3d m2)
 {
     Matrix3d ret = new Matrix3d();
     for (int i = 0; i < row_size; i++)
         for (int j = 0; j < row_size; j++)
         {
             ret[i, j] = 0.0;
             for (int k = 0; k < row_size; k++)
                 ret[i, j] += m1[i, k] * m2[k, j];
         }
     return ret;
 }
 public Matrix3d InverseSVD()
 {
     SVD svd = new SVD(e, 3, 3);
     Matrix3d inv = new Matrix3d(svd.Inverse);
     lastSVDIsFullRank = svd.FullRank;
     return inv;
 }
        public Matrix3d InverseTranspose()
        {
            double a = e[0];
            double b = e[1];
            double c = e[2];
            double d = e[3];
            double E = e[4];
            double f = e[5];
            double g = e[6];
            double h = e[7];
            double i = e[8];
            double det = a * (E * i - f * h) - b * (d * i - f * g) + c * (d * h - E * g);
            if (det == 0) throw new ArithmeticException();

            Matrix3d inv = new Matrix3d();
            inv[0] = (E * i - f * h) / det;
            inv[3] = (c * h - b * i) / det;
            inv[6] = (b * f - c * E) / det;
            inv[1] = (f * g - d * i) / det;
            inv[4] = (a * i - c * g) / det;
            inv[7] = (c * d - a * f) / det;
            inv[2] = (d * h - E * g) / det;
            inv[5] = (b * g - a * h) / det;
            inv[8] = (a * E - b * d) / det;
            return inv;
        }
 public static Matrix3d IdentityMatrix()
 {
     Matrix3d m = new Matrix3d();
     m[0] = m[4] = m[8] =  1.0;
     return m;
 }
 public static Matrix3d operator /(Matrix3d m, double d)
 {
     Matrix3d ret = new Matrix3d();
     for (int i=0; i<len; i++) ret[i] = m[i] / d;
     return ret;
 }
 public static Matrix3d operator -(Matrix3d m1, Matrix3d m2)
 {
     Matrix3d ret = new Matrix3d();
     for (int i = 0; i < len; i++) ret[i] = m1[i] - m2[i];
     return ret;
 }
Example #14
0
 public Matrix3d(Matrix3d m) : this(m.e)
 {
 }
 public Matrix3d Transpose()
 {
     Matrix3d m = new Matrix3d();
     for (int i = 0; i < row_size; i++)
         for (int j = 0; j < row_size; j++)
             m[j, i] = this[i, j];
     return m;
 }
 public Matrix3d OuterCross(Vector3d v)
 {
     Matrix3d m = new Matrix3d();
     m[0, 0] = x * v.x;
     m[0, 1] = x * v.y;
     m[0, 2] = x * v.z;
     m[1, 0] = y * v.x;
     m[1, 1] = y * v.y;
     m[1, 2] = y * v.z;
     m[2, 0] = z * v.x;
     m[2, 1] = z * v.y;
     m[2, 2] = z * v.z;
     return m;
 }
 public Matrix3d(Matrix3d m)
     : this(m.e)
 {
 }