51 point_radii_(point_radii),
52 contact_points_on_face_(contact_points_on_face),
53 contact_points_motion_(contact_points_motion),
54 face_normals_(face_normals),
55 face_margins_(face_margins),
56 compliance_terms_(compliance_terms),
57 static_frictions_(static_frictions),
58 dynamic_frictions_(dynamic_frictions),
59 error_scales_(error_scales),
60 active_states_(active_states),
61 lambdas_normal_(lambdas_normal)
74 const int constraint_i)
const
76 const int point_i = points_[constraint_i];
77 const float radius = point_radii_[constraint_i];
79 const float3 &face_pos = contact_points_on_face_[constraint_i];
80 const float3 &face_nor = face_normals_[constraint_i];
81 const float margin = face_margins_[constraint_i];
82 const float compliance_term = compliance_terms_[constraint_i];
83 const float inv_m = params.
inv_mass(geo_i_, point_i);
84 bool &is_active = active_states_[constraint_i];
93 const float normal_distance =
math::dot(
diff, face_nor) - margin - radius;
94 is_active = normal_distance < 0.0f;
101 float &lambda_normal = lambdas_normal_[constraint_i];
102 const float error_squared =
math::square(normal_distance + compliance_term * lambda_normal);
103 const float delta_lambda_normal = -normal_distance / (inv_m + compliance_term);
104 offset += delta_lambda_normal * inv_m * face_nor;
105 lambda_normal += delta_lambda_normal;
109 const float3 &collider_velocity = contact_points_motion_[constraint_i];
110 const float3 velocity = (pos - prev_pos) - collider_velocity;
111 const float3 velocity_tangent = velocity -
math::dot(velocity, face_nor) * face_nor;
113 (inv_m + compliance_term));
114 const bool is_static = lambda_tangent_sq <
115 math::square(static_frictions_[constraint_i] * lambda_normal);
117 offset -= velocity_tangent * inv_m / (inv_m + compliance_term);
120 updater.update_position(geo_i_, point_i, offset);
121 updater.add_residual_error(geo_i_, error_squared * error_scales_[constraint_i]);