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

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

Browse Source 
fix rare getKeyboardEvents threading issue
change texture color to default plane.urdf blue
This commit is contained in:
Erwin Coumans
2019-07-29 20:23:38 -07:00
parent 36f3adc03f
commit 51fba6f78d
14 changed files with 286 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

2
examples/SharedMemory/PhysicsClientC_API.h
View File

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

187
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

9
examples/SharedMemory/PhysicsServerExample.cpp
View File

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

1
examples/SharedMemory/SharedMemoryCommands.h
View File

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