1151 lines
28 KiB
C++
1151 lines
28 KiB
C++
/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
*
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely, subject to the following restrictions:
|
|
*
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
|
* claim that you wrote the original software. If you use this software
|
|
* in a product, an acknowledgment in the product documentation would be
|
|
* appreciated but is not required.
|
|
*
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
|
* misrepresented as being the original software.
|
|
*
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "dgWorld.h"
|
|
#include "hpl1/debug.h"
|
|
#include "hpl1/engine/libraries/newton/core/dg.h"
|
|
|
|
#include "dgCollisionBox.h"
|
|
#include "dgCollisionCapsule.h"
|
|
#include "dgCollisionChamferCylinder.h"
|
|
#include "dgCollisionCompound.h"
|
|
#include "dgCollisionCone.h"
|
|
#include "dgCollisionConvexHull.h"
|
|
#include "dgCollisionConvexModifier.h"
|
|
#include "dgCollisionCylinder.h"
|
|
#include "dgCollisionEllipse.h"
|
|
#include "dgCollisionNull.h"
|
|
#include "dgCollisionScene.h"
|
|
#include "dgCollisionSphere.h"
|
|
#include "dgMinkowskiConv.h"
|
|
#include "dgWorldDynamicUpdate.h"
|
|
|
|
|
|
#include "dgBallConstraint.h"
|
|
#include "dgConnectorConstraint.h"
|
|
#include "dgCorkscrewConstraint.h"
|
|
#include "dgHingeConstraint.h"
|
|
#include "dgSlidingConstraint.h"
|
|
#include "dgUniversalConstraint.h"
|
|
#include "dgUpVectorConstraint.h"
|
|
#include "dgUserConstraint.h"
|
|
|
|
//#include "dgPointToCurveConstraint.h"
|
|
|
|
#define DG_INITIAL_ISLAND_SIZE (1024 * 4)
|
|
#define DG_INITIAL_BODIES_SIZE (1024 * 4)
|
|
#define DG_INITIAL_JOINTS_SIZE (1024 * 4)
|
|
#define DG_INITIAL_JACOBIAN_SIZE (1024 * 16)
|
|
#define DG_INITIAL_CONTATCT_SIZE (1024 * 64)
|
|
|
|
/*
|
|
static char *xxx[10] = {"bbbbbbbbbb",
|
|
"baaaaaaaab",
|
|
"babbbbbaab",
|
|
"babaaabaab",
|
|
"baaababaab",
|
|
"bbbaaabaab",
|
|
"bbbbaabaab",
|
|
"babbbbbaab",
|
|
"baaaaaaaab",
|
|
"bbbbbbbbbb"};
|
|
|
|
struct myPath: public dgPathFinder<dgInt32, float>
|
|
{
|
|
dgInt32 goalx;
|
|
dgInt32 goaly;
|
|
myPath ()
|
|
:dgPathFinder<dgInt32, float>(1024, 10)
|
|
{
|
|
for (int i = 0; i < 10; i ++) {
|
|
strcpy (&m_map[i][0], xxx[i]);
|
|
}
|
|
}
|
|
|
|
const dgPathNode<dgInt32, float>* CalCulatePath (dgInt32 source, dgInt32 goal)
|
|
{
|
|
goalx = goal % 10;
|
|
goaly = goal / 10;
|
|
return dgPathFinder<dgInt32, float>::CalCulatePath (source, goal) ;
|
|
}
|
|
|
|
|
|
float GetCostFromParent(const dgPathNode<dgInt32, float>& node) const
|
|
{
|
|
dgInt32 x;
|
|
dgInt32 y;
|
|
dgInt32 x0;
|
|
dgInt32 y0;
|
|
dgInt32 x1;
|
|
dgInt32 y1;
|
|
dgInt32 id;
|
|
|
|
id = node.GetId();
|
|
x = id % 10;
|
|
y = id / 10;
|
|
|
|
const dgPathNode<dgInt32, float>* parent = node.GetParent();
|
|
id = parent->GetId();
|
|
x0 = id % 10;
|
|
y0 = id / 10;
|
|
|
|
const dgPathNode<dgInt32, float>* grandParent = parent->GetParent();
|
|
x1 = 2 * x0 - x;
|
|
y1 = 2 * y0 - y;
|
|
if (grandParent) {
|
|
id = grandParent->GetId();
|
|
x1 = id % 10;
|
|
y1 = id / 10;
|
|
}
|
|
|
|
dgInt32 dx0;
|
|
dgInt32 dy0;
|
|
dgInt32 dx1;
|
|
dgInt32 dy1;
|
|
float penalty;
|
|
|
|
dx0 = x0 - x;
|
|
dy0 = y0 - y;
|
|
dx1 = x1 - x0;
|
|
dy1 = y1 - y0;
|
|
penalty = 0.0f;
|
|
if (dx1 * dy0 - dx0 * dy1) {
|
|
penalty = dgFloat32(1.0f);
|
|
}
|
|
|
|
static dgInt32 xxxx;
|
|
if (!xxxx){
|
|
xxxx = 1;
|
|
penalty = 9.1f;
|
|
}
|
|
|
|
return (xxx[y][x] == 'a') ? (dgFloat32(1.0f) + penalty): 50.0f;
|
|
}
|
|
|
|
float GetEstimatedCostToGoal(dgInt32 id) const
|
|
{
|
|
dgInt32 x;
|
|
dgInt32 y;
|
|
|
|
x = id % 10 - goalx;
|
|
y = id / 10 - goaly;
|
|
return dgSqrt ((float)(x * x + y * y));
|
|
}
|
|
|
|
dgInt32 EnumerateChildren(dgInt32 parent, dgInt32 array[]) const
|
|
{
|
|
dgInt32 x;
|
|
dgInt32 y;
|
|
|
|
x = parent % 10;
|
|
y = parent / 10;
|
|
|
|
array[0] = (y - 1) * 10 + x;
|
|
array[1] = (y - 0) * 10 + x - 1;
|
|
array[2] = (y + 1) * 10 + x;
|
|
array[3] = (y + 0) * 10 + x + 1;
|
|
return 4;
|
|
}
|
|
|
|
char m_map[20][20];
|
|
};
|
|
|
|
|
|
void xxxxx()
|
|
{
|
|
myPath path;
|
|
const dgPathNode<dgInt32, float>* firtNode;
|
|
for (firtNode = path.CalCulatePath (5 * 10 + 3, 4 * 10 + 8); firtNode; firtNode= firtNode->GetNext()) {
|
|
dgInt32 id;
|
|
dgInt32 x;
|
|
dgInt32 y;
|
|
|
|
id = firtNode->GetId();
|
|
x = id % 10;
|
|
y = id / 10;
|
|
path.m_map[y][x] = '_';
|
|
}
|
|
}
|
|
*/
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// Construction/Destruction
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
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;
|
|
m_maxTheads = 1;
|
|
|
|
dgBroadPhaseCollision::Init();
|
|
dgCollidingPairCollector::Init();
|
|
|
|
m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
|
|
AddSentinelBody();
|
|
SetPerfomanceCounter(NULL);
|
|
}
|
|
|
|
dgWorld::~dgWorld() {
|
|
DestroyAllBodies();
|
|
RemoveAllGroupID();
|
|
m_destroyCollision = NULL;
|
|
ReleaseCollision(m_pointCollision);
|
|
DestroyBody(m_sentionelBody);
|
|
|
|
m_allocator->FreeLow(m_jointsMemory);
|
|
m_allocator->FreeLow(m_bodiesMemory);
|
|
m_allocator->FreeLow(m_islandMemory);
|
|
m_allocator->FreeLow(m_pairMemoryBuffer);
|
|
for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++) {
|
|
m_allocator->FreeLow(m_jacobiansMemory[i]);
|
|
m_allocator->FreeLow(m_internalForcesMemory[i]);
|
|
m_allocator->FreeLow(m_contactBuffers[i]);
|
|
}
|
|
}
|
|
|
|
dgUnsigned32 dgWorld::GetPerformanceCount() {
|
|
return 0;
|
|
}
|
|
|
|
void dgWorld::AddSentinelBody() {
|
|
dgCollision *collision;
|
|
|
|
collision = new (m_allocator) dgCollisionNull(m_allocator, 0x4352fe67);
|
|
m_sentionelBody = CreateBody(collision, dgGetIdentityMatrix());
|
|
ReleaseCollision(collision);
|
|
|
|
// dgBodyMasterList::m_sentinel = m_sentionelBody;
|
|
dgCollidingPairCollector::m_sentinel = m_sentionelBody;
|
|
}
|
|
|
|
dgBody *dgWorld::GetSentinelBody() const {
|
|
return m_sentionelBody;
|
|
}
|
|
|
|
void dgWorld::SetSolverMode(dgInt32 mode) {
|
|
m_solverMode = dgUnsigned32(GetMax(dgInt32(0), mode));
|
|
}
|
|
|
|
void dgWorld::SetFrictionMode(dgInt32 mode) {
|
|
m_frictionMode = dgUnsigned32(mode);
|
|
}
|
|
|
|
void dgWorld::SetHardwareMode(dgInt32 mode) {
|
|
HPL1_UNIMPLEMENTED(dgWorld::SetHardwareMode);
|
|
}
|
|
|
|
dgInt32 dgWorld::GetHardwareMode(char *description) const {
|
|
dgInt32 mode;
|
|
|
|
// if (0) {
|
|
// } else {
|
|
if (m_cpu == dgNoSimdPresent) {
|
|
mode = 0;
|
|
if (description) {
|
|
snprintf(description, 5, "x87");
|
|
}
|
|
} else {
|
|
mode = 1;
|
|
if (description) {
|
|
snprintf(description, 5, "simd");
|
|
}
|
|
}
|
|
// }
|
|
return mode;
|
|
}
|
|
|
|
void dgWorld::SetThreadsCount(dgInt32 count) {
|
|
// count = 1;
|
|
|
|
m_threadsManager.CreateThreaded(count);
|
|
m_numberOfTheads = dgUnsigned32(m_threadsManager.GetThreadCount());
|
|
}
|
|
|
|
dgInt32 dgWorld::GetThreadsCount() const {
|
|
return dgInt32(m_numberOfTheads);
|
|
}
|
|
|
|
dgInt32 dgWorld::GetMaxThreadsCount() const {
|
|
return dgInt32(m_maxTheads);
|
|
}
|
|
|
|
void dgWorld::EnableThreadOnSingleIsland(dgInt32 mode) {
|
|
// for now disable this option
|
|
// m_singleIslandMultithreading = 1;
|
|
m_singleIslandMultithreading = mode ? 1 : 0;
|
|
}
|
|
|
|
dgInt32 dgWorld::GetThreadOnSingleIsland() const {
|
|
return m_singleIslandMultithreading ? 1 : 0;
|
|
}
|
|
|
|
void dgWorld::SetFrictionThreshold(dgFloat32 acceleration) {
|
|
m_frictiomTheshold = GetMax(dgFloat32(1.0e-2f), acceleration);
|
|
}
|
|
|
|
void dgWorld::RemoveAllGroupID() {
|
|
while (dgBodyMaterialList::GetCount()) {
|
|
dgBodyMaterialList::Remove(dgBodyMaterialList::GetRoot());
|
|
}
|
|
m_bodyGroupID = 0;
|
|
m_defualtBodyGroupID = CreateBodyGroupID();
|
|
}
|
|
|
|
void dgWorld::DestroyAllBodies() {
|
|
dgBody *body;
|
|
dgBodyMasterList::dgListNode *node;
|
|
|
|
dgBodyMasterList &me = *this;
|
|
// dgBodyMasterList::Iterator iter(me);
|
|
// for (iter.Begin(); iter; ) {
|
|
|
|
NEWTON_ASSERT(
|
|
dgBodyMasterList::GetFirst()->GetInfo().GetBody() == m_sentionelBody);
|
|
for (node = me.GetFirst()->GetNext(); node;) {
|
|
body = node->GetInfo().GetBody();
|
|
node = node->GetNext();
|
|
DestroyBody(body);
|
|
}
|
|
|
|
NEWTON_ASSERT(me.GetFirst()->GetInfo().GetCount() == 0);
|
|
NEWTON_ASSERT(dgBodyCollisionList::GetCount() == 0);
|
|
}
|
|
|
|
dgBody *dgWorld::CreateBody(dgCollision *const collision,
|
|
const dgMatrix &matrix) {
|
|
dgBody *body;
|
|
|
|
NEWTON_ASSERT(collision);
|
|
|
|
body = new (m_allocator) dgBody();
|
|
NEWTON_ASSERT((sizeof(dgBody) & 0xf) == 0);
|
|
NEWTON_ASSERT((dgUnsigned64(body) & 0xf) == 0);
|
|
|
|
body->reset();
|
|
|
|
// m_bodiesCount ++;
|
|
m_bodiesUniqueID++;
|
|
|
|
body->m_world = this;
|
|
|
|
body->m_freeze = false;
|
|
body->m_sleeping = false;
|
|
body->m_autoSleep = true;
|
|
body->m_isInWorld = true;
|
|
body->m_equilibrium = false;
|
|
body->m_continueCollisionMode = false;
|
|
body->m_collideWithLinkedBodies = true;
|
|
body->m_solverInContinueCollision = false;
|
|
body->m_spawnnedFromCallback = dgUnsigned32(m_inUpdate ? true : false);
|
|
body->m_uniqueID = dgInt32(m_bodiesUniqueID);
|
|
|
|
dgBodyMasterList::AddBody(body);
|
|
|
|
// dgBodyActiveList___::AddBody(body);
|
|
// NEWTON_ASSERT (body->m_activeNode);
|
|
|
|
// body->m_freezeAccel2 = m_freezeAccel2;
|
|
// body->m_freezeAlpha2 = m_freezeAlpha2;
|
|
// body->m_freezeSpeed2 = m_freezeSpeed2;
|
|
// body->m_freezeOmega2 = m_freezeOmega2;
|
|
|
|
body->SetCentreOfMass(
|
|
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
|
|
dgFloat32(1.0f)));
|
|
body->SetLinearDamping(dgFloat32(0.1045f));
|
|
body->SetAngularDamping(
|
|
dgVector(dgFloat32(0.1045f), dgFloat32(0.1045f), dgFloat32(0.1045f),
|
|
dgFloat32(0.0f)));
|
|
|
|
body->AttachCollision(collision);
|
|
body->m_bodyGroupId = dgInt32(m_defualtBodyGroupID);
|
|
|
|
body->SetMassMatrix(DG_INFINITE_MASS * dgFloat32(2.0f), DG_INFINITE_MASS,
|
|
DG_INFINITE_MASS, DG_INFINITE_MASS);
|
|
dgBroadPhaseCollision::Add(body);
|
|
|
|
// body->SetMatrix (dgGetIdentityMatrix());
|
|
body->SetMatrix(matrix);
|
|
body->m_invWorldInertiaMatrix[3][3] = dgFloat32(1.0f);
|
|
return body;
|
|
}
|
|
|
|
void dgWorld::DestroyBody(dgBody *const body) {
|
|
if (body->m_destructor) {
|
|
body->m_destructor(reinterpret_cast<const NewtonBody *>(body));
|
|
}
|
|
|
|
dgBroadPhaseCollision::Remove(body);
|
|
|
|
// m_bodiesCount --;
|
|
// while (body->m_firstConstraintLink) {
|
|
// DestroyConstraint (body->m_firstConstraintLink->m_constraint);
|
|
// }
|
|
dgBodyMasterList::RemoveBody(body);
|
|
|
|
NEWTON_ASSERT(body->m_collision);
|
|
ReleaseCollision(body->m_collision);
|
|
|
|
delete body;
|
|
}
|
|
|
|
void dgWorld::DestroyConstraint(dgConstraint *const constraint) {
|
|
RemoveConstraint(constraint);
|
|
delete constraint;
|
|
}
|
|
|
|
/*
|
|
void dgWorld::UnfreezeBody (dgBody *body)
|
|
{
|
|
if (body->m_activation && !body->m_active) {
|
|
body->m_activation (*body, 1);
|
|
}
|
|
body->m_active = true;
|
|
|
|
}
|
|
|
|
void dgWorld::FreezeBody (dgBody *body)
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
if (body->m_activeNode) {
|
|
NEWTON_ASSERT (0);
|
|
dgBodyActiveList___::RemoveBody(body);
|
|
if (body->m_activation && body->m_active) {
|
|
body->m_activation (*body, 0);
|
|
}
|
|
body->m_active = false;
|
|
}
|
|
}
|
|
*/
|
|
|
|
void dgWorld::UpdateCollision() {
|
|
dgFloat32 timestep;
|
|
NEWTON_ASSERT(m_inUpdate == 0);
|
|
|
|
m_threadsManager.ClearTimers();
|
|
memset(m_perfomanceCounters, 0, sizeof(m_perfomanceCounters));
|
|
dgUnsigned32 ticks = m_getPerformanceCount();
|
|
|
|
m_inUpdate++;
|
|
NEWTON_ASSERT(m_numberOfTheads >= 1);
|
|
|
|
|
|
timestep = dgFloat32(0.0f);
|
|
|
|
if (m_cpu == dgSimdPresent) {
|
|
#ifdef DG_BUILD_SIMD_CODE
|
|
simd_env rounding;
|
|
rounding = simd_get_ctrl();
|
|
simd_set_FZ_mode();
|
|
|
|
UpdateContactsSimd(timestep, true);
|
|
simd_set_ctrl(rounding);
|
|
#endif
|
|
|
|
} else {
|
|
UpdateContacts(timestep, true);
|
|
}
|
|
m_inUpdate--;
|
|
|
|
m_perfomanceCounters[m_worldTicks] = m_getPerformanceCount() - ticks;
|
|
}
|
|
|
|
void dgWorld::Update(dgFloat32 timestep) {
|
|
dgUnsigned32 ticks;
|
|
// timestep = 1.0f/ 60.0f;
|
|
// timestep = 1.0f/ 120.0f;
|
|
// timestep = 1.0f/ 180.0f;
|
|
// timestep = 1.0f/ 240.0f;
|
|
// timestep = 1.0f/ 300.0f;
|
|
// timestep = 1.0f/ 600.0f;
|
|
// timestep = 1.0f/ 1000.0f;
|
|
|
|
// m_cpu = dgNoSimdPresent;
|
|
// m_cpu = dgSimdPresent;
|
|
// m_solverMode = 1;
|
|
|
|
// xxxxx();
|
|
|
|
// static int xxx;
|
|
// dgTrace (("pass %d\n", xxx));
|
|
// xxx ++;
|
|
|
|
// m_cpu = dgNoSimdPresent;
|
|
|
|
NEWTON_ASSERT(m_inUpdate == 0);
|
|
|
|
m_threadsManager.ClearTimers();
|
|
memset(m_perfomanceCounters, 0, sizeof(m_perfomanceCounters));
|
|
|
|
ticks = m_getPerformanceCount();
|
|
|
|
m_destroyeddBodiesPool.m_count = 0;
|
|
|
|
m_inUpdate++;
|
|
NEWTON_ASSERT(m_numberOfTheads >= 1);
|
|
|
|
if (m_cpu == dgSimdPresent) {
|
|
#ifdef DG_BUILD_SIMD_CODE
|
|
simd_env rounding = simd_get_ctrl();
|
|
simd_set_FZ_mode();
|
|
|
|
UpdateContactsSimd(timestep, false);
|
|
m_dynamicSolver.UpdateDynamics(this, 1, timestep);
|
|
|
|
simd_set_ctrl(rounding);
|
|
#endif
|
|
|
|
} else {
|
|
UpdateContacts(timestep, false);
|
|
m_dynamicSolver.UpdateDynamics(this, 0, timestep);
|
|
}
|
|
m_inUpdate--;
|
|
|
|
if (m_destroyBodyByExeciveForce) {
|
|
for (dgInt32 i = 0; i < m_destroyeddBodiesPool.m_count; i++) {
|
|
m_destroyBodyByExeciveForce(m_destroyeddBodiesPool.m_bodies[i],
|
|
m_destroyeddBodiesPool.m_joint[i]);
|
|
}
|
|
}
|
|
|
|
m_perfomanceCounters[m_worldTicks] = m_getPerformanceCount() - ticks;
|
|
}
|
|
|
|
OnGetPerformanceCountCallback dgWorld::GetPerformaceFuntion() const {
|
|
return m_getPerformanceCount;
|
|
}
|
|
|
|
void dgWorld::SetPerfomanceCounter(OnGetPerformanceCountCallback callback) {
|
|
m_threadsManager.SetPerfomanceCounter(callback);
|
|
|
|
if (!callback) {
|
|
callback = GetPerformanceCount;
|
|
}
|
|
m_getPerformanceCount = callback;
|
|
memset(m_perfomanceCounters, 0, sizeof(m_perfomanceCounters));
|
|
}
|
|
|
|
// dgUnsigned32 dgWorld::GetPerfomanceTicks (dgInt32 thread, dgUnsigned32 entry) const
|
|
dgUnsigned32 dgWorld::GetPerfomanceTicks(dgUnsigned32 entry) const {
|
|
entry = ClampValue(dgUnsigned32(entry), dgUnsigned32(0),
|
|
dgUnsigned32(m_counterSize - 1));
|
|
return m_perfomanceCounters[entry];
|
|
}
|
|
|
|
dgUnsigned32 dgWorld::GetThreadPerfomanceTicks(dgUnsigned32 threadIndex) const {
|
|
return m_threadsManager.GetPerfomanceTicks(threadIndex);
|
|
}
|
|
|
|
void dgWorld::SetUserData(void *const userData) {
|
|
m_userData = userData;
|
|
}
|
|
|
|
void *dgWorld::GetUserData() const {
|
|
return m_userData;
|
|
}
|
|
|
|
void dgWorld::SetIslandUpdateCallback(OnIslandUpdate callback) {
|
|
m_islandUpdate = callback;
|
|
}
|
|
|
|
void dgWorld::SetDestroyCollisionCallback(OnDestroyCollision callback) {
|
|
m_destroyCollision = callback;
|
|
}
|
|
|
|
void dgWorld::SetLeavingWorldCallback(OnLeavingWorldAction callback) {
|
|
m_leavingWorldNotify = callback;
|
|
}
|
|
|
|
void dgWorld::SetBodyDestructionByExeciveForce(
|
|
OnBodyDestructionByExeciveForce callback) {
|
|
m_destroyBodyByExeciveForce = callback;
|
|
}
|
|
|
|
dgBallConstraint *dgWorld::CreateBallConstraint(const dgVector &pivot,
|
|
dgBody *const body0, dgBody *const body1) {
|
|
dgBallConstraint *constraint;
|
|
|
|
NEWTON_ASSERT(body0);
|
|
NEWTON_ASSERT(body0 != body1);
|
|
// constraint = dgBallConstraint::Create(this);
|
|
constraint = new (m_allocator) dgBallConstraint;
|
|
|
|
AttachConstraint(constraint, body0, body1);
|
|
constraint->SetPivotPoint(pivot);
|
|
return constraint;
|
|
}
|
|
|
|
dgHingeConstraint *dgWorld::CreateHingeConstraint(const dgVector &pivot,
|
|
const dgVector &pinDir, dgBody *const body0, dgBody *const body1) {
|
|
dgHingeConstraint *constraint;
|
|
|
|
NEWTON_ASSERT(body0);
|
|
NEWTON_ASSERT(body0 != body1);
|
|
// constraint = dgHingeConstraint::Create(this);
|
|
constraint = new (m_allocator) dgHingeConstraint;
|
|
|
|
AttachConstraint(constraint, body0, body1);
|
|
constraint->SetPivotAndPinDir(pivot, pinDir);
|
|
return constraint;
|
|
}
|
|
|
|
dgUpVectorConstraint *dgWorld::CreateUpVectorConstraint(const dgVector &pin,
|
|
dgBody *body) {
|
|
dgUpVectorConstraint *constraint;
|
|
|
|
NEWTON_ASSERT(body);
|
|
// constraint = dgUpVectorConstraint::Create(this);
|
|
constraint = new (m_allocator) dgUpVectorConstraint;
|
|
|
|
AttachConstraint(constraint, body, NULL);
|
|
constraint->InitPinDir(pin);
|
|
return constraint;
|
|
}
|
|
|
|
dgSlidingConstraint *dgWorld::CreateSlidingConstraint(const dgVector &pivot,
|
|
const dgVector &pinDir, dgBody *const body0, dgBody *const body1) {
|
|
dgSlidingConstraint *constraint;
|
|
|
|
NEWTON_ASSERT(body0);
|
|
NEWTON_ASSERT(body0 != body1);
|
|
// constraint = dgSlidingConstraint::Create(this);
|
|
constraint = new (m_allocator) dgSlidingConstraint;
|
|
|
|
AttachConstraint(constraint, body0, body1);
|
|
constraint->SetPivotAndPinDir(pivot, pinDir);
|
|
return constraint;
|
|
}
|
|
|
|
dgCorkscrewConstraint *dgWorld::CreateCorkscrewConstraint(const dgVector &pivot,
|
|
const dgVector &pinDir, dgBody *const body0, dgBody *const body1) {
|
|
dgCorkscrewConstraint *constraint;
|
|
|
|
NEWTON_ASSERT(body0);
|
|
NEWTON_ASSERT(body0 != body1);
|
|
// constraint = dgCorkscrewConstraint::Create(this);
|
|
constraint = new (m_allocator) dgCorkscrewConstraint;
|
|
|
|
AttachConstraint(constraint, body0, body1);
|
|
constraint->SetPivotAndPinDir(pivot, pinDir);
|
|
return constraint;
|
|
}
|
|
|
|
dgUniversalConstraint *dgWorld::CreateUniversalConstraint(const dgVector &pivot,
|
|
const dgVector &pin0, const dgVector &pin1, dgBody *const body0,
|
|
dgBody *const body1) {
|
|
dgUniversalConstraint *constraint;
|
|
|
|
NEWTON_ASSERT(body0);
|
|
NEWTON_ASSERT(body0 != body1);
|
|
// constraint = dgUniversalConstraint::Create(this);
|
|
constraint = new (m_allocator) dgUniversalConstraint;
|
|
|
|
AttachConstraint(constraint, body0, body1);
|
|
constraint->SetPivotAndPinDir(pivot, pin0, pin1);
|
|
return constraint;
|
|
}
|
|
|
|
/*
|
|
dgPointToCurveConstraint* dgWorld::AttachPointToCurveConstraint (
|
|
const dgVector& pivot,
|
|
dgBody* const body0,
|
|
OnPointToCurveCallback funt,
|
|
void *curveContext)
|
|
{
|
|
dgPointToCurveConstraint *constraint;
|
|
|
|
NEWTON_ASSERT (body0);
|
|
constraint = dgPointToCurveConstraint::Create();
|
|
|
|
AttachConstraint (constraint, body0, NULL);
|
|
constraint->SetPivotPoint (pivot, funt, curveContext);
|
|
return constraint;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
dgConnectorConstraint* dgWorld::CreateConnectorConstraint (dgBody *body0__, dgBody *body1__)
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
return NULL;
|
|
|
|
dgConnectorConstraint *constraint;
|
|
|
|
NEWTON_ASSERT (body0);
|
|
NEWTON_ASSERT (body0 != body1);
|
|
constraint = dgConnectorConstraint::Create();
|
|
|
|
AttachConstraint (constraint, body0, body1);
|
|
constraint->Setup();
|
|
return constraint;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
dgInt32 dgWorld::GetActiveBodiesCount() const
|
|
{
|
|
return m_activeBodiesCount;
|
|
}
|
|
*/
|
|
|
|
dgInt32 dgWorld::GetBodiesCount() const {
|
|
const dgBodyMasterList &list = *this;
|
|
return list.GetCount() - 1;
|
|
}
|
|
|
|
dgInt32 dgWorld::GetConstraintsCount() const {
|
|
const dgBodyMasterList &list = *this;
|
|
return dgInt32(list.m_constraintCount);
|
|
}
|
|
|
|
/*
|
|
dgLink* dgWorld::FindConstraintLink (const dgBody* const body0, const dgBody* const body1) const
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
|
|
dgLink *ptr;
|
|
dgLink *link;
|
|
|
|
NEWTON_ASSERT (body0);
|
|
if (!body0) {
|
|
NEWTON_ASSERT (0);
|
|
Swap (body0, body1);
|
|
}
|
|
|
|
if (body0) {
|
|
link = body0->m_firstConstraintLink;
|
|
if (link) {
|
|
ptr = link;
|
|
do {
|
|
NEWTON_ASSERT (ptr->m_body == body0);
|
|
if (ptr->m_twin->m_body == body1) {
|
|
return ptr;
|
|
}
|
|
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != link);
|
|
}
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
*/
|
|
/*
|
|
dgConstraint* dgWorld::GetConstraint (const dgLink* constraintLink) const
|
|
{
|
|
return constraintLink->m_constraint;
|
|
}
|
|
*/
|
|
|
|
void dgWorld::BodySetMatrix(dgBody *body, const dgMatrix &matrix) {
|
|
#define DG_RECURSIVE_SIZE 1024
|
|
dgInt32 index;
|
|
dgBody *queue[DG_RECURSIVE_SIZE];
|
|
|
|
index = 1;
|
|
queue[0] = body;
|
|
m_genericLRUMark++;
|
|
body->m_genericLRUMark = m_genericLRUMark;
|
|
dgMatrix relMatrix(body->GetMatrix().Inverse() * matrix);
|
|
while (index) {
|
|
dgBody *bodyI;
|
|
|
|
index--;
|
|
bodyI = queue[index];
|
|
NEWTON_ASSERT(bodyI != m_sentionelBody);
|
|
|
|
dgBroadPhaseCollision::Remove(bodyI);
|
|
dgBroadPhaseCollision::Add(bodyI);
|
|
|
|
dgMatrix matrixI(bodyI->GetMatrix() * relMatrix);
|
|
bodyI->SetVelocity(
|
|
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
|
|
dgFloat32(0.0f)));
|
|
bodyI->SetOmega(
|
|
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
|
|
dgFloat32(0.0f)));
|
|
bodyI->SetMatrix(matrixI);
|
|
bodyI->m_isInWorld = true;
|
|
|
|
for (dgBodyMasterListRow::dgListNode *jointNode =
|
|
bodyI->m_masterNode->GetInfo().GetFirst();
|
|
jointNode; jointNode =
|
|
jointNode->GetNext()) {
|
|
dgBodyMasterListCell &cell = jointNode->GetInfo();
|
|
bodyI = cell.m_bodyNode;
|
|
if (bodyI != m_sentionelBody) {
|
|
if (bodyI->m_genericLRUMark != m_genericLRUMark) {
|
|
dgConstraint *constraint;
|
|
constraint = cell.m_joint;
|
|
if (constraint->GetId() != dgContactConstraintId) {
|
|
bodyI->m_genericLRUMark = m_genericLRUMark;
|
|
queue[index] = bodyI;
|
|
index++;
|
|
NEWTON_ASSERT(index < DG_RECURSIVE_SIZE);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void dgWorld::AddBodyImpulse(dgBody *body, const dgVector &pointVeloc,
|
|
const dgVector &pointPosit) {
|
|
if (body->m_invMass.m_w > dgFloat32(0.0f)) {
|
|
// UnfreezeBody (body);
|
|
body->AddImpulse(pointVeloc, pointPosit);
|
|
}
|
|
}
|
|
|
|
void dgWorld::ApplyImpulseArray(dgBody *body, dgInt32 count,
|
|
dgInt32 strideInBytes, const dgFloat32 *const impulseArray,
|
|
const dgFloat32 *const pointArray) {
|
|
if (body->m_invMass.m_w > dgFloat32(0.0f)) {
|
|
// UnfreezeBody (body);
|
|
body->ApplyImpulseArray(count, strideInBytes, impulseArray, pointArray);
|
|
}
|
|
}
|
|
|
|
/*
|
|
dgInt32 dgWorld::GetBodyArray (dgBody* root, dgBody** array, dgInt32 maxSize) const
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
return 0;
|
|
|
|
dgInt32 i;
|
|
dgInt32 count;
|
|
dgLink* ptr;
|
|
dgLink* link;
|
|
dgBody* body;
|
|
dgConstraint* constraint;
|
|
dgLink* stack[2048];
|
|
const dgInt64 mask = dgInt64 (65536) * dgInt64 (65536) * dgInt64 (65536) * dgInt64 (4096);
|
|
|
|
count = 0;
|
|
link = root->m_firstConstraintLink;
|
|
if (link) {
|
|
root->m_lru |= mask;
|
|
stack[0] = link;
|
|
i = 1;
|
|
while (i) {
|
|
i --;
|
|
link = stack[i];
|
|
ptr = link;
|
|
do {
|
|
body = ptr->m_twin->m_body;
|
|
if (body) {
|
|
NEWTON_ASSERT (body);
|
|
if (~body->m_lru & mask) {
|
|
constraint = ptr->m_constraint;
|
|
if (constraint->IsBilateral()) {
|
|
if (count < maxSize) {
|
|
array[count] = body;
|
|
count ++;
|
|
}
|
|
stack[i] = ptr->m_twin;
|
|
i ++;
|
|
}
|
|
body->m_lru |= mask;
|
|
}
|
|
}
|
|
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != link);
|
|
}
|
|
|
|
link = root->m_firstConstraintLink;
|
|
root->m_lru &= ~ mask;
|
|
stack[0] = link;
|
|
i = 1;
|
|
while (i) {
|
|
i --;
|
|
link = stack[i];
|
|
ptr = link;
|
|
do {
|
|
body = ptr->m_twin->m_body;
|
|
if (body) {
|
|
NEWTON_ASSERT (body);
|
|
if (body->m_lru & mask) {
|
|
constraint = ptr->m_constraint;
|
|
if (constraint->IsBilateral()) {
|
|
stack[i] = ptr->m_twin;
|
|
i ++;
|
|
}
|
|
body->m_lru &= ~ mask;
|
|
}
|
|
}
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != link);
|
|
}
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
|
|
dgInt32 dgWorld::GetConstraintArray (dgConstraint* root, dgConstraint** constraintArray, dgInt32 maxSize) const
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
return 0;
|
|
|
|
dgInt32 i;
|
|
dgInt32 stack;
|
|
dgInt32 count;
|
|
dgLink* ptr;
|
|
dgLink* link;
|
|
dgBody* body;
|
|
dgConstraint* constraint;
|
|
dgConstraint* constraintPtr;
|
|
dgConstraint* stackPool[2048];
|
|
const dgInt64 mask = dgInt64 (65536) * dgInt64 (65536) * dgInt64 (65536) * dgInt64 (4096);
|
|
|
|
count = 0;
|
|
stack = 1;
|
|
stackPool[0] = root;
|
|
root->m_lru |= mask;
|
|
|
|
while (stack && (count < maxSize)) {
|
|
stack --;
|
|
constraint = stackPool[stack];
|
|
constraintArray[count] = constraint;
|
|
count ++;
|
|
|
|
body = constraint->m_body0;
|
|
if (body && (body->m_invMass.m_w != dgFloat32 (0.0f))) {
|
|
link = body->m_firstConstraintLink;
|
|
NEWTON_ASSERT (link);
|
|
ptr = link;
|
|
do {
|
|
constraintPtr = ptr->m_constraint;
|
|
if (!(constraintPtr->m_lru & mask)) {
|
|
constraintPtr->m_lru |= mask;
|
|
stackPool[stack] = constraintPtr;
|
|
stack ++;
|
|
}
|
|
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != link);
|
|
}
|
|
|
|
body = constraint->m_body1;
|
|
if (body && (body->m_invMass.m_w != dgFloat32 (0.0f))) {
|
|
link = body->m_firstConstraintLink;
|
|
NEWTON_ASSERT (link);
|
|
ptr = link;
|
|
do {
|
|
constraintPtr = ptr->m_constraint;
|
|
if (!(constraintPtr->m_lru & mask)) {
|
|
constraintPtr->m_lru |= mask;
|
|
stackPool[stack] = constraintPtr;
|
|
stack ++;
|
|
}
|
|
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != link);
|
|
}
|
|
}
|
|
|
|
for (i = 0; i < stack; i ++) {
|
|
constraint = stackPool[i];
|
|
constraint->m_lru &= ~mask;
|
|
}
|
|
|
|
for (i = 0; i < count; i ++) {
|
|
constraint = constraintArray[i];
|
|
constraint->m_lru &= ~mask;
|
|
|
|
#ifdef _DEBUG
|
|
for (stack = i + 1; stack < count; stack ++) {
|
|
NEWTON_ASSERT (constraint != constraintArray[stack]);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
return count;
|
|
|
|
}
|
|
*/
|
|
|
|
bool dgWorld::AreBodyConnectedByJoints(dgBody *const originSrc,
|
|
dgBody *const targetSrc) {
|
|
#define DG_QEUEU_SIZE 1024
|
|
dgBody *queue[DG_QEUEU_SIZE];
|
|
|
|
m_genericLRUMark++;
|
|
|
|
dgBody *origin1 = originSrc;
|
|
dgBody *target1 = targetSrc;
|
|
if (origin1->m_mass[3] == dgFloat32(0.0f)) {
|
|
Swap(origin1, target1);
|
|
}
|
|
|
|
dgBody *const origin = origin1;
|
|
dgBody *const target = target1;
|
|
|
|
dgInt32 end = 1;
|
|
dgInt32 start = 0;
|
|
queue[0] = origin;
|
|
origin->m_genericLRUMark = m_genericLRUMark;
|
|
|
|
while (start != end) {
|
|
dgBody *const originI = queue[start];
|
|
start++;
|
|
start &= (DG_QEUEU_SIZE - 1);
|
|
|
|
for (dgBodyMasterListRow::dgListNode *jointNode =
|
|
originI->m_masterNode->GetInfo().GetFirst();
|
|
jointNode; jointNode =
|
|
jointNode->GetNext()) {
|
|
dgBodyMasterListCell &cell = jointNode->GetInfo();
|
|
|
|
dgBody *const body = cell.m_bodyNode;
|
|
if (body->m_genericLRUMark != m_genericLRUMark) {
|
|
dgConstraint *const constraint = cell.m_joint;
|
|
if (constraint->GetId() != dgContactConstraintId) {
|
|
if (body == target) {
|
|
return true;
|
|
}
|
|
body->m_genericLRUMark = m_genericLRUMark;
|
|
queue[end] = body;
|
|
end++;
|
|
end &= (DG_QEUEU_SIZE - 1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void dgWorld::FlushCache() {
|
|
|
|
// delete all contacts
|
|
dgActiveContacts &contactList = *this;
|
|
for (dgActiveContacts::dgListNode *contactNode = contactList.GetFirst();
|
|
contactNode;) {
|
|
dgContact *contact;
|
|
contact = contactNode->GetInfo();
|
|
contactNode = contactNode->GetNext();
|
|
DestroyConstraint(contact);
|
|
}
|
|
|
|
// clean up memory in bradPhase
|
|
dgBroadPhaseCollision::InvalidateCache();
|
|
|
|
// sort body list
|
|
SortMasterList();
|
|
}
|