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; } }