From 1cb3655f719408605f7ed3694efe3247dff5a916 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 5 Aug 2019 13:38:43 -0700 Subject: [PATCH 1/2] add support to load rigidbody --- .../PhysicsServerCommandProcessor.cpp | 95 ++++++++++++------- src/BulletDynamics/Dynamics/btRigidBody.cpp | 1 + src/BulletDynamics/Dynamics/btRigidBody.h | 6 ++ 3 files changed, 68 insertions(+), 34 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a336fee9a..788f41fca 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7042,48 +7042,62 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc hasStatus = true; } - else if (body && body->m_rigidBody){ - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + else if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - serverCmd.m_sendActualStateArgs.m_numLinks = 0; - serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage); - serverCmd.m_sendActualStateArgs.m_stateDetails = 0; + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0; + serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage); + serverCmd.m_sendActualStateArgs.m_stateDetails = 0; - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = + body->m_rootLocalInertialFrame.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = + body->m_rootLocalInertialFrame.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = + body->m_rootLocalInertialFrame.getOrigin()[2]; - btTransform tr = rb->getWorldTransform(); - //base position in world space, cartesian - stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; - stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; - stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = + body->m_rootLocalInertialFrame.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = + body->m_rootLocalInertialFrame.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = + body->m_rootLocalInertialFrame.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = + body->m_rootLocalInertialFrame.getRotation()[3]; - //base orientation, quaternion x,y,z,w, in world space, cartesian - stateDetails->m_actualStateQ[3] = tr.getRotation()[0]; - stateDetails->m_actualStateQ[4] = tr.getRotation()[1]; - stateDetails->m_actualStateQ[5] = tr.getRotation()[2]; - stateDetails->m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ += 7; //pos + quaternion + btTransform tr = rb->getWorldTransform(); + //base position in world space, cartesian + stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; + stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; + stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; - //base linear velocity (in world space, cartesian) - stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0]; - stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1]; - stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + //base orientation, quaternion x,y,z,w, in world space, cartesian + stateDetails->m_actualStateQ[3] = tr.getRotation()[0]; + stateDetails->m_actualStateQ[4] = tr.getRotation()[1]; + stateDetails->m_actualStateQ[5] = tr.getRotation()[2]; + stateDetails->m_actualStateQ[6] = tr.getRotation()[3]; + int totalDegreeOfFreedomQ = 7; //pos + quaternion - //base angular velocity (in world space, cartesian) - stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0]; - stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1]; - stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2]; - totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF + //base linear velocity (in world space, cartesian) + stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + //base angular velocity (in world space, cartesian) + stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + int totalDegreeOfFreedomU = 6; //3 linear and 3 angular DOF - hasStatus = true; - } + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD else if (body && body->m_softBody) { @@ -8774,6 +8788,19 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S } hasStatus = true; } + else if (body && body->m_rigidBody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; + + btRigidBody* rb = body->m_rigidBody; + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction(); + serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = rb->getRollingFriction(); + serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = rb->getSpinningFriction(); + serverCmd.m_dynamicsInfo.m_angularDamping = rb->getAngularDamping(); + serverCmd.m_dynamicsInfo.m_linearDamping = rb->getLinearDamping(); + serverCmd.m_dynamicsInfo.m_mass = rb->getMass(); + } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD else if (body && body->m_softBody){ //todo: @fuchuyuan implement dynamics info diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index f4bcabada..ada686f99 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -86,6 +86,7 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_debugBodyId = uniqueId++; setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); + m_mass = constructionInfo.m_mass; updateInertiaTensor(); m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 05f270a4b..45c706878 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -62,6 +62,7 @@ class btRigidBody : public btCollisionObject btVector3 m_linearVelocity; btVector3 m_angularVelocity; btScalar m_inverseMass; + btScalar m_mass; btVector3 m_linearFactor; btVector3 m_gravity; @@ -259,6 +260,9 @@ public: m_invMass = m_linearFactor * m_inverseMass; } btScalar getInvMass() const { return m_inverseMass; } + + btScalar getMass() const { return m_mass; } + const btMatrix3x3& getInvInertiaTensorWorld() const { return m_invInertiaTensorWorld; @@ -554,6 +558,7 @@ struct btRigidBodyFloatData btVector3FloatData m_totalForce; btVector3FloatData m_totalTorque; float m_inverseMass; + float m_mass; float m_linearDamping; float m_angularDamping; float m_additionalDampingFactor; @@ -580,6 +585,7 @@ struct btRigidBodyDoubleData btVector3DoubleData m_totalForce; btVector3DoubleData m_totalTorque; double m_inverseMass; + double m_mass; double m_linearDamping; double m_angularDamping; double m_additionalDampingFactor; From 4f70e71afadef9de23235a54309d5cc0e9536274 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 6 Aug 2019 16:18:43 -0700 Subject: [PATCH 2/2] take out btrigidbody field mass --- src/BulletDynamics/Dynamics/btRigidBody.cpp | 1 - src/BulletDynamics/Dynamics/btRigidBody.h | 7 +------ 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index ada686f99..f4bcabada 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -86,7 +86,6 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_debugBodyId = uniqueId++; setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); - m_mass = constructionInfo.m_mass; updateInertiaTensor(); m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 45c706878..705331b88 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -62,7 +62,6 @@ class btRigidBody : public btCollisionObject btVector3 m_linearVelocity; btVector3 m_angularVelocity; btScalar m_inverseMass; - btScalar m_mass; btVector3 m_linearFactor; btVector3 m_gravity; @@ -260,9 +259,7 @@ public: m_invMass = m_linearFactor * m_inverseMass; } btScalar getInvMass() const { return m_inverseMass; } - - btScalar getMass() const { return m_mass; } - + btScalar getMass() const { return m_inverseMass == btScalar(0.) ? btScalar(0.) : btScalar(1.0) / m_inverseMass; } const btMatrix3x3& getInvInertiaTensorWorld() const { return m_invInertiaTensorWorld; @@ -558,7 +555,6 @@ struct btRigidBodyFloatData btVector3FloatData m_totalForce; btVector3FloatData m_totalTorque; float m_inverseMass; - float m_mass; float m_linearDamping; float m_angularDamping; float m_additionalDampingFactor; @@ -585,7 +581,6 @@ struct btRigidBodyDoubleData btVector3DoubleData m_totalForce; btVector3DoubleData m_totalTorque; double m_inverseMass; - double m_mass; double m_linearDamping; double m_angularDamping; double m_additionalDampingFactor;