diff --git a/examples/Constraints/TestHingeTorque.cpp b/examples/Constraints/TestHingeTorque.cpp index 3e321145c..bdc9c2f6a 100644 --- a/examples/Constraints/TestHingeTorque.cpp +++ b/examples/Constraints/TestHingeTorque.cpp @@ -85,7 +85,7 @@ void TestHingeTorque::stepSimulation(float deltaTime) for (int i=0;im_appliedForceBodyB.x(), @@ -111,7 +111,7 @@ void TestHingeTorque::initPhysics() createEmptyDynamicsWorld(); m_dynamicsWorld->getSolverInfo().m_splitImpulse = false; - m_dynamicsWorld->setGravity(btVector3(0,-1,-10)); + m_dynamicsWorld->setGravity(btVector3(0,0,-10)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); int mode = btIDebugDraw::DBG_DrawWireframe diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 19a525b98..300ced7db 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -235,7 +235,7 @@ void TestJointTorqueSetup::initPhysics() mbC->setAngularDamping(0.9f); } // - m_dynamicsWorld->setGravity(btVector3(0,-1,-10)); + m_dynamicsWorld->setGravity(btVector3(0,0,-10)); ////////////////////////////////////////////// if(0)//numLinks > 0) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 9dbaed648..759871fb6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -30,8 +30,9 @@ #include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM -///todo: determine if we need this option. If so, make a proper API, otherwise delete the global +///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals bool gJointFeedbackInWorldSpace = false; +bool gJointFeedbackInJointFrame = false; namespace { const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) @@ -1026,18 +1027,28 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, if (m_links[i].m_jointFeedback) { - - m_internalNeedsJointFeedback = true; - //m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1]; + + btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + + if (gJointFeedbackInJointFrame) + { + //shift the reaction forces to the joint frame + //linear (force) component is the same + //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector + angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); + } + + if (gJointFeedbackInWorldSpace) { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + 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 = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; } }