1use crate::BodyHandle;
2use std::collections::{BTreeMap, BTreeSet};
3
4const ELASTIC_STRAIN: f32 = f32::INFINITY;
5
6#[derive(Clone, Copy, Debug, PartialEq, Eq)]
7pub struct SoftBodyHandle {
8 pub id: u32,
9 pub generation: u32,
10}
11
12#[derive(Clone, Copy, Debug, PartialEq, Eq)]
13pub enum SoftElementKind {
14 Distance = 0,
15 Area = 1,
16 Bend = 2,
17 Volume = 3,
18}
19
20impl SoftElementKind {
21 pub const fn arity(self) -> usize {
22 match self {
23 Self::Distance => 2,
24 Self::Area => 3,
25 Self::Bend | Self::Volume => SoftElement::PARTICLES,
26 }
27 }
28}
29
30#[derive(Clone, Copy, Debug, PartialEq)]
31pub struct SoftElement {
32 kind: SoftElementKind,
33 particles: [u32; SoftElement::PARTICLES],
34 rest: f32,
35 compliance: f32,
36 yield_strain: f32,
37 break_strain: f32,
38 plastic_flow: f32,
39}
40
41impl SoftElement {
42 pub const PARTICLES: usize = 4;
43 pub const UNUSED: u32 = u32::MAX;
44
45 fn build(kind: SoftElementKind, particles: [u32; Self::PARTICLES], rest: f32) -> Self {
46 let participants = &particles[..kind.arity()];
47 for (slot, particle) in participants.iter().enumerate() {
48 assert!(
49 participants[..slot].iter().all(|other| other != particle),
50 "a soft element needs distinct particles"
51 );
52 }
53 Self {
54 kind,
55 particles,
56 rest,
57 compliance: 0.0,
58 yield_strain: ELASTIC_STRAIN,
59 break_strain: ELASTIC_STRAIN,
60 plastic_flow: 0.0,
61 }
62 }
63
64 pub fn distance(first: u32, second: u32, rest: f32) -> Self {
65 assert!(
66 rest > 0.0,
67 "a distance element needs a positive rest length"
68 );
69 let mut particles = [Self::UNUSED; Self::PARTICLES];
70 particles[..2].copy_from_slice(&[first, second]);
71 Self::build(SoftElementKind::Distance, particles, rest)
72 }
73
74 pub fn area(first: u32, second: u32, third: u32, rest: f32) -> Self {
75 assert!(rest > 0.0, "an area element needs a positive rest area");
76 let mut particles = [Self::UNUSED; Self::PARTICLES];
77 particles[..3].copy_from_slice(&[first, second, third]);
78 Self::build(SoftElementKind::Area, particles, rest)
79 }
80
81 pub fn bend(apex_a: u32, apex_b: u32, edge_a: u32, edge_b: u32, rest: f32) -> Self {
82 assert!(
83 (0.0..=std::f32::consts::PI).contains(&rest),
84 "a bend element needs a rest angle within [0, pi]"
85 );
86 Self::build(
87 SoftElementKind::Bend,
88 [apex_a, apex_b, edge_a, edge_b],
89 rest,
90 )
91 }
92
93 pub fn volume(first: u32, second: u32, third: u32, fourth: u32, rest: f32) -> Self {
94 assert!(rest > 0.0, "a volume element needs a positive rest volume");
95 Self::build(
96 SoftElementKind::Volume,
97 [first, second, third, fourth],
98 rest,
99 )
100 }
101
102 pub fn compliance(mut self, compliance: f32) -> Self {
103 assert!(
104 compliance >= 0.0,
105 "a soft element compliance must be non-negative"
106 );
107 self.compliance = compliance;
108 self
109 }
110
111 pub fn yielding(mut self, yield_strain: f32, plastic_flow: f32) -> Self {
112 self.yield_strain = assert_strain(yield_strain, "yield");
113 self.plastic_flow = assert_flow(plastic_flow);
114 self
115 }
116
117 pub fn fracturing(mut self, break_strain: f32) -> Self {
118 self.break_strain = assert_strain(break_strain, "break");
119 self
120 }
121
122 pub const fn kind(&self) -> SoftElementKind {
123 self.kind
124 }
125
126 pub const fn rest(&self) -> f32 {
127 self.rest
128 }
129
130 pub const fn compliance_of(&self) -> f32 {
131 self.compliance
132 }
133
134 pub const fn yield_strain_of(&self) -> f32 {
135 self.yield_strain
136 }
137
138 pub const fn break_strain_of(&self) -> f32 {
139 self.break_strain
140 }
141
142 pub const fn plastic_flow_of(&self) -> f32 {
143 self.plastic_flow
144 }
145
146 pub fn carries_strength(&self) -> bool {
147 self.yield_strain.is_finite() || self.break_strain.is_finite()
148 }
149
150 pub const fn particles(&self) -> [u32; Self::PARTICLES] {
151 self.particles
152 }
153
154 pub fn participants(&self) -> impl Iterator<Item = u32> + '_ {
155 self.particles[..self.kind.arity()].iter().copied()
156 }
157}
158
159#[derive(Clone, Copy, Debug, PartialEq)]
160pub struct SoftMaterial {
161 pub stretch: f32,
162 pub shear: f32,
163 pub bend: f32,
164 pub volume: f32,
165 pub yield_strain: f32,
166 pub break_strain: f32,
167 pub plastic_flow: f32,
168}
169
170impl SoftMaterial {
171 pub const fn rigid() -> Self {
172 Self {
173 stretch: 0.0,
174 shear: 0.0,
175 bend: 0.0,
176 volume: 0.0,
177 yield_strain: ELASTIC_STRAIN,
178 break_strain: ELASTIC_STRAIN,
179 plastic_flow: 0.0,
180 }
181 }
182
183 pub fn new(stretch: f32, shear: f32, bend: f32, volume: f32) -> Self {
184 let material = Self {
185 stretch,
186 shear,
187 bend,
188 volume,
189 ..Self::rigid()
190 };
191 assert!(
192 material.stretch >= 0.0
193 && material.shear >= 0.0
194 && material.bend >= 0.0
195 && material.volume >= 0.0,
196 "soft material compliances must be non-negative"
197 );
198 material
199 }
200
201 pub fn yielding(mut self, yield_strain: f32, plastic_flow: f32) -> Self {
202 self.yield_strain = assert_strain(yield_strain, "yield");
203 self.plastic_flow = assert_flow(plastic_flow);
204 self
205 }
206
207 pub fn fracturing(mut self, break_strain: f32) -> Self {
208 self.break_strain = assert_strain(break_strain, "break");
209 self
210 }
211}
212
213fn assert_strain(strain: f32, role: &str) -> f32 {
214 assert!(strain >= 0.0, "a soft {role} strain must be non-negative");
215 strain
216}
217
218fn assert_flow(plastic_flow: f32) -> f32 {
219 assert!(
220 (0.0..=1.0).contains(&plastic_flow),
221 "a soft plastic flow must be within [0, 1]"
222 );
223 plastic_flow
224}
225
226fn apply_strength(material: &SoftMaterial, element: SoftElement) -> SoftElement {
227 element
228 .yielding(material.yield_strain, material.plastic_flow)
229 .fracturing(material.break_strain)
230}
231
232#[derive(Clone, Copy, Debug, PartialEq)]
233pub struct SoftAttachment {
234 particle: u32,
235 body: BodyHandle,
236 local: [f32; 3],
237}
238
239impl SoftAttachment {
240 pub fn new(particle: u32, body: BodyHandle, local: [f32; 3]) -> Self {
241 assert!(
242 local.iter().all(|value| value.is_finite()),
243 "a soft attachment anchor must be finite"
244 );
245 Self {
246 particle,
247 body,
248 local,
249 }
250 }
251
252 pub const fn particle(&self) -> u32 {
253 self.particle
254 }
255
256 pub const fn body(&self) -> BodyHandle {
257 self.body
258 }
259
260 pub const fn local(&self) -> [f32; 3] {
261 self.local
262 }
263}
264
265#[derive(Clone, Copy, Debug, PartialEq)]
266pub struct SoftElementState {
267 kind: SoftElementKind,
268 particles: [u32; SoftElement::PARTICLES],
269 rest: f32,
270 broken: bool,
271}
272
273impl SoftElementState {
274 pub const fn new(
275 kind: SoftElementKind,
276 particles: [u32; SoftElement::PARTICLES],
277 rest: f32,
278 broken: bool,
279 ) -> Self {
280 Self {
281 kind,
282 particles,
283 rest,
284 broken,
285 }
286 }
287
288 pub const fn kind(&self) -> SoftElementKind {
289 self.kind
290 }
291
292 pub const fn rest(&self) -> f32 {
293 self.rest
294 }
295
296 pub const fn broken(&self) -> bool {
297 self.broken
298 }
299
300 pub fn participants(&self) -> impl Iterator<Item = u32> + '_ {
301 self.particles[..self.kind.arity()].iter().copied()
302 }
303}
304
305#[derive(Clone, Copy, Debug, PartialEq)]
306pub struct FluidMaterial {
307 spacing: f32,
308 support: f32,
309}
310
311impl FluidMaterial {
312 pub fn new(spacing: f32, support: f32) -> Self {
313 assert!(
314 spacing > 0.0,
315 "a fluid rest spacing must be strictly positive"
316 );
317 assert!(
318 support > spacing,
319 "a fluid support radius must exceed its rest spacing"
320 );
321 Self { spacing, support }
322 }
323
324 pub const fn spacing(&self) -> f32 {
325 self.spacing
326 }
327
328 pub const fn support(&self) -> f32 {
329 self.support
330 }
331}
332
333#[derive(Clone, Debug, PartialEq)]
334pub struct SoftBodyDesc {
335 pub particles: Vec<[f32; 3]>,
336 pub inverse_masses: Vec<f32>,
337 pub elements: Vec<SoftElement>,
338 pub attachments: Vec<SoftAttachment>,
339 pub fluid: Option<FluidMaterial>,
340 pub radius: f32,
341 pub friction: f32,
342 pub position: [f32; 3],
343 pub orientation: [f32; 4],
344 pub velocity: [f32; 3],
345}
346
347impl SoftBodyDesc {
348 pub fn new(particles: Vec<[f32; 3]>, elements: Vec<SoftElement>) -> Self {
349 assert!(
350 !particles.is_empty(),
351 "a soft body requires at least one particle"
352 );
353 assert!(
354 particles
355 .iter()
356 .all(|particle| particle.iter().all(|value| value.is_finite())),
357 "soft body particles must be finite"
358 );
359 for element in &elements {
360 for particle in element.participants() {
361 assert!(
362 (particle as usize) < particles.len(),
363 "a soft element must reference live particles"
364 );
365 }
366 }
367 let inverse_masses = vec![1.0; particles.len()];
368 Self {
369 particles,
370 inverse_masses,
371 elements,
372 attachments: Vec::new(),
373 fluid: None,
374 radius: 0.0,
375 friction: 0.5,
376 position: [0.0; 3],
377 orientation: [0.0, 0.0, 0.0, 1.0],
378 velocity: [0.0; 3],
379 }
380 }
381
382 pub fn attach(mut self, attachment: SoftAttachment) -> Self {
383 self.attachments.push(attachment);
384 self.assert_attachments();
385 self
386 }
387
388 pub fn assert_attachments(&self) {
389 for (slot, attachment) in self.attachments.iter().enumerate() {
390 assert!(
391 (attachment.particle() as usize) < self.particles.len(),
392 "a soft attachment must reference a live particle"
393 );
394 assert!(
395 self.attachments[..slot]
396 .iter()
397 .all(|other| other.particle() != attachment.particle()),
398 "a soft particle carries at most one attachment"
399 );
400 }
401 }
402
403 pub fn fluid(particles: Vec<[f32; 3]>, radius: f32, material: FluidMaterial) -> Self {
404 assert!(
405 radius > 0.0,
406 "a fluid particle radius must be strictly positive"
407 );
408 assert!(
409 2.0 * radius <= material.spacing(),
410 "a fluid particle must be narrower than its rest spacing"
411 );
412 let mut body = Self::new(particles, Vec::new());
413 body.fluid = Some(material);
414 body.radius = radius;
415 body
416 }
417
418 pub fn net(particles: Vec<[f32; 3]>, links: Vec<[u32; 2]>) -> Self {
419 let elements = links
420 .iter()
421 .map(|link| {
422 SoftElement::distance(
423 link[0],
424 link[1],
425 particle_distance(&particles[link[0] as usize], &particles[link[1] as usize]),
426 )
427 })
428 .collect();
429 Self::new(particles, elements)
430 }
431
432 pub fn cloth(extent: [u32; 2], spacing: f32, material: SoftMaterial) -> Self {
433 let [rows, columns] = extent;
434 assert!(
435 rows > 1 && columns > 1,
436 "a cloth needs at least two rows and two columns"
437 );
438 assert!(
439 rows.max(columns) <= 64,
440 "a cloth axis must not exceed 64 particles"
441 );
442 assert!(spacing > 0.0, "cloth spacing must be positive");
443 let index = |row: u32, column: u32| column * rows + row;
444 let mut particles = Vec::with_capacity((rows * columns) as usize);
445 for column in 0..columns {
446 for row in 0..rows {
447 particles.push([row as f32 * spacing, 0.0, column as f32 * spacing]);
448 }
449 }
450 let mut triangles = Vec::new();
451 for column in 0..columns - 1 {
452 for row in 0..rows - 1 {
453 let first = index(row, column);
454 let second = index(row + 1, column);
455 let third = index(row, column + 1);
456 let fourth = index(row + 1, column + 1);
457 triangles.push([first, second, fourth]);
458 triangles.push([first, fourth, third]);
459 }
460 }
461 let mut elements = Vec::new();
462 for column in 0..columns {
463 for row in 0..rows {
464 if row + 1 < rows {
465 elements.push(apply_strength(
466 &material,
467 edge_element(
468 &particles,
469 index(row, column),
470 index(row + 1, column),
471 material.stretch,
472 ),
473 ));
474 }
475 if column + 1 < columns {
476 elements.push(apply_strength(
477 &material,
478 edge_element(
479 &particles,
480 index(row, column),
481 index(row, column + 1),
482 material.stretch,
483 ),
484 ));
485 }
486 }
487 }
488 for triangle in &triangles {
489 elements.push(apply_strength(
490 &material,
491 SoftElement::area(
492 triangle[0],
493 triangle[1],
494 triangle[2],
495 triangle_area(&particles, *triangle),
496 )
497 .compliance(material.shear),
498 ));
499 }
500 for [edge_a, edge_b, apex_a, apex_b] in shared_edges(&triangles) {
501 elements.push(apply_strength(
502 &material,
503 SoftElement::bend(
504 apex_a,
505 apex_b,
506 edge_a,
507 edge_b,
508 dihedral_angle(&particles, apex_a, apex_b, edge_a, edge_b),
509 )
510 .compliance(material.bend),
511 ));
512 }
513 Self::new(particles, elements)
514 }
515
516 pub fn lattice(extent: [u32; 3], spacing: f32, material: SoftMaterial) -> Self {
517 let [rows, columns, layers] = extent;
518 assert!(
519 rows > 0 && columns > 0 && layers > 0,
520 "a lattice needs a positive extent on every axis"
521 );
522 assert!(
523 rows.max(columns).max(layers) <= 64,
524 "a lattice axis must not exceed 64 particles"
525 );
526 assert!(spacing > 0.0, "lattice spacing must be positive");
527 let index = |row: u32, column: u32, layer: u32| (layer * columns + column) * rows + row;
528 let mut particles = Vec::with_capacity((rows * columns * layers) as usize);
529 for layer in 0..layers {
530 for column in 0..columns {
531 for row in 0..rows {
532 particles.push([
533 row as f32 * spacing,
534 layer as f32 * spacing,
535 column as f32 * spacing,
536 ]);
537 }
538 }
539 }
540 let mut edges: BTreeSet<[u32; 2]> = BTreeSet::new();
541 let mut tets: Vec<[u32; 4]> = Vec::new();
542 for layer in 0..layers.saturating_sub(1) {
543 for column in 0..columns.saturating_sub(1) {
544 for row in 0..rows.saturating_sub(1) {
545 let corners = [
546 index(row, column, layer),
547 index(row + 1, column, layer),
548 index(row, column, layer + 1),
549 index(row + 1, column, layer + 1),
550 index(row, column + 1, layer),
551 index(row + 1, column + 1, layer),
552 index(row, column + 1, layer + 1),
553 index(row + 1, column + 1, layer + 1),
554 ];
555 for tet in cube_tetrahedra(corners) {
556 for first in 0..3 {
557 for second in first + 1..4 {
558 let edge = if tet[first] < tet[second] {
559 [tet[first], tet[second]]
560 } else {
561 [tet[second], tet[first]]
562 };
563 edges.insert(edge);
564 }
565 }
566 tets.push(tet);
567 }
568 }
569 }
570 }
571 let mut elements = Vec::new();
572 for edge in &edges {
573 let compliance = if axis_aligned(&particles, *edge) {
574 material.stretch
575 } else {
576 material.shear
577 };
578 elements.push(apply_strength(
579 &material,
580 edge_element(&particles, edge[0], edge[1], compliance),
581 ));
582 }
583 for tet in &tets {
584 elements.push(apply_strength(
585 &material,
586 SoftElement::volume(
587 tet[0],
588 tet[1],
589 tet[2],
590 tet[3],
591 tetrahedron_volume(&particles, *tet),
592 )
593 .compliance(material.volume),
594 ));
595 }
596 Self::new(particles, elements)
597 }
598
599 pub fn inverse_masses(mut self, inverse_masses: Vec<f32>) -> Self {
600 assert_eq!(
601 self.particles.len(),
602 inverse_masses.len(),
603 "a soft body requires one inverse mass per particle"
604 );
605 assert!(
606 inverse_masses.iter().all(|mass| *mass >= 0.0),
607 "soft body inverse masses must be non-negative"
608 );
609 self.inverse_masses = inverse_masses;
610 self
611 }
612
613 pub fn compliance(mut self, compliance: f32) -> Self {
614 assert!(
615 compliance >= 0.0,
616 "soft body compliance must be non-negative"
617 );
618 for element in &mut self.elements {
619 *element = element.compliance(compliance);
620 }
621 self
622 }
623
624 pub fn yielding(mut self, yield_strain: f32, plastic_flow: f32) -> Self {
625 for element in &mut self.elements {
626 *element = element.yielding(yield_strain, plastic_flow);
627 }
628 self
629 }
630
631 pub fn fracturing(mut self, break_strain: f32) -> Self {
632 for element in &mut self.elements {
633 *element = element.fracturing(break_strain);
634 }
635 self
636 }
637
638 pub fn carries_strength(&self) -> bool {
639 self.elements.iter().any(SoftElement::carries_strength)
640 }
641
642 pub fn pinned(mut self, indices: &[u32]) -> Self {
643 for index in indices {
644 let slot = *index as usize;
645 assert!(
646 slot < self.particles.len(),
647 "a pinned particle must be part of the soft body"
648 );
649 self.inverse_masses[slot] = 0.0;
650 }
651 self
652 }
653
654 pub fn radius(mut self, radius: f32) -> Self {
655 assert!(radius >= 0.0, "particle radius must be non-negative");
656 self.radius = radius;
657 self
658 }
659
660 pub fn friction(mut self, friction: f32) -> Self {
661 assert!(friction >= 0.0, "soft body friction must be non-negative");
662 self.friction = friction;
663 self
664 }
665
666 pub fn position(mut self, position: [f32; 3]) -> Self {
667 self.position = position;
668 self
669 }
670
671 pub fn orientation(mut self, orientation: [f32; 4]) -> Self {
672 assert!(
673 (orientation[0] * orientation[0]
674 + orientation[1] * orientation[1]
675 + orientation[2] * orientation[2]
676 + orientation[3] * orientation[3]
677 - 1.0)
678 .abs()
679 < 1e-4,
680 "orientation must be a unit quaternion"
681 );
682 self.orientation = orientation;
683 self
684 }
685
686 pub fn velocity(mut self, velocity: [f32; 3]) -> Self {
687 self.velocity = velocity;
688 self
689 }
690}
691
692fn particle_distance(first: &[f32; 3], second: &[f32; 3]) -> f32 {
693 let dx = second[0] - first[0];
694 let dy = second[1] - first[1];
695 let dz = second[2] - first[2];
696 (dx * dx + dy * dy + dz * dz).sqrt()
697}
698
699fn offset(particles: &[[f32; 3]], first: u32, second: u32) -> [f32; 3] {
700 let from = particles[first as usize];
701 let to = particles[second as usize];
702 [to[0] - from[0], to[1] - from[1], to[2] - from[2]]
703}
704
705fn cross(first: [f32; 3], second: [f32; 3]) -> [f32; 3] {
706 [
707 first[1] * second[2] - first[2] * second[1],
708 first[2] * second[0] - first[0] * second[2],
709 first[0] * second[1] - first[1] * second[0],
710 ]
711}
712
713fn dot(first: [f32; 3], second: [f32; 3]) -> f32 {
714 first[0] * second[0] + first[1] * second[1] + first[2] * second[2]
715}
716
717fn length(vector: [f32; 3]) -> f32 {
718 dot(vector, vector).sqrt()
719}
720
721fn edge_element(particles: &[[f32; 3]], first: u32, second: u32, compliance: f32) -> SoftElement {
722 SoftElement::distance(
723 first,
724 second,
725 particle_distance(&particles[first as usize], &particles[second as usize]),
726 )
727 .compliance(compliance)
728}
729
730fn triangle_area(particles: &[[f32; 3]], triangle: [u32; 3]) -> f32 {
731 0.5 * length(cross(
732 offset(particles, triangle[0], triangle[1]),
733 offset(particles, triangle[0], triangle[2]),
734 ))
735}
736
737fn tetrahedron_volume(particles: &[[f32; 3]], tet: [u32; 4]) -> f32 {
738 dot(
739 cross(
740 offset(particles, tet[0], tet[1]),
741 offset(particles, tet[0], tet[2]),
742 ),
743 offset(particles, tet[0], tet[3]),
744 )
745 .abs()
746 / 6.0
747}
748
749fn dihedral_angle(
750 particles: &[[f32; 3]],
751 apex_a: u32,
752 apex_b: u32,
753 edge_a: u32,
754 edge_b: u32,
755) -> f32 {
756 let first = cross(
757 offset(particles, apex_a, edge_a),
758 offset(particles, apex_a, edge_b),
759 );
760 let second = cross(
761 offset(particles, apex_b, edge_b),
762 offset(particles, apex_b, edge_a),
763 );
764 let first_length = length(first);
765 let second_length = length(second);
766 assert!(
767 first_length > 0.0 && second_length > 0.0,
768 "a bend element needs two non-degenerate triangles"
769 );
770 (dot(first, second) / (first_length * second_length))
771 .clamp(-1.0, 1.0)
772 .acos()
773}
774
775fn shared_edges(triangles: &[[u32; 3]]) -> Vec<[u32; 4]> {
776 let mut apexes: BTreeMap<[u32; 2], Vec<u32>> = BTreeMap::new();
777 for triangle in triangles {
778 for role in 0..3 {
779 let edge_a = triangle[role];
780 let edge_b = triangle[(role + 1) % 3];
781 let apex = triangle[(role + 2) % 3];
782 let key = if edge_a < edge_b {
783 [edge_a, edge_b]
784 } else {
785 [edge_b, edge_a]
786 };
787 let shared = apexes.entry(key).or_default();
788 assert!(
789 shared.len() < 2,
790 "a cloth edge must belong to at most two triangles"
791 );
792 shared.push(apex);
793 }
794 }
795 apexes
796 .into_iter()
797 .filter_map(|([edge_a, edge_b], shared)| {
798 let [apex_a, apex_b] = shared[..] else {
799 return None;
800 };
801 Some([edge_a, edge_b, apex_a, apex_b])
802 })
803 .collect()
804}
805
806fn axis_aligned(particles: &[[f32; 3]], edge: [u32; 2]) -> bool {
807 let first = particles[edge[0] as usize];
808 let second = particles[edge[1] as usize];
809 let differing = (0..3)
810 .filter(|axis| (first[*axis] - second[*axis]).abs() > 1e-6)
811 .count();
812 differing == 1
813}
814
815fn cube_tetrahedra(corners: [u32; 8]) -> [[u32; 4]; 6] {
816 [
817 [corners[0], corners[1], corners[3], corners[7]],
818 [corners[0], corners[3], corners[2], corners[7]],
819 [corners[0], corners[2], corners[6], corners[7]],
820 [corners[0], corners[6], corners[4], corners[7]],
821 [corners[0], corners[4], corners[5], corners[7]],
822 [corners[0], corners[5], corners[1], corners[7]],
823 ]
824}