Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

PyBullet: allow to update an existing heightfield shape

Browse Source 
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
This commit is contained in:
Erwin Coumans
2019-08-14 21:06:10 -07:00
parent c7fa8bfc58
commit 88d1788ee5
11 changed files with 201 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -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) {}

5
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@@ -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);
}

1
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@@ -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);

6
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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*/]);

157
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;
bool flipQuadEdges = false;
int upAxis = 2;
/*btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height,
heightfieldData,
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;
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);
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();
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]);
// 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);
btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(heightfieldShape, triangleInfoMap);
this->m_data->m_heightfieldDatas.push_back(heightfieldData);
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;
}

23
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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)

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

23
examples/pybullet/examples/heightfield.py
View File

@@ -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)

17
examples/pybullet/pybullet.c
View File

@@ -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);

4
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
View File

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