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