Merge pull request #2367 from erwincoumans/master

allow to update heightfield, see PyBullet: allow to update an existing heightfield shape Also, use flags = p.GEOM_CONCAVE_INTERNAL_EDGE to enable internal edge filtering for heightfield (disabled by default) See https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/heightfield.py

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -47,7 +47,7 @@ struct GUIHelperInterface
 	virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {}
 	virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {}
 	virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) {}
-
+	virtual void updateShape(int shapeIndex, float* vertices) {}
 	virtual int getShapeIndexFromInstance(int instanceUid) { return -1; }
 	virtual void replaceTexture(int shapeIndex, int textureUid) {}
 	virtual void removeTexture(int textureUid) {}

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -1496,3 +1496,8 @@ void OpenGLGuiHelper::computeSoftBodyVertices(btCollisionShape* collisionShape,
 		}
 	}
 }
+
+void OpenGLGuiHelper::updateShape(int shapeIndex, float* vertices)
+{
+	m_data->m_glApp->m_renderer->updateShape(shapeIndex, vertices);
+}

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -33,6 +33,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	virtual void removeTexture(int textureUid);
 	virtual int getShapeIndexFromInstance(int instanceUid);
 	virtual void replaceTexture(int shapeIndex, int textureUid);
+	virtual void updateShape(int shapeIndex, float* vertices);
 
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1340,6 +1340,8 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHand
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = -1;
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = -1;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = -1;
+
 			command->m_createUserShapeArgs.m_numUserShapes++;
 			return shapeIndex;
 		}
@@ -1347,7 +1349,7 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHand
 	return -1;
 }
 
-B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns)
+B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns, int replaceHeightfieldIndex)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
 	b3Assert(cl);
@@ -1370,6 +1372,8 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle ph
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = numHeightfieldRows;
 			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = numHeightfieldColumns;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = replaceHeightfieldIndex;
+			
 			cl->uploadBulletFileToSharedMemory((const char*)heightfieldData, numHeightfieldRows*numHeightfieldColumns* sizeof(float));
 			command->m_createUserShapeArgs.m_numUserShapes++;
 			return shapeIndex;

examples/SharedMemory/PhysicsClientC_API.h

@@ -490,7 +490,7 @@ extern "C"
 	B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
 	B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
 	B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/], double textureScaling);
-	B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns);
+	B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns, int replaceHeightfieldIndex);
 	
 	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*/]);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4428,6 +4428,37 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
 	return 0;
 }
 
+class MyTriangleCollector4 : public btTriangleCallback
+{
+public:
+	btAlignedObjectArray<GLInstanceVertex>* m_pVerticesOut;
+	btAlignedObjectArray<int>* m_pIndicesOut;
+
+	MyTriangleCollector4()
+	{
+		m_pVerticesOut = 0;
+		m_pIndicesOut = 0;
+	}
+
+	virtual void processTriangle(btVector3* tris, int partId, int triangleIndex)
+	{
+		for (int k = 0; k < 3; k++)
+		{
+			GLInstanceVertex v;
+			v.xyzw[3] = 0;
+			v.uv[0] = v.uv[1] = 0.5f;
+			btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]);
+			normal.safeNormalize();
+			for (int l = 0; l < 3; l++)
+			{
+				v.xyzw[l] = tris[k][l];
+				v.normal[l] = normal[l];
+			}
+			m_pIndicesOut->push_back(m_pVerticesOut->size());
+			m_pVerticesOut->push_back(v);
+		}
+	}
+};
 
 bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
@@ -4569,6 +4600,70 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 					heightfieldData = MyGetRawHeightfieldData(*fileIO, scalarType, clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName, width, height, minHeight, maxHeight);
 				}
 				if (heightfieldData)
