示例#1
0
void dgCollisionSphere::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
	dgTriplex pool[1024 * 2];
	dgVector tmpVectex[1024 * 2];

	dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
	dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));

	dgInt32 i = 3;
	dgInt32 count = 0;
	TesselateTriangle (i, p4, p0, p2, count, tmpVectex);
	TesselateTriangle (i, p4, p2, p1, count, tmpVectex);
	TesselateTriangle (i, p4, p1, p3, count, tmpVectex);
	TesselateTriangle (i, p4, p3, p0, count, tmpVectex);
	TesselateTriangle (i, p5, p2, p0, count, tmpVectex);
	TesselateTriangle (i, p5, p1, p2, count, tmpVectex);
	TesselateTriangle (i, p5, p3, p1, count, tmpVectex);
	TesselateTriangle (i, p5, p0, p3, count, tmpVectex);

	for (dgInt32 i = 0; i < count; i ++) {
		tmpVectex[i] = tmpVectex[i].Scale4 (m_radius);
	}

	//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &tmpVectex[0].m_x, sizeof (dgVector), count);
	for (dgInt32 i = 0; i < count; i += 3) {
		callback (userData, 3, &pool[i].m_x, 0);
	}
}
void dgCollisionChamferCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;

	dgInt32 slices = 12;
	dgInt32 brakes = 24;
	dgFloat32 sliceAngle = dgFloat32 (0.0f);
	dgFloat32 sliceStep = dgPI  / slices; 
	dgFloat32 breakStep = dgPI2 / brakes;

	dgTriplex pool[24 * (12 + 1)];

	dgMatrix rot (dgPitchMatrix (breakStep));	
	dgInt32 index = 0;
	for (dgInt32 j = 0; j <= slices; j ++) {
		dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
		sliceAngle += sliceStep;
		for (dgInt32 i = 0; i < brakes; i ++) {
			pool[index].m_x = p0.m_x;
			pool[index].m_y = p0.m_y;
			pool[index].m_z = p0.m_z;
			index ++;
			p0 = rot.UnrotateVector (p0);
		}
	}

	//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * (12 + 1));

	dgTriplex face[32];

	index = 0;
	for (dgInt32 j = 0; j < slices; j ++) {
		dgInt32 index0 = index + brakes - 1;
		for (dgInt32 i = 0; i < brakes; i ++) {
			face[0] = pool[index];
			face[1] = pool[index0];
			face[2] = pool[index0 + brakes];
			face[3] = pool[index + brakes];
			index0 = index;
			index ++;
			callback (userData, 4, &face[0].m_x, 0);
		}
	}

	for (dgInt32 i = 0; i < brakes; i ++) { 
		face[i] = pool[i];
	}
	callback (userData, 24, &face[0].m_x, 0);

	for (dgInt32 i = 0; i < brakes; i ++) { 
		face[i] = pool[brakes * (slices + 1) - i - 1];
	}
	callback (userData, 24, &face[0].m_x, 0);
}
void dgCollisionTaperedCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
	dgTriplex pool[24 * 2];

	dgFloat32 angle = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < 24; i ++) {
		dgFloat32 z = dgSin (angle);
		dgFloat32 y = dgCos (angle);
		pool[i].m_x = - m_height;
		pool[i].m_y = y * m_radio1;
		pool[i].m_z = z * m_radio1;
		pool[i + 24].m_x = m_height;
		pool[i + 24].m_y = y * m_radio0;
		pool[i + 24].m_z = z * m_radio0;
		angle += dgPI2 / dgFloat32 (24.0f);
	}

	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * 2);

	dgTriplex face[24];

	dgInt32 j = 24 - 1;
	for (dgInt32 i = 0; i < 24; i ++) { 
		face[0] = pool[j];
		face[1] = pool[i];
		face[2] = pool[i + 24];
		face[3] = pool[j + 24];
		j = i;
		callback (userData, 4, &face[0].m_x, 0);
	}

	for (dgInt32 i = 0; i < 24; i ++) { 
		face[i] = pool[24 - 1 - i];
	}
	callback (userData, 24, &face[0].m_x, 0);
		
	for (dgInt32 i = 0; i < 24; i ++) { 
		face[i] = pool[i + 24];
	}
	callback (userData, 24, &face[0].m_x, 0);
}
void dgCollisionCone::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;

	#define NUMBER_OF_DEBUG_SEGMENTS  40
	dgTriplex pool[NUMBER_OF_DEBUG_SEGMENTS + 1];
	dgTriplex face[NUMBER_OF_DEBUG_SEGMENTS];

	dgFloat32 angle = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
		dgFloat32 z = dgSin (angle) * m_radius;
		dgFloat32 y = dgCos (angle) * m_radius;
		pool[i].m_x = -m_height;
		pool[i].m_y = y;
		pool[i].m_z = z;
		angle += dgPI2 / dgFloat32 (NUMBER_OF_DEBUG_SEGMENTS);
	}
	
	pool[NUMBER_OF_DEBUG_SEGMENTS].m_x = m_height;
	pool[NUMBER_OF_DEBUG_SEGMENTS].m_y = dgFloat32 (0.0f);
	pool[NUMBER_OF_DEBUG_SEGMENTS].m_z = dgFloat32 (0.0f);

	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), NUMBER_OF_DEBUG_SEGMENTS + 1);
	dgInt32 j = NUMBER_OF_DEBUG_SEGMENTS - 1;
	for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { 
		face[0] = pool[j];
		face[1] = pool[i];
		face[2] = pool[NUMBER_OF_DEBUG_SEGMENTS];
		j = i;
		callback (userData, 3, &face[0].m_x, 0);
	}

	for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { 
		face[i] = pool[NUMBER_OF_DEBUG_SEGMENTS - 1 - i];
	}
	callback (userData, NUMBER_OF_DEBUG_SEGMENTS, &face[0].m_x, 0);
}
void dgCollisionConvexHull::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//	dgTriplex tmp[1024 * 4];
//	matrix.TransformTriplex (&tmp[0].m_x, sizeof (dgTriplex), &m_vertex[0].m_x, sizeof (dgVector), m_vertexCount);

	dgTriplex vertex[256];
	for (dgInt32 i = 0; i < m_faceCount; i ++) {
		dgConvexSimplexEdge* const face = m_faceArray[i];
		dgConvexSimplexEdge* ptr = face;
		dgInt32 count = 0;
		do {
			//vertex[count] = tmp[ptr->m_vertex];
			vertex[count].m_x = m_vertex[ptr->m_vertex].m_x;
			vertex[count].m_y = m_vertex[ptr->m_vertex].m_y;
			vertex[count].m_z = m_vertex[ptr->m_vertex].m_z;
			count ++;
			dgAssert (count < sizeof (vertex)/ sizeof (vertex[0]));
			ptr = ptr->m_next;
		} while (ptr != face);
		matrix.TransformTriplex (&vertex[0].m_x, sizeof (dgTriplex), &vertex[0].m_x, sizeof (dgTriplex), count);
		callback (userData, count, &vertex[0].m_x, 0);
	}
}
void dgCollisionTaperedCapsule::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
	#define POWER 2
	dgVector tmpVectex[1024 * 2];

	dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
	dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));

	dgInt32 count = 0;
	TesselateTriangle (POWER, p0, p2, p4, count, tmpVectex);
	TesselateTriangle (POWER, p0, p4, p3, count, tmpVectex);
	TesselateTriangle (POWER, p0, p3, p5, count, tmpVectex);
	TesselateTriangle (POWER, p0, p5, p2, count, tmpVectex);

	TesselateTriangle (POWER, p1, p4, p2, count, tmpVectex);
	TesselateTriangle (POWER, p1, p3, p4, count, tmpVectex);
	TesselateTriangle (POWER, p1, p5, p3, count, tmpVectex);
	TesselateTriangle (POWER, p1, p2, p5, count, tmpVectex);

	dgFloat32 scale = m_radio1 / m_radio0;
	dgVector edgeP0(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector edgeP1(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));

	for (dgInt32 i = 0; i < count; i += 3) {
		dgVector face0[4]; 	
		dgVector face1[4]; 	
		dgInt32 n0 = 0;
		dgInt32 n1 = 0;
		dgVector p0 (tmpVectex[i + 2]);
		for (dgInt32 j = 0; j < 3; j ++) {
			dgVector p1 (tmpVectex[i + j]);
			if (p1.m_x > m_clip0) {
				if (p0.m_x < m_clip0) {
					dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
					edgeP0 = p0 + (p1 - p0).Scale3 (t);
					edgeP0.m_x = m_clip0;

					face0[n0] = edgeP0;
					face0[n0].m_x += m_height;
					n0 ++;

					face1[n1] = edgeP0.Scale3 (scale);
					face1[n1].m_x -= m_height;
					n1 ++;
				}
				face0[n0] = p1;
				face0[n0].m_x += m_height;
				n0 ++;
			} else {
				if (p0.m_x > m_clip0) {
					dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
					edgeP1 = p0 + (p1 - p0).Scale3 (t);
					edgeP1.m_x = m_clip0;

					face0[n0] = edgeP1;
					face0[n0].m_x += m_height;
					n0 ++;

					face1[n1] = edgeP1.Scale3 (scale);
					face1[n1].m_x -= m_height;
					n1 ++;
				}
				face1[n1] = p1.Scale3 (scale);;
				face1[n1].m_x -= m_height;
				
				n1 ++;
			}
			p0 = p1;
		}

		dgTriplex face[4];
		if (n0) {
			matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), n0);			
			callback (userData, n0, &face[0].m_x, 0);
		}
		if (n1) {
			matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face1[0].m_x, sizeof (dgVector), n1);
			callback (userData, n1, &face[0].m_x, 0);
		}
		if (n0 && n1) {
			face0[0] = edgeP0;
			face0[1] = edgeP1;
			face0[2] = edgeP1.Scale3 (scale);
			face0[3] = edgeP0.Scale3 (scale);
			face0[0].m_x += m_height;
			face0[1].m_x += m_height;
			face0[2].m_x -= m_height;
			face0[3].m_x -= m_height;

			dgPlane plane (face0[0], face0[1], face0[2]);
			if (plane.m_w >= dgFloat32 (0.0f)) {
				dgAssert (0);
			}

			matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), 4);
			callback (userData, 4, &face[0].m_x, 0);
		}
	}
}
void dgPolygonSoupDatabaseBuilder::AddMesh (const hacd::HaF32* const vertex, hacd::HaI32 vertexCount, hacd::HaI32 strideInBytes, hacd::HaI32 faceCount,	
	const hacd::HaI32* const faceArray, const hacd::HaI32* const indexArray, const hacd::HaI32* const faceTagsData, const dgMatrix& worldMatrix) 
{
	hacd::HaI32 faces[256];
	hacd::HaI32 pool[2048];


	m_vertexPoints[m_vertexCount + vertexCount].m_x = hacd::HaF64 (0.0f);
	dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount];

	worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount);
	for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
		vertexPool[i].m_w = hacd::HaF64 (0.0f);
	}

	hacd::HaI32 totalIndexCount = faceCount;
	for (hacd::HaI32 i = 0; i < faceCount; i ++) {
		totalIndexCount += faceArray[i];
	}

	m_vertexIndex[m_indexCount + totalIndexCount] = 0;
	m_faceVertexCount[m_faceCount + faceCount] = 0;

	hacd::HaI32 k = 0;
	for (hacd::HaI32 i = 0; i < faceCount; i ++) {
		hacd::HaI32 count = faceArray[i];
		for (hacd::HaI32 j = 0; j < count; j ++) {
			hacd::HaI32 index = indexArray[k];
			pool[j] = index + m_vertexCount;
			k ++;
		}

		hacd::HaI32 convexFaces = 0;
		if (count == 3) {
			convexFaces = 1;
			dgBigVector p0 (m_vertexPoints[pool[2]]);
			for (hacd::HaI32 i = 0; i < 3; i ++) {
				dgBigVector p1 (m_vertexPoints[pool[i]]);
				dgBigVector edge (p1 - p0);
				hacd::HaF64 mag2 = edge % edge;
				if (mag2 < hacd::HaF32 (1.0e-6f)) {
					convexFaces = 0;
				}
				p0 = p1;
			}

			if (convexFaces) {
				dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
				dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
				dgBigVector normal (edge0 * edge1);
				hacd::HaF64 mag2 = normal % normal;
				if (mag2 < hacd::HaF32 (1.0e-8f)) {
					convexFaces = 0;
				}
			}

			if (convexFaces) {
				faces[0] = 3;
			}

		} else {
			convexFaces = AddConvexFace (count, pool, faces);
		}

		hacd::HaI32 index = 0;
		for (hacd::HaI32 k = 0; k < convexFaces; k ++) {
			hacd::HaI32 count = faces[k];
			m_vertexIndex[m_indexCount] = faceTagsData[i];
			m_indexCount ++;
			for (hacd::HaI32 j = 0; j < count; j ++) {
				m_vertexIndex[m_indexCount] = pool[index];
				index ++;
				m_indexCount ++;
			}
			m_faceVertexCount[m_faceCount] = count + 1;
			m_faceCount ++;
		}
	}
	m_vertexCount += vertexCount;
	m_run -= vertexCount;
	if (m_run <= 0) {
		PackArray();
	}
}