void dgConvexHull4d::TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const
{
if (level) {
dgAssert (dgAbsf (p0.DotProduct3(p0) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1.DotProduct3(p1) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2.DotProduct3(p2) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
p01 = p01.Scale3 (dgRsqrt(p01.DotProduct3(p01)));
p12 = p12.Scale3 (dgRsqrt(p12.DotProduct3(p12)));
p20 = p20.Scale3 (dgRsqrt(p20.DotProduct3(p20)));
dgAssert (dgAbsf (p01.DotProduct3(p01) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12.DotProduct3(p12) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20.DotProduct3(p20) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TessellateTriangle (level - 1, p0, p01, p20, count, ouput, start);
TessellateTriangle (level - 1, p1, p12, p01, count, ouput, start);
TessellateTriangle (level - 1, p2, p20, p12, count, ouput, start);
TessellateTriangle (level - 1, p01, p12, p20, count, ouput, start);
} else {
dgBigPlane n (p0, p1, p2);
n = n.Scale (dgFloat64(1.0f) / sqrt (n.DotProduct3(n)));
n.m_w = dgFloat64(0.0f);
ouput[start] = n;
start += 8;
count ++;
}
}
dgVector dgCollisionBox::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir.DotProduct3(dir) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dir.m_w == dgFloat32 (0.0f));
dgVector mask (dir < dgVector (dgFloat32 (0.0f)));
if (vertexIndex) {
dgVector index (m_indexMark.CompProduct4(mask & dgVector::m_one));
index = (index.AddHorizontal()).GetInt();
*vertexIndex = dgInt32 (index.m_ix);
}
return (m_size[1] & mask) + m_size[0].AndNot(mask);
}
dgVector dgCollisionBox::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert(dgAbsf(dir.DotProduct3(dir) - dgFloat32(1.0f)) < dgFloat32(1.0e-3f));
dgAssert(dir.m_w == dgFloat32(0.0f));
dgVector mask(dir < dgVector(dgFloat32(0.0f)));
if (vertexIndex) {
dgVector index(m_indexMark.CompProduct4(mask & dgVector::m_one));
index = (index.AddHorizontal()).GetInt();
*vertexIndex = dgInt32 (index.m_ix);
}
dgVector padd (D_BOX_SKIN_THINCKNESS);
padd = padd & dgVector::m_triplexMask;
dgVector size0 (m_size[0] - padd);
dgVector size1 (m_size[1] + padd);
return (size1 & mask) + size0.AndNot(mask);
}
dgVector dgCollisionBox::SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const
{
dgAssert(dgAbsf((dir.DotProduct3(dir) - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
return point + dir.Scale4 (D_BOX_SKIN_THINCKNESS);
}
