Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-10-26 02:25:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

pybullet: enable getMeshData for multibody/rigid body (and filter on collisionShapeIndex for compound shapes)

Browse Source
This commit is contained in:
Erwin Coumans
2020-03-17 00:38:12 -07:00
parent 7991012f9d
commit 1640eef937
6 changed files with 119 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/SharedMemory/PhysicsClientC_API.cpp
+12
View File
@@ -1390,6 +1390,18 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClie
return 0;
}
B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_MESH_DATA);
if (command->m_type == CMD_REQUEST_MESH_DATA)
{
command->m_updateFlags = B3_MESH_DATA_COLLISIONSHAPEINDEX;
command->m_requestMeshDataArgs.m_collisionShapeIndex = shapeIndex;
}
}
B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
examples/SharedMemory/PhysicsClientC_API.h
+2
View File
@@ -510,6 +510,8 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex);
B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData);
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+91 -16
View File
@@ -5168,6 +5168,40 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
return hasStatus;
}
static void gatherVertices(const btTransform& trans, const btCollisionShape* colShape, btAlignedObjectArray<btVector3>& verticesOut, int collisionShapeIndex)
{
switch (colShape->getShapeType())
{
case COMPOUND_SHAPE_PROXYTYPE:
{
const btCompoundShape* compound = (const btCompoundShape*)colShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
btTransform childTr = trans * compound->getChildTransform(i);
if ((collisionShapeIndex < 0) || (collisionShapeIndex == i))
{
gatherVertices(childTr, compound->getChildShape(i), verticesOut, collisionShapeIndex);
}
}
break;
}
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
const btConvexHullShape* convex = (const btConvexHullShape*)colShape;
btVector3 vtx;
for (int i = 0; i < convex->getNumVertices(); i++)
{
convex->getVertex(i, vtx);
btVector3 trVertex = trans * vtx;
verticesOut.push_back(trVertex);
}
break;
}
default:
{
}
}
}
bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -5178,38 +5212,79 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
if (bodyHandle)
{
int totalBytesPerVertex = sizeof(btVector3);
btVector3* verticesOut = (btVector3*)bufferServerToClient;
const btCollisionShape* colShape = 0;
if (bodyHandle->m_multiBody)
{
//todo
//collision shape
if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1)
{
colShape = bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape();
}
else
{
colShape = bodyHandle->m_multiBody->getLinkCollider(clientCmd.m_requestMeshDataArgs.m_linkIndex)->getCollisionShape();
}
}
if (bodyHandle->m_rigidBody)
{
//todo
colShape = bodyHandle->m_rigidBody->getCollisionShape();
}
if (colShape)
{
btAlignedObjectArray<btVector3> vertices;
btTransform tr;
tr.setIdentity();
int collisionShapeIndex = -1;
if (clientCmd.m_updateFlags& B3_MESH_DATA_COLLISIONSHAPEINDEX)
{
collisionShapeIndex = clientCmd.m_requestMeshDataArgs.m_collisionShapeIndex;
}
gatherVertices(tr, colShape, vertices, collisionShapeIndex);
int numVertices = vertices.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
for (int i = 0; i < verticesCopied; ++i)
{
verticesOut[i] = vertices[i];
}
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (bodyHandle->m_softBody)
{
btSoftBody* psb = bodyHandle->m_softBody;
int totalBytesPerVertex = sizeof(btVector3);
bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
btVector3* verticesOut = (btVector3*)bufferServerToClient;
for (int i = 0; i < verticesCopied; ++i)
{
if (separateRenderMesh)
{
const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
else
{
const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
if (separateRenderMesh)
{
const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
else
{
const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
}
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
examples/SharedMemory/SharedMemoryCommands.h
+1
View File
@@ -1097,6 +1097,7 @@ struct b3RequestMeshDataArgs
int m_bodyUniqueId;
int m_linkIndex;
int m_startingVertex;
int m_collisionShapeIndex;
};
struct b3SendMeshDataArgs
examples/SharedMemory/SharedMemoryPublic.h
+6
View File
@@ -449,6 +449,12 @@ struct b3MeshVertex
double x, y, z, w;
};
enum eMeshDataEnum
{
B3_MESH_DATA_COLLISIONSHAPEINDEX=1,
};
struct b3MeshData
{
int m_numVertices;
examples/pybullet/pybullet.c
+7 -2
View File
@@ -8620,6 +8620,7 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
{
int bodyUniqueId = -1;
int linkIndex = -1;
int collisionShapeIndex = -1;
b3PhysicsClientHandle sm = 0;
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
@@ -8627,8 +8628,8 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
int statusType;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "collisionShapeIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|iii", kwlist, &bodyUniqueId, &linkIndex,&collisionShapeIndex, &physicsClientId))
{
return NULL;
}
@@ -8639,6 +8640,10 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
return NULL;
}
command = b3GetMeshDataCommandInit(sm, bodyUniqueId, linkIndex);
if (collisionShapeIndex >= 0)
{
b3GetMeshDataSetCollisionShapeIndex(command, collisionShapeIndex);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_MESH_DATA_COMPLETED)