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