void dgBallConstraint::SetLimits(const dgVector& coneDir,
dgFloat32 minConeAngle, dgFloat32 maxConeAngle, dgFloat32 maxTwistAngle,
const dgVector& bilateralDir, dgFloat32 negativeBilateralConeAngle__,
dgFloat32 positiveBilateralConeAngle__)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle(matrix0, matrix1);
_ASSERTE(m_body0);
_ASSERTE(m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
dgVector lateralDir(bilateralDir * coneDir);
if ((lateralDir % lateralDir) < dgFloat32(1.0e-3f))
{
dgMatrix tmp(coneDir);
lateralDir = tmp.m_up;
}
m_localMatrix0.m_front = body0_Matrix.UnrotateVector(coneDir);
m_localMatrix0.m_up = body0_Matrix.UnrotateVector(lateralDir);
m_localMatrix0.m_posit = body0_Matrix.UntransformVector(matrix1.m_posit);
m_localMatrix0.m_front =
m_localMatrix0.m_front.Scale(
dgFloat32(
1.0f) / dgSqrt (m_localMatrix0.m_front % m_localMatrix0.m_front));
m_localMatrix0.m_up = m_localMatrix0.m_up.Scale(
dgFloat32(1.0f) / dgSqrt (m_localMatrix0.m_up % m_localMatrix0.m_up));
m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up;
m_localMatrix0.m_front.m_w = dgFloat32(0.0f);
m_localMatrix0.m_up.m_w = dgFloat32(0.0f);
m_localMatrix0.m_right.m_w = dgFloat32(0.0f);
m_localMatrix0.m_posit.m_w = dgFloat32(1.0f);
// dgMatrix body1_Matrix (dgGetIdentityMatrix());
// if (m_body1) {
// body1_Matrix = m_body1->GetMatrix();
// }
const dgMatrix& body1_Matrix = m_body1->GetMatrix();
m_twistAngle = ClampValue(maxTwistAngle, dgFloat32(5.0f) * dgDEG2RAD,
dgFloat32(90.0f) * dgDEG2RAD);
m_coneAngle = ClampValue((maxConeAngle - minConeAngle) * dgFloat32(0.5f),
dgFloat32(5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD);
m_coneAngleCos = dgCos (m_coneAngle);
dgMatrix coneMatrix(
dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32(0.5f)));
m_localMatrix0 = coneMatrix * m_localMatrix0;
m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse();
}
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);
}
dgQuaternion::dgQuaternion (const dgVector &unitAxis, dgFloat32 angle)
{
angle *= dgFloat32 (0.5f);
m_q0 = dgCos (angle);
dgFloat32 sinAng = dgSin (angle);
#ifdef _DEBUG
if (dgAbsf (angle) > dgFloat32(dgEPSILON / 10.0f)) {
dgAssert (dgAbsf (dgFloat32(1.0f) - unitAxis % unitAxis) < dgFloat32(dgEPSILON * 10.0f));
}
#endif
m_q1 = unitAxis.m_x * sinAng;
m_q2 = unitAxis.m_y * sinAng;
m_q3 = unitAxis.m_z * sinAng;
}
void dgCollisionCone::DebugCollision (const dgMatrix& matrixPtr, OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgInt32 i;
dgInt32 j;
dgFloat32 y;
dgFloat32 z;
dgFloat32 angle;
#define NUMBER_OF_DEBUG_SEGMENTS 24
dgTriplex pool[NUMBER_OF_DEBUG_SEGMENTS + 1];
dgTriplex face[NUMBER_OF_DEBUG_SEGMENTS];
angle = dgFloat32 (0.0f);
for (i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
z = dgSin (angle) * m_radius;
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[i].m_x = m_height;
pool[i].m_y = dgFloat32 (0.0f);
pool[i].m_z = dgFloat32 (0.0f);
// const dgMatrix &matrix = myBody.GetCollisionMatrix();
dgMatrix matrix (GetOffsetMatrix() * matrixPtr);
matrix.TransformTriplex (pool, sizeof (dgTriplex), pool, sizeof (dgTriplex), NUMBER_OF_DEBUG_SEGMENTS + 1);
j = NUMBER_OF_DEBUG_SEGMENTS - 1;
for (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 (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 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 dgCollisionTaperedCapsule::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionTaperedCapsule_RTTI;
m_radio0 = dgAbsf (radio0);
m_radio1 = dgAbsf (radio1);
m_height = dgAbsf (height * 0.5f);
m_clip1 = dgFloat32 (0.0f);
m_clip0 = dgFloat32 (0.0f);
dgFloat32 angle0 = dgFloat32 (-3.141592f / 2.0f);
dgFloat32 angle1 = dgFloat32 ( 3.141592f / 2.0f);
do {
dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f);
dgVector dir (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p0(dir.Scale3 (m_radio0));
dgVector p1(dir.Scale3 (m_radio1));
p0.m_x += m_height;
p1.m_x -= m_height;
dgFloat32 dir0 = p0 % dir;
dgFloat32 dir1 = p1 % dir;
if (dir0 > dir1) {
angle1 = angle;
m_clip0 = p0.m_x - m_height;
} else {
angle0 = angle;
m_clip1 = p1.m_x + m_height;
}
} while ((angle1 - angle0) > dgFloat32 (0.001f * 3.141592f/180.0f));
dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f);
m_sideNormal = dgVector (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f));
m_clipRadio0 = dgSqrt (m_radio0 * m_radio0 - m_clip0 * m_clip0);
m_clipRadio1 = dgSqrt (m_radio1 * m_radio1 - m_clip1 * m_clip1);
dgInt32 i1 = 0;
dgInt32 i0 = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;
dgFloat32 dx0 = (m_clip0 - m_radio0) / DG_CAP_SEGMENTS;
dgFloat32 x0 = m_radio0 + dx0;
dgFloat32 dx1 = (m_clip1 + m_radio1) / DG_CAP_SEGMENTS;
dgFloat32 x1 = -m_radio1 + dx1;
for (dgInt32 j = 0; j < DG_CAP_SEGMENTS; j ++) {
dgFloat32 angle = dgFloat32 (0.0f);
dgFloat32 r0 = dgSqrt (m_radio0 * m_radio0 - x0 * x0);
dgFloat32 r1 = dgSqrt (m_radio1 * m_radio1 - x1 * x1);
i0 -= DG_CAPSULE_SEGMENTS;
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle);
dgFloat32 y = dgCos (angle);
m_vertex[i0] = dgVector ( m_height + x0, y * r0, z * r0, dgFloat32 (0.0f));
m_vertex[i1] = dgVector (-m_height + x1, y * r1, z * r1, dgFloat32 (0.0f));
i0 ++;
i1 ++;
angle += dgPI2 / DG_CAPSULE_SEGMENTS;
}
x0 += dx0;
x1 += dx1;
i0 -= DG_CAPSULE_SEGMENTS;
}
m_vertexCount = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;
m_edgeCount = DG_CAPSULE_SEGMENTS * (6 + 8 * (DG_CAP_SEGMENTS - 1));
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CAPSULE_SEGMENTS + 10];
i1 = 0;
i0 = DG_CAPSULE_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 j = 0; j < DG_CAP_SEGMENTS * 2 - 1; j ++) {
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
wireframe[0] = i0;
wireframe[1] = i1;
wireframe[2] = i1 + DG_CAPSULE_SEGMENTS;
wireframe[3] = i0 + DG_CAPSULE_SEGMENTS;
i0 = i1;
i1 ++;
polyhedra.AddFace (4, wireframe);
}
i0 = i1 + DG_CAPSULE_SEGMENTS - 1;
}
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
wireframe[i] = DG_CAPSULE_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) {
wireframe[i] = i + DG_CAPSULE_SEGMENTS * (DG_CAP_SEGMENTS * 2 - 1);
}
polyhedra.AddFace (DG_CAPSULE_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_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
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 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();
}
void dgCollisionTaperedCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
m_rtti |= dgCollisionTaperedCylinder_RTTI;
m_radio0 = dgAbsf (radio0);
m_radio1 = dgAbsf (radio1);
m_height = dgAbsf (height * dgFloat32 (0.5f));
m_dirVector.m_x = radio1 - radio0;
m_dirVector.m_y = m_height * dgFloat32 (2.0f);
m_dirVector.m_z = dgFloat32 (0.0f);
m_dirVector.m_w = dgFloat32 (0.0f);
m_dirVector = m_dirVector.Scale3(dgRsqrt(m_dirVector % m_dirVector));
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
dgFloat32 sinAngle = dgSin (angle);
dgFloat32 cosAngle = dgCos (angle);
m_vertex[i ] = dgVector (- m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector ( m_height, m_radio0 * cosAngle, m_radio0 * sinAngle, dgFloat32 (0.0f));
angle += dgPI2 / DG_CYLINDER_SEGMENTS;
}
m_edgeCount = DG_CYLINDER_SEGMENTS * 6;
m_vertexCount = DG_CYLINDER_SEGMENTS * 2;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CYLINDER_SEGMENTS];
dgInt32 j = DG_CYLINDER_SEGMENTS - 1;
polyhedra.BeginFace ();
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[0] = j;
wireframe[1] = i;
wireframe[2] = i + DG_CYLINDER_SEGMENTS;
wireframe[3] = j + DG_CYLINDER_SEGMENTS;
j = i;
polyhedra.AddFace (4, wireframe);
}
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i;
}
polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
wireframe[i] = i + DG_CYLINDER_SEGMENTS;
}
polyhedra.AddFace (DG_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_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgMatrix::CalcPitchYawRoll (dgVector& euler0, dgVector& euler1) const
{
const dgMatrix& matrix = *this;
dgAssert (dgAbsf (((matrix[0] * matrix[1]) % matrix[2]) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
// Assuming the angles are in radians.
if (matrix[0][2] > dgFloat32 (0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32 (-3.141592f * 0.5f);
dgFloat32 roll0 = - dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else if (matrix[0][2] < dgFloat32 (-0.99995f)) {
dgFloat32 picth0 = dgFloat32 (0.0f);
dgFloat32 yaw0 = dgFloat32 (3.141592f * 0.5f);
dgFloat32 roll0 = dgAtan2(matrix[2][1], matrix[1][1]);
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth0;
euler1[1] = yaw0;
euler1[2] = roll0;
} else {
dgFloat32 yaw0 = -dgAsin ( matrix[0][2]);
dgFloat32 yaw1 = dgFloat32 (3.141592f) - yaw0;
dgFloat32 sign0 = dgSign(dgCos (yaw0));
dgFloat32 sign1 = dgSign(dgCos (yaw1));
dgFloat32 picth0 = dgAtan2(matrix[1][2] * sign0, matrix[2][2] * sign0);
dgFloat32 picth1 = dgAtan2(matrix[1][2] * sign1, matrix[2][2] * sign1);
dgFloat32 roll0 = dgAtan2(matrix[0][1] * sign0, matrix[0][0] * sign0);
dgFloat32 roll1 = dgAtan2(matrix[0][1] * sign1, matrix[0][0] * sign1);
if (yaw1 > dgFloat32 (3.141592f)) {
yaw1 -= dgFloat32 (2.0f * 3.141592f);
}
euler0[0] = picth0;
euler0[1] = yaw0;
euler0[2] = roll0;
euler1[0] = picth1;
euler1[1] = yaw1;
euler1[2] = roll1;
}
euler0[3] = dgFloat32(0.0f);
euler1[3] = dgFloat32(0.0f);
#ifdef _DEBUG
dgMatrix m0 (dgPitchMatrix (euler0[0]) * dgYawMatrix(euler0[1]) * dgRollMatrix(euler0[2]));
dgMatrix m1 (dgPitchMatrix (euler1[0]) * dgYawMatrix(euler1[1]) * dgRollMatrix(euler1[2]));
for (int i = 0; i < 3; i ++) {
for (int j = 0; j < 3; j ++) {
dgFloat32 error = dgAbsf (m0[i][j] - matrix[i][j]);
dgAssert (error < 5.0e-2f);
error = dgAbsf (m1[i][j] - matrix[i][j]);
dgAssert (error < 5.0e-2f);
}
}
#endif
}
