Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1981 from erwincoumans/master

Browse Source 
fix some race conditions
This commit is contained in:
erwincoumans
2018-11-05 18:12:52 -08:00
committed by GitHub
parent 1cb8dc383f 882252f8c0
commit 0b513c825d
11 changed files with 71 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Extras/InverseDynamics/invdyn_bullet_comparison.cpp
View File

@@ -176,7 +176,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
btAlignedObjectArray<btQuaternion> world_to_local; btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin; btAlignedObjectArray<btVector3> local_origin;
btmb->forwardKinematics(world_to_local, 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 // read generalized accelerations back from btMultiBody
// the mapping from scratch variables to accelerations is taken from the implementation // the mapping from scratch variables to accelerations is taken from the implementation

9
examples/MultiBody/InvertedPendulumPDControl.cpp
View File

@@ -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) btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase)
{ {
@@ -315,8 +312,10 @@ void InvertedPendulumPDControl::initPhysics()
} }
int upAxis = 1; int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true; m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);

9
examples/MultiBody/MultiBodyConstraintFeedback.cpp
View File

@@ -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() void MultiBodyConstraintFeedbackSetup::initPhysics()
{ {
int upAxis = 2; int upAxis = 2;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] = btVector4 colors[4] =
@@ -69,6 +64,10 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
//btIDebugDraw::DBG_DrawConstraints //btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); +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 //create a static ground object
if (1) if (1)
{ {

9
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -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() void TestJointTorqueSetup::initPhysics()
{ {
int upAxis = 1; int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
@@ -71,6 +66,10 @@ void TestJointTorqueSetup::initPhysics()
//btIDebugDraw::DBG_DrawConstraints //btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); +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 //create a static ground object
if (1) if (1)
{ {

10
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -96,8 +96,6 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#endif #endif
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;
int gInternalSimFlags = 0; int gInternalSimFlags = 0;
bool gResetSimulation = 0; bool gResetSimulation = 0;
@@ -7697,11 +7695,11 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0; 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; 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; 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) if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
{ {
gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (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_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0;
} }
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME) if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)

15
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -27,7 +27,6 @@ bool gEnableRendering = true;
bool gActivedVRRealTimeSimulation = false; bool gActivedVRRealTimeSimulation = false;
bool gEnableSyncPhysicsRendering = true; bool gEnableSyncPhysicsRendering = true;
bool gEnableUpdateDebugDrawLines = true;
static int gCamVisualizerWidth = 228; static int gCamVisualizerWidth = 228;
static int gCamVisualizerHeight = 192; static int gCamVisualizerHeight = 192;
@@ -177,6 +176,7 @@ struct MotionArgs
{ {
MotionArgs() MotionArgs()
: m_debugDrawFlags(0), : m_debugDrawFlags(0),
m_enableUpdateDebugDrawLines(true),
m_physicsServerPtr(0) m_physicsServerPtr(0)
{ {
for (int i = 0; i < MAX_VR_CONTROLLERS; i++) for (int i = 0; i < MAX_VR_CONTROLLERS; i++)
@@ -201,7 +201,7 @@ struct MotionArgs
b3CriticalSection* m_csGUI; b3CriticalSection* m_csGUI;
int m_debugDrawFlags; int m_debugDrawFlags;
bool m_enableUpdateDebugDrawLines;
btAlignedObjectArray<MyMouseCommand> m_mouseCommands; btAlignedObjectArray<MyMouseCommand> m_mouseCommands;
b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS]; 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()); 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); args->m_physicsServerPtr->physicsDebugDraw(args->m_debugDrawFlags);
gEnableUpdateDebugDrawLines = false; args->m_enableUpdateDebugDrawLines = false;
args->m_csGUI->unlock();
} }
args->m_csGUI->unlock();
} }
deltaTimeInSeconds = 0; deltaTimeInSeconds = 0;
} }
@@ -1763,7 +1763,6 @@ void PhysicsServerExample::initPhysics()
#endif #endif
} }
} }
m_args[0].m_cs->lock(); m_args[0].m_cs->lock();
m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle);
m_args[0].m_cs->unlock(); m_args[0].m_cs->unlock();
@@ -2845,7 +2844,7 @@ void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
//draw stuff and flush? //draw stuff and flush?
this->m_multiThreadedHelper->m_debugDraw->drawDebugDrawerLines(); this->m_multiThreadedHelper->m_debugDraw->drawDebugDrawerLines();
m_args[0].m_debugDrawFlags = debugDrawFlags; m_args[0].m_debugDrawFlags = debugDrawFlags;
gEnableUpdateDebugDrawLines = true; m_args[0].m_enableUpdateDebugDrawLines = true;
m_args[0].m_csGUI->unlock(); m_args[0].m_csGUI->unlock();
} }
} }

