Adding support for units at inertia matrix definition.

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@172 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/XML/BaseIO.cpp

@@ -261,6 +261,29 @@ bool BaseIO::hasUnitsAttribute(rapidxml::xml_node<char> *node)
 	return (attr!=NULL);
 }
 
+std::vector< Units > BaseIO::getUnitsAttributes(rapidxml::xml_node<char> *node)
+{
+	std::vector< Units > result;
+	rapidxml::xml_attribute<> *attr = node->first_attribute("unit", 0, false);
+	while (attr)
+	{
+		Units unitsAttribute(getLowerCase(attr->value()));
+		result.push_back(unitsAttribute);
+
+		attr = attr->next_attribute("unit", 0, false);
+	}
+
+	attr = node->first_attribute("units", 0, false);
+	while (attr)
+	{
+		Units unitsAttribute(getLowerCase(attr->value()));
+		result.push_back(unitsAttribute);
+
+		attr = attr->next_attribute("units", 0, false);
+	}
+	return result;
+}
+
 /**
  * This method processes the unit or units attribute of xml_node \p node.
  *
@@ -712,6 +735,26 @@ void BaseIO::processPhysicsTag(rapidxml::xml_node<char> *physicsXMLNode, const s
 	if (inMatXMLNode)
 	{
 		physics.intertiaMatrix = process3x3Matrix(inMatXMLNode);
+		std::vector< Units > unitsAttr = getUnitsAttributes(inMatXMLNode);
+		Units uWeight("kg");
+		Units uLength("m");
+		for (size_t i=0;i<unitsAttr.size();i++)
+		{
+			if (unitsAttr[i].isWeight())
+				uWeight = unitsAttr[i];
+			if (unitsAttr[i].isLength())
+				uLength = unitsAttr[i];
+		}
+		float factor = 1.0f;
+		if (uWeight.isGram())
+			factor *= 0.001f;
+		if (uWeight.isTon())
+			factor *= 1000.0f;
+		if (uLength.isMillimeter())
+			factor *= 0.000001f;
+
+		physics.intertiaMatrix *= factor;
+
 	}
 	rapidxml::xml_node<> *velXMLNode = physicsXMLNode->first_node("maxVelocity",0,false);
 	if (velXMLNode)

VirtualRobot/XML/BaseIO.h

@@ -84,6 +84,7 @@ protected:
 	static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char>* setXMLNode, RobotPtr robo, const std::string& robotRootNode, int& robotNodeSetCounter );
 	static Eigen::Matrix3f process3x3Matrix(rapidxml::xml_node<char> *matrixXMLNode);
 	static bool hasUnitsAttribute(rapidxml::xml_node<char> *node);
+	static std::vector< Units > getUnitsAttributes(rapidxml::xml_node<char> *node);
 	static boost::mutex mutex; 
 };
 

VirtualRobot/tests/VirtualRobotRobotTest.cpp

@@ -226,7 +226,7 @@ BOOST_AUTO_TEST_CASE(testVirtualRobotPhysicsTag)
 		"   <MaxVelocity value='0.3'/>"
 		"   <MaxAcceleration value='0.1'/>"
 		"   <MaxTorque value='0.2'/>"
-		"   <IntertiaMatrix>"
+		"   <IntertiaMatrix units='ton' units='mm'>"
 		"     <row1 c1='1' c2='2' c3='3'/>"
 		"     <row2 c1='4' c2='5' c3='6'/>"
 		"     <row3 c1='7' c2='8' c3='9'/>"
@@ -253,7 +253,7 @@ BOOST_AUTO_TEST_CASE(testVirtualRobotPhysicsTag)
 
 	Eigen::Matrix3f inertia = rn->getInertiaMatrix();
 	Eigen::Matrix3f expectedMat;
-	expectedMat << 1,2,3,4,5,6,7,8,9;
+	expectedMat << 0.001f,0.002f,0.003f,0.004f,0.005f,0.006f,0.007f,0.008f,0.009f;
 	bool inertiaMatrixOK = inertia.isApprox(expectedMat);
 	BOOST_REQUIRE(inertiaMatrixOK);
 }