1#[allow(dead_code)]
4pub struct SpringJoint {
5 pub name: String,
6 pub position: [f32; 3],
7 pub velocity: [f32; 3],
8 pub rest_position: [f32; 3],
9 pub stiffness: f32,
10 pub damping: f32,
11 pub mass: f32,
12}
13
14#[allow(dead_code)]
15pub struct ExpressionPhysics {
16 pub joints: Vec<SpringJoint>,
17 pub gravity: [f32; 3],
18 pub enabled: bool,
19}
20
21#[allow(dead_code)]
22pub struct PhysicsExpressionResult {
23 pub deltas: Vec<[f32; 3]>,
24 pub kinetic_energy: f32,
25}
26
27#[allow(dead_code)]
28pub fn new_expression_physics(gravity: [f32; 3]) -> ExpressionPhysics {
29 ExpressionPhysics {
30 joints: Vec::new(),
31 gravity,
32 enabled: true,
33 }
34}
35
36#[allow(dead_code)]
37pub fn add_spring_joint(
38 ep: &mut ExpressionPhysics,
39 name: &str,
40 rest_pos: [f32; 3],
41 stiffness: f32,
42 damping: f32,
43 mass: f32,
44) -> usize {
45 let idx = ep.joints.len();
46 ep.joints.push(SpringJoint {
47 name: name.to_string(),
48 position: rest_pos,
49 velocity: [0.0; 3],
50 rest_position: rest_pos,
51 stiffness,
52 damping,
53 mass,
54 });
55 idx
56}
57
58#[allow(dead_code)]
59pub fn step_expression_physics(ep: &mut ExpressionPhysics, dt: f32) {
60 if !ep.enabled {
61 return;
62 }
63 let gravity = ep.gravity;
64 for joint in &mut ep.joints {
65 let m = if joint.mass > 0.0 { joint.mass } else { 1.0 };
66 let force = [
67 -joint.stiffness * (joint.position[0] - joint.rest_position[0])
68 - joint.damping * joint.velocity[0]
69 + gravity[0],
70 -joint.stiffness * (joint.position[1] - joint.rest_position[1])
71 - joint.damping * joint.velocity[1]
72 + gravity[1],
73 -joint.stiffness * (joint.position[2] - joint.rest_position[2])
74 - joint.damping * joint.velocity[2]
75 + gravity[2],
76 ];
77 joint.velocity[0] += (force[0] / m) * dt;
78 joint.velocity[1] += (force[1] / m) * dt;
79 joint.velocity[2] += (force[2] / m) * dt;
80 joint.position[0] += joint.velocity[0] * dt;
81 joint.position[1] += joint.velocity[1] * dt;
82 joint.position[2] += joint.velocity[2] * dt;
83 }
84}
85
86#[allow(dead_code)]
87pub fn set_rest_position(ep: &mut ExpressionPhysics, idx: usize, pos: [f32; 3]) {
88 if idx < ep.joints.len() {
89 ep.joints[idx].rest_position = pos;
90 }
91}
92
93#[allow(dead_code)]
94pub fn apply_impulse_to_joint(ep: &mut ExpressionPhysics, idx: usize, impulse: [f32; 3]) {
95 if idx < ep.joints.len() {
96 let joint = &mut ep.joints[idx];
97 let m = if joint.mass > 0.0 { joint.mass } else { 1.0 };
98 joint.velocity[0] += impulse[0] / m;
99 joint.velocity[1] += impulse[1] / m;
100 joint.velocity[2] += impulse[2] / m;
101 }
102}
103
104#[allow(dead_code)]
105pub fn evaluate_expression_physics(ep: &ExpressionPhysics) -> PhysicsExpressionResult {
106 let mut deltas = Vec::with_capacity(ep.joints.len());
107 let mut kinetic_energy = 0.0f32;
108 for joint in &ep.joints {
109 let delta = [
110 joint.position[0] - joint.rest_position[0],
111 joint.position[1] - joint.rest_position[1],
112 joint.position[2] - joint.rest_position[2],
113 ];
114 deltas.push(delta);
115 kinetic_energy += joint_kinetic_energy(joint);
116 }
117 PhysicsExpressionResult {
118 deltas,
119 kinetic_energy,
120 }
121}
122
123#[allow(dead_code)]
124pub fn reset_to_rest(ep: &mut ExpressionPhysics) {
125 for joint in &mut ep.joints {
126 joint.position = joint.rest_position;
127 joint.velocity = [0.0; 3];
128 }
129}
130
131#[allow(dead_code)]
132pub fn joint_displacement(joint: &SpringJoint) -> f32 {
133 let dx = joint.position[0] - joint.rest_position[0];
134 let dy = joint.position[1] - joint.rest_position[1];
135 let dz = joint.position[2] - joint.rest_position[2];
136 (dx * dx + dy * dy + dz * dz).sqrt()
137}
138
139#[allow(dead_code)]
140pub fn default_facial_physics() -> ExpressionPhysics {
141 let mut ep = new_expression_physics([0.0, -9.81, 0.0]);
142 add_spring_joint(&mut ep, "cheek_L", [-0.05, 0.0, 0.03], 80.0, 8.0, 0.01);
143 add_spring_joint(&mut ep, "cheek_R", [0.05, 0.0, 0.03], 80.0, 8.0, 0.01);
144 add_spring_joint(&mut ep, "jaw", [0.0, -0.03, 0.02], 60.0, 6.0, 0.015);
145 add_spring_joint(&mut ep, "nose_tip", [0.0, 0.01, 0.05], 120.0, 12.0, 0.005);
146 add_spring_joint(&mut ep, "chin", [0.0, -0.05, 0.02], 70.0, 7.0, 0.012);
147 add_spring_joint(&mut ep, "forehead", [0.0, 0.06, 0.01], 100.0, 10.0, 0.008);
148 ep
149}
150
151#[allow(dead_code)]
152pub fn joint_kinetic_energy(joint: &SpringJoint) -> f32 {
153 let v_sq = joint.velocity[0] * joint.velocity[0]
154 + joint.velocity[1] * joint.velocity[1]
155 + joint.velocity[2] * joint.velocity[2];
156 0.5 * joint.mass * v_sq
157}
158
159#[allow(dead_code)]
160pub fn set_enabled(ep: &mut ExpressionPhysics, enabled: bool) {
161 ep.enabled = enabled;
162}
163
164#[allow(dead_code)]
165pub fn joint_count(ep: &ExpressionPhysics) -> usize {
166 ep.joints.len()
167}
168
169#[cfg(test)]
170mod tests {
171 use super::*;
172
173 #[test]
174 fn test_new_expression_physics() {
175 let ep = new_expression_physics([0.0, -9.81, 0.0]);
176 assert!(ep.joints.is_empty());
177 assert!(ep.enabled);
178 assert!((ep.gravity[1] + 9.81).abs() < 1e-5);
179 }
180
181 #[test]
182 fn test_add_spring_joint() {
183 let mut ep = new_expression_physics([0.0; 3]);
184 let idx = add_spring_joint(&mut ep, "cheek_L", [0.1, 0.2, 0.3], 100.0, 10.0, 0.01);
185 assert_eq!(idx, 0);
186 assert_eq!(joint_count(&ep), 1);
187 assert_eq!(ep.joints[0].name, "cheek_L");
188 }
189
190 #[test]
191 fn test_step_changes_position_after_impulse() {
192 let mut ep = new_expression_physics([0.0; 3]);
193 add_spring_joint(&mut ep, "jaw", [0.0; 3], 10.0, 1.0, 1.0);
194 apply_impulse_to_joint(&mut ep, 0, [1.0, 0.0, 0.0]);
195 step_expression_physics(&mut ep, 0.016);
196 assert!(ep.joints[0].position[0] > 0.0);
197 }
198
199 #[test]
200 fn test_reset_zeroes_velocity() {
201 let mut ep = new_expression_physics([0.0; 3]);
202 add_spring_joint(&mut ep, "jaw", [0.0; 3], 10.0, 1.0, 1.0);
203 apply_impulse_to_joint(&mut ep, 0, [2.0, 1.0, 0.5]);
204 step_expression_physics(&mut ep, 0.1);
205 reset_to_rest(&mut ep);
206 let j = &ep.joints[0];
207 assert_eq!(j.velocity, [0.0; 3]);
208 assert_eq!(j.position, j.rest_position);
209 }
210
211 #[test]
212 fn test_evaluate_returns_correct_count() {
213 let mut ep = new_expression_physics([0.0; 3]);
214 add_spring_joint(&mut ep, "a", [0.0; 3], 1.0, 0.1, 1.0);
215 add_spring_joint(&mut ep, "b", [1.0, 0.0, 0.0], 1.0, 0.1, 1.0);
216 let result = evaluate_expression_physics(&ep);
217 assert_eq!(result.deltas.len(), 2);
218 }
219
220 #[test]
221 fn test_kinetic_energy_formula() {
222 let joint = SpringJoint {
223 name: "test".to_string(),
224 position: [0.0; 3],
225 velocity: [1.0, 0.0, 0.0],
226 rest_position: [0.0; 3],
227 stiffness: 10.0,
228 damping: 1.0,
229 mass: 2.0,
230 };
231 let ke = joint_kinetic_energy(&joint);
232 assert!((ke - 1.0).abs() < 1e-6); }
234
235 #[test]
236 fn test_default_facial_physics_count() {
237 let ep = default_facial_physics();
238 assert_eq!(joint_count(&ep), 6);
239 }
240
241 #[test]
242 fn test_displacement_formula() {
243 let joint = SpringJoint {
244 name: "test".to_string(),
245 position: [1.0, 0.0, 0.0],
246 velocity: [0.0; 3],
247 rest_position: [0.0; 3],
248 stiffness: 10.0,
249 damping: 1.0,
250 mass: 1.0,
251 };
252 let d = joint_displacement(&joint);
253 assert!((d - 1.0).abs() < 1e-6);
254 }
255
256 #[test]
257 fn test_set_rest_position() {
258 let mut ep = new_expression_physics([0.0; 3]);
259 add_spring_joint(&mut ep, "j", [0.0; 3], 10.0, 1.0, 1.0);
260 set_rest_position(&mut ep, 0, [1.0, 2.0, 3.0]);
261 assert_eq!(ep.joints[0].rest_position, [1.0, 2.0, 3.0]);
262 }
263
264 #[test]
265 fn test_apply_impulse_changes_velocity() {
266 let mut ep = new_expression_physics([0.0; 3]);
267 add_spring_joint(&mut ep, "j", [0.0; 3], 10.0, 1.0, 2.0);
268 apply_impulse_to_joint(&mut ep, 0, [2.0, 0.0, 0.0]);
269 assert!((ep.joints[0].velocity[0] - 1.0).abs() < 1e-6);
271 }
272
273 #[test]
274 fn test_set_enabled_disables_stepping() {
275 let mut ep = new_expression_physics([0.0; 3]);
276 add_spring_joint(&mut ep, "j", [0.0; 3], 10.0, 1.0, 1.0);
277 apply_impulse_to_joint(&mut ep, 0, [1.0, 0.0, 0.0]);
278 set_enabled(&mut ep, false);
279 let pos_before = ep.joints[0].position;
280 step_expression_physics(&mut ep, 0.1);
281 assert_eq!(ep.joints[0].position, pos_before);
282 }
283
284 #[test]
285 fn test_evaluate_kinetic_energy_at_rest_zero() {
286 let ep = new_expression_physics([0.0; 3]);
287 let result = evaluate_expression_physics(&ep);
288 assert!((result.kinetic_energy).abs() < 1e-9);
289 }
290
291 #[test]
292 fn test_evaluate_deltas_at_rest_zero() {
293 let mut ep = new_expression_physics([0.0; 3]);
294 add_spring_joint(&mut ep, "j", [1.0, 2.0, 3.0], 10.0, 1.0, 1.0);
295 let result = evaluate_expression_physics(&ep);
296 assert_eq!(result.deltas[0], [0.0; 3]);
297 }
298
299 #[test]
300 fn test_spring_returns_to_rest() {
301 let mut ep = new_expression_physics([0.0; 3]);
302 add_spring_joint(&mut ep, "j", [0.0; 3], 200.0, 40.0, 1.0);
304 apply_impulse_to_joint(&mut ep, 0, [1.0, 0.0, 0.0]);
305 for _ in 0..500 {
306 step_expression_physics(&mut ep, 0.01);
307 }
308 let d = joint_displacement(&ep.joints[0]);
309 assert!(d < 0.01, "displacement should settle near rest: {}", d);
310 }
311
312 #[test]
313 fn test_joint_count_empty() {
314 let ep = new_expression_physics([0.0; 3]);
315 assert_eq!(joint_count(&ep), 0);
316 }
317}