From 2b4757f51298d52b04eb052f19cc0e9c5f9b2e57 Mon Sep 17 00:00:00 2001 From: Maarten Behn <46872913+MaartenBehn@users.noreply.github.com> Date: Wed, 21 Dec 2022 14:44:35 +0100 Subject: extend wrapper --- examples/SharedMemory/PhysicsClientC_API.cpp | 18 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsServerCommandProcessor.cpp | 12 ++--- examples/SharedMemory/SharedMemoryCommands.h | 10 +++- examples/SharedMemory/SharedMemoryPublic.h | 6 +++ examples/pybullet/pybullet.c | 58 ++++++++++++++++++++++ 6 files changed, 98 insertions(+), 7 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ae6015da4..f464c5b70 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1525,6 +1525,24 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClie return 0; } +B3_SHARED_API b3SharedMemoryCommandHandle b3GetTetraMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_REQUEST_TETRA_MESH_DATA; + command->m_updateFlags = 0; + command->m_resetTetraMeshDataArgs.m_startingVertex = 0; + command->m_resetTetraMeshDataArgs.m_bodyUniqueId = bodyUniqueId; + return (b3SharedMemoryCommandHandle)command; + } + return 0; +} + B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e73930b58..4313d24ec 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -533,6 +533,7 @@ 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 b3SharedMemoryCommandHandle b3GetTetraMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle); B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle); B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 081b6e204..2f788bf2c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5726,7 +5726,7 @@ bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const str serverStatusOut.m_numDataStreamBytes = 0; int sizeInBytes = 0; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_resetTetraMeshDataArgs.m_bodyUniqueId); if (bodyHandle) { int totalBytesPerTetra = sizeof(btVector3) * 4; @@ -5741,16 +5741,16 @@ bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const str int numTetra = psb->m_tetras.size(); int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerTetra - 1; - int numVerticesRemaining = numTetra * 4 - clientCmd.m_requestMeshDataArgs.m_startingVertex; + int numVerticesRemaining = numTetra * 4 - clientCmd.m_resetTetraMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumnumVertecies, numVerticesRemaining); for (int i = 0; i < verticesCopied; i += 4) { const btSoftBody::Tetra& n = psb->m_tetras[i / 4]; - verticesOut[i].setValue(n.m_n[0].m_x.x(), n.m_n[0].m_x.y(), n.m_n[0].m_x.z()); - verticesOut[i+1].setValue(n.m_n[1].m_x.x(), n.m_n[1].m_x.y(), n.m_n[1].m_x.z()); - verticesOut[i+2].setValue(n.m_n[2].m_x.x(), n.m_n[2].m_x.y(), n.m_n[2].m_x.z()); - verticesOut[i+3].setValue(n.m_n[3].m_x.x(), n.m_n[3].m_x.y(), n.m_n[3].m_x.z()); + verticesOut[i].setValue(n.m_n[0]->m_x.x(), n.m_n[0]->m_x.y(), n.m_n[0]->m_x.z()); + verticesOut[i+1].setValue(n.m_n[1]->m_x.x(), n.m_n[1]->m_x.y(), n.m_n[1]->m_x.z()); + verticesOut[i+2].setValue(n.m_n[2]->m_x.x(), n.m_n[2]->m_x.y(), n.m_n[2]->m_x.z()); + verticesOut[i+3].setValue(n.m_n[3]->m_x.x(), n.m_n[3]->m_x.y(), n.m_n[3]->m_x.z()); } sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_TETRA_MESH_DATA_COMPLETED; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 341c93123..37fc35145 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1144,6 +1144,13 @@ struct b3RequestMeshDataArgs int m_flags; }; +struct b3RequestTetraMeshDataArgs +{ + int m_bodyUniqueId; + int m_startingVertex; + int m_flags; +}; + struct b3ResetMeshDataArgs { int m_bodyUniqueId; @@ -1219,8 +1226,9 @@ struct SharedMemoryCommand struct UserDataRequestArgs m_removeUserDataRequestArgs; struct b3CollisionFilterArgs m_collisionFilterArgs; struct b3RequestMeshDataArgs m_requestMeshDataArgs; + struct b3RequestTetraMeshDataArgs m_requestTetraMeshDataArgs; struct b3ResetMeshDataArgs m_resetMeshDataArgs; - + struct b3RequestTetraMeshDataArgs m_resetTetraMeshDataArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index c209d5bf4..9b7598e0b 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -486,6 +486,12 @@ struct b3MeshData struct b3MeshVertex* m_vertices; }; +struct b3TetraMeshData +{ + int m_numVertices; + struct b3MeshVertex* m_vertices; +}; + struct b3OpenGLVisualizerCameraInfo { int m_width; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f78f464f9..4d62b5f8e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9190,6 +9190,64 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject* } +static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + struct b3MeshData meshData; + int statusType; + int flags = -1; + + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|iiii", kwlist, &bodyUniqueId, &flags , &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + command = b3GetTetraMeshDataCommandInit(sm, bodyUniqueId); + if (flags >= 0) + { + b3GetMeshDataSetFlags(command, flags); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_REQUEST_TETRA_MESH_DATA_COMPLETED) + { + int i; + PyObject* pyVertexData; + PyObject* pyListMeshData = PyTuple_New(2); + b3GetMeshData(sm, &meshData); + PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices)); + pyVertexData = PyTuple_New(meshData.m_numVertices); + PyTuple_SetItem(pyListMeshData, 1, pyVertexData); + + for (i = 0; i < meshData.m_numVertices; i++) + { + PyObject* pyListVertex = PyTuple_New(3); + PyTuple_SetItem(pyListVertex, 0, PyFloat_FromDouble(meshData.m_vertices[i].x)); + PyTuple_SetItem(pyListVertex, 1, PyFloat_FromDouble(meshData.m_vertices[i].y)); + PyTuple_SetItem(pyListVertex, 2, PyFloat_FromDouble(meshData.m_vertices[i].z)); + PyTuple_SetItem(pyVertexData, i, pyListVertex); + } + + return pyListMeshData; + } + + PyErr_SetString(SpamError, "getMeshData failed"); + return NULL; +} + + + static PyObject* pybullet_resetMeshData(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId = -1; -- cgit v1.2.1