Add hopf neighborhood test

VirtualRobot/tests/VirtualRobotMathToolsTest.cpp

@@ -49,6 +49,71 @@ BOOST_AUTO_TEST_CASE(testMathToolsHopf)
     }
 }
 
+
+BOOST_AUTO_TEST_CASE(testMathToolsHopfNeighborhood)
+{
+    Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+    const int NR_TESTS = 50000;
+    const float maxAngHopf = 0.01f;
+    const float maxAngQuat = 2.5f*maxAngHopf;
+    for (int i = 0; i<NR_TESTS; i++)
+    {
+        Eigen::Vector3f ax = Eigen::Vector3f::Random();
+        ax.normalize();
+        float ang = float(rand() % 1000 / 1000.0f * 2.0f*M_PI - M_PI);
+        Eigen::Matrix3f m3 = Eigen::AngleAxisf(ang, ax).matrix();
+        m.block(0, 0, 3, 3) = m3;
+        VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(m);
+        Eigen::Vector3f h0 = VirtualRobot::MathTools::quat2hopf(q);
+
+        ax = Eigen::Vector3f::Random();
+        ax.normalize();
+        ax *= maxAngHopf;
+
+        Eigen::Vector3f h = h0 + ax;
+
+        if (h(0) < 0)
+            continue;
+        //h(0) = float(2.0f*M_PI) - h(0);
+        if (h(0) > float(2.0f*M_PI))
+            continue;
+        //h(0) = h(0) - float(2.0f*M_PI);
+        if (h(1) < 0)
+            continue;
+        //h(1) = float(2.0f*M_PI) - h(1);
+        if (h(1) > float(2.0f*M_PI))
+            continue;
+        //h(1) = h(1) - float(2.0f*M_PI);
+        if (h(2) < 0)
+            continue;
+            //h(2) = float(M_PI*0.5f) - h(2);
+        if (h(2) > float(M_PI*0.5f))
+            continue;
+            //h(2) = h(2) - float(M_PI*0.5f);
+
+
+        BOOST_CHECK_GE(h(0), 0.0f);
+        BOOST_CHECK_GE(h(1), 0.0f);
+        BOOST_CHECK_GE(h(2), 0.0f);
+        BOOST_CHECK_LE(h(0), 2.0f*(float)M_PI);
+        BOOST_CHECK_LE(h(1), 2.0f*(float)M_PI);
+        BOOST_CHECK_LE(h(2), (float)M_PI*0.5f);
+
+        VirtualRobot::MathTools::Quaternion q2 = VirtualRobot::MathTools::hopf2quat(h);
+
+        VirtualRobot::MathTools::Quaternion qDelta = VirtualRobot::MathTools::getDelta(q, q2);
+        float a = (float)fabs(VirtualRobot::MathTools::getAngle(qDelta));
+        BOOST_CHECK_SMALL(a, maxAngQuat);
+        
+        if (a>maxAngQuat)
+        {
+            std::cout << "H0:" << h0.transpose() << ", " << "h-new:" << h.transpose() << std::endl;
+            std::cout << "Q:" << q.w << ", " << q.x << "," << q.y << "," << q.z << " / Q2:" << q2.w << ", " << q2.x << "," << q2.y << "," << q2.z << std::endl;
+        }
+    }
+}
+
+
 BOOST_AUTO_TEST_CASE(testMathToolsRPY)
 {
     float r = (float)M_PI * 0.25f;