void dgBallConstraint::SetLimits (
const dgVector& coneDir,
dgFloat32 minConeAngle,
dgFloat32 maxConeAngle,
dgFloat32 maxTwistAngle,
const dgVector& bilateralDir,
dgFloat32 negativeBilateralConeAngle__,
dgFloat32 positiveBilateralConeAngle__)
{
dgMatrix matrix0;
dgMatrix matrix1;
CalculateGlobalMatrixAndAngle (matrix0, matrix1);
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0_Matrix = m_body0->GetMatrix();
dgVector lateralDir (bilateralDir * coneDir);
if ((lateralDir % lateralDir) < dgFloat32 (1.0e-3f)) {
dgMatrix tmp (coneDir);
lateralDir = tmp.m_up;
}
m_localMatrix0.m_front = body0_Matrix.UnrotateVector (coneDir);
m_localMatrix0.m_up = body0_Matrix.UnrotateVector (lateralDir);
m_localMatrix0.m_posit = body0_Matrix.UntransformVector (matrix1.m_posit);
m_localMatrix0.m_front = m_localMatrix0.m_front.Scale3 (dgRsqrt (m_localMatrix0.m_front % m_localMatrix0.m_front));
m_localMatrix0.m_up = m_localMatrix0.m_up.Scale3 (dgRsqrt (m_localMatrix0.m_up % m_localMatrix0.m_up));
m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up;
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_twistAngle = dgClamp (maxTwistAngle, dgFloat32 (5.0f) * dgDEG2RAD, dgFloat32 (90.0f) * dgDEG2RAD);
m_coneAngle = dgClamp ((maxConeAngle - minConeAngle) * dgFloat32 (0.5f), dgFloat32 (5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD);
m_coneAngleCos = dgCos (m_coneAngle);
dgMatrix coneMatrix (dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32 (0.5f)));
m_localMatrix0 = coneMatrix * m_localMatrix0;
m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse();
}
dgBigVector dgPointToRayDistance (const dgBigVector& point, const dgBigVector& ray_p0, const dgBigVector& ray_p1)
{
dgBigVector dp (ray_p1 - ray_p0);
dgAssert (dp.m_w == dgFloat32 (0.0f));
dgFloat64 t = dgClamp (dp.DotProduct3 (point - ray_p0) / dp.DotProduct3 (dp), dgFloat64(0.0f), dgFloat64 (1.0f));
return ray_p0 + dp.Scale (t);
}
DG_INLINE void InitKinematicMassMatrix (const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgFloat32 correctionFactor)
{
dgAssert (0);
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
m_bodyForce.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
m_jointForce[m_dof] = -dgClamp(row->m_penetration * correctionFactor, dgFloat32(-0.25f), dgFloat32(0.25f));
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
void NewtonUserJoint::SetRowStiffness (dgFloat32 stiffness)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
stiffness = dgFloat32 (1.0f) - dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
stiffness = -dgFloat32 (1.0f) - stiffness / DG_PSD_DAMP_TOL;
m_param->m_jointStiffness[index] = stiffness;
}
}
void dgWorld::GetHardwareVendorString (dgInt32 deviceIndex, char* const description, dgInt32 maxlength) const
{
deviceIndex = dgClamp(deviceIndex, 0, EnumerateHardwareModes() - 1);
if (deviceIndex == 0) {
sprintf (description, "newton cpu");
} else if (m_amp) {
#ifdef _NEWTON_AMP
m_amp->GetVendorString (deviceIndex - 1, description, maxlength);
#endif
}
}
void dgWorld::SetCurrentHardwareMode(dgInt32 deviceIndex)
{
#ifdef _NEWTON_AMP
if (m_amp) {
m_amp->CleanUp();
}
#endif
m_hardwaredIndex = dgClamp(deviceIndex, 0, EnumerateHardwareModes() - 1);
if ((m_hardwaredIndex > 0) && m_amp){
#ifdef _NEWTON_AMP
m_amp->SelectPlaform (m_hardwaredIndex - 1);
#endif
}
}
void NewtonUserJoint::SetLowerFriction (dgFloat32 friction)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF)))
{
m_param->m_forceBounds[index].m_low = dgClamp (friction, dgFloat32 (DG_MIN_BOUND), dgFloat32 (-0.001f));
m_param->m_forceBounds[index].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
#ifdef _DEBUG
dgInt32 i0 = 0;
dgInt32 i1 = m_rows - 1;
while ((i0 <= i1) && (m_param->m_forceBounds[i0].m_normalIndex != DG_BILATERAL_FRICTION_CONSTRAINT)) i0++;
while ((i1 >= i0) && (m_param->m_forceBounds[i1].m_normalIndex == DG_BILATERAL_FRICTION_CONSTRAINT)) i1--;
dgAssert ((i0 - i1) == 1);
if ((i0 - i1) != 1)
dgTrace (("make sure that friction joint are issue at last\n"));
#endif
}
}
void dgBody::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia)
{
dgFloat32 Ixx = inertia[0][0];
dgFloat32 Iyy = inertia[1][1];
dgFloat32 Izz = inertia[2][2];
mass = dgAbsf (mass);
if (m_collision->IsType(dgCollision::dgCollisionMesh_RTTI) || m_collision->IsType(dgCollision::dgCollisionScene_RTTI)) {
mass = DG_INFINITE_MASS * 2.0f;
}
if (mass < DG_MINIMUM_MASS) {
mass = DG_INFINITE_MASS * 2.0f;
}
//dgAssert (m_masterNode);
m_world->GetBroadPhase()->CheckStaticDynamic(this, mass);
if (mass >= DG_INFINITE_MASS) {
m_mass.m_x = DG_INFINITE_MASS;
m_mass.m_y = DG_INFINITE_MASS;
m_mass.m_z = DG_INFINITE_MASS;
m_mass.m_w = DG_INFINITE_MASS;
m_invMass.m_x = dgFloat32 (0.0f);
m_invMass.m_y = dgFloat32 (0.0f);
m_invMass.m_z = dgFloat32 (0.0f);
m_invMass.m_w = dgFloat32 (0.0f);
if (m_masterNode) {
dgBodyMasterList& masterList (*m_world);
if (masterList.GetFirst() != m_masterNode) {
masterList.InsertAfter (masterList.GetFirst(), m_masterNode);
}
}
SetAparentMassMatrix (m_mass);
} else {
Ixx = dgAbsf (Ixx);
Iyy = dgAbsf (Iyy);
Izz = dgAbsf (Izz);
dgFloat32 Ixx1 = dgClamp (Ixx, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgFloat32 Iyy1 = dgClamp (Iyy, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgFloat32 Izz1 = dgClamp (Izz, dgFloat32 (0.001f) * mass, dgFloat32 (1000.0f) * mass);
dgAssert (Ixx > dgFloat32 (0.0f));
dgAssert (Iyy > dgFloat32 (0.0f));
dgAssert (Izz > dgFloat32 (0.0f));
m_mass.m_x = Ixx1;
m_mass.m_y = Iyy1;
m_mass.m_z = Izz1;
m_mass.m_w = mass;
m_invMass.m_x = dgFloat32 (1.0f) / Ixx1;
m_invMass.m_y = dgFloat32 (1.0f) / Iyy1;
m_invMass.m_z = dgFloat32 (1.0f) / Izz1;
m_invMass.m_w = dgFloat32 (1.0f) / mass;
if (m_masterNode) {
dgBodyMasterList& masterList (*m_world);
masterList.RotateToEnd (m_masterNode);
}
SetAparentMassMatrix (dgVector (Ixx, Iyy, Izz, mass));
}
#ifdef _DEBUG
dgBodyMasterList& me = *m_world;
for (dgBodyMasterList::dgListNode* refNode = me.GetFirst(); refNode; refNode = refNode->GetNext()) {
dgBody* const body0 = refNode->GetInfo().GetBody();
dgVector invMass (body0->GetInvMass());
if (invMass.m_w != 0.0f) {
for (; refNode; refNode = refNode->GetNext()) {
dgBody* const body1 = refNode->GetInfo().GetBody();
dgVector invMass (body1->GetInvMass());
dgAssert (invMass.m_w != 0.0f);
}
break;
}
}
#endif
}
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;
}
}
void dgBilateralConstraint::SetStiffness(dgFloat32 stiffness)
{
m_stiffness = dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
}
void dgBilateralConstraint::CalculatePointDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir, const dgPointParam& param, dgForceImpactPair* const jointForce)
{
dgAssert (jointForce);
dgAssert (m_body0);
dgAssert (m_body1);
dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
dgVector r0CrossDir (param.m_r0 * dir);
jacobian0.m_linear[0] = dir.m_x;
jacobian0.m_linear[1] = dir.m_y;
jacobian0.m_linear[2] = dir.m_z;
jacobian0.m_linear[3] = dgFloat32 (0.0f);
jacobian0.m_angular[0] = r0CrossDir.m_x;
jacobian0.m_angular[1] = r0CrossDir.m_y;
jacobian0.m_angular[2] = r0CrossDir.m_z;
jacobian0.m_angular[3] = dgFloat32 (0.0f);
dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgVector r1CrossDir (dir * param.m_r1);
jacobian1.m_linear[0] = -dir.m_x;
jacobian1.m_linear[1] = -dir.m_y;
jacobian1.m_linear[2] = -dir.m_z;
jacobian1.m_linear[3] = dgFloat32 (0.0f);
jacobian1.m_angular[0] = r1CrossDir.m_x;
jacobian1.m_angular[1] = r1CrossDir.m_y;
jacobian1.m_angular[2] = r1CrossDir.m_z;
jacobian1.m_angular[3] = dgFloat32 (0.0f);
m_rowIsMotor[index] = 0;
m_motorAcceleration[index] = dgFloat32 (0.0f);
dgVector velocError (param.m_veloc1 - param.m_veloc0);
dgFloat32 relVeloc = velocError % dir;
if (desc.m_timestep > dgFloat32 (0.0f)) {
dgVector positError (param.m_posit1 - param.m_posit0);
dgVector centrError (param.m_centripetal1 - param.m_centripetal0);
dgFloat32 relPosit = positError % dir;
dgFloat32 relCentr = centrError % dir;
relCentr = dgClamp (relCentr, dgFloat32(-100000.0f), dgFloat32(100000.0f));
desc.m_zeroRowAcceleration[index] = (relPosit + relVeloc * desc.m_timestep) * desc.m_invTimestep * desc.m_invTimestep;
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = DG_POS_DAMP;
dgFloat32 kd = DG_VEL_DAMP;
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
dgFloat32 accelError = num / den;
desc.m_penetration[index] = relPosit;
desc.m_penetrationStiffness[index] = dgFloat32 (0.01f/4.0f);
desc.m_jointStiffness[index] = param.m_stiffness;
desc.m_jointAccel[index] = accelError + relCentr;
// save centripetal acceleration in the restitution member
desc.m_restitution[index] = relCentr;
desc.m_forceBounds[index].m_jointForce = jointForce;
} else {
desc.m_penetration[index] = dgFloat32 (0.0f);
desc.m_penetrationStiffness[index] = dgFloat32 (0.0f);
desc.m_jointStiffness[index] = param.m_stiffness;
desc.m_jointAccel[index] = relVeloc;
desc.m_restitution[index] = dgFloat32 (0.0f);
desc.m_zeroRowAcceleration[index] = dgFloat32 (0.0f);
desc.m_forceBounds[index].m_jointForce = jointForce;
}
}
void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex)
{
dgPointParam pointData;
dgFloat32 impulseOrForceScale = (params.m_timestep > dgFloat32 (0.0f)) ? params.m_invTimestep : dgFloat32 (1.0f);
InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point);
CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData);
dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
dgFloat32 restitution = contact.m_restitution;
dgFloat32 relVelocErr = velocError % contact.m_normal;
dgFloat32 penetration = dgClamp (contact.m_penetration - DG_RESTING_CONTACT_PENETRATION, dgFloat32(0.0f), dgFloat32(0.5f));
dgFloat32 penetrationStiffness = MAX_PENETRATION_STIFFNESS * contact.m_softness;
dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
dgAssert (dgAbsf (penetrationVeloc - MAX_PENETRATION_STIFFNESS * contact.m_softness * penetration) < dgFloat32 (1.0e-6f));
if (relVelocErr > REST_RELATIVE_VELOCITY) {
relVelocErr *= (restitution + dgFloat32 (1.0f));
}
params.m_restitution[normalIndex] = restitution;
params.m_penetration[normalIndex] = penetration;
params.m_penetrationStiffness[normalIndex] = penetrationStiffness;
params.m_forceBounds[normalIndex].m_low = dgFloat32 (0.0f);
params.m_forceBounds[normalIndex].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[normalIndex].m_jointForce = (dgForceImpactPair*) &contact.m_normal_Force;
params.m_jointStiffness[normalIndex] = dgFloat32 (1.0f);
params.m_isMotor[normalIndex] = 0;
// params.m_jointAccel[normalIndex] = GetMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * params.m_invTimestep;
params.m_jointAccel[normalIndex] = dgMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * impulseOrForceScale;
if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel) {
params.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force;
}
// first dir friction force
if (contact.m_flags & dgContactMaterial::m_friction0Enable) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData);
relVelocErr = velocError % contact.m_dir0;
params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
// if (contact.m_override0Accel) {
if (contact.m_flags & dgContactMaterial::m_override0Accel) {
params.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force;
params.m_isMotor[jacobIndex] = 1;
} else {
//params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
params.m_isMotor[jacobIndex] = 0;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0;
} else {
params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir0_Force;
}
// if (contact.m_friction1Enable) {
if (contact.m_flags & dgContactMaterial::m_friction1Enable) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData);
relVelocErr = velocError % contact.m_dir1;
params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
// if (contact.m_override1Accel) {
if (contact.m_flags & dgContactMaterial::m_override1Accel) {
dgAssert (0);
params.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force;
params.m_isMotor[jacobIndex] = 1;
} else {
//params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
params.m_isMotor[jacobIndex] = 0;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1;
} else {
params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir1_Force;
}
//dgTrace (("p(%f %f %f)\n", params.m_jointAccel[normalIndex], params.m_jointAccel[normalIndex + 1], params.m_jointAccel[normalIndex + 2]));
}
void dgWorldDynamicUpdate::BuildJacobianMatrix (const dgBodyInfo* const bodyInfoArray, const dgJointInfo* const jointInfo, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 forceImpulseScale) const
{
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgBody* const body0 = bodyInfoArray[m0].m_body;
const dgBody* const body1 = bodyInfoArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgVector invMass0(body0->m_invMass[3]);
const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix;
const dgVector invMass1(body1->m_invMass[3]);
const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix;
dgVector force0(dgVector::m_zero);
dgVector torque0(dgVector::m_zero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = ((dgDynamicBody*)body0)->m_externalForce;
torque0 = ((dgDynamicBody*)body0)->m_externalTorque;
}
dgVector force1(dgVector::m_zero);
dgVector torque1(dgVector::m_zero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = ((dgDynamicBody*)body1)->m_externalForce;
torque1 = ((dgDynamicBody*)body1)->m_externalTorque;
}
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
dgJacobian forceAcc0;
dgJacobian forceAcc1;
forceAcc0.m_linear = dgVector::m_zero;
forceAcc0.m_angular = dgVector::m_zero;
forceAcc1.m_linear = dgVector::m_zero;
forceAcc1.m_angular = dgVector::m_zero;
for (dgInt32 i = 0; i < count; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f));
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);
dgVector tmpAccel(row->m_JMinv.m_jacobianM0.m_linear * force0 + row->m_JMinv.m_jacobianM0.m_angular * torque0 +
row->m_JMinv.m_jacobianM1.m_linear * force1 + row->m_JMinv.m_jacobianM1.m_angular * torque1);
dgAssert(tmpAccel.m_w == dgFloat32(0.0f));
dgFloat32 extenalAcceleration = -(tmpAccel.AddHorizontal()).GetScalar();
row->m_deltaAccel = extenalAcceleration * forceImpulseScale;
row->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(row->m_jointFeebackForce);
const dgFloat32 force = row->m_jointFeebackForce->m_force * forceImpulseScale;
row->m_force = isBilateral ? dgClamp(force, row->m_lowerBoundFrictionCoefficent, row->m_upperBoundFrictionCoefficent) : force;
row->m_force0 = row->m_force;
row->m_maxImpact = dgFloat32(0.0f);
dgVector jMinvM0linear (scale0 * row->m_JMinv.m_jacobianM0.m_linear);
dgVector jMinvM0angular (scale0 * row->m_JMinv.m_jacobianM0.m_angular);
dgVector jMinvM1linear (scale1 * row->m_JMinv.m_jacobianM1.m_linear);
dgVector jMinvM1angular (scale1 * row->m_JMinv.m_jacobianM1.m_angular);
dgVector tmpDiag(jMinvM0linear * row->m_Jt.m_jacobianM0.m_linear + jMinvM0angular * row->m_Jt.m_jacobianM0.m_angular +
jMinvM1linear * row->m_Jt.m_jacobianM1.m_linear + jMinvM1angular * row->m_Jt.m_jacobianM1.m_angular);
dgAssert (tmpDiag.m_w == dgFloat32 (0.0f));
dgFloat32 diag = (tmpDiag.AddHorizontal()).GetScalar();
dgAssert(diag > dgFloat32(0.0f));
row->m_diagDamp = diag * row->m_stiffness;
diag *= (dgFloat32(1.0f) + row->m_stiffness);
row->m_jMinvJt = diag;
row->m_invJMinvJt = dgFloat32(1.0f) / diag;
dgAssert(dgCheckFloat(row->m_force));
dgVector val(row->m_force);
forceAcc0.m_linear += row->m_Jt.m_jacobianM0.m_linear * val;
forceAcc0.m_angular += row->m_Jt.m_jacobianM0.m_angular * val;
forceAcc1.m_linear += row->m_Jt.m_jacobianM1.m_linear * val;
forceAcc1.m_angular += row->m_Jt.m_jacobianM1.m_angular * val;
}
forceAcc0.m_linear = forceAcc0.m_linear * scale0;
forceAcc0.m_angular = forceAcc0.m_angular * scale0;
forceAcc1.m_linear = forceAcc1.m_linear * scale1;
forceAcc1.m_angular = forceAcc1.m_angular * scale1;
if (!body0->m_equilibrium) {
internalForces[m0].m_linear += forceAcc0.m_linear;
internalForces[m0].m_angular += forceAcc0.m_angular;
}
if (!body1->m_equilibrium) {
internalForces[m1].m_linear += forceAcc1.m_linear;
internalForces[m1].m_angular += forceAcc1.m_angular;
}
}
dgUnsigned32 dgWorld::GetPerfomanceTicks (dgUnsigned32 entry) const
{
entry = dgClamp(dgUnsigned32 (entry), dgUnsigned32 (0), dgUnsigned32 (m_counterSize - 1));
return m_perfomanceCountersBack[entry];
}
void dgBilateralConstraint::SetStiffness(dgFloat32 stiffness)
{
stiffness = dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
m_stiffness = DG_JOINT_STIFFNESS_RANGE - stiffness * (DG_JOINT_STIFFNESS_RANGE - dgFloat32 (1.0f));
}
