Fixed fromAttr for vector3f

VirtualRobot/MJCF/elements/core/mjcf_utils.h

@@ -155,9 +155,9 @@ namespace mjcf
         }
         
         long i = 0;
-        for (auto& coeff : coeffs)
+        for (const auto& coeff : coeffs)
         {
-            value(i) = coeff;
+            value(i++) = coeff;
         }
     }
     

VirtualRobot/tests/VirtualRobotMjcfTest.cpp

@@ -15,9 +15,16 @@
 
 namespace Eigen
 {
+    std::ostream& operator<<(std::ostream& os, const Vector3f& rhs)
+    {
+        static const IOFormat iof(4, 0, " ", " ", "", "", "[", "]");
+        os << rhs.format(iof);
+        return os;
+    }
+
     bool operator==(const Quaternionf& lhs, const Quaternionf& rhs)
     {
-        return lhs.isApprox(rhs, 0);
+        return lhs.isApprox(rhs);
     }
     
     std::ostream& operator<<(std::ostream& os, const Quaternionf& rhs)
@@ -27,8 +34,10 @@ namespace Eigen
     }
 }
 
-BOOST_AUTO_TEST_SUITE(VirtualRobotMjcfTest)
+#define MSG_CONVERSION(in, string, out) \
+    BOOST_TEST_MESSAGE(in << " -> '" << string << "' -> " << out)
 
+BOOST_AUTO_TEST_SUITE(VirtualRobotMjcfTest)
 
 BOOST_AUTO_TEST_CASE(test_boost_lexical_cast)
 {
@@ -36,7 +45,7 @@ BOOST_AUTO_TEST_CASE(test_boost_lexical_cast)
     {
         const std::string string = boost::lexical_cast<std::string>(in);
         bool out = boost::lexical_cast<bool>(string);
-        BOOST_TEST_MESSAGE(in << " -> '" << string << "' -> " << out);
+        MSG_CONVERSION(in, string, out);
         BOOST_CHECK_EQUAL(in, out);
     }
     
@@ -49,16 +58,44 @@ BOOST_AUTO_TEST_SUITE_END()
 
 using namespace Eigen;
 
-BOOST_AUTO_TEST_CASE(test_attrib_conversion_vector3)
+
+BOOST_AUTO_TEST_CASE(test_parseCoeffs)
+{
+    const std::string string = "1 -3 2.4";
+    Eigen::Vector3f vector(1, -3, 2.4f);
+    
+    std::vector<float> coeffs = mjcf::parseCoeffs<float>(string, ' ');
+    
+    BOOST_CHECK_EQUAL(coeffs[0], vector.x());
+    BOOST_CHECK_EQUAL(coeffs[1], vector.y());
+    BOOST_CHECK_EQUAL(coeffs[2], vector.z());
+}
+
+
+BOOST_AUTO_TEST_CASE(test_attrib_conversion_vector3f)
 {
     Vector3f in(1, -3, 2.4f), out;
     
     const std::string string = mjcf::toAttr(in);
     mjcf::fromAttr(string, out);
+    MSG_CONVERSION(in, string, out);
     
     BOOST_CHECK_EQUAL(in, out);
 }
 
+BOOST_AUTO_TEST_CASE(test_attrib_conversion_quaternionf)
+{
+    Quaternionf in(AngleAxisf(1.4f, Vector3f(.5, -1, .3f).normalized())), out;
+    
+    const std::string string = mjcf::toAttr(in);
+    mjcf::fromAttr(string, out);
+    MSG_CONVERSION(in, string, out);
+    
+    BOOST_CHECK_EQUAL(in, out);
+}
+
+
+
 
 struct Fixture
 {