Blender V5.3
GEO_xpbd_constraint_collision_face.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
9
10namespace blender::xpbd {
11
12/* Constraint implementation for static and dynamic friction is based on
13 * "Detailed Rigid Body Simulation with Extended Position Based Dynamics",
14 * Mueller, Macklin, et al., 2020 */
15class CollisionFaceConstraintSet : public TemplatedConstraintSet<CollisionFaceConstraintSet> {
16 private:
17 int geo_i_;
18 Span<int> points_;
19 Span<float> point_radii_;
20 Span<float3> contact_points_on_face_;
21 Span<float3> contact_points_motion_;
22 Span<float3> face_normals_;
23 Span<float> face_margins_;
24 Span<float> compliance_terms_;
25 Span<float> static_frictions_;
26 Span<float> dynamic_frictions_;
27 /* Scale factor for residual error. */
28 Span<float> error_scales_;
29 MutableSpan<bool> active_states_;
30 MutableSpan<float> lambdas_normal_;
31
32 public:
33 static constexpr StringRefNull debug_name = "Collision Plane";
34
36 const Span<int> points,
37 const Span<float> point_radii,
38 const Span<float3> contact_points_on_face,
39 const Span<float3> contact_points_motion,
40 const Span<float3> face_normals,
41 const Span<float> face_margins,
42 const Span<float> compliance_terms,
43 const Span<float> static_frictions,
44 const Span<float> dynamic_frictions,
45 const Span<float> error_scales,
46 MutableSpan<bool> active_states,
47 MutableSpan<float> lambdas_normal)
48 : TemplatedConstraintSet<CollisionFaceConstraintSet>(points.size(), {geo_i}),
49 geo_i_(geo_i),
50 points_(points),
51 point_radii_(point_radii),
52 contact_points_on_face_(contact_points_on_face),
53 contact_points_motion_(contact_points_motion),
54 face_normals_(face_normals),
55 face_margins_(face_margins),
56 compliance_terms_(compliance_terms),
57 static_frictions_(static_frictions),
58 dynamic_frictions_(dynamic_frictions),
59 error_scales_(error_scales),
60 active_states_(active_states),
61 lambdas_normal_(lambdas_normal)
62 {
63 }
64
65 void reset_force(const int constraint_i) const
66 {
67 active_states_[constraint_i] = false;
68 lambdas_normal_[constraint_i] = 0.0f;
69 }
70
71 template<typename UpdaterT>
73 UpdaterT &updater,
74 const int constraint_i) const
75 {
76 const int point_i = points_[constraint_i];
77 const float radius = point_radii_[constraint_i];
78 const float3 &pos = params.position(geo_i_, point_i);
79 const float3 &face_pos = contact_points_on_face_[constraint_i];
80 const float3 &face_nor = face_normals_[constraint_i];
81 const float margin = face_margins_[constraint_i];
82 const float compliance_term = compliance_terms_[constraint_i];
83 const float inv_m = params.inv_mass(geo_i_, point_i);
84 bool &is_active = active_states_[constraint_i];
85
86 if (inv_m <= 0.0f) {
87 /* Points with infinite mass are pinned and don't collide dynamically. */
88 is_active = false;
89 return;
90 }
91
92 const float3 diff = pos - face_pos;
93 const float normal_distance = math::dot(diff, face_nor) - margin - radius;
94 is_active = normal_distance < 0.0f;
95 if (!is_active) {
96 return;
97 }
98
99 /* Positional correction for penetration. */
100 float3 offset = float3(0.0f);
101 float &lambda_normal = lambdas_normal_[constraint_i];
102 const float error_squared = math::square(normal_distance + compliance_term * lambda_normal);
103 const float delta_lambda_normal = -normal_distance / (inv_m + compliance_term);
104 offset += delta_lambda_normal * inv_m * face_nor;
105 lambda_normal += delta_lambda_normal;
106
107 /* Apply static friction as a direct positional update. */
108 const float3 &prev_pos = params.prev_position(geo_i_, point_i);
109 const float3 &collider_velocity = contact_points_motion_[constraint_i];
110 const float3 velocity = (pos - prev_pos) - collider_velocity;
111 const float3 velocity_tangent = velocity - math::dot(velocity, face_nor) * face_nor;
112 const float lambda_tangent_sq = math::length_squared(velocity_tangent /
113 (inv_m + compliance_term));
114 const bool is_static = lambda_tangent_sq <
115 math::square(static_frictions_[constraint_i] * lambda_normal);
116 if (is_static) {
117 offset -= velocity_tangent * inv_m / (inv_m + compliance_term);
118 }
119
120 updater.update_position(geo_i_, point_i, offset);
121 updater.add_residual_error(geo_i_, error_squared * error_scales_[constraint_i]);
122 }
123
125 {
126 return color_constraints__unary(points_, memory);
127 }
128};
129
130} // namespace blender::xpbd
void solve_single(const ConstraintSetParams &params, UpdaterT &updater, const int constraint_i) const
ConstraintColoring color_constraints(LinearAllocator<> &memory) const override
CollisionFaceConstraintSet(const int geo_i, const Span< int > points, const Span< float > point_radii, const Span< float3 > contact_points_on_face, const Span< float3 > contact_points_motion, const Span< float3 > face_normals, const Span< float > face_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 > lambdas_normal)
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)
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt)
Definition frames.inl:1166
T length_squared(const VecBase< T, Size > &a)
T dot(const QuaternionBase< T > &a, const QuaternionBase< T > &b)
T square(const T &a)
ConstraintColoring color_constraints__unary(const Span< int > affected_points, LinearAllocator<> &memory)
VecBase< float, 3 > float3