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;
}
C_FUNC_DEF double v_tri_aspect_frobenius( int /*num_nodes*/, double coordinates[][3] )
{
static const double two_times_root_of_3 = 2*sqrt(3.0);
// three vectors for each side
VerdictVector side1( coordinates[1][0] - coordinates[0][0],
coordinates[1][1] - coordinates[0][1],
coordinates[1][2] - coordinates[0][2] );
VerdictVector side2( coordinates[2][0] - coordinates[1][0],
coordinates[2][1] - coordinates[1][1],
coordinates[2][2] - coordinates[1][2] );
VerdictVector side3( coordinates[0][0] - coordinates[2][0],
coordinates[0][1] - coordinates[2][1],
coordinates[0][2] - coordinates[2][2] );
//sum the lengths squared of each side
double srms = (side1.length_squared() + side2.length_squared()
+ side3.length_squared());
// find two times the area of the triangle by cross product
double areaX2 = ((side1 * (-side3)).length());
if(areaX2 == 0.0)
return (double)VERDICT_DBL_MAX;
double aspect = (double)(srms / (two_times_root_of_3 * (areaX2)));
if( aspect > 0 )
return (double) VERDICT_MIN( aspect, VERDICT_DBL_MAX );
return (double) VERDICT_MAX( aspect, -VERDICT_DBL_MAX );
}
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);
}
}
/*!
The area of a tri
0.5 * jacobian at a node
*/
C_FUNC_DEF double v_tri_area( int /*num_nodes*/, double coordinates[][3] )
{
// two vectors for two sides
VerdictVector side1( coordinates[1][0] - coordinates[0][0],
coordinates[1][1] - coordinates[0][1],
coordinates[1][2] - coordinates[0][2] );
VerdictVector side3( coordinates[2][0] - coordinates[0][0],
coordinates[2][1] - coordinates[0][1],
coordinates[2][2] - coordinates[0][2] );
// the cross product of the two vectors representing two sides of the
// triangle
VerdictVector tmp = side1 * side3;
// return the magnitude of the vector divided by two
double area = 0.5 * tmp.length();
if( area > 0 )
return (double) VERDICT_MIN( area, VERDICT_DBL_MAX );
return (double) VERDICT_MAX( area, -VERDICT_DBL_MAX );
}
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;
}