From 1b2db1ff090504ba8bb6915a6e1cb95ef604aadc Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwincoumans@google.com>
Date: Mon, 23 Jan 2017 22:22:02 -0800
Subject: [PATCH] more btCapsuleShape fixes use hello_mjcf.xml for MJCF
 importer

---
 examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp  | 2 +-
 src/BulletCollision/CollisionShapes/btCapsuleShape.cpp | 7 ++-----
 src/BulletCollision/CollisionShapes/btCapsuleShape.h   | 7 ++++++-
 src/BulletCollision/CollisionShapes/btConvexShape.cpp  | 1 -
 4 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
index 75f6b83b3..79fff963f 100644
--- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
+++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
@@ -123,7 +123,7 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
 			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
 			gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
 			gMCFJFileNameArray.push_back("mjcf/ant.xml");
-			gMCFJFileNameArray.push_back("mjcf/hello_mjcf2.xml");
+			gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml");
 			gMCFJFileNameArray.push_back("mjcf/capsule.xml");
 //			gMCFJFileNameArray.push_back("mjcf/hopper.xml");
 //			gMCFJFileNameArray.push_back("mjcf/swimmer.xml");
diff --git a/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
index 16dc346d8..0345501ce 100644
--- a/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
@@ -16,7 +16,6 @@ subject to the following restrictions:
 
 #include "btCapsuleShape.h"
 
-#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
 
 btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
@@ -49,8 +48,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 	btVector3 vtx;
 	btScalar newDot;
 	
-	btScalar radius = getRadius();
-
+	
 
 	{
 		btVector3 pos(0,0,0);
@@ -85,8 +83,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter
 {
 
 	
-	btScalar radius = getRadius();
-
+	
 	for (int j=0;j<numVectors;j++)
 	{
 		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
diff --git a/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/src/BulletCollision/CollisionShapes/btCapsuleShape.h
index 388fa85ae..aa65d5439 100644
--- a/src/BulletCollision/CollisionShapes/btCapsuleShape.h
+++ b/src/BulletCollision/CollisionShapes/btCapsuleShape.h
@@ -86,7 +86,12 @@ public:
 
 	virtual void	setLocalScaling(const btVector3& scaling)
 	{
-		btConvexInternalShape::setLocalScaling(scaling);
+		btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling;
+                btConvexInternalShape::setLocalScaling(scaling);
+		m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling);
+		//update m_collisionMargin, since entire radius==margin
+		int radiusAxis = (m_upAxis+2)%3;
+		m_collisionMargin = m_implicitShapeDimensions[radiusAxis];
 	}
 
 	virtual btVector3	getAnisotropicRollingFrictionDirection() const
diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/src/BulletCollision/CollisionShapes/btConvexShape.cpp
index 06a872c86..8d7fb054d 100644
--- a/src/BulletCollision/CollisionShapes/btConvexShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btConvexShape.cpp
@@ -230,7 +230,6 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV
 		btScalar halfHeight = capsuleShape->getHalfHeight();
 		int capsuleUpAxis = capsuleShape->getUpAxis();
 
-		btScalar radius = capsuleShape->getRadius();
 		btVector3 supVec(0,0,0);
 
 		btScalar maxDot(btScalar(-BT_LARGE_FLOAT));