Increase mesh allocation for vertices/indices in PyBullet.createCollisionShape

See createMesh.py for an example.

The data has to fit in shared memory, hence the limit on Mac is lower than Windows and Linux:

#ifdef __APPLE__
#define B3_MAX_NUM_VERTICES 8192
#define B3_MAX_NUM_INDICES 32768
#else
#define B3_MAX_NUM_VERTICES 131072
#define B3_MAX_NUM_INDICES 524288
#endif

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1280,8 +1280,11 @@ B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle comm
 	return -1;
 }
 
-B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices)
+B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices)
 {
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command);
 	b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
@@ -1305,12 +1308,7 @@ B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandl
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
 
-			for (i=0;i<numVertices;i++)
+			cl->uploadBulletFileToSharedMemory((const char*)vertices, numVertices * sizeof(double)*3);
-			{
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0];
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1];
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2];
-			}
 			command->m_createUserShapeArgs.m_numUserShapes++;
 			return shapeIndex;
 		}
@@ -1318,8 +1316,10 @@ B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandl
 	return -1;
 }
 
-B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices)
+B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices)
 {
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command);
 	b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
@@ -1342,11 +1342,16 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHand
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
 
+			int totalUploadSizeInBytes = numVertices * sizeof(double) *3  + numIndices * sizeof(int);
+			char* data = (char*)malloc(totalUploadSizeInBytes);
+			double* vertexUpload = (double*)data;
+			int* indexUpload = (int*)(data + numVertices*sizeof(double)*3);
+
 			for (i=0;i<numVertices;i++)
 			{
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0];
+				vertexUpload[i*3+0]=vertices[i*3+0];
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1];
+				vertexUpload[i*3+1]=vertices[i*3+1];
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2];
+				vertexUpload[i*3+2]=vertices[i*3+2];
 			}
 			if (numIndices>B3_MAX_NUM_INDICES)
 				numIndices = B3_MAX_NUM_INDICES;
@@ -1354,9 +1359,11 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHand
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
 			for (i=0;i<numIndices;i++)
 			{
-				command->m_createUserShapeArgs.m_shapes[shapeIndex].m_indices[i]=indices[i];
+				indexUpload[i]=indices[i];
 			}
 			command->m_createUserShapeArgs.m_numUserShapes++;
+			cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
+			free(data);
 			return shapeIndex;
 		}
 	}

examples/SharedMemory/PhysicsClientC_API.h

@@ -466,8 +466,8 @@ extern "C"
 	B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
 	B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant);
 	B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]);
-	B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices);
+	B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices);
-	B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices);
+	B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices);
 	B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags);
 	B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]);
 	B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4215,6 +4215,17 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 
 				if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices)
 				{
+
+					int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices;
+					int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices;
+
+					//int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int);
+
+					char* data = bufferServerToClient;
+					double* vertexUpload = (double*)data;
+					int* indexUpload = (int*)(data + numVertices * sizeof(double)*3);
+
+
 					if (compound == 0)
 					{
 						compound = worldImporter->createCompoundShape();
@@ -4231,19 +4242,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 
 							for (int j = 0; j < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices / 3; j++)
 							{
-								int i0 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+0];
+								int i0 = indexUpload[j*3+0];
-								int i1 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+1];
+								int i1 = indexUpload[j*3+1];
-								int i2 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+2];
+								int i2 = indexUpload[j*3+2];
 
-								btVector3 v0(	clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+0],
+								btVector3 v0(vertexUpload[i0*3+0],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+1],
+									vertexUpload[i0*3+1],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+2]);
+									vertexUpload[i0*3+2]);
-								btVector3 v1(	clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+0],
+								btVector3 v1(vertexUpload[i1*3+0],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+1],
+									vertexUpload[i1*3+1],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+2]);
+									vertexUpload[i1*3+2]);
-								btVector3 v2(	clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+0],
+								btVector3 v2(vertexUpload[i2*3+0],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+1],
+									vertexUpload[i2*3+1],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+2]);
+									vertexUpload[i2*3+2]);
 								meshInterface->addTriangle(v0, v1, v2);
 							}
 						}
@@ -4274,9 +4285,9 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++)
 						{
 
-							btVector3 pt(	clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+0],
+							btVector3 pt(vertexUpload[v*3+0],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+1],
+								vertexUpload[v*3+1],
-											clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+2]);
+								vertexUpload[v*3+2]);
 							convexHull->addPoint(pt, false);
 						}
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -928,9 +928,7 @@ struct b3CreateUserShapeData
 	int m_collisionFlags;
 	int m_visualFlags;
 	int m_numVertices;
-	double m_vertices[B3_MAX_NUM_VERTICES*3];
 	int m_numIndices;
-	int m_indices[B3_MAX_NUM_INDICES];
 	double m_rgbaColor[4];
 	double m_specularColor[3];
 };

examples/SharedMemory/SharedMemoryPublic.h

@@ -941,8 +941,19 @@ enum eFileIOTypes
 	eInMemoryFileIO,
 };
 
+
 //limits for vertices/indices in PyBullet::createCollisionShape
-#define B3_MAX_NUM_VERTICES 16
+//Make sure the data fits in SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE
-#define B3_MAX_NUM_INDICES 16
+//(numVertices*sizeof(double)*3 + numIndices*sizeof(int)) < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE
+
+
+#ifdef __APPLE__
+#define B3_MAX_NUM_VERTICES 8192
+#define B3_MAX_NUM_INDICES 32768
+#else
+#define B3_MAX_NUM_VERTICES 131072
+#define B3_MAX_NUM_INDICES 524288
+#endif
+
 
 #endif  //SHARED_MEMORY_PUBLIC_H

