bool dgCollisionConvexHull::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
bool ret;
_ASSERTE (cacheOrder);
ret = dgCollisionConvex::OOBBTest (matrix, shape, cacheOrder);
if (ret) {
const dgConvexSimplexEdge* const* faceArray = m_faceArray;
dgCollisionBoundPlaneCache* const cache = (dgCollisionBoundPlaneCache*)cacheOrder;
for (dgInt32 i = 0; i < dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)); i ++) {
dgFloat32 dist;
const dgPlane& plane = cache->m_planes[i];
if ((plane % plane) > dgFloat32 (0.0f)) {
dgVector dir (matrix.UnrotateVector(plane.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
dist = plane.Evalue (p);
if (dist > dgFloat32 (0.1f)){
return false;
}
}
}
for (dgInt32 i = 0; i < m_boundPlanesCount; i ++) {
dgInt32 i0;
dgInt32 i1;
dgInt32 i2;
dgFloat32 dist;
const dgConvexSimplexEdge* const face = faceArray[i];
i0 = face->m_prev->m_vertex;
i1 = face->m_vertex;
i2 = face->m_next->m_vertex;
const dgVector& p0 = m_vertex[i0];
dgVector normal ((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));
dgVector dir (matrix.UnrotateVector(normal.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
//_ASSERTE ((normal % (m_boxOrigin - p0)) < 0.0f);
dist = normal % (p - p0);
if (dist > dgFloat32 (0.1f)){
for (dgInt32 j = 0; j < (dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)) - 1); j ++) {
cache->m_planes[j + 1] = cache->m_planes[j];
}
cache->m_planes[1] = dgPlane (normal, - (normal % p0));
return false;
}
}
}
return ret;
}
void dgCollisionScene::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
dgVector origin (matrix.TransformVector(m_boxOrigin));
dgVector size (m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
dgFloat32 (0.0f));
p0 = origin - size;
p1 = origin + size;
#ifdef DG_DEBUG_AABB
dgInt32 i;
dgVector q0;
dgVector q1;
dgMatrix trans (matrix.Transpose());
for (i = 0; i < 3; i ++) {
q0[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i].Scale (-1.0f)))[i];
q1[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i]))[i];
}
dgVector err0 (p0 - q0);
dgVector err1 (p1 - q1);
dgFloat32 err;
err = GetMax (size.m_x, size.m_y, size.m_z) * 0.5f;
_ASSERTE ((err0 % err0) < err);
_ASSERTE ((err1 % err1) < err);
#endif
}
void dgCollisionDeformableMesh::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
dgVector origin (matrix.TransformVector(m_boxOrigin));
dgVector size (matrix.m_front.Abs().Scale4(m_boxSize.m_x) + matrix.m_up.Abs().Scale4(m_boxSize.m_y) + matrix.m_right.Abs().Scale4(m_boxSize.m_z));
p0 = (origin - size) & dgVector::m_triplexMask;
p1 = (origin + size) & dgVector::m_triplexMask;
}
void dgCollisionDeformableSolidMesh::SetMatrix(const dgMatrix& matrix)
{
dgAssert (m_myBody);
//dgMatrix globalMatrix (GetBody()->GetCollision()->GetLocalMatrix() * matrix);
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_particles.m_posit[i] = m_shapePosit[i];
m_posit[i] = matrix.TransformVector(m_shapePosit[i]) & dgVector::m_triplexMask;
}
}
void dgCollisionDeformableMesh::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
const dgVector* const particlePosit = m_particles.m_posit;
for (dgInt32 i = 0; i < m_trianglesCount; i ++ ) {
dgTriplex points[3];
for (dgInt32 j = 0; j < 3; j ++) {
dgInt32 index = m_indexList[i * 3 + j];
dgVector p (matrix.TransformVector(particlePosit[index]));
points[j].m_x = p.m_x;
points[j].m_y = p.m_y;
points[j].m_z = p.m_z;
}
callback (userData, 3, &points[0].m_x, 0);
}
}
