add capsule support in urdf parser (non-standard extension), capsule.urdf

fix btCapsuleShape (X,Y,Z) margin issue
fix uninitialized variables in TinyRenderer: m_hasLightDistance, m_hasLightAmbientCoeff, m_hasLightDiffuseCoeff, m_hasLightSpecularCoeff
pybullet/shared memory API 'getClosestPoints' / b3InitClosestDistanceQuery, only report contacts equal/smaller distance than given

data/capsule.urdf

@@ -0,0 +1,23 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="base_link">
+    <contact>
+      <rolling_friction value="0.001"/>
+      <spinning_friction value="0.001"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<capsule length="0.1" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+
+
+

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -455,6 +455,16 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
     switch (collision->m_geometry.m_type)
     {
+		case URDF_GEOM_CAPSULE:
+        {
+			btScalar radius = collision->m_geometry.m_capsuleRadius;
+			btScalar height = btScalar(2.f)*collision->m_geometry.m_capsuleHalfHeight;
+			btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height);
+            shape = capsuleShape;
+			shape ->setMargin(gUrdfDefaultCollisionMargin);
+			break;
+		}
+
         case URDF_GEOM_CYLINDER:
         {
 			btScalar cylRadius = collision->m_geometry.m_cylinderRadius;

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -380,7 +380,20 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
 		geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
 		
 	}
-	
+	else if (type_name == "capsule")
+	{
+		geom.m_type = URDF_GEOM_CAPSULE;
+		if (!shape->Attribute("length") ||
+			!shape->Attribute("radius"))
+	  {
+		  logger->reportError("Capsule shape must have both length and radius attributes");
+		  return false;
+	  }
+		geom.m_hasFromTo = false;
+		geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
+		geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
+		
+	}
   else if (type_name == "mesh")
   {
 	  geom.m_type = URDF_GEOM_MESH;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3358,25 +3358,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 										virtual	btScalar	addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1)
 										{
-											b3ContactPointData pt;
+											if (cp.m_distance1<=m_closestDistanceThreshold)
-											pt.m_bodyUniqueIdA = m_bodyUniqueIdA;
-											pt.m_bodyUniqueIdB = m_bodyUniqueIdB;
-											const btManifoldPoint& srcPt = cp;
-											pt.m_contactDistance = srcPt.getDistance();
-											pt.m_contactFlags = 0;
-											pt.m_linkIndexA = m_linkIndexA;
-											pt.m_linkIndexB = m_linkIndexB;
-											for (int j = 0; j < 3; j++)
 											{
-												pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
+												b3ContactPointData pt;
-												pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
+												pt.m_bodyUniqueIdA = m_bodyUniqueIdA;
-												pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
+												pt.m_bodyUniqueIdB = m_bodyUniqueIdB;
+												const btManifoldPoint& srcPt = cp;
+												pt.m_contactDistance = srcPt.getDistance();
+												pt.m_contactFlags = 0;
+												pt.m_linkIndexA = m_linkIndexA;
+												pt.m_linkIndexB = m_linkIndexB;
+												for (int j = 0; j < 3; j++)
+												{
+													pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
+													pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
+													pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
+												}
+												pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime;
+												//                                    pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
+												m_cachedContactPoints.push_back(pt);
 											}
-											pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime;
-											//                                    pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
-											m_cachedContactPoints.push_back(pt);
-
 											return 1;
+
 										}
 									};
 

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -94,9 +94,19 @@ struct TinyRendererVisualShapeConverterInternalData
 	m_swWidth(START_WIDTH),
 	m_swHeight(START_HEIGHT),
 	m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB),
+	m_lightDirection(btVector3(-5,200,-40)),
 	m_hasLightDirection(false),
+	m_lightColor(btVector3(1.0,1.0,1.0)),
     m_hasLightColor(false),
-    m_hasShadow(false)
+	m_lightDistance(2.0),
+	m_hasLightDistance(false),
+	m_lightAmbientCoeff(0.6),
+    m_hasLightAmbientCoeff(false),
+	m_lightDiffuseCoeff(0.35),
+    m_hasLightDiffuseCoeff(false),
+	m_lightSpecularCoeff(0.05),
+    m_hasLightSpecularCoeff(false),
+	m_hasShadow(false)
 	{
 	    m_depthBuffer.resize(m_swWidth*m_swHeight);
         m_shadowBuffer.resize(m_swWidth*m_swHeight);

src/BulletCollision/CollisionShapes/btCapsuleShape.cpp

@@ -19,8 +19,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
 #include "LinearMath/btQuaternion.h"
 
-btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
+btCapsuleShape::btCapsuleShape(btScalar orgRadius, btScalar height) : btConvexInternalShape ()
 {
+	m_collisionMargin = orgRadius;
+	btScalar radius = 0.f;
 	m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
 	m_upAxis = 1;
 	m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
@@ -133,7 +135,7 @@ void	btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con
 	btVector3 halfExtents(radius,radius,radius);
 	halfExtents[getUpAxis()]+=getHalfHeight();
 
-	btScalar margin = CONVEX_DISTANCE_MARGIN;
+	btScalar margin = m_collisionMargin;
 
 	btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
 	btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
@@ -149,8 +151,10 @@ void	btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con
 
 }
 
-btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
+btCapsuleShapeX::btCapsuleShapeX(btScalar orgRadius,btScalar height)
 {
+	m_collisionMargin = orgRadius;
+	btScalar radius = 0.f;
 	m_upAxis = 0;
 	m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
 }
@@ -160,8 +164,10 @@ btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
 
 
 
-btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
+btCapsuleShapeZ::btCapsuleShapeZ(btScalar orgRadius,btScalar height)
 {
+	m_collisionMargin = orgRadius;
+	btScalar radius = 0.f;
 	m_upAxis = 2;
 	m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
 }