diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 56f6bdfc2..fb5ce8934 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -357,7 +357,8 @@ void btMultiBody::setupPlanar(int i, void btMultiBody::finalizeMultiDof() { btAssert(m_isMultiDof); - + m_deltaV.resize(0); + m_deltaV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) @@ -543,7 +544,17 @@ btVector3 btMultiBody::getAngularMomentum() const return result; } +void btMultiBody::clearConstraintForces() +{ + m_baseConstraintForce.setValue(0, 0, 0); + m_baseConstraintTorque.setValue(0, 0, 0); + + for (int i = 0; i < getNumLinks(); ++i) { + m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); + m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); + } +} void btMultiBody::clearForcesAndTorques() { m_baseForce.setValue(0, 0, 0); @@ -574,6 +585,18 @@ void btMultiBody::addLinkTorque(int i, const btVector3 &t) m_links[i].m_appliedTorque += t; } +void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) +{ + m_links[i].m_appliedConstraintForce += f; +} + +void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) +{ + m_links[i].m_appliedConstraintTorque += t; +} + + + void btMultiBody::addJointTorque(int i, btScalar Q) { m_links[i].m_jointTorque[0] += Q; @@ -637,7 +660,8 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //r void btMultiBody::stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) + btAlignedObjectArray &scratch_m, + bool isConstraintPass) { // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) // and the base linear & angular accelerations. @@ -734,8 +758,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, } else { + btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; + btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; //external forces - zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce)); + zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); //adding damping terms (only) btScalar linDampMult = 1., angDampMult = 1.; @@ -804,7 +830,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // calculate zhat_i^A // //external forces - zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce)); + btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; + btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; + + zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); if (0) { @@ -1043,12 +1072,28 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, if (gJointFeedbackInWorldSpace) { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + if (isConstraintPass) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + } else + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + } } else { + if (isConstraintPass) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; + + } + else + { m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; + } } } @@ -1084,9 +1129,13 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, ///////////////// // Final step: add the accelerations (times dt) to the velocities. + + if (!isConstraintPass) + { if(dt > 0.) applyDeltaVeeMultiDof(output, dt); + } ///// //btScalar angularThres = 1; //btScalar maxAngVel = 0.; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index d136c7bd5..49770f497 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -273,6 +273,8 @@ public: // void clearForcesAndTorques(); + void clearConstraintForces(); + void clearVelocities(); void addBaseForce(const btVector3 &f) @@ -282,7 +284,17 @@ public: void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } void addLinkForce(int i, const btVector3 &f); void addLinkTorque(int i, const btVector3 &t); - void addJointTorque(int i, btScalar Q); + + void addBaseConstraintForce(const btVector3 &f) + { + m_baseConstraintForce += f; + } + void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } + void addLinkConstraintForce(int i, const btVector3 &f); + void addLinkConstraintTorque(int i, const btVector3 &t); + + +void addJointTorque(int i, btScalar Q); void addJointTorqueMultiDof(int i, int dof, btScalar Q); void addJointTorqueMultiDof(int i, const btScalar *Q); @@ -318,7 +330,9 @@ public: void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m); + btAlignedObjectArray &scratch_m, + bool isConstraintPass=false + ); // calcAccelerationDeltas // input: force vector (in same format as jacobian, i.e.: @@ -374,6 +388,23 @@ public: } } + void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_deltaV[dof] += delta_vee[dof] * multiplier; + } + } + void processDeltaVeeMultiDof2() + { + applyDeltaVeeMultiDof(&m_deltaV[0],1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_deltaV[dof] = 0.f; + } + } + void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) { //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) @@ -591,7 +622,10 @@ private: btVector3 m_baseForce; // external force applied to base. World frame. btVector3 m_baseTorque; // external torque applied to base. World frame. - + + btVector3 m_baseConstraintForce; // external force applied to base. World frame. + btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. + btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; @@ -611,7 +645,7 @@ private: // offset size array // 0 num_links+1 rot_from_parent // - + btAlignedObjectArray m_deltaV; btAlignedObjectArray m_realBuf; btAlignedObjectArray m_vectorBuf; btAlignedObjectArray m_matrixBuf; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index b4a97c8b8..3e7853ead 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -170,7 +170,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity if(c.m_multiBodyA->isMultiDof()) - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); else c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS @@ -186,7 +186,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity if(c.m_multiBodyB->isMultiDof()) - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); else c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS @@ -887,7 +887,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS #include "Bullet3Common/b3Logging.h" void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { -#if 1 +#if 1 //bod->addBaseForce(m_gravity * bod->getBaseMass()); //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); @@ -898,78 +898,49 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if(c.m_multiBodyA->isMultiDof()) { - if (c.m_useJointForce) + c.m_multiBodyA->setCompanionId(-1); + btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); + btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); + if (c.m_linkA<0) { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - //c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime); + c.m_multiBodyA->addBaseConstraintForce(force); + c.m_multiBodyA->addBaseConstraintTorque(torque); } else - //if (c.m_multiBodyA->getCompanionId()>=0) { - c.m_multiBodyA->setCompanionId(-1); - btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkA<0) - { - c.m_multiBodyA->addBaseForce(force); - c.m_multiBodyA->addBaseTorque(torque); - } else - { - if (c.m_useJointForce) - { - c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime); - } else - { - c.m_multiBodyA->addLinkForce(c.m_linkA,force); - //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyA->addLinkTorque(c.m_linkA,torque); - } - } - //c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); + c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); + //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); } } - else - { - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } } if (c.m_multiBodyB) { if(c.m_multiBodyB->isMultiDof()) { - if (c.m_useJointForce) - { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } else - - //if (c.m_multiBodyB->getCompanionId()>=0) { c.m_multiBodyB->setCompanionId(-1); btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); if (c.m_linkB<0) { - c.m_multiBodyB->addBaseForce(force); - c.m_multiBodyB->addBaseTorque(torque); + c.m_multiBodyB->addBaseConstraintForce(force); + c.m_multiBodyB->addBaseConstraintTorque(torque); } else { - if (!c.m_useJointForce) { - c.m_multiBodyB->addLinkForce(c.m_linkB,force); + c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyB->addLinkTorque(c.m_linkB,torque); + c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); } } - //c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); } } - else - { - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } } -#else +#endif + +#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS if (c.m_multiBodyA) { @@ -1006,7 +977,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish"); int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); -#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) //or as applied force, so we can measure the joint reaction forces easier @@ -1029,7 +999,6 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); } -#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 579cb47f5..b6cae190f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -441,7 +441,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); - +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY { BT_PROFILE("btMultiBody addForce"); for (int i=0;im_multiBodies.size();i++) @@ -476,7 +476,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) }//if (!isSleeping) } } - +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY { @@ -671,16 +671,61 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); } +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY bod->clearForcesAndTorques(); +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY }//if (!isSleeping) } } + clearMultiBodyConstraintForces(); + m_solverMultiBodyIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) + scratch_v.resize(bod->getNumLinks()+1); + scratch_m.resize(bod->getNumLinks()+1); + + if(bod->isMultiDof()) + { + if(!bod->isUsingRK4Integration()) + { + bool isConstraintPass = true; + bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); + } + } + } + } + } + + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } } @@ -882,3 +927,87 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() btDiscreteDynamicsWorld::debugDrawWorld(); } + + + +void btMultiBodyDynamicsWorld::applyGravity() +{ + btDiscreteDynamicsWorld::applyGravity(); +#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + BT_PROFILE("btMultiBody addGravity"); + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + }//if (!isSleeping) + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY +} + +void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() +{ + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearConstraintForces(); + } +} +void btMultiBodyDynamicsWorld::clearMultiBodyForces() +{ + { + BT_PROFILE("clearMultiBodyForces"); + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + btMultiBody* bod = m_multiBodies[i]; + bod->clearForcesAndTorques(); + } + } + } + +} +void btMultiBodyDynamicsWorld::clearForces() +{ + btDiscreteDynamicsWorld::clearForces(); + +#ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + clearMultiBodyForces(); +#endif +} + + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index edb30c01e..155b9e3df 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -18,6 +18,7 @@ subject to the following restrictions: #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY class btMultiBody; class btMultiBodyConstraint; @@ -85,6 +86,10 @@ public: virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); void forwardKinematics(); + virtual void clearForces(); + virtual void clearMultiBodyConstraintForces(); + virtual void clearMultiBodyForces(); + virtual void applyGravity(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 92b3e4fc2..c27c8ef02 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -106,6 +106,9 @@ struct btMultibodyLink btVector3 m_appliedForce; // In WORLD frame btVector3 m_appliedTorque; // In WORLD frame +btVector3 m_appliedConstraintForce; // In WORLD frame + btVector3 m_appliedConstraintTorque; // In WORLD frame + btScalar m_jointPos[7]; //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.