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