void dgCollisionCone::Init (dgFloat32 radius, dgFloat32 height)
{
// dgInt32 i;
// dgInt32 j;
// dgEdge *edge;
// dgFloat32 y;
// dgFloat32 z;
// dgFloat32 angle;
m_rtti |= dgCollisionCone_RTTI;
m_radius = dgAbsf (radius);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_sinAngle = m_radius / dgSqrt (height * height + m_radius * m_radius);;
m_amp = dgFloat32 (0.5f) * m_radius / m_height;
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle) * m_radius;
dgFloat32 y = dgCos (angle) * m_radius;
m_vertex[i] = dgVector (- m_height, y, z, dgFloat32 (1.0f));
angle += dgPI2 / DG_CONE_SEGMENTS;
}
m_vertex[DG_CONE_SEGMENTS] = dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f));
m_edgeCount = DG_CONE_SEGMENTS * 4;
m_vertexCount = DG_CONE_SEGMENTS + 1;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CONE_SEGMENTS];
dgInt32 j = DG_CONE_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = DG_CONE_SEGMENTS;
j = i;
polyhedra.AddFace (3, wireframe);
}
for (dgInt32 i = 0; i < DG_CONE_SEGMENTS; i ++) {
wireframe[i] = DG_CONE_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CONE_SEGMENTS, wireframe);
polyhedra.EndFace ();
_ASSERTE (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator)
{
m_rtti |= dgCollisionSphere_RTTI;
m_radius = radius;
m_edgeCount = DG_SPHERE_EDGE_COUNT;
m_vertexCount = DG_SPHERE_VERTEX_COUNT;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgInt32 indexList[256];
dgVector tmpVectex[256];
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 = 1;
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);
//dgAssert (count == EDGE_COUNT);
dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f);
dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT);
for (dgInt32 i = 0; i < vertexCount; i ++) {
m_unitSphere[i] = tmpVectex[i];
}
dgPolyhedra polyhedra(m_allocator);
polyhedra.BeginFace();
for (dgInt32 i = 0; i < count; i += 3) {
#ifdef _DEBUG
dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
dgAssert (edge);
#else
polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
#endif
}
polyhedra.EndFace();
dgUnsigned64 i1 = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i1;
i1 ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) {
m_vertex[i] = m_unitSphere[i].Scale4 (m_radius);
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgCollisionCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionCylinder_RTTI;
m_radio0 = dgMax (dgAbsf (radio0), dgFloat32 (2.0f) * D_CYLINDER_SKIN_THINCKNESS);
m_radio1 = dgMax (dgAbsf (radio1), dgFloat32 (2.0f) * D_CYLINDER_SKIN_THINCKNESS);
m_height = dgMax (dgAbsf (height * dgFloat32 (0.5f)), dgFloat32 (2.0f) * D_CYLINDER_SKIN_THINCKNESS);
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
dgFloat32 sinAngle = dgSin (angle);
dgFloat32 cosAngle = dgCos (angle);
m_vertex[i ] = dgVector (- m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f));
m_vertex[i + DG_TAPED_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
angle += dgPI2 / DG_TAPED_CYLINDER_SEGMENTS;
}
m_edgeCount = DG_TAPED_CYLINDER_SEGMENTS * 6;
m_vertexCount = DG_TAPED_CYLINDER_SEGMENTS * 2;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_TAPED_CYLINDER_SEGMENTS];
dgInt32 j = DG_TAPED_CYLINDER_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = i + DG_TAPED_CYLINDER_SEGMENTS;
wireframe[3] = j + DG_TAPED_CYLINDER_SEGMENTS;
j = i;
polyhedra.AddFace (4, wireframe);
}
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = DG_TAPED_CYLINDER_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_TAPED_CYLINDER_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_TAPED_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = i + DG_TAPED_CYLINDER_SEGMENTS;
}
polyhedra.AddFace (DG_TAPED_CYLINDER_SEGMENTS, wireframe);
polyhedra.EndFace ();
dgAssert (SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i ++;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_profile[0] = dgVector( m_height, m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_profile[1] = dgVector(-m_height, m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_profile[2] = dgVector(-m_height, -m_radio0, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_profile[3] = dgVector(m_height, -m_radio1, dgFloat32 (0.0f), dgFloat32 (0.0f));
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgCollisionBox::Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z)
{
m_rtti |= dgCollisionBox_RTTI;
m_size[0].m_x = dgAbsf (size_x) * dgFloat32 (0.5f);
m_size[0].m_y = dgAbsf (size_y) * dgFloat32 (0.5f);
m_size[0].m_z = dgAbsf (size_z) * dgFloat32 (0.5f);
m_size[0].m_w = dgFloat32 (0.0f);
m_size[1].m_x = - m_size[0].m_x;
m_size[1].m_y = - m_size[0].m_y;
m_size[1].m_z = - m_size[0].m_z;
m_size[1].m_w = dgFloat32 (0.0f);
m_edgeCount = 24;
m_vertexCount = 8;
m_vertex[0] = dgVector ( m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[1] = dgVector (-m_size[0].m_x, m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[2] = dgVector ( m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[3] = dgVector (-m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[4] = dgVector ( m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[5] = dgVector (-m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[6] = dgVector ( m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
m_vertex[7] = dgVector (-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
dgCollisionConvex::m_vertex = m_vertex;
dgCollisionConvex::m_simplex = m_edgeArray;
if (!m_initSimplex) {
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgInt32 i = 0; i < 6; i ++) {
polyhedra.AddFace (4, &m_faces[i][0]);
}
polyhedra.EndFace();
int index = 0;
dgInt32 mark = polyhedra.IncLRU();;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = index;
index ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
dgAssert (index == 24);
polyhedra.IncLRU();
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = ptr->m_incidentVertex;
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
m_vertexToEdgeMap[edge->m_incidentVertex] = &m_simplex[edge->m_userData];
}
dgInt32 count = 0;
mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
edge->m_mark = mark;
edge->m_twin->m_mark = mark;
m_edgeEdgeMap[count] = &m_simplex[edge->m_userData];
count ++;
dgAssert (count <= 12);
}
}
m_initSimplex = 1;
}
SetVolumeAndCG ();
}
void dgCollisionChamferCylinder::Init(dgFloat32 radius, dgFloat32 height)
{
// dgInt32 i;
// dgInt32 j;
// dgInt32 index;
// dgInt32 index0;
// dgFloat32 sliceStep;
// dgFloat32 sliceAngle;
// dgFloat32 breakStep;
// dgFloat32 breakAngle;
// dgEdge *edge;
m_rtti |= dgCollisionChamferCylinder_RTTI;
m_radius = dgAbsf(radius);
m_height = dgAbsf(height * dgFloat32(0.5f));
m_radius = GetMax(dgFloat32(0.001f), m_radius - m_height);
m_silhuette[0] = dgVector(m_height, m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
m_silhuette[1] = dgVector(m_height, -m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
m_silhuette[2] = dgVector(-m_height, -m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
m_silhuette[3] = dgVector(-m_height, m_radius, dgFloat32(0.0f),
dgFloat32(0.0f));
// m_tethaStep = GetDiscretedAngleStep (m_radius);
// m_tethaStepInv = dgFloat32 (1.0f) / m_tethaStep;
// m_delCosTetha = dgCos (m_tethaStep);
// m_delSinTetha = dgSin (m_tethaStep);
dgFloat32 sliceAngle = dgFloat32(0.0f);
//dgFloat32 breakAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / DG_CHAMFERCYLINDER_SLICES;
dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES;
dgMatrix rot(dgPitchMatrix(breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j++)
{
dgVector p0(-m_height * dgCos(sliceAngle), dgFloat32(0.0f),
m_radius + m_height * dgSin(sliceAngle), dgFloat32(1.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
m_vertex[index] = p0;
index++;
p0 = rot.UnrotateVector(p0);
}
}
m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2) * DG_CHAMFERCYLINDER_BRAKES;
m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1);
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount)
{
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10];
for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i++)
{
dgMatrix matrix(
dgPitchMatrix(
dgFloat32(dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT));
m_shapesDirs[i] = matrix.RotateVector(
dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f),
dgFloat32(0.0f)));
}
dgInt32 index = 0;
for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j++)
{
dgInt32 index0 = index + DG_CHAMFERCYLINDER_BRAKES - 1;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
wireframe[0] = index;
wireframe[1] = index0;
wireframe[2] = index0 + DG_CHAMFERCYLINDER_BRAKES;
wireframe[3] = index + DG_CHAMFERCYLINDER_BRAKES;
index0 = index;
index++;
polyhedra.AddFace(4, wireframe);
}
}
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
wireframe[i] = i;
}
polyhedra.AddFace(DG_CHAMFERCYLINDER_BRAKES, wireframe);
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++)
{
wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1)
- i - 1;
}
polyhedra.AddFace(DG_CHAMFERCYLINDER_BRAKES, wireframe);
polyhedra.EndFace();
_ASSERTE(SanityCheck (polyhedra));
dgUnsigned64 i = 0;
dgPolyhedra::Iterator iter(polyhedra);
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
edge->m_userData = i;
i++;
}
for (iter.Begin(); iter; iter++)
{
dgEdge* const edge = &(*iter);
dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
ptr->m_vertex = edge->m_incidentVertex;
ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
}
}
m_shapeRefCount++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG();
// CalculateDistanceTravel();
}