dgVector dgMatrix::CalcPitchYawRoll () const
{
const hacd::HaF32 minSin = hacd::HaF32(0.99995f);
const dgMatrix& matrix = *this;
hacd::HaF32 roll = hacd::HaF32(0.0f);
hacd::HaF32 pitch = hacd::HaF32(0.0f);
hacd::HaF32 yaw = dgAsin (-ClampValue (matrix[0][2], hacd::HaF32(-0.999999f), hacd::HaF32(0.999999f)));
HACD_ASSERT (dgCheckFloat (yaw));
if (matrix[0][2] < minSin) {
if (matrix[0][2] > (-minSin)) {
roll = dgAtan2 (matrix[0][1], matrix[0][0]);
pitch = dgAtan2 (matrix[1][2], matrix[2][2]);
} else {
pitch = dgAtan2 (matrix[1][0], matrix[1][1]);
}
} else {
pitch = -dgAtan2 (matrix[1][0], matrix[1][1]);
}
#ifdef _DEBUG
dgMatrix m (dgPitchMatrix (pitch) * dgYawMatrix(yaw) * dgRollMatrix(roll));
for (hacd::HaI32 i = 0; i < 3; i ++) {
for (hacd::HaI32 j = 0; j < 3; j ++) {
hacd::HaF32 error = dgAbsf (m[i][j] - matrix[i][j]);
HACD_ASSERT (error < 5.0e-2f);
}
}
#endif
return dgVector (pitch, yaw, roll, hacd::HaF32(0.0f));
}
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);
}
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 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
}
