Example #1
0
        public Quaternion(Matrix3 matrix)
        {
            // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
            // article "Quaternion Calculus and Fast Animation".

            tuple = new double[4];

            int[] next = {1, 2, 0};
            var trace = matrix.entry[0]
                         + matrix.entry[4]
                         + matrix.entry[8];
            double root;

            if (trace > 0)
            {
                // |w| > 1/2, may as well choose w > 1/2
                root = System.Math.Sqrt(trace + 1);  // 2w
                tuple[0] = 0.5 * root;
                root = 0.5 / root;  // 1/(4w)
                tuple[1] = (matrix.entry[5] - matrix.entry[7]) * root;
                tuple[2] = (matrix.entry[6] - matrix.entry[2]) * root;
                tuple[3] = (matrix.entry[1] - matrix.entry[3]) * root;
            }
            else
            {
                // |w| <= 1/2
                var i = 0;
                if (matrix.entry[4] > matrix.entry[0])
                    i = 1;
                if (matrix.entry[8] > matrix.entry[i + 3 * i])
                    i = 2;
                var j = next[i];
                var k = next[j];

                root = System.Math.Sqrt(matrix.entry[i + 3 * i]
                               - matrix.entry[j + 3 * j]
                               - matrix.entry[k + 3 * k] + 1);
                tuple[i + 1] = 0.5 * root;
                root = 0.5 / root;
                tuple[0] = (matrix.entry[j + 3 * k] - matrix.entry[k + 3 * j]) * root;
                tuple[j + 1] = (matrix.entry[i + 3 * j] + matrix.entry[j + 3 * i]) * root;
                tuple[k + 1] = (matrix.entry[i + 3 * k] + matrix.entry[k + 3 * i]) * root;
            }
        }
Example #2
0
 public Vector3 Mult(Matrix3 matrix)
 {
     return new Vector3(
             tuple[0] * matrix.entry[0] + tuple[1] * matrix.entry[1] + tuple[2] * matrix.entry[2],
             tuple[0] * matrix.entry[3] + tuple[1] * matrix.entry[4] + tuple[2] * matrix.entry[5],
             tuple[0] * matrix.entry[6] + tuple[1] * matrix.entry[7] + tuple[2] * matrix.entry[8]);
 }
Example #3
0
 public Matrix3 TransposeTimes(Matrix3 matrix)
 {
     // P = A^T*B, P[r][c] = sum_m A[m][r]*B[m][c]
     Matrix3 result = new Matrix3();
     for (int row = 0; row < 3; row++)
     {
         for (int col = 0; col < 3; col++)
         {
             int i = I(row, col);
             result.entry[i] = 0;
             for (int mid = 0; mid < 3; mid++)
                 result.entry[i] += entry[I(mid, row)] * matrix.entry[I(mid, col)];
         }
     }
     return result;
 }
Example #4
0
 public Matrix3 Transpose()
 {
     Matrix3 result = new Matrix3();
     for (int i = 0; i < 3; i++)
         for (int j = 0; j < 3; j++)
             result.entry[I(i, j)] = entry[I(j, i)];
     return result;
 }
Example #5
0
 public Matrix3 TimesTranspose(Matrix3 matrix)
 {
     // P = A*B^T, P[r][c] = sum_m A[r][m]*B[c][m]
     Matrix3 result = new Matrix3();
     for (int row = 0; row < 3; row++)
     {
         for (int col = 0; col < 3; col++)
         {
             int i = I(row, col);
             result.entry[i] = 0;
             for (int mid = 0; mid < 3; mid++)
                 result.entry[i] += entry[I(row, mid)] * matrix.entry[I(col, mid)];
         }
     }
     return result;
 }
Example #6
0
 public Matrix3 Sub(Matrix3 matrix)
 {
     Matrix3 result = new Matrix3();
     for (int i = 0; i < 9; i++)
         result.entry[i] = entry[i] - matrix.entry[i];
     return result;
 }
Example #7
0
 public Matrix3 Negative()
 {
     Matrix3 result = new Matrix3();
     for (int i = 0; i < 9; i++)
         result.entry[i] = -entry[i];
     return result;
 }
