dgVector dgCollisionSphere::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));
dgAssert (dir.m_w == 0.0f);
return dir.Scale (m_radius);
}
dgInt32 dgCollisionSphere::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgAssert (normal.m_w == 0.0f);
//contactsOut[0] = normal.Scale3(normal % point);
contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point));
return 1;
}
void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const
{
if (level) {
dgAssert (dgAbs (p0.DotProduct(p0).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p1.DotProduct(p1).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p2.DotProduct(p2).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
//p01 = p01 * p01.InvMagSqrt();
//p12 = p12 * p12.InvMagSqrt();
//p20 = p20 * p20.InvMagSqrt();
p01 = p01.Normalize();
p12 = p12.Normalize();
p20 = p20.Normalize();
dgAssert (dgAbs (p01.DotProduct(p01).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p12.DotProduct(p12).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p20.DotProduct(p20).GetScalar() - 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 ++] = p0;
ouput[count ++] = p1;
ouput[count ++] = p2;
}
}
void dgConvexHull4d::TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const
{
if (level) {
dgAssert (dgAbsf (p0.DotProduct3(p0) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1.DotProduct3(p1) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2.DotProduct3(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.DotProduct3(p01)));
p12 = p12.Scale3 (dgRsqrt(p12.DotProduct3(p12)));
p20 = p20.Scale3 (dgRsqrt(p20.DotProduct3(p20)));
dgAssert (dgAbsf (p01.DotProduct3(p01) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12.DotProduct3(p12) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20.DotProduct3(p20) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TessellateTriangle (level - 1, p0, p01, p20, count, ouput, start);
TessellateTriangle (level - 1, p1, p12, p01, count, ouput, start);
TessellateTriangle (level - 1, p2, p20, p12, count, ouput, start);
TessellateTriangle (level - 1, p01, p12, p20, count, ouput, start);
} else {
dgBigPlane n (p0, p1, p2);
n = n.Scale (dgFloat64(1.0f) / sqrt (n.DotProduct3(n)));
n.m_w = dgFloat64(0.0f);
ouput[start] = n;
start += 8;
count ++;
}
}
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 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
dgAssert (normal.m_w == 0.0f);
dgAssert (normal.DotProduct(normal).GetScalar() > dgFloat32 (0.999f));
contactsOut[0] = normal * normal.DotProduct(point);
return 1;
}
dgInt32 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgAssert (normal.m_w == 0.0f);
dgAssert ((normal % normal) > dgFloat32 (0.999f));
//contactsOut[0] = normal.Scale3 (normal % point);
contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point));
return 1;
}
dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
dgJacobian velocStep;
if (m_gyroTorqueOn) {
dgVector dtHalf(timestep * dgVector::m_half);
dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);
dgVector localOmega(matrix.UnrotateVector(m_omega));
dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega * dtHalf);
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
dgVector::m_wOne);
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
//dgVector gradientStep(localTorque * timestep);
dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));
dgVector omega(matrix.RotateVector(localOmega + gradientStep));
dgAssert(omega.m_w == dgFloat32(0.0f));
// integrate rotation here
dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
dgVector omegaAxis(omega.Scale(invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
dgQuaternion deltaRotation(omegaAxis, omegaAngle);
m_gyroRotation = m_gyroRotation * deltaRotation;
dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
localOmega = matrix.UnrotateVector(omega);
//dgVector angularMomentum(inertia * localOmega);
//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
velocStep.m_angular = matrix.RotateVector(gradientStep);
} else {
velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
}
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
return velocStep;
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.9999f)) {
return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgVector sideDir (m_yzMask & dir);
sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, 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.99995f)) {
return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgVector sideDir (m_yzMask & dir);
dgAssert (sideDir.DotProduct(sideDir).GetScalar() > dgFloat32 (0.0f));
return sideDir.Normalize().Scale(m_radius);
}
dgIntersectStatus dgCollisionBVH::GetPolygon (void* const context, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 hitDistance)
{
dgPolygonMeshDesc& data = (*(dgPolygonMeshDesc*) context);
if (data.m_faceCount >= DG_MAX_COLLIDING_FACES) {
dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n"));
return t_StopSearh;
}
if ((data.m_globalIndexCount + indexCount * 2 + 3) >= DG_MAX_COLLIDING_INDICES) {
dgTrace (("buffer Over float, try using a lower resolution mesh for collision\n"));
return t_StopSearh;
}
if (data.m_me->GetDebugCollisionCallback()) {
dgTriplex triplex[128];
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
const dgVector scale = data.m_polySoupInstance->GetScale();
dgMatrix matrix (data.m_polySoupInstance->GetLocalMatrix() * data.m_polySoupBody->GetMatrix());
for (dgInt32 i = 0; i < indexCount; i ++ ) {
//dgVector p (&polygon[indexArray[i] * stride]);
//p = matrix.TransformVector(scale.CompProduct4(p));
dgVector p (matrix.TransformVector(scale.CompProduct4(dgVector(&polygon[indexArray[i] * stride]))));
triplex[i].m_x = p.m_x;
triplex[i].m_y = p.m_y;
triplex[i].m_z = p.m_z;
}
if (data.m_polySoupBody) {
data.m_me->GetDebugCollisionCallback() (data.m_polySoupBody, data.m_objBody, indexArray[indexCount], indexCount, &triplex[0].m_x, sizeof (dgTriplex));
}
}
dgAssert (data.m_vertex == polygon);
dgInt32 count = indexCount * 2 + 3;
data.m_faceIndexCount[data.m_faceCount] = indexCount;
// data.m_faceIndexStart[data.m_faceCount] = data.m_faceCount ? (data.m_faceIndexStart[data.m_faceCount - 1] + data.m_faceIndexCount[data.m_faceCount - 1]) : 0;
data.m_faceIndexStart[data.m_faceCount] = data.m_globalIndexCount;
data.m_hitDistance[data.m_faceCount] = hitDistance;
data.m_faceCount ++;
dgInt32* const dst = &data.m_faceVertexIndex[data.m_globalIndexCount];
//the docks say memcpy is an intrinsic function but as usual this is another Microsoft lied
//memcpy (dst, indexArray, sizeof (dgInt32) * count);
for (dgInt32 i = 0; i < count; i ++) {
dst[i] = indexArray[i];
}
data.m_globalIndexCount += count;
return t_ContinueSearh;
}
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);
}
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 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::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();
}
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();
}
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;
}
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;
}
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.9999f)) {
dgFloat32 x0 = (x >= dgFloat32 (0.0f)) ? m_height : -m_height;
return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f));
}
// dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
dgVector sideDir (m_yzMask & dir);
// sideDir = sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
// return sideDir + dir.Scale3 (m_height);
return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
void dgSolver::IntegrateBodiesVelocity(dgInt32 threadID)
{
dgVector speedFreeze2(m_world->m_freezeSpeed2 * dgFloat32(0.1f));
dgVector freezeOmega2(m_world->m_freezeOmega2 * dgFloat32(0.1f));
dgVector timestep4(m_timestepRK);
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
const dgInt32 step = m_threadCounts;;
const dgInt32 bodyCount = m_cluster->m_bodyCount - 1;
for (dgInt32 j = threadID; j < bodyCount; j += step) {
const dgInt32 i = j + 1;
dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body;
dgAssert(body->m_index == i);
if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
const dgJacobian& forceAndTorque = internalForces[i];
const dgVector force(body->m_externalForce + forceAndTorque.m_linear);
const dgVector torque(body->m_externalTorque + forceAndTorque.m_angular);
//const dgVector velocStep((force.Scale(body->m_invMass.m_w)) * timestep4);
//const dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4);
const dgJacobian velocStep(body->IntegrateForceAndToque(force, torque, timestep4));
if (!body->m_resting) {
//body->m_veloc += velocStep;
//body->m_omega += omegaStep;
body->m_veloc += velocStep.m_linear;
body->m_omega += velocStep.m_angular;
} else {
//const dgVector velocStep2(velocStep.DotProduct(velocStep));
//const dgVector omegaStep2(omegaStep.DotProduct(omegaStep));
const dgVector velocStep2(velocStep.m_linear.DotProduct(velocStep.m_linear));
const dgVector omegaStep2(velocStep.m_angular.DotProduct(velocStep.m_angular));
const dgVector test(((velocStep2 > speedFreeze2) | (omegaStep2 > speedFreeze2)) & m_negOne);
const dgInt32 equilibrium = test.GetSignMask() ? 0 : 1;
body->m_resting &= equilibrium;
}
dgAssert(body->m_veloc.m_w == dgFloat32(0.0f));
dgAssert(body->m_omega.m_w == dgFloat32(0.0f));
}
}
}
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 dgCollisionChamferCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgFloat32 disc2 = point.m_y * point.m_y + point.m_z * point.m_z;
if (disc2 < m_radius * m_radius) {
dgVector cylinderNormal ((point.m_x >= dgFloat32 (0.0)) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
return CalculateContactsGeneric (point, cylinderNormal, proxy, contactsOut);
} else {
dgVector r (dgFloat32 (0.0f), point.m_y, point.m_z, dgFloat32 (0.0f));
dgAssert ((r % r) > dgFloat32 (0.0f));
// r = r.Scale3(m_radius * dgRsqrt (r % r));
r = r.CompProduct4(r.InvMagSqrt()).Scale4(m_radius);
//dgFloat32 t = normal % (r - point);
dgVector t (normal.DotProduct4(r - point));
//contactsOut[0] = r - normal.Scale3 (t);
contactsOut[0] = r - normal.CompProduct4(t);
return 1;
}
}
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 dgCollisionBox::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir.DotProduct3(dir) - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dir.m_w == dgFloat32 (0.0f));
dgVector mask (dir < dgVector (dgFloat32 (0.0f)));
if (vertexIndex) {
dgVector index (m_indexMark.CompProduct4(mask & dgVector::m_one));
index = (index.AddHorizontal()).GetInt();
*vertexIndex = dgInt32 (index.m_ix);
}
return (m_size[1] & mask) + m_size[0].AndNot(mask);
}
dgFloat32 dgCollisionCone::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const
{
dgFloat32 volume;
dgFloat32 inertaxx;
dgFloat32 inertayyzz;
//dgVector centerOfMass1;
//dgVector inertia1;
//dgVector crossInertia1;
//volume = dgCollisionConvex::CalculateMassProperties (inertia1, crossInertia1, centerOfMass1);
volume = dgFloat32 (3.1616f * 2.0f / 3.0f) * m_radius * m_radius * m_height;
centerOfMass = GetOffsetMatrix().m_posit - GetOffsetMatrix().m_front.Scale (dgFloat32 (0.5f) * m_height);
inertaxx = dgFloat32 (3.0f / 10.0f) * m_radius * m_radius * volume;
inertayyzz = (dgFloat32 (3.0f / 20.0f) * m_radius * m_radius + dgFloat32 (4.0f / 10.0f) * m_height * m_height) * volume;
dgMatrix inertiaTensor (dgGetIdentityMatrix());
inertiaTensor[0][0] = inertaxx;
inertiaTensor[1][1] = inertayyzz;
inertiaTensor[2][2] = inertayyzz;
inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix();
crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z;
crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x;
crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y;
dgVector central (centerOfMass.CompProduct(centerOfMass));
inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z);
inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x);
inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y);
centerOfMass = centerOfMass.Scale (volume);
return volume;
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgInt32* const vertexIndex) const
{
*vertexIndex = -1;
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.99995f)) {
return dgVector::m_zero;
}
dgVector sideDir (m_yzMask & dir);
dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f));
return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius);
}
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 dgCollisionChamferCylinder::ConvexConicSupporVertex (const dgVector& dir) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
//if (dgAbsf (dir.m_x) > dgFloat32 (0.99995f)) {
if (dgAbsf (x) > dgFloat32 (0.99995f)) {
return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
}
//dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
dgVector sideDir (m_yzMask & dir);
dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f));
//return sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir));
return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir) const
{
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dgAbsf (dir.m_x) > dgFloat32 (0.9998f)) {
dgFloat32 x0;
x0 = (dir.m_x >= dgFloat32 (0.0f)) ? m_height : -m_height;
return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f));
}
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
sideDir = sideDir.Scale (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
return sideDir + dir.Scale (m_height);
}
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;
}
