Blender V5.3
GEO_xpbd_constraint_rod_stretch_shear.hh
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2026 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
5#pragma once
6
7#include "BLI_math_base_c.hh"
8
11
12namespace blender::xpbd {
13
22
24 const float3 &p0,
25 const float3 &p1,
26 const math::Quaternion &rot,
27 const float inv_m0,
28 const float inv_m1,
29 const float3 &inertia,
30 const float rest_length,
31 const float compliance_term,
32 const float3 &lambda_pos_prev,
33 const float3 &lambda_rot_prev)
34{
35 /* Lumped weight for the rotation influence. The higher the inertia, the lower the change of
36 * the rotation should be compared to the change in point positions. */
37 const float inv_lumped_inertia = math::safe_rcp(0.5f * (inertia.x + inertia.y + inertia.z));
38
39 if (inv_m0 == 0.0f && inv_m1 == 0.0f && inv_lumped_inertia == 0.0f) {
40 /* Everything is pinned, so the constraint can't do anything. */
41 return {};
42 }
43
44 /* TODO The positional and rotational parts use different residuals to avoid errors when the
45 * current segment length deviates too much from the rest length. The rotational offset uses
46 * the residual as the angle of rotation which becomes larger with stretching. To avoid
47 * instabilities the rotation residual is computed relative to the current length.
48 * This should be cleaned up and optimized if possible. */
49
50 /* Current non-normalized tangent of the rod. */
51 const float3 p_diff = p1 - p0;
52 const float p_len = math::length(p_diff);
53 /* Expected non-normalized tangent of the rod based on the rotation. */
54 const float3 forward_rest = math::transform_point(rot, float3(0.0f, 0.0f, rest_length));
55 const float3 forward = math::transform_point(rot, float3(0.0f, 0.0f, p_len));
56 /* How much the rod is stretched and sheared. */
57 const float3 residual_pos = p_diff - forward_rest;
58 const float3 residual_rot = p_diff - forward;
59
60 const float error_squared = math::length_squared(residual_pos +
61 compliance_term * lambda_pos_prev);
62
63 /* Based on "Position and Orientation Based Cosserat Rods" (Kugelstadt, Schömer, 2016). */
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);
70
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;
76 result.offset_rot = math::Quaternion(0.0f, -delta_lambda_rot * inv_lumped_inertia * p_len) *
77 rot * math::Quaternion(0, 0, 0, -1);
78 result.residual_error_squared = error_squared;
79 return result;
80}
81
88 : public TemplatedConstraintSet<RodStretchAndShearConstraintSet> {
89 private:
91 IndexRange curves_range_;
92 OffsetIndices<int> points_by_curve_;
93
95 Span<float> rest_lengths_;
96 MutableSpan<float3> lambdas_pos_;
97 MutableSpan<float3> lambdas_rot_;
98
100 Span<float> compliances_;
101
102 /* Scale factor for residual error. */
103 float error_scale_;
104
105 public:
106 static constexpr StringRefNull debug_name = "Rod Stretch and Shear";
107
109 const IndexRange curves_range,
110 const OffsetIndices<int> points_by_curve,
111 const Span<float> rest_lengths,
112 const Span<float> compliances,
113 const float error_scale,
114 MutableSpan<float3> lambdas_pos,
115 MutableSpan<float3> lambdas_rot)
116 : TemplatedConstraintSet<RodStretchAndShearConstraintSet>(curves_range.size(), {geo_i}),
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)
124 {
125 }
126
127 void reset_force(const int constraint_i) const
128 {
129 const int curve_i = curves_range_[constraint_i];
130 const IndexRange points = points_by_curve_[curve_i];
131 lambdas_pos_.slice(points).fill(float3(0.0f));
132 lambdas_rot_.slice(points).fill(float3(0.0f));
133 }
134
135 template<typename UpdaterT>
137 UpdaterT &updater,
138 const int constraint_i) const
139 {
140 const int curve_i = curves_range_[constraint_i];
141 const IndexRange points = points_by_curve_[curve_i];
142 const int geo_i = affected_geo_indices_[0];
143 const int first_point_i_in_constraint_set = points_by_curve_[curves_range_.first()].first();
144
145 /* Could try implementing bilateral interleaving ordering for better stability. */
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];
150 params.position(geo_i, point_i0),
151 params.position(geo_i, point_i1),
152 params.rotation(geo_i, point_i0),
153 params.inv_mass(geo_i, point_i0),
154 params.inv_mass(geo_i, point_i1),
155 params.moment_of_inertia(geo_i, point_i0),
156 rest_lengths_[point_i1],
157 compliance * params.compliance_term_factor,
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_);
166 }
167 }
168
173};
174
175} // namespace blender::xpbd
constexpr IndexRange drop_back(int64_t n) const
constexpr IndexRange slice(int64_t start, int64_t size) const
const T & first() 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 inv_mass(int geo_i, int point_i) const
void solve_single(const ConstraintSetParams &params, UpdaterT &updater, const int constraint_i) const
ConstraintColoring color_constraints(LinearAllocator<> &) const override
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)
#define rot(x, k)
QuaternionBase< float > Quaternion
T length_squared(const VecBase< T, Size > &a)
T safe_rcp(const T &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