From fa1d1fc8eeb80d8887faea3cb3c83d9abe3c1935 Mon Sep 17 00:00:00 2001 From: Johannes Brust Date: Tue, 28 Sep 2021 22:39:31 +0200 Subject: Add deformable contact forces info - add contact information for collisions between rigid and soft bodies - collisions between different soft bodies are not supported - uses impulse acting on tetrahedral nodes for calculation of forces - contact points are approximated by node positions - multiple forces acting on identical nodes are accumulated (cherry picked from commit 5795bd676f285c5e830218d73181dbf098a03cb4) --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 232 +++++++++++++-------- .../btDeformableContactConstraint.cpp | 5 +- src/BulletSoftBody/btSoftBody.h | 10 +- 3 files changed, 152 insertions(+), 95 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8275451ed..59fd491d3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2802,7 +2802,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags) m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2; //need to check if there are artifacts with frictionERP m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; - if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD) + if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD) { m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128; } @@ -5677,7 +5677,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S separateRenderMesh = (psb->m_renderNodes.size() != 0); } bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY; - + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; @@ -8187,7 +8187,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc } bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndexA, int& linkIndexB, int& objectIndexA, int& objectIndexB, bool& swap){ - + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) { if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) @@ -8232,7 +8232,7 @@ bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndex { return true; } - + return false; } @@ -8313,115 +8313,167 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c { return false; } - int numSoftbodyContact = 0; - for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) - { - numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size(); - } - int num_contact_points = m_data->m_cachedContactPoints.size(); - m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact); for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) { btSoftBody* psb = deformWorld->getSoftBodyArray()[i]; + btAlignedObjectArray distinctContactPoints; + btAlignedObjectArray nodesInContact; for (int c = 0; c < psb->m_faceRigidContacts.size(); c++) { const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c]; - //convert rigidbody contact - int linkIndexA = -1; - int linkIndexB = -1; - int objectIndexA = psb->getUserIndex2(); - - int objectIndexB = -1; - const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj); - if (bodyB) - { - objectIndexB = bodyB->getUserIndex2(); - } - const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj); - if (mblB && mblB->m_multiBody) - { - linkIndexB = mblB->m_link; - objectIndexB = mblB->m_multiBody->getUserIndex2(); - } + // calculate normal and tangent impulse + btVector3 impulse = contact->m_cti.m_impulse; + btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal; + btVector3 impulseTangent = impulse - impulseNormal; + // get node in contact + int contactNodeIdx = contact->m_bary.maxAxis(); + btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx]; + // check if node is already in the list + int idx = nodesInContact.findLinearSearch2(node); + if (idx < 0) + { + // add new node and contact point + nodesInContact.push_back(node); + //convert rigidbody contact + int linkIndexA = -1; + int linkIndexB = -1; + int objectIndexA = psb->getUserIndex2(); - //apply the filter, if the user provides it - bool swap = false; - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) + int objectIndexB = -1; + const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj); + if (bodyB) { - swap = false; + objectIndexB = bodyB->getUserIndex2(); } - else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj); + if (mblB && mblB->m_multiBody) { - swap = true; + linkIndexB = mblB->m_link; + objectIndexB = mblB->m_multiBody->getUserIndex2(); } - else + + //apply the filter, if the user provides it + bool swap = false; + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) { - continue; + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) + { + swap = false; + } + else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) + { + swap = true; + } + else + { + continue; + } } - } - if (swap) - { - std::swap(objectIndexA, objectIndexB); - std::swap(linkIndexA, linkIndexB); - } + if (swap) + { + std::swap(objectIndexA, objectIndexB); + std::swap(linkIndexA, linkIndexB); + } - //apply the second object filter, if the user provides it - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) + //apply the second object filter, if the user provides it + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) { - continue; + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) + { + continue; + } } - } - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) - { - continue; - } + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + { + continue; + } - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) - { - continue; - } - b3ContactPointData pt; - pt.m_bodyUniqueIdA = objectIndexA; - pt.m_bodyUniqueIdB = objectIndexB; - pt.m_contactDistance = contact->m_cti.m_offset; - pt.m_contactFlags = 0; - pt.m_linkIndexA = linkIndexA; - pt.m_linkIndexB = linkIndexB; - for (int j = 0; j < 3; j++) - { - if (swap) + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) { - pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j]; - pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j]; - pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j]; + continue; } - else + b3ContactPointData pt; + pt.m_bodyUniqueIdA = objectIndexA; + pt.m_bodyUniqueIdB = objectIndexB; + pt.m_contactDistance = -contact->m_cti.m_offset; + pt.m_contactFlags = 0; + pt.m_linkIndexA = linkIndexA; + pt.m_linkIndexB = linkIndexB; + for (int j = 0; j < 3; j++) { - pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j]; - pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j]; - pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j]; + if (swap) + { + pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j]; + pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912 + // node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node) + pt.m_positionOnBInWS[j] = node->m_x[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j]; + // node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node) + pt.m_positionOnAInWS[j] = node->m_x[j]; + pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912 + } + } + pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm(); + pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm(); + pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm(); + for (int j = 0; j < 3; j++) + { + pt.m_linearFrictionDirection1[j] = contact->t1[j]; + pt.m_linearFrictionDirection2[j] = contact->t2[j]; } + distinctContactPoints.push_back(pt); } - pt.m_normalForce = 1; - pt.m_linearFrictionForce1 = 0; - pt.m_linearFrictionForce2 = 0; - for (int j = 0; j < 3; j++) + else { - pt.m_linearFrictionDirection1[j] = 0; - pt.m_linearFrictionDirection2[j] = 0; + // add values to existing contact point + b3ContactPointData* pt = &distinctContactPoints[idx]; + // current normal force of node + btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]), + btScalar(pt->m_contactNormalOnBInWS[1]), + btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce; + // add normal force of additional node contact + normalForce += contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm(); + // get magnitude of normal force + pt->m_normalForce = normalForce.norm(); + // get direction of normal force + if (!normalForce.fuzzyZero()) + { + // normalize for unit vectors if above numerical threshold + normalForce.normalize(); + for (int j = 0; j < 3; j++) + { + pt->m_contactNormalOnBInWS[j] = normalForce[j]; + } + } + + // add magnitudes of tangential forces in existing directions + btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]), + btScalar(pt->m_linearFrictionDirection1[1]), + btScalar(pt->m_linearFrictionDirection1[2])); + btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]), + btScalar(pt->m_linearFrictionDirection2[1]), + btScalar(pt->m_linearFrictionDirection2[2])); + pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm(); + pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm(); } - m_data->m_cachedContactPoints.push_back(pt); + } + + int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size(); + m_data->m_cachedContactPoints.reserve(num_contact_points); + // add points to contact points cache + for (int p = 0; p < distinctContactPoints.size(); p++) + { + m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]); } } #endif @@ -9929,7 +9981,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe softWorld->addSoftBody(rsb); } } - + *bodyUniqueId = m_data->m_bodyHandles.allocHandle(); InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId); bodyHandle->m_softBody = rsb; @@ -9980,7 +10032,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe float scaling[4] = { 1,1,1,1 }; int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling); rsb->setUserIndex(instanceUid); - + if (m_data->m_enableTinyRenderer) { int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight); @@ -10080,7 +10132,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe } } } - + btAlignedObjectArray vertices; @@ -10093,7 +10145,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe for (int i = 0; i < rsb->m_faces.size(); i++) // Foreach face { - + for (int k = 0; k < 3; k++) // Foreach vertex on a face { int currentIndex = i * 3 + k; diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 09398d79a..9c8e72f50 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv { dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; } - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + // dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); if (!infoGlobal.m_splitImpulse) { @@ -487,6 +487,9 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul btVector3 dv = impulse * contact->m_c2; btSoftBody::Face* face = contact->m_face; + // save applied impulse + contact->m_cti.m_impulse = impulse; + btVector3& v0 = face->m_n[0]->m_v; btVector3& v1 = face->m_n[1]->m_v; btVector3& v2 = face->m_n[2]->m_v; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index f91640acb..27705e600 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -223,10 +223,12 @@ public: /* sCti is Softbody contact info */ struct sCti { - const btCollisionObject* m_colObj; /* Rigid body */ - btVector3 m_normal; /* Outward normal */ - btScalar m_offset; /* Offset from origin */ + const btCollisionObject* m_colObj; /* Rigid body */ + btVector3 m_normal; /* Outward normal */ + mutable btVector3 m_impulse; /* Applied impulse */ + btScalar m_offset; /* Offset from origin */ btVector3 m_bary; /* Barycentric weights for faces */ + sCti() : m_impulse(0, 0, 0) {} }; /* sMedium */ @@ -897,7 +899,7 @@ public: int node1) const; bool checkLink(const Node* node0, const Node* node1) const; - /* Check for existring face */ + /* Check for existing face */ bool checkFace(int node0, int node1, int node2) const; -- cgit v1.2.1 From 60512c7872eb42a460920c8823f7692e00530e90 Mon Sep 17 00:00:00 2001 From: Johannes Brust Date: Thu, 21 Oct 2021 17:38:13 +0200 Subject: Fix bug due to missing swap (cherry picked from commit 61d5bdb783d8673eabedd10f660cb222b6a286fd) --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 113 +++++++++++---------- 1 file changed, 57 insertions(+), 56 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 59fd491d3..376e833a6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8331,74 +8331,74 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx]; // check if node is already in the list int idx = nodesInContact.findLinearSearch2(node); - if (idx < 0) - { - // add new node and contact point - nodesInContact.push_back(node); - //convert rigidbody contact - int linkIndexA = -1; - int linkIndexB = -1; - int objectIndexA = psb->getUserIndex2(); - int objectIndexB = -1; - const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj); - if (bodyB) - { - objectIndexB = bodyB->getUserIndex2(); - } - const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj); - if (mblB && mblB->m_multiBody) - { - linkIndexB = mblB->m_link; - objectIndexB = mblB->m_multiBody->getUserIndex2(); - } - - //apply the filter, if the user provides it - bool swap = false; - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) - { - swap = false; - } - else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) - { - swap = true; - } - else - { - continue; - } - } + //apply the filter, if the user provides it + int linkIndexA = -1; + int linkIndexB = -1; + int objectIndexA = psb->getUserIndex2(); - if (swap) + int objectIndexB = -1; + const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj); + if (mblB && mblB->m_multiBody) + { + linkIndexB = mblB->m_link; + objectIndexB = mblB->m_multiBody->getUserIndex2(); + } + bool swap = false; + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) { - std::swap(objectIndexA, objectIndexB); - std::swap(linkIndexA, linkIndexB); + swap = false; } - - //apply the second object filter, if the user provides it - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) + else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) { - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) - { - continue; - } + swap = true; } - - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + else { continue; } + } - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) + if (swap) + { + std::swap(objectIndexA, objectIndexB); + std::swap(linkIndexA, linkIndexB); + } + + //apply the second object filter, if the user provides it + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) { continue; } + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + { + continue; + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) + { + continue; + } + + if (idx < 0) + { + // add new node and contact point + nodesInContact.push_back(node); b3ContactPointData pt; pt.m_bodyUniqueIdA = objectIndexA; pt.m_bodyUniqueIdB = objectIndexB; @@ -8442,7 +8442,8 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c btScalar(pt->m_contactNormalOnBInWS[1]), btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce; // add normal force of additional node contact - normalForce += contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm(); + btScalar swapFactor = swap ? -1.0 : 1.0; + normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm(); // get magnitude of normal force pt->m_normalForce = normalForce.norm(); // get direction of normal force -- cgit v1.2.1 From 9dd446609a7a9af8b5a586a67ab57019edbc2328 Mon Sep 17 00:00:00 2001 From: Maarten Behn Date: Mon, 19 Dec 2022 15:31:21 +0100 Subject: Adding getTetraMeshData() --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 53 ++++++++++++++++++++++ .../SharedMemory/PhysicsServerCommandProcessor.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 5 ++ 3 files changed, 59 insertions(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 376e833a6..081b6e204 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5718,6 +5718,54 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S return hasStatus; } +bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_MESH_DATA"); + serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED; + serverStatusOut.m_numDataStreamBytes = 0; + int sizeInBytes = 0; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId); + if (bodyHandle) + { + int totalBytesPerTetra = sizeof(btVector3) * 4; + btVector3* verticesOut = (btVector3*)bufferServerToClient; + const btCollisionShape* colShape = 0; + +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + if (bodyHandle->m_softBody) + { + btSoftBody* psb = bodyHandle->m_softBody; + + int numTetra = psb->m_tetras.size(); + int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerTetra - 1; + int numVerticesRemaining = numTetra * 4 - clientCmd.m_requestMeshDataArgs.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()); + } + sizeInBytes = verticesCopied * sizeof(btVector3); + serverStatusOut.m_type = CMD_REQUEST_TETRA_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; + } +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + } + + serverStatusOut.m_numDataStreamBytes = sizeInBytes; + + return hasStatus; +} + bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -15120,6 +15168,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } + case CMD_REQUEST_TETRA_MESH_DATA: + { + hasStatus = processRequestTetraMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_RESET_MESH_DATA: { hasStatus = processResetMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index fb23a1bb9..4035d7d0c 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 processRequestTetraMeshDataCommand(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); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7541ddb5a..c209d5bf4 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -118,6 +118,8 @@ enum EnumSharedMemoryClientCommand CMD_PERFORM_COLLISION_DETECTION, CMD_RESET_MESH_DATA, + + CMD_REQUEST_TETRA_MESH_DATA, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, }; @@ -244,6 +246,9 @@ enum EnumSharedMemoryServerStatus CMD_PERFORM_COLLISION_DETECTION_COMPLETED, CMD_RESET_MESH_DATA_COMPLETED, CMD_RESET_MESH_DATA_FAILED, + + CMD_REQUEST_TETRA_MESH_DATA_COMPLETED, + CMD_REQUEST_TETRA_MESH_DATA_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; -- cgit v1.2.1 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 From 54cdf3993325273ec976f0308a00d70b0d06536b Mon Sep 17 00:00:00 2001 From: Maarten Behn <46872913+MaartenBehn@users.noreply.github.com> Date: Thu, 22 Dec 2022 19:27:16 +0100 Subject: extending wrapper --- examples/SharedMemory/PhysicsClient.h | 2 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 21 ++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../SharedMemory/PhysicsClientSharedMemory.cpp | 28 ++++++++++++++++++++++ examples/SharedMemory/PhysicsClientSharedMemory.h | 2 ++ examples/SharedMemory/PhysicsDirect.cpp | 12 ++++++++++ examples/SharedMemory/PhysicsDirect.h | 2 ++ examples/SharedMemory/PhysicsLoopBack.cpp | 5 ++++ examples/SharedMemory/PhysicsLoopBack.h | 2 ++ .../SharedMemory/PhysicsServerCommandProcessor.cpp | 7 ++---- examples/SharedMemory/SharedMemoryPublic.h | 5 ++++ examples/pybullet/pybullet.c | 11 +++++---- 12 files changed, 90 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 33c139ef1..8fc863f27 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -67,6 +67,8 @@ public: virtual void getCachedMeshData(struct b3MeshData* meshData) = 0; + virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData) = 0; + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0; virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f464c5b70..71bdd5c54 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1567,6 +1567,18 @@ B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHand } } +B3_SHARED_API void b3GetTetraMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_TETRA_MESH_DATA); + if (command->m_type == CMD_REQUEST_TETRA_MESH_DATA) + { + command->m_updateFlags = B3_TETRA_MESH_DATA_FLAGS; + command->m_requestMeshDataArgs.m_flags = flags; + } +} + B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -1592,6 +1604,15 @@ B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3Mesh } } +B3_SHARED_API void b3GetTetraMeshData(b3PhysicsClientHandle physClient, struct b3TetraMeshData* meshData) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + cl->getCachedTetraMeshData(meshData); + } +} + B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius) { return b3CreateCollisionShapeAddSphere(commandHandle, radius); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4313d24ec..563272942 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -538,8 +538,10 @@ extern "C" B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle); B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex); B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); + B3_SHARED_API void b3GetTetraMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData); + B3_SHARED_API void b3GetTetraMeshData(b3PhysicsClientHandle physClient, struct b3TetraMeshData* meshData); B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int num_vertices, const double* vertices); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 3512fe3ef..1d8d966a7 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -54,6 +54,7 @@ struct PhysicsClientSharedMemoryInternalData btAlignedObjectArray m_cachedCollisionShapes; b3MeshData m_cachedMeshData; + b3TetraMeshData m_cachedTetraMeshData; btAlignedObjectArray m_cachedVertexPositions; btAlignedObjectArray m_cachedVREvents; @@ -1074,6 +1075,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() b3Warning("Request mesh data failed"); break; } + case CMD_REQUEST_TETRA_MESH_DATA_COMPLETED: + { + m_data->m_cachedVertexPositions.resize(serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied); + btVector3* verticesReceived = (btVector3*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + for (int i = 0; i < serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; i++) + { + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].x = verticesReceived[i].x(); + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].y = verticesReceived[i].y(); + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].z = verticesReceived[i].z(); + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].w = verticesReceived[i].w(); + } + break; + } + case CMD_REQUEST_TETRA_MESH_DATA_FAILED: + { + b3Warning("Request tetra mesh data failed"); + break; + } case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED: { break; @@ -2088,6 +2107,15 @@ void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData) *meshData = m_data->m_cachedMeshData; } +void PhysicsClientSharedMemory::getCachedTetraMeshData(struct b3TetraMeshData* meshData) +{ + m_data->m_cachedTetraMeshData.m_numVertices = m_data->m_cachedVertexPositions.size(); + + m_data->m_cachedTetraMeshData.m_vertices = m_data->m_cachedTetraMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0; + + *meshData = m_data->m_cachedTetraMeshData; +} + const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { if (m_data->m_debugLinesFrom.size()) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index f1a67c525..cf9113c6d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -80,6 +80,8 @@ public: virtual void getCachedMeshData(struct b3MeshData* meshData); + virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData); + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index a8aa72a60..f12057ae0 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -67,6 +67,7 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedCollisionShapes; b3MeshData m_cachedMeshData; + b3TetraMeshData m_cachedTetraMeshData; btAlignedObjectArray m_cachedVertexPositions; btAlignedObjectArray m_cachedVREvents; @@ -99,6 +100,7 @@ struct PhysicsDirectInternalData m_timeOutInSeconds(1e30) { memset(&m_cachedMeshData.m_numVertices, 0, sizeof(b3MeshData)); + memset(&m_cachedTetraMeshData.m_numVertices, 0, sizeof(b3TetraMeshData)); memset(&m_command, 0, sizeof(m_command)); memset(&m_serverStatus, 0, sizeof(m_serverStatus)); memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); @@ -1691,6 +1693,16 @@ void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData) *meshData = m_data->m_cachedMeshData; } +void PhysicsDirect::getCachedTetraMeshData(struct b3TetraMeshData* meshData) +{ + m_data->m_cachedTetraMeshData.m_numVertices = m_data->m_cachedVertexPositions.size(); + + m_data->m_cachedTetraMeshData.m_vertices = m_data->m_cachedTetraMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0; + + *meshData = m_data->m_cachedTetraMeshData; +} + + void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 0d33f8119..441b58e74 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -106,6 +106,8 @@ public: virtual void getCachedMeshData(struct b3MeshData* meshData); + virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData); + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 58e4d59c7..427da31e8 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -183,6 +183,11 @@ void PhysicsLoopBack::getCachedMeshData(struct b3MeshData* meshData) return m_data->m_physicsClient->getCachedMeshData(meshData); } +void PhysicsLoopBack::getCachedTetraMeshData(struct b3TetraMeshData* meshData) +{ + return m_data->m_physicsClient->getCachedTetraMeshData(meshData); +} + void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData); diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 4626aac81..2b6eb87f2 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -78,6 +78,8 @@ public: virtual void getCachedMeshData(struct b3MeshData* meshData); + virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData); + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2f788bf2c..202ed6c9b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5721,8 +5721,8 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; - BT_PROFILE("CMD_REQUEST_MESH_DATA"); - serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED; + BT_PROFILE("CMD_REQUEST_TETRA_MESH_DATA"); + serverStatusOut.m_type = CMD_REQUEST_TETRA_MESH_DATA_FAILED; serverStatusOut.m_numDataStreamBytes = 0; int sizeInBytes = 0; @@ -5733,8 +5733,6 @@ bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const str btVector3* verticesOut = (btVector3*)bufferServerToClient; const btCollisionShape* colShape = 0; -#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - if (bodyHandle->m_softBody) { btSoftBody* psb = bodyHandle->m_softBody; @@ -5758,7 +5756,6 @@ bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const str serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex; serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied; } -#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD } serverStatusOut.m_numDataStreamBytes = sizeInBytes; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9b7598e0b..4e3f6d3f3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -486,6 +486,11 @@ struct b3MeshData struct b3MeshVertex* m_vertices; }; +enum eTetraMeshDataEnum +{ + B3_TETRA_MESH_DATA_FLAGS=2, +}; + struct b3TetraMeshData { int m_numVertices; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 4d62b5f8e..242ed98dd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9196,7 +9196,7 @@ static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObj b3PhysicsClientHandle sm = 0; b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; - struct b3MeshData meshData; + struct b3TetraMeshData meshData; int statusType; int flags = -1; @@ -9215,7 +9215,7 @@ static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObj command = b3GetTetraMeshDataCommandInit(sm, bodyUniqueId); if (flags >= 0) { - b3GetMeshDataSetFlags(command, flags); + b3GetTetraMeshDataSetFlags(command, flags); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -9225,7 +9225,7 @@ static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObj int i; PyObject* pyVertexData; PyObject* pyListMeshData = PyTuple_New(2); - b3GetMeshData(sm, &meshData); + b3GetTetraMeshData(sm, &meshData); PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices)); pyVertexData = PyTuple_New(meshData.m_numVertices); PyTuple_SetItem(pyListMeshData, 1, pyVertexData); @@ -9242,7 +9242,7 @@ static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObj return pyListMeshData; } - PyErr_SetString(SpamError, "getMeshData failed"); + PyErr_SetString(SpamError, "getTetraMeshData failed"); return NULL; } @@ -12702,6 +12702,9 @@ static PyMethodDef SpamMethods[] = { {"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS, "Get mesh data. Returns vertices etc from the mesh."}, + {"getTetraMeshData", (PyCFunction)pybullet_getTetraMeshData, METH_VARARGS | METH_KEYWORDS, + "Get mesh data. Returns tetra from the mesh."}, + {"resetMeshData", (PyCFunction)pybullet_resetMeshData, METH_VARARGS | METH_KEYWORDS, "Reset mesh data. Only implemented for deformable bodies."}, -- cgit v1.2.1 From 5c8dfe4b21e3427730a34697b1da11779a15a2d8 Mon Sep 17 00:00:00 2001 From: Maarten Behn Date: Tue, 27 Dec 2022 19:46:22 +0100 Subject: got the data tunnelt with getMeshData --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 28 ++++++++++++++++++---- examples/pybullet/pybullet.c | 2 +- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 202ed6c9b..636203f60 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5678,6 +5678,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S } bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY; + + + int numTetra = psb->m_tetras.size(); + int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerVertex - 1; + int numVerticesRemaining = numTetra * 4; + 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()); + } + + /* int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; @@ -5704,6 +5720,8 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S } } } + */ + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; @@ -5729,7 +5747,7 @@ bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const str InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_resetTetraMeshDataArgs.m_bodyUniqueId); if (bodyHandle) { - int totalBytesPerTetra = sizeof(btVector3) * 4; + int totalBytesPerVertex = sizeof(btVector3); btVector3* verticesOut = (btVector3*)bufferServerToClient; const btCollisionShape* colShape = 0; @@ -5738,18 +5756,18 @@ bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const str btSoftBody* psb = bodyHandle->m_softBody; int numTetra = psb->m_tetras.size(); - int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerTetra - 1; - int numVerticesRemaining = numTetra * 4 - clientCmd.m_resetTetraMeshDataArgs.m_startingVertex; + int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerVertex - 1; + int numVerticesRemaining = numTetra * 4; int verticesCopied = btMin(maxNumnumVertecies, numVerticesRemaining); for (int i = 0; i < verticesCopied; i += 4) { - const btSoftBody::Tetra& n = psb->m_tetras[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()); } + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_TETRA_MESH_DATA_COMPLETED; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 242ed98dd..5a8c2eb03 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9202,7 +9202,7 @@ static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObj int physicsClientId = 0; static char* kwlist[] = {"bodyUniqueId", "flags", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|iiii", kwlist, &bodyUniqueId, &flags , &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|iii", kwlist, &bodyUniqueId, &flags , &physicsClientId)) { return NULL; } -- cgit v1.2.1 From e323404ac393aeb834a3f8f21549e119c76b67ff Mon Sep 17 00:00:00 2001 From: Maarten Behn Date: Tue, 27 Dec 2022 20:28:50 +0100 Subject: ok it's working I guess --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 19 ++----------------- 1 file changed, 2 insertions(+), 17 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 636203f60..1d691c550 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5678,22 +5678,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S } bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY; - - - int numTetra = psb->m_tetras.size(); - int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerVertex - 1; - int numVerticesRemaining = numTetra * 4; - 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()); - } - - /* + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; @@ -5720,7 +5705,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S } } } - */ + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; -- cgit v1.2.1