oxiphysics_gpu/kernels/rigid/
functions.rs

1//! Auto-generated module
2//!
3//! 🤖 Generated with [SplitRS](https://github.com/cool-japan/splitrs)
4
5use super::types::{AccumulatedImpulse, RigidBodyState};
6
7/// Quaternion multiplication: `q_out = q_a * q_b`.
8///
9/// Both quaternions are `[qx, qy, qz, qw]`.
10pub(super) fn quat_mul(a: [f64; 4], b: [f64; 4]) -> [f64; 4] {
11    let [ax, ay, az, aw] = a;
12    let [bx, by, bz, bw] = b;
13    [
14        aw * bx + ax * bw + ay * bz - az * by,
15        aw * by - ax * bz + ay * bw + az * bx,
16        aw * bz + ax * by - ay * bx + az * bw,
17        aw * bw - ax * bx - ay * by - az * bz,
18    ]
19}
20/// Normalise a quaternion to unit length.
21pub(super) fn quat_normalise(q: [f64; 4]) -> [f64; 4] {
22    let len = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
23    if len < 1e-30 {
24        return [0.0, 0.0, 0.0, 1.0];
25    }
26    [q[0] / len, q[1] / len, q[2] / len, q[3] / len]
27}
28/// Compute dq/dt = 0.5 * \[omega, 0\] * q (pure quaternion derivative).
29pub(super) fn quat_derivative(q: [f64; 4], omega: [f64; 3]) -> [f64; 4] {
30    let omega_q = [omega[0], omega[1], omega[2], 0.0];
31    let dq = quat_mul(omega_q, q);
32    [dq[0] * 0.5, dq[1] * 0.5, dq[2] * 0.5, dq[3] * 0.5]
33}
34/// Rotate a vector by a quaternion: v' = q * v * q^{-1}.
35pub(super) fn quat_rotate(q: [f64; 4], v: [f64; 3]) -> [f64; 3] {
36    let [qx, qy, qz, qw] = q;
37    let cx = qy * v[2] - qz * v[1];
38    let cy = qz * v[0] - qx * v[2];
39    let cz = qx * v[1] - qy * v[0];
40    [
41        v[0] + 2.0 * (qw * cx + qy * cz - qz * cy),
42        v[1] + 2.0 * (qw * cy + qz * cx - qx * cz),
43        v[2] + 2.0 * (qw * cz + qx * cy - qy * cx),
44    ]
45}
46/// Euler-step all rigid bodies by `dt` given external forces and torques.
47pub fn integrate_euler(
48    states: &mut [RigidBodyState],
49    forces: &[[f64; 3]],
50    torques: &[[f64; 3]],
51    dt: f64,
52) {
53    for (s, (f, tau)) in states.iter_mut().zip(forces.iter().zip(torques.iter())) {
54        let acc = [
55            f[0] * s.inverse_mass,
56            f[1] * s.inverse_mass,
57            f[2] * s.inverse_mass,
58        ];
59        s.velocity[0] += acc[0] * dt;
60        s.velocity[1] += acc[1] * dt;
61        s.velocity[2] += acc[2] * dt;
62        s.position[0] += s.velocity[0] * dt;
63        s.position[1] += s.velocity[1] * dt;
64        s.position[2] += s.velocity[2] * dt;
65        let alpha = [
66            tau[0] * s.inverse_mass,
67            tau[1] * s.inverse_mass,
68            tau[2] * s.inverse_mass,
69        ];
70        s.angular_velocity[0] += alpha[0] * dt;
71        s.angular_velocity[1] += alpha[1] * dt;
72        s.angular_velocity[2] += alpha[2] * dt;
73        let dq = quat_derivative(s.orientation, s.angular_velocity);
74        s.orientation[0] += dq[0] * dt;
75        s.orientation[1] += dq[1] * dt;
76        s.orientation[2] += dq[2] * dt;
77        s.orientation[3] += dq[3] * dt;
78        s.orientation = quat_normalise(s.orientation);
79    }
80}
81/// RK4 integration of a single rigid body for one time step.
82pub fn integrate_rk4(
83    state: &RigidBodyState,
84    force: [f64; 3],
85    torque: [f64; 3],
86    dt: f64,
87) -> RigidBodyState {
88    let deriv = |s: &RigidBodyState| -> ([f64; 3], [f64; 3], [f64; 4], [f64; 3]) {
89        let dpos = s.velocity;
90        let dvel = [
91            force[0] * s.inverse_mass,
92            force[1] * s.inverse_mass,
93            force[2] * s.inverse_mass,
94        ];
95        let dq = quat_derivative(s.orientation, s.angular_velocity);
96        let domega = [
97            torque[0] * s.inverse_mass,
98            torque[1] * s.inverse_mass,
99            torque[2] * s.inverse_mass,
100        ];
101        (dpos, dvel, dq, domega)
102    };
103    let step = |s: &RigidBodyState,
104                dp: [f64; 3],
105                dv: [f64; 3],
106                dq: [f64; 4],
107                dw: [f64; 3],
108                h: f64|
109     -> RigidBodyState {
110        let mut ns = *s;
111        for (k, (&dp_k, (&dv_k, &dw_k))) in dp.iter().zip(dv.iter().zip(dw.iter())).enumerate() {
112            ns.position[k] = s.position[k] + dp_k * h;
113            ns.velocity[k] = s.velocity[k] + dv_k * h;
114            ns.angular_velocity[k] = s.angular_velocity[k] + dw_k * h;
115        }
116        for (k, &dq_k) in dq.iter().enumerate() {
117            ns.orientation[k] = s.orientation[k] + dq_k * h;
118        }
119        ns.orientation = quat_normalise(ns.orientation);
120        ns
121    };
122    let (dp1, dv1, dq1, dw1) = deriv(state);
123    let s2 = step(state, dp1, dv1, dq1, dw1, dt * 0.5);
124    let (dp2, dv2, dq2, dw2) = deriv(&s2);
125    let s3 = step(state, dp2, dv2, dq2, dw2, dt * 0.5);
126    let (dp3, dv3, dq3, dw3) = deriv(&s3);
127    let s4 = step(state, dp3, dv3, dq3, dw3, dt);
128    let (dp4, dv4, dq4, dw4) = deriv(&s4);
129    let mut out = *state;
130    for k in 0..3 {
131        out.position[k] =
132            state.position[k] + dt / 6.0 * (dp1[k] + 2.0 * dp2[k] + 2.0 * dp3[k] + dp4[k]);
133        out.velocity[k] =
134            state.velocity[k] + dt / 6.0 * (dv1[k] + 2.0 * dv2[k] + 2.0 * dv3[k] + dv4[k]);
135        out.angular_velocity[k] =
136            state.angular_velocity[k] + dt / 6.0 * (dw1[k] + 2.0 * dw2[k] + 2.0 * dw3[k] + dw4[k]);
137    }
138    for k in 0..4 {
139        out.orientation[k] =
140            state.orientation[k] + dt / 6.0 * (dq1[k] + 2.0 * dq2[k] + 2.0 * dq3[k] + dq4[k]);
141    }
142    out.orientation = quat_normalise(out.orientation);
143    out
144}
145#[cfg(test)]
146mod tests {
147    use super::*;
148    use crate::compute::ComputeKernel;
149    use crate::kernels::rigid::Aabb;
150
151    use crate::kernels::rigid::BroadphaseUpdateKernel;
152    use crate::kernels::rigid::ConstraintSolverKernel;
153
154    use crate::kernels::rigid::ContactGenerationKernel;
155    use crate::kernels::rigid::ContactPoint;
156    use crate::kernels::rigid::DistanceConstraint;
157    use crate::kernels::rigid::IntegratePositionKernel;
158    use crate::kernels::rigid::IntegrateVelocityKernel;
159    use crate::kernels::rigid::IslandSolver;
160    use crate::kernels::rigid::QuaternionNormKernel;
161    use crate::kernels::rigid::RigidBodyState;
162    use crate::kernels::rigid::SemiImplicitEulerKernel;
163
164    use crate::kernels::rigid::SoaRigidBody;
165
166    #[test]
167    fn test_rigid_gravity_integration() {
168        let n = 10_usize;
169        let g = -9.81_f64;
170        let dt = 0.1_f64;
171        let vel = vec![0.0_f64; n * 3];
172        let mut force = vec![0.0_f64; n * 3];
173        for i in 0..n {
174            force[i * 3 + 1] = g;
175        }
176        let inv_mass = vec![1.0_f64; n];
177        let dt_slice = vec![dt];
178        let mut outputs = vec![Vec::new()];
179        IntegrateVelocityKernel.execute(&[&vel, &force, &inv_mass, &dt_slice], &mut outputs, n);
180        assert_eq!(outputs[0].len(), n * 3, "output length should be 3n");
181        let expected_vy = g * dt;
182        for i in 0..n {
183            let vx = outputs[0][i * 3];
184            let vy = outputs[0][i * 3 + 1];
185            let vz = outputs[0][i * 3 + 2];
186            assert!(vx.abs() < 1e-15, "body {i}: vx should be 0, got {vx}");
187            assert!(
188                (vy - expected_vy).abs() < 1e-12,
189                "body {i}: vy should be {expected_vy}, got {vy}"
190            );
191            assert!(vz.abs() < 1e-15, "body {i}: vz should be 0, got {vz}");
192        }
193    }
194    #[test]
195    fn integrate_velocity_updates_correctly() {
196        let vel = vec![1.0, 0.0, 0.0];
197        let force = vec![10.0, 0.0, 0.0];
198        let inv_mass = vec![0.5];
199        let dt = vec![0.1];
200        let mut outputs = vec![Vec::new()];
201        IntegrateVelocityKernel.execute(&[&vel, &force, &inv_mass, &dt], &mut outputs, 1);
202        assert!((outputs[0][0] - 1.5).abs() < 1e-12);
203        assert!((outputs[0][1]).abs() < 1e-12);
204        assert!((outputs[0][2]).abs() < 1e-12);
205    }
206    #[test]
207    fn integrate_position_updates_correctly() {
208        let pos = vec![0.0, 0.0, 0.0];
209        let vel = vec![3.0, 4.0, 0.0];
210        let dt = vec![0.5];
211        let mut outputs = vec![Vec::new()];
212        IntegratePositionKernel.execute(&[&pos, &vel, &dt], &mut outputs, 1);
213        assert!((outputs[0][0] - 1.5).abs() < 1e-12);
214        assert!((outputs[0][1] - 2.0).abs() < 1e-12);
215        assert!((outputs[0][2]).abs() < 1e-12);
216    }
217    #[test]
218    fn integrate_euler_free_fall() {
219        let g = -9.81_f64;
220        let dt = 0.1_f64;
221        let mut state = RigidBodyState::at_rest([0.0, 0.0, 0.0], 1.0);
222        let force = [0.0, g, 0.0];
223        let torque = [0.0; 3];
224        integrate_euler(std::slice::from_mut(&mut state), &[force], &[torque], dt);
225        let expected_vy = g * dt;
226        assert!((state.velocity[1] - expected_vy).abs() < 1e-12);
227        let expected_y = expected_vy * dt;
228        assert!((state.position[1] - expected_y).abs() < 1e-12);
229        assert!(state.position[0].abs() < 1e-15);
230        assert!(state.position[2].abs() < 1e-15);
231    }
232    #[test]
233    fn integrate_rk4_free_fall() {
234        let g = -9.81_f64;
235        let dt = 0.1_f64;
236        let state = RigidBodyState::at_rest([0.0, 0.0, 0.0], 1.0);
237        let force = [0.0, g, 0.0];
238        let torque = [0.0; 3];
239        let new_state = integrate_rk4(&state, force, torque, dt);
240        let expected_vy = g * dt;
241        let expected_y = 0.5 * g * dt * dt;
242        assert!((new_state.velocity[1] - expected_vy).abs() < 1e-10);
243        assert!((new_state.position[1] - expected_y).abs() < 1e-10);
244    }
245    #[test]
246    fn integrate_euler_orientation_stays_normalised() {
247        let mut state = RigidBodyState::at_rest([0.0, 0.0, 0.0], 1.0);
248        let torque = [0.0, 0.0, 1.0];
249        let force = [0.0; 3];
250        let dt = 0.01;
251        for _ in 0..100 {
252            integrate_euler(std::slice::from_mut(&mut state), &[force], &[torque], dt);
253        }
254        let q = state.orientation;
255        let len = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
256        assert!((len - 1.0).abs() < 1e-10, "quaternion norm = {len}");
257    }
258    /// Distance constraint should pull bodies together.
259    #[test]
260    fn test_distance_constraint_converges() {
261        let mut positions = [[0.0, 0.0, 0.0], [3.0, 0.0, 0.0]];
262        let inv_masses = [1.0, 1.0];
263        let constraints = [DistanceConstraint {
264            body_a: 0,
265            body_b: 1,
266            rest_length: 1.0,
267            compliance: 0.0,
268        }];
269        for _ in 0..10 {
270            ConstraintSolverKernel::solve_distance_constraints(
271                &mut positions,
272                &inv_masses,
273                &constraints,
274                0.01,
275            );
276        }
277        let dx = positions[1][0] - positions[0][0];
278        let dist = dx.abs();
279        assert!(
280            (dist - 1.0).abs() < 0.1,
281            "distance should approach 1.0, got {dist}"
282        );
283    }
284    /// Static body should not move under constraint.
285    #[test]
286    fn test_constraint_static_body() {
287        let mut positions = [[0.0, 0.0, 0.0], [3.0, 0.0, 0.0]];
288        let inv_masses = [0.0, 1.0];
289        let constraints = [DistanceConstraint {
290            body_a: 0,
291            body_b: 1,
292            rest_length: 1.0,
293            compliance: 0.0,
294        }];
295        ConstraintSolverKernel::solve_distance_constraints(
296            &mut positions,
297            &inv_masses,
298            &constraints,
299            0.01,
300        );
301        assert!((positions[0][0]).abs() < 1e-14, "static body moved!");
302    }
303    /// Overlapping AABBs should be detected.
304    #[test]
305    fn test_aabb_overlap() {
306        let a = Aabb::from_center([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
307        let b = Aabb::from_center([1.5, 0.0, 0.0], [1.0, 1.0, 1.0]);
308        assert!(a.overlaps(&b));
309    }
310    /// Separated AABBs should not overlap.
311    #[test]
312    fn test_aabb_no_overlap() {
313        let a = Aabb::from_center([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
314        let b = Aabb::from_center([5.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
315        assert!(!a.overlaps(&b));
316    }
317    /// Broadphase should find overlapping pairs.
318    #[test]
319    fn test_broadphase_finds_pairs() {
320        let positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [10.0, 0.0, 0.0]];
321        let half_extents = [[1.0, 1.0, 1.0]; 3];
322        let pairs = BroadphaseUpdateKernel::find_overlapping_pairs(&positions, &half_extents, 0.0);
323        assert!(pairs.contains(&(0, 1)), "bodies 0 and 1 should overlap");
324        assert!(
325            !pairs.contains(&(0, 2)),
326            "bodies 0 and 2 should not overlap"
327        );
328    }
329    /// AABB volume.
330    #[test]
331    fn test_aabb_volume() {
332        let a = Aabb::from_center([0.0, 0.0, 0.0], [1.0, 2.0, 3.0]);
333        let vol = a.volume();
334        assert!((vol - 48.0).abs() < 1e-12, "volume = {vol}, expected 48");
335    }
336    /// Two separate pairs should form two islands.
337    #[test]
338    fn test_island_solver_two_islands() {
339        let constraints = [(0, 1), (2, 3)];
340        let islands = IslandSolver::build(4, &constraints);
341        assert_eq!(islands.num_islands, 2);
342        assert_eq!(islands.island_ids[0], islands.island_ids[1]);
343        assert_eq!(islands.island_ids[2], islands.island_ids[3]);
344        assert_ne!(islands.island_ids[0], islands.island_ids[2]);
345    }
346    /// Connected chain should form one island.
347    #[test]
348    fn test_island_solver_chain() {
349        let constraints = [(0, 1), (1, 2), (2, 3)];
350        let islands = IslandSolver::build(4, &constraints);
351        assert_eq!(islands.num_islands, 1);
352        let all_same = islands
353            .island_ids
354            .iter()
355            .all(|&id| id == islands.island_ids[0]);
356        assert!(all_same, "all bodies should be in the same island");
357    }
358    /// Bodies in island query.
359    #[test]
360    fn test_bodies_in_island() {
361        let constraints = [(0, 1), (2, 3)];
362        let islands = IslandSolver::build(4, &constraints);
363        let island_0 = islands.bodies_in_island(islands.island_ids[0]);
364        assert_eq!(island_0.len(), 2);
365        assert!(island_0.contains(&0));
366        assert!(island_0.contains(&1));
367    }
368    /// Overlapping spheres should generate a contact.
369    #[test]
370    fn test_contact_generation_overlap() {
371        let positions = [[0.0, 0.0, 0.0], [1.5, 0.0, 0.0]];
372        let radii = [1.0, 1.0];
373        let pairs = [(0, 1)];
374        let contacts =
375            ContactGenerationKernel::generate_sphere_contacts(&positions, &radii, &pairs);
376        assert_eq!(contacts.len(), 1);
377        assert!(contacts[0].depth > 0.0, "depth should be positive");
378        assert!(
379            (contacts[0].depth - 0.5).abs() < 1e-12,
380            "depth = {}",
381            contacts[0].depth
382        );
383    }
384    /// Separated spheres should not generate contacts.
385    #[test]
386    fn test_contact_generation_no_overlap() {
387        let positions = [[0.0, 0.0, 0.0], [5.0, 0.0, 0.0]];
388        let radii = [1.0, 1.0];
389        let pairs = [(0, 1)];
390        let contacts =
391            ContactGenerationKernel::generate_sphere_contacts(&positions, &radii, &pairs);
392        assert!(contacts.is_empty());
393    }
394    /// Contact resolution should separate overlapping bodies.
395    #[test]
396    fn test_contact_resolution() {
397        let mut positions = [[0.0, 0.0, 0.0], [1.5, 0.0, 0.0]];
398        let mut velocities = [[0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]];
399        let inv_masses = [1.0, 1.0];
400        let contacts = [ContactPoint {
401            position: [0.75, 0.0, 0.0],
402            normal: [1.0, 0.0, 0.0],
403            depth: 0.5,
404            body_a: 0,
405            body_b: 1,
406        }];
407        ContactGenerationKernel::resolve_contacts(
408            &mut positions,
409            &mut velocities,
410            &inv_masses,
411            &contacts,
412            0.5,
413        );
414        let dx = positions[1][0] - positions[0][0];
415        assert!(dx > 1.5, "bodies should be pushed apart, dx = {dx}");
416    }
417    /// Semi-implicit Euler should update both velocity and position.
418    #[test]
419    fn test_semi_implicit_euler() {
420        let pos = vec![0.0, 0.0, 0.0];
421        let vel = vec![0.0, 0.0, 0.0];
422        let force = vec![10.0, 0.0, 0.0];
423        let inv_mass = vec![1.0];
424        let dt = vec![0.1];
425        let mut outputs = vec![Vec::new(), Vec::new()];
426        SemiImplicitEulerKernel.execute(&[&pos, &vel, &force, &inv_mass, &dt], &mut outputs, 1);
427        assert!((outputs[0][0] - 1.0).abs() < 1e-12, "v = {}", outputs[0][0]);
428        assert!((outputs[1][0] - 0.1).abs() < 1e-12, "p = {}", outputs[1][0]);
429    }
430    /// Quaternion rotation should preserve vector length.
431    #[test]
432    fn test_quat_rotate_preserves_length() {
433        let angle = std::f64::consts::FRAC_PI_4;
434        let q = [0.0, 0.0, angle.sin(), angle.cos()];
435        let v = [1.0, 0.0, 0.0];
436        let rotated = quat_rotate(q, v);
437        let len =
438            (rotated[0] * rotated[0] + rotated[1] * rotated[1] + rotated[2] * rotated[2]).sqrt();
439        assert!((len - 1.0).abs() < 1e-10, "rotated length = {len}");
440    }
441    #[test]
442    fn test_soa_rigid_body_from_vec() {
443        let states = vec![
444            RigidBodyState::at_rest([0.0, 0.0, 0.0], 1.0),
445            RigidBodyState::at_rest([1.0, 0.0, 0.0], 0.5),
446        ];
447        let soa = SoaRigidBody::from_slice(&states);
448        assert_eq!(soa.count, 2);
449        assert!((soa.pos_x[0] - 0.0).abs() < 1e-14);
450        assert!((soa.pos_x[1] - 1.0).abs() < 1e-14);
451        assert!((soa.inv_mass[0] - 1.0).abs() < 1e-14);
452        assert!((soa.inv_mass[1] - 0.5).abs() < 1e-14);
453    }
454    #[test]
455    fn test_soa_rigid_body_to_vec() {
456        let states = vec![
457            RigidBodyState::at_rest([1.0, 2.0, 3.0], 0.25),
458            RigidBodyState::at_rest([4.0, 5.0, 6.0], 0.1),
459        ];
460        let soa = SoaRigidBody::from_slice(&states);
461        let back = soa.to_vec();
462        assert!((back[0].position[0] - 1.0).abs() < 1e-14);
463        assert!((back[1].position[1] - 5.0).abs() < 1e-14);
464    }
465    #[test]
466    fn test_soa_integrate_euler_gravity() {
467        let states = vec![RigidBodyState::at_rest([0.0, 10.0, 0.0], 1.0)];
468        let mut soa = SoaRigidBody::from_slice(&states);
469        let forces = vec![[0.0f64, -9.81, 0.0]];
470        let torques = vec![[0.0f64; 3]];
471        soa.integrate_euler(&forces, &torques, 0.1);
472        let back = soa.to_vec();
473        assert!(
474            back[0].velocity[1] < 0.0,
475            "velocity should be negative after gravity"
476        );
477        assert!(back[0].position[1] < 10.0, "y should decrease");
478    }
479    #[test]
480    fn test_soa_quaternion_stays_normalised() {
481        let states = vec![RigidBodyState::at_rest([0.0; 3], 1.0)];
482        let mut soa = SoaRigidBody::from_slice(&states);
483        let forces = vec![[0.0f64; 3]];
484        let torques = vec![[0.0, 0.0, 2.0]];
485        for _ in 0..50 {
486            soa.integrate_euler(&forces, &torques, 0.01);
487        }
488        let q = [soa.quat_x[0], soa.quat_y[0], soa.quat_z[0], soa.quat_w[0]];
489        let len = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
490        assert!((len - 1.0).abs() < 1e-10, "quaternion norm = {len}");
491    }
492    #[test]
493    fn test_soa_angular_velocity_accumulates() {
494        let states = vec![RigidBodyState::at_rest([0.0; 3], 1.0)];
495        let mut soa = SoaRigidBody::from_slice(&states);
496        let forces = vec![[0.0f64; 3]];
497        let torques = vec![[0.0, 0.0, 1.0]];
498        soa.integrate_euler(&forces, &torques, 0.5);
499        let back = soa.to_vec();
500        assert!(
501            back[0].angular_velocity[2] > 0.0,
502            "angular velocity z should increase"
503        );
504    }
505    #[test]
506    fn test_quat_norm_kernel_normalizes() {
507        let quats = vec![[2.0f64, 0.0, 0.0, 0.0], [0.0, 3.0, 0.0, 0.0]];
508        let normed = QuaternionNormKernel::normalize_batch(&quats);
509        for q in &normed {
510            let len = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
511            assert!((len - 1.0).abs() < 1e-12, "not unit: {len}");
512        }
513    }
514    #[test]
515    fn test_quat_norm_kernel_identity_unchanged() {
516        let quats = vec![[0.0, 0.0, 0.0, 1.0f64]];
517        let normed = QuaternionNormKernel::normalize_batch(&quats);
518        assert!((normed[0][3] - 1.0).abs() < 1e-12);
519    }
520    #[test]
521    fn test_quat_norm_kernel_zero_fallback() {
522        let quats = vec![[0.0f64; 4]];
523        let normed = QuaternionNormKernel::normalize_batch(&quats);
524        assert!((normed[0][3] - 1.0).abs() < 1e-12);
525    }
526    #[test]
527    fn test_angular_velocity_integration_increases_omega() {
528        let mut state = RigidBodyState::at_rest([0.0; 3], 1.0);
529        let force = [0.0; 3];
530        let torque = [1.0, 0.0, 0.0];
531        let dt = 0.1;
532        integrate_euler(std::slice::from_mut(&mut state), &[force], &[torque], dt);
533        assert!(state.angular_velocity[0] > 0.0, "omega_x should increase");
534    }
535    #[test]
536    fn test_angular_velocity_rk4_matches_euler_roughly() {
537        let state = RigidBodyState::at_rest([0.0; 3], 1.0);
538        let force = [0.0; 3];
539        let torque = [0.0, 1.0, 0.0];
540        let dt = 0.001;
541        let rk4 = integrate_rk4(&state, force, torque, dt);
542        let mut euler = state;
543        integrate_euler(std::slice::from_mut(&mut euler), &[force], &[torque], dt);
544        let diff = (rk4.angular_velocity[1] - euler.angular_velocity[1]).abs();
545        assert!(
546            diff < 0.01,
547            "RK4 and Euler should roughly agree for small dt, diff={diff}"
548        );
549    }
550    #[test]
551    fn test_mock_cuda_integrate_kernel() {
552        let n = 100;
553        let mut states: Vec<RigidBodyState> = (0..n)
554            .map(|_| RigidBodyState::at_rest([0.0, 100.0, 0.0], 1.0))
555            .collect();
556        let forces: Vec<[f64; 3]> = vec![[0.0, -9.81, 0.0]; n];
557        let torques: Vec<[f64; 3]> = vec![[0.0; 3]; n];
558        let dt = 1.0 / 60.0;
559        for _ in 0..3 {
560            integrate_euler(&mut states, &forces, &torques, dt);
561        }
562        for s in &states {
563            assert!(s.position[1] < 100.0, "y should have decreased");
564        }
565    }
566    #[test]
567    fn test_mock_cuda_kernel_simd_batch() {
568        let n = 64;
569        let states: Vec<RigidBodyState> = (0..n)
570            .map(|i| RigidBodyState::at_rest([i as f64, 0.0, 0.0], 1.0))
571            .collect();
572        let soa = SoaRigidBody::from_slice(&states);
573        assert_eq!(soa.count, n);
574        for i in 0..n {
575            assert!((soa.pos_x[i] - i as f64).abs() < 1e-12);
576        }
577    }
578}
579/// Integrate a single [`RigidBodyState`] using the semi-implicit Euler method.
580///
581/// The velocity is updated first (using forces/torques at time *t*), then the
582/// position / orientation is updated using the new velocity at time *t + dt*.
583/// This is symplectic, energy-preserving in the absence of dissipation, and is
584/// the standard method used in real-time physics engines.
585pub fn integrate_semi_implicit(
586    state: &RigidBodyState,
587    force: [f64; 3],
588    torque: [f64; 3],
589    dt: f64,
590) -> RigidBodyState {
591    let mut s = *state;
592    if s.inverse_mass < 1e-30 {
593        return s;
594    }
595    let a = [
596        force[0] * s.inverse_mass,
597        force[1] * s.inverse_mass,
598        force[2] * s.inverse_mass,
599    ];
600    s.velocity[0] += a[0] * dt;
601    s.velocity[1] += a[1] * dt;
602    s.velocity[2] += a[2] * dt;
603    s.position[0] += s.velocity[0] * dt;
604    s.position[1] += s.velocity[1] * dt;
605    s.position[2] += s.velocity[2] * dt;
606    let alpha = [
607        torque[0] * s.inverse_mass,
608        torque[1] * s.inverse_mass,
609        torque[2] * s.inverse_mass,
610    ];
611    s.angular_velocity[0] += alpha[0] * dt;
612    s.angular_velocity[1] += alpha[1] * dt;
613    s.angular_velocity[2] += alpha[2] * dt;
614    let dq = quat_derivative(s.orientation, s.angular_velocity);
615    for (ok, &dqk) in s.orientation.iter_mut().zip(dq.iter()) {
616        *ok += dqk * dt;
617    }
618    s.orientation = quat_normalise(s.orientation);
619    s
620}
621/// GPU-batch semi-implicit Euler: integrate all bodies in a slice.
622pub fn batch_integrate_semi_implicit(
623    states: &mut [RigidBodyState],
624    forces: &[[f64; 3]],
625    torques: &[[f64; 3]],
626    dt: f64,
627) {
628    for (s, (f, tau)) in states.iter_mut().zip(forces.iter().zip(torques.iter())) {
629        *s = integrate_semi_implicit(s, *f, *tau, dt);
630    }
631}
632/// Integrate only the angular velocity of each body (no linear dynamics).
633///
634/// This mimics a GPU kernel that handles only rotational degrees of freedom
635/// — useful when linear and angular integration are split into separate passes.
636pub fn integrate_angular_velocity_only(
637    states: &mut [RigidBodyState],
638    torques: &[[f64; 3]],
639    dt: f64,
640) {
641    for (s, tau) in states.iter_mut().zip(torques.iter()) {
642        if s.inverse_mass < 1e-30 {
643            continue;
644        }
645        let alpha = [
646            tau[0] * s.inverse_mass,
647            tau[1] * s.inverse_mass,
648            tau[2] * s.inverse_mass,
649        ];
650        s.angular_velocity[0] += alpha[0] * dt;
651        s.angular_velocity[1] += alpha[1] * dt;
652        s.angular_velocity[2] += alpha[2] * dt;
653        let dq = quat_derivative(s.orientation, s.angular_velocity);
654        for (ok, &dqk) in s.orientation.iter_mut().zip(dq.iter()) {
655            *ok += dqk * dt;
656        }
657        s.orientation = quat_normalise(s.orientation);
658    }
659}
660/// Compute the world-space AABB of a rigid body given its OBB half-extents.
661///
662/// The OBB is transformed by the body's orientation quaternion to produce a
663/// world AABB that tightly (conservatively) bounds the oriented box.
664pub fn compute_world_aabb(
665    position: [f64; 3],
666    orientation: [f64; 4],
667    half_extents: [f64; 3],
668) -> ([f64; 3], [f64; 3]) {
669    let signs = [
670        [-1.0f64, -1.0, -1.0],
671        [-1.0, -1.0, 1.0],
672        [-1.0, 1.0, -1.0],
673        [-1.0, 1.0, 1.0],
674        [1.0, -1.0, -1.0],
675        [1.0, -1.0, 1.0],
676        [1.0, 1.0, -1.0],
677        [1.0, 1.0, 1.0],
678    ];
679    let mut aabb_min = [f64::INFINITY; 3];
680    let mut aabb_max = [f64::NEG_INFINITY; 3];
681    for s in &signs {
682        let local = [
683            s[0] * half_extents[0],
684            s[1] * half_extents[1],
685            s[2] * half_extents[2],
686        ];
687        let world = quat_rotate(orientation, local);
688        for k in 0..3 {
689            let v = position[k] + world[k];
690            aabb_min[k] = f64::min(aabb_min[k], v);
691            aabb_max[k] = f64::max(aabb_max[k], v);
692        }
693    }
694    (aabb_min, aabb_max)
695}
696/// Batch AABB update kernel.
697///
698/// Returns a `Vec<(min, max)>` of world AABBs for all bodies.
699pub fn batch_update_world_aabbs(
700    states: &[RigidBodyState],
701    half_extents: &[[f64; 3]],
702) -> Vec<([f64; 3], [f64; 3])> {
703    assert_eq!(states.len(), half_extents.len());
704    states
705        .iter()
706        .zip(half_extents.iter())
707        .map(|(s, he)| compute_world_aabb(s.position, s.orientation, *he))
708        .collect()
709}
710/// Batch impulse application: apply accumulated impulses to all bodies.
711pub fn apply_impulses(states: &mut [RigidBodyState], impulses: &[AccumulatedImpulse]) {
712    assert_eq!(states.len(), impulses.len());
713    for (s, imp) in states.iter_mut().zip(impulses.iter()) {
714        imp.apply(s);
715    }
716}
717#[cfg(test)]
718mod extended_rigid_tests {
719
720    use crate::kernels::rigid::AccumulatedImpulse;
721
722    use crate::kernels::rigid::ContactBatchProcessor;
723
724    use crate::kernels::rigid::ContactPoint;
725
726    use crate::kernels::rigid::RigidBodyState;
727
728    use crate::kernels::rigid::SleepParams;
729    use crate::kernels::rigid::SleepState;
730    use crate::kernels::rigid::SleepTest;
731    use crate::kernels::rigid::SoaRigidBody;
732    use crate::kernels::rigid::apply_impulses;
733    use crate::kernels::rigid::batch_integrate_semi_implicit;
734    use crate::kernels::rigid::batch_update_world_aabbs;
735    use crate::kernels::rigid::compute_world_aabb;
736    use crate::kernels::rigid::integrate_angular_velocity_only;
737    use crate::kernels::rigid::integrate_semi_implicit;
738    #[test]
739    fn semi_implicit_linear_motion() {
740        let state = RigidBodyState::at_rest([0.0, 0.0, 0.0], 1.0);
741        let force = [10.0, 0.0, 0.0];
742        let torque = [0.0; 3];
743        let dt = 0.1;
744        let next = integrate_semi_implicit(&state, force, torque, dt);
745        assert!((next.velocity[0] - 1.0).abs() < 1e-12);
746        assert!((next.position[0] - 0.1).abs() < 1e-12);
747    }
748    #[test]
749    fn semi_implicit_static_body_unchanged() {
750        let state = RigidBodyState::at_rest([5.0, 5.0, 5.0], 0.0);
751        let next = integrate_semi_implicit(&state, [100.0; 3], [100.0; 3], 1.0);
752        assert_eq!(next.position, state.position);
753        assert_eq!(next.velocity, state.velocity);
754    }
755    #[test]
756    fn semi_implicit_gravity_free_fall() {
757        let dt = 0.01;
758        let mut s = RigidBodyState::at_rest([0.0, 100.0, 0.0], 1.0);
759        let force = [0.0, -9.81, 0.0];
760        let torque = [0.0; 3];
761        for _ in 0..100 {
762            s = integrate_semi_implicit(&s, force, torque, dt);
763        }
764        assert!(
765            (s.velocity[1] + 9.81).abs() < 0.01,
766            "v_y = {}",
767            s.velocity[1]
768        );
769    }
770    #[test]
771    fn batch_semi_implicit_matches_single() {
772        let n = 5;
773        let mut states: Vec<RigidBodyState> = (0..n)
774            .map(|i| RigidBodyState::at_rest([i as f64, 0.0, 0.0], 1.0))
775            .collect();
776        let forces = vec![[1.0, 0.0, 0.0]; n];
777        let torques = vec![[0.0; 3]; n];
778        let dt = 0.01;
779        let singles: Vec<RigidBodyState> = states
780            .iter()
781            .map(|s| integrate_semi_implicit(s, [1.0, 0.0, 0.0], [0.0; 3], dt))
782            .collect();
783        batch_integrate_semi_implicit(&mut states, &forces, &torques, dt);
784        for (s, e) in states.iter().zip(singles.iter()) {
785            for k in 0..3 {
786                assert!((s.position[k] - e.position[k]).abs() < 1e-12);
787                assert!((s.velocity[k] - e.velocity[k]).abs() < 1e-12);
788            }
789        }
790    }
791    #[test]
792    fn angular_only_changes_omega_not_position() {
793        let mut states = vec![RigidBodyState::at_rest([1.0, 2.0, 3.0], 1.0)];
794        let torques = vec![[0.0, 0.0, 1.0]];
795        integrate_angular_velocity_only(&mut states, &torques, 0.1);
796        assert_eq!(states[0].position, [1.0, 2.0, 3.0]);
797        assert!(states[0].angular_velocity[2] > 0.0);
798    }
799    #[test]
800    fn angular_only_static_body_unchanged() {
801        let mut states = vec![RigidBodyState::at_rest([0.0; 3], 0.0)];
802        let torques = vec![[10.0; 3]];
803        integrate_angular_velocity_only(&mut states, &torques, 1.0);
804        assert_eq!(states[0].angular_velocity, [0.0; 3]);
805    }
806    #[test]
807    fn world_aabb_identity_orientation() {
808        let pos = [1.0, 2.0, 3.0];
809        let q = [0.0, 0.0, 0.0, 1.0];
810        let he = [1.0, 2.0, 3.0];
811        let (mn, mx) = compute_world_aabb(pos, q, he);
812        assert!((mn[0] - 0.0).abs() < 1e-10);
813        assert!((mx[0] - 2.0).abs() < 1e-10);
814        assert!((mn[1] - 0.0).abs() < 1e-10);
815        assert!((mx[1] - 4.0).abs() < 1e-10);
816    }
817    #[test]
818    fn world_aabb_bounds_contain_corners() {
819        let pos = [0.0; 3];
820        let q = [0.0, 0.0, 0.0, 1.0];
821        let he = [1.0, 1.0, 1.0];
822        let (mn, mx) = compute_world_aabb(pos, q, he);
823        for k in 0..3 {
824            assert!(mn[k] <= -0.99, "min[{k}] = {}", mn[k]);
825            assert!(mx[k] >= 0.99, "max[{k}] = {}", mx[k]);
826        }
827    }
828    #[test]
829    fn batch_world_aabbs_correct_count() {
830        let states = vec![
831            RigidBodyState::at_rest([0.0; 3], 1.0),
832            RigidBodyState::at_rest([5.0; 3], 1.0),
833        ];
834        let hes = vec![[1.0; 3], [0.5; 3]];
835        let aabbs = batch_update_world_aabbs(&states, &hes);
836        assert_eq!(aabbs.len(), 2);
837    }
838    #[test]
839    fn sleep_test_body_falls_asleep() {
840        let mut test = SleepTest::new(1);
841        let params = SleepParams {
842            linear_threshold: 0.1,
843            angular_threshold: 0.1,
844            sleep_frames: 3,
845        };
846        let state = RigidBodyState::at_rest([0.0; 3], 1.0);
847        for _ in 0..3 {
848            test.update(&[state], &params);
849        }
850        assert_eq!(test.sleeping_count(), 1);
851    }
852    #[test]
853    fn sleep_test_moving_body_stays_awake() {
854        let mut test = SleepTest::new(1);
855        let params = SleepParams::default();
856        let mut state = RigidBodyState::at_rest([0.0; 3], 1.0);
857        state.velocity = [1.0, 0.0, 0.0];
858        for _ in 0..20 {
859            test.update(&[state], &params);
860        }
861        assert_eq!(test.sleeping_count(), 0);
862    }
863    #[test]
864    fn sleep_test_wake_all_resets() {
865        let mut test = SleepTest::new(2);
866        let params = SleepParams {
867            sleep_frames: 1,
868            ..Default::default()
869        };
870        let state = RigidBodyState::at_rest([0.0; 3], 1.0);
871        test.update(&[state, state], &params);
872        test.wake_all();
873        assert_eq!(test.sleeping_count(), 0);
874        assert!(test.dormant_frames.iter().all(|&f| f == 0));
875    }
876    #[test]
877    fn sleep_test_static_body_is_sleeping() {
878        let mut test = SleepTest::new(1);
879        let params = SleepParams::default();
880        let state = RigidBodyState::at_rest([0.0; 3], 0.0);
881        test.update(&[state], &params);
882        assert_eq!(test.sleep_states[0], SleepState::Sleeping);
883    }
884    #[test]
885    fn accumulated_impulse_apply_changes_velocity() {
886        let mut state = RigidBodyState::at_rest([0.0; 3], 1.0);
887        let mut imp = AccumulatedImpulse::default();
888        imp.add_linear([5.0, 0.0, 0.0]);
889        imp.apply(&mut state);
890        assert!((state.velocity[0] - 5.0).abs() < 1e-12);
891    }
892    #[test]
893    fn accumulated_impulse_magnitude() {
894        let mut imp = AccumulatedImpulse::default();
895        imp.add_linear([3.0, 4.0, 0.0]);
896        assert!((imp.linear_magnitude() - 5.0).abs() < 1e-12);
897    }
898    #[test]
899    fn accumulated_impulse_add_angular() {
900        let mut imp = AccumulatedImpulse::default();
901        imp.add_angular([0.0, 0.0, 1.0]);
902        assert!((imp.angular_magnitude() - 1.0).abs() < 1e-12);
903    }
904    #[test]
905    fn apply_impulses_batch() {
906        let n = 3;
907        let mut states: Vec<RigidBodyState> = (0..n)
908            .map(|_| RigidBodyState::at_rest([0.0; 3], 1.0))
909            .collect();
910        let impulses: Vec<AccumulatedImpulse> = (0..n)
911            .map(|i| {
912                let mut imp = AccumulatedImpulse::default();
913                imp.add_linear([i as f64, 0.0, 0.0]);
914                imp
915            })
916            .collect();
917        apply_impulses(&mut states, &impulses);
918        for (i, s) in states.iter().enumerate() {
919            assert!(
920                (s.velocity[0] - i as f64).abs() < 1e-12,
921                "body {i}: v_x = {}",
922                s.velocity[0]
923            );
924        }
925    }
926    #[test]
927    fn contact_batch_generates_nonzero_impulses() {
928        let positions = [[0.0, 0.0, 0.0], [1.5, 0.0, 0.0]];
929        let velocities = [[0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]];
930        let inv_masses = [1.0, 1.0];
931        let contacts = [ContactPoint {
932            position: [0.75, 0.0, 0.0],
933            normal: [1.0, 0.0, 0.0],
934            depth: 0.5,
935            body_a: 0,
936            body_b: 1,
937        }];
938        let proc = ContactBatchProcessor::new(0.5, 0.2);
939        let impulses =
940            proc.process_contacts(&contacts, &positions, &velocities, &inv_masses, 2, 0.01);
941        assert!(impulses[0].linear_magnitude() > 0.0 || impulses[1].linear_magnitude() > 0.0);
942    }
943    #[test]
944    fn contact_batch_no_contacts_zero_impulse() {
945        let positions = [[0.0; 3], [5.0, 0.0, 0.0]];
946        let velocities = [[0.0; 3]; 2];
947        let inv_masses = [1.0, 1.0];
948        let contacts: &[ContactPoint] = &[];
949        let proc = ContactBatchProcessor::new(0.5, 0.2);
950        let impulses =
951            proc.process_contacts(contacts, &positions, &velocities, &inv_masses, 2, 0.01);
952        for imp in &impulses {
953            assert!((imp.linear_magnitude()).abs() < 1e-12);
954        }
955    }
956    #[test]
957    fn world_aabb_90_degree_rotation_x() {
958        let angle = std::f64::consts::FRAC_PI_4;
959        let q = [angle.sin(), 0.0, 0.0, angle.cos()];
960        let he = [1.0, 2.0, 3.0];
961        let (mn, mx) = compute_world_aabb([0.0; 3], q, he);
962        for k in 0..3 {
963            assert!(mn[k] < mx[k], "min[{k}] >= max[{k}]");
964        }
965    }
966    #[test]
967    fn soa_batch_integrate_all_fall_equally() {
968        let n = 10;
969        let states: Vec<RigidBodyState> = (0..n)
970            .map(|_| RigidBodyState::at_rest([0.0, 100.0, 0.0], 1.0))
971            .collect();
972        let mut soa = SoaRigidBody::from_slice(&states);
973        let forces = vec![[0.0f64, -9.81, 0.0]; n];
974        let torques = vec![[0.0f64; 3]; n];
975        soa.integrate_euler(&forces, &torques, 0.1);
976        let back = soa.to_vec();
977        let vy0 = back[0].velocity[1];
978        for s in &back {
979            assert!(
980                (s.velocity[1] - vy0).abs() < 1e-12,
981                "all bodies should fall equally"
982            );
983        }
984    }
985    #[test]
986    fn sleep_test_not_sleeping_before_threshold() {
987        let mut test = SleepTest::new(1);
988        let params = SleepParams {
989            sleep_frames: 5,
990            ..Default::default()
991        };
992        let state = RigidBodyState::at_rest([0.0; 3], 1.0);
993        for _ in 0..4 {
994            test.update(&[state], &params);
995        }
996        assert_eq!(
997            test.sleeping_count(),
998            0,
999            "body should not sleep before threshold"
1000        );
1001    }
1002    #[test]
1003    fn contact_batch_static_pair_no_impulse() {
1004        let positions = [[0.0; 3], [0.5, 0.0, 0.0]];
1005        let velocities = [[0.0; 3]; 2];
1006        let inv_masses = [0.0, 0.0];
1007        let contacts = [ContactPoint {
1008            position: [0.25, 0.0, 0.0],
1009            normal: [1.0, 0.0, 0.0],
1010            depth: 0.5,
1011            body_a: 0,
1012            body_b: 1,
1013        }];
1014        let proc = ContactBatchProcessor::new(0.5, 0.2);
1015        let impulses =
1016            proc.process_contacts(&contacts, &positions, &velocities, &inv_masses, 2, 0.01);
1017        for imp in &impulses {
1018            assert!(
1019                (imp.linear_magnitude()).abs() < 1e-12,
1020                "static pair should produce zero impulse"
1021            );
1022        }
1023    }
1024}
oxiphysics_gpu/kernels/rigid/functions.rs

oxiphysics_gpu/kernels/rigid/
functions.rs
