export btHeightfieldTerrainShape to PyBullet. Note that tinyrenderer doesn't support rendering it (it would be too slow on CPU)

fix rare getKeyboardEvents threading issue
change texture color to default plane.urdf blue

Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp

@@ -15,6 +15,7 @@ subject to the following restrictions:
 
 #include "btWorldImporter.h"
 #include "btBulletDynamicsCommon.h"
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
 #ifdef USE_GIMPACT
 #include "BulletCollision/Gimpact/btGImpactShape.h"
 #endif
@@ -1784,6 +1785,19 @@ btMultiSphereShape* btWorldImporter::createMultiSphereShape(const btVector3* pos
 	return shape;
 }
 
+class btHeightfieldTerrainShape* btWorldImporter::createHeightfieldShape(int heightStickWidth, int heightStickLength,
+	const void* heightfieldData, btScalar heightScale,
+	btScalar minHeight, btScalar maxHeight,
+	int upAxis, int heightDataType,
+	bool flipQuadEdges)
+{
+
+	btHeightfieldTerrainShape* shape = new btHeightfieldTerrainShape(heightStickWidth, heightStickLength,
+			heightfieldData, heightScale, minHeight, maxHeight, upAxis, PHY_ScalarType(heightDataType), flipQuadEdges);
+	m_allocatedCollisionShapes.push_back(shape);
+	return shape;
+}
+
 btRigidBody& btWorldImporter::getFixedBody()
 {
 	static btRigidBody s_fixed(0, 0, 0);

Extras/Serialize/BulletWorldImporter/btWorldImporter.h

@@ -197,6 +197,12 @@ public:
 
 	virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData);
 
+	virtual class btHeightfieldTerrainShape* createHeightfieldShape(int heightStickWidth, int heightStickLength,
+		const void* heightfieldData, btScalar heightScale,
+		btScalar minHeight, btScalar maxHeight,
+		int upAxis, int heightDataType,
+		bool flipQuadEdges);
+
 	///acceleration and connectivity structures
 	virtual btOptimizedBvh* createOptimizedBvh();
 	virtual btTriangleInfoMap* createTriangleInfoMap();

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -262,9 +262,10 @@ public:
 	btAlignedObjectArray<GLInstanceVertex>* m_pVerticesOut;
 	btAlignedObjectArray<int>* m_pIndicesOut;
 	btVector3 m_aabbMin, m_aabbMax;
+	btScalar m_textureScaling;
 
 	MyTriangleCollector2(const btVector3& aabbMin, const btVector3& aabbMax)
-		:m_aabbMin(aabbMin), m_aabbMax(aabbMax)
+		:m_aabbMin(aabbMin), m_aabbMax(aabbMax), m_textureScaling(1)
 	{
 		m_pVerticesOut = 0;
 		m_pIndicesOut = 0;
@@ -284,9 +285,11 @@ public:
 				v.xyzw[l] = tris[k][l];
 				v.normal[l] = normal[l];
 			}
-
-			v.uv[1] = 1-((v.xyzw[0] - m_aabbMin[0]) / (m_aabbMax[0] - m_aabbMin[0]));
-			v.uv[0] = ((v.xyzw[1] - m_aabbMin[1]) / (m_aabbMax[1] - m_aabbMin[1]));
+			
+			btVector3 extents = m_aabbMax - m_aabbMin;
+			
+			v.uv[0] = (1.-((v.xyzw[0] - m_aabbMin[0]) / (m_aabbMax[0] - m_aabbMin[0])))*m_textureScaling;
+			v.uv[1] = (1.-(v.xyzw[1] - m_aabbMin[1]) / (m_aabbMax[1] - m_aabbMin[1]))*m_textureScaling;
 
 			m_pIndicesOut->push_back(m_pVerticesOut->size());
 			m_pVerticesOut->push_back(v);
@@ -433,7 +436,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
 
 	if (m_data->m_checkedTexture < 0)
 	{
-		m_data->m_checkedTexture = createCheckeredTexture(192, 192, 255);
+		m_data->m_checkedTexture = createCheckeredTexture(173, 199, 255);
 	}
 
 	if (m_data->m_checkedTextureGrey < 0)
@@ -459,6 +462,10 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
 		tr.setIdentity();
 		heightField->getAabb(tr, aabbMin, aabbMax);
 		MyTriangleCollector2  col(aabbMin, aabbMax);
