From bee9bd9999ba8a1c68579bf7c9f02f15c240fade Mon Sep 17 00:00:00 2001 From: JulioJerez Date: Sun, 26 Dec 2021 12:24:47 -0800 Subject: [PATCH] fine release candidate for newton 4.00 2021 make some collision system also parallel, lock and atomic free. I now need to revisit the joint limits, since I broke something there, but is not really critical now. --- .../ndSandbox/demos/ndBasicStacks.cpp | 3 +- .../ndSandbox/ndDemoEntityManager.cpp | 4 +- .../ndSandbox/toolbox/ndDebugDisplay.cpp | 4 +- newton-4.00/sdk/dCollision/ndBodyKinematic.h | 6 + newton-4.00/sdk/dCollision/ndCollision.h | 2 +- newton-4.00/sdk/dCollision/ndContact.h | 2 +- .../{ndContactList.cpp => ndContactArray.cpp} | 10 +- .../{ndContactList.h => ndContactArray.h} | 16 +- newton-4.00/sdk/dCollision/ndScene.cpp | 306 ++++++++++-------- newton-4.00/sdk/dCollision/ndScene.h | 44 ++- newton-4.00/sdk/dCollision/ndShape.cpp | 1 - newton-4.00/sdk/dCollision/ndShape.h | 1 - .../dAvx2/ndDynamicsUpdateAvx2.cpp | 16 +- newton-4.00/sdk/dNewton/ndDynamicsUpdate.cpp | 22 +- .../sdk/dNewton/ndDynamicsUpdateSoa.cpp | 14 +- newton-4.00/sdk/dNewton/ndNewton.h | 2 +- .../sdk/dNewton/ndSkeletonContainer.cpp | 2 +- newton-4.00/sdk/dNewton/ndWorld.cpp | 51 +-- newton-4.00/sdk/dNewton/ndWorld.h | 7 +- 19 files changed, 263 insertions(+), 250 deletions(-) rename newton-4.00/sdk/dCollision/{ndContactList.cpp => ndContactArray.cpp} (84%) rename newton-4.00/sdk/dCollision/{ndContactList.h => ndContactArray.h} (85%) diff --git a/newton-4.00/applications/ndSandbox/demos/ndBasicStacks.cpp b/newton-4.00/applications/ndSandbox/demos/ndBasicStacks.cpp index b0aeaeca6b..69d20b34ff 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndBasicStacks.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndBasicStacks.cpp @@ -280,8 +280,7 @@ void ndBasicStacks (ndDemoEntityManager* const scene) BuildSphereColumn(scene, 10.0f, origin, ndVector(0.5f, 0.5f, 0.5f, 0.0f), 20); origin.m_z += 6.0f; - //BuildBoxColumn(scene, 10.0f, origin, ndVector(0.5f, 0.5f, 0.5f, 0.0f), 7); - BuildBoxColumn(scene, 10.0f, origin, ndVector(0.5f, 0.5f, 0.5f, 0.0f), 20); + BuildBoxColumn(scene, 10.0f, origin, ndVector(0.5f, 0.5f, 0.5f, 0.0f), 18); origin.m_z += 6.0f; BuildCylinderColumn(scene, 10.0f, origin, ndVector(0.75f, 0.6f, 1.0f, 0.0f), 20); diff --git a/newton-4.00/applications/ndSandbox/ndDemoEntityManager.cpp b/newton-4.00/applications/ndSandbox/ndDemoEntityManager.cpp index 453b7ae083..18216d39b5 100644 --- a/newton-4.00/applications/ndSandbox/ndDemoEntityManager.cpp +++ b/newton-4.00/applications/ndSandbox/ndDemoEntityManager.cpp @@ -39,7 +39,7 @@ //#define DEFAULT_SCENE 1 // gpu basic rigidbody //#define DEFAULT_SCENE 2 // friction ramp //#define DEFAULT_SCENE 3 // conservation of momentum -//#define DEFAULT_SCENE 4 // basic Stacks +#define DEFAULT_SCENE 4 // basic Stacks //#define DEFAULT_SCENE 5 // basic Trigger //#define DEFAULT_SCENE 6 // basic player //#define DEFAULT_SCENE 7 // particle fluid @@ -50,7 +50,7 @@ //#define DEFAULT_SCENE 12 // active rag doll //#define DEFAULT_SCENE 13 // basic vehicle //#define DEFAULT_SCENE 14 // heavy vehicle -#define DEFAULT_SCENE 15 // simple voronoi fracture +//#define DEFAULT_SCENE 15 // simple voronoi fracture //#define DEFAULT_SCENE 16 // basic voronoi fracture //#define DEFAULT_SCENE 17 // linked voronoi fracture //#define DEFAULT_SCENE 18 // skin peel voronoi fracture diff --git a/newton-4.00/applications/ndSandbox/toolbox/ndDebugDisplay.cpp b/newton-4.00/applications/ndSandbox/toolbox/ndDebugDisplay.cpp index ba6d6bc678..ffc5a0ec22 100644 --- a/newton-4.00/applications/ndSandbox/toolbox/ndDebugDisplay.cpp +++ b/newton-4.00/applications/ndSandbox/toolbox/ndDebugDisplay.cpp @@ -72,7 +72,6 @@ static void DrawBox(const ndVector& p0, const ndVector& p1, glVector3 box[12][2] void RenderBodiesAABB(ndDemoEntityManager* const scene) { ndWorld* const world = scene->GetWorld(); - const ndBodyList& bodyList = world->GetBodyList(); GLuint shader = scene->GetShaderCache().m_wireFrame; @@ -97,6 +96,7 @@ void RenderBodiesAABB(ndDemoEntityManager* const scene) glEnableClientState(GL_VERTEX_ARRAY); glVertexPointer(3, GL_FLOAT, sizeof (glVector3), box); + const ndBodyList& bodyList = world->GetBodyList(); for (ndBodyList::ndNode* bodyNode = bodyList.GetFirst(); bodyNode; bodyNode = bodyNode->GetNext()) { ndVector p0; @@ -181,7 +181,7 @@ void RenderContactPoints(ndDemoEntityManager* const scene) glVector3 pointBuffer[4]; glEnableClientState(GL_VERTEX_ARRAY); glVertexPointer(3, GL_FLOAT, sizeof (glVector3), pointBuffer); - const ndContactList& contactList = world->GetContactList(); + const ndContactArray& contactList = world->GetContactList(); for (ndInt32 i = 0; i < contactList.GetCount(); ++i) { const ndContact* const contact = contactList[i]; diff --git a/newton-4.00/sdk/dCollision/ndBodyKinematic.h b/newton-4.00/sdk/dCollision/ndBodyKinematic.h index 054d4d237e..02f741c732 100644 --- a/newton-4.00/sdk/dCollision/ndBodyKinematic.h +++ b/newton-4.00/sdk/dCollision/ndBodyKinematic.h @@ -175,6 +175,7 @@ class ndBodyKinematic: public ndBody void SetAccel(const ndJacobian& accel); virtual void SpecialUpdate(ndFloat32 timestep); virtual void IntegrateGyroSubstep(const ndVector& timestep); + virtual void ApplyExternalForces(ndInt32 threadIndex, ndFloat32 timestep); virtual ndJacobian IntegrateForceAndToque(const ndVector& force, const ndVector& torque, const ndVector& timestep) const; void UpdateCollisionMatrix(); @@ -514,5 +515,10 @@ inline void ndBodyKinematic::SpecialUpdate(ndFloat32) { dAssert(0); } + +inline void ndBodyKinematic::ApplyExternalForces(ndInt32, ndFloat32) +{ +} + #endif diff --git a/newton-4.00/sdk/dCollision/ndCollision.h b/newton-4.00/sdk/dCollision/ndCollision.h index 384450f720..5da18f224a 100644 --- a/newton-4.00/sdk/dCollision/ndCollision.h +++ b/newton-4.00/sdk/dCollision/ndCollision.h @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/newton-4.00/sdk/dCollision/ndContact.h b/newton-4.00/sdk/dCollision/ndContact.h index 3336a61bbc..9983e46018 100644 --- a/newton-4.00/sdk/dCollision/ndContact.h +++ b/newton-4.00/sdk/dCollision/ndContact.h @@ -133,7 +133,7 @@ class ndContact: public ndConstraint static ndVector m_initialSeparatingVector; friend class ndScene; - friend class ndContactList; + friend class ndContactArray; friend class ndBodyKinematic; friend class ndContactSolver; friend class ndShapeInstance; diff --git a/newton-4.00/sdk/dCollision/ndContactList.cpp b/newton-4.00/sdk/dCollision/ndContactArray.cpp similarity index 84% rename from newton-4.00/sdk/dCollision/ndContactList.cpp rename to newton-4.00/sdk/dCollision/ndContactArray.cpp index e4568d42fa..f3c450b704 100644 --- a/newton-4.00/sdk/dCollision/ndContactList.cpp +++ b/newton-4.00/sdk/dCollision/ndContactArray.cpp @@ -22,10 +22,10 @@ #include "ndCoreStdafx.h" #include "ndCollisionStdafx.h" #include "ndContact.h" -#include "ndContactList.h" +#include "ndContactArray.h" #include "ndBodyKinematic.h" -ndContact* ndContactList::CreateContact(ndBodyKinematic* const body0, ndBodyKinematic* const body1) +ndContact* ndContactArray::CreateContact(ndBodyKinematic* const body0, ndBodyKinematic* const body1) { ndContact* const contact = new ndContact; contact->SetBodies(body0, body1); @@ -35,16 +35,16 @@ ndContact* ndContactList::CreateContact(ndBodyKinematic* const body0, ndBodyKine return contact; } -void ndContactList::DeleteContact(ndContact* const contact) +void ndContactArray::DeleteContact(ndContact* const contact) { if (contact->m_isAttached) { - contact->DetachFromBodies(); + contact->DetachFromBodies(); } contact->m_isDead = 1; } -void ndContactList::DeleteAllContacts() +void ndContactArray::DeleteAllContacts() { for (ndInt32 i = GetCount() - 1; i >= 0; --i) { diff --git a/newton-4.00/sdk/dCollision/ndContactList.h b/newton-4.00/sdk/dCollision/ndContactArray.h similarity index 85% rename from newton-4.00/sdk/dCollision/ndContactList.h rename to newton-4.00/sdk/dCollision/ndContactArray.h index 9f49ac9af8..b5f95eb1cf 100644 --- a/newton-4.00/sdk/dCollision/ndContactList.h +++ b/newton-4.00/sdk/dCollision/ndContactArray.h @@ -25,26 +25,16 @@ #include "ndCollisionStdafx.h" #include "ndContact.h" - -class ndConstraintArray: public ndArray -{ - public: - ndConstraintArray() - :ndArray(1024) - { - } -}; - -class ndContactList : public ndArray +class ndContactArray : public ndArray { public: - ndContactList() + ndContactArray() :ndArray(1024) ,m_lock() { } - ~ndContactList() + ~ndContactArray() { } diff --git a/newton-4.00/sdk/dCollision/ndScene.cpp b/newton-4.00/sdk/dCollision/ndScene.cpp index 49f9b38b76..e48d58d780 100644 --- a/newton-4.00/sdk/dCollision/ndScene.cpp +++ b/newton-4.00/sdk/dCollision/ndScene.cpp @@ -190,20 +190,21 @@ ndFloat64 ndScene::ndFitnessList::TotalCost() const ndScene::ndScene() :ndThreadPool("newtonWorker") ,m_bodyList() - ,m_contactList() - ,m_scrathContactList() - ,m_activeConstraintArray() + ,m_contactArray() + ,m_scratchBuffer(1024) ,m_sceneBodyArray(1024) ,m_activeBodyArray(1024) - ,m_activeBodyArrayBuffer(1024) + ,m_activeConstraintArray(1024) ,m_specialUpdateList() - ,m_contactLock() + ,m_lock() ,m_rootNode(nullptr) ,m_contactNotifyCallback(new ndContactNotify()) ,m_treeEntropy(ndFloat32(0.0f)) ,m_fitness() ,m_timestep(ndFloat32 (0.0f)) ,m_lru(D_CONTACT_DELAY_FRAMES) + ,m_bodyListChanged(0) + ,m_currentThreadsMem(0) { m_contactNotifyCallback->m_scene = this; } @@ -221,8 +222,7 @@ void ndScene::CollisionOnlyUpdate() D_TRACKTIME(); Begin(); m_lru = m_lru + 1; - BuildBodyArray(); - UpdateAabb(); + InitBodyArray(); BalanceScene(); FindCollidingPairs(); CalculateContacts(); @@ -287,6 +287,7 @@ bool ndScene::AddBody(ndBodyKinematic* const body) { if ((body->m_scene == nullptr) && (body->m_sceneNode == nullptr)) { + m_bodyListChanged = 1; ndBodyList::ndNode* const node = m_bodyList.Append(body); body->SetSceneNodes(this, node); m_contactNotifyCallback->OnBodyAdded(body); @@ -317,11 +318,12 @@ bool ndScene::RemoveBody(ndBodyKinematic* const body) while (contactMap.GetRoot()) { ndContact* const contact = contactMap.GetRoot()->GetInfo(); - m_contactList.DeleteContact(contact); + m_contactArray.DeleteContact(contact); } if (body->m_scene && body->m_sceneNode) { + m_bodyListChanged = 1; if (body->GetAsBodyTriggerVolume() || body->GetAsBodyPlayerCapsule()) { m_specialUpdateList.Remove(body->m_spetialUpdateNode); @@ -1246,85 +1248,148 @@ void ndScene::AddPair(ndBodyKinematic* const body0, ndBodyKinematic* const body1 const bool isCollidable = bilateral ? bilateral->IsCollidable() : true; if (isCollidable) { - m_contactList.CreateContact(body0, body1); + m_contactArray.CreateContact(body0, body1); } } } -void ndScene::BuildBodyArray() +void ndScene::InitBodyArray() { D_TRACKTIME(); + class ndBodyInfo + { + public: + ndInt32 m_scan[D_MAX_THREADS_COUNT][2]; + }; + class ndBuildBodyArray : public ndBaseJob { public: - #define D_LOCAL_POOL_SIZE 256 virtual void Execute() { D_TRACKTIME(); - ndAtomic& activeBodyCount = *((ndAtomic*)m_context); ndArray& activeBodyArray = m_owner->m_activeBodyArray; - const ndArray& bodyArrayBuffer = m_owner->m_activeBodyArrayBuffer; - - ndInt32 bodyCount = 0; - const ndStartEnd startEnd(bodyArrayBuffer.GetCount(), GetThreadId(), m_owner->GetThreadCount()); - for (ndInt32 j = startEnd.m_start; j < startEnd.m_end; ++j) + + const ndInt32 threadIndex = GetThreadId(); + const ndBodyListRun& run = m_owner->m_bodyListRuns[GetThreadId()]; + + ndBodyInfo& info = *((ndBodyInfo*)m_context); + ndInt32* const scan = &info.m_scan[threadIndex][0]; + scan[0] = 0; + scan[1] = 0; + + const ndFloat32 timestep = m_timestep; + ndBodyList::ndNode* node = run.m_begin; + for (ndInt32 i = 0; i < run.m_count; ++i) { - ndBodyKinematic* const body = bodyArrayBuffer[j]; - const ndShape* const shape = body->GetCollisionShape().GetShape()->GetAsShapeNull(); - if (!shape) + ndBodyKinematic* const body = node->GetInfo(); + node = node->GetNext(); + dAssert (!body->GetCollisionShape().GetShape()->GetAsShapeNull()); + bool inScene = true; + if (!body->GetSceneBodyNode()) { - bool inScene = true; - if (!body->GetSceneBodyNode()) - { - ndScopeSpinLock lock(m_owner->m_contactLock); - inScene = m_owner->AddBody(body); - } - if (inScene) - { - m_buffer[bodyCount] = body; - bodyCount++; - if (bodyCount >= D_LOCAL_POOL_SIZE) - { - bodyCount = 0; - const ndInt32 baseIndex = activeBodyCount.fetch_add(D_LOCAL_POOL_SIZE); - for (ndInt32 i = 0; i < D_LOCAL_POOL_SIZE; i++) - { - const ndInt32 index = baseIndex + i; - m_buffer[i]->PrepareStep(index); - activeBodyArray[index] = m_buffer[i]; - } - } - } + ndScopeSpinLock lock(m_owner->m_lock); + inScene = m_owner->AddBody(body); } - } + dAssert(inScene && body->m_sceneNode); - if (bodyCount) - { - const ndInt32 baseIndex = activeBodyCount.fetch_add(bodyCount); - for (ndInt32 i = 0; i < bodyCount; i++) + body->ApplyExternalForces(threadIndex, timestep); + const ndInt32 index = i + run.m_start; + body->PrepareStep(index); + activeBodyArray[index] = body; + + const ndInt32 key = body->m_equilibrium; + scan[key] ++; + if (!body->m_equilibrium) { - const ndInt32 index = baseIndex + i; - m_buffer[i]->PrepareStep(index); - activeBodyArray[index] = m_buffer[i]; + m_owner->UpdateAabb(threadIndex, body); } } } + }; + + class ndClassifyMovingBodies : public ndBaseJob + { + public: + virtual void Execute() + { + D_TRACKTIME(); + const ndArray& activeBodyArray = m_owner->m_activeBodyArray; + ndBodyKinematic** const sceneBodyArray = &m_owner->m_sceneBodyArray[0]; - ndBodyKinematic* m_buffer[D_LOCAL_POOL_SIZE]; + const ndInt32 threadIndex = GetThreadId(); + const ndBodyListRun& run = m_owner->m_bodyListRuns[GetThreadId()]; + + ndBodyInfo& info = *((ndBodyInfo*)m_context); + ndInt32* const scan = &info.m_scan[threadIndex][0]; + + for (ndInt32 i = 0; i < run.m_count; ++i) + { + ndBodyKinematic* const body = activeBodyArray[i + run.m_start]; + const ndInt32 key = body->m_equilibrium; + const ndInt32 index = scan[key]; + sceneBodyArray[index] = body; + scan[key] ++; + } + } }; - ndInt32 index = 0; - m_activeBodyArrayBuffer.SetCount(m_bodyList.GetCount()); - for (ndBodyList::ndNode* node = m_bodyList.GetFirst(); node; node = node->GetNext()) + if (m_bodyListChanged || (m_currentThreadsMem != GetThreadCount())) { - m_activeBodyArrayBuffer[index] = node->GetInfo(); - index++; + m_bodyListChanged = 0; + const ndInt32 threadCount = GetThreadCount(); + if (m_bodyList.GetCount() < (2 * threadCount)) + { + for (ndInt32 i = 0; i < threadCount; ++i) + { + m_bodyListRuns[i].m_count = 0; + m_bodyListRuns[i].m_start = 0; + m_bodyListRuns[i].m_begin = nullptr; + } + m_bodyListRuns[0].m_count = m_bodyList.GetCount(); + m_bodyListRuns[0].m_begin = m_bodyList.GetFirst(); + } + else + { + ndInt32 start = 0; + ndInt32 count = m_bodyList.GetCount() / threadCount; + ndBodyList::ndNode* node = m_bodyList.GetFirst(); + for (ndInt32 i = 0; i < threadCount; ++i) + { + m_bodyListRuns[i].m_start = start; + m_bodyListRuns[i].m_count = count; + start += count; + m_bodyListRuns[i].m_begin = node; + for (ndInt32 j = 0; j < count; ++j) + { + node = node->GetNext(); + } + } + m_bodyListRuns[threadCount - 1].m_count = m_bodyList.GetCount() - m_bodyListRuns[threadCount - 1].m_start; + } + m_currentThreadsMem = ndInt8(GetThreadCount()); } - ndAtomic activeBodyCount(0); + ndBodyInfo info; m_activeBodyArray.SetCount(m_bodyList.GetCount()); - SubmitJobs(&activeBodyCount); - m_activeBodyArray.SetCount(activeBodyCount); + SubmitJobs(&info); + + ndInt32 sum = 0; + ndInt32 threadCount = GetThreadCount(); + for (ndInt32 j = 0; j < 2; j++) + { + for (ndInt32 i = 0; i < threadCount; i++) + { + const ndInt32 count = info.m_scan[i][j]; + info.m_scan[i][j] = sum; + sum += count; + } + } + + ndInt32 movingBodyCount = info.m_scan[0][1] - info.m_scan[0][0]; + m_sceneBodyArray.SetCount(m_bodyList.GetCount()); + SubmitJobs(&info); + m_sceneBodyArray.SetCount(movingBodyCount); } void ndScene::CalculateContacts() @@ -1342,9 +1407,10 @@ void ndScene::CalculateContacts() virtual void Execute() { D_TRACKTIME(); - ndContactList& activeContacts = m_owner->m_contactList; + ndContactArray& activeContacts = m_owner->m_contactArray; ndContactInfo& info = *((ndContactInfo*)m_context); - ndContactList& dstContacts = m_owner->m_scrathContactList; + ndContact** const dstContacts = (ndContact**)&m_owner->m_scratchBuffer[0]; + const ndInt32 threadIndex = GetThreadId(); ndInt32* const scan = &info.m_digitScan[threadIndex][0]; @@ -1382,9 +1448,9 @@ void ndScene::CalculateContacts() { D_TRACKTIME(); ndContactInfo& info = *((ndContactInfo*)m_context); - ndContactList& dstContacts = m_owner->m_contactList; - ndContactList& srcContacts = m_owner->m_scrathContactList; - ndConstraintArray& activeConstraintArray = m_owner->m_activeConstraintArray; + ndContactArray& dstContacts = m_owner->m_contactArray; + ndContact** const srcContacts = (ndContact**)&m_owner->m_scratchBuffer[0]; + ndArray& activeConstraintArray = m_owner->m_activeConstraintArray; const ndInt32 threadIndex = GetThreadId(); ndInt32 keyLookUp[4]; @@ -1408,69 +1474,47 @@ void ndScene::CalculateContacts() } }; - ndContactInfo info; - m_scrathContactList.SetCount(m_contactList.GetCount()); - m_activeConstraintArray.SetCount(m_contactList.GetCount()); - SubmitJobs(&info); - - ndInt32 sum = 0; - ndInt32 threadCount = GetThreadCount(); - for (ndInt32 j = 0; j < 4; j++) + m_activeConstraintArray.SetCount(0); + if (m_contactArray.GetCount()) { - for (ndInt32 i = 0; i < threadCount; i++) - { - const ndInt32 count = info.m_digitScan[i][j]; - info.m_digitScan[i][j] = sum; - sum += count; - } - } + ndContactInfo info; + m_scratchBuffer.SetCount(m_contactArray.GetCount()); + m_activeConstraintArray.SetCount(m_contactArray.GetCount()); + SubmitJobs(&info); - ndInt32 activeJoints = info.m_digitScan[0][1] - info.m_digitScan[0][0]; - ndInt32 inactiveJoints = info.m_digitScan[0][2] - info.m_digitScan[0][1]; - ndInt32 deadJoints = info.m_digitScan[0][3] - info.m_digitScan[0][2]; - - SubmitJobs(&info); - if (deadJoints) - { - D_TRACKTIME(); - // this could be parallelized, monitor it to see if is worth doing it. - const ndInt32 start = activeJoints + inactiveJoints; - for (ndInt32 i = 0; i < deadJoints; i++) + ndInt32 sum = 0; + ndInt32 threadCount = GetThreadCount(); + for (ndInt32 j = 0; j < 4; j++) { - ndContact* const contact = m_contactList[start + i]; - m_contactList.DeleteContact(contact); - delete contact; + for (ndInt32 i = 0; i < threadCount; i++) + { + const ndInt32 count = info.m_digitScan[i][j]; + info.m_digitScan[i][j] = sum; + sum += count; + } } - } - m_activeConstraintArray.SetCount(activeJoints); - m_contactList.SetCount(activeJoints + inactiveJoints); -} + ndInt32 activeJoints = info.m_digitScan[0][1] - info.m_digitScan[0][0]; + ndInt32 inactiveJoints = info.m_digitScan[0][2] - info.m_digitScan[0][1]; + ndInt32 deadContacts = info.m_digitScan[0][3] - info.m_digitScan[0][2]; -void ndScene::UpdateAabb() -{ - D_TRACKTIME(); - class ndUpdateAabbJob : public ndBaseJob - { - public: - virtual void Execute() + SubmitJobs(&info); + if (deadContacts) { D_TRACKTIME(); - const ndArray& bodyArray = m_owner->GetActiveBodyArray(); - const ndInt32 threadIndex = GetThreadId(); - const ndStartEnd startEnd(bodyArray.GetCount() - 1, GetThreadId(), m_owner->GetThreadCount()); - for (ndInt32 i = startEnd.m_start; i < startEnd.m_end; ++i) + // this could be parallelized, monitor it to see if is worth doing it. + const ndInt32 start = activeJoints + inactiveJoints; + for (ndInt32 i = 0; i < deadContacts; i++) { - ndBodyKinematic* const body = bodyArray[i]; - if (!body->m_equilibrium) - { - m_owner->UpdateAabb(threadIndex, body); - } + ndContact* const contact = m_contactArray[start + i]; + m_contactArray.DeleteContact(contact); + delete contact; } } - }; - SubmitJobs(); + m_activeConstraintArray.SetCount(activeJoints); + m_contactArray.SetCount(activeJoints + inactiveJoints); + } } void ndScene::FindCollidingPairs(ndBodyKinematic* const body) @@ -1571,20 +1615,21 @@ void ndScene::FindCollidingPairs() } }; - ndInt32 index = 0; - m_sceneBodyArray.SetCount(m_activeBodyArray.GetCount()); +#ifdef _DEBUG + ndInt32 count = 0; for (ndInt32 i = 0; i < m_activeBodyArray.GetCount(); i++) { ndBodyKinematic* const body = m_activeBodyArray[i]; - if (!(body->m_equilibrium & body->m_autoSleep)) + if (!body->m_equilibrium) { - m_sceneBodyArray[index] = body; - index++; + dAssert(body == m_sceneBodyArray[count]); + count++; } } - m_sceneBodyArray.SetCount(index); - - bool fullScan = (3 * index) > m_activeBodyArray.GetCount(); + m_sceneBodyArray.SetCount(count); +#endif + + bool fullScan = (3 * m_sceneBodyArray.GetCount()) > m_activeBodyArray.GetCount(); // uncomment line below to test full versus partial scan //fullScan = true; @@ -1943,19 +1988,28 @@ void ndScene::BodiesInAabb(ndBodiesInAabbNotify& callback, const ndSceneNode** s void ndScene::Cleanup() { Sync(); - m_contactList.DeleteAllContacts(); + m_bodyListChanged = 1; while (m_bodyList.GetFirst()) { ndBodyKinematic* const body = m_bodyList.GetFirst()->GetInfo(); RemoveBody(body); delete body; } + m_contactArray.DeleteAllContacts(); + ndFreeListAlloc::Flush(); + + m_contactArray.Resize(1024); + m_scratchBuffer.Resize(1024); m_sceneBodyArray.Resize(1024); m_activeBodyArray.Resize(1024); - m_scrathContactList.Resize(1024); m_activeConstraintArray.Resize(1024); - m_activeBodyArrayBuffer.Resize(1024); + + m_contactArray.SetCount(0); + m_scratchBuffer.SetCount(0); + m_sceneBodyArray.SetCount(0); + m_activeBodyArray.SetCount(0); + m_activeConstraintArray.SetCount(0); } void ndScene::AddNode(ndSceneNode* const newNode) diff --git a/newton-4.00/sdk/dCollision/ndScene.h b/newton-4.00/sdk/dCollision/ndScene.h index 5a806f02a5..518e3ac626 100644 --- a/newton-4.00/sdk/dCollision/ndScene.h +++ b/newton-4.00/sdk/dCollision/ndScene.h @@ -25,10 +25,10 @@ #include "ndCollisionStdafx.h" #include "ndBodyList.h" #include "ndSceneNode.h" -#include "ndContactList.h" +#include "ndContactArray.h" -#define D_SCENE_MAX_STACK_DEPTH 256 -#define D_PRUNE_CONTACT_TOLERANCE ndFloat32 (5.0e-2f) +#define D_SCENE_MAX_STACK_DEPTH 256 +#define D_PRUNE_CONTACT_TOLERANCE ndFloat32 (5.0e-2f) class ndWorld; class ndScene; @@ -92,8 +92,8 @@ class ndScene : public ndThreadPool virtual ndWorld* GetWorld() const; const ndBodyList& GetBodyList() const; - ndConstraintArray& GetActiveContactArray(); - const ndConstraintArray& GetActiveContactArray() const; + ndArray& GetActiveContactArray(); + const ndArray& GetActiveContactArray() const; ndArray& GetActiveBodyArray(); const ndArray& GetActiveBodyArray() const; @@ -147,7 +147,7 @@ class ndScene : public ndThreadPool ndSceneNode* BuildTopDownBig(ndSceneNode** const leafArray, ndInt32 firstBox, ndInt32 lastBox, ndFitnessList::ndNode** const nextNode); D_COLLISION_API void CollisionOnlyUpdate(); - const ndContactList& GetContactList() const; + const ndContactArray& GetContactArray() const; ndSceneTreeNode* InsertNode(ndSceneNode* const root, ndSceneNode* const node); ndContact* FindContactJoint(ndBodyKinematic* const body0, ndBodyKinematic* const body1) const; @@ -165,30 +165,40 @@ class ndScene : public ndThreadPool protected: D_COLLISION_API ndScene(); - D_COLLISION_API void UpdateAabb(); D_COLLISION_API void UpdateSpecial(); - D_COLLISION_API void BuildBodyArray(); + D_COLLISION_API void InitBodyArray(); D_COLLISION_API void UpdateTransform(); D_COLLISION_API void CalculateContacts(); D_COLLISION_API void FindCollidingPairs(); D_COLLISION_API virtual void BalanceScene(); D_COLLISION_API virtual void ThreadFunction(); + + class ndBodyListRun + { + public: + ndBodyList::ndNode* m_begin; + ndInt32 m_count; + ndInt32 m_start; + }; ndBodyList m_bodyList; - ndContactList m_contactList; - ndContactList m_scrathContactList; - ndConstraintArray m_activeConstraintArray; + ndContactArray m_contactArray; + + ndArray m_scratchBuffer; ndArray m_sceneBodyArray; ndArray m_activeBodyArray; - ndArray m_activeBodyArrayBuffer; + ndArray m_activeConstraintArray; ndList m_specialUpdateList; - ndSpinLock m_contactLock; + ndSpinLock m_lock; + ndBodyListRun m_bodyListRuns[D_MAX_THREADS_COUNT]; ndSceneNode* m_rootNode; ndContactNotify* m_contactNotifyCallback; ndFloat64 m_treeEntropy; ndFitnessList m_fitness; ndFloat32 m_timestep; ndUnsigned32 m_lru; + ndUnsigned8 m_bodyListChanged; + ndUnsigned8 m_currentThreadsMem; static ndVector m_velocTol; static ndVector m_linearContactError2; @@ -221,12 +231,12 @@ inline const ndBodyList& ndScene::GetBodyList() const return m_bodyList; } -inline ndConstraintArray& ndScene::GetActiveContactArray() +inline ndArray& ndScene::GetActiveContactArray() { return m_activeConstraintArray; } -inline const ndConstraintArray& ndScene::GetActiveContactArray() const +inline const ndArray& ndScene::GetActiveContactArray() const { return m_activeConstraintArray; } @@ -241,9 +251,9 @@ inline const ndArray& ndScene::GetActiveBodyArray() const return m_activeBodyArray; } -inline const ndContactList& ndScene::GetContactList() const +inline const ndContactArray& ndScene::GetContactArray() const { - return m_contactList; + return m_contactArray; } template diff --git a/newton-4.00/sdk/dCollision/ndShape.cpp b/newton-4.00/sdk/dCollision/ndShape.cpp index 81b0e5f9cf..195e79138e 100644 --- a/newton-4.00/sdk/dCollision/ndShape.cpp +++ b/newton-4.00/sdk/dCollision/ndShape.cpp @@ -26,7 +26,6 @@ ndVector ndShape::m_flushZero(ndFloat32(1.0e-7f)); ndShape::ndShape(ndShapeID id) - //:ndClassAlloc() :ndContainersFreeListAlloc() ,m_inertia(ndVector::m_zero) ,m_crossInertia(ndVector::m_zero) diff --git a/newton-4.00/sdk/dCollision/ndShape.h b/newton-4.00/sdk/dCollision/ndShape.h index 3aae7efd9b..d7f200b3b2 100644 --- a/newton-4.00/sdk/dCollision/ndShape.h +++ b/newton-4.00/sdk/dCollision/ndShape.h @@ -207,7 +207,6 @@ class ndShapeInfo } D_GCC_NEWTON_ALIGN_32; D_MSV_NEWTON_ALIGN_32 -//class ndShape: public ndClassAlloc class ndShape: public ndContainersFreeListAlloc { public: diff --git a/newton-4.00/sdk/dExtensions/dAvx2/ndDynamicsUpdateAvx2.cpp b/newton-4.00/sdk/dExtensions/dAvx2/ndDynamicsUpdateAvx2.cpp index 9245a9051c..15a61d8cec 100644 --- a/newton-4.00/sdk/dExtensions/dAvx2/ndDynamicsUpdateAvx2.cpp +++ b/newton-4.00/sdk/dExtensions/dAvx2/ndDynamicsUpdateAvx2.cpp @@ -692,7 +692,7 @@ void ndDynamicsUpdateAvx2::SortJoints() public: void SetRowsCount() { - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); const ndInt32 count = jointArray.GetCount(); ndInt32 rowCount = 1; @@ -710,7 +710,7 @@ void ndDynamicsUpdateAvx2::SortJoints() { ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateAvx2* const me = (ndDynamicsUpdateAvx2*)world->m_solver; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); ndInt32 soaJointRowCount = 0; ndArray& soaJointRows = me->m_avxJointRows; @@ -755,7 +755,7 @@ void ndDynamicsUpdateAvx2::SortJoints() } ndScene* const scene = m_world->GetScene(); - ndConstraintArray& jointArray = scene->GetActiveContactArray(); + ndArray& jointArray = scene->GetActiveContactArray(); #ifdef _DEBUG for (ndInt32 i = 1; i < m_activeJointCount; ++i) @@ -1768,7 +1768,7 @@ void ndDynamicsUpdateAvx2::UpdateForceFeedback() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateAvx2* const me = (ndDynamicsUpdateAvx2*)world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndArray& rightHandSide = me->m_rightHandSide; const ndInt32 threadIndex = GetThreadId(); @@ -1910,7 +1910,7 @@ void ndDynamicsUpdateAvx2::CalculateJointsAcceleration() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateAvx2* const me = (ndDynamicsUpdateAvx2*)world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndJointAccelerationDecriptor joindDesc; joindDesc.m_timestep = me->m_timestepRK; @@ -1943,7 +1943,7 @@ void ndDynamicsUpdateAvx2::CalculateJointsAcceleration() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateAvx2* const me = (ndDynamicsUpdateAvx2*)world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); const ndArray& rightHandSide = me->m_rightHandSide; const ndInt32 threadIndex = GetThreadId(); @@ -2295,7 +2295,7 @@ void ndDynamicsUpdateAvx2::CalculateJointsForce() a = a & (f < upperFrictionForce) & (f > lowerFrictionForce); maxAccel = maxAccel.MulAdd(a, a); #endif - + f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce); normalForce[j + 1] = f; @@ -2476,7 +2476,7 @@ void ndDynamicsUpdateAvx2::CalculateJointsForce() m_internalForces = &me->GetInternalForces()[0]; m_jointPartialForces = &me->GetTempInternalForces()[0]; m_jointBodyPairIndexBuffer = &me->GetJointBodyPairIndexBuffer()[0]; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); const ndInt32* const soaJointRows = &me->m_avxJointRows[0]; dAvxMatrixArray& soaMassMatrixArray = *me->m_avxMassMatrixArray; diff --git a/newton-4.00/sdk/dNewton/ndDynamicsUpdate.cpp b/newton-4.00/sdk/dNewton/ndDynamicsUpdate.cpp index bfa5ff7e18..68d3fa4834 100644 --- a/newton-4.00/sdk/dNewton/ndDynamicsUpdate.cpp +++ b/newton-4.00/sdk/dNewton/ndDynamicsUpdate.cpp @@ -95,7 +95,7 @@ void ndDynamicsUpdate::SortBodyJointScan() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdate* const me = world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndJointBodyPairIndex* const jointBodyBuffer = &me->GetJointBodyPairIndexBuffer()[0]; const ndStartEnd startEnd(jointArray.GetCount(), GetThreadId(), m_owner->GetThreadCount()); @@ -117,7 +117,7 @@ void ndDynamicsUpdate::SortBodyJointScan() }; ndScene* const scene = m_world->GetScene(); - ndConstraintArray& jointArray = scene->GetActiveContactArray(); + ndArray& jointArray = scene->GetActiveContactArray(); ndArray& bodyJointPairs = GetJointBodyPairIndexBuffer(); bodyJointPairs.SetCount(jointArray.GetCount() * 2); @@ -179,7 +179,7 @@ void ndDynamicsUpdate::SortJointsScan() virtual void Execute() { D_TRACKTIME(); - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); const ndStartEnd startEnd(jointArray.GetCount(), GetThreadId(), m_owner->GetThreadCount()); for (ndInt32 i = startEnd.m_start; i < startEnd.m_end; ++i) @@ -215,7 +215,7 @@ void ndDynamicsUpdate::SortJointsScan() virtual void Execute() { D_TRACKTIME(); - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); ndInt32 activeJointCount = 0; @@ -250,7 +250,7 @@ void ndDynamicsUpdate::SortJointsScan() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdate* const me = world->m_solver; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); ndInt32* const histogram = ((ndInt32*)m_context) + 2 * GetThreadId(); ndConstraint** const sortBuffer = (ndConstraint**)me->GetTempBuffer(); @@ -278,7 +278,7 @@ void ndDynamicsUpdate::SortJointsScan() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdate* const me = world->m_solver; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); ndInt32* const histogram = ((ndInt32*)m_context) + 2 * GetThreadId(); ndConstraint** const sortBuffer = (ndConstraint**)me->GetTempBuffer(); @@ -338,7 +338,7 @@ void ndDynamicsUpdate::SortJointsScan() } const ndJointList& jointList = m_world->GetJointList(); - ndConstraintArray& jointArray = scene->GetActiveContactArray(); + ndArray& jointArray = scene->GetActiveContactArray(); ndInt32 jointCount = jointArray.GetCount(); jointArray.SetCount(jointCount + jointList.GetCount()); @@ -467,7 +467,7 @@ void ndDynamicsUpdate::SortJoints() } ndScene* const scene = m_world->GetScene(); - ndConstraintArray& jointArray = scene->GetActiveContactArray(); + ndArray& jointArray = scene->GetActiveContactArray(); ndInt32 rowCount = 1; for (ndInt32 i = 0; i < jointArray.GetCount(); ++i) @@ -1108,7 +1108,7 @@ void ndDynamicsUpdate::CalculateJointsAcceleration() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdate* const me = world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndJointAccelerationDecriptor joindDesc; joindDesc.m_timestep = me->m_timestepRK; @@ -1201,7 +1201,7 @@ void ndDynamicsUpdate::UpdateForceFeedback() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdate* const me = world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndArray& rightHandSide = me->m_rightHandSide; const ndFloat32 timestepRK = me->GetTimestepRK(); @@ -1783,7 +1783,7 @@ void ndDynamicsUpdate::CalculateJointsForce() m_internalForces = &me->GetInternalForces()[0]; m_jointPartialForces = &me->GetTempInternalForces()[0]; m_jointBodyPairIndexBuffer = &me->GetJointBodyPairIndexBuffer()[0]; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); const ndInt32 threadIndex = GetThreadId(); const ndInt32 jointCount = jointArray.GetCount(); diff --git a/newton-4.00/sdk/dNewton/ndDynamicsUpdateSoa.cpp b/newton-4.00/sdk/dNewton/ndDynamicsUpdateSoa.cpp index 92b412d6ac..c8a2b0d665 100644 --- a/newton-4.00/sdk/dNewton/ndDynamicsUpdateSoa.cpp +++ b/newton-4.00/sdk/dNewton/ndDynamicsUpdateSoa.cpp @@ -316,7 +316,7 @@ void ndDynamicsUpdateSoa::SortJoints() public: void SetRowsCount() { - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); const ndInt32 count = jointArray.GetCount(); ndInt32 rowCount = 1; @@ -334,7 +334,7 @@ void ndDynamicsUpdateSoa::SortJoints() { ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateSoa* const me = (ndDynamicsUpdateSoa*)world->m_solver; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); ndInt32 soaJointRowCount = 0; ndArray& soaJointRows = me->m_soaJointRows; @@ -379,7 +379,7 @@ void ndDynamicsUpdateSoa::SortJoints() } ndScene* const scene = m_world->GetScene(); - ndConstraintArray& jointArray = scene->GetActiveContactArray(); + ndArray& jointArray = scene->GetActiveContactArray(); const ndInt32 mask = -ndInt32(D_SSE_WORK_GROUP); const ndInt32 jointCount = jointArray.GetCount(); @@ -1353,7 +1353,7 @@ void ndDynamicsUpdateSoa::UpdateForceFeedback() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateSoa* const me = (ndDynamicsUpdateSoa*)world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndArray& rightHandSide = me->m_rightHandSide; const ndInt32 threadIndex = GetThreadId(); @@ -1498,7 +1498,7 @@ void ndDynamicsUpdateSoa::CalculateJointsAcceleration() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateSoa* const me = (ndDynamicsUpdateSoa*)world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); ndJointAccelerationDecriptor joindDesc; joindDesc.m_timestep = me->m_timestepRK; @@ -1531,7 +1531,7 @@ void ndDynamicsUpdateSoa::CalculateJointsAcceleration() D_TRACKTIME(); ndWorld* const world = m_owner->GetWorld(); ndDynamicsUpdateSoa* const me = (ndDynamicsUpdateSoa*)world->m_solver; - const ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + const ndArray& jointArray = m_owner->GetActiveContactArray(); const ndArray& rightHandSide = me->m_rightHandSide; const ndInt32 threadIndex = GetThreadId(); @@ -2033,7 +2033,7 @@ void ndDynamicsUpdateSoa::CalculateJointsForce() m_internalForces = &me->GetInternalForces()[0]; m_jointPartialForces = &me->GetTempInternalForces()[0]; m_jointBodyPairIndexBuffer = &me->GetJointBodyPairIndexBuffer()[0]; - ndConstraintArray& jointArray = m_owner->GetActiveContactArray(); + ndArray& jointArray = m_owner->GetActiveContactArray(); const ndInt32* const soaJointRows = &me->m_soaJointRows[0]; ndSoaMatrixElement* const soaMassMatrix = &me->m_soaMassMatrix[0]; diff --git a/newton-4.00/sdk/dNewton/ndNewton.h b/newton-4.00/sdk/dNewton/ndNewton.h index 1ec99b4397..f309242d74 100644 --- a/newton-4.00/sdk/dNewton/ndNewton.h +++ b/newton-4.00/sdk/dNewton/ndNewton.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/newton-4.00/sdk/dNewton/ndSkeletonContainer.cpp b/newton-4.00/sdk/dNewton/ndSkeletonContainer.cpp index e1c3ea654f..d79deffa2f 100644 --- a/newton-4.00/sdk/dNewton/ndSkeletonContainer.cpp +++ b/newton-4.00/sdk/dNewton/ndSkeletonContainer.cpp @@ -471,7 +471,7 @@ void ndSkeletonContainer::ClearSelfCollision() void ndSkeletonContainer::AddSelfCollisionJoint(ndConstraint* const joint) { - ndScopeSpinLock lock(joint->GetBody0()->GetScene()->m_contactLock); + ndScopeSpinLock lock(joint->GetBody0()->GetScene()->m_lock); if (m_loopingJoints.GetCount() < (m_loopCount + m_dynamicsLoopCount + 1)) { m_loopingJoints.SetCount(2 * (m_loopCount + m_dynamicsLoopCount + 1)); diff --git a/newton-4.00/sdk/dNewton/ndWorld.cpp b/newton-4.00/sdk/dNewton/ndWorld.cpp index 6c476a30b5..f4d3eb1e91 100644 --- a/newton-4.00/sdk/dNewton/ndWorld.cpp +++ b/newton-4.00/sdk/dNewton/ndWorld.cpp @@ -192,20 +192,8 @@ void ndWorld::CleanUp() delete body; } - const ndBodyList& bodyList = GetBodyList(); - while (bodyList.GetFirst()) - { - ndBodyKinematic* const body = bodyList.GetFirst()->GetInfo(); - RemoveBody(body); - delete body; - } - - m_scene->m_contactList.DeleteAllContacts(); - m_scene->m_scrathContactList.SetCount(0); - m_scene->m_activeConstraintArray.SetCount(0); - ndBody::m_uniqueIdCount = 1; - dAssert(!m_scene->GetContactList().GetCount()); + m_scene->Cleanup(); } void ndWorld::SelectSolver(ndSolverModes solverMode) @@ -269,33 +257,6 @@ void ndWorld::UpdateTransforms() m_scene->UpdateTransform(); } -void ndWorld::ApplyExternalForces() -{ - D_TRACKTIME(); - class ndApplyExternalForces: public ndScene::ndBaseJob - { - public: - virtual void Execute() - { - D_TRACKTIME(); - const ndArray& bodyArray = m_owner->GetActiveBodyArray(); - - const ndFloat32 timestep = m_timestep; - const ndInt32 threadIndex = GetThreadId(); - const ndStartEnd startEnd(bodyArray.GetCount() - 1, GetThreadId(), m_owner->GetThreadCount()); - for (ndInt32 i = startEnd.m_start; i < startEnd.m_end; ++i) - { - ndBodyDynamic* const body = bodyArray[i]->GetAsBodyDynamic(); - if (body) - { - body->ApplyExternalForces(threadIndex, timestep); - } - } - } - }; - m_scene->SubmitJobs(); -} - void ndWorld::PostUpdate(ndFloat32) { D_TRACKTIME(); @@ -519,7 +480,7 @@ void ndWorld::SubStepUpdate(ndFloat32 timestep) m_scene->SetTimestep(timestep); UpdateSkeletons(); - m_scene->BuildBodyArray(); + m_scene->InitBodyArray(); ndBodyKinematic* sentinelBody = m_sentinelBody; sentinelBody->PrepareStep(m_scene->GetActiveBodyArray().GetCount()); @@ -533,17 +494,13 @@ void ndWorld::SubStepUpdate(ndFloat32 timestep) sentinelBody->m_isJointFence1 = 1; sentinelBody->m_bodyIsConstrained = 0; sentinelBody->m_weigh = ndFloat32(0.0f); - m_scene->GetActiveBodyArray().PushBack(sentinelBody); - ApplyExternalForces(); - // update the collision system - m_scene->UpdateAabb(); m_scene->FindCollidingPairs(); m_scene->CalculateContacts(); - // update all special models, particle, models, ect. + // update all special bodies. m_scene->UpdateSpecial(); // Update Particle base physics @@ -701,7 +658,7 @@ void ndWorld::UpdateSkeletons() m_skeletonList.Remove(m_skeletonList.GetFirst()); } - m_scene->BuildBodyArray(); + m_scene->InitBodyArray(); // build connectivity graph and reset of all joint dirty state ndDynamicsUpdate& solverUpdate = *m_solver; diff --git a/newton-4.00/sdk/dNewton/ndWorld.h b/newton-4.00/sdk/dNewton/ndWorld.h index b09f2e2ee2..3e6556064c 100644 --- a/newton-4.00/sdk/dNewton/ndWorld.h +++ b/newton-4.00/sdk/dNewton/ndWorld.h @@ -93,7 +93,7 @@ class ndWorld: public ndClassAlloc const ndBodyList& GetBodyList() const; const ndJointList& GetJointList() const; const ndModelList& GetModelList() const; - const ndContactList& GetContactList() const; + const ndContactArray& GetContactList() const; const ndSkeletonList& GetSkeletonList() const; const ndBodyParticleSetList& GetParticleList() const; @@ -127,7 +127,6 @@ class ndWorld: public ndClassAlloc D_NEWTON_API virtual void UpdateSkeletons(); D_NEWTON_API virtual void UpdateTransforms(); D_NEWTON_API virtual void PostModelTransform(); - D_NEWTON_API virtual void ApplyExternalForces(); private: class dgSolverProgressiveSleepEntry @@ -258,9 +257,9 @@ inline const ndJointList& ndWorld::GetJointList() const return m_jointList; } -inline const ndContactList& ndWorld::GetContactList() const +inline const ndContactArray& ndWorld::GetContactList() const { - return m_scene->GetContactList(); + return m_scene->GetContactArray(); } inline const ndSkeletonList& ndWorld::GetSkeletonList() const