PyBullet: allow createVisualShape to pass vertices, indices, normals and uv coordinates. This can be combined with changeVisualShape to set the texture.

This commit is contained in:
Erwin Coumans
2019-01-29 12:03:11 -08:00
parent 63683e8f02
commit b257bd731b
9 changed files with 522 additions and 12 deletions

View File

@@ -1377,6 +1377,9 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy
{
indexUpload[i]=indices[i];
}
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = 0;
command->m_createUserShapeArgs.m_numUserShapes++;
cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
delete [] data;
@@ -1386,6 +1389,82 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle phy
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs)
{
if (numUVs == 0 && numNormals == 0)
{
return b3CreateCollisionShapeAddConcaveMesh(physClient, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
}
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
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 && numVertices >= 0 && numIndices >= 0)
{
int i = 0;
if (numVertices>B3_MAX_NUM_VERTICES)
numVertices = B3_MAX_NUM_VERTICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
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_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0] = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int) + numNormals*sizeof(double)*3+numUVs*sizeof(double)*2;
char* data = new char[totalUploadSizeInBytes];
double* vertexUpload = (double*)data;
int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int));
double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+ numNormals * sizeof(double) * 3);
for (i = 0; i<numVertices; i++)
{
vertexUpload[i * 3 + 0] = vertices[i * 3 + 0];
vertexUpload[i * 3 + 1] = vertices[i * 3 + 1];
vertexUpload[i * 3 + 2] = vertices[i * 3 + 2];
}
if (numIndices>B3_MAX_NUM_INDICES)
numIndices = B3_MAX_NUM_INDICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
for (i = 0; i<numIndices; i++)
{
indexUpload[i] = indices[i];
}
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = numNormals;
for (i = 0; i < numNormals; i++)
{
normalUpload[i * 3 + 0] = normals[i * 3 + 0];
normalUpload[i * 3 + 1] = normals[i * 3 + 1];
normalUpload[i * 3 + 2] = normals[i * 3 + 2];
}
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = numUVs;
for (i = 0; i < numUVs; i++)
{
uvUpload[i * 2 + 0] = uvs[i * 2 + 0];
uvUpload[i * 2 + 1] = uvs[i * 2 + 1];
}
command->m_createUserShapeArgs.m_numUserShapes++;
cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
delete[] data;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/])
{
return b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);