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();
}
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);
}
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();
}
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) 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 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 dgCollisionInstance::CalculateBuoyancyAcceleration (const dgMatrix& matrix, const dgVector& origin, const dgVector& gravity, const dgVector& fluidPlane, dgFloat32 fluidDensity, dgFloat32 fluidViscosity, dgVector& unitForce, dgVector& unitTorque)
{
dgMatrix globalMatrix (m_localMatrix * matrix);
unitForce = dgVector (dgFloat32 (0.0f));
unitTorque = dgVector (dgFloat32 (0.0f));
dgVector volumeIntegral (m_childShape->CalculateVolumeIntegral (globalMatrix, fluidPlane, *this));
if (volumeIntegral.m_w > dgFloat32 (0.0f)) {
dgVector buoyanceCenter (volumeIntegral - origin);
dgVector force (gravity.Scale (-fluidDensity * volumeIntegral.m_w));
dgVector torque (buoyanceCenter.CrossProduct(force));
unitForce += force;
unitTorque += torque;
}
}
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);
const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
const dgInt32 step = m_threadCounts;;
const dgInt32 bodyCount = m_cluster->m_bodyCount;
for (dgInt32 i = threadID; i < bodyCount; i += step) {
dgDynamicBody* const body = (dgDynamicBody*)m_bodyArray[i].m_body;
dgAssert(body->m_index == i);
if (body->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
const dgVector w(bodyProxyArray[i].m_weight);
const dgJacobian& forceAndTorque = internalForces[i];
const dgVector force(body->m_externalForce + forceAndTorque.m_linear * w);
const dgVector torque(body->m_externalTorque + forceAndTorque.m_angular * w);
const dgVector velocStep((force.Scale(body->m_invMass.m_w)) * timestep4);
const dgVector omegaStep((body->m_invWorldInertiaMatrix.RotateVector(torque)) * timestep4);
if (!body->m_resting) {
body->m_veloc += velocStep;
body->m_omega += omegaStep;
} else {
const dgVector velocStep2(velocStep.DotProduct(velocStep));
const dgVector omegaStep2(omegaStep.DotProduct(omegaStep));
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));
}
}
}
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 dgPointToTriangleDistance (const dgVector& point, const dgVector& p0, const dgVector& p1, const dgVector& p2)
{
// const dgVector p (float (0.0f), float (0.0f), float (0.0f));
const dgVector p10 (p1 - p0);
const dgVector p20 (p2 - p0);
const dgVector p_p0 (point - p0);
float alpha1 = p10 % p_p0;
float alpha2 = p20 % p_p0;
if ((alpha1 <= float (0.0f)) && (alpha2 <= float (0.0f))) {
return p0;
}
dgVector p_p1 (point - p1);
float alpha3 = p10 % p_p1;
float alpha4 = p20 % p_p1;
if ((alpha3 >= float (0.0f)) && (alpha4 <= alpha3)) {
return p1;
}
float vc = alpha1 * alpha4 - alpha3 * alpha2;
if ((vc <= float (0.0f)) && (alpha1 >= float (0.0f)) && (alpha3 <= float (0.0f))) {
float t = alpha1 / (alpha1 - alpha3);
HACD_ASSERT (t >= float (0.0f));
HACD_ASSERT (t <= float (1.0f));
return p0 + p10.Scale (t);
}
dgVector p_p2 (point - p2);
float alpha5 = p10 % p_p2;
float alpha6 = p20 % p_p2;
if ((alpha6 >= float (0.0f)) && (alpha5 <= alpha6)) {
return p2;
}
float vb = alpha5 * alpha2 - alpha1 * alpha6;
if ((vb <= float (0.0f)) && (alpha2 >= float (0.0f)) && (alpha6 <= float (0.0f))) {
float t = alpha2 / (alpha2 - alpha6);
HACD_ASSERT (t >= float (0.0f));
HACD_ASSERT (t <= float (1.0f));
return p0 + p20.Scale (t);
}
float va = alpha3 * alpha6 - alpha5 * alpha4;
if ((va <= float (0.0f)) && ((alpha4 - alpha3) >= float (0.0f)) && ((alpha5 - alpha6) >= float (0.0f))) {
float t = (alpha4 - alpha3) / ((alpha4 - alpha3) + (alpha5 - alpha6));
HACD_ASSERT (t >= float (0.0f));
HACD_ASSERT (t <= float (1.0f));
return p1 + (p2 - p1).Scale (t);
}
float den = float(float (1.0f)) / (va + vb + vc);
float t = vb * den;
float s = vc * den;
HACD_ASSERT (t >= float (0.0f));
HACD_ASSERT (s >= float (0.0f));
HACD_ASSERT (t <= float (1.0f));
HACD_ASSERT (s <= float (1.0f));
return p0 + p10.Scale (t) + p20.Scale (s);
}
dgVector dgCollisionSphere::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const
{
return dir.Scale (m_radius - DG_PENETRATION_TOL);
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const
{
dgAssert (dir.m_w == 0.0f);
return point + dir.Scale(m_height - DG_PENETRATION_TOL);
}
dgVector dgPointToTriangleDistance (const dgVector& point, const dgVector& p0, const dgVector& p1, const dgVector& p2)
{
// const dgVector p (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
const dgVector p10 (p1 - p0);
const dgVector p20 (p2 - p0);
const dgVector p_p0 (point - p0);
dgFloat32 alpha1 = p10 % p_p0;
dgFloat32 alpha2 = p20 % p_p0;
if ((alpha1 <= dgFloat32 (0.0f)) && (alpha2 <= dgFloat32 (0.0f))) {
return p0;
}
dgVector p_p1 (point - p1);
dgFloat32 alpha3 = p10 % p_p1;
dgFloat32 alpha4 = p20 % p_p1;
if ((alpha3 >= dgFloat32 (0.0f)) && (alpha4 <= alpha3)) {
return p1;
}
dgFloat32 vc = alpha1 * alpha4 - alpha3 * alpha2;
if ((vc <= dgFloat32 (0.0f)) && (alpha1 >= dgFloat32 (0.0f)) && (alpha3 <= dgFloat32 (0.0f))) {
dgFloat32 t = alpha1 / (alpha1 - alpha3);
_ASSERTE (t >= dgFloat32 (0.0f));
_ASSERTE (t <= dgFloat32 (1.0f));
return p0 + p10.Scale (t);
}
dgVector p_p2 (point - p2);
dgFloat32 alpha5 = p10 % p_p2;
dgFloat32 alpha6 = p20 % p_p2;
if ((alpha6 >= dgFloat32 (0.0f)) && (alpha5 <= alpha6)) {
return p2;
}
dgFloat32 vb = alpha5 * alpha2 - alpha1 * alpha6;
if ((vb <= dgFloat32 (0.0f)) && (alpha2 >= dgFloat32 (0.0f)) && (alpha6 <= dgFloat32 (0.0f))) {
dgFloat32 t = alpha2 / (alpha2 - alpha6);
_ASSERTE (t >= dgFloat32 (0.0f));
_ASSERTE (t <= dgFloat32 (1.0f));
return p0 + p20.Scale (t);
}
dgFloat32 va = alpha3 * alpha6 - alpha5 * alpha4;
if ((va <= dgFloat32 (0.0f)) && ((alpha4 - alpha3) >= dgFloat32 (0.0f)) && ((alpha5 - alpha6) >= dgFloat32 (0.0f))) {
dgFloat32 t = (alpha4 - alpha3) / ((alpha4 - alpha3) + (alpha5 - alpha6));
_ASSERTE (t >= dgFloat32 (0.0f));
_ASSERTE (t <= dgFloat32 (1.0f));
return p1 + (p2 - p1).Scale (t);
}
dgFloat32 den = float(dgFloat32 (1.0f)) / (va + vb + vc);
dgFloat32 t = vb * den;
dgFloat32 s = vc * den;
_ASSERTE (t >= dgFloat32 (0.0f));
_ASSERTE (s >= dgFloat32 (0.0f));
_ASSERTE (t <= dgFloat32 (1.0f));
_ASSERTE (s <= dgFloat32 (1.0f));
return p0 + p10.Scale (t) + p20.Scale (s);
}
