DEBUG: add error message in Quater_Test.cpp

sofa-framework · Nov 23, 2017 · 823bd99 · 823bd99
1 parent ee6a33f
commit 823bd99
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/SofaKernel/framework/framework_test/helper/Quater_test.cpp b/SofaKernel/framework/framework_test/helper/Quater_test.cpp
@@ -51,6 +51,11 @@ TEST(QuaterTest, EulerAngles)
             q0=q2;
         }
 
+        for(std::size_t i=0; i<q0.size(); ++i)
+        {
+            ASSERT_FALSE(std::isnan(q0[i]));
+        }
+
         //Rotate p with q0
         sofa::defaulttype::Vec<3,double>  p0 = q0.Quater<double>::rotate(p);       
 
@@ -63,6 +68,11 @@ TEST(QuaterTest, EulerAngles)
         //Rotate p with q1
         sofa::defaulttype::Vec<3,double> p1 = q1.Quater<double>::rotate(p);
 
+        for(std::size_t i=0; i<q1.size(); ++i)
+        {
+            ASSERT_FALSE(std::isnan(q1[i]));
+        }
+
         //Compare the result of the two rotations on p
         EXPECT_EQ(p0,p1);
 

diff --git a/SofaKernel/framework/sofa/helper/Quater.inl b/SofaKernel/framework/sofa/helper/Quater.inl
@@ -553,11 +553,12 @@ defaulttype::Vec<3,Real> Quater<Real>::toEulerVector() const
 
     Quater<Real> q = *this;
         q.normalize();
-        Real y = std::max(Real(-1.0),std::min(Real(1.0),Real(2.)*(q[3]*q[1] - q[2]*q[0])));
+//        Real y = std::max(Real(-1.0),std::min(Real(1.0),Real(2.)*(q[3]*q[1] - q[2]*q[0])));
 
         defaulttype::Vec<3,Real> vEuler;
         vEuler[0] = atan2(2*(q[3]*q[0] + q[1]*q[2]) , (1-2*(q[0]*q[0] + q[1]*q[1])));   //roll
-        vEuler[1] = asin(y);                                     //pitch
+//        vEuler[1] = asin(y);                                                            //pitch
+        vEuler[1] =asin(2*(q[3]*q[1] - q[2]*q[0]));
         vEuler[2] = atan2(2*(q[3]*q[2] + q[0]*q[1]) , (1-2*(q[1]*q[1] + q[2]*q[2])));   //yaw
         return vEuler;
 }