30 const float rest_length,
31 const float compliance_term,
32 const float3 &lambda_pos_prev,
33 const float3 &lambda_rot_prev)
37 const float inv_lumped_inertia =
math::safe_rcp(0.5f * (inertia.x + inertia.y + inertia.z));
39 if (inv_m0 == 0.0f && inv_m1 == 0.0f && inv_lumped_inertia == 0.0f) {
51 const float3 p_diff = p1 - p0;
57 const float3 residual_pos = p_diff - forward_rest;
58 const float3 residual_rot = p_diff - forward;
61 compliance_term * lambda_pos_prev);
64 const float weight_sum = inv_m0 + inv_m1 + 4.0f * inv_lumped_inertia *
pow2f(rest_length);
65 const float weight_sum_rot = inv_m0 + inv_m1 + 4.0f * inv_lumped_inertia *
pow2f(p_len);
66 const float3 delta_lambda_pos = (-residual_pos - compliance_term * lambda_pos_prev) /
67 (weight_sum + compliance_term);
68 const float3 delta_lambda_rot = (-residual_rot - compliance_term * lambda_rot_prev) /
69 (weight_sum_rot + compliance_term);
72 result.delta_lambda_pos = delta_lambda_pos;
73 result.delta_lambda_rot = delta_lambda_rot;
74 result.offset0 = -delta_lambda_pos * inv_m0;
75 result.offset1 = delta_lambda_pos * inv_m1;
78 result.residual_error_squared = error_squared;
113 const float error_scale,
117 curves_range_(curves_range),
118 points_by_curve_(points_by_curve),
119 rest_lengths_(rest_lengths),
120 lambdas_pos_(lambdas_pos),
121 lambdas_rot_(lambdas_rot),
122 compliances_(compliances),
123 error_scale_(error_scale)
129 const int curve_i = curves_range_[constraint_i];
130 const IndexRange points = points_by_curve_[curve_i];
135 template<
typename UpdaterT>
138 const int constraint_i)
const
140 const int curve_i = curves_range_[constraint_i];
141 const IndexRange points = points_by_curve_[curve_i];
143 const int first_point_i_in_constraint_set = points_by_curve_[curves_range_.first()].
first();
146 for (
const int point_i0 : points.
drop_back(1)) {
147 const int point_i1 = point_i0 + 1;
148 const float compliance = compliances_[point_i0 - first_point_i_in_constraint_set];
156 rest_lengths_[point_i1],
158 lambdas_pos_[point_i1],
159 lambdas_rot_[point_i1]);
160 lambdas_pos_[point_i1] +=
result.delta_lambda_pos;
161 lambdas_rot_[point_i1] +=
result.delta_lambda_rot;
162 updater.update_position(geo_i, point_i0,
result.offset0);
163 updater.update_position(geo_i, point_i1,
result.offset1);
164 updater.update_rotation(geo_i, point_i0,
result.offset_rot);
165 updater.add_residual_error(geo_i,
result.residual_error_squared * error_scale_);
constexpr IndexRange drop_back(int64_t n) const
constexpr IndexRange slice(int64_t start, int64_t size) const
const math::Quaternion & rotation(int geo_i, int point_i) const
const float3 & position(int geo_i, int point_i) const
float3 moment_of_inertia(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_
void solve_single(const ConstraintSetParams ¶ms, UpdaterT &updater, const int constraint_i) const
ConstraintColoring color_constraints(LinearAllocator<> &) const override
static constexpr StringRefNull debug_name
void reset_force(const int constraint_i) const
RodStretchAndShearConstraintSet(const int geo_i, const IndexRange curves_range, const OffsetIndices< int > points_by_curve, const Span< float > rest_lengths, const Span< float > compliances, const float error_scale, MutableSpan< float3 > lambdas_pos, MutableSpan< float3 > lambdas_rot)
TemplatedConstraintSet(const int constraints_num, Vector< int > affected_geo_indices)
QuaternionBase< float > Quaternion
T length_squared(const VecBase< T, Size > &a)
T length(const VecBase< T, Size > &a)
VecBase< T, 3 > transform_point(const CartesianBasis &basis, const VecBase< T, 3 > &v)
RodStretchAndShearConstraintResult evaluate_rod_stretch_and_shear_constraint(const float3 &p0, const float3 &p1, const math::Quaternion &rot, const float inv_m0, const float inv_m1, const float3 &inertia, const float rest_length, const float compliance_term, const float3 &lambda_pos_prev, const float3 &lambda_rot_prev)
ConstraintColoring color_constraints__all_independent(const int constraints_num)
MINLINE float pow2f(float x)
VecBase< float, 3 > float3
float residual_error_squared
math::Quaternion offset_rot