4
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -62,6 +62,8 @@ struct btContactSolverInfoData
btScalar m_singleAxisRollingFrictionThreshold; btScalar m_singleAxisRollingFrictionThreshold;
btScalar m_leastSquaresResidualThreshold; btScalar m_leastSquaresResidualThreshold;
btScalar m_restitutionVelocityThreshold; btScalar m_restitutionVelocityThreshold;
bool m_jointFeedbackInWorldSpace;
bool m_jointFeedbackInJointFrame;
}; };
struct btContactSolverInfo : public btContactSolverInfoData 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_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_leastSquaresResidualThreshold = 0.f;
m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution
m_jointFeedbackInWorldSpace = false;
m_jointFeedbackInJointFrame = false;
} }
}; };

17
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -30,9 +30,6 @@
//#include "Bullet3Common/b3Logging.h" //#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM // #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 namespace
{ {
@@ -718,10 +715,12 @@ inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //ren
// //
void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v, btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m, btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass) bool isConstraintPass,
bool jointFeedbackInWorldSpace,
bool jointFeedbackInJointFrame)
{ {
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations. // 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 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; 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 //shift the reaction forces to the joint frame
//linear (force) component is the same //linear (force) component is the same
@@ -1132,7 +1131,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
} }
if (gJointFeedbackInWorldSpace) if (jointFeedbackInWorldSpace)
{ {
if (isConstraintPass) if (isConstraintPass)
{ {

21
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -338,17 +338,20 @@ public:
btAlignedObjectArray<btScalar> & scratch_r, btAlignedObjectArray<btScalar> & scratch_r,
btAlignedObjectArray<btVector3> & scratch_v, btAlignedObjectArray<btVector3> & scratch_v,
btAlignedObjectArray<btMatrix3x3> & scratch_m, btAlignedObjectArray<btMatrix3x3> & scratch_m,
bool isConstraintPass = false); bool isConstraintPass,
bool jointFeedbackInWorldSpace,
bool jointFeedbackInJointFrame
);
///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
void stepVelocitiesMultiDof(btScalar dt, //void stepVelocitiesMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> & scratch_r, // btAlignedObjectArray<btScalar> & scratch_r,
btAlignedObjectArray<btVector3> & scratch_v, // btAlignedObjectArray<btVector3> & scratch_v,
btAlignedObjectArray<btMatrix3x3> & scratch_m, // btAlignedObjectArray<btMatrix3x3> & scratch_m,
bool isConstraintPass = false) // bool isConstraintPass = false)
{ //{
computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
} //}
// calcAccelerationDeltasMultiDof // calcAccelerationDeltasMultiDof
// input: force vector (in same format as jacobian, i.e.: // input: force vector (in same format as jacobian, i.e.:

31
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -491,11 +491,14 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
m_scratch_v.resize(bod->getNumLinks() + 1); m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1); m_scratch_m.resize(bod->getNumLinks() + 1);
bool doNotUpdatePos = false; bool doNotUpdatePos = false;
bool isConstraintPass = false;
{ {
if (!bod->isUsingRK4Integration()) 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 else
{ {
@@ -593,7 +596,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
btScalar h = solverInfo.m_timeStep; btScalar h = solverInfo.m_timeStep;
#define output &m_scratch_r[bod->getNumDofs()] #define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0 //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); pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0 //calc q1 = q0 + h/2 * qd0
pResetQx(); pResetQx();
@@ -603,7 +608,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
// //
//calc qdd1 from: q1 & qd1 //calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_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); pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1 //calc q2 = q0 + h/2 * qd1
pResetQx(); pResetQx();
@@ -613,7 +620,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
// //
//calc qdd2 from: q2 & qd2 //calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_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); pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2 //calc q3 = q0 + h * qd2
pResetQx(); pResetQx();
@@ -623,7 +632,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
// //
//calc qdd3 from: q3 & qd3 //calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_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); pCopy(output, scratch_qdd3, 0, numDofs);
// //
@@ -660,7 +671,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{ {
for (int link = 0; link < bod->getNumLinks(); ++link) for (int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof(); 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()) if (!bod->isUsingRK4Integration())
{ {
bool isConstraintPass = true; 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);
} }
} }
} }

10
src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp
View File

@@ -198,12 +198,10 @@ static void* threadFunction(void* argument)
status->m_status = 3; status->m_status = 3;
status->m_cs->unlock(); status->m_cs->unlock();
checkPThreadFunction(sem_post(status->m_mainSemaphore)); checkPThreadFunction(sem_post(status->m_mainSemaphore));
printf("Thread with taskId %i exiting\n", status->m_taskId);
break; break;
} }
} }
printf("Thread TERMINATED\n");
return 0; return 0;
} }
@@ -272,7 +270,6 @@ void btThreadSupportPosix::waitForAllTasks()
void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo) void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo)
{ {
m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already
printf("%s creating %i threads.\n", __FUNCTION__, m_numThreads);
m_activeThreadStatus.resize(m_numThreads); m_activeThreadStatus.resize(m_numThreads);
m_startedThreadsMask = 0; m_startedThreadsMask = 0;
@@ -281,7 +278,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi
for (int i = 0; i < m_numThreads; i++) for (int i = 0; i < m_numThreads; i++)
{ {
printf("starting thread %d\n", i);
btThreadStatus& threadStatus = m_activeThreadStatus[i]; btThreadStatus& threadStatus = m_activeThreadStatus[i];
threadStatus.startSemaphore = createSem("threadLocal"); threadStatus.startSemaphore = createSem("threadLocal");
threadStatus.m_userPtr = 0; threadStatus.m_userPtr = 0;
@@ -294,7 +290,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi
threadStatus.threadUsed = 0; threadStatus.threadUsed = 0;
checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); 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) for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
{ {
btThreadStatus& threadStatus = m_activeThreadStatus[t]; btThreadStatus& threadStatus = m_activeThreadStatus[t];
printf("%s: Thread %i used: %ld\n", __FUNCTION__, int(t), threadStatus.threadUsed);
threadStatus.m_userPtr = 0; threadStatus.m_userPtr = 0;
checkPThreadFunction(sem_post(threadStatus.startSemaphore)); checkPThreadFunction(sem_post(threadStatus.startSemaphore));
checkPThreadFunction(sem_wait(m_mainSemaphore)); checkPThreadFunction(sem_wait(m_mainSemaphore));
printf("destroy semaphore\n");
destroySem(threadStatus.startSemaphore); destroySem(threadStatus.startSemaphore);
printf("semaphore destroyed\n");
checkPThreadFunction(pthread_join(threadStatus.thread, 0)); checkPThreadFunction(pthread_join(threadStatus.thread, 0));
} }
printf("destroy main semaphore\n");
destroySem(m_mainSemaphore); destroySem(m_mainSemaphore);
printf("main semaphore destroyed\n");
m_activeThreadStatus.clear(); m_activeThreadStatus.clear();
} }