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 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;
}
