oxiphysics_gpu/kernels/rigid/
functions.rs

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

oxiphysics_gpu/kernels/rigid/
functions.rs
