59 point_pairs_(point_pairs),
60 point_radii_(point_radii),
61 contact_points_on_edge_(contact_points_on_edge),
62 contact_points_motion_(contact_points_motion),
63 edge_directions_(edge_directions),
64 edge_normals_(edge_normals),
65 edge_margins_(edge_margins),
66 compliance_terms_(compliance_terms),
67 static_frictions_(static_frictions),
68 dynamic_frictions_(dynamic_frictions),
69 error_scales_(error_scales),
70 active_states_(active_states),
71 point_mix_factors_(point_mix_factors),
72 lambdas_normal_(lambdas_normal)
85 const int constraint_i)
const
87 const int2 &point_pair = point_pairs_[constraint_i];
90 const float3 &edge_pos = contact_points_on_edge_[constraint_i];
91 const float3 &edge_dir = edge_directions_[constraint_i];
92 const float3 &edge_nor = edge_normals_[constraint_i];
93 const float margin = edge_margins_[constraint_i];
94 const float compliance_term = compliance_terms_[constraint_i];
95 const float inv_m0 = params.
inv_mass(geo_i_, point_pair[0]);
96 const float inv_m1 = params.
inv_mass(geo_i_, point_pair[1]);
97 const float radius0 = point_radii_[constraint_i][0];
98 const float radius1 = point_radii_[constraint_i][1];
101 bool &is_active = active_states_[constraint_i];
102 float &point_mix_factor = point_mix_factors_[constraint_i];
104 if (inv_m0 <= 0.0f && inv_m1 <= 0.0f) {
111 pos0, pos1, edge_pos, edge_dir,
true);
112 const float segment_factor =
closest.segment_lambda;
113 point_mix_factor = segment_factor;
116 const float weight0 = (1.0f - segment_factor) * inv_m0;
117 const float weight1 = segment_factor * inv_m1;
118 if (weight0 <= 0.0f && weight1 <= 0.0f) {
125 const float3 closest_on_ray = edge_pos +
closest.ray_lambda * edge_dir;
132 float3 seg_nor = closest_on_ray - closest_on_segment;
134 pos0, pos1, edge_pos, edge_dir, edge_nor);
135 if (intersection &&
math::dot(intersection->position - edge_pos, edge_nor) < 0.0f) {
138 seg_nor = 2.0f * edge_dir *
math::dot(edge_dir, seg_nor) - seg_nor;
145 (closest_on_ray + margin * edge_nor);
147 if (residual >= 0.0f) {
151 const float3 gradient = -seg_nor;
156 float &lambda_normal = lambdas_normal_[constraint_i];
157 const float error_squared =
math::square(residual + compliance_term * lambda_normal);
158 const float delta_lambda_normal = -residual / (weight0 + weight1 + compliance_term);
159 offset0 += delta_lambda_normal * weight0 * gradient;
160 offset1 += delta_lambda_normal * weight1 * gradient;
161 lambda_normal += delta_lambda_normal;
166 const float3 &collider_velocity = contact_points_motion_[constraint_i];
169 const float3 velocity_tangent = velocity -
math::dot(velocity, gradient) * gradient;
171 (weight0 + weight1 + compliance_term));
172 const bool is_static = lambda_tangent_sq <
173 math::square(static_frictions_[constraint_i] * lambda_normal);
175 offset0 -= velocity_tangent * weight0 / (weight0 + weight1 + compliance_term);
176 offset1 -= velocity_tangent * weight1 / (weight0 + weight1 + compliance_term);
180 updater.update_position(geo_i_, point_pair[0], offset0);
181 updater.update_position(geo_i_, point_pair[1], offset1);
182 updater.add_residual_error(geo_i_, error_squared * error_scales_[constraint_i]);