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;
}
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);
}
}
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;
}
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;
}
