25 const float compliance_term,
28 const float inv_lumped_inertia0 =
math::safe_rcp(0.5f * (inertia0.x + inertia0.y + inertia0.z));
29 const float inv_lumped_inertia1 =
math::safe_rcp(0.5f * (inertia1.x + inertia1.y + inertia1.z));
30 if (inv_lumped_inertia0 == 0.0f && inv_lumped_inertia1 == 0.0f) {
44 const float4 residual_neg = rot_diff_f - rest_rot_f;
45 const float4 residual_pos = rot_diff_f + rest_rot_f;
52 const float4 delta_lambda = (-residual - compliance_term * lambda_prev) /
53 (inv_lumped_inertia0 + inv_lumped_inertia1 + compliance_term);
59 return {delta_lambda, offset0, offset1, error_squared};