+				{
+					
+					//replace heightfield data or create new heightfield
+					if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_replaceHeightfieldIndex >=0)
+					{
+						int collisionShapeUid = clientCmd.m_createUserShapeArgs.m_shapes[i].m_replaceHeightfieldIndex;
+
+						InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid);
+						if (handle && handle->m_collisionShape && handle->m_collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
+						{
+							btHeightfieldTerrainShape* terrainShape = (btHeightfieldTerrainShape*)handle->m_collisionShape;
+							btScalar* heightfieldDest = (btScalar*)terrainShape->getHeightfieldRawData();
+							//replace the data
+
+							btScalar* datafl = (btScalar*)heightfieldData;
+
+							for (int i = 0; i < width*height; i++)
+							{
+								heightfieldDest[i] = datafl[i];
+							}
+							//update graphics
+
+							btAlignedObjectArray<GLInstanceVertex> gfxVertices;
+							btAlignedObjectArray<int> indices;
+							int strideInBytes = 9 * sizeof(float);
+
+
+							MyTriangleCollector4  col;
+							col.m_pVerticesOut = &gfxVertices;
+							col.m_pIndicesOut = &indices;
+							btVector3 aabbMin, aabbMax;
+							for (int k = 0; k < 3; k++)
+							{
+								aabbMin[k] = -BT_LARGE_FLOAT;
+								aabbMax[k] = BT_LARGE_FLOAT;
+							}
+							terrainShape->processAllTriangles(&col, aabbMin, aabbMax);
+							if (gfxVertices.size() && indices.size())
+							{
+								m_data->m_guiHelper->updateShape(terrainShape->getUserIndex(), &gfxVertices[0].xyzw[0]);
+							}
+
+							terrainShape->clearAccelerator();
+							terrainShape->buildAccelerator();
+
+
+							btTriangleInfoMap* oldTriangleInfoMap = terrainShape->getTriangleInfoMap();
+							delete (oldTriangleInfoMap);
+							terrainShape->setTriangleInfoMap(0);
+
+							if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
+							{
+								btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
+								btGenerateInternalEdgeInfo(terrainShape, triangleInfoMap);
+							}
+							serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid;
+							delete worldImporter;
+							serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;
+						}
+						
+						delete heightfieldData;
+						return hasStatus;
+					}
+					else
 					{
 
 						btScalar gridSpacing = 0.5;
@@ -4593,8 +4688,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						shape = heightfieldShape;
 						if (upAxis == 2)
 							heightfieldShape->setFlipTriangleWinding(true);
-					//buildAccelerator is optional, it may not support all features.
-					heightfieldShape->buildAccelerator();
 						
 						// scale the shape
 						btVector3 localScaling(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
@@ -4602,12 +4695,16 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 							clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
 
 						heightfieldShape->setLocalScaling(localScaling);
+						//buildAccelerator is optional, it may not support all features.
+						heightfieldShape->buildAccelerator();
 
+						if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
+						{
 							btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
 							btGenerateInternalEdgeInfo(heightfieldShape, triangleInfoMap);
-					
+						}
 						this->m_data->m_heightfieldDatas.push_back(heightfieldData);
-						
+					}
 				}
 				break;
 			}

examples/SharedMemory/PhysicsServerExample.cpp

@@ -127,6 +127,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIHelperChangeTexture,
 	eGUIHelperRemoveTexture,
 	eGUIHelperSetVisualizerFlagCheckRenderedFrame,
+	eGUIHelperUpdateShape,
 };
 
 #include <stdio.h>
@@ -865,6 +866,18 @@ public:
 		workerThreadWait();
 	}
 
+	int m_updateShapeIndex;
+	float* m_updateShapeVertices;
+
+	virtual void updateShape(int shapeIndex, float* vertices)
+	{
+		m_updateShapeIndex = shapeIndex;
+		m_updateShapeVertices = vertices;
+		
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIHelperUpdateShape);
+		workerThreadWait();
+	}
 	virtual int registerTexture(const unsigned char* texels, int width, int height)
 	{
 		int* cachedTexture = m_cachedTextureIds[texels];
@@ -1916,6 +1929,15 @@ void PhysicsServerExample::updateGraphics()
 			m_multiThreadedHelper->mainThreadRelease();
 			break;
 		}
+
+		case eGUIHelperUpdateShape:
+		{
+			B3_PROFILE("eGUIHelperUpdateShape");
+			m_multiThreadedHelper->m_childGuiHelper->updateShape(m_multiThreadedHelper->m_updateShapeIndex, m_multiThreadedHelper->m_updateShapeVertices);
+			m_multiThreadedHelper->mainThreadRelease();
+			break;
+		}
+
 		case eGUIHelperRegisterGraphicsShape:
 		{
 			B3_PROFILE("eGUIHelperRegisterGraphicsShape");
@@ -2039,6 +2061,7 @@ void PhysicsServerExample::updateGraphics()
 			break;
 		}
 
+
 		case eGUIHelperSetVisualizerFlagCheckRenderedFrame:
 		{
 			if (m_renderedFrames != m_multiThreadedHelper->m_renderedFrames)

examples/SharedMemory/SharedMemoryCommands.h

@@ -965,6 +965,7 @@ struct b3CreateUserShapeData
 	int m_numHeightfieldColumns;
 	double m_rgbaColor[4];
 	double m_specularColor[3];
+	int m_replaceHeightfieldIndex;
 };
 
 #define MAX_COMPOUND_COLLISION_SHAPES 16

examples/pybullet/examples/heightfield.py

@@ -1,6 +1,6 @@
 import pybullet as p
 import pybullet_data as pd
-
+import math
 import time
 
 p.connect(p.GUI)
@@ -11,6 +11,7 @@ textureId = -1
 useProgrammatic = 0
 useTerrainFromPNG = 1
 useDeepLocoCSV = 2
+updateHeightfield = False
 
 heightfieldSource = useProgrammatic
 import random
@@ -30,7 +31,7 @@ if heightfieldSource==useProgrammatic:
       heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
       
       
-  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, )
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
   terrain  = p.createMultiBody(0, terrainShape)
   p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
 
@@ -114,7 +115,23 @@ for i in range(p.getNumJoints(sphereUid)):
 
 
 while (p.isConnected()):
