--- trunk/src/math/SquareMatrix3.hpp	2005/09/27 20:02:57	633
+++ trunk/src/math/SquareMatrix3.hpp	2006/05/17 21:51:42	963
@@ -47,7 +47,7 @@
  */
 #ifndef MATH_SQUAREMATRIX3_HPP
 #define  MATH_SQUAREMATRIX3_HPP
-
+#include <vector>
 #include "Quaternion.hpp"
 #include "SquareMatrix.hpp"
 #include "Vector3.hpp"
@@ -168,6 +168,26 @@ namespace oopse {
       *this = q.toRotationMatrix3();
     }
 
+    void setupSkewMat(Vector3<Real> v) {
+        setupSkewMat(v[0], v[1], v[2]);
+    }
+
+    void setupSkewMat(Real v1, Real v2, Real v3) {
+        this->data_[0][0] = 0;
+        this->data_[0][1] = -v3;
+        this->data_[0][2] = v2;
+        this->data_[1][0] = v3;
+        this->data_[1][1] = 0;
+        this->data_[1][2] = -v1;
+        this->data_[2][0] = -v2;
+        this->data_[2][1] = v1;
+        this->data_[2][2] = 0;
+        
+        
+    }
+
+
+
     /**
      * Returns the quaternion from this rotation matrix
      * @return the quaternion from this rotation matrix
@@ -179,7 +199,7 @@ namespace oopse {
       Real ad1, ad2, ad3;    
       t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0;
 
-      if( t > 0.0 ){
+      if( t > NumericConstant::epsilon ){
 
 	s = 0.5 / sqrt( t );
 	q[0] = 0.25 / s;
@@ -239,7 +259,7 @@ namespace oopse {
                 
       // set the tolerance for Euler angles and rotation elements
 
-      theta = acos(std::min(1.0, std::max(-1.0,this->data_[2][2])));
+      theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2])));
       ctheta = this->data_[2][2]; 
       stheta = sqrt(1.0 - ctheta * ctheta);
 
@@ -298,25 +318,67 @@ namespace oopse {
      */
     SquareMatrix3<Real>  inverse() const {
       SquareMatrix3<Real> m;
-      double det = determinant();
+      RealType det = determinant();
       if (fabs(det) <= oopse::epsilon) {
 	//"The method was called on a matrix with |determinant| <= 1e-6.",
 	//"This is a runtime or a programming error in your application.");
-      }
+        std::vector<int> zeroDiagElementIndex;
+        for (int i =0; i < 3; ++i) {
+            if (fabs(this->data_[i][i]) <= oopse::epsilon) {
+                zeroDiagElementIndex.push_back(i);
+            }
+        }
 
-      m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1];
-      m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2];
-      m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0];
-      m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1];
-      m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2];
-      m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0];
-      m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1];
-      m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2];
-      m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0];
-
-      m /= det;
+        if (zeroDiagElementIndex.size() == 2) {
+            int index = zeroDiagElementIndex[0];
+            m(index, index) = 1.0 / this->data_[index][index];
+        }else if (zeroDiagElementIndex.size() == 1) {
+
+            int a = (zeroDiagElementIndex[0] + 1) % 3;
+            int b = (zeroDiagElementIndex[0] + 2) %3;
+            RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b];
+            m(a, a) = this->data_[b][b] /denom;
+            m(b, a) = -this->data_[b][a]/denom;
+
+            m(a,b) = -this->data_[a][b]/denom;
+            m(b, b) = this->data_[a][a]/denom;
+                
+        }
+      
+/*
+        for(std::vector<int>::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) {
+            if (this->data_[*iter][0] > oopse::epsilon || this->data_[*iter][1] ||this->data_[*iter][2] ||
+                this->data_[0][*iter] > oopse::epsilon || this->data_[1][*iter] ||this->data_[2][*iter] ) {
+                std::cout << "can not inverse matrix" << std::endl;
+            }
+        }
+*/
+      } else {
+
+          m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1];
+          m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2];
+          m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0];
+          m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1];
+          m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2];
+          m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0];
+          m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1];
+          m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2];
+          m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0];
+
+          m /= det;
+	}
       return m;
     }
+
+    SquareMatrix3<Real> transpose() const{
+      SquareMatrix3<Real> result;
+                
+      for (unsigned int i = 0; i < 3; i++)
+	for (unsigned int j = 0; j < 3; j++)              
+	  result(j, i) = this->data_[i][j];
+
+      return result;
+    }
     /**
      * Extract the eigenvalues and eigenvectors from a 3x3 matrix.
      * The eigenvectors (the columns of V) will be normalized. 
@@ -499,8 +561,8 @@ namespace oopse {
   }
 
     
-  typedef SquareMatrix3<double> Mat3x3d;
-  typedef SquareMatrix3<double> RotMat3x3d;
+  typedef SquareMatrix3<RealType> Mat3x3d;
+  typedef SquareMatrix3<RealType> RotMat3x3d;
 
 } //namespace oopse
 #endif // MATH_SQUAREMATRIX_HPP