rapier3d/dynamics/soft_body/
soft_body_plasticity.rs

1//! Soft-body plasticity: edge flow modes, the rest-length and rest-angle flow rules, the fit of
2//! the rest positions to the flowed rest shapes, and the reset. The cells' flow rule runs on the
3//! solver's strain rows (`soft_constraints_set::plastic_flow`).
4use crate::alloc_prelude::*;
5#[cfg(feature = "dim3")]
6use crate::dynamics::SoftBodyDihedral;
7use crate::dynamics::{SoftBody, SoftBodyCellModel, SoftBodyEdge, SoftBodyMaterial};
8use crate::math::{DIM, Matrix, Real, Vector};
9#[cfg(not(feature = "std"))]
10#[allow(unused_imports)]
11use simba::scalar::{ComplexField as _, RealField as _};
12
13/// Which strains make a soft-body edge flow plastically (see
14/// [`SoftBodyMaterial::edge_plastic_yield`]).
15#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
16#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
17pub enum SoftEdgePlasticFlow {
18    /// Both a squeeze and a stretch past the yield take a permanent set.
19    #[default]
20    Both,
21    /// Only a squeeze sets; a stretched edge always springs back (clay dents, but does not
22    /// stay stretched).
23    Compression,
24    /// Only a stretch sets; a squeezed edge always springs back.
25    Tension,
26}
27
28/// Jacobi sweeps of the rest-position fit per step.
29const REST_FIT_SWEEPS: usize = 8;
30/// The fit has settled when no particle moved by more than this fraction of the rest size in a
31/// sweep.
32const REST_FIT_TOLERANCE: Real = 1.0e-5;
33
34impl SoftBodyEdge {
35    /// The permanent set of this edge: how far its rest length has flowed from the length it was
36    /// created with, as a fraction of that length (negative: a squeeze that stayed, positive: a
37    /// stretch that stayed; see [`SoftBodyMaterial::edge_plastic_yield`]).
38    pub fn plastic_strain(&self) -> Real {
39        self.plastic_strain
40    }
41
42    /// The rest length this edge was created with (its `rest_length` before any plastic flow).
43    /// The tear strain is measured against it, so plastic flow counts toward a tear.
44    pub fn initial_rest_length(&self) -> Real {
45        self.rest_length / (1.0 + self.plastic_strain)
46    }
47
48    /// Flows the rest length toward `length` when the strain exceeds the material's edge yield
49    /// ([`SoftBodyMaterial::edge_plastic_yield`]; `dt`: the step length). Returns whether the edge
50    /// flowed significantly (such a body is kept awake).
51    pub(crate) fn plastic_flow(
52        &mut self,
53        length: Real,
54        material: &SoftBodyMaterial,
55        dt: Real,
56    ) -> bool {
57        let yield_strain = material.edge_plastic_yield;
58        if yield_strain <= 0.0 || material.edge_plastic_creep <= 0.0 || self.rest_length <= 0.0 {
59            return false;
60        }
61        let strain = length / self.rest_length - 1.0;
62        let allowed = match material.edge_plastic_flow {
63            SoftEdgePlasticFlow::Both => true,
64            SoftEdgePlasticFlow::Compression => strain < 0.0,
65            SoftEdgePlasticFlow::Tension => strain > 0.0,
66        };
67        if !allowed || strain.abs() <= yield_strain {
68            return false;
69        }
70        // The rest length that would leave exactly the yield strain: only the excess flows.
71        let signed_yield = if strain > 0.0 {
72            yield_strain
73        } else {
74            -yield_strain
75        };
76        let target = length / (1.0 + signed_yield);
77        let blend = (material.edge_plastic_creep * dt).min(1.0);
78        let initial = self.initial_rest_length();
79        let max = material.edge_plastic_max.max(0.0);
80        let new_rest = (self.rest_length + (target - self.rest_length) * blend).clamp(
81            initial * (1.0 - max).max(Real::EPSILON),
82            initial * (1.0 + max),
83        );
84        let increment = (new_rest - self.rest_length).abs() / initial;
85        // The warm-start impulse followed the old length error; it scales with what is left of it.
86        let error = length - self.rest_length;
87        if error != 0.0 {
88            self.impulse *= ((length - new_rest) / error).clamp(0.0, 1.0);
89        }
90        self.rest_length = new_rest;
91        self.plastic_strain = new_rest / initial - 1.0;
92        increment > 1.0e-4
93    }
94}
95
96#[cfg(feature = "dim3")]
97impl SoftBodyDihedral {
98    /// Flows the rest angle toward the current `angle` when the fold exceeds the material's
99    /// edge yield, read in radians (see [`SoftBodyMaterial::edge_plastic_yield`]; `dt`: the step
100    /// length). Returns whether the dihedral flowed by a significant amount.
101    pub(crate) fn plastic_flow(
102        &mut self,
103        angle: Real,
104        material: &SoftBodyMaterial,
105        dt: Real,
106    ) -> bool {
107        let yield_angle = material.edge_plastic_yield;
108        if yield_angle <= 0.0 || material.edge_plastic_creep <= 0.0 {
109            return false;
110        }
111        let deviation = angle - self.rest_angle;
112        if deviation.abs() <= yield_angle {
113            return false;
114        }
115        // The rest angle that would leave exactly the yield: only the excess flows.
116        let target = angle - yield_angle.copysign(deviation);
117        let blend = (material.edge_plastic_creep * dt).min(1.0);
118        let initial = self.initial_rest_angle();
119        let max = material.edge_plastic_max.max(0.0);
120        let new_rest = (self.rest_angle + (target - self.rest_angle) * blend)
121            .clamp(initial - max, initial + max);
122        let increment = (new_rest - self.rest_angle).abs();
123        // The warm-start impulse followed the old angle error; it scales with what is left of it.
124        self.impulse *= ((angle - new_rest) / deviation).clamp(0.0, 1.0);
125        self.rest_angle = new_rest;
126        self.plastic_set = new_rest - initial;
127        increment > 1.0e-4
128    }
129}
130
131impl SoftBody {
132    /// Moves the particles' rest positions toward the flowed rest shapes of the elements that can
133    /// flow (cells with a cell yield, edges with an edge yield): Jacobi sweeps to a least-squares
134    /// fit. Returns whether the fit settled; until then it continues at the next step.
135    pub(crate) fn fit_rest_positions(&mut self) -> bool {
136        let cells_flow = self.material.plastic_yield > 0.0
137            && self.cell_model != SoftBodyCellModel::Volume
138            && !self.cells.is_empty();
139        let edges_flow = self.material.edge_plastic_yield > 0.0 && !self.edges.is_empty();
140        if !cells_flow && !edges_flow {
141            return true;
142        }
143        let n = self.particles.len();
144        let size = self
145            .particles
146            .iter()
147            .map(|p| p.rest_position.length())
148            .fold(0.0, Real::max);
149        let tolerance = size.max(Real::EPSILON) * REST_FIT_TOLERANCE;
150        let mut sum = vec![Vector::ZERO; n];
151        let mut count: Vec<Real> = vec![0.0; n];
152        let mut settled = false;
153        for _ in 0..REST_FIT_SWEEPS {
154            sum.fill(Vector::ZERO);
155            count.fill(0.0);
156            if cells_flow {
157                for c in &self.cells {
158                    if c.rest_volume == 0.0 {
159                        continue;
160                    }
161                    let rest0: [Vector; DIM + 1] = core::array::from_fn(|k| {
162                        self.particles[c.vertices[k] as usize].initial_rest_position
163                    });
164                    let dm = c.plastic_stretch * Self::cell_edge_matrix(rest0);
165                    let mut offsets = [Vector::ZERO; DIM + 1];
166                    let mut centroid = Vector::ZERO;
167                    for k in 0..DIM {
168                        offsets[k + 1] = dm.col(k);
169                    }
170                    let mean = offsets.iter().sum::<Vector>() / (DIM + 1) as Real;
171                    for &v in &c.vertices {
172                        centroid += self.particles[v as usize].rest_position;
173                    }
174                    centroid /= (DIM + 1) as Real;
175                    for (k, &v) in c.vertices.iter().enumerate() {
176                        sum[v as usize] += centroid + offsets[k] - mean;
177                        count[v as usize] += 1.0;
178                    }
179                }
180            }
181            if edges_flow {
182                for e in &self.edges {
183                    if e.rest_length <= 0.0 {
184                        continue;
185                    }
186                    let [a, b] = e.vertices;
187                    let (ra, rb) = (
188                        self.particles[a as usize].rest_position,
189                        self.particles[b as usize].rest_position,
190                    );
191                    let d = rb - ra;
192                    let len = d.length();
193                    if len <= Real::EPSILON {
194                        continue;
195                    }
196                    let half = d * (0.5 * e.rest_length / len);
197                    let mid = (ra + rb) * 0.5;
198                    sum[a as usize] += mid - half;
199                    count[a as usize] += 1.0;
200                    sum[b as usize] += mid + half;
201                    count[b as usize] += 1.0;
202                }
203            }
204            let mut max_move: Real = 0.0;
205            for (i, p) in self.particles.iter_mut().enumerate() {
206                if count[i] > 0.0 {
207                    let target = sum[i] / count[i];
208                    max_move = max_move.max((target - p.rest_position).length());
209                    p.rest_position = target;
210                }
211            }
212            if max_move <= tolerance {
213                settled = true;
214                break;
215            }
216        }
217        // The rest positions stay relative to the rest center of mass.
218        let mut com = Vector::ZERO;
219        let mut mass = 0.0;
220        for p in &self.particles {
221            com += p.rest_position * p.mass;
222            mass += p.mass;
223        }
224        if mass > 0.0 {
225            com /= mass;
226            for p in &mut self.particles {
227                p.rest_position -= com;
228            }
229        }
230        settled
231    }
232
233    /// Resets every plastic flow (edge rest lengths, dihedral rest angles, cell rest shapes,
234    /// particle rest positions) to the creation state; the particles stay put and spring back
235    /// elastically from there.
236    pub fn reset_plasticity(&mut self) {
237        for p in &mut self.particles {
238            p.rest_position = p.initial_rest_position;
239        }
240        for e in &mut self.edges {
241            e.rest_length = e.initial_rest_length();
242            e.plastic_strain = 0.0;
243        }
244        #[cfg(feature = "dim3")]
245        for d in &mut self.dihedrals {
246            d.rest_angle = d.initial_rest_angle();
247            d.plastic_set = 0.0;
248        }
249        for c in &mut self.cells {
250            let rest0: [Vector; DIM + 1] = core::array::from_fn(|k| {
251                self.particles[c.vertices[k] as usize].initial_rest_position
252            });
253            let volume = Self::cell_volume(rest0);
254            c.plastic_stretch = Matrix::IDENTITY;
255            c.rest_volume = volume;
256            c.inv_rest_matrix = if volume.abs() > Real::EPSILON {
257                Self::cell_edge_matrix(rest0).inverse()
258            } else {
259                Matrix::ZERO
260            };
261        }
262        self.rest_fit_pending = false;
263        self.modified = true;
264    }
265}
rapier3d/dynamics/soft_body/soft_body_plasticity.rs

rapier3d/dynamics/soft_body/
soft_body_plasticity.rs
