gizmo_physics_soft/
rope.rs

1use gizmo_math::Vec3;
2
3/// Represents a single particle in the rope or chain.
4#[derive(Debug, Clone, Copy)]
5pub struct RopeNode {
6    pub position: Vec3,
7    pub prev_position: Vec3,
8    pub mass: f32,
9    pub inv_mass: f32,
10    pub is_fixed: bool,
11}
12
13/// A rope or chain simulated using Position Based Dynamics (PBD).
14#[derive(Debug, Clone)]
15pub struct Rope {
16    pub nodes: Vec<RopeNode>,
17    pub link_length: f32,
18    pub iterations: usize,
19    pub stiffness: f32,
20    pub damping: f32,
21}
22
23impl Rope {
24    pub fn new(
25        start_pos: Vec3,
26        direction: Vec3,
27        num_segments: usize,
28        segment_length: f32,
29        node_mass: f32,
30        fix_start: bool,
31        fix_end: bool,
32    ) -> Self {
33        let mut nodes = Vec::with_capacity(num_segments + 1);
34        let inv_mass = if node_mass > 0.0 {
35            1.0 / node_mass
36        } else {
37            0.0
38        };
39
40        let dir_norm = direction.normalize_or_zero();
41
42        for i in 0..=num_segments {
43            let pos = start_pos + dir_norm * (i as f32 * segment_length);
44            let is_fixed = (i == 0 && fix_start) || (i == num_segments && fix_end);
45            nodes.push(RopeNode {
46                position: pos,
47                prev_position: pos,
48                mass: if is_fixed { 0.0 } else { node_mass },
49                inv_mass: if is_fixed { 0.0 } else { inv_mass },
50                is_fixed,
51            });
52        }
53
54        Self {
55            nodes,
56            link_length: segment_length,
57            iterations: 10,
58            stiffness: 1.0,
59            damping: 0.98,
60        }
61    }
62
63    pub fn step(&mut self, dt: f32, gravity: Vec3) {
64        if dt <= 0.0 {
65            return;
66        }
67
68        // 1. Explicit Euler integration for unconstrained motion
69        for node in &mut self.nodes {
70            if node.is_fixed {
71                continue;
72            }
73
74            let velocity = (node.position - node.prev_position) / dt;
75            node.prev_position = node.position;
76
77            // Add gravity and damping
78            let new_vel = (velocity + gravity * dt) * self.damping.powf(dt);
79            node.position += new_vel * dt;
80        }
81
82        // 2. Position Based Dynamics constraint solving (XPBD)
83        // Compliance = inverse stiffness; clamp stiffness to [0, 1] to avoid negative alpha
84        let compliance = (1.0 - self.stiffness.min(1.0)).max(0.0);
85        let alpha = compliance / (dt * dt);
86
87        for _ in 0..self.iterations {
88            for i in 0..(self.nodes.len() - 1) {
89                let n1 = self.nodes[i];
90                let n2 = self.nodes[i + 1];
91
92                let w1 = n1.inv_mass;
93                let w2 = n2.inv_mass;
94                let w_sum = w1 + w2;
95
96                if w_sum == 0.0 {
97                    continue; // Both are fixed
98                }
99
100                let dir = n2.position - n1.position;
101                let current_len = dir.length();
102                if current_len < 1e-6 {
103                    continue; // Prevent division by zero
104                }
105
106                let err = current_len - self.link_length;
107                let correction_mag = err / (w_sum + alpha); // XPBD compliance
108                let correction = dir.normalize() * correction_mag;
109
110                if !n1.is_fixed {
111                    self.nodes[i].position += correction * w1;
112                }
113                if !n2.is_fixed {
114                    self.nodes[i + 1].position -= correction * w2;
115                }
116            }
117        }
118
119        // 3. Simple ground collision
120        for node in &mut self.nodes {
121            if node.position.y < 0.0 {
122                node.position.y = 0.0;
123                node.prev_position.y = node.position.y; // zero vertical velocity
124            }
125        }
126    }
127}
128
129impl gizmo_core::Component for Rope {}
gizmo_physics_soft/rope.rs

gizmo_physics_soft/
rope.rs
