Variuos errors and minor changes.

vcg/math/matrix44.h

@@ -91,7 +91,7 @@ public:
   T &element(const int row, const int col);
   T element(const int row, const int col) const;
   T &operator[](const int i);
-  T operator[](const int i) const;
+  const T &operator[](const int i) const;
 
   Matrix44 operator+(const Matrix44 &m) const;
   Matrix44 operator-(const Matrix44 &m) const;
@@ -155,9 +155,10 @@ template <class T> Point3<T> operator*(const Matrix44<T> &m, const Point3<T> &p)
 template <class T> Matrix44<T> &Transpose(Matrix44<T> &m);
 //return NULL matrix if not invertible
 template <class T> Matrix44<T> &Invert(Matrix44<T> &m);   
+template <class T> Matrix44<T> Inverse(const Matrix44<T> &m);   
 
 typedef Matrix44<short>  Matrix44s;
-typedef Matrix44<int>	 Matrix44i;
+typedef Matrix44<int>    Matrix44i;
 typedef Matrix44<float>  Matrix44f;
 typedef Matrix44<double> Matrix44d;
 
@@ -184,7 +185,7 @@ template <class T> T &Matrix44<T>::operator[](const int i) {
   return ((T *)_a)[i];
 }
 
-template <class T> T Matrix44<T>::operator[](const int i) const {
+template <class T> const T &Matrix44<T>::operator[](const int i) const {
   assert(i >= 0 && i < 16);
   return ((T *)_a)[i];
 }
@@ -398,6 +399,19 @@ template <class T> Matrix44<T> &Invert(Matrix44<T> &m) {
   return m;
 }
 
+template <class T> Matrix44<T> Inverse(const Matrix44<T> &m) {        
+  LinearSolve<T> solve(m);
+  Matrix44<T> res;
+  for(int j = 0; j < 4; j++) { //Find inverse by columns.
+    Point4<T> col(0, 0, 0, 0);
+    col[j] = 1.0;
+    col = solve.Solve(col);
+    for(int i = 0; i < 4; i++) 
+      res.element(i, j) = col[i];
+  }  
+  return res;
+}
+
 
 
 /* LINEAR SOLVE IMPLEMENTATION */
@@ -424,6 +438,59 @@ d is +or -1 depeneing of row permutation even or odd.*/
 #define TINY 1e-100
 
 template <class T> bool LinearSolve<T>::Decompose() {  
+  
+ /* Matrix44<T> A;
+  for(int i = 0; i < 16; i++)
+    A[i] = operator[](i);  
+  SetIdentity();    
+  Point4<T> scale;
+  //* Set scale factor, scale(i) = max( |a(i,j)| ), for each row
+  for(int i = 0; i < 4; i++ ) {
+    index[i] = i;			  // Initialize row index list
+    T scalemax = (T)0.0;
+    for(int j = 0; j < 4; j++) 
+      scalemax = (scalemax > math::Abs(A.element(i,j))) ? scalemax : math::Abs(A.element(i,j));
+    scale[i] = scalemax;
+  }
+
+  //* Loop over rows k = 1, ..., (N-1)
+  int signDet = 1;
+  for(int k = 0; k < 3; k++ ) {
+	  //* Select pivot row from max( |a(j,k)/s(j)| )
+    T ratiomax = (T)0.0;
+	  int jPivot = k;
+    for(int i = k; i < 4; i++ ) {
+      T ratio = math::Abs(A.element(index[i], k))/scale[index[i]];
+      if(ratio > ratiomax) {
+        jPivot = i;
+        ratiomax = ratio;
+      }
+    }
+	  //* Perform pivoting using row index list
+	  int indexJ = index[k];
+	  if( jPivot != k ) {	          // Pivot
+      indexJ = index[jPivot];
+      index[jPivot] = index[k];   // Swap index jPivot and k
+      index[k] = indexJ;
+	    signDet *= -1;			  // Flip sign of determinant
+	  }
+	  //* Perform forward elimination
+    for(int i=k+1; i < 4; i++ ) {
+      T coeff = A.element(index[i],k)/A.element(indexJ,k);
+      for(int j=k+1; j < 4; j++ )
+        A.element(index[i],j) -= coeff*A.element(indexJ,j);
+      A.element(index[i],k) = coeff;
+      //for( j=1; j< 4; j++ ) 
+      //  element(index[i],j) -= A.element(index[i], k)*element(indexJ, j);
+    }
+  }
+  for(int i = 0; i < 16; i++)
+    operator[](i) = A[i];
+
+  d = signDet; 
+  //*this = A;
+  return true;  */
+
   d = 1; //no permutation still
     
   T scaling[4];
@@ -485,16 +552,17 @@ template <class T> bool LinearSolve<T>::Decompose() {
 
 template <class T> Point4<T> LinearSolve<T>::Solve(const Point4<T> &b) { 
   Point4<T> x(b);
-  int first = 0, ip;  
+  int first = -1, ip;  
   for(int i = 0; i < 4; i++) { 
     ip = index[i];
     T sum = x[ip];
     x[ip] = x[i];
-    if(first)
+    if(first!= -1)
       for(int j = first; j <= i-1; j++) 
         sum -= element(i,j) * x[j];
     else 
-      if (sum) first = i; 
+      if(sum) first = i; 
+    x[i] = sum;
   }
   for (int i = 3; i >= 0; i--) { 
     T sum = x[i];

vcg/math/quaternion.h

@@ -38,7 +38,9 @@ public:
   Point3<S> Rotate(const Point3<S> vec) const;  
 };
 
-template <class S> Quaternion<S> interpolate(const Quaternion<S> a, const Quaternion<S> b, double t);
+template <class S> Quaternion<S> Interpolate(const Quaternion<S> a, const Quaternion<S> b, double t);
+template <class S> Quaternion<S> &Invert(Quaternion<S> &q);
+template <class S> Quaternion<S> Inverse(const Quaternion<S> &q);
 
 
 //Implementation
@@ -192,7 +194,18 @@ template <class S> void Quaternion<S>::FromMatrix(Matrix44<S> &m) {
 		}
 	}
 }
-template <class S> Quaternion<S> interpolate(const Quaternion<S> a, const Quaternion<S> b, double t) {
+template <class S> Quaternion<S> &Invert(Quaternion<S> &m) {
+  m.Invert();
+  return m;
+}
+
+template <class S> Quaternion<S> Inverse(const Quaternion<S> &m) {
+  Quaternion<S> a = m;
+  a.Invert();
+  return a;
+}
+
+template <class S> Quaternion<S> Interpolate(const Quaternion<S> a, const Quaternion<S> b, double t) {
 		double v = a.V(0) * b.V(0) + a.V(1) * b.V(1) + a.V(2) * b.V(2) + a.V(3) * b.V(3);
 		double phi = Acos(v);
 		if(phi > 0.01) {

vcg/math/similarity.h

@@ -48,15 +48,21 @@ public:
   Similarity &SetScale(const S s);
 	Similarity &SetTranslate(const Point3<S> &t);	
   ///use radiants for angle.
-  void SetRotate(S angle, const Point3<S> & axis); 
+  Similarity &SetRotate(S angle, const Point3<S> & axis); 
+  Similarity &SetRotate(const Quaternion<S> &q);
 
   Matrix44<S> Matrix() const;
+  void FromMatrix(const Matrix44<S> &m);
 
   Quaternion<S> rot;
   Point3<S> tra;
   S sca;  
 };
 
+template <class S> Similarity<S> &Invert(Similarity<S> &m);
+template <class S> Similarity<S> Inverse(const Similarity<S> &m);
+
+
 template <class S> Similarity<S> Similarity<S>::operator*(const Similarity &a) const {
   Similarity<S> r;
   r.rot = rot * a.rot;
@@ -98,11 +104,19 @@ template <class S> Similarity<S> &Similarity<S>::SetTranslate(const Point3<S> &t
   return *this;
 }
 
-template <class S> void Similarity<S>::SetRotate(S angle, const Point3<S> &axis) {
+template <class S> Similarity<S> &Similarity<S>::SetRotate(S angle, const Point3<S> &axis) {
   SetIdentity();
   rot.FromAxis(angle, axis);
+  return *this;
 }
 
+template <class S> Similarity<S> &Similarity<S>::SetRotate(const Quaternion<S> &q) {
+  SetIdentity();
+  rot = q;  
+  return *this;
+}
+
+
 template <class S> Matrix44<S> Similarity<S>::Matrix() const {
   Matrix44<S> r;
   rot.ToMatrix(r);
@@ -112,15 +126,30 @@ template <class S> Matrix44<S> Similarity<S>::Matrix() const {
   return r;
 }
 
-template <class S> Similarity<S> &invert(Similarity<S> &a) {
+template <class S> void Similarity<S>::FromMatrix(const Matrix44<S> &m) {
-  //WARNING:: TEST THIS!!!
+  sca = pow(m.Determinant(), 1/3);  
+  assert(sca != 0);
+  Matrix44<S> t = m * Matrix44<S>().SetScale(1/sca, 1/sca, 1/sca);
+  rot.FromMatrix(t);
+  tra[0] = t.element(3, 0);
+  tra[1] = t.element(3, 1);
+  tra[2] = t.element(3, 2);
+}
+
+template <class S> Similarity<S> &Invert(Similarity<S> &a) {  
   a.rot.Invert();
   a.sca = 1/a.sca;
   a.tra = a.rot.Rotate(-a.tra)*a.sca;
   return a;
 }
 
-template <class S> Similarity<S> interpolate(const Similarity<S> &a, const Similarity<S> &b, const S t) {
+template <class S> Similarity<S> Inverse(const Similarity<S> &m) {
+  Similarity<S> a = m;
+  return Invert(a);
+}
+
+
+template <class S> Similarity<S> Interpolate(const Similarity<S> &a, const Similarity<S> &b, const S t) {
   Similarity<S> r;
   r.rot = interpolate(a.rot, b.rot, t);
   r.tra = t * a.tra + (1-t) * b.tra;