-  #keys = p.getKeyboardEvents()
+  keys = p.getKeyboardEvents()
+  
+  if updateHeightfield and heightfieldSource==useProgrammatic:
+    for j in range (int(numHeightfieldColumns/2)):
+      for i in range (int(numHeightfieldRows/2) ):
+        height = random.uniform(0,heightPerturbationRange)#+math.sin(time.time())
+        heightfieldData[2*i+2*j*numHeightfieldRows]=height
+        heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+        heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+        heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+    #GEOM_CONCAVE_INTERNAL_EDGE may help avoid getting stuck at an internal (shared) edge of the triangle/heightfield.
+    #GEOM_CONCAVE_INTERNAL_EDGE is a bit slower to build though.
+    #flags = p.GEOM_CONCAVE_INTERNAL_EDGE
+    flags = 0
+    terrainShape2 = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, flags = flags, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, replaceHeightfieldIndex = terrainShape)
+    
+
   #print(keys)
   #getCameraImage note: software/TinyRenderer doesn't render/support heightfields!
   #p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)

examples/pybullet/pybullet.c

@@ -1,4 +1,4 @@
-
+//#include "D:/develop/visual_leak_detector/include/vld.h"
 #include "../SharedMemory/PhysicsClientC_API.h"
 #include "../SharedMemory/PhysicsDirectC_API.h"
 #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
@@ -7987,7 +7987,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 	PyObject* heightfieldDataObj = 0;
 	int numHeightfieldRows = -1;
 	int numHeightfieldColumns = -1;
-
+	int replaceHeightfieldIndex = -1;
 	static char* kwlist[] = {"shapeType",
 							 "radius",
 							 "halfExtents",
@@ -8004,9 +8004,10 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 							 "heightfieldData",
 							 "numHeightfieldRows",
 							 "numHeightfieldColumns",
+							 "replaceHeightfieldIndex",
 							 "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdOiii", kwlist,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdOiiii", kwlist,
-									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &heightfieldDataObj, &numHeightfieldRows, &numHeightfieldColumns, &physicsClientId))
+									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &heightfieldDataObj, &numHeightfieldRows, &numHeightfieldColumns, &replaceHeightfieldIndex, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -8053,17 +8054,20 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 		}
 		if (shapeType == GEOM_HEIGHTFIELD && fileName==0 && heightfieldDataObj && numHeightfieldColumns>0 && numHeightfieldRows > 0)
 		{
+			PyObject* seqPoints=0;
+			int numHeightfieldPoints;
 			if (meshScaleObj)
 			{
 				pybullet_internalSetVectord(meshScaleObj, meshScale);
 			}
-			PyObject* seqPoints = PySequence_Fast(heightfieldDataObj, "expected a sequence");
+			seqPoints = PySequence_Fast(heightfieldDataObj, "expected a sequence");
-			int numHeightfieldPoints = PySequence_Size(heightfieldDataObj);
+			numHeightfieldPoints = PySequence_Size(heightfieldDataObj);
 			if (numHeightfieldPoints != numHeightfieldColumns*numHeightfieldRows)
 			{
 				PyErr_SetString(SpamError, "Size of heightfieldData doesn't match numHeightfieldColumns*numHeightfieldRows");
 				return NULL;
 			}
+			{
 				PyObject* item;
 				int i;
 				float* pointBuffer = (float*)malloc(numHeightfieldPoints*sizeof(float));
@@ -8083,11 +8087,13 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 						pointBuffer[i] = (float)PyFloat_AsDouble(item);
 					}
 				}
-			shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm, commandHandle, meshScale, heightfieldTextureScaling, pointBuffer, numHeightfieldRows, numHeightfieldColumns);
+				shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm, commandHandle, meshScale, heightfieldTextureScaling, pointBuffer, numHeightfieldRows, numHeightfieldColumns, replaceHeightfieldIndex);
+			
 				free(pointBuffer);
 				if (seqPoints)
 					Py_DECREF(seqPoints);
 			}
+		}
 		if (shapeType == GEOM_MESH && fileName)
 		{
 			pybullet_internalSetVectord(meshScaleObj, meshScale);
@@ -8141,8 +8147,11 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 			{
 				pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation);
 			}
+			if (collisionFramePositionObj || collisionFrameOrientationObj)
+			{
 				b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
 			}
+		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 		statusType = b3GetStatusType(statusHandle);
 		if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)

setup.py

@@ -485,7 +485,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
 
 setup(
     name='pybullet',
-    version='2.5.3',
+    version='2.5.5',
     description=
     'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
     long_description=

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -220,6 +220,10 @@ public:
 	{
 		m_triangleInfoMap = map;
 	}
+	const unsigned char* getHeightfieldRawData() const
+	{
+		return m_heightfieldDataUnsignedChar;
+	}
 };
 
 #endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H

src/BulletSoftBody/btCGProjection.h

@@ -33,7 +33,7 @@ struct DeformableContactConstraint
         append(rcontact);
     }
     
-    DeformableContactConstraint(const btVector3 dir)
+    DeformableContactConstraint(const btVector3& dir)
     {
         m_contact.push_back(NULL);
         m_direction.push_back(dir);