+		if (heightField->getUserValue3())
+		{
+			col.m_textureScaling = heightField->getUserValue3();
+		}
 		col.m_pVerticesOut = &gfxVertices;
 		col.m_pIndicesOut = &indices;
 		for (int k = 0; k < 3; k++)
@@ -472,7 +479,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
 			int userImage = heightField->getUserIndex2();
 			if (userImage == -1)
 			{
-				userImage = m_data->m_checkedTextureGrey;
+				userImage = m_data->m_checkedTexture;
 			}
 			int shapeId = m_data->m_glApp->m_renderer->registerShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(),1, userImage);
 			collisionShape->setUserIndex(shapeId);

examples/Heightfield/HeightfieldExample.cpp

@@ -491,8 +491,22 @@ getRawHeightfieldData
                     for (int j = 0; j < width; ++j)
                     {
                         float y = j * s_gridSpacing;
-                        float z = double(image[i*3+width*j*3])*(40./256.);
+                        float z = double(image[i*3+width*j*3])*(14./256.);
                         convertFromFloat(p, z, type);
+						// update min/max
+						if (!i && !j) {
+							minHeight = z;
+							maxHeight = z;
+						}
+						else {
+							if (z < minHeight) {
+								minHeight = z;
+							}
+							if (z > maxHeight) {
+								maxHeight = z;
+							}
+						}
+
                         p += bytesPerElement;
                     }
                 }
@@ -574,6 +588,19 @@ getRawHeightfieldData
                         float y = j * s_gridSpacing;
                         float z = allValues[i+width*j];
                         convertFromFloat(p, z, type);
+						// update min/max
+						if (!i && !j) {
+							minHeight = z;
+							maxHeight = z;
+						}
+						else {
+							if (z < minHeight) {
+								minHeight = z;
+							}
+							if (z > maxHeight) {
+								maxHeight = z;
+							}
+						}
                         p += bytesPerElement;
                     }
                 }
@@ -1159,11 +1186,11 @@ void HeightfieldExample::resetPhysics(void)
 	// set origin to middle of heightfield
 	btTransform tr;
 	tr.setIdentity();
-	tr.setOrigin(btVector3(0, 0, 0));
+	tr.setOrigin(btVector3(0, 0, -4));
 
 	if (m_model== eImageFile)
 	{
-		tr.setOrigin(btVector3(0, 0, -24));
+		
 		b3BulletDefaultFileIO fileIO;
 		char relativeFileName[1024];
 		int found = fileIO.findFile("heightmaps/gimp_overlay_out.png", relativeFileName, 1024);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1310,6 +1310,32 @@ B3_SHARED_API int b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle comm
 	return b3CreateCollisionShapeAddCapsule(commandHandle, radius, height);
 }
 
+B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/], double textureScaling)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
+	if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
+	{
+		int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
+		if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
+		{
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_HEIGHTFIELD;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
+			strcpy(command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName, fileName);
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
+			command->m_createUserShapeArgs.m_numUserShapes++;
+			return shapeIndex;
+		}
+	}
+	return -1;
+}
+
 B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -488,6 +488,8 @@ extern "C"
 	B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle, const double halfExtents[/*3*/]);
 	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 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(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -15,7 +15,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h"
 #include "../Utils/b3BulletDefaultFileIO.h"
-
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -1610,6 +1610,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
+	btAlignedObjectArray<unsigned char*> m_heightfieldDatas;
 	btAlignedObjectArray<int> m_allocatedTextures;
 	btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
 	btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
@@ -2806,6 +2807,10 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 		}
 		delete shape;
 	}
