Merge pull request #14 from pariterre/master

Added 3x3 inverse
pyomeca · Jun 10, 2020 · 32df293 · 32df293
2 parents 41205b9 + f806eb5
commit 32df293
Showing 1 changed file with 22 additions and 1 deletion.
diff --git a/include/rbdl/CasadiMath/MX_Xd_static.h b/include/rbdl/CasadiMath/MX_Xd_static.h
@@ -359,7 +359,28 @@ class MX_Xd_static : public casadi::MX{
         return T();
     }
     MX_Xd_static<ncols, nrows> inverse() const {
-        return inv(*this);
+        if (ncols == 3 && nrows == 3){
+            // computes the inverse of a matrix m
+            MX_Xd_scalar det = (*this)(0, 0) * ((*this)(1, 1) * (*this)(2, 2) - (*this)(2, 1) * (*this)(1, 2)) -
+                         (*this)(0, 1) * ((*this)(1, 0) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 0)) +
+                         (*this)(0, 2) * ((*this)(1, 0) * (*this)(2, 1) - (*this)(1, 1) * (*this)(2, 0));
+
+            MX_Xd_scalar invdet = 1 / det;
+
+            MX_Xd_static<3, 3> minv; // inverse of matrix m
+            minv(0, 0) = ((*this)(1, 1) * (*this)(2, 2) - (*this)(2, 1) * (*this)(1, 2)) * invdet;
+            minv(0, 1) = ((*this)(0, 2) * (*this)(2, 1) - (*this)(0, 1) * (*this)(2, 2)) * invdet;
+            minv(0, 2) = ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1)) * invdet;
+            minv(1, 0) = ((*this)(1, 2) * (*this)(2, 0) - (*this)(1, 0) * (*this)(2, 2)) * invdet;
+            minv(1, 1) = ((*this)(0, 0) * (*this)(2, 2) - (*this)(0, 2) * (*this)(2, 0)) * invdet;
+            minv(1, 2) = ((*this)(1, 0) * (*this)(0, 2) - (*this)(0, 0) * (*this)(1, 2)) * invdet;
+            minv(2, 0) = ((*this)(1, 0) * (*this)(2, 1) - (*this)(2, 0) * (*this)(1, 1)) * invdet;
+            minv(2, 1) = ((*this)(2, 0) * (*this)(0, 1) - (*this)(0, 0) * (*this)(2, 1)) * invdet;
+            minv(2, 2) = ((*this)(0, 0) * (*this)(1, 1) - (*this)(1, 0) * (*this)(0, 1)) * invdet;
+            return minv;
+        } else {
+            return inv(*this);
+        }
     }
 
     MX_Xd_scalar norm() const{