dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const { dgAssert (dir.m_w == dgFloat32 (0.0f)); dgAssert (dgAbs(dir.DotProduct(dir).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgFloat32 x = dir.GetScalar(); if (dgAbs (x) > dgFloat32 (0.9999f)) { //return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); return dgVector (dgSign (x) * m_height, m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f)); } dgVector sideDir (m_yzMask & dir); //sideDir = sideDir * sideDir.InvMagSqrt(); sideDir = sideDir.Normalize(); return sideDir.Scale(m_radius) + dir.Scale (m_height); }
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const { dgVector normal(normalIn); dgInt32 count = 0; dgFloat32 maxDist = dgFloat32 (1.0f); dgFloat32 projectFactor = m_normal % normal; if (projectFactor < dgFloat32 (0.0f)) { projectFactor *= dgFloat32 (-1.0f); normal = normal.Scale3 (dgFloat32 (-1.0f)); } if (projectFactor > dgFloat32 (0.9999f)) { for (dgInt32 i = 0; i < m_count; i ++) { contactsOut[count] = m_localPoly[i]; count ++; } #ifdef _DEBUG dgInt32 j = count - 1; for (dgInt32 i = 0; i < count; i ++) { dgVector error (contactsOut[i] - contactsOut[j]); dgAssert ((error % error) > dgFloat32 (1.0e-20f)); j = i; } #endif } else if (projectFactor > dgFloat32 (0.1736f)) { maxDist = dgFloat32 (0.0f); dgPlane plane (normal, - (normal % origin)); dgVector p0 (m_localPoly[m_count - 1]); dgFloat32 side0 = plane.Evalue (p0); for (dgInt32 i = 0; i < m_count; i ++) { dgVector p1 (m_localPoly[i]); dgFloat32 side1 = plane.Evalue (p1); if (side0 > dgFloat32 (0.0f)) { maxDist = dgMax (maxDist, side0); contactsOut[count] = p0 - plane.Scale3 (side0); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } if (side1 <= dgFloat32 (0.0f)) { dgVector dp (p1 - p0); dgFloat32 t = plane % dp; dgAssert (dgAbsf (t) >= dgFloat32 (0.0f)); if (dgAbsf (t) < dgFloat32 (1.0e-8f)) { t = dgSign(t) * dgFloat32 (1.0e-8f); } contactsOut[count] = p0 - dp.Scale3 (side0 / t); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } } } else if (side1 > dgFloat32 (0.0f)) { dgVector dp (p1 - p0); dgFloat32 t = plane % dp; dgAssert (dgAbsf (t) >= dgFloat32 (0.0f)); if (dgAbsf (t) < dgFloat32 (1.0e-8f)) { t = dgSign(t) * dgFloat32 (1.0e-8f); } contactsOut[count] = p0 - dp.Scale3 (side0 / t); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } } side0 = side1; p0 = p1; } } else { maxDist = dgFloat32 (1.0e10f); dgPlane plane (normal, - (normal % origin)); dgVector p0 (m_localPoly[m_count - 1]); dgFloat32 side0 = plane.Evalue (p0); for (dgInt32 i = 0; i < m_count; i ++) { dgVector p1 (m_localPoly[i]); dgFloat32 side1 = plane.Evalue (p1); if ((side0 * side1) < dgFloat32 (0.0f)) { dgVector dp (p1 - p0); dgFloat32 t = plane % dp; dgAssert (dgAbsf (t) >= dgFloat32 (0.0f)); if (dgAbsf (t) < dgFloat32 (1.0e-8f)) { t = dgSign(t) * dgFloat32 (1.0e-8f); } contactsOut[count] = p0 - dp.Scale3 (side0 / t); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } } side0 = side1; p0 = p1; } } if (count > 1) { if (maxDist < dgFloat32 (1.0e-3f)) { dgVector maxPoint (contactsOut[0]); dgVector minPoint (contactsOut[0]); dgVector lineDir (m_normal * normal); dgFloat32 proj = contactsOut[0] % lineDir; dgFloat32 maxProjection = proj; dgFloat32 minProjection = proj; for (dgInt32 i = 1; i < count; i ++) { proj = contactsOut[i] % lineDir; if (proj > maxProjection) { maxProjection = proj; maxPoint = contactsOut[i]; } if (proj < minProjection) { minProjection = proj; minPoint = contactsOut[i]; } } contactsOut[0] = maxPoint; contactsOut[1] = minPoint; count = 2; } dgVector error (contactsOut[count - 1] - contactsOut[0]); if ((error % error) < dgFloat32 (1.0e-8f)) { count --; } } #ifdef _DEBUG if (count > 1) { dgInt32 j = count - 1; for (dgInt32 i = 0; i < count; i ++) { dgVector error (contactsOut[i] - contactsOut[j]); dgAssert ((error % error) > dgFloat32 (1.0e-20f)); j = i; } if (count >= 3) { dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector e0 (contactsOut[1] - contactsOut[0]); for (dgInt32 i = 2; i < count; i ++) { dgVector e1 (contactsOut[i] - contactsOut[0]); n += e0 * e1; e0 = e1; } n = n.Scale3 (dgRsqrt(n % n)); dgFloat32 val = n % normal; dgAssert (val > dgFloat32 (0.9f)); } } #endif return count; }
void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix* const initialStretchAxis) const { // a polar decomposition decompose matrix A = O * S // where S = sqrt (transpose (L) * L) /* // calculate transpose (L) * L dgMatrix LL ((*this) * Transpose()); // check is this is a pure uniformScale * rotation * translation dgFloat32 det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * dgFloat32 (1.0f / 3.0f); dgFloat32 invdet2 = 1.0f / det2; dgMatrix pureRotation (LL); pureRotation[0] = pureRotation[0].Scale3 (invdet2); pureRotation[1] = pureRotation[1].Scale3 (invdet2); pureRotation[2] = pureRotation[2].Scale3 (invdet2); dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f; dgFloat32 det = (pureRotation[0] * pureRotation[1]) % pureRotation[2]; if (dgAbsf (det - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)) { // this is a pure scale * rotation * translation det = sign * dgSqrt (det2); scale[0] = det; scale[1] = det; scale[2] = det; det = dgFloat32 (1.0f)/ det; transformMatrix.m_front = m_front.Scale3 (det); transformMatrix.m_up = m_up.Scale3 (det); transformMatrix.m_right = m_right.Scale3 (det); transformMatrix[0][3] = dgFloat32 (0.0f); transformMatrix[1][3] = dgFloat32 (0.0f); transformMatrix[2][3] = dgFloat32 (0.0f); transformMatrix.m_posit = m_posit; stretchAxis = dgGetIdentityMatrix(); } else { stretchAxis = LL; stretchAxis.EigenVectors (scale); // I need to deal with by seeing of some of the Scale are duplicated // do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work) // for now just us the matrix scale[0] = sign * dgSqrt (scale[0]); scale[1] = sign * dgSqrt (scale[1]); scale[2] = sign * dgSqrt (scale[2]); scale[3] = dgFloat32 (0.0f); dgMatrix scaledAxis; scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]); scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]); scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]); scaledAxis[3] = stretchAxis[3]; dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis); transformMatrix = symetricInv * (*this); transformMatrix.m_posit = m_posit; } */ // test the f*****g factorization dgMatrix xxxxx(dgRollMatrix(30.0f * 3.1416f / 180.0f)); xxxxx = dgYawMatrix(30.0f * 3.1416f / 180.0f) * xxxxx; dgMatrix xxxxx1(dgGetIdentityMatrix()); xxxxx1[0][0] = 2.0f; dgMatrix xxxxx2(xxxxx.Inverse() * xxxxx1 * xxxxx); dgMatrix xxxxx3 (xxxxx2); xxxxx2.EigenVectors(scale); dgMatrix xxxxx4(xxxxx2.Inverse() * xxxxx1 * xxxxx2); //dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f; dgFloat32 sign = dgSign(((*this)[0] * (*this)[1]) % (*this)[2]); stretchAxis = (*this) * Transpose(); stretchAxis.EigenVectors (scale); // I need to deal with by seeing of some of the Scale are duplicated // do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work) // for now just us the matrix scale[0] = sign * dgSqrt (scale[0]); scale[1] = sign * dgSqrt (scale[1]); scale[2] = sign * dgSqrt (scale[2]); scale[3] = dgFloat32 (0.0f); dgMatrix scaledAxis; scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]); scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]); scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]); scaledAxis[3] = stretchAxis[3]; dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis); transformMatrix = symetricInv * (*this); transformMatrix.m_posit = m_posit; }
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 }