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 dgThreads::dgGetLock() const { _ASSERTE(sizeof (dgInt32) == sizeof (long)); dgSpinLock(&m_globalSpinLock); //spinLock( &m_globalSpinLock ); // linux and mac may need to yeald time // while(! __sync_bool_compare_and_swap(&m_globalSpinLock, 0, 1) ) { // ThreadYield(); // } }
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::dgGetIndirectLock(dgInt32* lockVar) { _ASSERTE(sizeof (dgInt32) == sizeof (long)); dgSpinLock(lockVar); }
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 }