void dgBilateralConstraint::SetPivotAndPinDir (const dgVector& pivot, const dgVector& pinDirection0, const dgVector& pinDirection1)
{
_ASSERTE (m_body0);
_ASSERTE (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
_ASSERTE ((pinDirection0 % pinDirection0) > dgFloat32 (0.0f));
m_localMatrix0.m_front = pinDirection0.Scale (dgFloat32 (1.0f) / dgSqrt (pinDirection0 % pinDirection0));
m_localMatrix0.m_right = m_localMatrix0.m_front * pinDirection1;
m_localMatrix0.m_right = m_localMatrix0.m_right.Scale (dgFloat32 (1.0f) / dgSqrt (m_localMatrix0.m_right % m_localMatrix0.m_right));
m_localMatrix0.m_up = m_localMatrix0.m_right * m_localMatrix0.m_front;
m_localMatrix0.m_posit = pivot;
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_localMatrix1 = m_localMatrix0 * body1_Matrix.Inverse();
m_localMatrix0 = m_localMatrix0 * body0_Matrix.Inverse();
}
dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count = 0;
const dgFloat32 inclination = dgFloat32 (0.9999f);
if (normal.m_x < -inclination) {
dgMatrix matrix(normal);
dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f)));
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius + x);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else if (normal.m_x > inclination) {
dgMatrix matrix(normal);
dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f)));
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius + x);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else {
count = 1;
contactsOut[0] = SupportVertex (normal, NULL);
}
return count;
}
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 dgCollisionInstance::SetGlobalScale (const dgVector& scale)
{
// calculate current matrix
dgMatrix matrix(dgGetIdentityMatrix());
matrix[0][0] = m_scale.m_x;
matrix[1][1] = m_scale.m_y;
matrix[2][2] = m_scale.m_z;
matrix = m_aligmentMatrix * matrix * m_localMatrix;
// extract the original local matrix
dgMatrix transpose (matrix.Transpose());
dgVector globalScale (dgSqrt (transpose[0].DotProduct(transpose[0]).GetScalar()), dgSqrt (transpose[1].DotProduct(transpose[1]).GetScalar()), dgSqrt (transpose[2].DotProduct(transpose[2]).GetScalar()), dgFloat32 (1.0f));
dgVector invGlobalScale (dgFloat32 (1.0f) / globalScale.m_x, dgFloat32 (1.0f) / globalScale.m_y, dgFloat32 (1.0f) / globalScale.m_z, dgFloat32 (1.0f));
dgMatrix localMatrix (m_aligmentMatrix.Transpose() * m_localMatrix);
localMatrix.m_posit = matrix.m_posit * invGlobalScale;
dgAssert (localMatrix.m_posit.m_w == dgFloat32 (1.0f));
if ((dgAbs (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbs (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) {
m_localMatrix = localMatrix;
m_localMatrix.m_posit = m_localMatrix.m_posit * scale | dgVector::m_wOne;
m_aligmentMatrix = dgGetIdentityMatrix();
SetScale (scale);
} else {
// create a new scale matrix
localMatrix[0] = localMatrix[0] * scale;
localMatrix[1] = localMatrix[1] * scale;
localMatrix[2] = localMatrix[2] * scale;
localMatrix[3] = localMatrix[3] * scale;
localMatrix[3][3] = dgFloat32 (1.0f);
// decompose into to align * scale * local
localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix);
m_localMatrix = m_aligmentMatrix * m_localMatrix;
m_aligmentMatrix = m_aligmentMatrix.Transpose();
dgAssert (m_localMatrix.TestOrthogonal());
dgAssert (m_aligmentMatrix.TestOrthogonal());
//dgMatrix xxx1 (dgGetIdentityMatrix());
//xxx1[0][0] = m_scale.m_x;
//xxx1[1][1] = m_scale.m_y;
//xxx1[2][2] = m_scale.m_z;
//dgMatrix xxx (m_aligmentMatrix * xxx1 * m_localMatrix);
bool isIdentity = true;
for (dgInt32 i = 0; i < 3; i ++) {
isIdentity &= dgAbs (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f);
isIdentity &= dgAbs (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f);
}
m_scaleType = isIdentity ? m_nonUniform : m_global;
m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]);
m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f));
}
}
dgQuaternion::dgQuaternion (const dgMatrix& matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
if (trace > dgFloat32(0.0f)) {
trace = dgSqrt (trace + dgFloat32(1.0f));
m_q0 = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dgSqrt (trace);
dgFloat32* const ptr = &m_q1;
ptr[i] = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#ifdef _DEBUG
dgMatrix tmp (*this, matrix.m_posit);
dgMatrix unitMatrix (tmp * matrix.Inverse());
for (dgInt32 i = 0; i < 4; i ++) {
dgFloat32 err = dgAbsf (unitMatrix[i][i] - dgFloat32(1.0f));
dgAssert (err < dgFloat32 (1.0e-2f));
}
dgFloat32 err = dgAbsf (DotProduct(*this) - dgFloat32(1.0f));
dgAssert (err < dgFloat32(dgEPSILON * 100.0f));
#endif
}
dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
if (dir.m_x < dgFloat32(-0.9999f)) {
return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else if (dir.m_x > dgFloat32(0.9999f)) {
return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else {
dgVector dir_yz (dir);
dir_yz.m_x = dgFloat32 (0.0f);
dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar();
dgAssert (mag2 > dgFloat32 (0.0f));
dir_yz = dir_yz.Scale4 (dgFloat32 (1.0f) / dgSqrt (mag2));
dgVector p0 (dir_yz.Scale4 (m_radio0));
dgVector p1 (dir_yz.Scale4 (m_radio1));
p0.m_x = -m_height;
p1.m_x = m_height;
dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar();
dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar();
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
}
void dgBilateralConstraint::CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1)
{
dgFloat32 length;
_ASSERTE (m_body0);
_ASSERTE (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
length = dir % dir;
length = dgSqrt (length);
_ASSERTE (length > dgFloat32 (0.0f));
// matrix0.m_front = body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length));
// Create__Basis (matrix0.m_front, matrix0.m_up, matrix0.m_right);
matrix0 = dgMatrix (body0_Matrix.UnrotateVector (dir.Scale (dgFloat32 (1.0f) / length)));
matrix0.m_posit = body0_Matrix.UntransformVector (pivot);
matrix0.m_front.m_w = dgFloat32 (0.0f);
matrix0.m_up.m_w = dgFloat32 (0.0f);
matrix0.m_right.m_w = dgFloat32 (0.0f);
matrix0.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();
matrix1 = matrix0 * body0_Matrix * body1_Matrix.Inverse();
}
dgVector dgCollisionCone::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert(dgAbsf((dir % dir - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
if (dir.m_x < dgFloat32(-0.9999f)) {
return dgVector(-(m_height - D_CONE_SKIN_THINCKNESS), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else if (dir.m_x > dgFloat32(0.9999f)) {
return dgVector(m_height - D_CONE_SKIN_THINCKNESS, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else {
dgVector dir_yz(dir);
dir_yz.m_x = dgFloat32(0.0f);
dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar();
dgAssert(mag2 > dgFloat32(0.0f));
dir_yz = dir_yz.Scale4(dgFloat32(1.0f) / dgSqrt(mag2));
dgVector p0(dir_yz.Scale4(m_radius - D_CONE_SKIN_THINCKNESS));
dgVector p1(dgVector::m_zero);
p0.m_x = -(m_height - D_CONE_SKIN_THINCKNESS);
p1.m_x = m_height - D_CONE_SKIN_THINCKNESS;
dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar();
dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar();
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
}
dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
dgVector lateralDir (matrix0.m_front * matrix1.m_front);
dgInt32 ret = 0;
dgFloat32 mag = lateralDir % lateralDir;
if (mag > dgFloat32 (1.0e-6f)) {
mag = dgSqrt (mag);
lateralDir = lateralDir.Scale3 (dgFloat32 (1.0f) / mag);
dgFloat32 angle = dgAsin (mag);
CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]);
dgVector frontDir (lateralDir * matrix1.m_front);
CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
ret = 2;
} else {
CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]);
CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]);
ret = 2;
}
return dgUnsigned32 (ret);
}
FastRayTest::FastRayTest(const dgVector& l0, const dgVector& l1)
:m_p0 (l0), m_p1(l1), m_diff (l1 - l0)
,m_minT(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_maxT(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f))
,m_tolerance (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR)
,m_zero(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
{
m_diff.m_w = dgFloat32 (0.0f);
m_isParallel[0] = (dgAbsf (m_diff.m_x) > dgFloat32 (1.0e-8f)) ? 0 : dgInt32 (0xffffffff);
m_isParallel[1] = (dgAbsf (m_diff.m_y) > dgFloat32 (1.0e-8f)) ? 0 : dgInt32 (0xffffffff);
m_isParallel[2] = (dgAbsf (m_diff.m_z) > dgFloat32 (1.0e-8f)) ? 0 : dgInt32 (0xffffffff);
m_isParallel[3] = 0;
m_dpInv.m_x = (!m_isParallel[0]) ? (dgFloat32 (1.0f) / m_diff.m_x) : dgFloat32 (1.0e20f);
m_dpInv.m_y = (!m_isParallel[1]) ? (dgFloat32 (1.0f) / m_diff.m_y) : dgFloat32 (1.0e20f);
m_dpInv.m_z = (!m_isParallel[2]) ? (dgFloat32 (1.0f) / m_diff.m_z) : dgFloat32 (1.0e20f);
m_dpInv.m_w = dgFloat32 (0.0f);
m_dpBaseInv = m_dpInv;
m_ray_xxxx = simd_128(m_diff.m_x);
m_ray_yyyy = simd_128(m_diff.m_y);
m_ray_zzzz = simd_128(m_diff.m_z);
m_dirError = -dgFloat32 (0.0175f) * dgSqrt (m_diff % m_diff);
// tollerance = simd_set (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR);
// m_tolerance = dgVector (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, dgFloat32 (0.0f));
}
dgFastRayTest::dgFastRayTest(const dgVector& l0, const dgVector& l1)
:m_p0 (l0), m_p1(l1), m_diff (l1 - l0)
,m_minT(float (0.0f), float (0.0f), float (0.0f), float (0.0f))
,m_maxT(float (1.0f), float (1.0f), float (1.0f), float (1.0f))
,m_tolerance (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR)
,m_zero(float (0.0f), float (0.0f), float (0.0f), float (0.0f))
{
m_diff.m_w = float (0.0f);
m_isParallel[0] = (dgAbsf (m_diff.m_x) > float (1.0e-8f)) ? 0 : int32_t (0xffffffff);
m_isParallel[1] = (dgAbsf (m_diff.m_y) > float (1.0e-8f)) ? 0 : int32_t (0xffffffff);
m_isParallel[2] = (dgAbsf (m_diff.m_z) > float (1.0e-8f)) ? 0 : int32_t (0xffffffff);
m_isParallel[3] = 0;
m_dpInv.m_x = (!m_isParallel[0]) ? (float (1.0f) / m_diff.m_x) : float (1.0e20f);
m_dpInv.m_y = (!m_isParallel[1]) ? (float (1.0f) / m_diff.m_y) : float (1.0e20f);
m_dpInv.m_z = (!m_isParallel[2]) ? (float (1.0f) / m_diff.m_z) : float (1.0e20f);
m_dpInv.m_w = float (0.0f);
m_dpBaseInv = m_dpInv;
m_dirError = -float (0.0175f) * dgSqrt (m_diff % m_diff);
// tollerance = simd_set (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR);
// m_tolerance = dgVector (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, float (0.0f));
}
dgFloat32 dgRayCastSphere (const dgVector& p0, const dgVector& p1, const dgVector& origin, dgFloat32 radius)
{
dgVector p0Origin (p0 - origin);
if (p0Origin.DotProduct3(p0Origin) < (dgFloat32 (100.0f) * radius * radius)) {
dgVector dp (p1 - p0);
dgFloat32 a = dp.DotProduct3(dp);
dgFloat32 b = dgFloat32 (2.0f) * p0Origin.DotProduct3(dp);
dgFloat32 c = p0Origin.DotProduct3(p0Origin) - radius * radius;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = dgSqrt (desc);
dgFloat32 den = dgFloat32 (0.5f) / a;
dgFloat32 t0 = (-b + desc) * den;
dgFloat32 t1 = (-b - desc) * den;
if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
t0 = dgMin(t0, t1);
if (t0 <= dgFloat32 (1.0f)) {
return t0;
}
} else if (t0 >= dgFloat32 (0.0f)) {
if (t0 <= dgFloat32 (1.0f)) {
return t0;
}
} else {
if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
return t1;
}
}
}
} else {
dgBigVector p0Origin1 (p0Origin);
dgBigVector dp (p1 - p0);
dgFloat64 a = dp.DotProduct3(dp);
dgFloat64 b = dgFloat32 (2.0f) * p0Origin1.DotProduct3(dp);
dgFloat64 c = p0Origin1.DotProduct3(p0Origin1) - dgFloat64(radius) * radius;
dgFloat64 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = sqrt (desc);
dgFloat64 den = dgFloat32 (0.5f) / a;
dgFloat64 t0 = (-b + desc) * den;
dgFloat64 t1 = (-b - desc) * den;
if ((t0 >= dgFloat32 (0.0f)) && (t1 >= dgFloat32 (0.0f))) {
t0 = dgMin(t0, t1);
if (t0 <= dgFloat32 (1.0f)) {
return dgFloat32 (t0);
}
} else if (t0 >= dgFloat32 (0.0f)) {
if (t0 <= dgFloat32 (1.0f)) {
return dgFloat32 (t0);
}
} else {
if ((t1 >= dgFloat32 (0.0f)) && (t1 <= dgFloat32 (1.0f))) {
return dgFloat32 (t1);
}
}
}
}
return dgFloat32 (1.2f);
}
bool dgCollisionConvexHull::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
bool ret;
_ASSERTE (cacheOrder);
ret = dgCollisionConvex::OOBBTest (matrix, shape, cacheOrder);
if (ret) {
const dgConvexSimplexEdge* const* faceArray = m_faceArray;
dgCollisionBoundPlaneCache* const cache = (dgCollisionBoundPlaneCache*)cacheOrder;
for (dgInt32 i = 0; i < dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)); i ++) {
dgFloat32 dist;
const dgPlane& plane = cache->m_planes[i];
if ((plane % plane) > dgFloat32 (0.0f)) {
dgVector dir (matrix.UnrotateVector(plane.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
dist = plane.Evalue (p);
if (dist > dgFloat32 (0.1f)){
return false;
}
}
}
for (dgInt32 i = 0; i < m_boundPlanesCount; i ++) {
dgInt32 i0;
dgInt32 i1;
dgInt32 i2;
dgFloat32 dist;
const dgConvexSimplexEdge* const face = faceArray[i];
i0 = face->m_prev->m_vertex;
i1 = face->m_vertex;
i2 = face->m_next->m_vertex;
const dgVector& p0 = m_vertex[i0];
dgVector normal ((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));
dgVector dir (matrix.UnrotateVector(normal.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
//_ASSERTE ((normal % (m_boxOrigin - p0)) < 0.0f);
dist = normal % (p - p0);
if (dist > dgFloat32 (0.1f)){
for (dgInt32 j = 0; j < (dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)) - 1); j ++) {
cache->m_planes[j + 1] = cache->m_planes[j];
}
cache->m_planes[1] = dgPlane (normal, - (normal % p0));
return false;
}
}
}
return ret;
}
dgInt32 dgCollisionTaperedCapsule::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgVector n (-normal.m_x, -dgSqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z), dgFloat32 (0.0), dgFloat32 (0.0f));
dgFloat32 project = m_sideNormal % n;
if (project > dgFloat32 (0.9998f)) {
return CalculateContactsGeneric (point, normal, proxy, contactsOut);
} else if (point.m_x > (m_height + m_clip0)){
return CalculateSphereConicContacts ( m_height, m_radio0, normal, point, contactsOut);
} else if (point.m_x < (-m_height + m_clip1)) {
return CalculateSphereConicContacts (-m_height, m_radio1, normal, point, contactsOut);
}
return CalculateContactsGeneric (point, normal, proxy, contactsOut);
}
dgVector dgCollisionTaperedCylinder::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
dgVector p (SupportVertex(dir, NULL));
if (dgAbsf(dir.m_x) > dgFloat32 (0.9997f)) {
p.m_y = point.m_y;
p.m_z = point.m_z;
} else {
dgVector n (dir.m_x, dgSqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 project = n % m_dirVector;
if (project > dgFloat32 (0.9998f)) {
p.m_x = point.m_x;
}
}
return p;
}
dgVector dgCollisionTaperedCapsule::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
dgAssert (dgAbsf (dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgVector p (SupportVertex(dir, NULL));
dgVector n (dir.m_x, dgSqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z), dgFloat32 (0.0), dgFloat32 (0.0f));
dgAssert (dgAbsf (n % n - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 project = m_sideNormal % n;
if (project > dgFloat32 (0.9998f)) {
dgFloat32 t = dgFloat32 (0.5f) * (point.m_x + m_height) / m_height;
dgFloat32 r = m_radio1 + (m_radio0 - m_radio1) * t;
p = dir.Scale3 (r);
p.m_x += point.m_x;
}
return p;
}
void dgBilateralConstraint::CalculateMatrixOffset (const dgVector& pivot, const dgVector& dir, dgMatrix& matrix0, dgMatrix& matrix1)
{
dgFloat32 length;
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
length = dir % dir;
length = dgSqrt (length);
dgAssert (length > dgFloat32 (0.0f));
matrix0 = dgMatrix (body0_Matrix.UnrotateVector (dir.Scale3 (dgFloat32 (1.0f) / length)));
matrix0.m_posit = body0_Matrix.UntransformVector (pivot);
matrix0.m_front.m_w = dgFloat32 (0.0f);
matrix0.m_up.m_w = dgFloat32 (0.0f);
matrix0.m_right.m_w = dgFloat32 (0.0f);
matrix0.m_posit.m_w = dgFloat32 (1.0f);
const dgMatrix& body1_Matrix = m_body1->GetMatrix();
matrix1 = matrix0 * body0_Matrix * body1_Matrix.Inverse();
}
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 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::EigenVectors (dgVector &eigenValues, const dgMatrix* const initialGuess)
{
dgMatrix& mat = *this;
dgMatrix eigenVectors (dgGetIdentityMatrix());
if (initialGuess) {
eigenVectors = *initialGuess;
mat = eigenVectors.Transpose4X4() * mat * eigenVectors;
}
dgVector d (mat[0][0], mat[1][1], mat[2][2], dgFloat32 (0.0f));
dgVector b (d);
for (dgInt32 i = 0; i < 50; i++) {
dgFloat32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
if (sm < dgFloat32 (1.0e-12f)) {
// order the eigenvalue vectors
dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
if (tmp % eigenVectors.m_right < dgFloat32(0.0f)) {
dgAssert (0.0f);
eigenVectors.m_right = eigenVectors.m_right.Scale3 (-dgFloat32(1.0f));
}
break;
}
dgFloat32 thresh = dgFloat32 (0.0f);
if (i < 3) {
thresh = (dgFloat32)(0.2f / 9.0f) * sm;
}
dgVector z (dgVector::m_zero);
for (dgInt32 ip = 0; ip < 2; ip ++) {
for (dgInt32 iq = ip + 1; iq < 3; iq ++) {
dgFloat32 g = dgFloat32 (100.0f) * dgAbsf(mat[ip][iq]);
if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
mat[ip][iq] = dgFloat32 (0.0f);
} else if (dgAbsf(mat[ip][iq]) > thresh) {
dgFloat32 t;
dgFloat32 h = d[iq] - d[ip];
if (dgAbsf(h) + g == dgAbsf(h)) {
t = mat[ip][iq] / h;
} else {
dgFloat32 theta = dgFloat32 (0.5f) * h / mat[ip][iq];
t = dgFloat32(1.0f) / (dgAbsf(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
if (theta < dgFloat32 (0.0f)) {
t = -t;
}
}
dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t);
dgFloat32 s = t * c;
dgFloat32 tau = s / (dgFloat32(1.0f) + c);
h = t * mat[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
mat[ip][iq] = dgFloat32(0.0f);
for (dgInt32 j = 0; j <= ip - 1; j ++) {
dgFloat32 g = mat[j][ip];
dgFloat32 h = mat[j][iq];
mat[j][ip] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (dgInt32 j = ip + 1; j <= iq - 1; j ++) {
dgFloat32 g = mat[ip][j];
dgFloat32 h = mat[j][iq];
mat[ip][j] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (dgInt32 j = iq + 1; j < 3; j ++) {
dgFloat32 g = mat[ip][j];
dgFloat32 h = mat[iq][j];
mat[ip][j] = g - s * (h + g * tau);
mat[iq][j] = h + s * (g - h * tau);
}
dgVector sv (s);
dgVector tauv (tau);
dgVector gv (eigenVectors[ip]);
dgVector hv (eigenVectors[iq]);
eigenVectors[ip] -= sv.CompProduct4 (hv + gv.CompProduct4(tauv));
eigenVectors[iq] += sv.CompProduct4 (gv - hv.CompProduct4(tauv));
}
}
}
b += z;
d = b;
}
eigenValues = d;
*this = eigenVectors;
}
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 ();
}
dgFloat32 dgCollisionChamferCylinder::RayCast (const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector (dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector (dgFloat32 (-1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
dgVector dq ((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if ((dq % dq) <= 0.0f) {
return dgFloat32(1.2f);
}
//dgVector dir (dq.Scale3 (dgRsqrt(dq % dq)));
dgVector dir (dq.CompProduct4 (dq.InvMagSqrt()));
if (dgAbsf (dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast (q0, q1, maxT, contactOut, NULL, NULL, NULL);
}
dgVector p0 (q0);
dgVector p1 (q1);
dgVector dp (dq);
p0.m_x = dgFloat32 (0.0f);
p1.m_x = dgFloat32 (0.0f);
dp.m_x = dgFloat32 (0.0f);
dgFloat32 a = dp % dp;
//dgFloat32 b = dgFloat32 (2.0f) * (p0 % dp);
dgFloat32 b = dgFloat32 (2.0f) * (p0.DotProduct4(dp).GetScalar());
dgFloat32 r = m_radius + m_height;
dgFloat32 c = p0 % p0 - r * r;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = dgSqrt (desc);
dgFloat32 den = dgFloat32 (0.5f) / a;
dgFloat32 s0 = (-b + desc) * den;
dgFloat32 s1 = (-b - desc) * den;
dgVector origin0 (p0 + dp.Scale4 (s0));
dgVector origin1 (p0 + dp.Scale4 (s1));
dgFloat32 s = m_radius / (m_radius + m_height);
origin0 = origin0.Scale4 (s);
origin1 = origin1.Scale4 (s);
dgFloat32 t0 = dgRayCastSphere (q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere (q0, q1, origin1, m_height);
if (t0 < t1) {
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale4 (t0) - origin0;
dgAssert (contactOut.m_normal.m_w == dgFloat32 (0.0f));
//contactOut.m_normal = contactOut.m_normal.Scale3 (dgRsqrt (contactOut.m_normal % contactOut.m_normal));
contactOut.m_normal = contactOut.m_normal.CompProduct4(contactOut.m_normal.DotProduct4(contactOut.m_normal).InvSqrt());
//contactOut.m_userId = SetUserDataID();
return t0;
}
} else {
if ((t1 >= 0.0f) && (t1 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale4 (t1) - origin1;
dgAssert (contactOut.m_normal.m_w == dgFloat32 (0.0f));
//contactOut.m_normal = contactOut.m_normal.Scale3 (dgRsqrt (contactOut.m_normal % contactOut.m_normal));
contactOut.m_normal = contactOut.m_normal.CompProduct4(contactOut.m_normal.DotProduct4(contactOut.m_normal).InvSqrt());
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
return dgFloat32 (1.2f);
}
void dgCollisionScene::CollideCompoundPair (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy) const
{
const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2];
dgContact* const constraint = pair->m_contact;
dgBody* const myBody = constraint->GetBody1();
dgBody* const otherBody = constraint->GetBody0();
dgAssert (myBody == proxy.m_floatingBody);
dgAssert (otherBody == proxy.m_referenceBody);
dgCollisionInstance* const myCompoundInstance = myBody->m_collision;
dgCollisionInstance* const otherCompoundInstance = otherBody->m_collision;
dgAssert (myCompoundInstance->GetChildShape() == this);
dgAssert (otherCompoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI));
dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherCompoundInstance->GetChildShape();
const dgContactMaterial* const material = constraint->GetMaterial();
dgMatrix myMatrix (myCompoundInstance->GetLocalMatrix() * myBody->m_matrix);
dgMatrix otherMatrix (otherCompoundInstance->GetLocalMatrix() * otherBody->m_matrix);
dgOOBBTestData data (otherMatrix * myMatrix.Inverse());
dgInt32 stack = 1;
stackPool[0][0] = m_root;
stackPool[0][1] = otherCompound->m_root;
const dgVector& hullVeloc = otherBody->m_veloc;
dgFloat32 baseLinearSpeed = dgSqrt (hullVeloc % hullVeloc);
dgFloat32 closestDist = dgFloat32 (1.0e10f);
if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) {
dgAssert (0);
} else {
while (stack) {
stack --;
const dgNodeBase* const me = stackPool[stack][0];
const dgNodeBase* const other = stackPool[stack][1];
dgAssert (me && other);
if (me->BoxTest (data, other)) {
if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) {
dgAssert (!me->m_right);
bool processContacts = true;
if (material->m_compoundAABBOverlap) {
processContacts = material->m_compoundAABBOverlap (*material, myBody, me->m_myNode, otherBody, other->m_myNode, proxy.m_threadIndex);
}
if (processContacts) {
const dgCollisionInstance* const mySrcInstance = me->GetShape();
const dgCollisionInstance* const otherSrcInstance = other->GetShape();
//dgCollisionInstance childInstance (*mySrcInstance, mySrcInstance->GetChildShape());
//dgCollisionInstance otherInstance (*otherSrcInstance, otherSrcInstance->GetChildShape());
dgCollisionInstance childInstance (*me->GetShape(), me->GetShape()->GetChildShape());
dgCollisionInstance otherInstance (*other->GetShape(), other->GetShape()->GetChildShape());
childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix);
otherInstance.SetGlobalMatrix(otherInstance.GetLocalMatrix() * otherMatrix);
proxy.m_floatingCollision = &childInstance;
proxy.m_referenceCollision = &otherInstance;
dgInt32 count = pair->m_contactCount;
m_world->SceneChildContacts (pair, proxy);
if (pair->m_contactCount > count) {
dgContactPoint* const buffer = proxy.m_contacts;
for (dgInt32 i = count; i < pair->m_contactCount; i ++) {
//if (buffer[i].m_collision0 == proxy.m_floatingCollision) {
// buffer[i].m_collision0 = mySrcInstance;
//}
//if (buffer[i].m_collision1 == proxy.m_referenceCollision) {
// buffer[i].m_collision1 = otherSrcInstance;
//}
if (buffer[i].m_collision1->GetChildShape() == otherSrcInstance->GetChildShape()) {
dgAssert(buffer[i].m_collision0->GetChildShape() == mySrcInstance->GetChildShape());
buffer[i].m_collision0 = mySrcInstance;
buffer[i].m_collision1 = otherSrcInstance;
} else {
dgAssert(buffer[i].m_collision1->GetChildShape() == mySrcInstance->GetChildShape());
buffer[i].m_collision1 = mySrcInstance;
buffer[i].m_collision0 = otherSrcInstance;
}
}
}
closestDist = dgMin(closestDist, constraint->m_closestDistance);
}
} else if (me->m_type == m_leaf) {
dgAssert (other->m_type == m_node);
stackPool[stack][0] = me;
stackPool[stack][1] = other->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me;
stackPool[stack][1] = other->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
} else if (other->m_type == m_leaf) {
dgAssert (me->m_type == m_node);
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
} else {
dgAssert (me->m_type == m_node);
dgAssert (other->m_type == m_node);
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
}
}
}
}
constraint->m_closestDistance = closestDist;
}
dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
_ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));
dgPlane plane (normal1, - (normal1 % origin1));
count = 0;
dgVector maxDir ((normal1.m_x > dgFloat32 (0.0f)) ? m_silhuette[0].m_x : -m_silhuette[0].m_x,
(normal1.m_y > dgFloat32 (0.0f)) ? m_silhuette[0].m_y : -m_silhuette[0].m_y, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 test0 = plane.Evalue (maxDir);
dgFloat32 test1 = plane.Evalue (maxDir.Scale (dgFloat32 (-1.0f)));
if ((test0 * test1) > dgFloat32 (0.0f)) {
test0 = plane.m_w + plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
contactsOut[count] = normal1.Scale (-test0);
contactsOut[count].m_y += m_radius;
count ++;
} else {
test0 = plane.m_w - plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
contactsOut[count] = normal1.Scale (-test0);
contactsOut[count].m_y -= m_radius;
count ++;
}
}
} else {
dgVector dp (m_silhuette[1] - m_silhuette[0]);
dgFloat32 den = normal1 % dp;
_ASSERTE (dgAbsf (den) > dgFloat32 (0.0f));
test0 = -plane.Evalue (m_silhuette[0]) / den;
if ((test0 <= dgFloat32 (1.0)) && (test0 >= dgFloat32 (0.0f))) {
contactsOut[count] = m_silhuette[0] + dp.Scale (test0);
count ++;
}
if (count < 2) {
test0 = plane.m_w - plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
dgFloat32 r = -m_radius;
dgFloat32 d = plane.m_w + r * plane.m_y;
dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
dgFloat32 b = dgFloat32 (2.0f) * plane.m_y * d;
dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc > dgFloat32 (0.0f)) {
_ASSERTE (dgAbsf (a) > dgFloat32 (0.0f));
desc = dgSqrt (desc);
a = - dgFloat32 (0.5f) * b / a;
dgFloat32 y0 = a + desc;
dgFloat32 y1 = a - desc;
if (y0 > dgFloat32 (0.0f)) {
y0 = y1;
}
_ASSERTE (y0 < dgFloat32 (0.0f));
_ASSERTE (dgAbsf (plane.m_x) > dgFloat32 (0.0f));
dgFloat32 x = - (plane.m_y * y0 + d) / plane.m_x;
contactsOut[count] = dgVector (x, y0 + r, dgFloat32 (0.0f), dgFloat32 (0.0f));
count ++;
}
}
}
if (count < 2) {
dgVector dp (m_silhuette[3] - m_silhuette[2]);
den = normal1 % dp;
_ASSERTE (dgAbsf (den) > dgFloat32 (0.0f));
test0 = - plane.Evalue (m_silhuette[2]) / den;
if ((test0 <= dgFloat32 (1.0)) && (test0 >= dgFloat32 (0.0f))) {
contactsOut[count] = m_silhuette[2] + dp.Scale (test0);
count ++;
}
}
if (count < 2) {
test0 = plane.m_w + plane.m_y * m_radius;
if (dgAbsf (test0) < m_height) {
dgFloat32 r = m_radius;
dgFloat32 d = plane.m_w + r * plane.m_y;
dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
dgFloat32 b = dgFloat32 (2.0f) * plane.m_y * d;
dgFloat32 c = d * d - m_height * m_height * plane.m_x * plane.m_x;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc > dgFloat32 (0.0f)) {
_ASSERTE (dgAbsf (a) > dgFloat32 (0.0f));
desc = dgSqrt (desc);
a = - dgFloat32 (0.5f) * b / a;
dgFloat32 y0 = a + desc;
dgFloat32 y1 = a - desc;
if (y0 < dgFloat32 (0.0f)) {
y0 = y1;
}
_ASSERTE (y0 > dgFloat32 (0.0f));
_ASSERTE (dgAbsf (plane.m_x) > dgFloat32 (0.0f));
dgFloat32 x = - (plane.m_y * y0 + d) / plane.m_x;
contactsOut[count] = dgVector (x, y0 + r, dgFloat32 (0.0f), dgFloat32 (0.0f));
count ++;
}
}
}
}
for (dgInt32 i = 0; i < count; i ++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut);
}
return count;
}
void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
{
hacd::HaF32 b[3];
hacd::HaF32 z[3];
hacd::HaF32 d[3];
dgMatrix& mat = *this;
dgMatrix eigenVectors (initialGuess.Transpose4X4());
mat = initialGuess * mat * eigenVectors;
b[0] = mat[0][0];
b[1] = mat[1][1];
b[2] = mat[2][2];
d[0] = mat[0][0];
d[1] = mat[1][1];
d[2] = mat[2][2];
z[0] = hacd::HaF32 (0.0f);
z[1] = hacd::HaF32 (0.0f);
z[2] = hacd::HaF32 (0.0f);
for (hacd::HaI32 i = 0; i < 50; i++) {
hacd::HaF32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
if (sm < hacd::HaF32 (1.0e-6f)) {
HACD_ASSERT (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - hacd::HaF32(1.0f)) < dgEPSILON);
HACD_ASSERT (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - hacd::HaF32(1.0f)) < dgEPSILON);
HACD_ASSERT (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - hacd::HaF32(1.0f)) < dgEPSILON);
// order the eigenvalue vectors
dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
if (tmp % eigenVectors.m_right < hacd::HaF32(0.0f)) {
eigenVectors.m_right = eigenVectors.m_right.Scale (-hacd::HaF32(1.0f));
}
eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
*this = eigenVectors.Inverse();
return;
}
hacd::HaF32 thresh = hacd::HaF32 (0.0f);
if (i < 3) {
thresh = (hacd::HaF32)(0.2f / 9.0f) * sm;
}
for (hacd::HaI32 ip = 0; ip < 2; ip ++) {
for (hacd::HaI32 iq = ip + 1; iq < 3; iq ++) {
hacd::HaF32 g = hacd::HaF32 (100.0f) * dgAbsf(mat[ip][iq]);
//if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[ip])) && (dgAbsf(d[1]) + g == dgAbsf(d[1]))) {
if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
mat[ip][iq] = hacd::HaF32 (0.0f);
} else if (dgAbsf(mat[ip][iq]) > thresh) {
hacd::HaF32 t;
hacd::HaF32 h = d[iq] - d[ip];
if (dgAbsf(h) + g == dgAbsf(h)) {
t = mat[ip][iq] / h;
} else {
hacd::HaF32 theta = hacd::HaF32 (0.5f) * h / mat[ip][iq];
t = hacd::HaF32(1.0f) / (dgAbsf(theta) + dgSqrt(hacd::HaF32(1.0f) + theta * theta));
if (theta < hacd::HaF32 (0.0f)) {
t = -t;
}
}
hacd::HaF32 c = hacd::HaF32(1.0f) / dgSqrt (hacd::HaF32 (1.0f) + t * t);
hacd::HaF32 s = t * c;
hacd::HaF32 tau = s / (hacd::HaF32(1.0f) + c);
h = t * mat[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
mat[ip][iq] = hacd::HaF32(0.0f);
for (hacd::HaI32 j = 0; j <= ip - 1; j ++) {
//ROT (mat, j, ip, j, iq, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = mat[j][ip];
hacd::HaF32 h = mat[j][iq];
mat[j][ip] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (hacd::HaI32 j = ip + 1; j <= iq - 1; j ++) {
//ROT (mat, ip, j, j, iq, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = mat[ip][j];
hacd::HaF32 h = mat[j][iq];
mat[ip][j] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (hacd::HaI32 j = iq + 1; j < 3; j ++) {
//ROT (mat, ip, j, iq, j, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = mat[ip][j];
hacd::HaF32 h = mat[iq][j];
mat[ip][j] = g - s * (h + g * tau);
mat[iq][j] = h + s * (g - h * tau);
}
for (hacd::HaI32 j = 0; j < 3; j ++) {
//ROT (eigenVectors, j, ip, j, iq, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = eigenVectors[j][ip];
hacd::HaF32 h = eigenVectors[j][iq];
eigenVectors[j][ip] = g - s * (h + g * tau);
eigenVectors[j][iq] = h + s * (g - h * tau);
}
}
}
}
b[0] += z[0]; d[0] = b[0]; z[0] = hacd::HaF32 (0.0f);
b[1] += z[1]; d[1] = b[1]; z[1] = hacd::HaF32 (0.0f);
b[2] += z[2]; d[2] = b[2]; z[2] = hacd::HaF32 (0.0f);
}
eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
*this = dgGetIdentityMatrix();
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
return 0;
}
dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
if (den > dgFloat32 (1.0e-5f)) {
// this can actually happens
dgAssert(0);
return 0;
}
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
dgCollisionConvexHull::dgCollisionConvexHull(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgInt32 count, dgInt32 strideInBytes, dgFloat32 tolerance, const dgFloat32* vertexArray, const dgMatrix& matrix)
:dgCollisionConvex(allocator, signature, matrix, m_convexHullCollision)
{
m_faceCount = 0;
m_edgeCount = 0;
m_vertexCount = 0;
m_vertex = NULL;
m_simplex = NULL;
m_faceArray = NULL;
m_boundPlanesCount = 0;
m_rtti |= dgCollisionConvexHull_RTTI;
Create (count, strideInBytes, vertexArray, tolerance);
dgInt32 planeCount = 0;
dgPlaneLocation planesArray[1024];
const dgConvexSimplexEdge* const* faceArray = m_faceArray;
for (dgInt32 i = 0; i < m_faceCount; i ++) {
const dgConvexSimplexEdge* const face = faceArray[i];
dgInt32 i0 = face->m_prev->m_vertex;
dgInt32 i1 = face->m_vertex;
dgInt32 i2 = face->m_next->m_vertex;
const dgBigVector p0 (m_vertex[i0]);
const dgBigVector p1 (m_vertex[i1]);
const dgBigVector p2 (m_vertex[i2]);
dgBigVector normal1 ((p1 - p0) * (p2 - p0));
dgVector normal ((m_vertex[i1] - m_vertex[i0]) * (m_vertex[i2] - m_vertex[i0]));
normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));
dgInt32 add = 1;
for (dgInt32 j = 0; j < 3; j ++) {
if (dgAbsf (normal[j]) > dgFloat32 (0.98f)) {
add = 0;
}
}
if (add) {
for (dgInt32 j = 0; j < planeCount; j ++) {
dgFloat32 coplanar;
coplanar = normal % planesArray[j];
if (coplanar > 0.98f) {
add = 0;
break;
}
}
if (add) {
dgPlane plane (normal, dgFloat32 (0.0f));
dgVector planeSupport (SupportVertex (plane));
plane.m_w = - (plane % planeSupport);
// _ASSERTE (plane.Evalue(m_boxOrigin) < 0.0f);
dgPlane& tmpPlane = planesArray[planeCount];
tmpPlane = plane;
planesArray[planeCount].m_index = i;
planesArray[planeCount].m_face = face;
planeCount ++;
_ASSERTE (planeCount < (sizeof (planesArray) / sizeof (planesArray[0])));
}
}
}
m_boundPlanesCount = 0;
for (dgInt32 i = 0; i < planeCount; i ++) {
dgPlaneLocation& plane = planesArray[i];
if (plane.m_face == m_faceArray[plane.m_index]) {
Swap (m_faceArray[plane.m_index], m_faceArray[m_boundPlanesCount]);
} else {
dgInt32 j;
for (j = m_boundPlanesCount; j < m_faceCount; j ++) {
if (plane.m_face == m_faceArray[j]) {
Swap (m_faceArray[j], m_faceArray[m_boundPlanesCount]);
break;
}
}
_ASSERTE (j < m_faceCount);
}
m_boundPlanesCount ++;
}
m_destructionImpulse = dgFloat32 (1.0e20f);
}
void dgCollisionScene::CollidePair (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy) const
{
const dgNodeBase* stackPool[DG_COMPOUND_STACK_DEPTH];
dgAssert (proxy.m_contactJoint == pair->m_contact);
dgContact* const constraint = pair->m_contact;
dgBody* const otherBody = constraint->GetBody0();
dgBody* const sceneBody = constraint->GetBody1();
dgAssert (sceneBody->GetCollision()->GetChildShape() == this);
dgAssert (sceneBody->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
dgCollisionInstance* const sceneInstance = sceneBody->m_collision;
dgCollisionInstance* const otherInstance = otherBody->m_collision;
dgAssert (sceneInstance->GetChildShape() == this);
dgAssert (otherInstance->IsType (dgCollision::dgCollisionConvexShape_RTTI));
const dgContactMaterial* const material = constraint->GetMaterial();
const dgMatrix& myMatrix = sceneInstance->GetGlobalMatrix();
const dgMatrix& otherMatrix = otherInstance->GetGlobalMatrix();
dgMatrix matrix (otherMatrix * myMatrix.Inverse());
const dgVector& hullVeloc = otherBody->m_veloc;
dgFloat32 baseLinearSpeed = dgSqrt (hullVeloc % hullVeloc);
dgFloat32 closestDist = dgFloat32 (1.0e10f);
if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) {
dgVector p0;
dgVector p1;
otherInstance->CalcAABB (matrix, p0, p1);
const dgVector& hullOmega = otherBody->m_omega;
dgFloat32 minRadius = otherInstance->GetBoxMinRadius();
dgFloat32 maxAngularSpeed = dgSqrt (hullOmega % hullOmega);
dgFloat32 angularSpeedBound = maxAngularSpeed * (otherInstance->GetBoxMaxRadius() - minRadius);
dgFloat32 upperBoundSpeed = baseLinearSpeed + dgSqrt (angularSpeedBound);
dgVector upperBoundVeloc (hullVeloc.Scale3 (proxy.m_timestep * upperBoundSpeed / baseLinearSpeed));
dgVector boxDistanceTravelInMeshSpace (myMatrix.UnrotateVector(upperBoundVeloc.CompProduct4(otherInstance->m_invScale)));
dgInt32 stack = 1;
stackPool[0] = m_root;
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), boxDistanceTravelInMeshSpace);
while (stack) {
stack--;
const dgNodeBase* const me = stackPool[stack];
dgAssert (me);
if (me->BoxIntersect (ray, p0, p1)) {
if (me->m_type == m_leaf) {
dgAssert (!me->m_right);
bool processContacts = true;
if (material->m_compoundAABBOverlap) {
processContacts = material->m_compoundAABBOverlap (*material, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex);
}
if (processContacts) {
const dgCollisionInstance* const myInstance = me->GetShape();
dgCollisionInstance childInstance (*myInstance, myInstance->GetChildShape());
childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix);
proxy.m_floatingCollision = &childInstance;
dgInt32 count = pair->m_contactCount;
m_world->SceneChildContacts (pair, proxy);
if (pair->m_contactCount > count) {
dgContactPoint* const buffer = proxy.m_contacts;
for (dgInt32 i = count; i < pair->m_contactCount; i ++) {
dgAssert (buffer[i].m_collision0 == proxy.m_referenceCollision);
//if (buffer[i].m_collision1 == proxy.m_floatingCollision) {
// buffer[i].m_collision1 = myInstance;
//}
if (buffer[i].m_collision1->GetChildShape() == myInstance->GetChildShape()) {
buffer[i].m_collision1 = myInstance;
}
}
}
closestDist = dgMin(closestDist, constraint->m_closestDistance);
}
} else {
dgAssert (me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack] = me->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
}
}
}
} else {
dgVector origin;
dgVector size;
otherInstance->CalcObb(origin, size);
dgOOBBTestData data (matrix, origin, size);
dgInt32 stack = 1;
stackPool[0] = m_root;
while (stack) {
stack --;
const dgNodeBase* const me = stackPool[stack];
dgAssert (me);
if (me->BoxTest (data)) {
if (me->m_type == m_leaf) {
dgAssert (!me->m_right);
bool processContacts = true;
if (material->m_compoundAABBOverlap) {
processContacts = material->m_compoundAABBOverlap (*material, sceneBody, me->m_myNode, otherBody, NULL, proxy.m_threadIndex);
}
if (processContacts) {
const dgCollisionInstance* const myInstance = me->GetShape();
dgCollisionInstance childInstance (*myInstance, myInstance->GetChildShape());
childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix);
proxy.m_floatingCollision = &childInstance;
dgInt32 count = pair->m_contactCount;
m_world->SceneChildContacts (pair, proxy);
if (pair->m_contactCount > count) {
dgContactPoint* const buffer = proxy.m_contacts;
for (dgInt32 i = count; i < pair->m_contactCount; i ++) {
dgAssert (buffer[i].m_collision0 == proxy.m_referenceCollision);
//if (buffer[i].m_collision1 == proxy.m_floatingCollision) {
// buffer[i].m_collision1 = myInstance;
//}
if (buffer[i].m_collision1->GetChildShape() == myInstance->GetChildShape()) {
buffer[i].m_collision1 = myInstance;
}
}
} else if (pair->m_contactCount == -1) {
break;
}
closestDist = dgMin(closestDist, constraint->m_closestDistance);
}
} else {
dgAssert (me->m_type == m_node);
stackPool[stack] = me->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack] = me->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
}
}
}
}
constraint->m_closestDistance = closestDist;
}
dgFloat32 dgCollisionChamferCylinder::RayCast(const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
dgVector dq((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if (dq.DotProduct(dq).GetScalar() <= 0.0f) {
return dgFloat32(1.2f);
}
//dgVector dir(dq * dq.InvMagSqrt());
dgVector dir(dq.Normalize());
if (dgAbs(dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast(q0, q1, maxT, contactOut, body, NULL, NULL);
}
dgVector p0(q0 & dgVector::m_triplexMask);
dgVector p1(q1 & dgVector::m_triplexMask);
p0.m_x = dgFloat32 (0.0f);
p1.m_x = dgFloat32 (0.0f);
dgVector dp (p1 - p0);
dgFloat32 a = dp.DotProduct(dp).GetScalar();
dgFloat32 b = dgFloat32 (2.0f) * dp.DotProduct(p0).GetScalar();
dgFloat32 c = p0.DotProduct(p0).GetScalar() - m_radius * m_radius;
dgFloat32 disc = b * b - dgFloat32 (4.0f) * a * c;
if (disc >= dgFloat32 (0.0f)) {
disc = dgSqrt (disc);
dgVector origin0(p0 + dp.Scale ((-b + disc) / (dgFloat32 (2.0f) * a)));
dgVector origin1(p0 + dp.Scale ((-b - disc) / (dgFloat32 (2.0f) * a)));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere(q0, q1, origin1, m_height);
if(t1 < t0) {
t0 = t1;
origin0 = origin1;
}
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
} else {
dgVector origin0 (dgPointToRayDistance (dgVector::m_zero, p0, p1));
origin0 = origin0.Scale(m_radius / dgSqrt(origin0.DotProduct(origin0).GetScalar()));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
}
return dgFloat32(1.2f);
}
