diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp index ffa2a8e07..02adce603 100644 --- a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp @@ -176,7 +176,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g btAlignedObjectArray world_to_local; btAlignedObjectArray local_origin; btmb->forwardKinematics(world_to_local, local_origin); - btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); + btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); // read generalized accelerations back from btMultiBody // the mapping from scratch variables to accelerations is taken from the implementation diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index ff0c8d2b3..d842bb43b 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -48,9 +48,6 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase) { @@ -315,8 +312,10 @@ void InvertedPendulumPDControl::initPhysics() } int upAxis = 1; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; + + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + m_guiHelper->setUpAxis(upAxis); diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index 4e2bf60fd..b8b421fc1 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -44,14 +44,9 @@ MultiBodyConstraintFeedbackSetup::~MultiBodyConstraintFeedbackSetup() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; void MultiBodyConstraintFeedbackSetup::initPhysics() { int upAxis = 2; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = @@ -69,6 +64,10 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); + + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + //create a static ground object if (1) { diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 27122ba3c..7963713d2 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -44,15 +44,10 @@ TestJointTorqueSetup::~TestJointTorqueSetup() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; void TestJointTorqueSetup::initPhysics() { int upAxis = 1; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); @@ -71,6 +66,10 @@ void TestJointTorqueSetup::initPhysics() //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + + //create a static ground object if (1) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 537f3e555..2a48843ed 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -96,8 +96,6 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -7697,11 +7695,11 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0; - if (gJointFeedbackInWorldSpace) + if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace) { serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE; } - if (gJointFeedbackInJointFrame) + if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame) { serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME; } @@ -7747,8 +7745,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) { - gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; - gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0; + m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; + m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0; } if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 36ec1f18b..e864508b4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -27,7 +27,6 @@ bool gEnableRendering = true; bool gActivedVRRealTimeSimulation = false; bool gEnableSyncPhysicsRendering = true; -bool gEnableUpdateDebugDrawLines = true; static int gCamVisualizerWidth = 228; static int gCamVisualizerHeight = 192; @@ -177,6 +176,7 @@ struct MotionArgs { MotionArgs() : m_debugDrawFlags(0), + m_enableUpdateDebugDrawLines(true), m_physicsServerPtr(0) { for (int i = 0; i < MAX_VR_CONTROLLERS; i++) @@ -201,7 +201,7 @@ struct MotionArgs b3CriticalSection* m_csGUI; int m_debugDrawFlags; - + bool m_enableUpdateDebugDrawLines; btAlignedObjectArray m_mouseCommands; b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS]; @@ -424,13 +424,13 @@ void MotionThreadFunc(void* userPtr, void* lsMemory) args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents, numSendVrControllers, keyEvents, args->m_sendKeyEvents.size(), mouseEvents, args->m_sendMouseEvents.size()); } { - if (gEnableUpdateDebugDrawLines) + args->m_csGUI->lock(); + if (args->m_enableUpdateDebugDrawLines) { - args->m_csGUI->lock(); args->m_physicsServerPtr->physicsDebugDraw(args->m_debugDrawFlags); - gEnableUpdateDebugDrawLines = false; - args->m_csGUI->unlock(); + args->m_enableUpdateDebugDrawLines = false; } + args->m_csGUI->unlock(); } deltaTimeInSeconds = 0; } @@ -1763,7 +1763,6 @@ void PhysicsServerExample::initPhysics() #endif } } - m_args[0].m_cs->lock(); m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); m_args[0].m_cs->unlock(); @@ -2845,7 +2844,7 @@ void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) //draw stuff and flush? this->m_multiThreadedHelper->m_debugDraw->drawDebugDrawerLines(); m_args[0].m_debugDrawFlags = debugDrawFlags; - gEnableUpdateDebugDrawLines = true; + m_args[0].m_enableUpdateDebugDrawLines = true; m_args[0].m_csGUI->unlock(); } } diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 439cc4428..89f8db8b1 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -62,6 +62,8 @@ struct btContactSolverInfoData btScalar m_singleAxisRollingFrictionThreshold; btScalar m_leastSquaresResidualThreshold; btScalar m_restitutionVelocityThreshold; + bool m_jointFeedbackInWorldSpace; + bool m_jointFeedbackInJointFrame; }; struct btContactSolverInfo : public btContactSolverInfoData @@ -94,6 +96,8 @@ struct btContactSolverInfo : public btContactSolverInfoData m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. m_leastSquaresResidualThreshold = 0.f; m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution + m_jointFeedbackInWorldSpace = false; + m_jointFeedbackInJointFrame = false; } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a890da46f..172472988 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -30,9 +30,6 @@ //#include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM -///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals -bool gJointFeedbackInWorldSpace = false; -bool gJointFeedbackInJointFrame = false; namespace { @@ -718,10 +715,12 @@ inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //ren // void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m, - bool isConstraintPass) + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame) { // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) // and the base linear & angular accelerations. @@ -1124,7 +1123,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar 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) + if (jointFeedbackInJointFrame) { //shift the reaction forces to the joint frame //linear (force) component is the same @@ -1132,7 +1131,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); } - if (gJointFeedbackInWorldSpace) + if (jointFeedbackInWorldSpace) { if (isConstraintPass) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 145ed4677..8a693f539 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -338,17 +338,20 @@ public: btAlignedObjectArray & scratch_r, btAlignedObjectArray & scratch_v, btAlignedObjectArray & scratch_m, - bool isConstraintPass = false); + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame + ); ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray & scratch_r, - btAlignedObjectArray & scratch_v, - btAlignedObjectArray & scratch_m, - bool isConstraintPass = false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); - } + //void stepVelocitiesMultiDof(btScalar dt, + // btAlignedObjectArray & scratch_r, + // btAlignedObjectArray & scratch_v, + // btAlignedObjectArray & scratch_m, + // bool isConstraintPass = false) + //{ + // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); + //} // calcAccelerationDeltasMultiDof // input: force vector (in same format as jacobian, i.e.: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a452d61b7..1557987bc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -491,11 +491,14 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_scratch_v.resize(bod->getNumLinks() + 1); m_scratch_m.resize(bod->getNumLinks() + 1); bool doNotUpdatePos = false; - + bool isConstraintPass = false; { if (!bod->isUsingRK4Integration()) { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } else { @@ -593,7 +596,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btScalar h = solverInfo.m_timeStep; #define output &m_scratch_r[bod->getNumDofs()] //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); @@ -603,7 +608,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd1 from: q1 & qd1 pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); @@ -613,7 +620,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd2 from: q2 & qd2 pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); @@ -623,7 +632,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd3, 0, numDofs); // @@ -660,7 +671,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { for (int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } } } @@ -708,7 +721,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if (!bod->isUsingRK4Integration()) { bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } } } diff --git a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp index 25bfda0bf..02f4ed163 100644 --- a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp +++ b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp @@ -198,12 +198,10 @@ static void* threadFunction(void* argument) status->m_status = 3; status->m_cs->unlock(); checkPThreadFunction(sem_post(status->m_mainSemaphore)); - printf("Thread with taskId %i exiting\n", status->m_taskId); break; } } - printf("Thread TERMINATED\n"); return 0; } @@ -272,7 +270,6 @@ void btThreadSupportPosix::waitForAllTasks() void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo) { m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already - printf("%s creating %i threads.\n", __FUNCTION__, m_numThreads); m_activeThreadStatus.resize(m_numThreads); m_startedThreadsMask = 0; @@ -281,7 +278,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi for (int i = 0; i < m_numThreads; i++) { - printf("starting thread %d\n", i); btThreadStatus& threadStatus = m_activeThreadStatus[i]; threadStatus.startSemaphore = createSem("threadLocal"); threadStatus.m_userPtr = 0; @@ -294,7 +290,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi threadStatus.threadUsed = 0; checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); - printf("started thread %d \n", i); } } @@ -304,20 +299,15 @@ void btThreadSupportPosix::stopThreads() for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t) { btThreadStatus& threadStatus = m_activeThreadStatus[t]; - printf("%s: Thread %i used: %ld\n", __FUNCTION__, int(t), threadStatus.threadUsed); threadStatus.m_userPtr = 0; checkPThreadFunction(sem_post(threadStatus.startSemaphore)); checkPThreadFunction(sem_wait(m_mainSemaphore)); - printf("destroy semaphore\n"); destroySem(threadStatus.startSemaphore); - printf("semaphore destroyed\n"); checkPThreadFunction(pthread_join(threadStatus.thread, 0)); } - printf("destroy main semaphore\n"); destroySem(m_mainSemaphore); - printf("main semaphore destroyed\n"); m_activeThreadStatus.clear(); }