void dComplentaritySolver::dBilateralJoint::CalculatePointDerivative (dParamInfo* const constraintParams, const dVector& dir, const dPointDerivativeParam& param)
{
int index = constraintParams->m_count;
dJacobian &jacobian0 = constraintParams->m_jacobians[index].m_jacobian_IM0;
dVector 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] = dFloat (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] = 0.0f;
dJacobian &jacobian1 = constraintParams->m_jacobians[index].m_jacobian_IM1;
dVector 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] = dFloat (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] = 0.0f;
dVector velocError (param.m_veloc1 - param.m_veloc0);
dVector positError (param.m_posit1 - param.m_posit0);
dVector centrError (param.m_centripetal1 - param.m_centripetal0);
dFloat relPosit = positError % dir;
dFloat relVeloc = velocError % dir;
dFloat relCentr = centrError % dir;
dFloat dt = constraintParams->m_timestep;
dFloat ks = COMPLEMENTARITY_POS_DAMP;
dFloat kd = COMPLEMENTARITY_VEL_DAMP;
dFloat ksd = dt * ks;
dFloat num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dFloat den = dFloat (1.0f) + dt * kd + dt * ksd;
dFloat accelError = num / den;
m_rowIsMotor[index] = false;
m_motorAcceleration[index] = 0.0f;
constraintParams->m_jointAccel[index] = accelError + relCentr;
constraintParams->m_jointLowFriction[index] = COMPLEMENTARITY_MIN_FRICTION_BOUND;
constraintParams->m_jointHighFriction[index] = COMPLEMENTARITY_MAX_FRICTION_BOUND;
constraintParams->m_count = index + 1;
}
void dComplemtaritySolver::dFrictionLessContactJoint::JacobianDerivative (dParamInfo * const constraintParams)
{
for (int i = 0; i < m_count; i ++)
{
dPointDerivativeParam pointData;
InitPointParam (pointData, m_contacts[i].m_point);
CalculatePointDerivative (constraintParams, m_contacts[i].m_normal, pointData);
dVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
//dFloat restitution = 0.05f;
dFloat relVelocErr = velocError % m_contacts[i].m_normal;
dFloat penetration = 0.0f;
dFloat penetrationStiffness = 0.0f;
dFloat penetrationVeloc = penetration * penetrationStiffness;
if (relVelocErr > dFloat (1.0e-3f))
relVelocErr *= (m_restitution + dFloat (1.0f));
constraintParams->m_jointLowFriction[i] = dFloat (0.0f);
constraintParams->m_jointAccel[i] = dMax (dFloat (-4.0f), relVelocErr + penetrationVeloc) * constraintParams->m_timestepInv;
}
}
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params)
{
dgInt32 ret;
dgFloat32 relVelocErr;
dgFloat32 penetrationErr;
dgMatrix matrix0;
dgMatrix matrix1;
if (m_jointUserCallback)
{
m_jointUserCallback(*this, params.m_timestep);
}
dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1));
m_angles = angle.Scale(-dgFloat32(1.0f));
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix0.m_up;
const dgVector& dir2 = matrix0.m_right;
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgPointParam pointData;
InitPointParam(pointData, m_stiffness, p0, p1);
CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]);
CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]);
CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]);
ret = 3;
if (m_twistLimit)
{
if (angle.m_x > m_twistAngle)
{
dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
const dgVector& dir = matrix0.m_right;
CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
relVelocErr = velocError % dir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
else if (angle.m_x < -m_twistAngle)
{
dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f)));
CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
relVelocErr = velocError % dir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
}
if (m_coneLimit)
{
dgFloat32 coneCos;
coneCos = matrix0.m_front % matrix1.m_front;
if (coneCos < m_coneAngleCos)
{
dgVector p0(
matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
InitPointParam(pointData, m_stiffness, p0, p0);
dgVector tangentDir(matrix0.m_front * matrix1.m_front);
tangentDir = tangentDir.Scale(
dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f));
CalculatePointDerivative(ret, params, tangentDir, pointData,
&m_jointForce[ret]);
ret++;
dgVector normalDir(tangentDir * matrix0.m_front);
dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0);
//restitution = contact.m_restitution;
relVelocErr = velocError % normalDir;
if (relVelocErr > dgFloat32(1.0e-3f))
{
relVelocErr *= dgFloat32(1.1f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH
* (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle);
_ASSERTE(penetrationErr >= dgFloat32 (0.0f));
CalculatePointDerivative(ret, params, normalDir, pointData,
&m_jointForce[ret]);
params.m_forceBounds[ret].m_low = dgFloat32(0.0f);
params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret];
// params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep;
SetMotorAcceleration(ret,
(relVelocErr + penetrationErr) * params.m_invTimestep, params);
ret++;
}
}
return dgUnsigned32(ret);
}
void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex)
{
dgPointParam pointData;
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 = GetMin (contact.m_penetration, dgFloat32(0.5f));
dgFloat32 penetrationStiffness = dgFloat32 (50.0f) * contact.m_softness;
dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
_ASSERTE (dgAbsf (penetrationVeloc - dgFloat32 (50.0f) * contact.m_softness * GetMin (contact.m_penetration, dgFloat32(0.5f))) < 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 = (dgFloat32*)&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] = (penetrationVeloc + relVelocErr) * params.m_invTimestep;
if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel__) {
params.m_jointAccel[normalIndex] += contact.m_normal_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;
params.m_isMotor[jacobIndex] = 1;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
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 = (dgFloat32*)&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__) {
_ASSERTE (0);
params.m_jointAccel[jacobIndex] = contact.m_dir1_Force;
params.m_isMotor[jacobIndex] = 1;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
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 = (dgFloat32*)&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 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 dgBilateralConstraint::CalculatePointDerivative (
dgInt32 index,
dgContraintDescritor& desc,
const dgVector& dir,
const dgPointParam& param,
dgFloat32* jointForce)
{
dgFloat32 relPosit;
dgFloat32 relVeloc;
dgFloat32 relCentr;
dgFloat32 accelError;
_ASSERTE (jointForce);
_ASSERTE (m_body0);
_ASSERTE (m_body1);
dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobian_IM0;
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_jacobian_IM1;
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);
dgVector velocError (param.m_veloc1 - param.m_veloc0);
dgVector positError (param.m_posit1 - param.m_posit0);
dgVector centrError (param.m_centripetal1 - param.m_centripetal0);
relPosit = positError % dir;
relVeloc = velocError % dir;
relCentr = centrError % dir;
relCentr = ClampValue (relCentr, dgFloat32(-10000.0f), dgFloat32(10000.0f));
//relCentr = 0.0f;
//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;
accelError = num / den;
//_ASSERTE (dgAbsf (accelError - CalculateSpringDamperAcceleration (index, desc, 0.0f, param.m_posit0, param.m_posit1, LINEAR_POS_DAMP, LINEAR_VEL_DAMP)) < 1.0e-2f);
m_rowIsMotor[index] = 0;
desc.m_isMotor[index] = 0;
m_motorAcceleration[index] = dgFloat32 (0.0f);
// dgJacobianPair m_jacobian[DG_CONSTRAINT_MAX_ROWS];
// dgBilateralBounds m_forceBounds[DG_CONSTRAINT_MAX_ROWS];
// dgFloat32 m_jointAccel[DG_CONSTRAINT_MAX_ROWS];
// dgFloat32 m_jointStiffness[DG_CONSTRAINT_MAX_ROWS];
// dgFloat32 m_restitution[DG_CONSTRAINT_MAX_ROWS];
// dgFloat32 m_penetration[DG_CONSTRAINT_MAX_ROWS];
// dgFloat32 m_penetrationStiffness[DG_CONSTRAINT_MAX_ROWS];
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;
}
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 (0.5f);
// 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 = (contact.m_flags & dgContactMaterial::m_override0Friction) ? DG_BILATERAL_FRICTION_CONSTRAINT : normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (0.5f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
if (contact.m_flags & dgContactMaterial::m_override0Accel) {
params.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
}
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_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 = (contact.m_flags & dgContactMaterial::m_override0Friction) ? DG_BILATERAL_FRICTION_CONSTRAINT : normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (0.5f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
if (contact.m_flags & dgContactMaterial::m_override1Accel) {
params.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
}
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;
}
}