Example #8
0
 public Matrix3(Matrix3 matrix)
 {
     entry = new double[9];
     for (int i = 0; i < 9; i++)
         entry[i] = matrix.entry[i];
 }
Example #9
0
 public Matrix3 Div(double scalar)
 {
     Matrix3 result = new Matrix3();
     if (scalar != 0)
     {
         double invScalar = 1 / scalar;
         for (int i = 0; i < 9; i++)
             result.entry[i] = entry[i] * invScalar;;
     }
     else
         for (int i = 0; i < 9; i++)
             result.entry[i] = double.MaxValue;
     return result;
 }
Example #10
0
        public Matrix3 Adjoint()
        {
            Matrix3 result = new Matrix3();

            result.entry[0] = entry[4] * entry[8] - entry[5] * entry[7];
            result.entry[3] = entry[2] * entry[7] - entry[1] * entry[8];
            result.entry[6] = entry[1] * entry[5] - entry[2] * entry[4];
            result.entry[1] = entry[5] * entry[6] - entry[3] * entry[8];
            result.entry[4] = entry[0] * entry[8] - entry[2] * entry[6];
            result.entry[7] = entry[2] * entry[3] - entry[0] * entry[5];
            result.entry[2] = entry[3] * entry[7] - entry[4] * entry[6];
            result.entry[5] = entry[1] * entry[6] - entry[0] * entry[7];
            result.entry[8] = entry[0] * entry[4] - entry[1] * entry[3];

            return result;
        }
Example #11
0
 public Matrix3 Add(double scalar)
 {
     Matrix3 result = new Matrix3();
     for (int i = 0; i < 9; i++)
         result.entry[i] = entry[i] * scalar;
     return result;
 }
Example #12
0
        public static Matrix3 RotationMatrix(double alpha, double beta, double gamma)
        {
            Matrix3 result = new Matrix3();

            result.entry[0] = + System.Math.Cos(alpha)								* System.Math.Cos(gamma)
                              - System.Math.Sin(alpha) *	System.Math.Cos(beta)	* System.Math.Sin(gamma);
            result.entry[1] = - System.Math.Sin(alpha)								* System.Math.Cos(gamma)
                              - System.Math.Cos(alpha) *	System.Math.Cos(beta)	* System.Math.Sin(gamma);
            result.entry[2] = +								System.Math.Sin(beta)	* System.Math.Sin(gamma);

            result.entry[3] = + System.Math.Cos(alpha)								* System.Math.Sin(gamma)
                              + System.Math.Sin(alpha) *	System.Math.Cos(beta)	* System.Math.Cos(gamma);
            result.entry[4] = - System.Math.Sin(alpha)								* System.Math.Sin(gamma)
                              - System.Math.Cos(alpha) *	System.Math.Cos(beta)	* System.Math.Cos(gamma);
            result.entry[5] = -								System.Math.Sin(beta)	* System.Math.Cos(gamma);

            result.entry[6] = + System.Math.Sin(alpha) *	System.Math.Sin(beta);
            result.entry[7] = + System.Math.Cos(alpha) *	System.Math.Sin(beta);
            result.entry[8] = +								System.Math.Cos(beta);

            return result;
        }
Example #13
0
        public Matrix3 ToRotationMatrix()
        {
            var result = new Matrix3();

            var x  = 2 * tuple[1];
            var y  = 2 * tuple[2];
            var z  = 2 * tuple[3];
            var wx = x * tuple[0];
            var wy = y * tuple[0];
            var wz = z * tuple[0];
            var xx = x * tuple[1];
            var xy = y * tuple[1];
            var xz = z * tuple[1];
            var yy = y * tuple[2];
            var yz = z * tuple[2];
            var zz = z * tuple[3];

            result.entry[0] = 1 - (yy + zz);
            result.entry[3] = xy - wz;
            result.entry[6] = xz + wy;
            result.entry[1] = xy + wz;
            result.entry[4] = 1 - (xx + zz);
            result.entry[7] = yz - wx;
            result.entry[2] = xz - wy;
            result.entry[5] = yz + wx;
            result.entry[8] = 1 - (xx + yy);

            return result;
        }