examples/pybullet/examples/createMesh.py

@@ -0,0 +1,121 @@
+import pybullet as p
+import time
+import math
+
+p.connect(p.GUI)
+#don't create a ground plane, to allow for gaps etc
+p.resetSimulation()
+#p.createCollisionShape(p.GEOM_PLANE)
+#p.createMultiBody(0,0)
+#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
+p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]);
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
+
+#a few different ways to create a mesh:
+
+vertices=[ [-0.246350,-0.246483,-0.000624],
+[-0.151407, -0.176325, 0.172867],
+[ -0.246350, 0.249205, -0.000624],
+[ -0.151407, 0.129477, 0.172867],
+[ 0.249338, -0.246483, -0.000624],
+[ 0.154395, -0.176325, 0.172867],
+[ 0.249338, 0.249205, -0.000624],
+[ 0.154395, 0.129477, 0.172867]
+]
+indices=[0,3,2,
+3,6,2,
+7,4,6,
+5,0,4,
+6,0,2,
+3,5,7,
+0,1,3,
+3,7,6,
+7,5,4,
+5,1,0,
+6,4,0,
+3,1,5]
+#convex mesh from obj
+stoneId = p.createCollisionShape(p.GEOM_MESH,vertices=vertices,indices=indices)
+
+
+
+boxHalfLength = 0.5
+boxHalfWidth = 2.5
+boxHalfHeight = 0.1
+segmentLength = 5
+
+colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
+
+mass = 1
+visualShapeId = -1
+
+segmentStart = 0
+
+for i in range (segmentLength):
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
+	segmentStart=segmentStart-1
+	
+for i in range (segmentLength):
+	height = 0
+	if (i%2):
+		height=1
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height])
+	segmentStart=segmentStart-1
+
+baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.])
+
+for i in range (segmentLength):
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
+	segmentStart=segmentStart-1
+	if (i%2):
+		p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation)
+
+for i in range (segmentLength):
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
+	width=4
+	for j in range (width):
+		p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0])
+	segmentStart=segmentStart-1
+
+
+link_Masses=[1]
+linkCollisionShapeIndices=[colBoxId]
+linkVisualShapeIndices=[-1]
+linkPositions=[[0,0,0]]
+linkOrientations=[[0,0,0,1]]
+linkInertialFramePositions=[[0,0,0]]
+linkInertialFrameOrientations=[[0,0,0,1]]
+indices=[0]
+jointTypes=[p.JOINT_REVOLUTE]
+axis=[[1,0,0]]
+
+baseOrientation = [0,0,0,1]
+for i in range (segmentLength):
+	boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis)			
+	p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
+	print(p.getNumJoints(boxId))
+	for joint in range (p.getNumJoints(boxId)):
+		targetVelocity = 10
+		if (i%2):
+			targetVelocity =-10
+		p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100)
+	segmentStart=segmentStart-1.1
+
+
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+while (1):
+	camData = p.getDebugVisualizerCamera()
+	viewMat = camData[2]
+	projMat = camData[3]
+	p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+	keys = p.getKeyboardEvents()
+	p.stepSimulation()	
+	#print(keys)
+	time.sleep(0.01)
+	

examples/pybullet/pybullet.c

@@ -6696,9 +6696,12 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe
 					double vertex[3];
 					if (pybullet_internalSetVectord(vertexObj, vertex))
 					{
-						vertices[numVerticesOut*3+0]=vertex[0];
+						if (vertices)
-						vertices[numVerticesOut*3+1]=vertex[1];
+						{
-						vertices[numVerticesOut*3+2]=vertex[2];
+							vertices[numVerticesOut * 3 + 0] = vertex[0];
+							vertices[numVerticesOut * 3 + 1] = vertex[1];
+							vertices[numVerticesOut * 3 + 2] = vertex[2];
+						}
 						numVerticesOut++;
 					}
 				}
@@ -6730,7 +6733,10 @@ static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
 				for (i = 0; i < numIndicesSrc; i++)
 				{
 					int index = pybullet_internalGetIntFromSequence(seqIndicesObj,i);
-					indices[numIndicesOut]=index;
+					if (indices)
+					{
+						indices[numIndicesOut] = index;
+					}
 					numIndicesOut++;
 				}
 			}
@@ -6806,11 +6812,10 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 		}
 		if (shapeType == GEOM_MESH && verticesObj)
 		{
-			int numVertices=0;
+			int numVertices= extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
-			int numIndices=0;
+			int numIndices= extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES);
-
+			double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0;
-			double vertices[B3_MAX_NUM_VERTICES*3];
+			int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0;
-			int indices[B3_MAX_NUM_INDICES];
 
 			numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES);
 			pybullet_internalSetVectord(meshScaleObj, meshScale);
@@ -6822,11 +6827,13 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 
 			if (numIndices)
 			{
-				shapeIndex = b3CreateCollisionShapeAddConcaveMesh(commandHandle, meshScale, vertices, numVertices, indices, numIndices);
+				shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
 			} else
 			{
-				shapeIndex = b3CreateCollisionShapeAddConvexMesh(commandHandle, meshScale, vertices, numVertices);
+				shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices);
 			}
+			free(vertices);
+			free(indices);
 		}
 
 		if (shapeType == GEOM_PLANE)