void dgWorldDynamicUpdate::CalculateJointsForceParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
{
dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
dgWorld* const world = (dgWorld*) worldContext;
const dgIsland* const island = syncData->m_island;
dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
dgBodyInfo* const bodyArrayPtr = (dgBodyInfo*) &world->m_bodiesMemory[0];
const dgBodyInfo* const bodyArray = &bodyArrayPtr[island->m_bodyStart];
dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0];
dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart];
dgJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
// const int jointCount = syncData->m_bachIndex;
const int jointCount = syncData->m_jointCount;
dgInt32* const bodyLocks = syncData->m_bodyLocks;
dgParallelSolverSyncData::dgParallelJointMap* const jointInfoMap = syncData->m_jointConflicts;
dgInt32* const atomicIndex = &syncData->m_atomicIndex;
dgFloat32 accNorm = dgFloat32(0.0f);
for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < jointCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
dgInt32 index = jointInfoMap[i].m_jointIndex;
dgJointInfo* const jointInfo = &constraintArray[index];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
dgAssert(m0 != m1);
dgSpinLock(&syncData->m_lock0, false);
if (m0) {
dgSpinLock(&bodyLocks[m0], false);
}
if (m1) {
dgSpinLock(&bodyLocks[m1], false);
}
dgSpinUnlock(&syncData->m_lock0);
dgFloat32 accel = world->CalculateJointForce(jointInfo, bodyArray, internalForces, matrixRow);
dgSpinLock(&syncData->m_lock1, false);
if (m0) {
dgSpinUnlock(&bodyLocks[m0]);
}
if (m1) {
dgSpinUnlock(&bodyLocks[m1]);
}
dgSpinUnlock(&syncData->m_lock1);
accNorm = (accel > accNorm) ? accel : accNorm;
}
syncData->m_accelNorm[threadID] = accNorm;
}
dgInt32 dgThreads::GetWork(dgWorkerThread** job)
{
#ifdef _WIN32
HANDLE hWaitHandles[2];
hWaitHandles[0] = m_workToDo;
hWaitHandles[1] = m_exit;
if ((WaitForMultipleObjects(2, hWaitHandles, FALSE, INFINITE) - WAIT_OBJECT_0)
== 1)
{
return 0;
}
EnterCriticalSection(&m_criticalSection);
*job = m_queue[m_bottomIndex];
m_bottomIndex = (m_bottomIndex + 1) % DG_MAXQUEUE;
ReleaseSemaphore(m_emptySlot, 1, NULL);
LeaveCriticalSection(&m_criticalSection);
#else
for (;;)
{
while ( m_workToDo == 0 )
{
dgThreadYield();
}
dgSpinLock( &m_workToDoSpinLock );
if ( m_workToDo > 0 )
{
break;
}
dgSpinUnlock( &m_workToDoSpinLock );
}
dgInterlockedDecrement( &m_workToDo );
dgSpinUnlock( &m_workToDoSpinLock );
if ( m_exit )
{
return 0;
}
dgSpinLock( &m_criticalSection );
dgWorkerThread* cWorker = m_queue[m_bottomIndex];
*job = cWorker;
m_bottomIndex = (m_bottomIndex+1) % (DG_MAXQUEUE);
dgInterlockedIncrement( &m_emptySlot );
dgSpinUnlock( &m_criticalSection );
#endif
return 1;
}
void dgDeadJoints::DestroyJoints(dgWorld& world)
{
dgSpinLock (&m_lock);
Iterator iter (*this);
for (iter.Begin(); iter; iter++) {
dgTreeNode* const node = iter.GetNode();
dgConstraint* const joint = node->GetInfo();
world.DestroyConstraint (joint);
}
RemoveAll ();
dgSpinUnlock(&m_lock);
}
void dgDeadBodies::DestroyBodies(dgWorld& world)
{
dgSpinLock (&m_lock);
Iterator iter (*this);
for (iter.Begin(); iter; iter++) {
dgTreeNode* const node = iter.GetNode();
dgBody* const body = node->GetInfo();
world.DestroyBody(body);
}
RemoveAll ();
dgSpinUnlock(&m_lock);
}
void dgDeadBodies::DestroyBody(dgBody* const body)
{
dgAssert (0);
dgSpinLock (&m_lock);
dgWorld& me = *((dgWorld*)this);
if (me.m_delayDelateLock) {
// the engine is busy in the previous update, deferred the deletion
Insert (body, body);
} else {
me.DestroyBody(body);
}
dgSpinUnlock(&m_lock);
}
void dgDeadJoints::DestroyJoint(dgConstraint* const joint)
{
dgSpinLock (&m_lock);
dgWorld& me = *((dgWorld*)this);
if (me.m_delayDelateLock) {
// the engine is busy in the previous update, deferred the deletion
Insert (joint, joint);
} else {
me.DestroyConstraint (joint);
}
dgSpinUnlock(&m_lock);
}
void dgWorldDynamicUpdate::SolverInitInternalForcesParallelKernel (void* const context, void* const worldContext, dgInt32 threadID)
{
dgParallelSolverSyncData* const syncData = (dgParallelSolverSyncData*) context;
dgWorld* const world = (dgWorld*) worldContext;
const dgIsland* const island = syncData->m_island;
dgJacobian* const internalForces = &world->m_solverMemory.m_internalForces[0];
dgJacobianMatrixElement* const matrixRow = &world->m_solverMemory.m_memory[0];
dgInt32* const atomicIndex = &syncData->m_atomicIndex;
dgJointInfo* const constraintArrayPtr = (dgJointInfo*) &world->m_jointsMemory[0];
dgJointInfo* const constraintArray = &constraintArrayPtr[island->m_jointStart];
dgInt32* const bodyLocks = syncData->m_bodyLocks;
for (dgInt32 i = dgAtomicExchangeAndAdd(atomicIndex, 1); i < syncData->m_jointCount; i = dgAtomicExchangeAndAdd(atomicIndex, 1)) {
dgJointInfo* const jointInfo = &constraintArray[i];
if (jointInfo->m_joint->m_solverActive) {
dgJacobian y0;
dgJacobian y1;
world->InitJointForce (jointInfo, matrixRow, y0, y1);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
dgAssert (m0 != m1);
if (m0) {
dgSpinLock(&bodyLocks[m0], false);
internalForces[m0].m_linear += y0.m_linear;
internalForces[m0].m_angular += y0.m_angular;
dgSpinUnlock(&bodyLocks[m0]);
}
if (m1) {
dgSpinLock(&bodyLocks[m1], false);
internalForces[m1].m_linear += y1.m_linear;
internalForces[m1].m_angular += y1.m_angular;
dgSpinUnlock(&bodyLocks[m1]);
}
}
}
}
//Queues up another to work
dgInt32 dgThreads::SubmitJob(dgWorkerThread* const job)
{
if (!m_numOfThreads)
{
_ASSERTE(job->m_threadIndex != -1);
job->ThreadExecute();
}
else
{
#ifdef _WIN32
dgInterlockedIncrement(&m_workInProgress);
if (WaitForSingleObject(m_emptySlot, INFINITE) != WAIT_OBJECT_0)
{
return (0);
}
EnterCriticalSection(&m_criticalSection);
m_queue[m_topIndex] = job;
m_topIndex = (m_topIndex + 1) % DG_MAXQUEUE;
ReleaseSemaphore(m_workToDo, 1, NULL);
LeaveCriticalSection(&m_criticalSection);
#else
dgInterlockedIncrement(&m_workInProgress);
while ( m_emptySlot == 0 )
{
dgThreadYield();
}
dgInterlockedDecrement( &m_emptySlot );
dgSpinLock(&m_criticalSection);
m_queue[m_topIndex] = job;
m_topIndex = (m_topIndex + 1) % DG_MAXQUEUE;
dgInterlockedIncrement( &m_workToDo );
dgSpinUnlock( &m_criticalSection );
#endif
}
return 1;
}
void dgThreads::dgReleaseIndirectLock(dgInt32* lockVar)
{
_ASSERTE(sizeof (dgInt32) == sizeof (long));
dgSpinUnlock(lockVar);
}
void dgThreads::dgReleaseLock() const
{
dgSpinUnlock(&m_globalSpinLock);
}
void dgThreads::DestroydgThreads()
{
#ifdef _WIN32
_ASSERTE(m_workInProgress == 0);
while (m_workInProgress > 0)
{
Sleep(10);
}
SetEvent(m_exit);
DeleteCriticalSection(&m_criticalSection);
WaitForMultipleObjects(DWORD(m_numOfThreads), m_threadhandles, TRUE,
INFINITE);
for (dgInt32 i = 0; i < m_numOfThreads; i++)
{
CloseHandle(m_threadhandles[i]);
}
CloseHandle(m_exit);
CloseHandle(m_emptySlot);
CloseHandle(m_workToDo);
m_exit = NULL;
m_emptySlot = NULL;
m_workToDo = NULL;
memset(&m_criticalSection, 0, sizeof(CRITICAL_SECTION));
for (dgInt32 i = 0; i < m_numOfThreads; i++)
{
m_threadhandles[i] = NULL;
}
m_topIndex = 0;
m_bottomIndex = 0;
m_workInProgress = 0;
m_numOfThreads = 0;
#else
while(m_workInProgress > 0)
{
usleep(100000);
}
dgSpinLock( &m_criticalSection );
m_exit = true;
m_workToDo = DG_MAXQUEUE+1;
dgSpinUnlock( &m_criticalSection );
#ifndef TARGET_OS_IPHONE
for(dgInt32 i=0; i<m_numOfThreads; i++ )
{
pthread_join( m_threadhandles[i], NULL );
}
#endif
m_exit = false;
m_emptySlot = 0;
m_workToDo = 0;
m_workToDoSpinLock = 0;
m_topIndex = 0;
m_bottomIndex = 0;
m_workInProgress = 0;
m_numOfThreads = 0;
#endif
}