void dgCollisionTaperedCapsule::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* ouput) const
{
if (level) {
dgAssert (dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
p01 = p01.Scale3 (dgRsqrt(p01 % p01));
p12 = p12.Scale3 (dgRsqrt(p12 % p12));
p20 = p20.Scale3 (dgRsqrt(p20 % p20));
dgAssert (dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20 % p20 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TesselateTriangle (level - 1, p0, p01, p20, count, ouput);
TesselateTriangle (level - 1, p1, p12, p01, count, ouput);
TesselateTriangle (level - 1, p2, p20, p12, count, ouput);
TesselateTriangle (level - 1, p01, p12, p20, count, ouput);
} else {
ouput[count + 0] = p0.Scale3 (m_radio0);
ouput[count + 1] = p1.Scale3 (m_radio0);
ouput[count + 2] = p2.Scale3 (m_radio0);
count += 3;
}
}
dgInt32 dgCollisionTaperedCapsule::CalculateSphereConicContacts (dgFloat32 posit, dgFloat32 radius, const dgVector& normal, const dgVector& point, dgVector* const contact) const
{
dgVector r (posit, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 t = normal % (r - point);
contact[0] = r - normal.Scale3 (t);
return 1;
}
void dgBilateralConstraint::SetPivotAndPinDir (const dgVector& pivot, const dgVector& pinDirection0, const dgVector& pinDirection1)
{
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
dgAssert ((pinDirection0 % pinDirection0) > dgFloat32 (0.0f));
m_localMatrix0.m_front = pinDirection0.Scale3 (dgRsqrt (pinDirection0 % pinDirection0));
m_localMatrix0.m_right = m_localMatrix0.m_front * pinDirection1;
m_localMatrix0.m_right = m_localMatrix0.m_right.Scale3 (dgRsqrt (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);
const dgMatrix& body1_Matrix = m_body1->GetMatrix();
m_localMatrix1 = m_localMatrix0 * body1_Matrix.Inverse();
m_localMatrix0 = m_localMatrix0 * body0_Matrix.Inverse();
}
dgVector dgCollisionTaperedCapsule::ConvexConicSupporVertex (const dgVector& dir) const
{
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;
return dgVector ((dir0 >= dir1) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgInt32 dgCollisionTaperedCapsule::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgInt32 count = 0;
dgVector p0 (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector dir0 (p0 - origin);
dgFloat32 dist0 = dir0 % normal;
if ((dist0 * dist0) < (m_radio0 * m_radio0)) {
contactsOut[count] = p0 - normal.Scale3 (dist0);
count ++;
}
dgVector p1 (-m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector dir1 (p1 - origin);
dgFloat32 dist1 = dir1 % normal;
if ((dist1 * dist1) < (m_radio1 * m_radio1)) {
contactsOut[count] = p1 - normal.Scale3 (dist1);
count ++;
}
return count;
}
dgVector dgCollisionTaperedCapsule::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
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 (dir1 > dir0) {
p0 = p1;
}
return p0;
}
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 dgCollisionInstance::CalculateBuoyancyAcceleration (const dgMatrix& matrix, const dgVector& origin, const dgVector& gravity, const dgVector& fluidPlane, dgFloat32 fluidDensity, dgFloat32 fluidViscosity, dgVector& accel, dgVector& alpha)
{
dgMatrix scaledMatrix (m_localMatrix * matrix);
switch (m_scaleType)
{
case m_unit:
case m_uniform:
case m_nonUniform:
{
scaledMatrix[0] = scaledMatrix[0].Scale4 (m_scale[0]);
scaledMatrix[1] = scaledMatrix[1].Scale4 (m_scale[1]);
scaledMatrix[2] = scaledMatrix[2].Scale4 (m_scale[2]);
break;
}
default:
dgAssert(0);
}
accel = dgVector (dgFloat32 (0.0f));
alpha = dgVector (dgFloat32 (0.0f));
dgVector volumeIntegral (m_childShape->CalculateVolumeIntegral (scaledMatrix, fluidPlane));
if (volumeIntegral.m_w > dgFloat32 (0.0f)) {
dgVector buoyanceCenter (volumeIntegral - origin);
dgVector force (gravity.Scale3 (-fluidDensity * volumeIntegral.m_w));
dgVector torque (buoyanceCenter * force);
// dgFloat32 damp = GetMax (GetMin ((m_veloc % m_veloc) * dgFloat32 (100.0f) * fluidLinearViscousity, dgFloat32 (1.0f)), dgFloat32(dgFloat32 (10.0f)));
// force -= m_veloc.Scale3 (damp);
//damp = (m_omega % m_omega) * dgFloat32 (10.0f) * fluidAngularViscousity;
// damp = GetMax (GetMin ((m_omega % m_omega) * dgFloat32 (1000.0f) * fluidAngularViscousity, dgFloat32(0.25f)), dgFloat32(2.0f));
// torque -= m_omega.Scale3 (damp);
// dgThreadHiveScopeLock lock (m_world, &m_criticalSectionLock);
accel += force;
alpha += torque;
}
}