+	for (int j = 0; j < m_data->m_heightfieldDatas.size(); j++)
+	{
+		delete[] m_data->m_heightfieldDatas[j];
+	}
 	for (int j = 0; j < m_data->m_meshInterfaces.size(); j++)
 	{
 		delete m_data->m_meshInterfaces[j];
@@ -4217,18 +4222,25 @@ bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedM
 
 #define MYLINELENGTH 16*32768
 
-static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width, int& height)
+static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY_ScalarType type, const char* fileName, int& width, int& height,
+	btScalar& minHeight,
+	btScalar& maxHeight)
 {
-	int s_gridSize = width;
-	btScalar s_gridSpacing = 0.5;
-	btScalar s_gridHeightScale = 0.02;
-
-	PHY_ScalarType type = PHY_FLOAT;
-	if (1)//model == eImageFile)
+	
+	std::string ext;
+	std::string fn(fileName);
+	std::string ext_ = fn.substr(fn.size() - 4);
+	for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
 	{
-		b3BulletDefaultFileIO fileIO;
+		ext += char(tolower(*i));
+	}
+
+	
+	if (ext != ".txt")
+	{
+		
 		char relativeFileName[1024];
-		int found = fileIO.findFile(fileName, relativeFileName, 1024);
+		int found = fileIO.findResourcePath(fileName, relativeFileName, 1024);
 		
 		b3AlignedObjectArray<char> buffer;
 		buffer.reserve(1024);
@@ -4256,40 +4268,59 @@ static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width,
 			unsigned char* image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
 			if (image)
 			{
-				
 				fileIO.fileClose(fileId);
-				long nElements = ((long)s_gridSize) * s_gridSize;
+				int nElements = width * height;
 				int bytesPerElement = sizeof(btScalar); 
 				btAssert(bytesPerElement > 0 && "bad bytes per element");
 
-				long nBytes = nElements * bytesPerElement;
+				int nBytes = nElements * bytesPerElement;
 				unsigned char * raw = new unsigned char[nBytes];
 				btAssert(raw && "out of memory");
 
 				unsigned char * p = raw;
-				for (int i = 0; i < width; ++i)
+				for (int j = 0; j < height; ++j)
+				
 				{
-					float x = i * s_gridSpacing;
-					for (int j = 0; j < width; ++j)
+					
+					for (int i = 0; i < width; ++i)
 					{
-						float y = j * s_gridSpacing;
-						float z = double(image[i * 3 + width*j * 3])*(40. / 256.);
+						
+						float z = double(image[(width-1-i) * 3+ width*j * 3])*(1. / 255.);
 						btScalar * pf = (btScalar *)p;
 						*pf = z;
 						p += bytesPerElement;
+						// update min/max
+						if (!i && !j) 
+						{
+							minHeight = z;
+							maxHeight = z;
+						}
+						else 
+						{
+							if (z < minHeight) 
+							{
+								minHeight = z;
+							}
+							if (z > maxHeight) 
+							{
+								maxHeight = z;
+							}
+						}
 					}
 				}
+				free (image);
+
 				return raw;
 			}
 		}
 	}
-#if 0
-	if (model == eCSVFile)
+
+	if (ext == ".txt")
 	{
+		//read a csv file as used in DeepLoco
 		{
-			b3BulletDefaultFileIO fileIO;
 			char relativePath[1024];
-			int found = fileIO.findFile("heightmaps/ground0.txt", relativePath, 1024);
+			int found = fileIO.findResourcePath(fileName, relativePath, 1024);
 			char lineBuffer[MYLINELENGTH];
 			int slot = fileIO.fileOpen(relativePath, "r");
 			int rows = 0;
@@ -4302,37 +4333,40 @@ static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width,
 				while (lineChar = fileIO.readLine(slot, lineBuffer, MYLINELENGTH))
 				{
 					rows = 0;
-					char** values = urdfStrSplit(lineChar, ",");
-					if (values)
+					std::string line(lineChar);
+					int pos=0;
+					while (pos < line.length())
 					{
-						int index = 0;
-						char* value;
-						while (value = values[index++])
+						int nextPos = pos+1;
+						while (nextPos < line.length())
 						{
-							std::string strval(value);
-							double v;
-							if (sscanf(value, "%lf", &v) == 1)
+							if (line[nextPos-1] == ',')
 							{
-								//printf("strlen = %d\n", strval.length());
-								//printf("value[%d,%d]=%s or (%f)", cols,rows,value, v);
-								allValues.push_back(v);
-								rows++;
+								break;
 							}
+							nextPos++;
 						}
+						std::string substr = line.substr(pos, nextPos-pos-1);
+
+						double v;
+						if (sscanf(substr.c_str(), "%lf", &v) == 1)
+						{
+							allValues.push_back(v);
+							rows++;
+						}
+						pos = nextPos;
 					}
 					cols++;
 
 				}
-				printf("done, rows=%d, cols=%d\n", rows, cols);
-				int width = rows - 1;
-				s_gridSize = rows;
-				s_gridSpacing = 0.2;
-				s_gridHeightScale = 0.2;
+				width = rows;
+				height = cols;
+				
 				fileIO.fileClose(slot);
-				long nElements = ((long)s_gridSize) * s_gridSize;
+				int nElements = width * height;
 				//	std::cerr << "  nElements = " << nElements << "\n";
 
-				int bytesPerElement = getByteSize(type);
+				int bytesPerElement = sizeof(btScalar);
 				//	std::cerr << "  bytesPerElement = " << bytesPerElement << "\n";
 				btAssert(bytesPerElement > 0 && "bad bytes per element");
 
@@ -4341,23 +4375,43 @@ static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width,
 				unsigned char * raw = new unsigned char[nBytes];
 				btAssert(raw && "out of memory");
 
-				byte_t * p = raw;
+				unsigned char* p = raw;
 				for (int i = 0; i < width; ++i)
 				{
-					float x = i * s_gridSpacing;
-					for (int j = 0; j < width; ++j)
+					
+					for (int j = 0; j < height; ++j)
 					{
-						float y = j * s_gridSpacing;
+						
 						float z = allValues[i + width*j];
-						convertFromFloat(p, z, type);
+						//convertFromFloat(p, z, type);
+						btScalar * pf = (btScalar *)p;
+						*pf = z;
 						p += bytesPerElement;
+						// update min/max
+						if (!i && !j)
+						{
+							minHeight = z;
+							maxHeight = z;
+						}
+						else
+						{
+							if (z < minHeight)
+							{
+								minHeight = z;
+							}
+							if (z > maxHeight)
+							{
+								maxHeight = z;
+							}
+						}
+						
 					}
 				}
 				return raw;
 			}
 		}
 	}
