Merge pull request #1619 from erwincoumans/master

PyBullet fix createVisualShape (inertial frame related), minor backward compatibility fix

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -94,8 +94,8 @@ struct GUIHelperInterface
                                   int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
 		  int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied){}
 
-	virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])=0;
+	virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]){}
-	virtual void setProjectiveTexture(bool useProjectiveTexture)=0;
+	virtual void setProjectiveTexture(bool useProjectiveTexture){}
 
 	virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;
 	

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -150,16 +150,16 @@ struct InternalVisualShapeData
 	int m_tinyRendererVisualShapeIndex;
 	int m_OpenGLGraphicsIndex;
 
-	UrdfVisual m_visualShape;
+	b3AlignedObjectArray<UrdfVisual> m_visualShapes;
-	btTransform m_localInertiaFrame;
+	
-	std::string m_pathPrefix;
+	b3AlignedObjectArray<std::string> m_pathPrefixes;
 
 	void clear()
 	{
-		m_tinyRendererVisualShapeIndex = 0;
+		m_tinyRendererVisualShapeIndex = -1;
-		m_OpenGLGraphicsIndex = 0;
+		m_OpenGLGraphicsIndex = -1;
-		m_localInertiaFrame.setIdentity();
+		m_visualShapes.clear();
-		m_pathPrefix = "";
+		m_pathPrefixes.clear();
 	}
 };
 
@@ -1921,10 +1921,13 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 			const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
 			if (visHandle)
 			{
-				if (visHandle->m_visualShape.m_geometry.m_hasLocalMaterial)
+				for (int i=0;i<visHandle->m_visualShapes.size();i++)
 				{
-					matCol = visHandle->m_visualShape.m_geometry.m_localMaterial.m_matColor;
+					if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
-					return true;
+					{
+						matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
+						return true;
+					}
 				}
 			}
 		}
@@ -2092,39 +2095,91 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 		b3Assert(0);
 	}
 
-	///quick hack: need to rethink the API/dependencies of this
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
-    virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
 	{
+		int graphicsIndex = -1;
+		double globalScaling = 1.f;//todo!
+		int flags=0;
+		BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
+		u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
+		
+		btAlignedObjectArray<GLInstanceVertex> vertices;
+		btAlignedObjectArray<int> indices;
+		btTransform startTrans; startTrans.setIdentity();
+		btAlignedObjectArray<BulletURDFTexture> textures;
+
 		if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
 		{
-			const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
+			InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
 			if (visHandle)
 			{
-				
+				if (visHandle->m_OpenGLGraphicsIndex>=0)
-				return visHandle->m_OpenGLGraphicsIndex;
+				{
+					//instancing. assume the inertial frame is identical
+					graphicsIndex = visHandle->m_OpenGLGraphicsIndex;
+				} else
+				{
+					for (int v = 0;v<visHandle->m_visualShapes.size();v++)
+					{
+						u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse()*visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures);
+					}
+			
+					if (vertices.size() && indices.size())
+					{
+						if (1)
+						{
+							int textureIndex = -1;
+							if (textures.size())
+							{
+
+								textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
+							}
+						
+							{
+								B3_PROFILE("registerGraphicsShape");
+								graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
+								visHandle->m_OpenGLGraphicsIndex = graphicsIndex;
+							}
+						}
+					}
+				}
 			}
+
 		}
