1#![allow(clippy::needless_range_loop)]
6#[cfg(test)]
7use super::types::*;
8
9#[inline]
10pub(super) fn dot3(a: [f64; 3], b: [f64; 3]) -> f64 {
11 a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
12}
13#[inline]
14pub(super) fn cross3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
15 [
16 a[1] * b[2] - a[2] * b[1],
17 a[2] * b[0] - a[0] * b[2],
18 a[0] * b[1] - a[1] * b[0],
19 ]
20}
21#[inline]
22pub(super) fn length3(v: [f64; 3]) -> f64 {
23 dot3(v, v).sqrt()
24}
25#[inline]
26pub(super) fn normalize3(v: [f64; 3]) -> [f64; 3] {
27 let len = length3(v);
28 if len < 1e-30 {
29 return [0.0; 3];
30 }
31 scale3(v, 1.0 / len)
32}
33#[inline]
34pub(super) fn sub3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
35 [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
36}
37#[inline]
38pub(super) fn add3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
39 [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
40}
41#[inline]
42pub(super) fn scale3(v: [f64; 3], s: f64) -> [f64; 3] {
43 [v[0] * s, v[1] * s, v[2] * s]
44}
45#[inline]
47pub(super) fn len2_3(v: [f64; 3]) -> f64 {
48 dot3(v, v)
49}
50pub(super) fn quat_rotate(q: [f64; 4], p: [f64; 3]) -> [f64; 3] {
52 let [w, x, y, z] = q;
53 let qv = [x, y, z];
54 let t = scale3(cross3(qv, p), 2.0);
55 add3(add3(p, scale3(t, w)), cross3(qv, t))
56}
57#[cfg(test)]
58mod tests {
59 use super::*;
60 fn make_simple_mesh(n: usize) -> DeformableGpuMesh {
61 let mut mesh = DeformableGpuMesh::new();
62 for i in 0..n {
63 let x = i as f64;
64 mesh.add_vertex(DeformableVertex::new([x, 0.0, 0.0]));
65 }
66 mesh
67 }
68 #[test]
69 fn test_mesh_add_vertex() {
70 let mut mesh = DeformableGpuMesh::new();
71 let idx = mesh.add_vertex(DeformableVertex::new([1.0, 2.0, 3.0]));
72 assert_eq!(idx, 0);
73 assert_eq!(mesh.vertices.len(), 1);
74 }
75 #[test]
76 fn test_mesh_add_face() {
77 let mut mesh = make_simple_mesh(4);
78 mesh.add_face(0, 1, 2);
79 assert_eq!(mesh.faces.len(), 1);
80 assert_eq!(mesh.faces[0].a, 0);
81 }
82 #[test]
83 fn test_mesh_aabb() {
84 let mut mesh = DeformableGpuMesh::new();
85 mesh.add_vertex(DeformableVertex::new([0.0, 0.0, 0.0]));
86 mesh.add_vertex(DeformableVertex::new([1.0, 2.0, 3.0]));
87 let (lo, hi) = mesh.aabb();
88 assert!((lo[0]).abs() < 1e-10);
89 assert!((hi[2] - 3.0).abs() < 1e-10);
90 }
91 #[test]
92 fn test_mesh_aabb_single_vertex() {
93 let mut mesh = DeformableGpuMesh::new();
94 mesh.add_vertex(DeformableVertex::new([5.0, 6.0, 7.0]));
95 let (lo, hi) = mesh.aabb();
96 assert!((lo[0] - 5.0).abs() < 1e-10);
97 assert!((hi[0] - 5.0).abs() < 1e-10);
98 }
99 #[test]
100 fn test_mesh_face_normals() {
101 let mut mesh = DeformableGpuMesh::new();
102 mesh.add_vertex(DeformableVertex::new([0.0, 0.0, 0.0]));
103 mesh.add_vertex(DeformableVertex::new([1.0, 0.0, 0.0]));
104 mesh.add_vertex(DeformableVertex::new([0.0, 1.0, 0.0]));
105 mesh.add_face(0, 1, 2);
106 let normals = mesh.compute_face_normals();
107 assert_eq!(normals.len(), 1);
108 assert!((normals[0][2] - 1.0).abs() < 1e-10);
109 }
110 #[test]
111 fn test_blend_shape_apply() {
112 let mut mesh = DeformableGpuMesh::new();
113 mesh.add_vertex(DeformableVertex::new([0.0, 0.0, 0.0]));
114 let bs = BlendShape::new("test", vec![[0.0, 1.0, 0.0]]);
115 let idx = mesh.add_blend_shape(bs);
116 mesh.blend_shapes[idx].weight = 1.0;
117 mesh.apply_blend_shapes();
118 assert!((mesh.vertices[0].position[1] - 1.0).abs() < 1e-10);
119 }
120 #[test]
121 fn test_blend_shape_zero_weight() {
122 let mut mesh = DeformableGpuMesh::new();
123 mesh.add_vertex(DeformableVertex::new([0.0, 0.0, 0.0]));
124 let bs = BlendShape::new("test", vec![[0.0, 1.0, 0.0]]);
125 mesh.add_blend_shape(bs);
126 mesh.apply_blend_shapes();
127 assert!((mesh.vertices[0].position[1]).abs() < 1e-10);
128 }
129 #[test]
130 fn test_joint_transform_identity() {
131 let jt = JointTransform::identity();
132 let p = [1.0, 2.0, 3.0];
133 let tp = jt.transform_point(p);
134 for k in 0..3 {
135 assert!((tp[k] - p[k]).abs() < 1e-10, "k={k}");
136 }
137 }
138 #[test]
139 fn test_joint_transform_translation() {
140 let mut jt = JointTransform::identity();
141 jt.row0[3] = 5.0;
142 let tp = jt.transform_point([0.0, 0.0, 0.0]);
143 assert!((tp[0] - 5.0).abs() < 1e-10);
144 }
145 #[test]
146 fn test_dualquat_identity_transform() {
147 let dq = DualQuat::identity();
148 let p = [1.0, 2.0, 3.0];
149 let tp = dq.transform_point(p);
150 for k in 0..3 {
151 assert!((tp[k] - p[k]).abs() < 1e-9, "k={k}, got {}", tp[k]);
152 }
153 }
154 #[test]
155 fn test_dualquat_pure_translation() {
156 let dq = DualQuat::from_translation([3.0, 0.0, 0.0]);
157 let p = [0.0, 0.0, 0.0];
158 let tp = dq.transform_point(p);
159 assert!((tp[0] - 3.0).abs() < 1e-9, "got {}", tp[0]);
160 }
161 #[test]
162 fn test_dualquat_normalize_identity() {
163 let dq = DualQuat::identity();
164 let dn = dq.normalize();
165 assert!((dn.real[0] - 1.0).abs() < 1e-10);
166 }
167 #[test]
168 fn test_skinning_lbs_identity() {
169 let kernel = SkinningKernel::new(SkinningMode::LinearBlend, 1);
170 let v = DeformableVertex::new([1.0, 2.0, 3.0]);
171 let result = kernel.apply_lbs(&v);
172 for k in 0..3 {
173 assert!((result[k] - v.rest_position[k]).abs() < 1e-10);
174 }
175 }
176 #[test]
177 fn test_skinning_dqs_identity() {
178 let kernel = SkinningKernel::new(SkinningMode::DualQuaternion, 1);
179 let v = DeformableVertex::new([1.0, 2.0, 3.0]);
180 let result = kernel.apply_dqs(&v);
181 for k in 0..3 {
182 assert!((result[k] - v.rest_position[k]).abs() < 1e-9, "k={k}");
183 }
184 }
185 #[test]
186 fn test_skinning_dispatch_lbs() {
187 let mut kernel = SkinningKernel::new(SkinningMode::LinearBlend, 1);
188 kernel.joints[0].row1[3] = 5.0;
189 let mut mesh = make_simple_mesh(3);
190 kernel.dispatch(&mut mesh);
191 for v in &mesh.vertices {
192 assert!((v.position[1] - 5.0).abs() < 1e-10);
193 }
194 }
195 #[test]
196 fn test_skinning_dispatch_dqs() {
197 let mut kernel = SkinningKernel::new(SkinningMode::DualQuaternion, 1);
198 kernel.dq_joints[0] = DualQuat::from_translation([0.0, 5.0, 0.0]);
199 let mut mesh = make_simple_mesh(3);
200 kernel.dispatch(&mut mesh);
201 for v in &mesh.vertices {
202 assert!((v.position[1] - 5.0).abs() < 1e-9, "got {}", v.position[1]);
203 }
204 }
205 #[test]
206 fn test_xpbd_no_constraint() {
207 let kernel = XPBDGpuKernel::new(4);
208 let mut pos = vec![[0.0f64; 3], [1.0, 0.0, 0.0]];
209 let inv_m = vec![1.0, 1.0];
210 kernel.solve_step(&mut pos, &inv_m, 0.01);
211 assert!((pos[0][0]).abs() < 1e-10);
212 assert!((pos[1][0] - 1.0).abs() < 1e-10);
213 }
214 #[test]
215 fn test_xpbd_distance_constraint_satisfied() {
216 let mut kernel = XPBDGpuKernel::new(20);
217 kernel.add_constraint(XpbdDistConstraint::new(0, 1, 1.0, 0.0));
218 let mut pos = vec![[0.0f64; 3], [2.0, 0.0, 0.0]];
219 let inv_m = vec![1.0, 1.0];
220 kernel.solve_step(&mut pos, &inv_m, 0.01);
221 let dist = length3(sub3(pos[1], pos[0]));
222 assert!((dist - 1.0).abs() < 1e-6, "dist = {dist}");
223 }
224 #[test]
225 fn test_xpbd_fixed_particle() {
226 let mut kernel = XPBDGpuKernel::new(10);
227 kernel.add_constraint(XpbdDistConstraint::new(0, 1, 1.0, 0.0));
228 let mut pos = vec![[0.0f64; 3], [5.0, 0.0, 0.0]];
229 let inv_m = vec![0.0, 1.0];
230 kernel.solve_step(&mut pos, &inv_m, 0.01);
231 assert!((pos[0][0]).abs() < 1e-10);
232 let dist = length3(sub3(pos[1], pos[0]));
233 assert!((dist - 1.0).abs() < 1e-6, "dist = {dist}");
234 }
235 #[test]
236 fn test_xpbd_integrate_gravity() {
237 let kernel = XPBDGpuKernel::new(1);
238 let mut pos = vec![[0.0f64; 3]];
239 let mut vel = vec![[0.0f64; 3]];
240 let inv_m = vec![1.0];
241 let gravity = [0.0, -9.81, 0.0];
242 let dt = 0.1;
243 kernel.integrate(&mut pos, &mut vel, &inv_m, gravity, dt);
244 assert!((vel[0][1] - (-9.81 * dt)).abs() < 1e-10);
245 assert!((pos[0][1] - (-9.81 * dt * dt)).abs() < 1e-10);
246 }
247 #[test]
248 fn test_xpbd_integrate_fixed_particle() {
249 let kernel = XPBDGpuKernel::new(1);
250 let mut pos = vec![[0.0f64; 3]];
251 let mut vel = vec![[0.0f64; 3]];
252 let inv_m = vec![0.0];
253 kernel.integrate(&mut pos, &mut vel, &inv_m, [0.0, -9.81, 0.0], 0.1);
254 assert!((pos[0][1]).abs() < 1e-10);
255 }
256 #[test]
257 fn test_fem_forces_at_rest() {
258 let mut fem = FEMGpuKernel::new(4);
259 let elem = TetElement::new([0, 1, 2, 3], 1.0 / 6.0, 1e6, 0.3);
260 fem.add_element(elem);
261 let rest = vec![
262 [0.0f64, 0.0, 0.0],
263 [1.0, 0.0, 0.0],
264 [0.0, 1.0, 0.0],
265 [0.0, 0.0, 1.0],
266 ];
267 let forces = fem.compute_forces(&rest, &rest);
268 for f in &forces {
269 let mag = length3(*f);
270 assert!(mag < 1e-10, "mag = {mag}");
271 }
272 }
273 #[test]
274 fn test_fem_forces_nonzero_when_stretched() {
275 let mut fem = FEMGpuKernel::new(4);
276 let elem = TetElement::new([0, 1, 2, 3], 1.0 / 6.0, 1e6, 0.3);
277 fem.add_element(elem);
278 let rest = vec![
279 [0.0f64, 0.0, 0.0],
280 [1.0, 0.0, 0.0],
281 [0.0, 1.0, 0.0],
282 [0.0, 0.0, 1.0],
283 ];
284 let mut deformed = rest.clone();
285 deformed[1] = [2.0, 0.0, 0.0];
286 let forces = fem.compute_forces(&deformed, &rest);
287 let total_mag: f64 = forces.iter().map(|f| length3(*f)).sum();
288 assert!(total_mag > 1e-6, "expected nonzero forces");
289 }
290 #[test]
291 fn test_fem_strain_energy_zero_at_rest() {
292 let mut fem = FEMGpuKernel::new(4);
293 let elem = TetElement::new([0, 1, 2, 3], 1.0, 1e5, 0.3);
294 fem.add_element(elem);
295 let rest = vec![
296 [0.0f64, 0.0, 0.0],
297 [1.0, 0.0, 0.0],
298 [0.0, 1.0, 0.0],
299 [0.0, 0.0, 1.0],
300 ];
301 let energy = fem.compute_strain_energy(&rest, &rest);
302 assert!(energy < 1e-10, "energy = {energy}");
303 }
304 #[test]
305 fn test_fem_lame_params() {
306 let elem = TetElement::new([0, 1, 2, 3], 1.0, 1e6, 0.25);
307 let (lambda, mu) = elem.lame_params();
308 assert!(lambda > 0.0);
309 assert!(mu > 0.0);
310 }
311 #[test]
312 fn test_collision_signed_distance_above() {
313 let cr = CollisionResponseGpu::new(CollisionResponseParams::default());
314 let sd = cr.signed_distance_to_triangle(
315 [0.0, 1.0, 0.0],
316 [0.0, 0.0, 0.0],
317 [1.0, 0.0, 0.0],
318 [0.0, 0.0, 1.0],
319 );
320 assert!(sd > 0.0, "sd = {sd}");
321 }
322 #[test]
323 fn test_collision_signed_distance_below() {
324 let cr = CollisionResponseGpu::new(CollisionResponseParams::default());
325 let sd = cr.signed_distance_to_triangle(
326 [0.0, -0.5, 0.0],
327 [0.0, 0.0, 0.0],
328 [1.0, 0.0, 0.0],
329 [0.0, 0.0, 1.0],
330 );
331 assert!(sd < 0.0, "sd = {sd}");
332 }
333 #[test]
334 fn test_collision_penalty_forces_below_threshold() {
335 let cr = CollisionResponseGpu::new(CollisionResponseParams::new(1e4, 10.0, 1.0));
336 let positions = vec![[0.5, -0.001, 0.5f64]];
337 let velocities = vec![[0.0f64; 3]];
338 let faces = vec![([0.0, 0.0, 0.0f64], [1.0, 0.0, 0.0f64], [0.0, 0.0, 1.0f64])];
339 let mut forces = vec![[0.0f64; 3]];
340 cr.apply_penalty_forces(&positions, &velocities, &faces, &mut forces);
341 assert!(forces[0][1] > 0.0, "expected upward penalty force");
342 }
343 #[test]
344 fn test_collision_penalty_forces_above_plane() {
345 let cr = CollisionResponseGpu::new(CollisionResponseParams::default());
346 let positions = vec![[0.5, 1.0, 0.5f64]];
347 let velocities = vec![[0.0f64; 3]];
348 let faces = vec![([0.0, 0.0, 0.0f64], [1.0, 0.0, 0.0f64], [0.0, 0.0, 1.0f64])];
349 let mut forces = vec![[0.0f64; 3]];
350 cr.apply_penalty_forces(&positions, &velocities, &faces, &mut forces);
351 let mag = length3(forces[0]);
352 assert!(mag < 1e-10, "expected no force, got mag = {mag}");
353 }
354 #[test]
355 fn test_collision_sphere_penalty_inside() {
356 let cr = CollisionResponseGpu::new(CollisionResponseParams::new(1e4, 0.0, 0.01));
357 let positions = vec![[0.0, 0.5, 0.0f64]];
358 let velocities = vec![[0.0f64; 3]];
359 let mut forces = vec![[0.0f64; 3]];
360 cr.apply_sphere_penalty(&positions, &velocities, [0.0, 0.0, 0.0], 1.0, &mut forces);
361 let mag = length3(forces[0]);
362 assert!(mag > 0.0, "expected sphere penalty force");
363 }
364 #[test]
365 fn test_collision_sphere_penalty_outside() {
366 let cr = CollisionResponseGpu::new(CollisionResponseParams::default());
367 let positions = vec![[0.0, 2.0, 0.0f64]];
368 let velocities = vec![[0.0f64; 3]];
369 let mut forces = vec![[0.0f64; 3]];
370 cr.apply_sphere_penalty(&positions, &velocities, [0.0, 0.0, 0.0], 1.0, &mut forces);
371 let mag = length3(forces[0]);
372 assert!(mag < 1e-10, "expected no sphere force");
373 }
374 #[test]
375 fn test_pipeline_creation() {
376 let mesh = make_simple_mesh(4);
377 let config = PhysicsPipelineConfig::default();
378 let pipeline = PhysicsGpuPipeline::new(config, mesh);
379 assert_eq!(pipeline.mesh.vertices.len(), 4);
380 assert!((pipeline.time).abs() < 1e-10);
381 }
382 #[test]
383 fn test_pipeline_step_advances_time() {
384 let mesh = make_simple_mesh(4);
385 let config = PhysicsPipelineConfig::default();
386 let mut pipeline = PhysicsGpuPipeline::new(config, mesh);
387 pipeline.step();
388 assert!(pipeline.time > 0.0);
389 }
390 #[test]
391 fn test_pipeline_gravity_falls() {
392 let mut mesh = DeformableGpuMesh::new();
393 let v = DeformableVertex::new([0.0, 10.0, 0.0]);
394 mesh.add_vertex(v);
395 let config = PhysicsPipelineConfig {
396 dt: 0.1,
397 substeps: 1,
398 xpbd_enabled: true,
399 fem_enabled: false,
400 collision_enabled: false,
401 ..Default::default()
402 };
403 let mut pipeline = PhysicsGpuPipeline::new(config, mesh);
404 pipeline.step();
405 assert!(
406 pipeline.mesh.vertices[0].position[1] < 10.0,
407 "y = {}",
408 pipeline.mesh.vertices[0].position[1]
409 );
410 }
411 #[test]
412 fn test_pipeline_centre_of_mass() {
413 let mut mesh = DeformableGpuMesh::new();
414 mesh.add_vertex(DeformableVertex::new([0.0, 0.0, 0.0]));
415 mesh.add_vertex(DeformableVertex::new([2.0, 0.0, 0.0]));
416 let config = PhysicsPipelineConfig::default();
417 let pipeline = PhysicsGpuPipeline::new(config, mesh);
418 let com = pipeline.centre_of_mass();
419 assert!((com[0] - 1.0).abs() < 1e-10);
420 }
421 #[test]
422 fn test_pipeline_kinetic_energy_zero_at_rest() {
423 let mesh = make_simple_mesh(4);
424 let config = PhysicsPipelineConfig::default();
425 let pipeline = PhysicsGpuPipeline::new(config, mesh);
426 assert!((pipeline.kinetic_energy()).abs() < 1e-10);
427 }
428 #[test]
429 fn test_pipeline_xpbd_constraint_spring() {
430 let mut mesh = DeformableGpuMesh::new();
431 let mut v0 = DeformableVertex::new([0.0, 0.0, 0.0]);
432 v0.mass = 1e30;
433 mesh.add_vertex(v0);
434 let v1 = DeformableVertex::new([5.0, 0.0, 0.0]);
435 mesh.add_vertex(v1);
436 let config = PhysicsPipelineConfig {
437 gravity: [0.0; 3],
438 dt: 0.01,
439 substeps: 10,
440 collision_enabled: false,
441 fem_enabled: false,
442 xpbd_enabled: true,
443 };
444 let mut pipeline = PhysicsGpuPipeline::new(config, mesh);
445 pipeline
446 .xpbd
447 .add_constraint(XpbdDistConstraint::new(0, 1, 1.0, 0.0));
448 pipeline.step();
449 let dx = pipeline.mesh.vertices[1].position[0];
450 assert!(dx < 5.0, "particle should move, dx = {dx}");
451 }
452 #[test]
453 fn test_xpbd_multiple_constraints() {
454 let mut kernel = XPBDGpuKernel::new(50);
455 kernel.add_constraint(XpbdDistConstraint::new(0, 1, 1.0, 0.0));
456 kernel.add_constraint(XpbdDistConstraint::new(1, 2, 1.0, 0.0));
457 let mut pos = vec![[0.0f64; 3], [3.0, 0.0, 0.0], [6.0, 0.0, 0.0]];
458 let inv_m = vec![0.0, 1.0, 1.0];
459 kernel.solve_step(&mut pos, &inv_m, 0.01);
460 let d01 = length3(sub3(pos[1], pos[0]));
461 let d12 = length3(sub3(pos[2], pos[1]));
462 assert!((d01 - 1.0).abs() < 1e-5, "d01 = {d01}");
463 assert!((d12 - 1.0).abs() < 1e-5, "d12 = {d12}");
464 }
465 #[test]
466 fn test_blend_shape_partial_weight() {
467 let mut mesh = DeformableGpuMesh::new();
468 mesh.add_vertex(DeformableVertex::new([0.0, 0.0, 0.0]));
469 let bs = BlendShape::new("half", vec![[0.0, 2.0, 0.0]]);
470 let idx = mesh.add_blend_shape(bs);
471 mesh.blend_shapes[idx].weight = 0.5;
472 mesh.apply_blend_shapes();
473 assert!((mesh.vertices[0].position[1] - 1.0).abs() < 1e-10);
474 }
475 #[test]
476 fn test_fem_add_element_updates_num_nodes() {
477 let mut fem = FEMGpuKernel::new(0);
478 fem.add_element(TetElement::new([0, 1, 2, 10], 1.0, 1e5, 0.3));
479 assert!(fem.num_nodes >= 11);
480 }
481 #[test]
482 fn test_collision_params_default() {
483 let p = CollisionResponseParams::default();
484 assert!(p.penalty_stiffness > 0.0);
485 assert!(p.damping >= 0.0);
486 assert!(p.contact_threshold > 0.0);
487 }
488 #[test]
489 fn test_deformable_vertex_default_weights() {
490 let v = DeformableVertex::new([1.0, 2.0, 3.0]);
491 assert!((v.joint_weights[0] - 1.0).abs() < 1e-10);
492 assert!((v.joint_weights[1]).abs() < 1e-10);
493 }
494 #[test]
495 fn test_quat_rotate_identity() {
496 let q = [1.0, 0.0, 0.0, 0.0];
497 let p = [3.0, 4.0, 5.0];
498 let r = quat_rotate(q, p);
499 for k in 0..3 {
500 assert!((r[k] - p[k]).abs() < 1e-9);
501 }
502 }
503 #[test]
504 fn test_skinning_mode_eq() {
505 assert_eq!(SkinningMode::LinearBlend, SkinningMode::LinearBlend);
506 assert_ne!(SkinningMode::LinearBlend, SkinningMode::DualQuaternion);
507 }
508 #[test]
509 fn test_pipeline_multiple_steps() {
510 let mesh = make_simple_mesh(2);
511 let config = PhysicsPipelineConfig {
512 gravity: [0.0, -9.81, 0.0],
513 dt: 0.016,
514 substeps: 2,
515 collision_enabled: false,
516 fem_enabled: false,
517 xpbd_enabled: true,
518 };
519 let mut pipeline = PhysicsGpuPipeline::new(config, mesh);
520 for _ in 0..10 {
521 pipeline.step();
522 }
523 assert!((pipeline.time - 0.16).abs() < 1e-6);
524 }
525 #[test]
526 fn test_gpu_node_new() {
527 let node = DeformableGpuNode::new([1.0, 2.0, 3.0], 2.5_f64);
528 assert!((node.mass - 2.5_f64).abs() < 1e-10);
529 assert_eq!(node.vel, [0.0; 3]);
530 assert_eq!(node.force, [0.0; 3]);
531 }
532 #[test]
533 fn test_gpu_node_apply_force() {
534 let mut node = DeformableGpuNode::new([0.0; 3], 1.0_f64);
535 node.apply_force([1.0, 0.0, 0.0]);
536 node.apply_force([0.0, 2.0, 0.0]);
537 assert!((node.force[0] - 1.0_f64).abs() < 1e-10);
538 assert!((node.force[1] - 2.0_f64).abs() < 1e-10);
539 }
540 #[test]
541 fn test_gpu_node_reset_force() {
542 let mut node = DeformableGpuNode::new([0.0; 3], 1.0_f64);
543 node.apply_force([5.0, 5.0, 5.0]);
544 node.reset_force();
545 assert_eq!(node.force, [0.0; 3]);
546 }
547 #[test]
548 fn test_gpu_node_kinetic_energy() {
549 let mut node = DeformableGpuNode::new([0.0; 3], 2.0_f64);
550 node.vel = [3.0, 4.0, 0.0];
551 assert!((node.kinetic_energy() - 25.0_f64).abs() < 1e-10);
552 }
553 #[test]
554 fn test_gpu_buffer_push_len() {
555 let mut buf = DeformableGpuBuffer::new();
556 buf.push([0.0; 3], [0.0; 3], 1.0_f64);
557 buf.push([1.0, 0.0, 0.0], [0.0; 3], 1.0_f64);
558 assert_eq!(buf.len(), 2);
559 assert!(!buf.is_empty());
560 }
561 #[test]
562 fn test_gpu_buffer_from_nodes() {
563 let nodes = vec![
564 DeformableGpuNode::new([0.0; 3], 1.0_f64),
565 DeformableGpuNode::new([1.0, 0.0, 0.0], 2.0_f64),
566 ];
567 let buf = DeformableGpuBuffer::from_nodes(&nodes);
568 assert_eq!(buf.len(), 2);
569 assert!((buf.masses[1] - 2.0_f64).abs() < 1e-10);
570 assert!((buf.inv_masses[1] - 0.5_f64).abs() < 1e-10);
571 }
572 #[test]
573 fn test_gpu_buffer_reset_forces() {
574 let mut buf = DeformableGpuBuffer::new();
575 buf.push([0.0; 3], [0.0; 3], 1.0_f64);
576 buf.forces[0] = [5.0, 5.0, 5.0];
577 buf.reset_forces();
578 assert_eq!(buf.forces[0], [0.0; 3]);
579 }
580 #[test]
581 fn test_gpu_buffer_apply_gravity() {
582 let mut buf = DeformableGpuBuffer::new();
583 buf.push([0.0; 3], [0.0; 3], 2.0_f64);
584 buf.apply_gravity([0.0, -9.81_f64, 0.0]);
585 assert!((buf.forces[0][1] - (-19.62_f64)).abs() < 1e-5);
586 }
587 #[test]
588 fn test_gpu_buffer_integrate() {
589 let mut buf = DeformableGpuBuffer::new();
590 buf.push([0.0; 3], [0.0; 3], 1.0_f64);
591 buf.forces[0] = [0.0, -9.81_f64, 0.0];
592 buf.integrate(0.1_f64);
593 assert!((buf.velocities[0][1] - (-0.981_f64)).abs() < 1e-6);
594 }
595 #[test]
596 fn test_gpu_buffer_total_ke() {
597 let mut buf = DeformableGpuBuffer::new();
598 buf.push([0.0; 3], [1.0, 0.0, 0.0], 2.0_f64);
599 assert!((buf.total_kinetic_energy() - 1.0_f64).abs() < 1e-10);
600 }
601 #[test]
602 fn test_gpu_buffer_flatten_positions() {
603 let mut buf = DeformableGpuBuffer::new();
604 buf.push([1.0, 2.0, 3.0], [0.0; 3], 1.0_f64);
605 let flat = buf.flatten_positions();
606 assert_eq!(flat, vec![1.0, 2.0, 3.0]);
607 }
608 #[test]
609 fn test_gpu_buffer_fixed_particle_inv_mass() {
610 let mut buf = DeformableGpuBuffer::new();
611 buf.push([0.0; 3], [0.0; 3], 0.0_f64);
612 assert!((buf.inv_masses[0]).abs() < 1e-15);
613 }
614 #[test]
615 fn test_xpbd_solver_distance_constraint() {
616 let mut solver = XPBDGpuSolver::new(20);
617 solver.add_distance(0, 1, 1.0_f64, 0.0_f64);
618 let mut buf = DeformableGpuBuffer::new();
619 buf.push([0.0; 3], [0.0; 3], 1.0_f64);
620 buf.push([3.0, 0.0, 0.0], [0.0; 3], 1.0_f64);
621 solver.solve(&mut buf, 0.01_f64);
622 let dist = length3(sub3(buf.positions[1], buf.positions[0]));
623 assert!((dist - 1.0_f64).abs() < 0.01_f64, "dist = {dist}");
624 }
625 #[test]
626 fn test_xpbd_solver_no_constraint_unchanged() {
627 let solver = XPBDGpuSolver::new(5);
628 let mut buf = DeformableGpuBuffer::new();
629 buf.push([0.0; 3], [0.0; 3], 1.0_f64);
630 buf.push([2.0, 0.0, 0.0], [0.0; 3], 1.0_f64);
631 solver.solve(&mut buf, 0.01_f64);
632 assert!((buf.positions[0][0]).abs() < 1e-10);
633 assert!((buf.positions[1][0] - 2.0_f64).abs() < 1e-10);
634 }
635 #[test]
636 fn test_xpbd_solver_shape_matching() {
637 let mut solver = XPBDGpuSolver::new(20);
638 let rest = vec![[0.0; 3], [1.0, 0.0, 0.0]];
639 solver.add_shape(ShapeMatchConstraint::new(vec![0, 1], rest, 1.0_f64));
640 let mut buf = DeformableGpuBuffer::new();
641 buf.push([0.0; 3], [0.0; 3], 1.0_f64);
642 buf.push([3.0, 0.0, 0.0], [0.0; 3], 1.0_f64);
643 solver.solve(&mut buf, 0.01_f64);
644 let gap = (buf.positions[1][0] - buf.positions[0][0]).abs();
645 assert!(gap < 3.0_f64, "gap = {gap}");
646 }
647 #[test]
648 fn test_fem_solver_rest_zero_energy() {
649 let rest = [
650 [0.0, 0.0, 0.0f64],
651 [1.0, 0.0, 0.0],
652 [0.0, 1.0, 0.0],
653 [0.0, 0.0, 1.0],
654 ];
655 let elem = CorotationalElement::from_rest([0, 1, 2, 3], &rest, 1e6_f64, 0.3_f64);
656 let mut solver = FEMGpuSolver::new(4);
657 solver.add_element(elem);
658 let mut buf = DeformableGpuBuffer::new();
659 for &p in &rest {
660 buf.push(p, [0.0; 3], 1.0_f64);
661 }
662 let energy = solver.strain_energy(&buf);
663 assert!(energy < 1e-5_f64, "energy = {energy}");
664 }
665 #[test]
666 fn test_fem_solver_assemble_forces_at_rest() {
667 let rest = [
668 [0.0, 0.0, 0.0f64],
669 [1.0, 0.0, 0.0],
670 [0.0, 1.0, 0.0],
671 [0.0, 0.0, 1.0],
672 ];
673 let elem = CorotationalElement::from_rest([0, 1, 2, 3], &rest, 1e5_f64, 0.3_f64);
674 let mut solver = FEMGpuSolver::new(4);
675 solver.add_element(elem);
676 let mut buf = DeformableGpuBuffer::new();
677 for &p in &rest {
678 buf.push(p, [0.0; 3], 1.0_f64);
679 }
680 buf.reset_forces();
681 solver.assemble_forces(&mut buf);
682 for i in 0..4 {
683 let mag = length3(buf.forces[i]);
684 assert!(mag < 1e-5_f64, "node {i} force mag = {mag}");
685 }
686 }
687 #[test]
688 fn test_fem_corotational_element_lame() {
689 let rest = [[0.0; 3], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
690 let elem = CorotationalElement::from_rest([0, 1, 2, 3], &rest, 1e6_f64, 0.25_f64);
691 let (lambda, mu) = elem.lame();
692 assert!(lambda > 0.0_f64);
693 assert!(mu > 0.0_f64);
694 }
695 #[test]
696 fn test_fem_solver_num_nodes_update() {
697 let rest = [[0.0; 3]; 4];
698 let elem = CorotationalElement::from_rest([0, 1, 9, 3], &rest, 1e5_f64, 0.3_f64);
699 let mut solver = FEMGpuSolver::new(0);
700 solver.add_element(elem);
701 assert!(solver.num_nodes >= 10);
702 }
703 #[test]
704 fn test_gpu_collision_plane_detect() {
705 let cr = GPUCollisionResponse::default_params();
706 let positions = vec![[0.0, -0.01_f64, 0.0]];
707 let contacts = cr.detect_plane_contacts(&positions, [0.0, 1.0, 0.0], 0.0_f64);
708 assert!(!contacts.is_empty());
709 assert_eq!(contacts[0].particle_idx, 0);
710 }
711 #[test]
712 fn test_gpu_collision_plane_no_contact_above() {
713 let cr = GPUCollisionResponse::default_params();
714 let positions = vec![[0.0, 1.0_f64, 0.0]];
715 let contacts = cr.detect_plane_contacts(&positions, [0.0, 1.0, 0.0], 0.0_f64);
716 assert!(contacts.is_empty());
717 }
718 #[test]
719 fn test_gpu_collision_sphere_detect() {
720 let cr = GPUCollisionResponse::default_params();
721 let positions = vec![[0.5_f64, 0.0, 0.0]];
722 let contacts = cr.detect_sphere_contacts(&positions, [0.0; 3], 1.0_f64);
723 assert!(!contacts.is_empty());
724 }
725 #[test]
726 fn test_gpu_collision_resolve_positions() {
727 let cr = GPUCollisionResponse::default_params();
728 let mut buf = DeformableGpuBuffer::new();
729 buf.push([0.0, -0.05_f64, 0.0], [0.0; 3], 1.0_f64);
730 let contacts = cr.detect_plane_contacts(&buf.positions, [0.0, 1.0, 0.0], 0.0_f64);
731 cr.resolve_positions(&mut buf, &contacts);
732 assert!(
733 buf.positions[0][1] >= 0.0_f64 - 1e-10,
734 "y = {}",
735 buf.positions[0][1]
736 );
737 }
738 #[test]
739 fn test_gpu_collision_resolve_velocities() {
740 let cr = GPUCollisionResponse::new(1e4_f64, 10.0_f64, 0.01_f64, 0.5_f64);
741 let mut buf = DeformableGpuBuffer::new();
742 buf.push([0.0, -0.001_f64, 0.0], [0.0, -2.0_f64, 0.0], 1.0_f64);
743 let contacts = cr.detect_plane_contacts(&buf.positions, [0.0, 1.0, 0.0], 0.0_f64);
744 cr.resolve_velocities(&mut buf, &contacts);
745 assert!(
746 buf.velocities[0][1] > 0.0_f64,
747 "vel = {}",
748 buf.velocities[0][1]
749 );
750 }
751 #[test]
752 fn test_deformable_pipeline_creation() {
753 let pipeline = DeformableGpuPipeline::new([0.0, -9.81_f64, 0.0], 1.0 / 60.0_f64, 4);
754 assert_eq!(pipeline.buf.len(), 0);
755 assert!((pipeline.time).abs() < 1e-15);
756 }
757 #[test]
758 fn test_deformable_pipeline_add_particle() {
759 let mut pipeline = DeformableGpuPipeline::new([0.0; 3], 0.016_f64, 2);
760 let idx = pipeline.add_particle([0.0, 5.0_f64, 0.0], 1.0_f64);
761 assert_eq!(idx, 0);
762 assert_eq!(pipeline.buf.len(), 1);
763 }
764 #[test]
765 fn test_deformable_pipeline_step_advances_time() {
766 let mut pipeline = DeformableGpuPipeline::new([0.0, -9.81_f64, 0.0], 0.016_f64, 2);
767 pipeline.add_particle([0.0, 10.0_f64, 0.0], 1.0_f64);
768 pipeline.step();
769 assert!(pipeline.time > 0.0_f64);
770 }
771 #[test]
772 fn test_deformable_pipeline_gravity_falls() {
773 let mut pipeline = DeformableGpuPipeline::new([0.0, -9.81_f64, 0.0], 0.1_f64, 1);
774 pipeline.collision_enabled = false;
775 pipeline.xpbd_enabled = false;
776 pipeline.fem_enabled = false;
777 pipeline.add_particle([0.0, 10.0_f64, 0.0], 1.0_f64);
778 pipeline.step();
779 assert!(
780 pipeline.buf.positions[0][1] < 10.0_f64,
781 "y = {}",
782 pipeline.buf.positions[0][1]
783 );
784 }
785 #[test]
786 fn test_deformable_pipeline_kinetic_energy() {
787 let mut pipeline = DeformableGpuPipeline::new([0.0; 3], 0.016_f64, 1);
788 pipeline.add_particle([0.0; 3], 1.0_f64);
789 assert!((pipeline.kinetic_energy()).abs() < 1e-15);
790 }
791 #[test]
792 fn test_deformable_pipeline_centre_of_mass() {
793 let mut pipeline = DeformableGpuPipeline::new([0.0; 3], 0.016_f64, 1);
794 pipeline.add_particle([0.0; 3], 1.0_f64);
795 pipeline.add_particle([2.0, 0.0, 0.0], 1.0_f64);
796 let com = pipeline.centre_of_mass();
797 assert!((com[0] - 1.0_f64).abs() < 1e-10, "com.x = {}", com[0]);
798 }
799 #[test]
800 fn test_deformable_pipeline_empty_com() {
801 let pipeline = DeformableGpuPipeline::new([0.0; 3], 0.016_f64, 1);
802 let com = pipeline.centre_of_mass();
803 assert_eq!(com, [0.0; 3]);
804 }
805 #[test]
806 fn test_xpbd_solver_with_buffer_distance() {
807 let mut solver = XPBDGpuSolver::new(30);
808 solver.add_distance(0, 1, 1.0_f64, 0.0_f64);
809 let mut buf = DeformableGpuBuffer::new();
810 buf.push([0.0; 3], [0.0; 3], 0.0_f64);
811 buf.push([5.0, 0.0, 0.0], [0.0; 3], 1.0_f64);
812 solver.solve(&mut buf, 0.01_f64);
813 let dist = length3(sub3(buf.positions[1], buf.positions[0]));
814 assert!((dist - 1.0_f64).abs() < 0.01_f64, "dist = {dist}");
815 }
816 #[test]
817 fn test_gpu_buffer_fixed_particle_doesnt_move() {
818 let mut buf = DeformableGpuBuffer::new();
819 buf.push([0.0; 3], [0.0; 3], 0.0_f64);
820 buf.forces[0] = [100.0, 100.0, 100.0];
821 buf.integrate(0.1_f64);
822 assert_eq!(buf.positions[0], [0.0; 3]);
823 }
824}