Nuke some nalgebra types in the pipeline

Author: ryanhcode

common/src/main/rust/rapier/benches/collision_benchmark.rs

@@ -1,8 +1,8 @@
 use criterion::{Criterion, criterion_group, criterion_main};
 use marten::Real;
 use marten::octree::SubLevelOctree;
+use rapier3d::glamx::{DVec3, IVec3};
 use rapier3d::math::Pose3;
-use rapier3d::na::Vector3;
 use rapier3d::prelude::ColliderHandle;
 use sable_rapier::ActiveLevelColliderInfo;
 use sable_rapier::algo::{DEFAULT_COLLISION_PARALLEL_CUTOFF, find_collision_pairs};
@@ -15,9 +15,9 @@ fn setup_dummy_sable_handle_a() -> ActiveLevelColliderInfo {
     ActiveLevelColliderInfo {
         static_mount: None,
         collider: ColliderHandle::default(),
-        local_bounds_min: Some(Vector3::<i32>::new(0, 0, 0)),
-        local_bounds_max: Some(Vector3::<i32>::new(128, 128, 128)),
-        center_of_mass: Some(Vector3::<f64>::new(62.5, 62.5, 62.5)),
+        local_bounds_min: Some(IVec3::new(0, 0, 0)),
+        local_bounds_max: Some(IVec3::new(128, 128, 128)),
+        center_of_mass: Some(DVec3::new(62.5, 62.5, 62.5)),
         octree: Some(octree),
         chunk_map: None,
         scene_id: 0,
@@ -32,9 +32,9 @@ fn setup_dummy_sable_handle_b() -> ActiveLevelColliderInfo {
     ActiveLevelColliderInfo {
         static_mount: None,
         collider: ColliderHandle::default(),
-        local_bounds_min: Some(Vector3::<i32>::new(128, 0, 0)),
-        local_bounds_max: Some(Vector3::<i32>::new(256, 128, 128)),
-        center_of_mass: Some(Vector3::<f64>::new(128.0 + 64.5, 64.5, 64.5)),
+        local_bounds_min: Some(IVec3::new(128, 0, 0)),
+        local_bounds_max: Some(IVec3::new(256, 128, 128)),
+        center_of_mass: Some(DVec3::new(128.0 + 64.5, 64.5, 64.5)),
         octree: Some(octree),
         chunk_map: None,
         scene_id: 0,

common/src/main/rust/rapier/src/algo.rs

@@ -2,9 +2,9 @@ use std::cmp::min;
 
 use marten::Real;
 use marten::level::OCTREE_CHUNK_SHIFT;
-use rapier3d::glamx::Pose3;
-use rapier3d::math::Vector;
-use rapier3d::na::{SimdComplexField, Vector3};
+use rapier3d::glamx::{DVec3, IVec3, Pose3};
+use rapier3d::math::Vec3;
+use rapier3d::na::SimdComplexField;
 use rayon::iter::ParallelIterator;
 use rayon::prelude::{IntoParallelRefIterator, ParallelExtend};
 
@@ -21,11 +21,11 @@ pub fn find_collision_pairs(
     prediction: Real,
     cutoff: usize,
     liquid: bool,
-) -> Vec<(Vector3<i32>, Vector3<i32>)> {
+) -> Vec<(IVec3, IVec3)> {
     struct StackObject {
         index: u32,
         depth: u32,
-        min: Vector3<i32>,
+        min: IVec3,
     }
 
     let Some(octree) = &sable_body.octree else {
@@ -36,39 +36,36 @@ pub fn find_collision_pairs(
 
     let center_of_mass = sable_body.center_of_mass.unwrap();
 
-    let offset = Vector3::new(
+    let offset = DVec3::new(
         local_bounds_min.x as f64 - center_of_mass.x,
         local_bounds_min.y as f64 - center_of_mass.y,
         local_bounds_min.z as f64 - center_of_mass.z,
     );
-    let offset = Vector3::new(offset.x as Real, offset.y as Real, offset.z as Real);
+    let offset = Vec3::new(offset.x as Real, offset.y as Real, offset.z as Real);
 
     let offset = isometry.rotation.mul_vec3(offset.into());
     let translation = isometry.translation + offset;
 
     // start with the root node
     let mut current_level = Vec::with_capacity(128);
 
-    let com_offset: Vector3<f64> = if let Some(other_handle) = other_sable_body {
+    let com_offset: DVec3 = if let Some(other_handle) = other_sable_body {
         let com = other_handle.center_of_mass.unwrap();
-        Vector3::new(com.x, com.y, com.z)
+        DVec3::new(com.x, com.y, com.z)
     } else {
-        Vector3::new(0.0, 0.0, 0.0)
+        DVec3::ZERO
     };
 
     current_level.push(StackObject {
         index: 0,
         depth: 0,
-        min: Vector3::new(0, 0, 0),
+        min: IVec3::ZERO,
     });
 
     let mut pairs = Vec::with_capacity(16);
     // process nodes level by level to maintain some structure while parallelizing
     while !current_level.is_empty() {
-        type LevelData = (
-            Option<Vec<StackObject>>,
-            Option<Vec<(Vector3<i32>, Vector3<i32>)>>,
-        );
+        type LevelData = (Option<Vec<StackObject>>, Option<Vec<(IVec3, IVec3)>>);
         let mut next_level_data = Vec::<LevelData>::with_capacity(8);
 
         let do_level_parallel = current_level.len() >= cutoff;
@@ -78,12 +75,7 @@ pub fn find_collision_pairs(
             let node_size = 1 << (octree.log_size as u32 - entry.depth);
 
             // Calculate the center and radius for this node
-            let node_center = Vector3::new(
-                entry.min.x as Real + node_size as Real / 2.0,
-                entry.min.y as Real + node_size as Real / 2.0,
-                entry.min.z as Real + node_size as Real / 2.0,
-            );
-            let node_center = Vector::new(node_center.x, node_center.y, node_center.z);
+            let node_center = entry.min.as_vec3() + node_size as f32 / 2.0;
             let transformed_center = isometry.rotation.mul_vec3(node_center) + translation;
             let radius = node_size as Real / 2.0 * 1.7321 + prediction;
 
@@ -121,7 +113,7 @@ pub fn find_collision_pairs(
                         index: (node + i) as u32,
                         depth: entry.depth + 1,
                         min: entry.min
-                            + Vector3::new(
+                            + IVec3::new(
                                 (i & 1) * node_size / 2,
                                 ((i >> 1) & 1) * node_size / 2,
                                 ((i >> 2) & 1) * node_size / 2,
@@ -156,13 +148,13 @@ pub fn find_collision_pairs(
 
 fn get_overlapping_nodes(
     other_handle: Option<&ActiveLevelColliderInfo>,
-    com_offset: Vector3<f64>,
-    pos: Vector3<Real>,
+    com_offset: DVec3,
+    pos: Vec3,
     dist: Real,
     scene: &PhysicsScene,
     cancel_early: bool,
     liquid: bool,
-) -> (bool, Option<Vec<Vector3<i32>>>) {
+) -> (bool, Option<Vec<IVec3>>) {
     // biggest power of two that doesn't go over radius
     let log2 = ((dist * 2.0).simd_ln() / 2.0f32.simd_ln()).floor() as i32;
 
@@ -175,32 +167,14 @@ fn get_overlapping_nodes(
         min(log2, OCTREE_CHUNK_SHIFT)
     };
 
-    let min_block_pos = Vector3::new(
-        ((pos.x - dist) as f64 + com_offset.x).floor() as i32,
-        ((pos.y - dist) as f64 + com_offset.y).floor() as i32,
-        ((pos.z - dist) as f64 + com_offset.z).floor() as i32,
-    );
-    let max_block_pos = Vector3::new(
-        ((pos.x + dist) as f64 + com_offset.x).floor() as i32,
-        ((pos.y + dist) as f64 + com_offset.y).floor() as i32,
-        ((pos.z + dist) as f64 + com_offset.z).floor() as i32,
-    );
+    let min_block_pos = ((pos - dist).as_dvec3() + com_offset).floor().as_ivec3();
+    let max_block_pos = ((pos + dist).as_dvec3() + com_offset).floor().as_ivec3();
 
     if let Some(other_handle) = other_handle {
         let other_min = other_handle.local_bounds_min.unwrap();
 
-        let min_pos = Vector3::new(
-            (min_block_pos.x - other_min.x) >> log2,
-            (min_block_pos.y - other_min.y) >> log2,
-            (min_block_pos.z - other_min.z) >> log2,
-        )
-        .map(|x| x.max(0));
-
-        let max_pos = Vector3::new(
-            (max_block_pos.x - other_min.x) >> log2,
-            (max_block_pos.y - other_min.y) >> log2,
-            (max_block_pos.z - other_min.z) >> log2,
-        );
+        let min_pos = ((min_block_pos - other_min) >> log2).map(|x| x.max(0));
+        let max_pos = (max_block_pos - other_min) >> log2;
 
         let Some(oct) = &other_handle.octree else {
             panic!("No octree!")
@@ -211,14 +185,15 @@ fn get_overlapping_nodes(
         } else {
             Some(Vec::with_capacity(16))
         };
+
         for x in min_pos.x..=max_pos.x {
             for y in min_pos.y..=max_pos.y {
                 for z in min_pos.z..=max_pos.z {
                     if oct.query(x << log2, y << log2, z << log2, log2) > -2 {
                         if cancel_early {
                             return (true, None);
                         } else {
-                            blocks.as_mut().unwrap().push(Vector3::new(
+                            blocks.as_mut().unwrap().push(IVec3::new(
                                 (x << log2) + other_min.x,
                                 (y << log2) + other_min.y,
                                 (z << log2) + other_min.z,
@@ -237,16 +212,8 @@ fn get_overlapping_nodes(
     }
 
     // find all the octrees
-    let min_octree_pos = Vector3::new(
-        min_block_pos.x >> OCTREE_CHUNK_SHIFT,
-        min_block_pos.y >> OCTREE_CHUNK_SHIFT,
-        min_block_pos.z >> OCTREE_CHUNK_SHIFT,
-    );
-    let max_octree_pos = Vector3::new(
-        max_block_pos.x >> OCTREE_CHUNK_SHIFT,
-        max_block_pos.y >> OCTREE_CHUNK_SHIFT,
-        max_block_pos.z >> OCTREE_CHUNK_SHIFT,
-    );
+    let min_octree_pos = min_block_pos >> OCTREE_CHUNK_SHIFT;
+    let max_octree_pos = max_block_pos >> OCTREE_CHUNK_SHIFT;
 
     let mut blocks = if cancel_early {
         None
@@ -286,11 +253,7 @@ fn get_overlapping_nodes(
                                 if cancel_early {
                                     return (true, None);
                                 } else {
-                                    blocks.as_mut().unwrap().push(Vector3::new(
-                                        x << log2,
-                                        y << log2,
-                                        z << log2,
-                                    ));
+                                    blocks.as_mut().unwrap().push(IVec3::new(x, y, z) << log2);
                                 }
                             }
                         }

common/src/main/rust/rapier/src/boxes.rs

@@ -5,7 +5,7 @@ use marten::Real;
 use rapier3d::dynamics::RigidBodyBuilder;
 use rapier3d::geometry::{ColliderBuilder, SharedShape};
 use rapier3d::glamx::Quat;
-use rapier3d::math::Vector;
+use rapier3d::math::Vec3;
 
 use crate::get_scene_mut;
 use crate::scene::LevelColliderID;
@@ -34,7 +34,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cre
 
     let mut rigid_body = RigidBodyBuilder::dynamic()
         .ccd_enabled(true)
-        .translation(Vector::new(
+        .translation(Vec3::new(
             pose_arr[0] as Real,
             pose_arr[1] as Real,
             pose_arr[2] as Real,