--- trunk/src/math/SquareMatrix3.hpp	2005/09/27 20:02:57	633
+++ trunk/src/math/SquareMatrix3.hpp	2006/02/22 20:35:16	891
@@ -166,8 +166,28 @@ namespace oopse {
     void setupRotMat(Real w, Real x, Real y, Real z) {
       Quaternion<Real> q(w, x, y, z);
       *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;
@@ -317,6 +337,16 @@ namespace oopse {
       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.