Blender V5.3
GEO_xpbd_constraint_collision_edge.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
10
11namespace blender::xpbd {
12
13/* Constraint implementation for static and dynamic friction is based on
14 * "Detailed Rigid Body Simulation with Extended Position Based Dynamics",
15 * Mueller, Macklin, et al., 2020 */
16class CollisionEdgeConstraintSet : public TemplatedConstraintSet<CollisionEdgeConstraintSet> {
17 private:
18 int geo_i_;
19 Span<int2> point_pairs_;
20 Span<float2> point_radii_;
21 Span<float3> contact_points_on_edge_;
22 Span<float3> contact_points_motion_;
23 /* Direction of the collider edges. */
24 Span<float3> edge_directions_;
25 /* Normal vectors in the plane of the adjacent face.
26 * The edge normal is cross(edge_direction, face_normal). */
27 Span<float3> edge_normals_;
28 /* Margin of the collider surface. */
29 Span<float> edge_margins_;
30 Span<float> compliance_terms_;
31 Span<float> static_frictions_;
32 Span<float> dynamic_frictions_;
33 /* Scale factor for residual error. */
34 Span<float> error_scales_;
35 MutableSpan<bool> active_states_;
36 MutableSpan<float> point_mix_factors_;
37 MutableSpan<float> lambdas_normal_;
38
39 public:
40 static constexpr StringRefNull debug_name = "Collision Plane";
41
43 const Span<int2> point_pairs,
44 const Span<float2> point_radii,
45 const Span<float3> contact_points_on_edge,
46 const Span<float3> contact_points_motion,
47 const Span<float3> edge_directions,
48 const Span<float3> edge_normals,
49 const Span<float> edge_margins,
50 const Span<float> compliance_terms,
51 const Span<float> static_frictions,
52 const Span<float> dynamic_frictions,
53 const Span<float> error_scales,
54 MutableSpan<bool> active_states,
55 MutableSpan<float> point_mix_factors,
56 MutableSpan<float> lambdas_normal)
57 : TemplatedConstraintSet<CollisionEdgeConstraintSet>(point_pairs.size(), {geo_i}),
58 geo_i_(geo_i),
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)
73 {
74 }
75
76 void reset_force(const int constraint_i) const
77 {
78 active_states_[constraint_i] = false;
79 lambdas_normal_[constraint_i] = 0.0f;
80 }
81
82 template<typename UpdaterT>
84 UpdaterT &updater,
85 const int constraint_i) const
86 {
87 const int2 &point_pair = point_pairs_[constraint_i];
88 const float3 &pos0 = params.position(geo_i_, point_pair[0]);
89 const float3 &pos1 = params.position(geo_i_, point_pair[1]);
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];
99 BLI_assert(math::is_unit(edge_dir));
100 BLI_assert(math::is_unit(edge_nor));
101 bool &is_active = active_states_[constraint_i];
102 float &point_mix_factor = point_mix_factors_[constraint_i];
103
104 if (inv_m0 <= 0.0f && inv_m1 <= 0.0f) {
105 /* Points with infinite mass are pinned and don't collide dynamically. */
106 is_active = false;
107 return;
108 }
109
111 pos0, pos1, edge_pos, edge_dir, true);
112 const float segment_factor = closest.segment_lambda;
113 point_mix_factor = segment_factor;
114
115 /* Effective weight/mass */
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) {
119 /* Could happen if the segment_factor is exactly 0 or 1. */
120 is_active = false;
121 return;
122 }
123
124 const float3 closest_on_segment = math::interpolate(pos0, pos1, segment_factor);
125 const float3 closest_on_ray = edge_pos + closest.ray_lambda * edge_dir;
126 /* Curve surface is ambiguous: The closest point on the center line isn't necessarily the
127 * contact point of the implicit curve surface as defined by the curve radius. A prospective
128 * surface point is the intersection of connecting line between the closest points on the
129 * segment and the edge.
130 * If the curve is "inside" the collider (intersects the half-plane under the edge) then use
131 * the curve surface point on the opposite side is used as the contact. */
132 float3 seg_nor = closest_on_ray - closest_on_segment;
133 const std::optional<PlaneIntersection> intersection = intersect_plane(
134 pos0, pos1, edge_pos, edge_dir, edge_nor);
135 if (intersection && math::dot(intersection->position - edge_pos, edge_nor) < 0.0f) {
136 /* Reflect on segment direction. This produces the correct normal also in case the closest
137 * segment point is clamped and the normal isn't perpendicular to the edge direction. */
138 seg_nor = 2.0f * edge_dir * math::dot(edge_dir, seg_nor) - seg_nor;
139 }
140 float seg_dist;
141 seg_nor = math::normalize_and_get_length(seg_nor, seg_dist);
142
143 const float radius = math::interpolate(radius0, radius1, segment_factor);
144 const float3 distance = (closest_on_segment + radius * seg_nor) -
145 (closest_on_ray + margin * edge_nor);
146 const float residual = -math::dot(distance, seg_nor);
147 if (residual >= 0.0f) {
148 is_active = false;
149 return;
150 }
151 const float3 gradient = -seg_nor;
152
153 /* Positional correction for penetration. */
154 float3 offset0 = float3(0.0f);
155 float3 offset1 = float3(0.0f);
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;
162
163 /* Apply static friction as a direct positional update. */
164 const float3 &prev_pos0 = params.prev_position(geo_i_, point_pair[0]);
165 const float3 &prev_pos1 = params.prev_position(geo_i_, point_pair[1]);
166 const float3 &collider_velocity = contact_points_motion_[constraint_i];
167 const float3 velocity = math::interpolate(pos0 - prev_pos0, pos1 - prev_pos1, segment_factor) -
168 collider_velocity;
169 const float3 velocity_tangent = velocity - math::dot(velocity, gradient) * gradient;
170 const float lambda_tangent_sq = math::length_squared(velocity_tangent /
171 (weight0 + weight1 + compliance_term));
172 const bool is_static = lambda_tangent_sq <
173 math::square(static_frictions_[constraint_i] * lambda_normal);
174 if (is_static) {
175 offset0 -= velocity_tangent * weight0 / (weight0 + weight1 + compliance_term);
176 offset1 -= velocity_tangent * weight1 / (weight0 + weight1 + compliance_term);
177 }
178
179 is_active = true;
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]);
183 }
184
186 {
187 return color_constraints__binary(point_pairs_, memory);
188 }
189};
190
191} // namespace blender::xpbd
bool closest(btVector3 &v)
void solve_single(const ConstraintSetParams &params, UpdaterT &updater, const int constraint_i) const
CollisionEdgeConstraintSet(const int geo_i, const Span< int2 > point_pairs, const Span< float2 > point_radii, const Span< float3 > contact_points_on_edge, const Span< float3 > contact_points_motion, const Span< float3 > edge_directions, const Span< float3 > edge_normals, const Span< float > edge_margins, const Span< float > compliance_terms, const Span< float > static_frictions, const Span< float > dynamic_frictions, const Span< float > error_scales, MutableSpan< bool > active_states, MutableSpan< float > point_mix_factors, MutableSpan< float > lambdas_normal)
ConstraintColoring color_constraints(LinearAllocator<> &memory) const override
const float3 & position(int geo_i, int point_i) const
const float3 & prev_position(int geo_i, int point_i) const
float inv_mass(int geo_i, int point_i) const
TemplatedConstraintSet(const int constraints_num, Vector< int > affected_geo_indices)
float distance(float, float) RET
T length_squared(const VecBase< T, Size > &a)
QuaternionBase< T > normalize_and_get_length(const QuaternionBase< T > &q, T &out_length)
T dot(const QuaternionBase< T > &a, const QuaternionBase< T > &b)
T interpolate(const T &a, const T &b, const FactorT &t)
T square(const T &a)
bool is_unit(const VecBase< T, Size > &a, const T epsilon=T(10) *std::numeric_limits< T >::epsilon())
ConstraintColoring color_constraints__binary(const Span< int2 > affected_points, LinearAllocator<> &memory)
std::optional< PlaneIntersection > intersect_plane(const float3 &pos0, const float3 &pos1, const float3 &origin, const float3 &edge, const float3 &normal)
SegmentClosestToRay closest_on_segment_to_ray(const float3 &pos0, const float3 &pos1, const float3 &ray_pos, const float3 &ray_dir, const bool clamp)
BLI_assert(slot->slot_subtype.map==BMO_OP_SLOT_SUBTYPE_MAP_FLT)
VecBase< int32_t, 2 > int2
VecBase< float, 3 > float3