-#endif
+
 	
 	return 0;
 }
@@ -4474,7 +4528,43 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 			}
 			case GEOM_HEIGHTFIELD:
 			{
-				
+				int width;
+				int height;
+				btScalar minHeight, maxHeight;
+				PHY_ScalarType scalarType = PHY_FLOAT;
+				CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
+				unsigned char* heightfieldData = MyGetRawHeightfieldData(*fileIO, scalarType, clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName, width, height, minHeight, maxHeight);
+				if (heightfieldData)
+				{
+					
+					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);
+					
+					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);
+					
+					this->m_data->m_heightfieldDatas.push_back(heightfieldData);
+						
+				}
 				break;
 			}
 			case GEOM_PLANE:
@@ -4497,6 +4587,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 
 				break;
 			}
+			
 			case GEOM_MESH:
 			{
 				btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],

examples/SharedMemory/PhysicsServerExample.cpp

@@ -1499,6 +1499,7 @@ public:
 	{
 		//printf("key=%d, state=%d\n", key,state);
 		{
+			m_args[0].m_csGUI->lock();
 			int keyIndex = -1;
 			//is already there?
 			for (int i = 0; i < m_args[0].m_keyboardEvents.size(); i++)
@@ -1515,7 +1516,7 @@ public:
 				b3KeyboardEvent ev;
 				ev.m_keyCode = key;
 				ev.m_keyState = eButtonIsDown + eButtonTriggered;
-				m_args[0].m_csGUI->lock();
+				
 				if (keyIndex >= 0)
 				{
 					if (0 == (m_args[0].m_keyboardEvents[keyIndex].m_keyState & eButtonIsDown))
@@ -1527,11 +1528,10 @@ public:
 				{
 					m_args[0].m_keyboardEvents.push_back(ev);
 				}
-				m_args[0].m_csGUI->unlock();
+				
 			}
 			else
 			{
-				m_args[0].m_csGUI->lock();
 				b3KeyboardEvent ev;
 				ev.m_keyCode = key;
 				ev.m_keyState = eButtonReleased;
@@ -1543,8 +1543,9 @@ public:
 				{
 					m_args[0].m_keyboardEvents.push_back(ev);
 				}
-				m_args[0].m_csGUI->unlock();
+				
 			}
+			m_args[0].m_csGUI->unlock();
 		}
 		/*printf("m_args[0].m_keyboardEvents.size()=%d\n", m_args[0].m_keyboardEvents.size());
 		for (int i=0;i<m_args[0].m_keyboardEvents.size();i++)

examples/SharedMemory/SharedMemoryCommands.h

@@ -958,6 +958,7 @@ struct b3CreateUserShapeData
 	int m_numIndices;
 	int m_numUVs;
 	int m_numNormals;
+	double m_heightfieldTextureScaling;
 	double m_rgbaColor[4];
 	double m_specularColor[3];
 };

examples/pybullet/examples/heightfield.py

@@ -1,9 +1,27 @@
 import pybullet as p
+import pybullet_data as pd
+
 import time
 
 p.connect(p.GUI)
-p.createCollisionShape(p.GEOM_HEIGHTFIELD)
-p.createMultiBody(0, 0)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+textureId = -1
+useDeepLocoCSV = False
+
+if useDeepLocoCSV:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)#Image8x4.png")#wm_height_out.png")
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[-8,0,-2], [0,0,0,1])
+else:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.1,.1,24],fileName = "heightmaps/wm_height_out.png")
+  textureId = p.loadTexture("heightmaps/gimp_overlay_out.png")
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.changeVisualShape(terrain, -1, textureUniqueId = textureId)
+ 
+ 
+p.changeVisualShape(terrain, -1, rgbaColor=[1,1,1,1])
+
 
 sphereRadius = 0.05
 colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
@@ -28,7 +46,7 @@ for i in range(3):
   for j in range(3):
     for k in range(3):
       basePosition = [
-          1 + i * 5 * sphereRadius, 1 + j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
+          i * 5 * sphereRadius, j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
       ]
       baseOrientation = [0, 0, 0, 1]
       if (k & 2):
@@ -51,6 +69,7 @@ for i in range(3):
                                       linkJointTypes=jointTypes,
                                       linkJointAxis=axis)
 
+      
       p.changeDynamics(sphereUid,
                        -1,
                        spinningFriction=0.001,
@@ -69,5 +88,6 @@ for i in range(p.getNumJoints(sphereUid)):
 while (1):
   keys = p.getKeyboardEvents()
   print(keys)
-
+  #getCameraImage note: software/TinyRenderer doesn't render/support heightfields!
+  #p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)
   time.sleep(0.01)

examples/pybullet/pybullet.c

@@ -7967,14 +7967,14 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 	double collisionFrameOrientation[4] = {0, 0, 0, 1};
 	char* fileName = 0;
 	int flags = 0;
-
+	double heightfieldTextureScaling = 1;
 	PyObject* halfExtentsObj = 0;
 	PyObject* verticesObj = 0;
 	PyObject* indicesObj = 0;
 
-	static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "vertices", "indices", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
-									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &physicsClientId))
+	static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "vertices", "indices", "heightfieldTextureScaling", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdi", kwlist,
+									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -8010,6 +8010,15 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 		{
 			shapeIndex = b3CreateCollisionShapeAddCylinder(commandHandle, radius, height);
 		}
+		if (shapeType == GEOM_HEIGHTFIELD && fileName)
+		{
+			if (meshScaleObj)
+			{
+				pybullet_internalSetVectord(meshScaleObj, meshScale);
+			}
+			shapeIndex = b3CreateCollisionShapeAddHeightfield(commandHandle, fileName, meshScale, heightfieldTextureScaling);
+
+		}
 		if (shapeType == GEOM_MESH && fileName)
 		{
 			pybullet_internalSetVectord(meshScaleObj, meshScale);

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -21,7 +21,8 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
 	int heightStickWidth, int heightStickLength, const void* heightfieldData,
 	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
 	PHY_ScalarType hdt, bool flipQuadEdges)
-	:m_userIndex2(-1)
+	:m_userIndex2(-1),
+	m_userValue3(0)
 {
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
 			   heightScale, minHeight, maxHeight, upAxis, hdt,
@@ -29,7 +30,8 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
 }
 
 btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
-	:m_userIndex2(-1)
+	:m_userIndex2(-1),
+	m_userValue3(0)
 {
 	// legacy constructor: support only float or unsigned char,
 	// 	and min height is zero

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -115,6 +115,7 @@ protected:
 	int m_vboundsChunkSize;
 
 	int m_userIndex2;
+	btScalar m_userValue3;
 
 	virtual btScalar getRawHeightFieldValue(int x, int y) const;
 	void quantizeWithClamp(int* out, const btVector3& point, int isMax) const;
@@ -197,6 +198,14 @@ public:
 	{
 		return m_userIndex2;
 	}
+	void setUserValue3(btScalar value)
+	{
+		m_userValue3 = value;
+	}
+	btScalar getUserValue3() const
+	{
+		return m_userValue3;
+	}
 };
 
 #endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H