dgWorld::dgWorld(dgMemoryAllocator* allocator):
// dgThreadHive(),
dgBodyMasterList(allocator),
dgBroadPhaseCollision(allocator),
dgBodyMaterialList(allocator),
dgBodyCollisionList(allocator),
dgActiveContacts(allocator),
dgCollidingPairCollector(),
m_perInstanceData(allocator),
m_threadsManager(),
m_dynamicSolver()
{
dgInt32 steps;
dgFloat32 freezeAccel2;
dgFloat32 freezeAlpha2;
dgFloat32 freezeSpeed2;
dgFloat32 freezeOmega2;
// init exact arithmetic functions
m_allocator = allocator;
//dgThreadHive::SetThreadCount(16);
//dgThreadHive::SetThreadCount(32);
//_control87 (_EM_ZERODIVIDE | _EM_INEXACT | _EM_OVERFLOW | _EM_INVALID, _MCW_EM);
m_inUpdate = 0;
m_bodyGroupID = 0;
// m_activeBodiesCount = 0;
m_defualtBodyGroupID = CreateBodyGroupID();
m_islandMemorySizeInBytes = DG_INITIAL_ISLAND_SIZE;
m_bodiesMemorySizeInBytes = DG_INITIAL_BODIES_SIZE;
m_jointsMemorySizeInBytes = DG_INITIAL_JOINTS_SIZE;
m_pairMemoryBufferSizeInBytes = 1024 * 64 * sizeof (void*);
m_pairMemoryBuffer = m_allocator->MallocLow (m_pairMemoryBufferSizeInBytes);
m_islandMemory = m_allocator->MallocLow (m_islandMemorySizeInBytes);
m_jointsMemory = m_allocator->MallocLow (m_jointsMemorySizeInBytes);
m_bodiesMemory = m_allocator->MallocLow (m_bodiesMemorySizeInBytes);
for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i ++) {
m_jacobiansMemorySizeInBytes[i] = DG_INITIAL_JACOBIAN_SIZE;
m_jacobiansMemory[i] = m_allocator->MallocLow (m_jacobiansMemorySizeInBytes[i]);
m_internalForcesMemorySizeInBytes[i] = DG_INITIAL_BODIES_SIZE;
m_internalForcesMemory[i] = m_allocator->MallocLow (m_internalForcesMemorySizeInBytes[i]);
m_contactBuffersSizeInBytes[i] = DG_INITIAL_CONTATCT_SIZE;
m_contactBuffers[i] = m_allocator->MallocLow (m_contactBuffersSizeInBytes[i]);
}
m_genericLRUMark = 0;
m_singleIslandMultithreading = 1;
m_solverMode = 0;
m_frictionMode = 0;
m_dynamicsLru = 0;
m_broadPhaseLru = 0;
m_bodiesUniqueID = 0;
// m_bodiesCount = 0;
m_frictiomTheshold = dgFloat32 (0.25f);
m_userData = NULL;
m_islandUpdate = NULL;
m_destroyCollision = NULL;
m_leavingWorldNotify = NULL;
m_destroyBodyByExeciveForce = NULL;
m_freezeAccel2 = DG_FREEZE_MAG2;
m_freezeAlpha2 = DG_FREEZE_MAG2;
m_freezeSpeed2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
m_freezeOmega2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
steps = 1;
freezeAccel2 = m_freezeAccel2;
freezeAlpha2 = m_freezeAlpha2;
freezeSpeed2 = m_freezeSpeed2;
freezeOmega2 = m_freezeOmega2;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
m_sleepTable[i].m_maxAccel = freezeAccel2;
m_sleepTable[i].m_maxAlpha = freezeAlpha2;
m_sleepTable[i].m_maxVeloc = freezeSpeed2;
m_sleepTable[i].m_maxOmega = freezeOmega2;
m_sleepTable[i].m_steps = steps;
steps += 7;
freezeAccel2 *= dgFloat32 (1.5f);
freezeAlpha2 *= dgFloat32 (1.4f);
freezeSpeed2 *= dgFloat32 (1.5f);
freezeOmega2 *= dgFloat32 (1.5f);
}
steps += 300;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;
m_cpu = dgNoSimdPresent;
m_numberOfTheads = 1;
SetHardwareMode (1);
SetThreadsCount (DG_MAXIMUN_THREADS);
m_maxTheads = m_numberOfTheads;
SetHardwareMode (0);
SetThreadsCount (1);
dgBroadPhaseCollision::Init ();
dgCollidingPairCollector::Init ();
m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
AddSentinelBody();
SetPerfomanceCounter(NULL);
}
dgWorld::dgWorld(dgMemoryAllocator* const allocator)
:dgBodyMasterList(allocator)
,dgBodyMaterialList(allocator)
,dgBodyCollisionList(allocator)
,dgDeformableBodiesUpdate(allocator)
,dgActiveContacts(allocator)
,dgCollidingPairCollector()
,dgWorldDynamicUpdate()
,dgMutexThread("dgMutexThread", DG_MUTEX_THREAD_ID)
,dgAsyncThread("dgAsyncThread", DG_ASYNC_THREAD_ID)
,dgWorldThreadPool(allocator)
,m_broadPhase(NULL)
,m_sentinelBody(NULL)
,m_pointCollision(NULL)
,m_amp(NULL)
,m_preListener(allocator)
,m_postListener(allocator)
,m_perInstanceData(allocator)
,m_islandMemory (DG_INITIAL_ISLAND_SIZE, allocator, 64)
,m_bodiesMemory (DG_INITIAL_BODIES_SIZE, allocator, 64)
,m_jointsMemory (DG_INITIAL_JOINTS_SIZE, allocator, 64)
,m_pairMemoryBuffer (DG_INITIAL_CONTACT_SIZE, allocator, 64)
,m_solverMatrixMemory (DG_INITIAL_JACOBIAN_SIZE, allocator, 64)
,m_solverRightSideMemory (DG_INITIAL_BODIES_SIZE, allocator, 64)
{
dgMutexThread* const mutexThread = this;
SetMatertThread (mutexThread);
m_allocator = allocator;
m_islandUpdate = NULL;
m_getPerformanceCount = NULL;
m_onCollisionInstanceDestruction = NULL;
m_onCollisionInstanceCopyConstrutor = NULL;
m_inUpdate = 0;
m_bodyGroupID = 0;
m_defualtBodyGroupID = CreateBodyGroupID();
m_genericLRUMark = 0;
m_useParallelSolver = 0;
//m_solverMode = 0;
m_solverMode = 1;
m_frictionMode = 0;
m_dynamicsLru = 0;
m_bodiesUniqueID = 0;
m_frictiomTheshold = dgFloat32 (0.25f);
m_userData = NULL;
m_islandUpdate = NULL;
m_freezeAccel2 = DG_FREEZE_MAG2;
m_freezeAlpha2 = DG_FREEZE_MAG2;
m_freezeSpeed2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
m_freezeOmega2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
m_contactTolerance = DG_REDUCE_CONTACT_TOLERANCE;
dgInt32 steps = 1;
dgFloat32 freezeAccel2 = m_freezeAccel2;
dgFloat32 freezeAlpha2 = m_freezeAlpha2;
dgFloat32 freezeSpeed2 = m_freezeSpeed2;
dgFloat32 freezeOmega2 = m_freezeOmega2;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
m_sleepTable[i].m_maxAccel = freezeAccel2;
m_sleepTable[i].m_maxAlpha = freezeAlpha2;
m_sleepTable[i].m_maxVeloc = freezeSpeed2;
m_sleepTable[i].m_maxOmega = freezeOmega2;
m_sleepTable[i].m_steps = steps;
steps += 7;
freezeAccel2 *= dgFloat32 (1.5f);
freezeAlpha2 *= dgFloat32 (1.4f);
freezeSpeed2 *= dgFloat32 (1.5f);
freezeOmega2 *= dgFloat32 (1.5f);
}
steps += 300;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;
m_hardwaredIndex = 0;
SetThreadsCount (0);
//dgBroadPhase::Init ();
m_broadPhase = new (allocator) dgBroadPhase(this);
dgCollidingPairCollector::Init ();
//m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
dgCollision* const pointCollison = new (m_allocator) dgCollisionPoint(m_allocator);
m_pointCollision = CreateInstance(pointCollison, 0, dgGetIdentityMatrix());
pointCollison->Release();
AddSentinelBody();
SetPerfomanceCounter(NULL);
#ifdef _NEWTON_AMP
m_amp = new (GetAllocator()) dgAmpInstance(this);
#endif
}
dgWorld::dgWorld(dgMemoryAllocator* const allocator)
:dgBodyMasterList(allocator)
,dgBodyMaterialList(allocator)
,dgBodyCollisionList(allocator)
,dgSkeletonList(allocator)
,dgInverseDynamicsList(allocator)
,dgContactList(allocator)
,dgBilateralConstraintList(allocator)
,dgWorldDynamicUpdate(allocator)
,dgMutexThread("newtonMainThread", 0)
,dgWorldThreadPool(allocator)
,dgDeadBodies(allocator)
,dgDeadJoints(allocator)
,dgWorldPluginList(allocator)
,m_broadPhase(NULL)
,m_sentinelBody(NULL)
,m_pointCollision(NULL)
,m_userData(NULL)
,m_allocator (allocator)
,m_mutex()
,m_postUpdateCallback(NULL)
,m_listeners(allocator)
,m_perInstanceData(allocator)
,m_bodiesMemory (allocator, 64)
,m_jointsMemory (allocator, 64)
,m_clusterMemory (allocator, 64)
,m_solverJacobiansMemory (allocator, 64)
,m_solverRightHandSideMemory (allocator, 64)
,m_solverForceAccumulatorMemory (allocator, 64)
,m_concurrentUpdate(false)
{
//TestAStart();
//TestSort();
dgMutexThread* const myThread = this;
SetParentThread (myThread);
// avoid small memory fragmentations on initialization
m_bodiesMemory.Resize(1024);
m_clusterMemory.Resize(1024);
m_jointsMemory.Resize(1024 * 2);
m_solverJacobiansMemory.Resize(1024 * 64);
m_solverRightHandSideMemory.Resize(1024 * 64);
m_solverForceAccumulatorMemory.Resize(1024 * 32);
m_savetimestep = dgFloat32 (0.0f);
m_allocator = allocator;
m_clusterUpdate = NULL;
m_onCollisionInstanceDestruction = NULL;
m_onCollisionInstanceCopyConstrutor = NULL;
m_serializedJointCallback = NULL;
m_deserializedJointCallback = NULL;
m_inUpdate = 0;
m_bodyGroupID = 0;
m_lastExecutionTime = 0;
m_defualtBodyGroupID = CreateBodyGroupID();
m_genericLRUMark = 0;
m_delayDelateLock = 0;
m_clusterLRU = 0;
m_useParallelSolver = 0;
m_solverIterations = DG_DEFAULT_SOLVER_ITERATION_COUNT;
m_dynamicsLru = 0;
m_numberOfSubsteps = 1;
m_bodiesUniqueID = 0;
m_frictiomTheshold = dgFloat32 (0.25f);
m_userData = NULL;
m_clusterUpdate = NULL;
m_freezeAccel2 = DG_FREEZE_ACCEL2;
m_freezeAlpha2 = DG_FREEZE_ACCEL2;
m_freezeSpeed2 = DG_FREEZE_SPEED2;
m_freezeOmega2 = DG_FREEZE_SPEED2;
m_contactTolerance = DG_PRUNE_CONTACT_TOLERANCE;
dgInt32 steps = 1;
dgFloat32 freezeAccel2 = m_freezeAccel2;
dgFloat32 freezeAlpha2 = m_freezeAlpha2;
dgFloat32 freezeSpeed2 = m_freezeSpeed2;
dgFloat32 freezeOmega2 = m_freezeOmega2;
for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
m_sleepTable[i].m_maxAccel = freezeAccel2;
m_sleepTable[i].m_maxAlpha = freezeAlpha2;
m_sleepTable[i].m_maxVeloc = freezeSpeed2;
m_sleepTable[i].m_maxOmega = freezeOmega2;
m_sleepTable[i].m_steps = steps;
steps += 7;
freezeAccel2 *= dgFloat32 (1.5f);
freezeAlpha2 *= dgFloat32 (1.4f);
freezeSpeed2 *= dgFloat32 (1.5f);
freezeOmega2 *= dgFloat32 (1.5f);
}
steps += 300;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;
SetThreadsCount (0);
m_broadPhase = new (allocator) dgBroadPhaseMixed(this);
//m_broadPhase = new (allocator) dgBroadPhaseSegregated (this);
//m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
dgCollision* const pointCollison = new (m_allocator) dgCollisionPoint(m_allocator);
m_pointCollision = CreateInstance(pointCollison, 0, dgGetIdentityMatrix());
pointCollison->Release();
AddSentinelBody();
// LoadPlugins();
}
void
CThreadPool_Controller_PID::OnEvent(EEvent event)
{
if (event == eSuspend) {
return;
}
// All reads below are atomic reads of one variable, thus they cannot
// return bad results. They can be a little bit inconsistent with each
// other because of races with other threads but that's okay for the
// purposes of this controller.
unsigned int threads_count = GetPool()->GetThreadsCount();
unsigned int queued_tasks = GetPool()->GetQueuedTasksCount();
unsigned int run_tasks = GetPool()->GetExecutingTasksCount();
if (threads_count == 0) {
EnsureLimits();
threads_count = GetMinThreads();
// Special case when MinThreads == 0
if (threads_count == 0) {
if (queued_tasks == 0) {
return;
}
threads_count = 1;
SetThreadsCount(threads_count);
}
}
double now_err = (double(queued_tasks + run_tasks) - threads_count)
/ threads_count;
double now_time = m_Timer.Elapsed();
if (event == eResume) {
// When we resuming we need to avoid panic because of big changes in
// error value. So we will assume that current error value was began
// long time ago and didn't change afterwards.
m_ErrHistory.clear();
m_ErrHistory.push_back(SThreadPool_PID_ErrInfo(now_time - m_DerivTime,
now_err));
}
double period = now_time - m_ErrHistory.back().call_time;
if (now_err < 0 && threads_count == GetMinThreads()
&& m_IntegrErr <= 0)
{
now_err = 0;
}
double integr_err = m_IntegrErr + (now_err + m_ErrHistory.back().err) / 2
* period / m_IntegrCoeff;
while (m_ErrHistory.size() > 1
&& now_time - m_ErrHistory[1].call_time >= m_DerivTime)
{
m_ErrHistory.pop_front();
}
if (now_time - m_ErrHistory.back().call_time >= m_DerivTime / 10) {
m_ErrHistory.push_back(SThreadPool_PID_ErrInfo(now_time, now_err));
if (threads_count == GetMaxThreads() && integr_err > m_Threshold) {
m_IntegrErr = m_Threshold;
}
else if (threads_count == GetMinThreads()
&& integr_err < -m_Threshold)
{
m_IntegrErr = -m_Threshold;
}
else {
m_IntegrErr = integr_err;
}
}
double deriv_err = (now_err - m_ErrHistory[0].err)
/ m_DerivTime * m_DerivCoeff;
double final_val = (now_err + integr_err + deriv_err) / m_Threshold;
/*
LOG_POST(CTime(CTime::eCurrent).AsString("M/D/Y h:m:s.l").c_str()
<< " count=" << threads_count << ", queued=" << queued_tasks
<< ", run=" << run_tasks << ", err=" << now_err << ", time=" << now_time
<< ", intErr=" << m_IntegrErr << ", derivErr=" << deriv_err
<< ", final=" << final_val << ", hist_size=" << m_ErrHistory.size());
*/
if (final_val >= 1 || final_val <= -1) {
if (final_val < 0 && -final_val > threads_count)
SetThreadsCount(GetMinThreads());
else
SetThreadsCount(threads_count + int(final_val));
}
else {
EnsureLimits();
}
}
