quaternion fixes

cnr-isti-vclab · Oct 27, 2023 · 1ed603b · 1ed603b
1 parent abfc719
commit 1ed603b
Showing 1 changed file with 15 additions and 4 deletions.
diff --git a/include/vclib/space/quaternion.h b/include/vclib/space/quaternion.h
@@ -93,11 +93,14 @@ class Quaternion
     {
     }
 
-    Quaternion(const Eigen::Quaternion<Scalar>& qq) { q << qq; }
+    Quaternion(const Eigen::Quaternion<Scalar>& qq) : q(qq) {}
 
     Quaternion(const Matrix33<Scalar>& rotMatrix) : q(rotMatrix) {}
 
-    Quaternion(const Matrix44<Scalar>& rotMatrix) : q(rotMatrix) {}
+    Quaternion(const Matrix44<Scalar>& rotMatrix) :
+            q(Matrix33<Scalar>(rotMatrix.block(0, 0, 3, 3)))
+    {
+    }
 
     /**
      * @brief Constructs the quaternion that will represent the rotation between
@@ -277,8 +280,16 @@ class Quaternion
     Point3<Scalar> operator*(const Point3<Scalar>& p)
     {
         const Eigen::Matrix<Scalar, 1, 3>& v = p.eigenVector();
-        return Point3<Scalar>(
-            v + 2.0 * q.vec().cross(q.vec().cross(v) + q.w() * v));
+
+        Eigen::Matrix<Scalar, 1, 3> fc = q.vec().cross(v);
+
+        Eigen::Matrix<Scalar, 1, 3> fd = v * q.w();
+
+        Eigen::Matrix<Scalar, 1, 3> s = fc + fd;
+
+        Eigen::Matrix<Scalar, 1, 3> sc = q.vec().cross(s);
+
+        return Point3<Scalar>(v + sc * 2.0 );
     }
 
     Quaternion& operator*=(const Quaternion<Scalar>& q2)