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::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);
}
}
void dgCollisionScene::ImproveNodeFitness(dgNode* const node)
{
_ASSERTE(node->m_left);
_ASSERTE(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
{
_ASSERTE(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
{
_ASSERTE(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
{
_ASSERTE(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
{
_ASSERTE(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;
}
}
}
_ASSERTE(!m_rootNode->m_parent);
}
void dgCollisionDeformableMesh::ImproveNodeFitness (dgDeformableNode* const node)
{
dgAssert (node->m_left);
dgAssert (node->m_right);
if (!node->m_parent) {
node->m_surfaceArea = CalculateSurfaceArea (node->m_left, node->m_right, node->m_minBox, node->m_maxBox);
} else {
if (node->m_parent->m_left == node) {
dgFloat32 cost0 = CalculateSurfaceArea (node->m_left, node->m_right, node->m_minBox, node->m_maxBox);
node->m_surfaceArea = cost0;
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)) {
dgDeformableNode* 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 {
dgAssert (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)) {
dgDeformableNode* 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 {
dgAssert (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 = CalculateSurfaceArea (node->m_left, node->m_right, node->m_minBox, node->m_maxBox);
node->m_surfaceArea = cost0;
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)) {
dgDeformableNode* 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 {
dgAssert (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)) {
dgDeformableNode* 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 {
dgAssert (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;
}
}
}
dgAssert (!m_rootNode->m_parent);
}
dgCollisionDeformableMesh::dgDeformableNode* dgCollisionDeformableMesh::BuildTopDown (dgInt32 count, dgDeformableNode* const children, dgDeformableNode* const parent)
{
dgDeformableNode* root = NULL;
if (count == 1) {
root = children;
root->m_left = NULL;
root->m_right = NULL;
root->m_parent = parent;
} else if (count == 2) {
root = &m_nodesMemory[m_nodesCount];
m_nodesCount ++;
root->m_indexStart = -1;
root->m_parent = parent;
root->m_left = BuildTopDown (1, children, root);
root->m_right = BuildTopDown (1, &children[1], root);
root->m_surfaceArea = CalculateSurfaceArea (root->m_left, root->m_right, root->m_minBox, root->m_maxBox);
} else {
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 dgDeformableNode* const node = &children[i];
dgVector p ((node->m_minBox + node->m_maxBox).Scale3 (0.5f));
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (count)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat32 maxVarian = dgFloat32 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (count));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = count - 1;
do {
for (; i0 <= i1; i0 ++) {
const dgDeformableNode* const node = &children[i0];
dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f);
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
const dgDeformableNode* const node = &children[i1];
dgFloat32 val = (node->m_minBox[index] + node->m_maxBox[index]) * dgFloat32 (0.5f);
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(children[i0], children[i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 > 0){
i0 --;
}
if ((i0 + 1) >= count) {
i0 = count - 2;
}
dgInt32 spliteCount = i0 + 1;
root = &m_nodesMemory[m_nodesCount];
m_nodesCount ++;
root->m_indexStart = -1;
root->m_parent = parent;
root->m_left = BuildTopDown (spliteCount, children, root);
root->m_right = BuildTopDown (count - spliteCount, &children[spliteCount], root);
root->m_surfaceArea = CalculateSurfaceArea (root->m_left, root->m_right, root->m_minBox, root->m_maxBox);
}
return root;
}
