Exemplo n.º 1
0
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);
	}
}