diff options
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.cpp | 22 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.h | 1 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientSharedMemory.cpp | 6 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsDirect.cpp | 8 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 44 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.h | 1 | ||||
-rw-r--r-- | examples/SharedMemory/SharedMemoryCommands.h | 9 | ||||
-rw-r--r-- | examples/SharedMemory/SharedMemoryPublic.h | 3 | ||||
-rw-r--r-- | examples/TinyRenderer/TinyRenderer.cpp | 2 | ||||
-rw-r--r-- | examples/TinyRenderer/main.cpp | 18 | ||||
-rw-r--r-- | examples/pybullet/pybullet.c | 56 |
11 files changed, 165 insertions, 5 deletions
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e4bdb6fc5..a8edca29a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1484,6 +1484,28 @@ B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle co return -1; } +B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int numVertices, const double* vertices) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_RESET_MESH_DATA; + command->m_updateFlags = 0; + command->m_resetMeshDataArgs.m_numVertices = numVertices; + command->m_resetMeshDataArgs.m_bodyUniqueId = bodyUniqueId; + command->m_resetMeshDataArgs.m_flags = 0; + int totalUploadSizeInBytes = numVertices * sizeof(double) *3; + cl->uploadBulletFileToSharedMemory((const char*)vertices, totalUploadSizeInBytes); + return (b3SharedMemoryCommandHandle)command; + } + return 0; +} + + B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d7a9eadd2..0823b5f65 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -534,6 +534,7 @@ extern "C" B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData); + B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int num_vertices, const double* vertices); B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 456a1c4be..3512fe3ef 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1521,12 +1521,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() b3Warning("Removing user data failed"); break; } + case CMD_RESET_MESH_DATA_FAILED: + { + b3Warning("resetMeshData failed"); + break; + } case CMD_REQUEST_USER_DATA_COMPLETED: case CMD_SYNC_USER_DATA_COMPLETED: case CMD_REMOVE_USER_DATA_COMPLETED: case CMD_ADD_USER_DATA_COMPLETED: case CMD_REMOVE_STATE_FAILED: case CMD_REMOVE_STATE_COMPLETED: + case CMD_RESET_MESH_DATA_COMPLETED: { break; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 69490ed77..ef72e7e19 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1306,6 +1306,14 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } break; } + case CMD_RESET_MESH_DATA_COMPLETED: + { + break; + } + case CMD_RESET_MESH_DATA_FAILED: + { + break; + } case CMD_REMOVE_STATE_FAILED: { break; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 824bee9e5..bd12afb51 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5485,6 +5485,44 @@ static void gatherVertices(const btTransform& trans, const btCollisionShape* col } } } + +bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_MESH_DATA"); + serverStatusOut.m_type = CMD_RESET_MESH_DATA_FAILED; + int sizeInBytes = 0; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId); + if (bodyHandle) + { + int totalBytesPerVertex = sizeof(btVector3); + double* vertexUpload = (double*)bufferServerToClient; + +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + if (bodyHandle->m_softBody) + { + btSoftBody* psb = bodyHandle->m_softBody; + + int numVertices = psb->m_nodes.size(); + if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices) + { + for (int i = 0; i < numVertices; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); + } + serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED; + } + } +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + } + serverStatusOut.m_numDataStreamBytes = 0; + + return hasStatus; +} + bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -14345,6 +14383,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } + case CMD_RESET_MESH_DATA: + { + hasStatus = processResetMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_MULTI_BODY: { hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index e90ddb064..ad6c403aa 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -32,6 +32,7 @@ protected: bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 6347a44d7..75d7ab9a7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1141,6 +1141,13 @@ struct b3RequestMeshDataArgs int m_flags; }; +struct b3ResetMeshDataArgs +{ + int m_bodyUniqueId; + int m_numVertices; + int m_flags; +}; + struct b3SendMeshDataArgs { int m_numVerticesCopied; @@ -1209,6 +1216,8 @@ struct SharedMemoryCommand struct UserDataRequestArgs m_removeUserDataRequestArgs; struct b3CollisionFilterArgs m_collisionFilterArgs; struct b3RequestMeshDataArgs m_requestMeshDataArgs; + struct b3ResetMeshDataArgs m_resetMeshDataArgs; + }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9ea8c1694..2649fc44d 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -117,6 +117,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_MESH_DATA, CMD_PERFORM_COLLISION_DETECTION, + CMD_RESET_MESH_DATA, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, }; @@ -241,6 +242,8 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_MESH_DATA_FAILED, CMD_PERFORM_COLLISION_DETECTION_COMPLETED, + CMD_RESET_MESH_DATA_COMPLETED, + CMD_RESET_MESH_DATA_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 4ea51ebec..4340ce822 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -213,6 +213,7 @@ TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedOb m_lightAmbientCoeff = 0.6; m_lightDiffuseCoeff = 0.35; m_lightSpecularCoeff = 0.05; + } TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedObjectArray<float>& depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex, int linkIndex) @@ -254,6 +255,7 @@ TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer, b3AlignedOb Vec3f center(0, 0, 0); Vec3f up(0, 0, 1); m_lightDirWorld.setValue(0, 0, 0); + m_lightDistance = 10; m_lightColor.setValue(1, 1, 1); m_localScaling.setValue(1, 1, 1); m_modelMatrix = Matrix::identity(); diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp index fed8c39ca..0cd2e3cb6 100644 --- a/examples/TinyRenderer/main.cpp +++ b/examples/TinyRenderer/main.cpp @@ -56,7 +56,8 @@ void MyKeyboardCallback(int keycode, int state) sOldKeyboardCB(keycode, state); } #include "TinyRenderer.h" -float color2[4] = { 1,0,0,1 }; +#include "our_gl.h" + int main(int argc, char* argv[]) { @@ -108,10 +109,11 @@ int main(int argc, char* argv[]) b3Vector3 pos = b3MakeVector3(0, 0, 0); b3Quaternion orn(0, 0, 0, 1); + float color[4] = {1,1,1,1}; b3Vector3 scaling = b3MakeVector3(1, 1, 1); - //app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling); - //app->m_renderer->writeTransforms(); + app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling); + app->m_renderer->writeTransforms(); do { @@ -160,6 +162,15 @@ int main(int argc, char* argv[]) tr.setOrigin(org); tr.getOpenGLMatrix(modelMat); + TinyRender::Vec3f eye(1,1,3); + TinyRender::Vec3f center(0,0,0); + TinyRender::Vec3f up(0,1,0); + + renderData.m_viewMatrix = TinyRender::lookat(eye, center, up); + renderData.m_viewportMatrix = TinyRender::viewport(gWidth/8, gHeight/8, gWidth*3/4, gHeight*3/4); + renderData.m_projectionMatrix = TinyRender::projection(-1.f/(eye-center).norm()); + + for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) @@ -170,6 +181,7 @@ int main(int argc, char* argv[]) } //render the object + float color2[4] = { 1,1,1,1 }; renderData.m_model->setColorRGBA(color2); TinyRenderer::renderObject(renderData); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b26034e3d..f3ee74452 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1,4 +1,4 @@ -//#include "D:/dev/visual leak detector/include/vld.h" +//#include "D:/dev/VisualLeakDetector/include/vld.h" #include "../SharedMemory/PhysicsClientC_API.h" #include "../SharedMemory/PhysicsDirectC_API.h" @@ -9116,6 +9116,55 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject* return NULL; } + +static PyObject* pybullet_resetMeshData(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId = -1; + b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + struct b3MeshData meshData; + int statusType; + PyObject* verticesObj = 0; + int physicsClientId = 0; + int numVertices = 0; + + static char* kwlist[] = { "bodyUniqueId", "vertices", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &verticesObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES); + if (numVertices) + { + double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0; + numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES); + + command = b3ResetMeshDataCommandInit(sm, bodyUniqueId, numVertices, vertices); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + + free(vertices); + + if (statusType == CMD_RESET_MESH_DATA_COMPLETED) + { + Py_INCREF(Py_None); + return Py_None; + } + } + + PyErr_SetString(SpamError, "resetMeshData failed"); + return NULL; +} + static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; @@ -12522,9 +12571,12 @@ static PyMethodDef SpamMethods[] = { {"removeCollisionShape", (PyCFunction)pybullet_removeCollisionShape, METH_VARARGS | METH_KEYWORDS, "Remove a collision shape. Only useful when the collision shape is not used in a body (to perform a getClosestPoint query)."}, - {"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS, + {"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS, "Get mesh data. Returns vertices etc from the mesh."}, + {"resetMeshData", (PyCFunction)pybullet_resetMeshData, METH_VARARGS | METH_KEYWORDS, + "Reset mesh data. Only implemented for deformable bodies."}, + {"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS, "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, |