madcow/scummvm-cursorfix
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity
Files
main
scummvm-cursorfix/engines/hpl1/engine/libraries/newton/physics/dgCollisionScene.cpp
Leon Krieg 5b11698731 Initial commit
2026-02-02 04:50:13 +01:00

844 lines
27 KiB
C++
Raw Permalink Blame History

/* 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 "dgCollisionScene.h"
#include "dgBody.h"
#include "dgCollisionNull.h"
#include "dgContact.h"
#include "dgWorld.h"
#include "hpl1/engine/libraries/newton/core/dg.h"
#define DG_SCENE_MAX_STACK_DEPTH 128
#define DG_SCENE_AABB_SCALE dgFloat32(4.0f)
#define DG_SCENE_AABB_INV_SCALE (dgFloat32(1.0f) / DG_SCENE_AABB_SCALE)
dgCollisionScene::dgNode::dgNode() : m_parent(NULL), m_left(NULL), m_right(NULL), m_fitnessNode(NULL) {
}
dgCollisionScene::dgNode::dgNode(dgNode *const sibling, dgNode *const myNode) : m_parent(sibling->m_parent), m_left(sibling), m_right(myNode), m_fitnessNode(
NULL) {
if (m_parent) {
if (m_parent->m_left == sibling) {
m_parent->m_left = this;
} else {
NEWTON_ASSERT(m_parent->m_right == sibling);
m_parent->m_right = this;
}
}
sibling->m_parent = this;
myNode->m_parent = this;
dgNode *const left = m_left;
dgNode *const right = m_right;
m_minBox = dgVector(GetMin(left->m_minBox.m_x, right->m_minBox.m_x),
GetMin(left->m_minBox.m_y, right->m_minBox.m_y),
GetMin(left->m_minBox.m_z, right->m_minBox.m_z), dgFloat32(0.0f));
m_maxBox = dgVector(GetMax(left->m_maxBox.m_x, right->m_maxBox.m_x),
GetMax(left->m_maxBox.m_y, right->m_maxBox.m_y),
GetMax(left->m_maxBox.m_z, right->m_maxBox.m_z), dgFloat32(0.0f));
dgVector side0(m_maxBox - m_minBox);
dgVector side1(side0.m_y, side0.m_z, side0.m_x, dgFloat32(0.0f));
m_surfaceArea = side0 % side1;
}
dgCollisionScene::dgNode::~dgNode() {
if (m_left) {
delete (m_left);
}
if (m_right) {
delete (m_right);
}
}
dgCollisionScene::dgProxy::dgProxy(dgCollision *shape, const dgMatrix &matrix,
dgCollisionScene *const owner) : dgNode(), m_matrix(shape->GetOffsetMatrix() * matrix), m_userData(NULL), m_shape(shape), m_owner(owner), m_myNode(NULL) {
dgVector boxP0;
dgVector boxP1;
shape->CalcAABB(m_matrix, boxP0, boxP1);
dgVector p0(
boxP0.CompProduct(
dgVector(DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE,
DG_SCENE_AABB_SCALE, dgFloat32(0.0f))));
dgVector p1(
boxP1.CompProduct(
dgVector(DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE,
DG_SCENE_AABB_SCALE, dgFloat32(0.0f))));
m_minBox.m_x = dgFloor(p0.m_x) * DG_SCENE_AABB_INV_SCALE;
m_minBox.m_y = dgFloor(p0.m_y) * DG_SCENE_AABB_INV_SCALE;
m_minBox.m_z = dgFloor(p0.m_z) * DG_SCENE_AABB_INV_SCALE;
m_minBox.m_w = dgFloat32(0.0f);
m_maxBox.m_x = dgFloor(p1.m_x + dgFloat32(1.0f)) * DG_SCENE_AABB_INV_SCALE;
m_maxBox.m_y = dgFloor(p1.m_y + dgFloat32(1.0f)) * DG_SCENE_AABB_INV_SCALE;
m_maxBox.m_z = dgFloor(p1.m_z + dgFloat32(1.0f)) * DG_SCENE_AABB_INV_SCALE;
m_maxBox.m_w = dgFloat32(0.0f);
dgVector side0(m_maxBox - m_minBox);
dgVector side1(side0.m_y, side0.m_z, side0.m_x, dgFloat32(0.0f));
m_surfaceArea = side0 % side1;
}
dgCollisionScene::dgProxy::~dgProxy() {
}
dgCollisionScene::dgCollisionScene(dgWorld *world) : dgCollision(world->GetAllocator(), 0, dgGetIdentityMatrix(),
m_sceneCollision),
m_lock(0), m_list(world->GetAllocator()), m_fitnessList(
world->GetAllocator()) {
m_world = world;
m_rootNode = NULL;
m_rtti |= dgCollisionScene_RTTI;
SetCollisionBBox(
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f)),
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f)));
}
dgCollisionScene::dgCollisionScene(dgWorld *const world,
dgDeserialize deserialization, void *const userData) : dgCollision(world, deserialization, userData), m_lock(0), m_list(world->GetAllocator()), m_fitnessList(world->GetAllocator()) {
dgInt32 data[4];
m_world = world;
m_rtti |= dgCollisionScene_RTTI;
deserialization(userData, &data, sizeof(data));
for (dgInt32 i = 0; i < data[0]; i++) {
dgMatrix matrix;
void *dataNew;
deserialization(userData, &matrix, sizeof(dgMatrix));
deserialization(userData, &dataNew, sizeof(void *));
dgCollision *const collision = m_world->CreateFromSerialization(
deserialization, userData);
dgList<dgProxy *>::dgListNode *const proxyNode =
(dgList<dgProxy *>::dgListNode *)AddProxy(collision, matrix);
dgProxy *const proxy = proxyNode->GetInfo();
proxy->m_userData = dataNew;
collision->Release();
}
ImproveTotalFitness();
}
dgCollisionScene::~dgCollisionScene(void) {
for (dgList<dgProxy *>::dgListNode *node = m_list.GetFirst(); node;) {
dgList<dgProxy *>::dgListNode *const proxyNode = node;
node = node->GetNext();
RemoveProxy(proxyNode);
}
}
void dgCollisionScene::Serialize(dgSerialize callback,
void *const userData) const {
dgInt32 data[4];
SerializeLow(callback, userData);
data[0] = m_list.GetCount();
data[1] = 0;
data[2] = 0;
data[3] = 0;
callback(userData, &data, sizeof(data));
for (dgList<dgProxy *>::dgListNode *node = m_list.GetFirst(); node;
node = node->GetNext()) {
dgProxy *const proxy = node->GetInfo();
callback(userData, &proxy->m_matrix, sizeof(dgMatrix));
callback(userData, &proxy->m_userData, sizeof(void *));
m_world->Serialize(proxy->m_shape, callback, userData);
}
}
void dgCollisionScene::SetProxyMatrix(void *proxy, const dgMatrix &matrix) {
dgVector boxP0;
dgVector boxP1;
dgProxy *const entry = ((dgList<dgProxy *>::dgListNode *)proxy)->GetInfo();
entry->m_matrix = entry->m_shape->GetOffsetMatrix() * matrix;
entry->m_shape->CalcAABB(entry->m_matrix, boxP0, boxP1);
dgVector p0(
boxP0.CompProduct(
dgVector(DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE,
DG_SCENE_AABB_SCALE, dgFloat32(0.0f))));
dgVector p1(
boxP1.CompProduct(
dgVector(DG_SCENE_AABB_SCALE, DG_SCENE_AABB_SCALE,
DG_SCENE_AABB_SCALE, dgFloat32(0.0f))));
p0.m_x = dgFloor(p0.m_x) * DG_SCENE_AABB_INV_SCALE;
p0.m_y = dgFloor(p0.m_y) * DG_SCENE_AABB_INV_SCALE;
p0.m_z = dgFloor(p0.m_z) * DG_SCENE_AABB_INV_SCALE;
p1.m_x = dgFloor(p1.m_x + dgFloat32(1.0f)) * DG_SCENE_AABB_INV_SCALE;
p1.m_y = dgFloor(p1.m_y + dgFloat32(1.0f)) * DG_SCENE_AABB_INV_SCALE;
p1.m_z = dgFloor(p1.m_z + dgFloat32(1.0f)) * DG_SCENE_AABB_INV_SCALE;
entry->m_minBox = p0;
entry->m_maxBox = p1;
dgVector side0(p1 - p0);
dgVector side1(side0.m_y, side0.m_z, side0.m_x, dgFloat32(0.0f));
entry->m_surfaceArea = side0 % side1;
for (dgNode *parent = entry->m_parent; parent; parent = parent->m_parent) {
dgVector minBox;
dgVector maxBox;
dgFloat32 area = CalculateSurfaceArea(parent->m_left, parent->m_right,
minBox, maxBox);
if (!((parent->m_minBox.m_x < minBox.m_x) || (parent->m_minBox.m_y < minBox.m_y) || (parent->m_minBox.m_z < minBox.m_z) || (parent->m_maxBox.m_x > maxBox.m_x) || (parent->m_maxBox.m_y < maxBox.m_y) || (parent->m_maxBox.m_z < maxBox.m_z))) {
break;
}
m_world->dgGetIndirectLock(&m_lock);
parent->m_minBox = minBox;
parent->m_maxBox = maxBox;
parent->m_surfaceArea = area;
m_world->dgReleaseIndirectLock(&m_lock);
}
}
dgMatrix dgCollisionScene::GetProxyMatrix(void *const proxy) {
dgProxy *const entry = ((dgList<dgProxy *>::dgListNode *)proxy)->GetInfo();
return entry->m_shape->GetOffsetMatrix().Inverse() * entry->m_matrix;
}
void dgCollisionScene::SetProxyUserData(void *const proxy, void *const userData) {
dgProxy *const entry = ((dgList<dgProxy *>::dgListNode *)proxy)->GetInfo();
entry->m_userData = userData;
}
void *dgCollisionScene::GetProxyUserData(void *const proxy) const {
dgProxy *const entry = ((dgList<dgProxy *>::dgListNode *)proxy)->GetInfo();
return entry->m_userData;
}
void dgCollisionScene::SetCollisionCallback(
dgCollisionMeshCollisionCallback debugCallback) {
NEWTON_ASSERT(0);
/*
for (dgList<dgProxy>::dgListNode* node = m_list.GetFirst(); node; node = node->GetNext()) {
const dgCollisionScene::dgProxy& entry = node->GetInfo();
if (entry.m_shape->IsType (dgCollisionMesh_RTTI)) {
dgCollisionMesh* mesh;
mesh = (dgCollisionMesh*)entry.m_shape;
mesh->SetCollisionCallback (debugCallback);
}
}
*/
}
void dgCollisionScene::CalcAABB(const dgMatrix &matrix, dgVector &p0,
dgVector &p1) const {
dgVector origin(matrix.TransformVector(m_boxOrigin));
dgVector size(
m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
dgFloat32(0.0f));
p0 = origin - size;
p1 = origin + size;
#ifdef DG_DEBUG_AABB
dgInt32 i;
dgVector q0;
dgVector q1;
dgMatrix trans(matrix.Transpose());
for (i = 0; i < 3; i++) {
q0[i] = matrix.m_posit[i] + matrix.RotateVector(BoxSupportMapping(trans[i].Scale(-1.0f)))[i];
q1[i] = matrix.m_posit[i] + matrix.RotateVector(BoxSupportMapping(trans[i]))[i];
}
dgVector err0(p0 - q0);
dgVector err1(p1 - q1);
dgFloat32 err;
err = GetMax(size.m_x, size.m_y, size.m_z) * 0.5f;
NEWTON_ASSERT((err0 % err0) < err);
NEWTON_ASSERT((err1 % err1) < err);
#endif
}
void dgCollisionScene::CalcAABBSimd(const dgMatrix &matrix, dgVector &p0,
dgVector &p1) const {
CalcAABB(matrix, p0, p1);
}
void dgCollisionScene::DebugCollision(const dgMatrix &matrix,
OnDebugCollisionMeshCallback callback, void *const userData) const {
for (dgList<dgProxy *>::dgListNode *node = m_list.GetFirst(); node;
node = node->GetNext()) {
const dgCollisionScene::dgProxy *entry = node->GetInfo();
dgMatrix proxyMatrix(entry->m_matrix * matrix);
entry->m_shape->DebugCollision(
entry->m_shape->GetOffsetMatrix() * proxyMatrix, callback, userData);
}
}
dgFloat32 dgCollisionScene::GetVolume() const {
NEWTON_ASSERT(0);
return dgFloat32(0.0f);
}
dgInt32 dgCollisionScene::CalculateSignature() const {
NEWTON_ASSERT(0);
return 0;
}
dgFloat32 dgCollisionScene::GetBoxMinRadius() const {
return dgFloat32(0.0f);
}
dgFloat32 dgCollisionScene::GetBoxMaxRadius() const {
return dgFloat32(0.0f);
}
void dgCollisionScene::SetCollisionBBox(const dgVector &p0, const dgVector &p1) {
NEWTON_ASSERT(p0.m_x <= p1.m_x);
NEWTON_ASSERT(p0.m_y <= p1.m_y);
NEWTON_ASSERT(p0.m_z <= p1.m_z);
m_boxSize = (p1 - p0).Scale(dgFloat32(0.5f));
m_boxOrigin = (p1 + p0).Scale(dgFloat32(0.5f));
}
void dgCollisionScene::CalculateInertia(dgVector &inertia,
dgVector &origin) const {
inertia.m_x = dgFloat32(0.0f);
inertia.m_y = dgFloat32(0.0f);
inertia.m_z = dgFloat32(0.0f);
origin.m_x = dgFloat32(0.0f);
origin.m_y = dgFloat32(0.0f);
origin.m_z = dgFloat32(0.0f);
}
dgVector dgCollisionScene::CalculateVolumeIntegral(const dgMatrix &globalMatrix,
GetBuoyancyPlane bouyancyPlane, void *const context) const {
NEWTON_ASSERT(0);
return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
}
void dgCollisionScene::GetCollisionInfo(dgCollisionInfo *info) const {
dgCollision::GetCollisionInfo(info);
info->m_offsetMatrix = dgGetIdentityMatrix();
info->m_sceneCollision.m_childrenProxyCount = m_list.GetCount();
}
bool dgCollisionScene::OOBBTest(const dgMatrix &matrix,
const dgCollisionConvex *const shape, void *const cacheOrder) const {
NEWTON_ASSERT(0);
return true;
}
dgVector dgCollisionScene::SupportVertex(const dgVector &dir) const {
NEWTON_ASSERT(0);
return dgVector(0, 0, 0, 0);
}
dgFloat32 dgCollisionScene::RayCastSimd(const dgVector &localP0,
const dgVector &localP1, dgContactPoint &contactOut,
OnRayPrecastAction preFilter, const dgBody *const body,
void *const userData) const {
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
if (!m_rootNode) {
return dgFloat32(1.2f);
}
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
dgFloat32 maxParam = dgFloat32(1.2f);
dgFastRayTest ray(localP0, localP1);
while (stack) {
stack--;
const dgNode *const me = stackPool[stack];
if (ray.BoxTestSimd(me->m_minBox, me->m_maxBox)) {
if (!me->m_left) {
NEWTON_ASSERT(!me->m_right);
dgContactPoint tmpContactOut;
const dgProxy *const proxy = (const dgProxy *)me;
dgVector l0(proxy->m_matrix.UntransformVector(localP0));
dgVector l1(proxy->m_matrix.UntransformVector(localP1));
dgFloat32 param = proxy->m_shape->RayCastSimd(l0, l1, tmpContactOut,
preFilter, body, userData);
NEWTON_ASSERT(param >= dgFloat32(0.0f));
if (param < maxParam) {
contactOut.m_normal = proxy->m_matrix.RotateVectorSimd(
tmpContactOut.m_normal);
maxParam = param;
ray.Reset(maxParam);
}
} else {
NEWTON_ASSERT(me->m_left);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(me->m_right);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
return maxParam;
}
dgFloat32 dgCollisionScene::RayCast(const dgVector &localP0,
const dgVector &localP1, dgContactPoint &contactOut,
OnRayPrecastAction preFilter, const dgBody *const body,
void *const userData) const {
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
if (!m_rootNode) {
return dgFloat32(1.2f);
}
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
dgFloat32 maxParam = dgFloat32(1.2f);
// int xxx = 0;
dgFastRayTest ray(localP0, localP1);
while (stack) {
stack--;
const dgNode *const me = stackPool[stack];
// xxx ++;
if (ray.BoxTest(me->m_minBox, me->m_maxBox)) {
if (!me->m_left) {
NEWTON_ASSERT(!me->m_right);
dgContactPoint tmpContactOut;
const dgProxy *const proxy = (const dgProxy *)me;
dgVector l0(proxy->m_matrix.UntransformVector(localP0));
dgVector l1(proxy->m_matrix.UntransformVector(localP1));
dgFloat32 param = proxy->m_shape->RayCast(l0, l1, tmpContactOut,
preFilter, body, userData);
NEWTON_ASSERT(param >= dgFloat32(0.0f));
if (param < maxParam) {
contactOut.m_normal = proxy->m_matrix.RotateVector(
tmpContactOut.m_normal);
maxParam = param;
ray.Reset(maxParam);
}
} else {
NEWTON_ASSERT(me->m_left);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(me->m_right);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
return maxParam;
}
void dgCollisionScene::CollidePairSimd(
dgCollidingPairCollector::dgPair *const pair,
dgCollisionParamProxy &proxy) const {
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
NEWTON_ASSERT(pair->m_body1->GetCollision() == this);
NEWTON_ASSERT(
pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
dgVector p0;
dgVector p1;
NEWTON_ASSERT(m_world == pair->m_body1->GetWorld());
dgMatrix matrix(pair->m_body0->m_matrix * pair->m_body1->m_matrix.Inverse());
pair->m_body0->GetCollision()->CalcAABBSimd(matrix, p0, p1);
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
while (stack) {
stack--;
const dgNode *const me = stackPool[stack];
if (dgOverlapTestSimd(me->m_minBox, me->m_maxBox, p0, p1)) {
if (!me->m_left) {
NEWTON_ASSERT(!me->m_right);
const dgProxy *const sceneProxy = (const dgProxy *)me;
m_world->SceneContactsSimd(*sceneProxy, pair, proxy);
} else {
NEWTON_ASSERT(me->m_left);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(me->m_right);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
}
void dgCollisionScene::CollidePair(dgCollidingPairCollector::dgPair *const pair,
dgCollisionParamProxy &proxy) const {
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
NEWTON_ASSERT(pair->m_body1->GetCollision() == this);
NEWTON_ASSERT(
pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
dgVector p0;
dgVector p1;
NEWTON_ASSERT(m_world == pair->m_body1->GetWorld());
dgMatrix matrix(pair->m_body0->m_matrix * pair->m_body1->m_matrix.Inverse());
pair->m_body0->GetCollision()->CalcAABB(matrix, p0, p1);
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
while (stack) {
stack--;
const dgNode *const me = stackPool[stack];
if (dgOverlapTest(me->m_minBox, me->m_maxBox, p0, p1)) {
if (!me->m_left) {
NEWTON_ASSERT(!me->m_right);
const dgProxy *const sceneProxy = (const dgProxy *)me;
m_world->SceneContacts(*sceneProxy, pair, proxy);
} else {
NEWTON_ASSERT(me->m_left);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(me->m_right);
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNode *)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
}
void dgCollisionScene::ImproveNodeFitness(dgNode *const node) {
NEWTON_ASSERT(node->m_left);
NEWTON_ASSERT(node->m_right);
if (node->m_parent) {
if (node->m_parent->m_left == node) {
dgFloat32 cost0 = node->m_surfaceArea;
dgVector cost1P0;
dgVector cost1P1;
dgFloat32 cost1 = CalculateSurfaceArea(node->m_right,
node->m_parent->m_right, cost1P0, cost1P1);
dgVector cost2P0;
dgVector cost2P1;
dgFloat32 cost2 = CalculateSurfaceArea(node->m_left,
node->m_parent->m_right, cost2P0, cost2P1);
if ((cost1 <= cost0) && (cost1 <= cost2)) {
dgNode *const parent = node->m_parent;
node->m_minBox = parent->m_minBox;
node->m_maxBox = parent->m_maxBox;
node->m_surfaceArea = parent->m_surfaceArea;
if (parent->m_parent) {
if (parent->m_parent->m_left == parent) {
parent->m_parent->m_left = node;
} else {
NEWTON_ASSERT(parent->m_parent->m_right == parent);
parent->m_parent->m_right = node;
}
} else {
m_rootNode = node;
}
node->m_parent = parent->m_parent;
parent->m_parent = node;
node->m_right->m_parent = parent;
parent->m_left = node->m_right;
node->m_right = parent;
parent->m_minBox = cost1P0;
parent->m_maxBox = cost1P1;
parent->m_surfaceArea = cost1;
} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
dgNode *const parent = node->m_parent;
node->m_minBox = parent->m_minBox;
node->m_maxBox = parent->m_maxBox;
node->m_surfaceArea = parent->m_surfaceArea;
if (parent->m_parent) {
if (parent->m_parent->m_left == parent) {
parent->m_parent->m_left = node;
} else {
NEWTON_ASSERT(parent->m_parent->m_right == parent);
parent->m_parent->m_right = node;
}
} else {
m_rootNode = node;
}
node->m_parent = parent->m_parent;
parent->m_parent = node;
node->m_left->m_parent = parent;
parent->m_left = node->m_left;
node->m_left = parent;
parent->m_minBox = cost2P0;
parent->m_maxBox = cost2P1;
parent->m_surfaceArea = cost2;
}
} else {
dgFloat32 cost0 = node->m_surfaceArea;
dgVector cost1P0;
dgVector cost1P1;
dgFloat32 cost1 = CalculateSurfaceArea(node->m_left,
node->m_parent->m_left, cost1P0, cost1P1);
dgVector cost2P0;
dgVector cost2P1;
dgFloat32 cost2 = CalculateSurfaceArea(node->m_right,
node->m_parent->m_left, cost2P0, cost2P1);
if ((cost1 <= cost0) && (cost1 <= cost2)) {
dgNode *const parent = node->m_parent;
node->m_minBox = parent->m_minBox;
node->m_maxBox = parent->m_maxBox;
node->m_surfaceArea = parent->m_surfaceArea;
if (parent->m_parent) {
if (parent->m_parent->m_left == parent) {
parent->m_parent->m_left = node;
} else {
NEWTON_ASSERT(parent->m_parent->m_right == parent);
parent->m_parent->m_right = node;
}
} else {
m_rootNode = node;
}
node->m_parent = parent->m_parent;
parent->m_parent = node;
node->m_left->m_parent = parent;
parent->m_right = node->m_left;
node->m_left = parent;
parent->m_minBox = cost1P0;
parent->m_maxBox = cost1P1;
parent->m_surfaceArea = cost1;
} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
dgNode *const parent = node->m_parent;
node->m_minBox = parent->m_minBox;
node->m_maxBox = parent->m_maxBox;
node->m_surfaceArea = parent->m_surfaceArea;
if (parent->m_parent) {
if (parent->m_parent->m_left == parent) {
parent->m_parent->m_left = node;
} else {
NEWTON_ASSERT(parent->m_parent->m_right == parent);
parent->m_parent->m_right = node;
}
} else {
m_rootNode = node;
}
node->m_parent = parent->m_parent;
parent->m_parent = node;
node->m_right->m_parent = parent;
parent->m_right = node->m_right;
node->m_right = parent;
parent->m_minBox = cost2P0;
parent->m_maxBox = cost2P1;
parent->m_surfaceArea = cost2;
}
}
}
NEWTON_ASSERT(!m_rootNode->m_parent);
}
void dgCollisionScene::ImproveTotalFitness() {
dgInt32 maxPasses = 2 * exp_2(m_fitnessList.GetCount()) + 1;
dgFloat64 newCost = dgFloat32(1.0e20f);
dgFloat64 prevCost = newCost;
do {
prevCost = newCost;
for (dgList<dgNode *>::dgListNode *node = m_fitnessList.GetFirst(); node;
node = node->GetNext()) {
ImproveNodeFitness(node->GetInfo());
}
newCost = dgFloat32(0.0f);
for (dgList<dgNode *>::dgListNode *node = m_fitnessList.GetFirst(); node;
node = node->GetNext()) {
newCost += node->GetInfo()->m_surfaceArea;
}
maxPasses--;
} while (maxPasses && (newCost < prevCost));
SetCollisionBBox(m_rootNode->m_minBox, m_rootNode->m_maxBox);
}
dgFloat32 dgCollisionScene::CalculateSurfaceArea(const dgNode *const node0,
const dgNode *const node1, dgVector &minBox, dgVector &maxBox) const {
minBox = dgVector(GetMin(node0->m_minBox.m_x, node1->m_minBox.m_x),
GetMin(node0->m_minBox.m_y, node1->m_minBox.m_y),
GetMin(node0->m_minBox.m_z, node1->m_minBox.m_z), dgFloat32(0.0f));
maxBox = dgVector(GetMax(node0->m_maxBox.m_x, node1->m_maxBox.m_x),
GetMax(node0->m_maxBox.m_y, node1->m_maxBox.m_y),
GetMax(node0->m_maxBox.m_z, node1->m_maxBox.m_z), dgFloat32(0.0f));
dgVector side0(maxBox - minBox);
dgVector side1(side0.m_y, side0.m_z, side0.m_x, dgFloat32(0.0f));
return side0 % side1;
}
void *dgCollisionScene::GetFirstProxy() const {
return m_list.GetFirst();
}
void *dgCollisionScene::GetNextProxy(void *const proxy) const {
dgList<dgProxy *>::dgListNode *const node =
(dgList<dgProxy *>::dgListNode *)proxy;
return node->GetNext();
}
void *dgCollisionScene::AddProxy(dgCollision *const shape,
const dgMatrix &matrix) {
shape->AddRef();
dgProxy *const newNode = new (m_world->GetAllocator()) dgProxy(shape, matrix,
this);
newNode->m_myNode = m_list.Append(newNode);
if (!m_rootNode) {
m_rootNode = newNode;
} else {
dgVector p0;
dgVector p1;
dgNode *sibling = m_rootNode;
dgFloat32 surfaceArea = CalculateSurfaceArea(newNode, sibling, p0, p1);
while (sibling->m_left && sibling->m_right) {
if (surfaceArea > sibling->m_surfaceArea) {
break;
}
sibling->m_minBox = p0;
sibling->m_maxBox = p1;
sibling->m_surfaceArea = surfaceArea;
dgVector leftP0;
dgVector leftP1;
dgFloat32 leftSurfaceArea = CalculateSurfaceArea(newNode, sibling->m_left,
leftP0, leftP1);
dgVector rightP0;
dgVector rightP1;
dgFloat32 rightSurfaceArea = CalculateSurfaceArea(newNode,
sibling->m_right, rightP0, rightP1);
if (leftSurfaceArea < rightSurfaceArea) {
sibling = sibling->m_left;
p0 = leftP0;
p1 = leftP1;
surfaceArea = leftSurfaceArea;
} else {
sibling = sibling->m_right;
p0 = rightP0;
p1 = rightP1;
surfaceArea = rightSurfaceArea;
}
}
dgNode *const parent = new (m_world->GetAllocator()) dgNode(sibling,
newNode);
parent->m_fitnessNode = m_fitnessList.Append(parent);
if (!parent->m_parent) {
m_rootNode = parent;
}
}
return newNode->m_myNode;
}
void dgCollisionScene::RemoveProxy(void *const proxy) {
dgList<dgProxy *>::dgListNode *const node =
(dgList<dgProxy *>::dgListNode *)proxy;
dgProxy *const treeNode = node->GetInfo();
m_world->ReleaseCollision(treeNode->m_shape);
m_list.Remove(node);
if (!treeNode->m_parent) {
delete (m_rootNode);
m_rootNode = NULL;
} else if (!treeNode->m_parent->m_parent) {
m_fitnessList.Remove(treeNode->m_parent->m_fitnessNode);
dgNode *const root = m_rootNode;
if (treeNode->m_parent->m_left == treeNode) {
m_rootNode = treeNode->m_parent->m_right;
treeNode->m_parent->m_right = NULL;
} else {
m_rootNode = treeNode->m_parent->m_left;
treeNode->m_parent->m_left = NULL;
}
m_rootNode->m_parent = NULL;
delete (root);
} else {
m_fitnessList.Remove(treeNode->m_parent->m_fitnessNode);
dgNode *const root = treeNode->m_parent->m_parent;
if (treeNode->m_parent == root->m_left) {
if (treeNode->m_parent->m_right == treeNode) {
root->m_left = treeNode->m_parent->m_left;
treeNode->m_parent->m_left = NULL;
} else {
root->m_left = treeNode->m_parent->m_right;
treeNode->m_parent->m_right = NULL;
}
root->m_left->m_parent = root;
} else {
if (treeNode->m_parent->m_right == treeNode) {
root->m_right = treeNode->m_parent->m_left;
treeNode->m_parent->m_left = NULL;
} else {
root->m_right = treeNode->m_parent->m_right;
treeNode->m_parent->m_right = NULL;
}
root->m_right->m_parent = root;
}
delete (treeNode->m_parent);
}
}
Reference in New Issue View Git Blame Copy Permalink