gizmo_physics_soft/
cloth.rs

1use gizmo_math::Vec3;
2
3#[derive(Debug, Clone)]
4pub struct ClothNode {
5    pub position: Vec3,
6    pub prev_position: Vec3,
7    pub mass: f32,
8    pub inv_mass: f32,
9}
10
11#[derive(Debug, Clone, Copy)]
12pub struct DistanceConstraint {
13    pub node_a: usize,
14    pub node_b: usize,
15    pub rest_length: f32,
16    pub compliance: f32, // Inverse stiffness
17    pub lambda: f32,     // Accumulated XPBD multiplier
18}
19
20#[derive(Debug, Clone)]
21pub struct Cloth {
22    pub nodes: Vec<ClothNode>,
23    pub constraints: Vec<DistanceConstraint>,
24    pub thickness: f32,
25    pub friction: f32,
26}
27
28impl Cloth {
29    pub fn new(width: usize, height: usize, spacing: f32, mass_per_node: f32) -> Self {
30        let mut nodes = Vec::with_capacity(width * height);
31        let mut constraints = Vec::new();
32
33        for y in 0..height {
34            for x in 0..width {
35                let position = Vec3::new(x as f32 * spacing, y as f32 * spacing, 0.0);
36                nodes.push(ClothNode {
37                    position,
38                    prev_position: position,
39                    mass: mass_per_node,
40                    inv_mass: if mass_per_node > 0.0 {
41                        1.0 / mass_per_node
42                    } else {
43                        0.0
44                    },
45                });
46
47                let idx = y * width + x;
48
49                // Structural constraints
50                if x > 0 {
51                    constraints.push(DistanceConstraint {
52                        node_a: idx,
53                        node_b: idx - 1,
54                        rest_length: spacing,
55                        compliance: 0.001,
56                        lambda: 0.0,
57                    });
58                }
59                if y > 0 {
60                    constraints.push(DistanceConstraint {
61                        node_a: idx,
62                        node_b: idx - width,
63                        rest_length: spacing,
64                        compliance: 0.001,
65                        lambda: 0.0,
66                    });
67                }
68
69                // Bend constraints
70                if x > 1 {
71                    constraints.push(DistanceConstraint {
72                        node_a: idx,
73                        node_b: idx - 2,
74                        rest_length: spacing * 2.0,
75                        compliance: 0.1,
76                        lambda: 0.0,
77                    });
78                }
79                if y > 1 {
80                    constraints.push(DistanceConstraint {
81                        node_a: idx,
82                        node_b: idx - width * 2,
83                        rest_length: spacing * 2.0,
84                        compliance: 0.1,
85                        lambda: 0.0,
86                    });
87                }
88
89                // Shear constraints
90                if x > 0 && y > 0 {
91                    let diag = spacing * std::f32::consts::SQRT_2;
92                    constraints.push(DistanceConstraint {
93                        node_a: idx,
94                        node_b: idx - width - 1,
95                        rest_length: diag,
96                        compliance: 0.005,
97                        lambda: 0.0,
98                    });
99                    constraints.push(DistanceConstraint {
100                        node_a: idx - 1,
101                        node_b: idx - width,
102                        rest_length: diag,
103                        compliance: 0.005,
104                        lambda: 0.0,
105                    });
106                }
107            }
108        }
109
110        Self {
111            nodes,
112            constraints,
113            thickness: 0.02,
114            friction: 0.5,
115        }
116    }
117
118    pub fn pin_node(&mut self, idx: usize) {
119        if idx < self.nodes.len() {
120            self.nodes[idx].inv_mass = 0.0;
121            self.nodes[idx].mass = 0.0;
122        }
123    }
124
125    /// XPBD step
126    pub fn step(&mut self, dt: f32, gravity: Vec3, sub_steps: usize) {
127        let sub_dt = dt / (sub_steps as f32);
128        let sub_dt2 = sub_dt * sub_dt;
129
130        for _ in 0..sub_steps {
131            for c in &mut self.constraints {
132                c.lambda = 0.0;
133            }
134
135            // Predict
136            for node in &mut self.nodes {
137                if node.inv_mass == 0.0 {
138                    continue;
139                }
140                let velocity = (node.position - node.prev_position) / sub_dt;
141                node.prev_position = node.position;
142
143                // Add gravity and damping (frame-rate independent)
144                let damping = 0.99f32;
145                let next_vel = velocity * damping.powf(sub_dt) + gravity * sub_dt;
146                node.position += next_vel * sub_dt;
147            }
148
149            // Solve Constraints
150            for constraint in &mut self.constraints {
151                let (pos_a, pos_b, inv_m_a, inv_m_b) = {
152                    let a = &self.nodes[constraint.node_a];
153                    let b = &self.nodes[constraint.node_b];
154                    (a.position, b.position, a.inv_mass, b.inv_mass)
155                };
156
157                let w_sum = inv_m_a + inv_m_b;
158                if w_sum == 0.0 {
159                    continue;
160                }
161
162                let diff = pos_a - pos_b;
163                let dist = diff.length();
164                if dist < 1e-6 {
165                    continue;
166                }
167
168                let n = diff / dist;
169                let c = dist - constraint.rest_length;
170                let alpha = constraint.compliance / sub_dt2;
171
172                let delta_lambda = (-c - alpha * constraint.lambda) / (w_sum + alpha);
173                constraint.lambda += delta_lambda;
174
175                let p = n * delta_lambda;
176
177                self.nodes[constraint.node_a].position += p * inv_m_a;
178                self.nodes[constraint.node_b].position -= p * inv_m_b;
179            }
180
181            // Floor Collision
182            for node in &mut self.nodes {
183                if node.inv_mass == 0.0 {
184                    continue;
185                }
186                if node.position.y < self.thickness {
187                    node.position.y = self.thickness;
188
189                    // Simple friction: damp horizontal velocity when touching ground
190                    let mut vel = (node.position - node.prev_position) / sub_dt;
191                    vel.x *= 1.0 - self.friction;
192                    vel.z *= 1.0 - self.friction;
193                    node.prev_position = node.position - vel * sub_dt;
194                }
195            }
196        }
197    }
198}
gizmo_physics_soft/cloth.rs

gizmo_physics_soft/
cloth.rs
