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

11 files changed, 201 insertions, 44 deletions

diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h
index d29928e47..c604410a8 100644
--- a/examples/CommonInterfaces/CommonGUIHelperInterface.h
+++ b/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) {}
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
index a3197749d..eecf94df6 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp
+++ b/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);
+}
\ No newline at end of file
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h
index b388ec2c4..e4a43b4be 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.h
+++ b/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);
 
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 646f03985..cac6b161f 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/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;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index ce1baef0c..9a488562f 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/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*/]);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index b8e0275cd..f6b48a8ce 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/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)
 {
@@ -4571,43 +4602,109 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 				if (heightfieldData)
 				{
 					
-					btScalar gridSpacing = 0.5;
-					btScalar gridHeightScale = 1. / 256.;
+					//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();
+
 
-					bool flipQuadEdges = false;
-					int upAxis = 2;
-					/*btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height,
-						heightfieldData,
+							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;
+						btScalar gridHeightScale = 1. / 256.;
+
+						bool flipQuadEdges = false;
+						int upAxis = 2;
+						/*btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height,
+							heightfieldData,
+								gridHeightScale,
+								minHeight, maxHeight,
+								upAxis, int(scalarType), flipQuadEdges);
+						*/
+						btHeightfieldTerrainShape* heightfieldShape = new btHeightfieldTerrainShape(width, height,
+							heightfieldData,
 							gridHeightScale,
 							minHeight, maxHeight,
-							upAxis, int(scalarType), flipQuadEdges);
-					*/
-					btHeightfieldTerrainShape* heightfieldShape = new btHeightfieldTerrainShape( width, height,
-					heightfieldData,
-					gridHeightScale,
-					minHeight, maxHeight,
-					upAxis, scalarType, flipQuadEdges);
-					m_data->m_collisionShapes.push_back(heightfieldShape);
-
-					heightfieldShape->setUserValue3(clientCmd.m_createUserShapeArgs.m_shapes[i].m_heightfieldTextureScaling);
-					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],
-						clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
-						clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
-					
-					heightfieldShape->setLocalScaling(localScaling);
+							upAxis, scalarType, flipQuadEdges);
+						m_data->m_collisionShapes.push_back(heightfieldShape);
 
-					btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
-					btGenerateInternalEdgeInfo(heightfieldShape, triangleInfoMap);
-					
-					this->m_data->m_heightfieldDatas.push_back(heightfieldData);
+						heightfieldShape->setUserValue3(clientCmd.m_createUserShapeArgs.m_shapes[i].m_heightfieldTextureScaling);
+						shape = heightfieldShape;
+						if (upAxis == 2)
+							heightfieldShape->setFlipTriangleWinding(true);
 						
+						// scale the shape
+						btVector3 localScaling(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
+							clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
+							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;
 			}
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index f5e1046e1..f3e670c0d 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/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)
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index f429bee59..4ff925a59 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/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
diff --git a/examples/pybullet/examples/heightfield.py b/examples/pybullet/examples/heightfield.py
index f8ffefee5..b01a5e0bb 100644
--- a/examples/pybullet/examples/heightfield.py
+++ b/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)
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index c3fcdbb45..a808ff15a 100644
--- a/examples/pybullet/pybullet.c
+++ b/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,
-									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &heightfieldDataObj, &numHeightfieldRows, &numHeightfieldColumns, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdOiiii", kwlist,
+									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &heightfieldDataObj, &numHeightfieldRows, &numHeightfieldColumns, &replaceHeightfieldIndex, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -8083,7 +8084,8 @@ 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);
@@ -8141,7 +8143,10 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 			{
 				pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation);
 			}
-			b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
+			if (collisionFramePositionObj || collisionFrameOrientationObj)
+			{
+				b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
+			}
 		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 		statusType = b3GetStatusType(statusHandle);
diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
index 7cf7e26b5..8dea98fc6 100644
--- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
+++ b/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
\ No newline at end of file