dgCollisionScene::dgProxy::dgProxy(dgCollision* m_shape, const dgMatrix& matrix,
dgCollisionScene* const owner) :
dgNode(), m_matrix(m_shape->GetOffsetMatrix() * matrix), m_userData(NULL), m_shape(
m_shape), m_owner(owner), m_myNode(NULL)
{
dgVector boxP0;
dgVector boxP1;
m_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::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
{
_ASSERTE(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;
}
dgFloat32 dgCollisionDeformableMesh::CalculateSurfaceArea (const dgDeformableNode* const node0, const dgDeformableNode* const node1, dgVector& minBox, dgVector& maxBox) const
{
minBox = dgVector (dgMin (node0->m_minBox.m_x, node1->m_minBox.m_x), dgMin (node0->m_minBox.m_y, node1->m_minBox.m_y), dgMin (node0->m_minBox.m_z, node1->m_minBox.m_z), dgFloat32 (0.0f));
maxBox = dgVector (dgMax (node0->m_maxBox.m_x, node1->m_maxBox.m_x), dgMax (node0->m_maxBox.m_y, node1->m_maxBox.m_y), dgMax (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::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 CalculateBox (const dgVector* const position, const dgInt32* const faceIndices)
{
dgVector p0;
dgVector p1;
TriangleBox (position, faceIndices, p0, p1);
p0 = p0.CompProduct3(dgVector (DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, dgFloat32 (0.0f)));
p1 = p1.CompProduct3(dgVector (DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, dgFloat32 (0.0f)));
m_minBox.m_x = dgFloor (p0.m_x) * DG_DEFORMABLE_INV_PADDING;
m_minBox.m_y = dgFloor (p0.m_y) * DG_DEFORMABLE_INV_PADDING;
m_minBox.m_z = dgFloor (p0.m_z) * DG_DEFORMABLE_INV_PADDING;
m_minBox.m_w = dgFloat32 (0.0f);
m_maxBox.m_x = dgFloor (p1.m_x + dgFloat32 (1.0f)) * DG_DEFORMABLE_INV_PADDING;
m_maxBox.m_y = dgFloor (p1.m_y + dgFloat32 (1.0f)) * DG_DEFORMABLE_INV_PADDING;
m_maxBox.m_z = dgFloor (p1.m_z + dgFloat32 (1.0f)) * DG_DEFORMABLE_INV_PADDING;
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;
}
dgInt32 UpdateBox (const dgVector* const position, const dgInt32* const faceIndices)
{
dgVector p0;
dgVector p1;
TriangleBox (position, faceIndices, p0, p1);
p0 = p0.CompProduct3(dgVector (DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, dgFloat32 (0.0f)));
p1 = p1.CompProduct3(dgVector (DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, DG_DEFORMABLE_PADDING, dgFloat32 (0.0f)));
dgVector minP (dgFloor (p0.m_x) * DG_DEFORMABLE_INV_PADDING, dgFloor (p0.m_y) * DG_DEFORMABLE_INV_PADDING, dgFloor (p0.m_z) * DG_DEFORMABLE_INV_PADDING, dgFloat32(0.0f));
dgVector maxP (dgFloor (p1.m_x + dgFloat32 (1.0f)) * DG_DEFORMABLE_INV_PADDING,
dgFloor (p1.m_y + dgFloat32 (1.0f)) * DG_DEFORMABLE_INV_PADDING,
dgFloor (p1.m_z + dgFloat32 (1.0f)) * DG_DEFORMABLE_INV_PADDING, dgFloat32(0.0f));
dgInt32 state = dgCompareBox (minP, maxP, m_minBox, m_maxBox);
if (state) {
m_minBox = minP;
m_maxBox = maxP;
dgVector side0 (m_maxBox - m_minBox);
dgVector side1 (side0.m_y, side0.m_z, side0.m_x, dgFloat32 (0.0f));
m_surfaceArea = side0 % side1;
}
return state;
}