From 1524004aa86c53edd94062e5d1f273a5201cd241 Mon Sep 17 00:00:00 2001
From: Oleg Klimov <omgtech@gmail.com>
Date: Thu, 6 Apr 2017 04:25:56 +0300
Subject: [PATCH] MJCF: support for "axisangle", small code improvements

---
 .../ImportMJCFDemo/BulletMJCFImporter.cpp     | 51 ++++++++++---------
 .../Importers/ImportURDFDemo/URDF2Bullet.cpp  |  3 ++
 .../TinyRendererVisualShapeConverter.cpp      |  2 +-
 3 files changed, 30 insertions(+), 26 deletions(-)

diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index 3c5329809..d8db32b0b 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -630,11 +630,7 @@ struct BulletMJCFImporterInternalData
 		bool handledGeomType = false;
 		UrdfGeometry geom;
 
-		
-
-		const char* gType = link_xml->Attribute("type");
 		const char* sz = link_xml->Attribute("size");
-		const char* posS = link_xml->Attribute("pos");
 		int conDim = defaults.m_defaultConDim;
 
 		const char* conDimS = link_xml->Attribute("condim");
@@ -707,6 +703,7 @@ struct BulletMJCFImporterInternalData
 			geom.m_localMaterial.m_name = rgba;
 		}
 
+		const char* posS = link_xml->Attribute("pos");
 		if (posS)
 		{
 			btVector3 pos(0,0,0);
@@ -716,18 +713,32 @@ struct BulletMJCFImporterInternalData
 				linkLocalFrame.setOrigin(pos);
 			}
 		}
+
 		const char* ornS = link_xml->Attribute("quat");
 		if (ornS)
 		{
-			std::string ornStr = ornS;
 			btQuaternion orn(0,0,0,1);
 			btVector4 o4;
-			if (parseVector4(o4,ornStr))
+			if (parseVector4(o4, ornS))
 			{
 				orn.setValue(o4[1],o4[2],o4[3],o4[0]);
 				linkLocalFrame.setRotation(orn);
 			}
 		}
+
+		const char* axis_and_angle = link_xml->Attribute("axisangle");
+		if (axis_and_angle)
+		{
+			btQuaternion orn(0,0,0,1);
+			btVector4 o4;
+			if (parseVector4(o4, axis_and_angle))
+			{
+				orn.setRotation(btVector3(o4[0],o4[1],o4[2]), o4[3]);
+				linkLocalFrame.setRotation(orn);
+			}
+		}
+
+		const char* gType = link_xml->Attribute("type");
 		if (gType)
 		{
 			std::string geomType = gType;
@@ -792,8 +803,8 @@ struct BulletMJCFImporterInternalData
 					}
 				}
 
-				geom.m_capsuleRadius = 0;
-				geom.m_capsuleHeight = 0.f;
+				geom.m_capsuleRadius = 2.00f; // 2 to make it visible if something is wrong
+				geom.m_capsuleHeight = 2.00f;
 
 				if (sizes.size()>0)
 				{
@@ -877,6 +888,7 @@ struct BulletMJCFImporterInternalData
 
 				col.m_geometry = geom;
 				col.m_linkLocalFrame = linkLocalFrame;
+				col.m_sourceFileLocation = sourceFileLocation(link_xml);
 				linkPtr->m_collisionArray.push_back(col);
 
 			} else
@@ -950,22 +962,6 @@ struct BulletMJCFImporterInternalData
 					col->m_geometry.m_boxSize[2];
 				break;
 			}
-
-			case URDF_GEOM_CYLINDER:
-			{
-				double r = col->m_geometry.m_capsuleRadius;
-				btScalar h(0);
-				//and one cylinder of 'height'
-				if (col->m_geometry.m_hasFromTo)
-				{
-					h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length();
-				} else
-				{
-					h = col->m_geometry.m_capsuleHeight;
-				}
-				totalVolume += SIMD_PI*r*r*h;
-				break;
-			}
 			case URDF_GEOM_MESH:
 			{
 				//todo (based on mesh bounding box?)
@@ -976,11 +972,15 @@ struct BulletMJCFImporterInternalData
 				//todo
 				break;
 			}
+			case URDF_GEOM_CYLINDER:
 			case URDF_GEOM_CAPSULE:
 			{
 				//one sphere 
 				double r = col->m_geometry.m_capsuleRadius;
-				totalVolume += 4./3.*SIMD_PI*r*r*r;
+				if (col->m_geometry.m_type==URDF_GEOM_CAPSULE)
+				{
+					totalVolume += 4./3.*SIMD_PI*r*r*r;
+				}
 				btScalar h(0);
 				if (col->m_geometry.m_hasFromTo)
 				{
@@ -1205,6 +1205,7 @@ struct BulletMJCFImporterInternalData
 		}
 
 		linkPtr->m_linkTransformInWorld = linkTransform;
+
 		if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
 		{
 			//linkPtr->m_linkTransformInWorld.setIdentity();
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 94dcfcb04..2df69c430 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -285,6 +285,9 @@ void ConvertURDF2BulletInternal(
             if (!(flags & CUF_USE_URDF_INERTIA))
             {
                 compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
+                assert(localInertiaDiagonal[0] < 1e10);
+                assert(localInertiaDiagonal[1] < 1e10);
+                assert(localInertiaDiagonal[2] < 1e10);
             }
             URDFLinkContactInfo contactInfo;
             u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
index f0eaa8798..5e9579c08 100644
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -526,7 +526,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 		}
 		else
 		{
-			// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
+			// We have to see something, take collision shape. Useful for MuJoCo xml, where there are no explicit visual shapes.
 			useVisual = false;
 			cnt = linkPtr->m_collisionArray.size();
 		}