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