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

2319 lines
70 KiB
C++
Raw 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 "dgCollisionCompound.h"
#include "dgCollisionBVH.h"
#include "dgCollisionConvex.h"
#include "dgCollisionConvexModifier.h"
#include "dgCollisionEllipse.h"
#include "dgCollisionHeightField.h"
#include "dgWorld.h"
#include "hpl1/engine/libraries/newton/core/dg.h"
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
dgCollisionCompound::OOBBTestData::OOBBTestData(const dgMatrix &matrix) : m_matrix(matrix) {
for (dgInt32 i = 0; i < 3; i++) {
m_absMatrix[i][3] = dgFloat32(0.0f);
dgVector dir(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
dir[i] = dgFloat32(1.0f);
for (dgInt32 j = 0; j < 3; j++) {
m_absMatrix[i][j] = dgAbsf(m_matrix[i][j]);
dgVector axis(dir * m_matrix[j]);
m_crossAxis[i][j] = axis;
m_crossAxisAbs[i][j] = dgVector(dgAbsf(axis.m_x), dgAbsf(axis.m_y),
dgAbsf(axis.m_z), dgFloat32(0.0f));
m_crossAxisDotAbs[i][j] = dgVector(dgAbsf(axis % matrix[0]),
dgAbsf(axis % matrix[1]), dgAbsf(axis % matrix[2]), dgFloat32(0.0f));
}
}
m_absMatrix[3][3] = dgFloat32(1.0f);
}
dgCollisionCompound::OOBBTestData::OOBBTestData(const dgMatrix &matrix,
const dgVector &p0, const dgVector &p1) : m_matrix(matrix), m_localP0(p0), m_localP1(p1) {
m_size = (m_localP1 - m_localP0).Scale(dgFloat32(0.5f));
m_origin = (m_localP1 + m_localP0).Scale(dgFloat32(0.5f));
for (dgInt32 i = 0; i < 3; i++) {
m_absMatrix[i][3] = dgFloat32(0.0f);
dgVector dir(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
dir[i] = dgFloat32(1.0f);
for (dgInt32 j = 0; j < 3; j++) {
m_absMatrix[i][j] = dgAbsf(m_matrix[i][j]);
m_crossAxis[i][j] = dir * m_matrix[j];
}
}
m_absMatrix[3][3] = dgFloat32(1.0f);
dgVector size(m_absMatrix.RotateVector(m_size));
dgVector origin(m_matrix.TransformVector(m_origin));
m_aabbP0 = origin - size;
m_aabbP1 = origin + size;
for (dgInt32 i = 0; i < 3; i++) {
for (dgInt32 j = 0; j < 3; j++) {
dgFloat32 d;
dgFloat32 c;
dgVector &axis = m_crossAxis[i][j];
d = m_size.m_x * dgAbsf(axis % m_matrix[0]) + m_size.m_y * dgAbsf(axis % m_matrix[1]) + m_size.m_z * dgAbsf(axis % m_matrix[2]) + dgFloat32(1.0e-3f);
c = origin % axis;
m_extends[i][j] = dgVector(c - d, c + d, dgFloat32(0.0f),
dgFloat32(0.0f));
NEWTON_ASSERT(m_extends[i][j].m_x <= m_extends[i][j].m_y);
m_crossAxisAbs[i][j] = dgVector(dgAbsf(axis.m_x), dgAbsf(axis.m_y),
dgAbsf(axis.m_z), dgFloat32(0.0f));
}
}
}
dgCollisionCompound::dgNodeBase::dgNodeBase() {
m_id = -1;
m_shape = NULL;
m_left = NULL;
m_right = NULL;
}
dgCollisionCompound::dgNodeBase::dgNodeBase(dgCollisionConvex *const shape,
dgInt32 id) {
m_id = id;
m_left = NULL;
m_right = NULL;
m_parent = NULL;
m_shape = shape;
m_shape->AddRef();
m_type = m_leaf;
m_shape->CalcAABB(shape->GetOffsetMatrix(), m_p0, m_p1);
m_p0.m_w = 0.0f;
m_p1.m_w = 0.0f;
m_size = (m_p1 - m_p0).Scale(dgFloat32(0.5f));
m_origin = (m_p1 + m_p0).Scale(dgFloat32(0.5f));
dgVector size1(m_size.m_y, m_size.m_z, m_size.m_x, dgFloat32(0.0f));
m_area = m_size % size1;
}
dgCollisionCompound::dgNodeBase::dgNodeBase(dgNodeBase *const left,
dgNodeBase *const right, dgInt32 id) {
m_id = id;
m_left = left;
m_right = right;
m_parent = NULL;
m_type = m_node;
m_shape = NULL;
m_p0.m_x = GetMin(left->m_p0.m_x, right->m_p0.m_x);
m_p0.m_y = GetMin(left->m_p0.m_y, right->m_p0.m_y);
m_p0.m_z = GetMin(left->m_p0.m_z, right->m_p0.m_z);
m_p1.m_x = GetMax(left->m_p1.m_x, right->m_p1.m_x);
m_p1.m_y = GetMax(left->m_p1.m_y, right->m_p1.m_y);
m_p1.m_z = GetMax(left->m_p1.m_z, right->m_p1.m_z);
m_p0.m_w = 0.0f;
m_p1.m_w = 0.0f;
m_size = (m_p1 - m_p0).Scale(dgFloat32(0.5f));
m_origin = (m_p1 + m_p0).Scale(dgFloat32(0.5f));
dgVector size1(m_size.m_y, m_size.m_z, m_size.m_x, dgFloat32(0.0f));
m_area = m_size % size1;
}
dgCollisionCompound::dgNodeBase::~dgNodeBase() {
if (m_shape) {
m_shape->Release();
}
if (m_left) {
delete m_left;
}
if (m_right) {
delete m_right;
}
}
void dgCollisionCompound::dgNodeBase::reset() {
m_id = 0; // FIXME: Maybe should reset to -1
m_left = NULL;
m_right = NULL;
m_parent = NULL;
m_type = 0;
m_shape = NULL;
m_p0 = dgVector(0.0f, 0.0f, 0.0f, 0.0f);
m_p1 = dgVector(0.0f, 0.0f, 0.0f, 0.0f);
m_size = dgVector(0.0f, 0.0f, 0.0f, 0.0f);
m_origin = dgVector(0.0f, 0.0f, 0.0f, 0.0f);
m_area = 0.0f;
}
bool dgCollisionCompound::dgNodeBase::BoxTest(const OOBBTestData &data,
const dgNodeBase *const otherNode) const {
dgVector otherOrigin(data.m_matrix.TransformVector(otherNode->m_origin));
dgVector otherSize(data.m_absMatrix.RotateVector(otherNode->m_size));
dgVector otherP0(otherOrigin - otherSize);
dgVector otherP1(otherOrigin + otherSize);
if (dgOverlapTest(m_p0, m_p1, otherP0, otherP1)) {
dgVector origin(data.m_matrix.UntransformVector(m_origin));
dgVector size(data.m_absMatrix.UnrotateVector(m_size));
dgVector p0(origin - size);
dgVector p1(origin + size);
if (dgOverlapTest(p0, p1, otherNode->m_p0, otherNode->m_p1)) {
for (dgInt32 i = 0; i < 3; i++) {
for (dgInt32 j = 0; j < 3; j++) {
dgFloat32 x0;
dgFloat32 x1;
dgFloat32 z0;
dgFloat32 z1;
dgFloat32 d;
dgFloat32 c;
const dgVector &axis = data.m_crossAxis[i][j];
const dgVector &axisAbs = data.m_crossAxisAbs[i][j];
d = m_size.m_x * axisAbs.m_x + m_size.m_y * axisAbs.m_y + m_size.m_z * axisAbs.m_z + dgFloat32(1.0e-3f);
c = m_origin % axis;
x0 = c - d;
x1 = c + d;
NEWTON_ASSERT(x0 <= x1);
const dgVector &axisDotAbs = data.m_crossAxisDotAbs[i][j];
d = otherNode->m_size.m_x * axisDotAbs.m_x + otherNode->m_size.m_y * axisDotAbs.m_y + otherNode->m_size.m_z * axisDotAbs.m_z + dgFloat32(1.0e-3f);
c = otherOrigin % axis;
z0 = c - d;
z1 = c + d;
NEWTON_ASSERT(z0 <= z1);
if ((x1 < z0) || (x0 > z1)) {
return false;
}
}
}
return true;
}
}
return false;
}
bool dgCollisionCompound::dgNodeBase::BoxTest(const OOBBTestData &data) const {
if (dgOverlapTest(data.m_aabbP0, data.m_aabbP1, m_p0, m_p1)) {
dgVector origin(data.m_matrix.UntransformVector(m_origin));
dgVector size(data.m_absMatrix.UnrotateVector(m_size));
dgVector p0(origin - size);
dgVector p1(origin + size);
if (dgOverlapTest(p0, p1, data.m_localP0, data.m_localP1)) {
for (dgInt32 i = 0; i < 3; i++) {
for (dgInt32 j = 0; j < 3; j++) {
dgFloat32 x0;
dgFloat32 x1;
dgFloat32 d;
dgFloat32 c;
const dgVector &axis = data.m_crossAxisAbs[i][j];
d = m_size.m_x * axis.m_x + m_size.m_y * axis.m_y + m_size.m_z * axis.m_z + dgFloat32(1.0e-3f);
c = m_origin % data.m_crossAxis[i][j];
x0 = c - d;
x1 = c + d;
NEWTON_ASSERT(x0 <= x1);
const dgVector &extend = data.m_extends[i][j];
if ((x1 < extend.m_x) || (x0 > extend.m_y)) {
return false;
}
}
}
return true;
}
}
return false;
}
dgFloat32 dgCollisionCompound::dgNodeBase::BoxClosestDistance(
const dgVector *const points, dgInt32 count) const {
dgVector box[8];
box[0] = dgVector(m_p0.m_x, m_p0.m_y, m_p0.m_z, dgFloat32(0.0f));
box[1] = dgVector(m_p0.m_x, m_p0.m_y, m_p1.m_z, dgFloat32(0.0f));
box[2] = dgVector(m_p0.m_x, m_p1.m_y, m_p0.m_z, dgFloat32(0.0f));
box[3] = dgVector(m_p0.m_x, m_p1.m_y, m_p1.m_z, dgFloat32(0.0f));
box[4] = dgVector(m_p1.m_x, m_p0.m_y, m_p0.m_z, dgFloat32(0.0f));
box[5] = dgVector(m_p1.m_x, m_p0.m_y, m_p1.m_z, dgFloat32(0.0f));
box[6] = dgVector(m_p1.m_x, m_p1.m_y, m_p0.m_z, dgFloat32(0.0f));
box[7] = dgVector(m_p1.m_x, m_p1.m_y, m_p1.m_z, dgFloat32(0.0f));
dgFloat32 dist = dgFloat32(1.0e10f);
for (dgInt32 i = 0; i < count; i++) {
for (dgInt32 j = 0; j < 8; j++) {
dgVector dp(points[i] - box[j]);
dgFloat32 dist1 = dp % dp;
if (dist1 < dist) {
dist = dist1;
}
}
}
return dist;
}
dgCollisionCompound::dgCollisionCompound(dgWorld *world) : dgCollision(world->GetAllocator(), 0, dgGetIdentityMatrix(),
m_compoundCollision) {
m_world = world;
m_root = NULL;
m_count = 0;
}
dgCollisionCompound::dgCollisionCompound(dgInt32 count,
dgCollisionConvex *const shapeArray[], dgWorld *world) : dgCollision(world->GetAllocator(), 0, dgGetIdentityMatrix(),
m_compoundCollision) {
m_world = world;
m_root = NULL;
if (count) {
m_root = BuildTree(count, shapeArray);
}
Init(count, shapeArray);
}
dgCollisionCompound::dgCollisionCompound(const dgCollisionCompound &source) : dgCollision(source.GetAllocator(), 0, dgGetIdentityMatrix(),
m_compoundCollision) {
int stack;
dgNodeBase *pool[DG_COMPOUND_STACK_DEPTH];
dgNodeBase **parent[DG_COMPOUND_STACK_DEPTH];
m_root = NULL;
m_world = source.m_world;
parent[0] = &m_root;
pool[0] = source.m_root;
stack = 1;
while (stack) {
dgNodeBase *node;
dgNodeBase **parentNode;
stack--;
node = pool[stack];
parentNode = parent[stack];
if (node->m_type == m_leaf) {
*parentNode = new (m_allocator) dgNodeBase(node->m_shape, node->m_id);
} else {
dgNodeBase *newNode;
newNode = new (m_allocator) dgNodeBase(*node);
if (!m_root) {
m_root = newNode;
}
*parentNode = newNode;
pool[stack] = node->m_left;
parent[stack] = &newNode->m_left;
stack++;
pool[stack] = node->m_right;
parent[stack] = &newNode->m_right;
stack++;
}
}
Init(source.m_count, source.m_array);
m_preCollisionFilter = source.m_preCollisionFilter;
}
dgCollisionCompound::dgCollisionCompound(dgWorld *const world,
dgDeserialize deserialization, void *const userData) : dgCollision(world, deserialization, userData) {
dgInt32 stack;
dgInt32 count;
dgInt32 data[4];
dgNodeBase *pool[DG_COMPOUND_STACK_DEPTH];
// dgNodeBase* parentPool[DG_COMPOUND_STACK_DEPTH];
deserialization(userData, data, sizeof(data));
count = data[0];
m_world = world;
dgStack<dgCollisionConvex *> array(data[0]);
for (dgInt32 i = 0; i < count; i++) {
dgCollision *collision;
collision = world->CreateFromSerialization(deserialization, userData);
array[i] = (dgCollisionConvex *)collision;
}
struct Data : public dgNodeBase {
Data() {
}
~Data() {
this->reset();
}
dgInt8 m_padding[128];
};
m_root = NULL;
Data nodeInfo;
for (dgInt32 i = 0; i < 2 * count - 1; i++) {
dgNodeBase *newNode;
deserialization(userData, &nodeInfo.m_p0, sizeof(dgNodeBase));
if (nodeInfo.m_type == m_leaf) {
dgNodeBase *cell;
cell = new (m_allocator) dgNodeBase(nodeInfo);
cell->m_shape = array[nodeInfo.m_id];
cell->m_shape->AddRef();
newNode = cell;
} else {
dgNodeBase *node;
node = new (m_allocator) dgNodeBase((dgNodeBase &)nodeInfo);
node->m_left = NULL;
node->m_right = NULL;
newNode = node;
}
if (!m_root) {
m_root = newNode;
} else {
stack = 1;
pool[0] = m_root;
while (stack) {
dgNodeBase *node;
stack--;
node = pool[stack];
// if (node->m_id == dgInt32 (newNode->m_parent)) {
if (((dgNodeBase *)dgInt64(node->m_id)) == newNode->m_parent) {
if (node->m_left == NULL) {
node->m_left = newNode;
} else {
NEWTON_ASSERT(!node->m_right);
node->m_right = newNode;
}
break;
}
if (node->m_type == m_node) {
if (node->m_left) {
pool[stack] = node->m_left;
stack++;
}
if (node->m_right) {
pool[stack] = node->m_right;
stack++;
}
}
}
}
}
Init(count, &array[0]);
for (dgInt32 i = 0; i < count; i++) {
world->ReleaseCollision(array[i]);
}
}
dgCollisionCompound::~dgCollisionCompound() {
if (m_root) {
delete m_root;
}
for (dgInt32 i = 0; i < m_count; i++) {
m_world->ReleaseCollision(m_array[i]);
}
m_allocator->Free(m_array);
}
void dgCollisionCompound::Init(dgInt32 count,
dgCollisionConvex *const shapeArray[]) {
m_count = count;
m_rtti |= dgCollisionCompound_RTTI;
m_preCollisionFilter = NULL;
m_array = (dgCollisionConvex **)m_allocator->Malloc(
m_count * dgInt32(sizeof(dgCollisionConvex *)));
for (dgInt32 i = 0; i < m_count; i++) {
m_array[i] = shapeArray[i];
m_array[i]->AddRef();
}
m_boxMinRadius = GetMin(m_root->m_size.m_x, m_root->m_size.m_y,
m_root->m_size.m_z);
m_boxMaxRadius = dgSqrt(m_root->m_size % m_root->m_size);
LinkParentNodes();
}
void dgCollisionCompound::LinkParentNodes() {
dgInt32 stack;
dgNodeBase *pool[DG_COMPOUND_STACK_DEPTH];
dgNodeBase *parentPool[DG_COMPOUND_STACK_DEPTH];
pool[0] = m_root;
parentPool[0] = NULL;
stack = 1;
while (stack) {
dgNodeBase *node;
dgNodeBase *parent;
stack--;
node = pool[stack];
parent = parentPool[stack];
node->m_parent = parent;
if (node->m_type == m_node) {
parentPool[stack] = node;
pool[stack] = node->m_right;
stack++;
parentPool[stack] = node;
pool[stack] = node->m_left;
stack++;
}
}
}
void dgCollisionCompound::SetCollisionBBox(const dgVector &p0__,
const dgVector &p1__) {
NEWTON_ASSERT(0);
}
dgInt32 dgCollisionCompound::CalculateSignature() const {
NEWTON_ASSERT(0);
return 0;
}
void dgCollisionCompound::CalcAABB(const dgMatrix &matrix, dgVector &p0,
dgVector &p1) const {
dgVector origin(matrix.TransformVector(m_root->m_origin));
dgVector size(
m_root->m_size.m_x * dgAbsf(matrix[0][0]) + m_root->m_size.m_y * dgAbsf(matrix[1][0]) + m_root->m_size.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,
m_root->m_size.m_x * dgAbsf(matrix[0][1]) + m_root->m_size.m_y * dgAbsf(matrix[1][1]) + m_root->m_size.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,
m_root->m_size.m_x * dgAbsf(matrix[0][2]) + m_root->m_size.m_y * dgAbsf(matrix[1][2]) + m_root->m_size.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
dgFloat32(0.0f));
p0 = origin - size;
p1 = origin + size;
}
void dgCollisionCompound::CalcAABBSimd(const dgMatrix &matrix, dgVector &p0,
dgVector &p1) const {
dgVector origin(matrix.TransformVector(m_root->m_origin));
dgVector size(
m_root->m_size.m_x * dgAbsf(matrix[0][0]) + m_root->m_size.m_y * dgAbsf(matrix[1][0]) + m_root->m_size.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,
m_root->m_size.m_x * dgAbsf(matrix[0][1]) + m_root->m_size.m_y * dgAbsf(matrix[1][1]) + m_root->m_size.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,
m_root->m_size.m_x * dgAbsf(matrix[0][2]) + m_root->m_size.m_y * dgAbsf(matrix[1][2]) + m_root->m_size.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
dgFloat32(0.0f));
p0 = origin - size;
p1 = origin + size;
}
// void dgCollisionCompound::dgMatrix& matrix, DebugCollisionMeshCallback callback) const
void dgCollisionCompound::DebugCollision(const dgMatrix &matrix,
OnDebugCollisionMeshCallback callback, void *const userData) const {
for (dgInt32 i = 0; i < m_count; i++) {
m_array[i]->DebugCollision(matrix, callback, userData);
}
}
dgFloat32 dgCollisionCompound::RayCast(const dgVector &localP0,
const dgVector &localP1, dgContactPoint &contactOut,
OnRayPrecastAction preFilter, const dgBody *const body,
void *const userData) const {
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
if (!m_root) {
return dgFloat32(1.2f);
}
dgInt32 stack = 1;
stackPool[0] = m_root;
dgFloat32 maxParam = dgFloat32(1.2f);
dgFastRayTest ray(localP0, localP1);
while (stack) {
stack--;
const dgNodeBase *const me = stackPool[stack];
if (me && ray.BoxTest(me->m_p0, me->m_p1)) {
if (me->m_type == m_leaf) {
dgFloat32 param;
dgContactPoint tmpContactOut;
dgCollisionConvex *const shape = me->m_shape;
dgVector p0(shape->m_offset.UntransformVector(localP0));
dgVector p1(shape->m_offset.UntransformVector(localP1));
// param = shape->RayCast (p0, p1, tmpContactOut, NULL, NULL, NULL);
param = shape->RayCast(p0, p1, tmpContactOut, preFilter, body,
userData);
if (param < maxParam) {
maxParam = param;
contactOut.m_normal = shape->m_offset.RotateVector(
tmpContactOut.m_normal);
;
contactOut.m_userId = tmpContactOut.m_userId;
ray.Reset(maxParam);
}
} else {
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack] = me->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
}
}
}
return maxParam;
}
dgFloat32 dgCollisionCompound::RayCastSimd(const dgVector &localP0,
const dgVector &localP1, dgContactPoint &contactOut,
OnRayPrecastAction preFilter, const dgBody *const body,
void *const userData) const {
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
if (!m_root) {
return dgFloat32(1.2f);
}
dgInt32 stack = 1;
stackPool[0] = m_root;
dgFloat32 maxParam = dgFloat32(1.2f);
dgFastRayTest ray(localP0, localP1);
while (stack) {
stack--;
const dgNodeBase *const me = stackPool[stack];
if (me && ray.BoxTestSimd(me->m_p0, me->m_p1)) {
if (me->m_type == m_leaf) {
dgContactPoint tmpContactOut;
dgCollisionConvex *const shape = me->m_shape;
dgVector p0(shape->m_offset.UntransformVector(localP0));
dgVector p1(shape->m_offset.UntransformVector(localP1));
// param = shape->RayCastSimd (p0, p1, tmpContactOut, NULL, NULL, NULL);
dgFloat32 param = shape->RayCastSimd(p0, p1, tmpContactOut, preFilter,
body, userData);
if (param < maxParam) {
maxParam = param;
contactOut.m_normal = shape->m_offset.RotateVector(
tmpContactOut.m_normal);
contactOut.m_userId = tmpContactOut.m_userId;
ray.Reset(maxParam);
}
} else {
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack] = me->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
}
}
}
return maxParam;
}
dgFloat32 dgCollisionCompound::GetVolume() const {
dgFloat32 volume = dgFloat32(0.0f);
for (dgInt32 i = 0; i < m_count; i++) {
volume += m_array[i]->GetVolume();
}
return volume;
}
dgFloat32 dgCollisionCompound::GetBoxMinRadius() const {
return m_boxMinRadius;
}
dgFloat32 dgCollisionCompound::GetBoxMaxRadius() const {
return m_boxMaxRadius;
}
dgVector dgCollisionCompound::CalculateVolumeIntegral(
const dgMatrix &globalMatrix, GetBuoyancyPlane bouyancyPlane,
void *const context) const {
dgInt32 i;
dgFloat32 scale;
dgVector totalVolume(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
for (i = 0; i < m_count; i++) {
dgMatrix matrix(m_array[i]->m_offset * globalMatrix);
// dgVector vol (m_array[i]->CalculateVolumeIntegral (m_collisionMatrix[i], bouyancyPlane, context));
dgVector vol(
m_array[i]->CalculateVolumeIntegral(matrix, bouyancyPlane, context));
totalVolume.m_x += vol.m_x * vol.m_w;
totalVolume.m_y += vol.m_y * vol.m_w;
totalVolume.m_z += vol.m_z * vol.m_w;
totalVolume.m_w += vol.m_w;
}
scale = dgFloat32(1.0f) / (totalVolume.m_w + dgFloat32(1.0e-6f));
totalVolume.m_x *= scale;
totalVolume.m_y *= scale;
totalVolume.m_z *= scale;
return totalVolume;
}
void dgCollisionCompound::CalculateInertia(dgVector &inertia,
dgVector &origin) const {
dgInt32 i;
dgCollisionConvex *collision;
dgFloat32 invVolume;
dgFloat32 totalVolume;
dgVector tmpOrigin;
dgVector tmpInertia;
dgVector tmpCrossInertia;
dgVector totalOrigin(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
dgVector totalInertia(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
dgVector totalCrossInertia(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
#define DG_MIN_SIDE dgFloat32(1.0e-2f)
#define DG_MIN_VOLUME (DG_MIN_SIDE * DG_MIN_SIDE * DG_MIN_SIDE)
totalVolume = dgFloat32(0.0f);
for (i = 0; i < m_count; i++) {
collision = m_array[i];
totalVolume += collision->CalculateMassProperties(tmpInertia,
tmpCrossInertia, tmpOrigin);
totalOrigin += tmpOrigin;
totalInertia += tmpInertia;
totalCrossInertia += tmpCrossInertia;
}
invVolume = dgFloat32(1.0f) / GetMax(totalVolume, DG_MIN_VOLUME);
origin = totalOrigin.Scale(invVolume);
totalInertia = totalInertia.Scale(invVolume);
totalCrossInertia = totalCrossInertia.Scale(invVolume);
inertia.m_x = totalInertia.m_x - (origin.m_y * origin.m_y + origin.m_z * origin.m_z);
inertia.m_y = totalInertia.m_y - (origin.m_z * origin.m_z + origin.m_x * origin.m_x);
inertia.m_z = totalInertia.m_z - (origin.m_x * origin.m_x + origin.m_y * origin.m_y);
// crossInertia.m_x += totalCrossInertia.m_x + origin.m_y * origin.m_z;
// crossInertia.m_y += totalCrossInertia.m_x + origin.m_z * origin.m_x;
// crossInertia.m_z += totalCrossInertia.m_x + origin.m_x * origin.m_y;
NEWTON_ASSERT(inertia[0] > 0.0f);
NEWTON_ASSERT(inertia[1] > 0.0f);
NEWTON_ASSERT(inertia[2] > 0.0f);
}
void dgCollisionCompound::AddCollision(dgCollisionConvex *part) {
NEWTON_ASSERT(0);
/*
dgInt32 i;
dgInt8 *ptr;
dgCollisionConvex** array;
dgMatrix* collisionMatrix;
AABB* aabb;
NEWTON_ASSERT (0);
if (m_count >= m_maxCount) {
m_maxCount = m_maxCount * 2;
ptr = (dgInt8*) m_allocator->Malloc (m_maxCount * (sizeof (dgMatrix) + sizeof (dgCollisionConvex*) + sizeof(AABB)));
collisionMatrix = (dgMatrix*) ptr;
aabb = (AABB*) &ptr [m_maxCount * sizeof (dgMatrix)];
array = (dgCollisionConvex**) &ptr [m_maxCount * (sizeof (dgMatrix) + sizeof(AABB))];
for (i = 0; i < m_count; i ++) {
array[i] = m_array[i];
collisionMatrix[i] = m_collisionMatrix[i];
aabb[i] = m_aabb[i];
}
m_allocator->Free (m_collisionMatrix);
m_aabb = aabb;
m_array = array;
m_collisionMatrix = collisionMatrix;
}
m_array[m_count] = part;
m_array[m_count]->AddRef();
m_count ++;
*/
}
void dgCollisionCompound::RemoveCollision(dgNodeBase *treeNode) {
m_array[treeNode->m_id]->Release();
m_count--;
m_array[treeNode->m_id] = m_array[m_count];
if (!treeNode->m_parent) {
delete (m_root);
m_root = NULL;
} else if (!treeNode->m_parent->m_parent) {
dgNodeBase *const root = m_root;
if (treeNode->m_parent->m_left == treeNode) {
m_root = treeNode->m_parent->m_right;
treeNode->m_parent->m_right = NULL;
} else {
m_root = treeNode->m_parent->m_left;
treeNode->m_parent->m_left = NULL;
}
m_root->m_parent = NULL;
delete (root);
} else {
dgNodeBase *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);
}
}
dgVector dgCollisionCompound::SupportVertex(const dgVector &dir) const {
dgInt32 ix;
dgInt32 iy;
dgInt32 iz;
dgInt32 stack;
dgFloat32 maxProj;
dgFloat32 aabbProjection[DG_COMPOUND_STACK_DEPTH];
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
stack = 1;
stackPool[0] = m_root;
aabbProjection[0] = dgFloat32(1.0e10f);
maxProj = dgFloat32(-1.0e20f);
dgVector searchDir(m_offset.UnrotateVector(dir));
ix = (searchDir[0] > dgFloat32(0.0f)) ? 1 : 0;
iy = (searchDir[1] > dgFloat32(0.0f)) ? 1 : 0;
iz = (searchDir[2] > dgFloat32(0.0f)) ? 1 : 0;
dgVector supportVertex(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
while (stack) {
dgFloat32 boxSupportValue;
stack--;
boxSupportValue = aabbProjection[stack];
if (boxSupportValue > maxProj) {
const dgNodeBase *const me = stackPool[stack];
if (me->m_type == m_leaf) {
dgFloat32 dist;
dgCollision *const shape = me->m_shape;
dgVector newDir(shape->m_offset.UnrotateVector(searchDir));
dgVector vertex(
shape->m_offset.TransformVector(shape->SupportVertex(newDir)));
dist = dir % vertex;
if (dist > maxProj) {
maxProj = dist;
supportVertex = vertex;
}
} else {
dgFloat32 dist0;
dgFloat32 dist1;
const dgNodeBase *const left = me->m_left;
const dgNodeBase *const right = me->m_right;
const dgVector *const box0 = &left->m_p0;
dgVector p0(box0[ix].m_x, box0[iy].m_y, box0[iz].m_z, dgFloat32(0.0f));
const dgVector *const box1 = &right->m_p0;
dgVector p1(box1[ix].m_x, box1[iy].m_y, box1[iz].m_z, dgFloat32(0.0f));
dist0 = p0 % dir;
dist1 = p1 % dir;
if (dist0 > dist1) {
stackPool[stack] = right;
aabbProjection[stack] = dist1;
stack++;
stackPool[stack] = left;
aabbProjection[stack] = dist0;
stack++;
} else {
stackPool[stack] = left;
aabbProjection[stack] = dist0;
stack++;
stackPool[stack] = right;
aabbProjection[stack] = dist1;
stack++;
}
}
}
}
return m_offset.TransformVector(supportVertex);
}
void dgCollisionCompound::GetCollisionInfo(dgCollisionInfo *info) const {
dgCollision::GetCollisionInfo(info);
info->m_offsetMatrix = GetOffsetMatrix();
info->m_compoundCollision.m_chidrenCount = m_count;
info->m_compoundCollision.m_chidren = (dgCollision **)m_array;
info->m_collisionType = m_compoundCollision;
}
void dgCollisionCompound::Serialize(dgSerialize callback,
void *const userData) const {
dgInt32 stack;
dgInt32 data[4];
dgNodeBase *pool[DG_COMPOUND_STACK_DEPTH];
data[0] = m_count;
data[1] = 0;
data[2] = 0;
data[3] = 0;
SerializeLow(callback, userData);
callback(userData, &data, sizeof(data));
for (dgInt32 i = 0; i < m_count; i++) {
dgCollision *collision;
collision = m_array[i];
m_world->Serialize(collision, callback, userData);
}
pool[0] = m_root;
stack = 1;
while (stack) {
dgNodeBase *node;
dgNodeBase *parent;
stack--;
node = pool[stack];
parent = NULL;
if (node->m_parent) {
parent = node->m_parent;
node->m_parent = (dgNodeBase *)(dgInt64(node->m_parent->m_id));
}
callback(userData, &node->m_p0, sizeof(dgNodeBase));
node->m_parent = parent;
if (node->m_type == m_node) {
pool[stack] = node->m_right;
stack++;
pool[stack] = node->m_left;
stack++;
}
}
}
bool dgCollisionCompound::OOBBTest(const dgMatrix &matrix,
const dgCollisionConvex *const shape, void *const cacheOrder) const {
NEWTON_ASSERT(0);
return true;
}
/*
dgInt32 dgCollisionCompound::GetAxis (dgNodeBase** const proxiArray, dgInt32 boxCount) const
{
dgInt32 axis;
dgFloat32 maxVal;
dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < boxCount; i ++) {
const dgNodeBase* const proxy = proxiArray[i];
const dgVector& p0 = proxy->m_p0;
const dgVector& p1 = proxy->m_p1;
median += p0;
median += p1;
varian += p0.CompProduct(p0);
varian += p1.CompProduct(p1);
}
boxCount *= 2;
varian.m_x = boxCount * varian.m_x - median.m_x * median.m_x;
varian.m_y = boxCount * varian.m_y - median.m_y * median.m_y;
varian.m_z = boxCount * varian.m_z - median.m_z * median.m_z;
axis = 0;
maxVal = varian[0];
for (dgInt32 i = 1; i < 3; i ++) {
if (varian[i] > maxVal) {
axis = i;
maxVal = varian[i];
}
}
return axis;
}
dgInt32 dgCollisionCompound::CompareBox (const dgNodeBase* const boxA, const dgNodeBase* const boxB, void* const context)
{
dgInt32 axis;
axis = *((dgInt32*) context);
if (boxA->m_p0[axis] < boxB->m_p0[axis]) {
return -1;
} else if (boxA->m_p0[axis] > boxB->m_p0[axis]) {
return 1;
}
return 0;
}
dgCollisionCompound::dgNodeBase* dgCollisionCompound::BuildBottomUpTree(dgInt32 count, dgNodeBase** const proxiArray)
{
dgInt32 id;
dgStack<dgHeapNodePair> pool (count / 4 + DG_COMPOUND_STACK_DEPTH);
dgDownHeap<dgHeapNodePair, dgFloat32> heap (&pool[0], pool.GetSizeInBytes() - 64);
id = count;
while (count > 1){
dgInt32 axis;
dgInt32 newCount;
axis = GetAxis (proxiArray, count);
dgSortIndirect (proxiArray, count, CompareBox, &axis);
heap.Flush();
for (dgInt32 i = 0; i < count - 1; i ++) {
dgInt32 bestProxi;
dgFloat32 smallestVolume;
dgFloat32 breakValue;
dgNodeBase* nodeA;
nodeA = proxiArray[i];
bestProxi = -1;
smallestVolume = dgFloat32 (1.0e20f);
breakValue = ((count - i) < 32) ? dgFloat32 (1.0e20f) : nodeA->m_p1[axis] + dgFloat32 (2.0f);
if (breakValue < proxiArray[i + 1]->m_p0[axis]) {
breakValue = proxiArray[i + 1]->m_p0[axis] + dgFloat32 (2.0f);
}
for (dgInt32 j = i + 1; (j < count) && (proxiArray[j]->m_p0[axis] < breakValue); j ++) {
dgFloat32 volume;
dgVector p0;
dgVector p1;
dgNodeBase* nodeB;
nodeB = proxiArray[j];
p0.m_x = GetMin (nodeA->m_p0.m_x, nodeB->m_p0.m_x);
p0.m_y = GetMin (nodeA->m_p0.m_y, nodeB->m_p0.m_y);
p0.m_z = GetMin (nodeA->m_p0.m_z, nodeB->m_p0.m_z);
p0.m_w = dgFloat32 (0.0f);
p1.m_x = GetMax (nodeA->m_p1.m_x, nodeB->m_p1.m_x);
p1.m_y = GetMax (nodeA->m_p1.m_y, nodeB->m_p1.m_y);
p1.m_z = GetMax (nodeA->m_p1.m_z, nodeB->m_p1.m_z);
p1.m_w = dgFloat32 (0.0f);
dgVector dist (p1 - p0);
volume = dist.m_x * dist.m_y * dist.m_z;
if (volume < smallestVolume) {
bestProxi = j;
smallestVolume = volume;
}
}
NEWTON_ASSERT (bestProxi != -1);
dgHeapNodePair pair;
pair.m_nodeA = i;
pair.m_nodeB = bestProxi;
if (heap.GetCount() < heap.GetMaxCount()) {
heap.Push(pair, smallestVolume);
} else {
if (smallestVolume < heap.Value()) {
heap.Pop();
heap.Push(pair, smallestVolume);
}
}
}
heap.Sort ();
for (dgInt32 j = heap.GetCount() - 1; j >= 0; j --) {
dgHeapNodePair pair (heap[j]);
if ((proxiArray[pair.m_nodeA]->m_p0.m_w == dgFloat32 (0.0f)) && (proxiArray[pair.m_nodeB]->m_p0.m_w == dgFloat32 (0.0f))) {
proxiArray[pair.m_nodeA]->m_p0.m_w = dgFloat32 (1.0f);
proxiArray[pair.m_nodeB]->m_p0.m_w = dgFloat32 (1.0f);
proxiArray[count] = new (m_allocator) dgNodeBase (proxiArray[pair.m_nodeA], proxiArray[pair.m_nodeB], id);
proxiArray[pair.m_nodeA]->m_parent = proxiArray[count];
proxiArray[pair.m_nodeB]->m_parent = proxiArray[count];
id ++;
count ++;
}
}
newCount = 0;
for (dgInt32 i = 0; i < count; i ++) {
if (proxiArray[i]->m_p0.m_w == dgFloat32 (0.0f)) {
proxiArray[newCount] = proxiArray[i];
newCount ++;
}
}
NEWTON_ASSERT (newCount < count);
count = newCount;
}
return proxiArray[0];
}
*/
dgCollisionCompound::dgNodeBase *dgCollisionCompound::BuildTopDownTree(
dgInt32 count, dgNodeBase **const proxiArray, dgInt32 &id) {
dgNodeBase *tree = NULL;
if (count == 1) {
tree = proxiArray[0];
} else {
dgInt32 i0 = 1;
if (count > 2) {
dgVector median(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
dgVector varian(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
dgFloat32(0.0f));
for (dgInt32 i = 0; i < count; i++) {
const dgNodeBase *const proxy = proxiArray[i];
const dgVector &p0 = proxy->m_p0;
const dgVector &p1 = proxy->m_p1;
median += p0;
median += p1;
varian += p0.CompProduct(p0);
varian += p1.CompProduct(p1);
}
dgInt32 pointCount = count * 2;
varian.m_x = pointCount * varian.m_x - median.m_x * median.m_x;
varian.m_y = pointCount * varian.m_y - median.m_y * median.m_y;
varian.m_z = pointCount * varian.m_z - median.m_z * median.m_z;
dgInt32 axis = 0;
dgFloat32 maxVal = varian[0];
for (dgInt32 i = 1; i < 3; i++) {
if (varian[i] > maxVal) {
axis = i;
maxVal = varian[i];
}
}
dgVector center = median.Scale(dgFloat32(1.0f) / dgFloat32(pointCount));
dgFloat32 test = center[axis];
dgInt32 i1 = count - 1;
do {
for (; i0 <= i1; i0++) {
const dgNodeBase *const proxy = proxiArray[i0];
dgFloat32 val = (proxy->m_p0[axis] + proxy->m_p1[axis]) * dgFloat32(0.5f);
if (val > test) {
break;
}
}
for (; i1 >= i0; i1--) {
const dgNodeBase *const proxy = proxiArray[i1];
dgFloat32 val = (proxy->m_p0[axis] + proxy->m_p1[axis]) * dgFloat32(0.5f);
if (val < test) {
break;
}
}
if (i0 < i1) {
Swap(proxiArray[i0], proxiArray[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0) {
i0 = 1;
}
if (i0 >= count - 1) {
i0 = count - 1;
}
}
dgNodeBase *const left = BuildTopDownTree(i0, &proxiArray[0], id);
dgNodeBase *const right = BuildTopDownTree(count - i0, &proxiArray[i0], id);
tree = new (m_allocator) dgNodeBase(left, right, id);
left->m_parent = tree;
right->m_parent = tree;
id++;
}
return tree;
}
void dgCollisionCompound::PushNodes(dgNodeBase *const root,
dgNodeBase **const proxiArray, dgInt32 &index) const {
if (root->m_left) {
PushNodes(root->m_left, proxiArray, index);
}
if (root->m_right) {
PushNodes(root->m_right, proxiArray, index);
}
if (!root->m_shape) {
proxiArray[index] = root;
index++;
}
}
dgFloat32 dgCollisionCompound::CalculateSurfaceArea(dgNodeBase *const node0,
dgNodeBase *const node1, dgVector &minBox, dgVector &maxBox) const {
minBox = dgVector(GetMin(node0->m_p0.m_x, node1->m_p0.m_x),
GetMin(node0->m_p0.m_y, node1->m_p0.m_y),
GetMin(node0->m_p0.m_z, node1->m_p0.m_z), dgFloat32(0.0f));
maxBox = dgVector(GetMax(node0->m_p1.m_x, node1->m_p1.m_x),
GetMax(node0->m_p1.m_y, node1->m_p1.m_y),
GetMax(node0->m_p1.m_z, node1->m_p1.m_z), dgFloat32(0.0f));
dgVector side0((maxBox - minBox).Scale(dgFloat32(0.5f)));
dgVector side1(side0.m_y, side0.m_z, side0.m_x, dgFloat32(0.0f));
return side0 % side1;
}
void dgCollisionCompound::ImproveNodeFitness(dgNodeBase *const node) const {
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_area;
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)) {
dgNodeBase *const parent = node->m_parent;
node->m_p0 = parent->m_p0;
node->m_p1 = parent->m_p1;
node->m_area = parent->m_area;
node->m_size = parent->m_size;
node->m_origin = parent->m_origin;
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;
}
}
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_p0 = cost1P0;
parent->m_p1 = cost1P1;
parent->m_area = cost1;
parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32(0.5f));
parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32(0.5f));
} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
dgNodeBase *const parent = node->m_parent;
node->m_p0 = parent->m_p0;
node->m_p1 = parent->m_p1;
node->m_area = parent->m_area;
node->m_size = parent->m_size;
node->m_origin = parent->m_origin;
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;
}
}
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_p0 = cost2P0;
parent->m_p1 = cost2P1;
parent->m_area = cost2;
parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32(0.5f));
parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32(0.5f));
}
} else {
dgFloat32 cost0 = node->m_area;
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)) {
dgNodeBase *const parent = node->m_parent;
node->m_p0 = parent->m_p0;
node->m_p1 = parent->m_p1;
node->m_area = parent->m_area;
node->m_size = parent->m_size;
node->m_origin = parent->m_origin;
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;
}
}
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_p0 = cost1P0;
parent->m_p1 = cost1P1;
parent->m_area = cost1;
parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32(0.5f));
parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32(0.5f));
} else if ((cost2 <= cost0) && (cost2 <= cost1)) {
dgNodeBase *const parent = node->m_parent;
node->m_p0 = parent->m_p0;
node->m_p1 = parent->m_p1;
node->m_area = parent->m_area;
node->m_size = parent->m_size;
node->m_origin = parent->m_origin;
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;
}
}
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_p0 = cost2P0;
parent->m_p1 = cost2P1;
parent->m_area = cost2;
parent->m_size = (parent->m_p1 - parent->m_p0).Scale(dgFloat32(0.5f));
parent->m_origin = (parent->m_p1 + parent->m_p0).Scale(dgFloat32(0.5f));
}
}
} else {
// in the future I can handle this but it is too much work for little payoff
}
}
dgCollisionCompound::dgNodeBase *dgCollisionCompound::BuildTree(dgInt32 count,
dgCollisionConvex *const shapeArray[]) {
#if 0
dgStack<dgNodeBase *> nodeList(count * 2);
dgNodeBase **const proxiArray = &nodeList[0];
for (dgInt32 i = 0; i < count; i ++) {
proxiArray[i] = new (m_allocator) dgNodeBase(shapeArray[i], i);
}
dgNodeBase *tree = BuildBottomUpTree(count, proxiArray);
#else
dgStack<dgNodeBase *> nodeList(count);
dgNodeBase **const proxiArray = &nodeList[0];
for (dgInt32 i = 0; i < count; i++) {
proxiArray[i] = new (m_allocator) dgNodeBase(shapeArray[i], i);
}
dgInt32 id = count;
dgNodeBase *tree = BuildTopDownTree(count, proxiArray, id);
#endif
dgInt32 index = 0;
PushNodes(tree, proxiArray, index);
dgInt32 maxPasses = 2 * exp_2(index * 2) + 1;
dgFloat64 newCost = dgFloat32(1.0e20f);
dgFloat64 prevCost = newCost;
do {
prevCost = newCost;
for (dgInt32 i = 0; i < index; i++) {
dgNodeBase *const node = proxiArray[i];
ImproveNodeFitness(node);
}
newCost = dgFloat32(0.0f);
for (dgInt32 i = 0; i < index; i++) {
dgNodeBase *const node = proxiArray[i];
newCost += node->m_area;
}
maxPasses--;
} while (maxPasses && (newCost < prevCost));
if (tree->m_parent) {
NEWTON_ASSERT(index);
for (tree = proxiArray[index - 1]; tree->m_parent; tree = tree->m_parent)
;
}
return tree;
}
dgInt32 dgCollisionCompound::CalculateContacts(
dgCollidingPairCollector::dgPair *const pair, dgCollisionParamProxy &proxy,
dgInt32 useSimd) const {
dgInt32 contactCount = 0;
if (m_root) {
NEWTON_ASSERT(IsType(dgCollision::dgCollisionCompound_RTTI));
if (pair->m_body1->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
contactCount = CalculateContactsToSingle(pair, proxy, useSimd);
} else if (pair->m_body1->m_collision->IsType(
dgCollision::dgCollisionCompound_RTTI)) {
contactCount = CalculateContactsToCompound(pair, proxy, useSimd);
} else if (pair->m_body1->m_collision->IsType(
dgCollision::dgCollisionBVH_RTTI)) {
contactCount = CalculateContactsToCollisionTree(pair, proxy, useSimd);
} else if (pair->m_body1->m_collision->IsType(
dgCollision::dgCollisionHeightField_RTTI)) {
contactCount = CalculateContactsToHeightField(pair, proxy, useSimd);
} else {
NEWTON_ASSERT(
pair->m_body1->m_collision->IsType(dgCollision::dgCollisionUserMesh_RTTI));
contactCount = CalculateContactsBruteForce(pair, proxy, useSimd);
}
}
return contactCount;
}
dgInt32 dgCollisionCompound::CalculateContactsToSingle(
dgCollidingPairCollector::dgPair *const pair, dgCollisionParamProxy &proxy,
dgInt32 useSimd) const {
dgVector p0;
dgVector p1;
dgContactPoint *const contacts = pair->m_contactBuffer;
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
dgBody *const otherBody = pair->m_body1;
dgBody *const compoundBody = pair->m_body0;
NEWTON_ASSERT(pair->m_body0->m_collision == this);
NEWTON_ASSERT(
otherBody->m_collision->IsType(dgCollision::dgConvexCollision_RTTI));
// dgInt32 lru = m_world->m_broadPhaseLru;
proxy.m_referenceBody = compoundBody;
proxy.m_floatingBody = otherBody;
proxy.m_floatingCollision = otherBody->m_collision;
proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
dgInt32 contactCount = 0;
dgMatrix myMatrix(m_offset * compoundBody->m_matrix);
dgMatrix matrix(otherBody->m_collisionWorldMatrix * myMatrix.Inverse());
otherBody->m_collision->CalcAABB(dgGetIdentityMatrix(), p0, p1);
if (proxy.m_unconditionalCast) {
dgVector step(
(otherBody->m_veloc - compoundBody->m_veloc).Scale(proxy.m_timestep));
step = otherBody->m_collisionWorldMatrix.UnrotateVector(step);
for (dgInt32 j = 0; j < 3; j++) {
if (step[j] > dgFloat32(0.0f)) {
p1[j] += step[j];
} else {
p0[j] += step[j];
}
}
}
OOBBTestData data(matrix, p0, p1);
dgInt32 stack = 1;
stackPool[0] = m_root;
while (stack) {
stack--;
const dgNodeBase *const me = stackPool[stack];
NEWTON_ASSERT(me);
if (me->BoxTest(data)) {
if (me->m_type == m_leaf) {
dgInt32 processContacts;
processContacts = 1;
if (pair->m_material && pair->m_material->m_compoundAABBOverlap) {
processContacts = pair->m_material->m_compoundAABBOverlap(
reinterpret_cast<const NewtonMaterial *>(pair->m_material), reinterpret_cast<const NewtonBody *>(compoundBody), reinterpret_cast<const NewtonBody *>(otherBody),
proxy.m_threadIndex);
}
if (processContacts) {
proxy.m_referenceCollision = me->m_shape;
proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;
// proxy.m_floatingCollision = otherBody->m_collision;
// proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
proxy.m_contacts = &contacts[contactCount];
if (useSimd) {
contactCount += m_world->CalculateConvexToConvexContactsSimd(proxy);
} else {
contactCount += m_world->CalculateConvexToConvexContacts(proxy);
}
if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
contactCount = m_world->ReduceContacts(contactCount, contacts,
DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
}
}
} else {
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack] = me->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
}
}
}
return contactCount;
}
dgInt32 dgCollisionCompound::CalculateContactsToCompound(
dgCollidingPairCollector::dgPair *const pair, dgCollisionParamProxy &proxy,
dgInt32 useSimd) const {
dgContactPoint *const contacts = pair->m_contactBuffer;
const dgNodeBase *stackPool[4 * DG_COMPOUND_STACK_DEPTH][2];
dgInt32 contactCount = 0;
dgBody *const myBody = pair->m_body0;
dgBody *const otherBody = pair->m_body1;
NEWTON_ASSERT(pair->m_body0->m_collision == this);
dgCollisionCompound *const otherCompound =
(dgCollisionCompound *)otherBody->m_collision;
// dgInt32 lru = m_world->m_broadPhaseLru;
proxy.m_referenceBody = myBody;
proxy.m_floatingBody = otherBody;
dgMatrix myMatrix(m_offset * myBody->m_matrix);
dgMatrix otherMatrix(otherCompound->m_offset * otherBody->m_matrix);
OOBBTestData data(otherMatrix * myMatrix.Inverse());
dgInt32 stack = 1;
stackPool[0][0] = m_root;
stackPool[0][1] = otherCompound->m_root;
while (stack) {
stack--;
const dgNodeBase *const me = stackPool[stack][0];
const dgNodeBase *const other = stackPool[stack][1];
NEWTON_ASSERT(me && other);
if (me->BoxTest(data, other)) {
if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) {
dgInt32 processContacts;
processContacts = 1;
if (pair->m_material && pair->m_material->m_compoundAABBOverlap) {
processContacts = pair->m_material->m_compoundAABBOverlap(reinterpret_cast<const NewtonMaterial *>(pair->m_material), reinterpret_cast<const NewtonBody *>(myBody), reinterpret_cast<const NewtonBody *>(otherBody),
proxy.m_threadIndex);
}
if (processContacts) {
proxy.m_referenceCollision = me->m_shape;
proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;
proxy.m_floatingCollision = other->m_shape;
proxy.m_floatingMatrix = other->m_shape->m_offset * otherMatrix;
proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
proxy.m_contacts = &contacts[contactCount];
if (useSimd) {
contactCount += m_world->CalculateConvexToConvexContactsSimd(proxy);
} else {
contactCount += m_world->CalculateConvexToConvexContacts(proxy);
}
if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
contactCount = m_world->ReduceContacts(contactCount, contacts,
DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
}
}
} else if (me->m_type == m_leaf) {
// dgNode* const otherNode = (dgNode*)other;
NEWTON_ASSERT(other->m_type == m_node);
stackPool[stack][0] = me;
stackPool[stack][1] = other->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack][0] = me;
stackPool[stack][1] = other->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
} else if (other->m_type == m_leaf) {
// dgNode* const myNode = (dgNode*)me;
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
} else {
NEWTON_ASSERT(me->m_type == m_node);
NEWTON_ASSERT(other->m_type == m_node);
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
}
}
}
return contactCount;
}
dgInt32 dgCollisionCompound::CalculateContactsToCollisionTree(
dgCollidingPairCollector::dgPair *const pair, dgCollisionParamProxy &proxy,
dgInt32 useSimd) const {
dgContactPoint *const contacts = pair->m_contactBuffer;
dgTree<const dgNodeBase *, const dgNodeBase *> filter(m_allocator);
struct NodePairs {
dgNodeBase *m_myNode;
dgInt32 m_treeNodeIsLeaf;
const void *m_treeNode;
};
NodePairs stackPool[4 * DG_COMPOUND_STACK_DEPTH];
dgInt32 contactCount = 0;
dgBody *const myBody = pair->m_body0;
dgBody *const treeBody = pair->m_body1;
NEWTON_ASSERT(pair->m_body0->m_collision == this);
dgCollisionBVH *const treeCollision = (dgCollisionBVH *)treeBody->m_collision;
// dgInt32 lru = m_world->m_broadPhaseLru;
proxy.m_referenceBody = myBody;
proxy.m_floatingBody = treeBody;
proxy.m_floatingCollision = treeCollision;
proxy.m_floatingMatrix = treeBody->m_collisionWorldMatrix;
dgMatrix myMatrix(m_offset * myBody->m_matrix);
OOBBTestData data(proxy.m_floatingMatrix * myMatrix.Inverse());
dgInt32 stack = 1;
stackPool[0].m_myNode = m_root;
stackPool[0].m_treeNode = treeCollision->GetRootNode();
stackPool[0].m_treeNodeIsLeaf = 0;
dgNodeBase nodeProxi;
nodeProxi.m_left = NULL;
nodeProxi.m_right = NULL;
while (stack) {
dgInt32 treeNodeIsLeaf;
stack--;
dgNodeBase *const me = stackPool[stack].m_myNode;
const void *const other = stackPool[stack].m_treeNode;
treeNodeIsLeaf = stackPool[stack].m_treeNodeIsLeaf;
NEWTON_ASSERT(me && other);
treeCollision->GetNodeAABB(other, nodeProxi.m_p0, nodeProxi.m_p1);
nodeProxi.m_size = (nodeProxi.m_p1 - nodeProxi.m_p0).Scale(dgFloat32(0.5f));
nodeProxi.m_origin = (nodeProxi.m_p1 + nodeProxi.m_p0).Scale(dgFloat32(0.5f));
dgVector size(nodeProxi.m_size.m_y, nodeProxi.m_size.m_z,
nodeProxi.m_size.m_x, dgFloat32(0.0f));
nodeProxi.m_area = nodeProxi.m_size % size;
NEWTON_ASSERT(nodeProxi.m_area > dgFloat32(0.0f));
if (me->BoxTest(data, &nodeProxi)) {
if ((me->m_type == m_leaf) && treeNodeIsLeaf) {
if (!filter.Find(me)) {
m_world->dgGetUserLock();
filter.Insert(me, me);
m_world->dgReleasedUserLock();
// dgShapeCell* const myCell = (dgShapeCell*)me;
// m_world->dgGetIndirectLock(&myCell->m_criticalSection);
// if (myCell->m_lru != lru) {
// myCell->m_lru = lru;
// myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
// }
// m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);
proxy.m_referenceCollision = me->m_shape;
proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;
proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
proxy.m_contacts = &contacts[contactCount];
if (useSimd) {
contactCount += m_world->CalculateConvexToNonConvexContactsSimd(
proxy);
} else {
contactCount += m_world->CalculateConvexToNonConvexContacts(proxy);
}
if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
contactCount = m_world->ReduceContacts(contactCount, contacts,
DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
}
if (filter.GetCount() == m_count) {
break;
}
}
} else if (me->m_type == m_leaf) {
const void *const frontNode = treeCollision->GetFrontNode(other);
const void *const backNode = treeCollision->GetBackNode(other);
if (backNode && frontNode) {
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = backNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = frontNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
} else if (backNode && !frontNode) {
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = backNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
} else if (!backNode && frontNode) {
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = frontNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
} else {
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
}
} else if (treeNodeIsLeaf) {
stackPool[stack].m_myNode = me->m_left;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / (sizeof(dgNodeBase *))));
stackPool[stack].m_myNode = me->m_right;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / (sizeof(dgNodeBase *))));
} else if (nodeProxi.m_area > me->m_area) {
NEWTON_ASSERT(me->m_type == m_node);
const void *const frontNode = treeCollision->GetFrontNode(other);
const void *const backNode = treeCollision->GetBackNode(other);
if (backNode && frontNode) {
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = backNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = frontNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
} else if (backNode && !frontNode) {
stackPool[stack].m_myNode = (dgNodeBase *)me;
stackPool[stack].m_treeNode = backNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
stackPool[stack].m_myNode = me->m_left;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
stackPool[stack].m_myNode = me->m_right;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
} else if (!backNode && frontNode) {
stackPool[stack].m_myNode = me;
stackPool[stack].m_treeNode = frontNode;
stackPool[stack].m_treeNodeIsLeaf = 0;
stack++;
stackPool[stack].m_myNode = me->m_left;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
stackPool[stack].m_myNode = me->m_right;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
} else {
stackPool[stack].m_myNode = me->m_left;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
stackPool[stack].m_myNode = me->m_right;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = 1;
stack++;
}
} else {
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack].m_myNode = me->m_left;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf;
stack++;
stackPool[stack].m_myNode = me->m_right;
stackPool[stack].m_treeNode = other;
stackPool[stack].m_treeNodeIsLeaf = treeNodeIsLeaf;
stack++;
}
}
}
if (filter.GetCount()) {
m_world->dgGetUserLock();
filter.RemoveAll();
m_world->dgReleasedUserLock();
}
return contactCount;
}
dgInt32 dgCollisionCompound::CalculateContactsToHeightField(
dgCollidingPairCollector::dgPair *const pair, dgCollisionParamProxy &proxy,
dgInt32 useSimd) const {
dgNodeBase nodeProxi;
dgContactPoint *const contacts = pair->m_contactBuffer;
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
dgInt32 contactCount = 0;
dgBody *const myBody = pair->m_body0;
dgBody *const terrainBody = pair->m_body1;
NEWTON_ASSERT(pair->m_body0->m_collision == this);
dgCollisionHeightField *const terrainCollision =
(dgCollisionHeightField *)terrainBody->m_collision;
// dgInt32 lru = m_world->m_broadPhaseLru;
proxy.m_referenceBody = myBody;
proxy.m_floatingBody = terrainBody;
proxy.m_floatingCollision = terrainCollision;
proxy.m_floatingMatrix = terrainBody->m_collisionWorldMatrix;
dgMatrix myMatrix(m_offset * myBody->m_matrix);
OOBBTestData data(proxy.m_floatingMatrix * myMatrix.Inverse());
dgInt32 stack = 1;
stackPool[0] = m_root;
nodeProxi.m_left = NULL;
nodeProxi.m_right = NULL;
while (stack) {
stack--;
const dgNodeBase *const me = stackPool[stack];
dgVector origin(data.m_matrix.UntransformVector(me->m_origin));
dgVector size(data.m_absMatrix.UnrotateVector(me->m_size));
dgVector p0(origin - size);
dgVector p1(origin + size);
terrainCollision->GetLocalAABB(p0, p1, nodeProxi.m_p0, nodeProxi.m_p1);
nodeProxi.m_size = (nodeProxi.m_p1 - nodeProxi.m_p0).Scale(dgFloat32(0.5f));
nodeProxi.m_origin = (nodeProxi.m_p1 + nodeProxi.m_p0).Scale(dgFloat32(0.5f));
if (me->BoxTest(data, &nodeProxi)) {
if (me->m_type == m_leaf) {
// dgShapeCell* const myCell = (dgShapeCell*)me;
// m_world->dgGetIndirectLock(&myCell->m_criticalSection);
// if (myCell->m_lru != lru) {
// myCell->m_lru = lru;
// myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
// }
// m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);
proxy.m_referenceCollision = me->m_shape;
proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;
proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
proxy.m_contacts = &contacts[contactCount];
if (useSimd) {
contactCount += m_world->CalculateConvexToNonConvexContactsSimd(
proxy);
} else {
contactCount += m_world->CalculateConvexToNonConvexContacts(proxy);
}
if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
contactCount = m_world->ReduceContacts(contactCount, contacts,
DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
}
} else {
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
stackPool[stack] = me->m_right;
stack++;
}
}
}
return contactCount;
}
dgInt32 dgCollisionCompound::CalculateContactsBruteForce(
dgCollidingPairCollector::dgPair *const pair, dgCollisionParamProxy &proxy,
dgInt32 useSimd) const {
dgContactPoint *const contacts = pair->m_contactBuffer;
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
dgInt32 contactCount = 0;
dgBody *const myBody = pair->m_body0;
dgBody *const userBody = pair->m_body1;
NEWTON_ASSERT(pair->m_body0->m_collision == this);
dgCollisionMesh *const userCollision =
(dgCollisionMesh *)userBody->m_collision;
// dgInt32 lru = m_world->m_broadPhaseLru;
proxy.m_referenceBody = myBody;
proxy.m_floatingBody = userBody;
proxy.m_floatingCollision = userCollision;
proxy.m_floatingMatrix = userBody->m_collisionWorldMatrix;
dgMatrix myMatrix(m_offset * myBody->m_matrix);
dgInt32 stack = 1;
stackPool[0] = m_root;
dgNodeBase nodeProxi;
nodeProxi.m_left = NULL;
nodeProxi.m_right = NULL;
while (stack) {
stack--;
const dgNodeBase *const me = stackPool[stack];
if (me->m_type == m_leaf) {
// dgShapeCell* const myCell = (dgShapeCell*)me;
// m_world->dgGetIndirectLock(&myCell->m_criticalSection);
// if (myCell->m_lru != lru) {
// myCell->m_lru = lru;
// myCell->m_collisionMatrix = myCell->m_shape->m_offset * myMatrix;
// }
// m_world->dgReleaseIndirectLock(&myCell->m_criticalSection);
proxy.m_referenceCollision = me->m_shape;
proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;
proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
proxy.m_contacts = &contacts[contactCount];
if (useSimd) {
contactCount += m_world->CalculateConvexToNonConvexContactsSimd(proxy);
} else {
contactCount += m_world->CalculateConvexToNonConvexContacts(proxy);
}
if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
contactCount = m_world->ReduceContacts(contactCount, contacts,
DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
}
} else {
NEWTON_ASSERT(me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
stackPool[stack] = me->m_right;
stack++;
}
}
return contactCount;
}
dgInt32 dgCollisionCompound::ClosestDitance(dgBody *const compoundBody,
dgTriplex &contactA, dgBody *const bodyB, dgTriplex &contactB,
dgTriplex &normalAB) const {
if (!m_root) {
return 0;
}
#if 0
NEWTON_ASSERT(compoundBody->m_collision == this);
dgCollisionConvex *const collisionB = (dgCollisionConvex *) bodyB->m_collision;
// dgInt32 lru;
// dgInt32 stack;
// dgInt32 contactCount;
// dgBody* otherBody;
// dgBody* compoundBody;
// dgVector p0;
// dgVector p1;
// dgContactPoint* const contacts = pair->m_contactBuffer;
// const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];
// otherBody = pair->m_body1;
// compoundBody = pair->m_body0;
// NEWTON_ASSERT (pair->m_body0->m_collision == this);
// NEWTON_ASSERT (otherBody->m_collision->IsType (dgCollision::dgConvexCollision_RTTI));
// lru = m_world->m_broadPhaseLru;
// proxy.m_referenceBody = compoundBody;
// proxy.m_floatingBody = otherBody;
// proxy.m_floatingCollision = otherBody->m_collision;
// proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
// contactCount = 0;
dgVector p0;
dgVector p1;
dgMatrix myMatrix(m_offset * compoundBody->m_matrix);
dgMatrix matrix(bodyB->m_collisionWorldMatrix * myMatrix.Inverse());
collisionB->CalcAABB(dgGetIdentityMatrix(), p0, p1);
dgFloat32 distPool[DG_COMPOUND_STACK_DEPTH];
const dgNodeBase *stackPool[DG_COMPOUND_STACK_DEPTH];
dgVector points[8];
points[0] = dgVector(p0.m_x, p0.m_y, p0.m_z, dgFloat32(0.0f));
points[1] = dgVector(p0.m_x, p0.m_y, p1.m_z, dgFloat32(0.0f));
points[2] = dgVector(p0.m_x, p1.m_y, p0.m_z, dgFloat32(0.0f));
points[3] = dgVector(p0.m_x, p1.m_y, p1.m_z, dgFloat32(0.0f));
points[4] = dgVector(p1.m_x, p0.m_y, p0.m_z, dgFloat32(0.0f));
points[5] = dgVector(p1.m_x, p0.m_y, p1.m_z, dgFloat32(0.0f));
points[6] = dgVector(p1.m_x, p1.m_y, p0.m_z, dgFloat32(0.0f));
points[7] = dgVector(p1.m_x, p1.m_y, p1.m_z, dgFloat32(0.0f));
matrix.TransformTriplex(points, sizeof(dgVector), points, sizeof(dgVector), 8);
dgInt32 stack = 1;
stackPool[0] = m_root;
distPool[0] = m_root->BoxClosestDistance(points, 8);
dgFloat32 baseDist = dgFloat32(1.0e10f);
while (stack) {
stack --;
dgFloat32 dist = distPool[stack];
const dgNodeBase *const me = stackPool[stack];
NEWTON_ASSERT(me);
// if (me->BoxTest (data)) {
if (dist < baseDist) {
if (me->m_type == m_leaf) {
NEWTON_ASSERT(0);
/*
dgInt32 processContacts;
processContacts = 1;
if (pair->m_material && pair->m_material->m_compoundAABBOverlap) {
processContacts = pair->m_material->m_compoundAABBOverlap (*pair->m_material, *compoundBody, *otherBody, proxy.m_threadIndex);
}
if (processContacts) {
proxy.m_referenceCollision = me->m_shape;
proxy.m_referenceMatrix = me->m_shape->m_offset * myMatrix;
// proxy.m_floatingCollision = otherBody->m_collision;
// proxy.m_floatingMatrix = otherBody->m_collisionWorldMatrix;
proxy.m_maxContacts = DG_MAX_CONTATCS - contactCount;
proxy.m_contacts = &contacts[contactCount];
if (useSimd) {
contactCount += m_world->CalculateConvexToConvexContactsSimd (proxy);
} else {
contactCount += m_world->CalculateConvexToConvexContacts (proxy);
}
if (contactCount > (DG_MAX_CONTATCS - 2 * (DG_CONSTRAINT_MAX_ROWS / 3))) {
contactCount = m_world->ReduceContacts (contactCount, contacts, DG_CONSTRAINT_MAX_ROWS / 3, DG_REDUCE_CONTACT_TOLERANCE);
}
}
*/
} else {
const dgNodeBase *const left = me->m_left;
dgFloat32 leftDist = dgFloat32(0.0f);
if (left->m_type != m_leaf) {
leftDist = left->BoxClosestDistance(points, 8);
}
const dgNodeBase *const right = me->m_right;
dgFloat32 rightDist = dgFloat32(0.0f);
if (right->m_type != m_leaf) {
rightDist = right->BoxClosestDistance(points, 8);
}
NEWTON_ASSERT(me->m_type == m_node);
if (leftDist > rightDist) {
distPool[stack] = leftDist;
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
distPool[stack] = rightDist;
stackPool[stack] = me->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
} else {
distPool[stack] = rightDist;
stackPool[stack] = me->m_right;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
distPool[stack] = leftDist;
stackPool[stack] = me->m_left;
stack++;
NEWTON_ASSERT(stack < dgInt32(sizeof(stackPool) / sizeof(dgNodeBase *)));
}
}
}
}
return 0;
#endif
// this is temporary until I implement the code above using the spacial organization
dgInt32 retFlag = 1;
dgContactPoint contact0;
dgContactPoint contact1;
contact0.m_point = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
contact1.m_point = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
contact0.m_normal = dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f));
contact1.m_normal = dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f));
if (bodyB->m_collision->IsType(dgCollision::dgConvexCollision_RTTI)) {
dgCollisionParamProxy proxy(0);
dgContactPoint contacts[16];
proxy.m_referenceBody = compoundBody;
proxy.m_floatingBody = bodyB;
proxy.m_floatingCollision = bodyB->m_collision;
proxy.m_floatingMatrix = bodyB->m_collisionWorldMatrix;
proxy.m_timestep = dgFloat32(0.0f);
proxy.m_penetrationPadding = dgFloat32(0.0f);
proxy.m_unconditionalCast = 1;
proxy.m_continueCollision = 0;
proxy.m_maxContacts = 16;
proxy.m_contacts = &contacts[0];
dgMatrix myMatrix(m_offset * compoundBody->m_matrix);
dgFloat32 minDist2 = dgFloat32(1.0e10f);
for (dgInt32 i = 0; (i < m_count) && retFlag; i++) {
retFlag = 0;
proxy.m_referenceCollision = m_array[i];
proxy.m_referenceMatrix = m_array[i]->m_offset * myMatrix;
dgInt32 flag = m_world->ClosestPoint(proxy);
if (flag) {
retFlag = 1;
dgVector err(contacts[0].m_point - contacts[1].m_point);
dgFloat32 dist2 = err % err;
if (dist2 < minDist2) {
minDist2 = dist2;
contact0 = contacts[0];
contact1 = contacts[1];
}
}
}
} else {
dgCollisionParamProxy proxy(0);
dgContactPoint contacts[16];
NEWTON_ASSERT(
bodyB->m_collision->IsType(dgCollision::dgCollisionCompound_RTTI));
dgCollisionCompound *const compoundCollision1 =
(dgCollisionCompound *)bodyB->m_collision;
dgInt32 count1 = compoundCollision1->m_count;
dgCollisionConvex **collisionArray1 = compoundCollision1->m_array;
proxy.m_referenceBody = compoundBody;
proxy.m_floatingBody = bodyB;
proxy.m_timestep = dgFloat32(0.0f);
proxy.m_penetrationPadding = dgFloat32(0.0f);
proxy.m_unconditionalCast = 1;
proxy.m_continueCollision = 0;
proxy.m_maxContacts = 16;
proxy.m_contacts = &contacts[0];
dgMatrix myMatrix(m_offset * compoundBody->m_matrix);
dgMatrix otherMatrix(compoundCollision1->m_offset * bodyB->m_matrix);
dgFloat32 minDist2 = dgFloat32(1.0e10f);
for (dgInt32 i = 0; (i < m_count) && retFlag; i++) {
proxy.m_referenceCollision = m_array[i];
proxy.m_referenceMatrix = m_array[i]->m_offset * myMatrix;
for (dgInt32 j = 0; (j < count1) && retFlag; j++) {
retFlag = 0;
proxy.m_floatingCollision = collisionArray1[j];
proxy.m_floatingMatrix = collisionArray1[j]->m_offset * otherMatrix;
dgInt32 flag = m_world->ClosestPoint(proxy);
if (flag) {
retFlag = 1;
dgVector err(contacts[0].m_point - contacts[1].m_point);
dgFloat32 dist2 = err % err;
if (dist2 < minDist2) {
minDist2 = dist2;
contact0 = contacts[0];
contact1 = contacts[1];
}
}
}
}
}
if (retFlag) {
contactA.m_x = contact0.m_point.m_x;
contactA.m_y = contact0.m_point.m_y;
contactA.m_z = contact0.m_point.m_z;
contactB.m_x = contact1.m_point.m_x;
contactB.m_y = contact1.m_point.m_y;
contactB.m_z = contact1.m_point.m_z;
normalAB.m_x = contact0.m_normal.m_x;
normalAB.m_y = contact0.m_normal.m_y;
normalAB.m_z = contact0.m_normal.m_z;
}
return retFlag;
}
Reference in New Issue View Git Blame Copy Permalink