23 const float rest_distance,
24 const float compliance_term,
25 const float lambda_prev)
27 if (inv_m0 == 0.0f && inv_m1 == 0.0f) {
31 const float3 p_diff = p1 - p0;
34 const float length_diff =
length - rest_distance;
35 const float error_squared =
math::square(length_diff + compliance_term * lambda_prev);
36 const float delta_lambda = (-length_diff - compliance_term * lambda_prev) /
37 (inv_m0 + inv_m1 + compliance_term);
39 const float3 offset0 = -delta_lambda * inv_m0 * normalized_dir;
40 const float3 offset1 = delta_lambda * inv_m1 * normalized_dir;
42 return {delta_lambda, offset0, offset1, error_squared};
62 const float error_scale,
65 point_pairs_(point_pairs),
66 distances_(distances),
67 compliances_(compliances),
68 error_scale_(error_scale),
75 lambdas_[constraint_i] = 0.0f;
78 template<
typename UpdaterT>
81 const int constraint_i)
const
84 const int2 &point_pair = point_pairs_[constraint_i];
85 const int point_i0 = point_pair[0];
86 const int point_i1 = point_pair[1];
92 distances_[constraint_i],
94 lambdas_[constraint_i]);
95 lambdas_[constraint_i] +=
result.delta_lambda;
96 updater.update_position(geo_i, point_i0,
result.offset0);
97 updater.update_position(geo_i, point_i1,
result.offset1);
98 updater.add_residual_error(geo_i,
result.residual_error_squared * error_scale_);
const float3 & position(int geo_i, int point_i) const
float compliance_term_factor
float inv_mass(int geo_i, int point_i) const
Vector< int > affected_geo_indices_
static constexpr StringRefNull debug_name
void reset_force(const int constraint_i) const
void solve_single(const ConstraintSetParams ¶ms, UpdaterT &updater, const int constraint_i) const
DistanceConstraintSet(const int geo_i, const Span< int2 > point_pairs, const Span< float > distances, const Span< float > compliances, const float error_scale, MutableSpan< float > lambdas)
ConstraintColoring color_constraints(LinearAllocator<> &memory) const override
TemplatedConstraintSet(const int constraints_num, Vector< int > affected_geo_indices)
float length(VecOp< float, D >) RET
QuaternionBase< T > normalize_and_get_length(const QuaternionBase< T > &q, T &out_length)
ConstraintColoring color_constraints__binary(const Span< int2 > affected_points, LinearAllocator<> &memory)
DistanceConstraintResult evaluate_distance_constraint(const float3 &p0, const float3 &p1, const float inv_m0, const float inv_m1, const float rest_distance, const float compliance_term, const float lambda_prev)
VecBase< int32_t, 2 > int2
VecBase< float, 3 > float3
float residual_error_squared