text: /*
   Copyright (C) 2011 by Ivan Safrin
   
   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:
   
   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.
   
   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
  */
  
  #include "PolyMatrix4.h"
  
  using namespace Polycode;
  
  Matrix4::Matrix4() {
  	identity();
  }
  
  Matrix4::~Matrix4() {
  
  }
  
  Matrix4::Matrix4(const Number *m) {
  	memcpy(ml, m, sizeof(Number)*16);
  }
  
  Matrix4 Matrix4::inverse() const
  {
  	Number m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
  	Number m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
  	Number m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
  	Number m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
  
  	Number v0 = m20 * m31 - m21 * m30;
  	Number v1 = m20 * m32 - m22 * m30;
  	Number v2 = m20 * m33 - m23 * m30;
  	Number v3 = m21 * m32 - m22 * m31;
  	Number v4 = m21 * m33 - m23 * m31;
  	Number v5 = m22 * m33 - m23 * m32;
  
  	Number t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
  	Number t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
  	Number t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
  	Number t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
  
  	Number invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
  
  	Number d00 = t00 * invDet;
  	Number d10 = t10 * invDet;
  	Number d20 = t20 * invDet;
  	Number d30 = t30 * invDet;
  
  	Number d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
  	Number d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
  	Number d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
  	Number d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
  
  	v0 = m10 * m31 - m11 * m30;
  	v1 = m10 * m32 - m12 * m30;
  	v2 = m10 * m33 - m13 * m30;
  	v3 = m11 * m32 - m12 * m31;
  	v4 = m11 * m33 - m13 * m31;
  	v5 = m12 * m33 - m13 * m32;
  
  	Number d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
  	Number d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
  	Number d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
  	Number d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
  
  	v0 = m21 * m10 - m20 * m11;
  	v1 = m22 * m10 - m20 * m12;
  	v2 = m23 * m10 - m20 * m13;
  	v3 = m22 * m11 - m21 * m12;
  	v4 = m23 * m11 - m21 * m13;
  	v5 = m23 * m12 - m22 * m13;
  
  	Number d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
  	Number d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
  	Number d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
  	Number d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
  
  	return Matrix4(
  		d00, d01, d02, d03,
  		d10, d11, d12, d13,
  		d20, d21, d22, d23,
  		d30, d31, d32, d33);
  }
  
  Matrix4 Matrix4::inverseAffine(void) const
  {
  
  	Number m10 = m[1][0], m11 = m[1][1], m12 = m[1][2];
  	Number m20 = m[2][0], m21 = m[2][1], m22 = m[2][2];
  
  	Number t00 = m22 * m11 - m21 * m12;
  	Number t10 = m20 * m12 - m22 * m10;
  	Number t20 = m21 * m10 - m20 * m11;
  
  	Number m00 = m[0][0], m01 = m[0][1], m02 = m[0][2];
  
  	Number invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20);
  
  	t00 *= invDet; t10 *= invDet; t20 *= invDet;
  
  	m00 *= invDet; m01 *= invDet; m02 *= invDet;
  
  	Number r00 = t00;
  	Number r01 = m02 * m21 - m01 * m22;
  	Number r02 = m01 * m12 - m02 * m11;
  
  	Number r10 = t10;
  	Number r11 = m00 * m22 - m02 * m20;
  	Number r12 = m02 * m10 - m00 * m12;
  
  	Number r20 = t20;
  	Number r21 = m01 * m20 - m00 * m21;
  	Number r22 = m00 * m11 - m01 * m10;
  
  	Number m03 = m[0][3], m13 = m[1][3], m23 = m[2][3];
  
  	Number r03 = - (r00 * m03 + r01 * m13 + r02 * m23);
  	Number r13 = - (r10 * m03 + r11 * m13 + r12 * m23);
  	Number r23 = - (r20 * m03 + r21 * m13 + r22 * m23);
  
  	return Matrix4(
  		r00, r01, r02, r03,
  		r10, r11, r12, r13,
  		r20, r21, r22, r23,
  		  0,   0,   0,   1);
  }
  
  Number Matrix4::determinant() const {
  	const Number *cols[4] = {m[0],m[1],m[2],m[3]};
  	return generalDeterminant(cols, 4);
  }
  
  // Determinant function by Edward Popko
  // Source: http://paulbourke.net/miscellaneous/determinant/
  //==============================================================================
  // Recursive definition of determinate using expansion by minors.
  //
  // Notes: 1) arguments:
  //             a (double **) pointer to a pointer of an arbitrary square matrix
  //             n (int) dimension of the square matrix
  //
  //        2) Determinant is a recursive function, calling itself repeatedly
  //           each time with a sub-matrix of the original till a terminal
  //           2X2 matrix is achieved and a simple determinat can be computed.
  //           As the recursion works backwards, cumulative determinants are
  //           found till untimately, the final determinate is returned to the
  //           initial function caller.
  //
  //        3) m is a matrix (4X4 in example)  and m13 is a minor of it.
  //           A minor of m is a 3X3 in which a row and column of values
  //           had been excluded.   Another minor of the submartix is also
  //           possible etc.
  //             m  a b c d   m13 . . . .
  //                e f g h       e f . h     row 1 column 3 is elminated
  //                i j k l       i j . l     creating a 3 X 3 sub martix
  //                m n o p       m n . p
  //
  //        4) the following function finds the determinant of a matrix
  //           by recursively minor-ing a row and column, each time reducing
  //           the sub-matrix by one row/column.  When a 2X2 matrix is
  //           obtained, the determinat is a simple calculation and the
  //           process of unstacking previous recursive calls begins.
  //
  //                m n
  //                o p  determinant = m*p - n*o
  //
  //        5) this function uses dynamic memory allocation on each call to
  //           build a m X m matrix  this requires **  and * pointer variables
  //           First memory allocation is ** and gets space for a list of other
  //           pointers filled in by the second call to malloc.
  //
  //        6) C++ implements two dimensional arrays as an array of arrays
  //           thus two dynamic malloc's are needed and have corresponsing
  //           free() calles.
  //
  //        7) the final determinant value is the sum of sub determinants
  //
  //==============================================================================
  Number Matrix4::generalDeterminant(Number const* const*a,int n)
  {
      int i,j,j1,j2;                    // general loop and matrix subscripts
      Number det = 0;                   // init determinant
      Number **m = NULL;                // pointer to pointers to implement 2d
  	// square array
  	
      if (n < 1)    {   }                // error condition, should never get here
  	
      else if (n == 1) {                 // should not get here
          det = a[0][0];
  	}
  	
      else if (n == 2)  {                // basic 2X2 sub-matrix determinate
  		// definition. When n==2, this ends the
          det = a[0][0] * a[1][1] - a[1][0] * a[0][1];// the recursion series
  	}
  	
  	
  	// recursion continues, solve next sub-matrix
      else {                             // solve the next minor by building a
  		// sub matrix
          det = 0;                      // initialize determinant of sub-matrix
  		
  		// for each column in sub-matrix
          for (j1 = 0; j1 < n; j1++) {
  			// get space for the pointer list
              m = new Number*[n-1];
  			
              for (i = 0; i < n-1; i++)
                  m[i] = new Number[n-1];
  			//     i[0][1][2][3]  first malloc
  			//  m -> +  +  +  +   space for 4 pointers
  			//       |  |  |  |          j  second malloc
  			//       |  |  |  +-> _ _ _ [0] pointers to
  			//       |  |  +----> _ _ _ [1] and memory for
  			//       |  +-------> _ a _ [2] 4 doubles
  			//       +----------> _ _ _ [3]
  			//
  			//                   a[1][2]
  			// build sub-matrix with minor elements excluded
              for (i = 1; i < n; i++) {
                  j2 = 0;               // start at first sum-matrix column position
  				// loop to copy source matrix less one column
                  for (j = 0; j < n; j++) {
                      if (j == j1) continue; // don't copy the minor column element
  					
                      m[i-1][j2] = a[i][j];   // copy source element into new sub-matrix
  					// i-1 because new sub-matrix is one row
  					// (and column) smaller with excluded minors
                      j2++;                  // move to next sub-matrix column position
  				}
  			}
  			
              det += pow(-1.0,1.0 + j1 + 1.0) * a[0][j1] * generalDeterminant(m,n-1);
  			// sum x raised to y power
  			// recursively get determinant of next
  			// sub-matrix which is now one
  			// row & column smaller
  			
              for (i = 0 ; i < n-1 ; i++) delete[] m[i];// free the storage allocated to
  			// to this minor's set of pointers
              delete[] m;                       // free the storage for the original
  			// pointer to pointer
          }
      }
      return(det) ;
  }
  