-		return -1;
+		return graphicsIndex;
 	}
     
     virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const  
 	{
 		//if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
 
-		
-
-
 		UrdfModel model;// = m_data->m_urdfParser.getModel();
 		UrdfLink link;
-		int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
+
-		if (colShapeUniqueId>=0)
+		if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
 		{
-			InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
+			const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
-			if (handle)
+			if (visHandle)
 			{
-				for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
+				for (int i=0;i<visHandle->m_visualShapes.size();i++)
 				{
-					link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
+					link.m_visualArray.push_back(visHandle->m_visualShapes[i]);
+				}
+			}
+		}
+
+		if (link.m_visualArray.size()==0)
+		{
+			int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
+			if (colShapeUniqueId>=0)
+			{
+				InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
+				if (handle)
+				{
+					for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
+					{
+						link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
+					}
 				}
 			}
 		}
@@ -3968,19 +4023,16 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
 	u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
 	btTransform localInertiaFrame;
 	localInertiaFrame.setIdentity();
-	btTransform childTrans;
+	
-	childTrans.setIdentity();
 	const char* pathPrefix = "";
-
+	int visualShapeUniqueId = -1;
-	btAlignedObjectArray<GLInstanceVertex> vertices;
+	
-	btAlignedObjectArray<int> indices;
-	btTransform startTrans; startTrans.setIdentity();
-	btAlignedObjectArray<BulletURDFTexture> textures;
 	
     UrdfVisual visualShape;	
 	for (int userShapeIndex = 0; userShapeIndex< clientCmd.m_createUserShapeArgs.m_numUserShapes; userShapeIndex++)
 	{
-
+		btTransform childTrans;
+		childTrans.setIdentity();
 		visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type;
 		char relativeFileName[1024];
 		char pathPrefix[1024];
@@ -4106,41 +4158,23 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
 		}
 
 
-		u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures);
+		if (visualShapeUniqueId<0)
-
-	}
-	
-	if (vertices.size() && indices.size())
-	{
-		if (1)
 		{
-			int textureIndex = -1;
+			visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
-			if (textures.size())
-			{
-
-				textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
-			}
-			int graphicsIndex = -1;
-			{
-				B3_PROFILE("registerGraphicsShape");
-				graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
-				if (graphicsIndex >= 0)
-				{
-					int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
-					InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
-					visualHandle->m_OpenGLGraphicsIndex = graphicsIndex;
-					visualHandle->m_tinyRendererVisualShapeIndex = -1;
-					//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
-					//store needed info for tinyrenderer
-					visualHandle->m_localInertiaFrame = localInertiaFrame;
-					visualHandle->m_visualShape = visualShape;
-					visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : "";
-
-					serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
-					serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
-				}
-			}
 		}
+		InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
+		visualHandle->m_OpenGLGraphicsIndex = -1;
+		visualHandle->m_tinyRendererVisualShapeIndex = -1;
+		//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
+		//store needed info for tinyrenderer
+		
+		visualShape.m_linkLocalFrame = childTrans;
+		visualHandle->m_visualShapes.push_back(visualShape);
+		visualHandle->m_pathPrefixes.push_back(pathPrefix[0] ? pathPrefix : "");
+		
+		serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
+		serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
+
 	}
 
 	return hasStatus;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -64,31 +64,31 @@ protected:
 
 	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									const btContactSolverInfo& infoGlobal,
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
 	void setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
 	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 	btSolverConstraint&	addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
 
-	
+
-	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
+	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
 								const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
 
 	static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
 
-	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
+	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
 										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long	m_btSeed2;
 
-	
+
 	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
 
 	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
@@ -96,7 +96,7 @@ protected:
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
     virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal);
-    void convertJoint(btSolverConstraint* destConstraintRow, btTypedConstraint* srcConstraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
+    void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
 
     virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
@@ -122,9 +122,9 @@ protected:
     {
         return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
     }
-		
+
 protected:
-	
+
     void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
     void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
     void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@@ -139,15 +139,15 @@ protected:
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btSequentialImpulseConstraintSolver();
 	virtual ~btSequentialImpulseConstraintSolver();
 
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-		
+
 	///clear internal cached data and reset random seed
 	virtual	void	reset();
-	
+
 	unsigned long btRand2();
 
 	int btRandInt2 (int n);
@@ -161,7 +161,7 @@ public:
 		return m_btSeed2;
 	}
 
-	
+
 	virtual btConstraintSolverType	getSolverType() const
 	{
 		return BT_SEQUENTIAL_IMPULSE_SOLVER;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp

@@ -537,7 +537,7 @@ void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersist
     BT_PROFILE( "allocAllContactConstraints" );
     btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray;
     cachedInfoArray.resizeNoInitialize( numManifolds );
-    if (false)
+    if (/* DISABLES CODE */ (false))
     {
         // sequential
         internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal);

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -127,7 +127,7 @@ protected:
 public:
 
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btSequentialImpulseConstraintSolverMt();
 	virtual ~btSequentialImpulseConstraintSolverMt();
 
@@ -136,11 +136,11 @@ public:
     btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
     btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
     btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
-    btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd );
+    btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd );
 
-    void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifold, int numManifolds, const btContactSolverInfo& infoGlobal);
+    void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
     void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
-    void internalSetupContactConstraints(int iContact, const btContactSolverInfo& infoGlobal);
+    void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
     void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
     void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
     void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);