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);
}
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 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, 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);
}
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);
}
DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide)
{
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgDynamicBody* const body0 = (dgDynamicBody*)m_bodyArray[m0].m_body;
const dgDynamicBody* const body1 = (dgDynamicBody*)m_bodyArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgMatrix invInertia0 = body0->m_invWorldInertiaMatrix;
const dgMatrix invInertia1 = body1->m_invWorldInertiaMatrix;
const dgVector invMass0(body0->m_invMass[3]);
const dgVector invMass1(body1->m_invMass[3]);
dgSoaFloat force0(m_soaZero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = dgSoaFloat(body0->m_externalForce, body0->m_externalTorque);
}
dgSoaFloat force1(m_soaZero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = dgSoaFloat(body1->m_externalForce, body1->m_externalTorque);
}
jointInfo->m_preconditioner0 = dgFloat32(1.0f);
jointInfo->m_preconditioner1 = dgFloat32(1.0f);
if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f)) && !(body0->GetSkeleton() && body1->GetSkeleton())) {
const dgFloat32 mass0 = body0->GetMass().m_w;
const dgFloat32 mass1 = body1->GetMass().m_w;
if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) {
jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER);
} else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) {
jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER);
}
}
const dgFloat32 forceImpulseScale = dgFloat32(1.0f);
const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
for (dgInt32 i = 0; i < count; i++) {
dgLeftHandSide* const row = &leftHandSide[index + i];
dgRightHandSide* const rhs = &rightHandSide[index + i];
row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);
const dgSoaFloat& JMinvM0 = (dgSoaFloat&)row->m_JMinv.m_jacobianM0;
const dgSoaFloat& JMinvM1 = (dgSoaFloat&)row->m_JMinv.m_jacobianM1;
const dgSoaFloat tmpAccel((JMinvM0 * force0).MulAdd(JMinvM1, force1));
dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal();
rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale;
rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(rhs->m_jointFeebackForce);
const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale;
rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force;
rhs->m_maxImpact = dgFloat32(0.0f);
const dgSoaFloat& JtM0 = (dgSoaFloat&)row->m_Jt.m_jacobianM0;
const dgSoaFloat& JtM1 = (dgSoaFloat&)row->m_Jt.m_jacobianM1;
const dgSoaFloat tmpDiag((weight0 * JMinvM0 * JtM0).MulAdd(weight1, JMinvM1 * JtM1));
dgFloat32 diag = tmpDiag.AddHorizontal();
dgAssert(diag > dgFloat32(0.0f));
rhs->m_diagDamp = diag * rhs->m_stiffness;
diag *= (dgFloat32(1.0f) + rhs->m_stiffness);
//rhs->m_jinvMJt = diag;
rhs->m_invJinvMJt = dgFloat32(1.0f) / diag;
}
}
