From 0c7309ace3738d52fe76616606a8fb25ac50c8ae Mon Sep 17 00:00:00 2001 From: Random-Scientist Date: Sun, 19 Apr 2026 17:03:52 -0700 Subject: [PATCH] rust refactors --- common/src/main/rust/Cargo.lock | 1 + common/src/main/rust/marten/Cargo.toml | 1 + common/src/main/rust/marten/src/level.rs | 36 +- common/src/main/rust/marten/src/octree.rs | 23 +- .../rapier/benches/collision_benchmark.rs | 16 +- common/src/main/rust/rapier/src/algo.rs | 154 ++--- common/src/main/rust/rapier/src/boxes.rs | 21 +- common/src/main/rust/rapier/src/buoyancy.rs | 115 +--- common/src/main/rust/rapier/src/collider.rs | 65 +- .../src/main/rust/rapier/src/contraptions.rs | 45 +- common/src/main/rust/rapier/src/dispatcher.rs | 371 ++++------- .../src/main/rust/rapier/src/event_handler.rs | 25 +- common/src/main/rust/rapier/src/hooks.rs | 37 +- common/src/main/rust/rapier/src/joints.rs | 159 ++--- common/src/main/rust/rapier/src/lib.rs | 591 +++++++----------- common/src/main/rust/rapier/src/rope.rs | 40 +- common/src/main/rust/rapier/src/scene.rs | 72 ++- .../main/rust/rapier/src/voxel_collider.rs | 15 +- 18 files changed, 636 insertions(+), 1151 deletions(-) diff --git a/common/src/main/rust/Cargo.lock b/common/src/main/rust/Cargo.lock index 8572d78..836c6ba 100644 --- a/common/src/main/rust/Cargo.lock +++ b/common/src/main/rust/Cargo.lock @@ -635,6 +635,7 @@ version = "0.1.0" dependencies = [ "colored 2.2.0", "fern", + "glamx", "humantime", "jni", "log", diff --git a/common/src/main/rust/marten/Cargo.toml b/common/src/main/rust/marten/Cargo.toml index 67ab342..134c3e5 100644 --- a/common/src/main/rust/marten/Cargo.toml +++ b/common/src/main/rust/marten/Cargo.toml @@ -9,6 +9,7 @@ log.workspace = true fern.workspace = true humantime.workspace = true colored.workspace = true +glamx = { version = "0.1" } #rapier3d = { git = "https://github.com/ryanhcode/rapier", rev = "a728067629d4f85cef99f388e12c9b0ee8cd1164", features = ["simd-nightly", "parallel"] } #parry3d = { version = "0.26.0", features = ["simd-nightly"] } diff --git a/common/src/main/rust/marten/src/level.rs b/common/src/main/rust/marten/src/level.rs index 2f3b491..70737d4 100644 --- a/common/src/main/rust/marten/src/level.rs +++ b/common/src/main/rust/marten/src/level.rs @@ -1,9 +1,11 @@ //! A sparse voxel world. use crate::octree::SubLevelOctree; +use glamx::{IVec3, Vec3}; use jni::JNIEnv; use jni::descriptors::Desc; use jni::objects::{GlobalRef, JMethodID}; +use jni::sys::jdouble; /// log_2 of the size of a chunk pub const CHUNK_SHIFT: u8 = 4; @@ -40,7 +42,7 @@ impl ChunkSection { /// # Safety /// This method assumes that the coordinate is > than 0 and < than `CHUNK_SIZE` on all axes. #[inline(always)] - fn get_index(&self, x: i32, y: i32, z: i32) -> usize { + fn get_index(&self, IVec3 { x, y, z }: IVec3) -> usize { (x + (z << 4) + (y << 8)) as usize } @@ -49,8 +51,8 @@ impl ChunkSection { /// # Safety /// This method assumes that the coordinate is > than 0 and < than `CHUNK_SIZE` on all axes. /// If the coordinate is out of bounds, behavior is undefined. - pub fn set_block(&mut self, x: i32, y: i32, z: i32, state: BlockState) { - let index = self.get_index(x, y, z); + pub fn set_block(&mut self, pos: IVec3, state: BlockState) { + let index = self.get_index(pos); self.blocks[index] = state; } @@ -59,8 +61,8 @@ impl ChunkSection { /// # Safety /// This method assumes that the coordinate is >= than 0 and < than `CHUNK_SIZE` on all axes. /// If the coordinate is out of bounds, behavior is undefined. - pub fn get_block(&self, x: i32, y: i32, z: i32) -> BlockState { - let index = self.get_index(x, y, z); + pub fn get_block(&self, pos: IVec3) -> BlockState { + let index = self.get_index(pos); unsafe { *self.blocks.get_unchecked(index) } } } @@ -76,12 +78,26 @@ unsafe impl<'local> Desc<'local, JMethodID> for &SableMethodID { } } +#[derive(Debug, Clone, Copy)] +pub struct CollisionBox { + pub min: Vec3, + pub max: Vec3, +} +impl From<[jdouble; 6]> for CollisionBox { + fn from(value: [jdouble; 6]) -> Self { + let [min_x, min_y, min_z, max_x, max_y, max_z] = value.map(|v| v as f32); + Self { + min: Vec3::new(min_x, min_y, min_z), + max: Vec3::new(max_x, max_y, max_z), + } + } +} + /// The physics data of a blockstate #[derive(Debug)] pub struct VoxelColliderData { /// Collision boxes within the 0-1 voxel space. - /// Formatted [min_x, min_y, min_z, max_x, max_y, max_z] - pub collision_boxes: Vec<(f32, f32, f32, f32, f32, f32)>, + pub collision_boxes: Vec, /// If this should be treated as a fluid for buoyancy pub is_fluid: bool, @@ -133,6 +149,12 @@ impl OctreeChunkSection { } } +impl Default for OctreeChunkSection { + fn default() -> Self { + Self::new() + } +} + #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub enum VoxelPhysicsState { Empty, diff --git a/common/src/main/rust/marten/src/octree.rs b/common/src/main/rust/marten/src/octree.rs index 9433a81..e6f7249 100644 --- a/common/src/main/rust/marten/src/octree.rs +++ b/common/src/main/rust/marten/src/octree.rs @@ -1,5 +1,7 @@ //! Flat octree for integer data +use glamx::IVec3; + /// The max size we allow an octree buffer to occupy const MAX_SIZE: i32 = i32::MAX - 8 * 2; @@ -55,7 +57,7 @@ impl SubLevelOctree { /// @return a unique 0-7 index from a given x, y, z position in 0-1 ranges #[inline(always)] - fn get_octant_index(x: i32, y: i32, z: i32) -> i32 { + fn get_octant_index(IVec3 { x, y, z }: IVec3) -> i32 { (x & 1) | ((y & 1) << 1) | ((z & 1) << 2) } @@ -146,15 +148,13 @@ impl SubLevelOctree { /// /// # Arguments /// - /// * `x` - the x position - /// * `y` - the y position - /// * `z` - the z position + /// * `pos` - the position /// * `block` - the block ID /// /// # Returns /// /// * `bool` - if the insert modified the tree - pub fn insert(&mut self, x: i32, y: i32, z: i32, block: i32) -> bool { + pub fn insert(&mut self, pos: IVec3, block: i32) -> bool { let mut shift = self.log_size - 1; let mut index = 0; let mut node = self.buffer[index as usize]; @@ -167,7 +167,7 @@ impl SubLevelOctree { return false; // already equivalent } - let octant_index = Self::get_octant_index(x >> shift, y >> shift, z >> shift); + let octant_index = Self::get_octant_index(pos >> shift); if node > 0 { branches_visited[branch_index as usize] = index; @@ -197,18 +197,17 @@ impl SubLevelOctree { /// /// # Arguments /// - /// * `x` - the x position - /// * `y` - the y position - /// * `z` - the z position + /// * `pos` - the position /// * `log_size_of_target` - the log size of the target /// /// # Returns /// /// * `i32` - the block ID at the position, or -2 if the position is empty - pub fn query(&self, x: i32, y: i32, z: i32, log_size_of_target: i32) -> i32 { + pub fn query(&self, pos: IVec3, log_size_of_target: i32) -> i32 { let size = 1 << self.log_size; + // check if out of bounds - if x < 0 || y < 0 || z < 0 || x >= size || y >= size || z >= size { + if pos.cmplt(IVec3::ZERO).any() || pos.cmpge(IVec3::splat(size)).any() { return -2; } @@ -223,7 +222,7 @@ impl SubLevelOctree { return -2; } - let octant_index = Self::get_octant_index(x >> shift, y >> shift, z >> shift); + let octant_index = Self::get_octant_index(pos >> shift); index = node + octant_index; node = *unsafe { self.buffer.get_unchecked(index as usize) }; diff --git a/common/src/main/rust/rapier/benches/collision_benchmark.rs b/common/src/main/rust/rapier/benches/collision_benchmark.rs index aa8afd7..63a9904 100644 --- a/common/src/main/rust/rapier/benches/collision_benchmark.rs +++ b/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::::new(0, 0, 0)), - local_bounds_max: Some(Vector3::::new(128, 128, 128)), - center_of_mass: Some(Vector3::::new(62.5, 62.5, 62.5)), + local_bounds_min: Some(IVec3::ZERO), + local_bounds_max: Some(IVec3::splat(128)), + center_of_mass: Some(DVec3::splat(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::::new(128, 0, 0)), - local_bounds_max: Some(Vector3::::new(256, 128, 128)), - center_of_mass: Some(Vector3::::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, @@ -51,7 +51,7 @@ fn setup_sphere(octree: &mut SubLevelOctree) { let dy = y as f64 - 64.5; let dz = z as f64 - 64.5; if dx * dx + dy * dy + dz * dz <= 63.5 * 63.5 { - octree.insert(x, y, z, 1); + octree.insert(IVec3::new(x, y, z), 1); } } } diff --git a/common/src/main/rust/rapier/src/algo.rs b/common/src/main/rust/rapier/src/algo.rs index 0bace9e..428777b 100644 --- a/common/src/main/rust/rapier/src/algo.rs +++ b/common/src/main/rust/rapier/src/algo.rs @@ -2,9 +2,10 @@ 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}; +use rapier3d::math::{Pose3, Vec3}; + +use rapier3d::na::SimdComplexField; use rayon::iter::ParallelIterator; use rayon::prelude::{IntoParallelRefIterator, ParallelExtend}; @@ -21,11 +22,12 @@ pub fn find_collision_pairs( prediction: Real, cutoff: usize, liquid: bool, -) -> Vec<(Vector3, Vector3)> { +) -> Vec<(IVec3, IVec3)> { + #[derive(Default)] struct StackObject { index: u32, depth: u32, - min: Vector3, + min: IVec3, } let Some(octree) = &sable_body.octree else { @@ -36,63 +38,44 @@ pub fn find_collision_pairs( let center_of_mass = sable_body.center_of_mass.unwrap(); - let offset = Vector3::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 = local_bounds_min.as_dvec3() - center_of_mass; - let offset = isometry.rotation.mul_vec3(offset.into()); + let offset = isometry.rotation.mul_vec3(offset.as_vec3()); let translation = isometry.translation + offset; // start with the root node let mut current_level = Vec::with_capacity(128); - let com_offset: Vector3 = if let Some(other_handle) = other_sable_body { - let com = other_handle.center_of_mass.unwrap(); - Vector3::new(com.x, com.y, com.z) - } else { - Vector3::new(0.0, 0.0, 0.0) - }; + let com_offset: DVec3 = other_sable_body + .map(|body| body.center_of_mass.unwrap()) + .unwrap_or(DVec3::ZERO); - current_level.push(StackObject { - index: 0, - depth: 0, - min: Vector3::new(0, 0, 0), - }); + current_level.push(StackObject::default()); 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>, - Option, Vector3)>>, - ); + type LevelData = (Option>, Option>); let mut next_level_data = Vec::::with_capacity(8); let do_level_parallel = current_level.len() >= cutoff; let process_stack_object = |entry: &StackObject| -> LevelData { let node = *unsafe { octree.buffer.get_unchecked(entry.index as usize) }; - let node_size = 1 << (octree.log_size as u32 - entry.depth); + let node_size: i32 = 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; + let radius = (node_size as Real / 2.0 * 1.7321) + prediction; let scene = get_scene_mut(sable_body.scene_id); let (has_any_intersections, blocks_opt) = get_overlapping_nodes( other_sable_body, com_offset, - transformed_center.into(), + transformed_center, radius, scene, node >= 0, @@ -121,7 +104,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 +139,13 @@ pub fn find_collision_pairs( fn get_overlapping_nodes( other_handle: Option<&ActiveLevelColliderInfo>, - com_offset: Vector3, - pos: Vector3, + com_offset: DVec3, + pos: Vec3, dist: Real, scene: &PhysicsScene, cancel_early: bool, liquid: bool, -) -> (bool, Option>>) { +) -> (bool, Option>) { // biggest power of two that doesn't go over radius let log2 = ((dist * 2.0).simd_ln() / 2.0f32.simd_ln()).floor() as i32; @@ -174,33 +157,15 @@ fn get_overlapping_nodes( } else { min(log2, OCTREE_CHUNK_SHIFT) }; + let min_block_pos = ((pos - dist).as_dvec3() + com_offset).floor().as_ivec3(); - 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 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).max(IVec3::ZERO); + let max_pos = (max_block_pos - other_min) >> log2; let Some(oct) = &other_handle.octree else { panic!("No octree!") @@ -214,15 +179,12 @@ fn get_overlapping_nodes( 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 { + let lp = IVec3::new(x, y, z) << log2; + if oct.query(lp, log2) > -2 { if cancel_early { return (true, None); } else { - blocks.as_mut().unwrap().push(Vector3::new( - (x << log2) + other_min.x, - (y << log2) + other_min.y, - (z << log2) + other_min.z, - )); + blocks.as_mut().unwrap().push(lp + other_min); } } } @@ -237,60 +199,38 @@ 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 mut blocks = if cancel_early { - None - } else { - Some(Vec::with_capacity(8)) - }; + let [min_octree_pos, max_octree_pos] = + [min_block_pos, max_block_pos].map(|v| v >> OCTREE_CHUNK_SHIFT); + + let mut blocks = cancel_early.then(|| Vec::with_capacity(8)); + for ox in min_octree_pos.x..=max_octree_pos.x { for oy in min_octree_pos.y..=max_octree_pos.y { for oz in min_octree_pos.z..=max_octree_pos.z { - let chunk = scene.octree_chunks.get(&pack_section_pos(ox, oy, oz)); + let opos = IVec3::new(ox, oy, oz); + let chunk = scene.octree_chunks.get(&pack_section_pos(opos)); let Some(chunk) = chunk else { continue; }; + let min = min_block_pos >> log2; + let max = max_block_pos >> log2; - let min_x = min_block_pos.x >> log2; - let min_y = min_block_pos.y >> log2; - let min_z = min_block_pos.z >> log2; - let max_x = max_block_pos.x >> log2; - let max_y = max_block_pos.y >> log2; - let max_z = max_block_pos.z >> log2; let chunk_octree = if liquid { &chunk.liquid_octree } else { &chunk.octree }; - - for x in min_x..=max_x { - for y in min_y..=max_y { - for z in min_z..=max_z { - if chunk_octree.query( - (x << log2) - (ox << OCTREE_CHUNK_SHIFT), - (y << log2) - (oy << OCTREE_CHUNK_SHIFT), - (z << log2) - (oz << OCTREE_CHUNK_SHIFT), - log2, - ) > -2 - { + let olp = opos << OCTREE_CHUNK_SHIFT; + for x in min.x..=max.x { + for y in min.y..=max.y { + for z in min.z..=max.z { + let lp = IVec3::new(x, y, z) << log2; + let query = lp - olp; + if chunk_octree.query(query, log2) > -2 { 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(lp); } } } diff --git a/common/src/main/rust/rapier/src/boxes.rs b/common/src/main/rust/rapier/src/boxes.rs index cb22eb9..7e9b7d4 100644 --- a/common/src/main/rust/rapier/src/boxes.rs +++ b/common/src/main/rust/rapier/src/boxes.rs @@ -4,11 +4,10 @@ use jni::sys::{jdouble, jint}; use marten::Real; use rapier3d::dynamics::RigidBodyBuilder; use rapier3d::geometry::{ColliderBuilder, SharedShape}; -use rapier3d::glamx::Quat; -use rapier3d::math::Vector; +use rapier3d::math::Pose3; -use crate::get_scene_mut; use crate::scene::LevelColliderID; +use crate::{PoseExt, get_scene_mut}; #[unsafe(no_mangle)] pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_createBox<'local>( @@ -25,22 +24,10 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cre let mut pose_arr: [jdouble; 7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; env.get_double_array_region(pose, 0, &mut pose_arr).unwrap(); - let quat = Quat::from_xyzw( - pose_arr[3] as Real, - pose_arr[4] as Real, - pose_arr[5] as Real, - pose_arr[6] as Real, - ); - - let mut rigid_body = RigidBodyBuilder::dynamic() + let rigid_body = RigidBodyBuilder::dynamic() .ccd_enabled(true) - .translation(Vector::new( - pose_arr[0] as Real, - pose_arr[1] as Real, - pose_arr[2] as Real, - )) + .pose(Pose3::from_jdouble_array(pose_arr)) .build(); - rigid_body.set_rotation(quat, false); let scene = get_scene_mut(scene_id); diff --git a/common/src/main/rust/rapier/src/buoyancy.rs b/common/src/main/rust/rapier/src/buoyancy.rs index a0e1d4a..a527d93 100644 --- a/common/src/main/rust/rapier/src/buoyancy.rs +++ b/common/src/main/rust/rapier/src/buoyancy.rs @@ -7,8 +7,8 @@ use crate::{ use marten::Real; use rapier3d::dynamics::RigidBody; use rapier3d::geometry::Aabb; -use rapier3d::math::Vector; -use rapier3d::na::Vector3; +use rapier3d::glamx::IVec3; +use rapier3d::math::Vec3; use rapier3d::prelude::RigidBodyVelocity; pub fn compute_buoyancy(scene: &mut PhysicsScene) { @@ -46,28 +46,18 @@ pub fn compute_buoyancy(scene: &mut PhysicsScene) { ); let vels: RigidBodyVelocity = *body.vels(); - let complex = (local_bounds_max - local_bounds_min).sum() < 10; + let complex = (local_bounds_max - local_bounds_min).element_sum() < 10; for (static_pos, dynamic_pos) in pairs.iter() { - let local_pos = Vector3::::new( - dynamic_pos.x as f64 + 0.5, - dynamic_pos.y as f64 + 0.5, - dynamic_pos.z as f64 + 0.5, - ); - let local_pos = Vector::new( - (local_pos.x - center_of_mass.x) as Real, - (local_pos.y - center_of_mass.y) as Real, - (local_pos.z - center_of_mass.z) as Real, - ); + let mut local_pos = ((dynamic_pos.as_dvec3() + 0.5) - center_of_mass).as_vec3(); + //TODO reduce duplication if complex { for i in 0..8 { let x = (i & 1) * 2 - 1; let y = ((i >> 1) & 1) * 2 - 1; let z = ((i >> 2) & 1) * 2 - 1; - let local_pos = Vector::new( - local_pos.x + x as Real * 0.25, - local_pos.y + y as Real * 0.25, - local_pos.z + z as Real * 0.25, - ); + + local_pos += IVec3::new(x, y, z).as_vec3() * 0.25; + do_drag(body, &vels, static_pos, &local_pos, 0.25, 1.0); } } else { @@ -76,53 +66,35 @@ pub fn compute_buoyancy(scene: &mut PhysicsScene) { } let scene = state.scenes.get_mut(&scene.scene_id).unwrap(); for (static_pos, dynamic_pos) in pairs.iter() { - let chunk = scene.get_chunk(dynamic_pos.x >> 4, dynamic_pos.y >> 4, dynamic_pos.z >> 4); + let chunk = scene.get_chunk(dynamic_pos >> 4); if chunk.is_none() { continue; } - let (block_id, _voxel_collider_state) = chunk.unwrap().get_block( - dynamic_pos.x & 15, - dynamic_pos.y & 15, - dynamic_pos.z & 15, - ); + let (block_id, _voxel_collider_state) = chunk.unwrap().get_block(dynamic_pos & 15); // block id's are unsigned, and offset by 1 to allow for a single "empty" at 0 if block_id == 0 { continue; } - let voxel_collider_data = &state.voxel_collider_map.get( - (block_id - 1) as usize, - Vector3::new(dynamic_pos.x, dynamic_pos.y, dynamic_pos.z), - ); + let voxel_collider_data = &state + .voxel_collider_map + .get((block_id - 1) as usize, *dynamic_pos); let Some(voxel_collider_data) = &voxel_collider_data else { continue; }; + let mut local_pos = ((dynamic_pos.as_dvec3() + 0.5) - center_of_mass).as_vec3(); - let local_pos = Vector3::::new( - dynamic_pos.x as f64 + 0.5, - dynamic_pos.y as f64 + 0.5, - dynamic_pos.z as f64 + 0.5, - ); - let local_pos = Vector::new( - (local_pos.x - center_of_mass.x) as Real, - (local_pos.y - center_of_mass.y) as Real, - (local_pos.z - center_of_mass.z) as Real, - ); - let complex = (local_bounds_max - local_bounds_min).sum() < 10; + let complex = (local_bounds_max - local_bounds_min).element_sum() < 10; if complex { for i in 0..8 { let x = (i & 1) * 2 - 1; let y = ((i >> 1) & 1) * 2 - 1; let z = ((i >> 2) & 1) * 2 - 1; - let local_pos = Vector::new( - local_pos.x + x as Real * 0.25, - local_pos.y + y as Real * 0.25, - local_pos.z + z as Real * 0.25, - ); + local_pos += IVec3::new(x, y, z).as_vec3() * 0.25; do_float( body, static_pos, @@ -144,30 +116,19 @@ pub fn compute_buoyancy(scene: &mut PhysicsScene) { } } +//TODO reduce duplication fn do_drag( body: &mut RigidBody, vels: &RigidBodyVelocity, - static_pos: &Vector3, - point: &Vector, + static_pos: &IVec3, + point: &Vec3, size: Real, strength: Real, ) { let point = body.position().transform_point(*point); - - let overlap = Aabb::new(point - Vector::splat(size), point + Vector::splat(size)).intersection( - &Aabb::new( - Vector::new( - static_pos.x as Real, - static_pos.y as Real, - static_pos.z as Real, - ), - Vector::new( - static_pos.x as Real + 1.0, - static_pos.y as Real + 1.0, - static_pos.z as Real + 1.0, - ), - ), - ); + let static_pos = static_pos.as_vec3(); + let overlap = Aabb::new(point - Vec3::splat(size), point + Vec3::splat(size)) + .intersection(&Aabb::new(static_pos, static_pos + 1.0)); if overlap.is_none() { return; @@ -179,29 +140,11 @@ fn do_drag( body.add_force_at_point(-velo * 1.7 * volume * strength, point, false); } -fn do_float( - body: &mut RigidBody, - static_pos: &Vector3, - point: &Vector, - size: Real, - strength: Real, -) { +fn do_float(body: &mut RigidBody, static_pos: &IVec3, point: &Vec3, size: Real, strength: Real) { let point = body.position().transform_point(*point); - - let overlap = Aabb::new(point - Vector::splat(size), point + Vector::splat(size)).intersection( - &Aabb::new( - Vector::new( - static_pos.x as Real, - static_pos.y as Real, - static_pos.z as Real, - ), - Vector::new( - static_pos.x as Real + 1.0, - static_pos.y as Real + 1.0, - static_pos.z as Real + 1.0, - ), - ), - ); + let static_pos = static_pos.as_vec3(); + let overlap = Aabb::new(point - Vec3::splat(size), point + Vec3::splat(size)) + .intersection(&Aabb::new(static_pos, static_pos + 1.0)); if overlap.is_none() { return; @@ -209,9 +152,5 @@ fn do_float( let volume = overlap.unwrap().volume(); - body.add_force_at_point( - Vector::new(0.0, 10.5 * volume * strength, 0.0), - point, - false, - ); + body.add_force_at_point(Vec3::new(0.0, 10.5 * volume * strength, 0.0), point, false); } diff --git a/common/src/main/rust/rapier/src/collider.rs b/common/src/main/rust/rapier/src/collider.rs index b1eecb4..da85b24 100644 --- a/common/src/main/rust/rapier/src/collider.rs +++ b/common/src/main/rust/rapier/src/collider.rs @@ -2,7 +2,7 @@ use crate::PHYSICS_STATE; use crate::scene::LevelColliderID; use rapier3d::dynamics::MassProperties; use rapier3d::geometry::{Shape, ShapeType, TypedShape}; -use rapier3d::math::Vector; +use rapier3d::math::Vec3; use rapier3d::parry::bounding_volume::{Aabb, BoundingSphere}; use rapier3d::prelude::*; use std::f32::consts::PI; @@ -30,7 +30,7 @@ impl LevelCollider { } } - fn scaled(self, _scale: &Vector) -> Self { + fn scaled(self, _scale: &Vec3) -> Self { Self { ..self } } } @@ -49,7 +49,7 @@ impl RayCast for LevelCollider { impl PointQuery for LevelCollider { fn project_local_point( &self, - _pt: Vector, + _pt: Vec3, _solid: bool, ) -> rapier3d::parry::query::PointProjection { todo!() @@ -57,7 +57,7 @@ impl PointQuery for LevelCollider { fn project_local_point_and_get_feature( &self, - _pt: Vector, + _pt: Vec3, ) -> (rapier3d::parry::query::PointProjection, FeatureId) { todo!() } @@ -67,47 +67,32 @@ impl Shape for LevelCollider { fn compute_local_aabb(&self) -> Aabb { if self.is_static { Aabb::new( - Vector::new(-WORLD_SIZE, -WORLD_SIZE, -WORLD_SIZE), - Vector::new(WORLD_SIZE, WORLD_SIZE, WORLD_SIZE), + Vec3::new(-WORLD_SIZE, -WORLD_SIZE, -WORLD_SIZE), + Vec3::new(WORLD_SIZE, WORLD_SIZE, WORLD_SIZE), ) } else { - unsafe { - let Some(state) = &PHYSICS_STATE else { - panic!("no physics state!") - }; - - let Some(scene) = state.scenes.get(&self.scene_id) else { - panic!("No scene with given ID!"); - }; - - let sable_body = &scene.level_colliders[&{ self.id.unwrap() }]; - - let center_of_mass = sable_body.center_of_mass.unwrap(); - let local_min = sable_body.local_bounds_min.unwrap(); - let local_max = sable_body.local_bounds_max.unwrap(); - - let min = Vector::new( - (local_min.x as f64 - center_of_mass.x) as Real, - (local_min.y as f64 - center_of_mass.y) as Real, - (local_min.z as f64 - center_of_mass.z) as Real, - ); - - let max = Vector::new( - ((local_max.x + 1) as f64 - center_of_mass.x) as Real, - ((local_max.y + 1) as f64 - center_of_mass.y) as Real, - ((local_max.z + 1) as f64 - center_of_mass.z) as Real, - ); - - Aabb::new(min, max) - } + let state = unsafe { PHYSICS_STATE.as_ref() }.expect("physics state to be present"); + let scene = state.scenes.get(&self.scene_id).expect("scene"); + + let sable_body = &scene.level_colliders[&{ self.id.unwrap() }]; + + let center_of_mass = sable_body.center_of_mass.unwrap(); + let local_min = sable_body.local_bounds_min.unwrap(); + let local_max = sable_body.local_bounds_max.unwrap(); + //TODO this gets duplicated a lot, there should be a factory function on AABB for it. + let min = (local_min.as_dvec3() - center_of_mass).as_vec3(); + + let max = ((local_max + 1).as_dvec3() - center_of_mass).as_vec3(); + + Aabb::new(min, max) } } fn compute_local_bounding_sphere(&self) -> BoundingSphere { if self.is_static { - BoundingSphere::new(Vector::ZERO, WORLD_SIZE) + BoundingSphere::new(Vec3::ZERO, WORLD_SIZE) } else { - BoundingSphere::new(Vector::ZERO, 1.0) + BoundingSphere::new(Vec3::ZERO, 1.0) // Bounding sphere that covers the entire bounding box // unsafe { // let Some(state) = &PHYSICS_STATE else { @@ -125,15 +110,15 @@ impl Shape for LevelCollider { Box::new(*self) } - fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vec3, _num_subdivisions: u32) -> Option> { Some(Box::new(self.scaled(&scale))) } fn mass_properties(&self, _density: Real) -> MassProperties { MassProperties { inv_mass: 0.0, - inv_principal_inertia: AngVector::new(0.0, 0.0, 0.0), - local_com: Vector::ZERO, + inv_principal_inertia: Vec3::ZERO, + local_com: Vec3::ZERO, principal_inertia_local_frame: Default::default(), } } diff --git a/common/src/main/rust/rapier/src/contraptions.rs b/common/src/main/rust/rapier/src/contraptions.rs index 1611453..4f0af06 100644 --- a/common/src/main/rust/rapier/src/contraptions.rs +++ b/common/src/main/rust/rapier/src/contraptions.rs @@ -3,19 +3,16 @@ use std::collections::HashMap; use jni::JNIEnv; use jni::objects::{JClass, JDoubleArray, JIntArray}; use jni::sys::{jdouble, jint}; -use marten::Real; use rapier3d::dynamics::RigidBodyBuilder; use rapier3d::geometry::{ColliderBuilder, SharedShape}; -use rapier3d::glamx::{Pose3, Quat}; -use rapier3d::math::Vector; -use rapier3d::na::Vector3; +use rapier3d::glamx::{DVec3, IVec3, Pose3}; use rapier3d::pipeline::{ActiveEvents, ActiveHooks}; use rapier3d::prelude::{RigidBodyHandle, RigidBodyVelocity}; use crate::collider::LevelCollider; use crate::groups::LEVEL_GROUP; use crate::scene::LevelColliderID; -use crate::{ActiveLevelColliderInfo, get_scene_mut_ref}; +use crate::{ActiveLevelColliderInfo, PoseExt, get_scene_mut_ref}; macro_rules! extract_jdouble_array { ($env:expr, $jarr:expr, $len:expr) => {{ @@ -119,17 +116,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set let center_of_mass_arr = extract_jdouble_array!(env, center_of_mass, 3); let pose_arr = extract_jdouble_array!(env, pose, 7); let velocities_arr = extract_jdouble_array!(env, velocities, 6); - let translation = Vector3::new( - pose_arr[0] as Real, - pose_arr[1] as Real, - pose_arr[2] as Real, - ); - let quat = Quat::from_xyzw( - pose_arr[3] as Real, - pose_arr[4] as Real, - pose_arr[5] as Real, - pose_arr[6] as Real, - ); + + let isometry = Pose3::from_jdouble_array(pose_arr); let scene = get_scene_mut_ref(scene_id); let info = get_kinematic_collider_info(scene, id); @@ -142,11 +130,6 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set return; } - let isometry = Pose3 { - rotation: quat, - translation: Vector::new(translation.x, translation.y, translation.z), - }; - // if (info.static_mount.is_some()) { // let body = scene.rigid_body_set.get_mut(info.static_mount.unwrap()).unwrap(); // @@ -162,23 +145,11 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set collider.set_position_wrt_parent(isometry); // } - info.center_of_mass = Some(Vector3::new( - center_of_mass_arr[0], - center_of_mass_arr[1], - center_of_mass_arr[2], - )); + info.center_of_mass = Some(DVec3::from_array(center_of_mass_arr)); info.fake_velocities = Some(RigidBodyVelocity::new( - Vector::new( - velocities_arr[0] as Real, - velocities_arr[1] as Real, - velocities_arr[2] as Real, - ), - Vector::new( - velocities_arr[3] as Real, - velocities_arr[4] as Real, - velocities_arr[5] as Real, - ), + DVec3::from_array(*velocities_arr.first_chunk().expect("const inbounds")).as_vec3(), + DVec3::from_array(*velocities_arr[3..].first_chunk().expect("const inbounds")).as_vec3(), )); } @@ -212,7 +183,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add let info = get_kinematic_collider_info(scene, id); if let Some(chunk_map) = &mut info.chunk_map { - chunk_map.insert(crate::scene::pack_section_pos(x, y, z), chunk); + chunk_map.insert(crate::scene::pack_section_pos(IVec3::new(x, y, z)), chunk); } } diff --git a/common/src/main/rust/rapier/src/dispatcher.rs b/common/src/main/rust/rapier/src/dispatcher.rs index 98be179..b89c743 100644 --- a/common/src/main/rust/rapier/src/dispatcher.rs +++ b/common/src/main/rust/rapier/src/dispatcher.rs @@ -1,9 +1,8 @@ use crate::collider::LevelCollider; use log::info; use rapier3d::geometry::{ContactManifoldData, Shape}; -use rapier3d::glamx::Pose3; -use rapier3d::math::Vector; -use rapier3d::na::Vector3; +use rapier3d::glamx::{DVec3, IVec3, Pose3}; +use rapier3d::math::Vec3; use rapier3d::parry::query::details::{NormalConstraints, contact_manifold_cuboid_cuboid_shapes}; use rapier3d::parry::query::{ ClosestPoints, Contact, ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, @@ -17,7 +16,7 @@ use crate::algo::find_collision_pairs; use crate::scene::{ChunkAccess, LevelColliderID, SableManifoldInfo}; use crate::{ActiveLevelColliderInfo, PhysicsState, get_physics_state, get_scene_ref}; use marten::level::VoxelPhysicsState::{Edge, Face, Interior}; -use marten::level::{NEEDS_HOOKS_USER_DATA, VoxelPhysicsState}; +use marten::level::{CollisionBox, NEEDS_HOOKS_USER_DATA, VoxelPhysicsState}; use std::sync::atomic::Ordering; /// The distance we scale collision points local to the box collider for before we check interior collisions @@ -30,8 +29,6 @@ const INTERIOR_COLLISION_SCALE_FACTOR: Real = 0.99; /// to rule it as an interior collision const INTERIOR_COLLISION_CHECK_DISTANCE: f64 = 0.015; -type IVec3 = Vector3; - pub struct SableDispatcher; impl SableDispatcher { @@ -40,22 +37,13 @@ impl SableDispatcher { #[allow(unused)] fn get_local_block_bounds(mut local_aabb: Aabb, inflation: Real) -> (IVec3, IVec3) { // Inflate the aabb by the prediction distance - local_aabb.maxs += Vector::splat(inflation); - local_aabb.mins -= Vector::splat(inflation); - - let local_min = IVec3::new( - local_aabb.mins.x.floor() as i32, - local_aabb.mins.y.floor() as i32, - local_aabb.mins.z.floor() as i32, - ); - - let local_max = IVec3::new( - local_aabb.maxs.x.floor() as i32, - local_aabb.maxs.y.floor() as i32, - local_aabb.maxs.z.floor() as i32, - ); + local_aabb.maxs += Vec3::splat(inflation); + local_aabb.mins -= Vec3::splat(inflation); - (local_min, local_max) + ( + local_aabb.mins.floor().as_ivec3(), + local_aabb.maxs.floor().as_ivec3(), + ) } } @@ -109,7 +97,7 @@ impl QueryDispatcher for SableDispatcher { fn cast_shapes( &self, _pos12: &Pose3, - _local_vel12: Vector, + _local_vel12: Vec3, _g1: &dyn Shape, _g2: &dyn Shape, _options: ShapeCastOptions, @@ -189,9 +177,9 @@ where .unwrap(); let extents_1 = body_1.local_bounds_max.unwrap() - body_1.local_bounds_min.unwrap() - + Vector3::new(1, 1, 1); + + IVec3::ONE; let extents_2 = body_2.local_bounds_max.unwrap() - body_2.local_bounds_min.unwrap() - + Vector3::new(1, 1, 1); + + IVec3::ONE; let volume_1 = extents_1.x * extents_1.y * extents_1.z; let volume_2 = extents_2.x * extents_2.y * extents_2.z; @@ -254,14 +242,13 @@ impl SableDispatcher { let collider_info = g1 .id .map(|id| &scene.level_colliders[&(id as LevelColliderID)]); - let center_of_mass_1 = - collider_info.map_or(Vector3::zeros(), |b| b.center_of_mass.unwrap()); + let center_of_mass_1 = collider_info.map_or(DVec3::ZERO, |b| b.center_of_mass.unwrap()); let mut local_aabb = g2.compute_aabb(pos12); let margin: Real = 0.1; - local_aabb.maxs += Vector::splat(prediction + margin); - local_aabb.mins -= Vector::splat(prediction + margin); + local_aabb.maxs += Vec3::splat(prediction + margin); + local_aabb.mins -= Vec3::splat(prediction + margin); let local_aabb = Self::adjust_aabb_for_body(local_aabb, collider_info, center_of_mass_1, prediction); let (local_min, local_max) = @@ -280,11 +267,12 @@ impl SableDispatcher { for x in local_min.x..=local_max.x { for y in local_min.y..=local_max.y { for z in local_min.z..=local_max.z { - let Some(chunk) = chunk_access.get_chunk(x >> 4, y >> 4, z >> 4) else { + let pos = IVec3::new(x, y, z); + let Some(chunk) = chunk_access.get_chunk(pos >> 4) else { // chunk doesn't exist continue; }; - let (block_id, _voxel_collider_state) = chunk.get_block(x & 15, y & 15, z & 15); + let (block_id, _voxel_collider_state) = chunk.get_block(pos & 15); // block id's are unsigned, and offset by 1 to allow for a single "empty" at 0 if block_id == 0 { @@ -307,27 +295,16 @@ impl SableDispatcher { continue; } - for (min_x, min_y, min_z, max_x, max_y, max_z) in - &voxel_collider_data.collision_boxes - { + for CollisionBox { min, max } in &voxel_collider_data.collision_boxes { if manifolds.len() <= manifold_index { manifolds.push(ContactManifold::new()); } - let center = Vector3::new( - ((min_x + max_x) / 2.0) as f64, - ((min_y + max_y) / 2.0) as f64, - ((min_z + max_z) / 2.0) as f64, - ) + Vector3::new(x as f64, y as f64, z as f64) - - center_of_mass_1; - let center = - Vector::new(center.x as Real, center.y as Real, center.z as Real); - - let half_extents = Vector::new( - (max_x - min_x) / 2.0, - (max_y - min_y) / 2.0, - (max_z - min_z) / 2.0, - ); + let center = (((min + max) / 2.0).as_dvec3() + pos.as_dvec3() + - center_of_mass_1) + .as_vec3(); + + let half_extents = (max - min) / 2.0; // Translate to match the center of the current block let mut block_isometry = *pos12; @@ -337,11 +314,7 @@ impl SableDispatcher { DefaultQueryDispatcher .contact_manifold_convex_convex( &block_isometry, - &rapier3d::parry::shape::Cuboid::new(Vector::new( - half_extents.x, - half_extents.y, - half_extents.z, - )), + &rapier3d::parry::shape::Cuboid::new(half_extents), g2, None, None, @@ -354,11 +327,7 @@ impl SableDispatcher { .contact_manifold_convex_convex( &block_isometry.inverse(), g2, - &rapier3d::parry::shape::Cuboid::new(Vector::new( - half_extents.x, - half_extents.y, - half_extents.z, - )), + &rapier3d::parry::shape::Cuboid::new(half_extents), None, None, prediction, @@ -377,7 +346,7 @@ impl SableDispatcher { } for point in &mut manifolds[manifold_index].points { - let diff = Vector::new(center.x, center.y, center.z); + let diff = Vec3::new(center.x, center.y, center.z); match swap { true => point.local_p2 -= diff, false => point.local_p1 += diff, @@ -410,8 +379,7 @@ impl SableDispatcher { .id .map(|id| &scene.level_colliders[&(id as LevelColliderID)]); let collider_info_2 = &scene.level_colliders[&(g2.id.unwrap() as LevelColliderID)]; - let center_of_mass_1 = - collider_info_1.map_or(Vector3::zeros(), |b| b.center_of_mass.unwrap()); + let center_of_mass_1 = collider_info_1.map_or(DVec3::ZERO, |b| b.center_of_mass.unwrap()); let center_of_mass_2 = collider_info_2.center_of_mass.unwrap(); let chunk_access_1: &dyn ChunkAccess = if let Some(info) = collider_info_1 @@ -446,58 +414,39 @@ impl SableDispatcher { // return; // } for (static_pos, dynamic_pos) in pairs.iter() { - let static_x = static_pos.x; - let static_y = static_pos.y; - let static_z = static_pos.z; - - let other_bx = dynamic_pos.x; - let other_by = dynamic_pos.y; - let other_bz = dynamic_pos.z; - - let Some(chunk) = chunk_access_1.get_chunk(static_x >> 4, static_y >> 4, static_z >> 4) - else { + let Some(chunk) = chunk_access_1.get_chunk(static_pos >> 4) else { // chunk doesn't exist continue; }; - let (block_id, voxel_collider_state) = - chunk.get_block(static_x & 15, static_y & 15, static_z & 15); + let (block_id, voxel_collider_state) = chunk.get_block(static_pos & 15); // block id's are unsigned, and offset by 1 to allow for a single "empty" at 0 if block_id == 0 { continue; } - let voxel_collider_data = &physics_state.voxel_collider_map.get( - (block_id - 1) as usize, - IVec3::new(static_x, static_y, static_z), - ); + let voxel_collider_data = &physics_state + .voxel_collider_map + .get((block_id - 1) as usize, *static_pos); let Some(voxel_collider_data) = &voxel_collider_data else { continue; }; - for (min_x, min_y, min_z, max_x, max_y, max_z) in &voxel_collider_data.collision_boxes { + for CollisionBox { min, max } in &voxel_collider_data.collision_boxes { if manifolds.len() <= manifold_index { manifolds.push(ContactManifold::new()); } - let center = Vector3::new( - ((min_x + max_x) / 2.0) as f64, - ((min_y + max_y) / 2.0) as f64, - ((min_z + max_z) / 2.0) as f64, - ) + Vector3::new(static_x as f64, static_y as f64, static_z as f64) - - center_of_mass_1; - let center = Vector3::new(center.x as Real, center.y as Real, center.z as Real); + let center = (((min + max) / 2.0).as_dvec3() + static_pos.as_dvec3() + - center_of_mass_1) + .as_vec3(); - let half_extents = Vector3::new( - (max_x - min_x) / 2.0, - (max_y - min_y) / 2.0, - (max_z - min_z) / 2.0, - ); + let half_extents = (max - min) / 2.0; // Translate to match the center of the current block let mut block_isometry = *pos12; - block_isometry.translation -= Vector::new(center.x, center.y, center.z); + block_isometry.translation -= Vec3::new(center.x, center.y, center.z); // let block_bounds = Aabb::new( // Point3::new(-half_extents.x, -half_extents.y, -half_extents.z) @@ -512,14 +461,12 @@ impl SableDispatcher { // for other_bx in other_block_min.x..=other_block_max.x { // for other_by in other_block_min.y..=other_block_max.y { // for other_bz in other_block_min.z..=other_block_max.z { - let Some(other_chunk) = - chunk_access_2.get_chunk(other_bx >> 4, other_by >> 4, other_bz >> 4) - else { + let Some(other_chunk) = chunk_access_2.get_chunk(dynamic_pos >> 4) else { // chunk doesn't exist continue; }; let (other_block_id, other_voxel_collider_state) = - other_chunk.get_block(other_bx & 15, other_by & 15, other_bz & 15); + other_chunk.get_block(dynamic_pos & 15); // block id's are unsigned, and offset by 1 to allow for a single "empty" at 0 if other_block_id == 0 { @@ -530,55 +477,34 @@ impl SableDispatcher { continue; } - let other_voxel_collider_data = &physics_state.voxel_collider_map.get( - (other_block_id - 1) as usize, - IVec3::new(other_bx, other_by, other_bz), - ); + let other_voxel_collider_data = &physics_state + .voxel_collider_map + .get((other_block_id - 1) as usize, *dynamic_pos); let Some(other_voxel_collider_data) = &other_voxel_collider_data else { continue; }; - for ( - other_min_x, - other_min_y, - other_min_z, - other_max_x, - other_max_y, - other_max_z, - ) in &other_voxel_collider_data.collision_boxes + for CollisionBox { + min: other_min, + max: other_max, + } in &other_voxel_collider_data.collision_boxes { if manifolds.len() <= manifold_index { manifolds.push(ContactManifold::new()); } - let other_center = - Vector3::new( - ((other_min_x + other_max_x) / 2.0) as f64, - ((other_min_y + other_max_y) / 2.0) as f64, - ((other_min_z + other_max_z) / 2.0) as f64, - ) + Vector3::new(other_bx as f64, other_by as f64, other_bz as f64) - - center_of_mass_2; - let other_center = Vector3::new( - other_center.x as Real, - other_center.y as Real, - other_center.z as Real, - ); + let other_center = (((other_min + other_max) / 2.0).as_dvec3() + + dynamic_pos.as_dvec3() + - center_of_mass_2) + .as_vec3(); - let other_half_extents = Vector3::new( - (other_max_x - other_min_x) / 2.0, - (other_max_y - other_min_y) / 2.0, - (other_max_z - other_min_z) / 2.0, - ); + let other_half_extents = (other_max - other_min) / 2.0; // combine block isometries let mut combined_block_isometry = block_isometry; - let transformed = combined_block_isometry.rotation.mul_vec3(Vector::new( - other_center.x, - other_center.y, - other_center.z, - )); + let transformed = combined_block_isometry.rotation.mul_vec3(other_center); combined_block_isometry.translation += transformed; @@ -586,16 +512,8 @@ impl SableDispatcher { ContactManifold::new(); contact_manifold_cuboid_cuboid_shapes( &combined_block_isometry, - &rapier3d::parry::shape::Cuboid::new(Vector::new( - half_extents.x, - half_extents.y, - half_extents.z, - )), - &rapier3d::parry::shape::Cuboid::new(Vector::new( - other_half_extents.x, - other_half_extents.y, - other_half_extents.z, - )), + &rapier3d::parry::shape::Cuboid::new(half_extents), + &rapier3d::parry::shape::Cuboid::new(other_half_extents), prediction, &mut new_manifold, ); @@ -605,8 +523,8 @@ impl SableDispatcher { chunk_access_2, collider_info_1, collider_info_2, - IVec3::new(static_x, static_y, static_z), - IVec3::new(other_bx, other_by, other_bz), + *static_pos, + *dynamic_pos, center, other_center, center_of_mass_1, @@ -622,15 +540,15 @@ impl SableDispatcher { index, if swap { SableManifoldInfo { - pos_a: IVec3::new(other_bx, other_by, other_bz), - pos_b: IVec3::new(static_x, static_y, static_z), + pos_a: *dynamic_pos, + pos_b: *static_pos, col_a: other_block_id as usize, col_b: block_id as usize, } } else { SableManifoldInfo { - pos_a: IVec3::new(static_x, static_y, static_z), - pos_b: IVec3::new(other_bx, other_by, other_bz), + pos_a: *static_pos, + pos_b: *dynamic_pos, col_a: block_id as usize, col_b: other_block_id as usize, } @@ -653,9 +571,8 @@ impl SableDispatcher { manifolds[manifold_index] = new_manifold; for point in &mut manifolds[manifold_index].points { - point.local_p1 += Vector::new(center.x, center.y, center.z); - point.local_p2 += - Vector::new(other_center.x, other_center.y, other_center.z); + point.local_p1 += center; + point.local_p2 += other_center; } manifold_index += 1; @@ -687,7 +604,7 @@ impl SableDispatcher { fn adjust_aabb_for_body( mut local_aabb: Aabb, body: Option<&ActiveLevelColliderInfo>, - center_of_mass: Vector3, + center_of_mass: DVec3, prediction: Real, ) -> Aabb { if let Some(body) = body { @@ -695,16 +612,8 @@ impl SableDispatcher { let local_bounds_max = body.local_bounds_max.unwrap(); let body_aabb = Aabb::new( - Vector::new( - (local_bounds_min.x as f64 - center_of_mass.x) as Real - prediction, - (local_bounds_min.y as f64 - center_of_mass.y) as Real - prediction, - (local_bounds_min.z as f64 - center_of_mass.z) as Real - prediction, - ), - Vector::new( - ((local_bounds_max.x + 1) as f64 - center_of_mass.x) as Real + prediction, - ((local_bounds_max.y + 1) as f64 - center_of_mass.y) as Real + prediction, - ((local_bounds_max.z + 1) as f64 - center_of_mass.z) as Real + prediction, - ), + (local_bounds_min.as_dvec3() - center_of_mass).as_vec3() - prediction, + ((local_bounds_max + 1).as_dvec3() - center_of_mass).as_vec3() + prediction, ); local_aabb = local_aabb.intersection(&body_aabb).unwrap_or(local_aabb); @@ -717,34 +626,13 @@ impl SableDispatcher { #[inline(always)] fn calculate_local_bounds( aabb: Aabb, - center_of_mass: Vector3, + center_of_mass: DVec3, prediction: Real, ) -> (IVec3, IVec3) { - let maxs = Vector3::::new( - aabb.maxs.x as f64 + center_of_mass.x, - aabb.maxs.y as f64 + center_of_mass.y, - aabb.maxs.z as f64 + center_of_mass.z, - ) + Vector3::repeat(prediction as f64); - - let mins = Vector3::::new( - aabb.mins.x as f64 + center_of_mass.x, - aabb.mins.y as f64 + center_of_mass.y, - aabb.mins.z as f64 + center_of_mass.z, - ) - Vector3::repeat(prediction as f64); - - let local_min = IVec3::new( - mins.x.floor() as i32, - mins.y.floor() as i32, - mins.z.floor() as i32, - ); + let maxs = aabb.maxs.as_dvec3() + center_of_mass + DVec3::splat(prediction as f64); + let mins = aabb.mins.as_dvec3() + center_of_mass - DVec3::splat(prediction as f64); - let local_max = IVec3::new( - maxs.x.floor() as i32, - maxs.y.floor() as i32, - maxs.z.floor() as i32, - ); - - (local_min, local_max) + (mins.floor().as_ivec3(), maxs.floor().as_ivec3()) } #[inline(always)] @@ -753,26 +641,13 @@ impl SableDispatcher { voxel_collider_state: VoxelPhysicsState, other_voxel_collider_state: VoxelPhysicsState, ) -> bool { - (other_voxel_collider_state == voxel_collider_state && voxel_collider_state == Face) - || voxel_collider_state == Interior - || other_voxel_collider_state == Interior - || (voxel_collider_state == Edge && other_voxel_collider_state == Face) - || (voxel_collider_state == Face && other_voxel_collider_state == Edge) + matches!( + (voxel_collider_state, other_voxel_collider_state), + (Face, Face) | (Interior, _) | (_, Interior) | (Edge, Face) | (Face, Edge) + ) } } -fn to_f64(vec: Vector) -> Vector3 { - Vector3::new(vec.x as f64, vec.y as f64, vec.z as f64) -} - -fn get_block_pos(vec: Vector3) -> IVec3 { - IVec3::new( - vec.x.floor() as i32, - vec.y.floor() as i32, - vec.z.floor() as i32, - ) -} - fn is_interior_collision( chunk_access_1: &dyn ChunkAccess, chunk_access_2: &dyn ChunkAccess, @@ -780,10 +655,10 @@ fn is_interior_collision, - other_center: Vector3, - center_of_mass_1: Vector3, - center_of_mass_2: Vector3, + center: Vec3, + other_center: Vec3, + center_of_mass_1: DVec3, + center_of_mass_2: DVec3, manifold: &mut ContactManifold, ) -> bool { let physics_state = unsafe { get_physics_state() }; @@ -793,12 +668,9 @@ fn is_interior_collision, + world_pos: DVec3, ) -> bool { - let block_pos = get_block_pos(world_pos); - - if ignore_block == block_pos { - return false; - } - - let Some(chunk) = chunk_access.get_chunk(block_pos.x >> 4, block_pos.y >> 4, block_pos.z >> 4) - else { - return false; - }; - let (block_id, _) = chunk.get_block(block_pos.x & 15, block_pos.y & 15, block_pos.z & 15); - - if block_id == 0 { - return false; - } - - let voxel_data = physics_state - .voxel_collider_map - .get((block_id - 1) as usize, block_pos); - - let Some(voxel_data) = voxel_data else { - return false; - }; - - if voxel_data.is_fluid { - return false; - } - - let local_pos = Vector3::new( - world_pos.x - block_pos.x as f64, - world_pos.y - block_pos.y as f64, - world_pos.z - block_pos.z as f64, - ); - - for &(min_x, min_y, min_z, max_x, max_y, max_z) in &voxel_data.collision_boxes { - if local_pos.x as Real >= min_x - && local_pos.x as Real <= max_x - && local_pos.y as Real >= min_y - && local_pos.y as Real <= max_y - && local_pos.z as Real >= min_z - && local_pos.z as Real <= max_z - { - return true; - } + let floor_pos = world_pos.floor(); + let block_pos = floor_pos.as_ivec3(); + + if ignore_block != block_pos + && let Some(chunk) = chunk_access.get_chunk(block_pos >> 4) + && let (block_id, _) = chunk.get_block(block_pos & 15) + && block_id != 0 + && let Some(voxel_data) = physics_state + .voxel_collider_map + .get((block_id - 1) as usize, block_pos) + && !voxel_data.is_fluid + { + let local_pos = (world_pos - floor_pos).as_vec3(); + voxel_data + .collision_boxes + .iter() + .any(|&CollisionBox { min, max }| { + local_pos.cmpge(min).all() && local_pos.cmple(max).all() + }) + } else { + false } - - false } diff --git a/common/src/main/rust/rapier/src/event_handler.rs b/common/src/main/rust/rapier/src/event_handler.rs index b527431..a4323c4 100644 --- a/common/src/main/rust/rapier/src/event_handler.rs +++ b/common/src/main/rust/rapier/src/event_handler.rs @@ -2,7 +2,6 @@ use crate::collider::LevelCollider; use crate::{PHYSICS_STATE, ReportedCollision}; use rapier3d::dynamics::RigidBodySet; use rapier3d::geometry::{ColliderSet, CollisionEvent, ContactPair}; -use rapier3d::na::Vector3; use rapier3d::pipeline::EventHandler; use rapier3d::prelude::*; @@ -61,26 +60,10 @@ impl EventHandler for SableEventHandler { body_a: level_collider_a.id, body_b: level_collider_b.id, force_amount: total_force_magnitude as f64, - local_normal_a: Vector3::::new( - local_n1.x as f64, - local_n1.y as f64, - local_n1.z as f64, - ), - local_normal_b: Vector3::::new( - local_n2.x as f64, - local_n2.y as f64, - local_n2.z as f64, - ), - local_point_a: Vector3::::new( - local_p1.x as f64, - local_p1.y as f64, - local_p1.z as f64, - ), - local_point_b: Vector3::::new( - local_p2.x as f64, - local_p2.y as f64, - local_p2.z as f64, - ), + local_normal_a: local_n1.as_dvec3(), + local_normal_b: local_n2.as_dvec3(), + local_point_a: local_p1.as_dvec3(), + local_point_b: local_p2.as_dvec3(), }; scene.reported_collisions.push(collision); diff --git a/common/src/main/rust/rapier/src/hooks.rs b/common/src/main/rust/rapier/src/hooks.rs index 0ab3fbd..5616070 100644 --- a/common/src/main/rust/rapier/src/hooks.rs +++ b/common/src/main/rust/rapier/src/hooks.rs @@ -4,8 +4,8 @@ use jni::sys::{jdouble, jint, jvalue}; use marten::Real; use marten::level::VoxelColliderData; use rapier3d::geometry::{Collider, SolverContact}; -use rapier3d::math::{Pose, Vec3, Vector}; -use rapier3d::na::Vector3; +use rapier3d::glamx::DVec3; +use rapier3d::math::{Pose, Vec3}; use rapier3d::pipeline::{ContactModificationContext, PhysicsHooks}; use crate::collider::LevelCollider; @@ -37,7 +37,7 @@ impl PhysicsHooks for SablePhysicsHooks { continue; } - let mut tangent_velo: Vector = Vector::ZERO; + let mut tangent_velo: Vec3 = Vec3::ZERO; let mut velocity = 0.0; let mut friction_multiplier = 1.0; @@ -117,7 +117,7 @@ impl SablePhysicsHooks { contact: &SolverContact, collider_a: &Collider, level_collider_a: Option<&LevelCollider>, - ) -> Vector { + ) -> Vec3 { if let Some(level_collider_a) = level_collider_a && level_collider_a.id.is_some() { @@ -131,11 +131,11 @@ impl SablePhysicsHooks { let transform = collider_a.position(); return transform.transform_vector(fake_velo.velocity_at_point( transform.inverse_transform_point(contact.point), - Vector::ZERO, + Vec3::ZERO, )); }; } - Vector::new(0.0, 0.0, 0.0) + Vec3::ZERO } } @@ -143,25 +143,25 @@ fn handle_block_params( isometry: &Pose, _collider: &Collider, level_collider: Option<&LevelCollider>, - global_point: &Vector, + global_point: &Vec3, velocity: Real, manifold_index: usize, body_a: bool, -) -> (Vector, bool, Real, Real) { +) -> (Vec3, bool, Real, Real) { let state = unsafe { get_physics_state() }; let scene = get_scene_mut(level_collider.unwrap().scene_id); let collider_info = level_collider.and_then(|lc| lc.id.map(|id| &scene.level_colliders[&(id)])); - let mut tangent_velo: Vector = Vector::ZERO; + let mut tangent_velo = Vec3::ZERO; if collider_info.is_some() && let Some(fake_velo) = collider_info.unwrap().fake_velocities { - tangent_velo += isometry.transform_vector(fake_velo.velocity_at_point( - isometry.inverse_transform_point(*global_point), - Vector::ZERO, - )); + tangent_velo += isometry.transform_vector( + fake_velo + .velocity_at_point(isometry.inverse_transform_point(*global_point), Vec3::ZERO), + ); }; // Get manifold info from the map @@ -181,10 +181,9 @@ fn handle_block_params( manifold_info.col_b as u32 }; - let center_of_mass = collider_info.map_or(Vector3::zeros(), |b| b.center_of_mass.unwrap()); + let center_of_mass = collider_info.map_or(DVec3::ZERO, |b| b.center_of_mass.unwrap()); let local = isometry.inverse_transform_point(*global_point); - let block_coord_d: Vector3 = - Vector3::new(local.x as f64, local.y as f64, local.z as f64) + center_of_mass; + let block_coord_d = local.as_dvec3() + center_of_mass; if block_id == 0 { return (tangent_velo, false, 1.0, 0.0); @@ -245,11 +244,7 @@ fn handle_block_params( .get_double_array_region(arr, 0, &mut velo_arr) .unwrap(); - let velo = Vec3::new( - velo_arr[0] as Real, - velo_arr[1] as Real, - velo_arr[2] as Real, - ); + let velo = DVec3::from_array(*velo_arr.first_chunk().expect("const inbounds")).as_vec3(); ( tangent_velo + isometry.transform_vector(velo), diff --git a/common/src/main/rust/rapier/src/joints.rs b/common/src/main/rust/rapier/src/joints.rs index 71f6a7d..17a7f12 100644 --- a/common/src/main/rust/rapier/src/joints.rs +++ b/common/src/main/rust/rapier/src/joints.rs @@ -8,9 +8,8 @@ use marten::Real; use rapier3d::dynamics::{ GenericJointBuilder, JointAxesMask, JointAxis, RevoluteJointBuilder, SpringCoefficients, }; -use rapier3d::glamx::Quat; -use rapier3d::math::Vector; -use rapier3d::na::Vector3; +use rapier3d::glamx::{DQuat, DVec3, Quat}; +use rapier3d::math::Vec3; use rapier3d::prelude::{FixedJointBuilder, ImpulseJointHandle}; use std::collections::HashMap; @@ -21,10 +20,10 @@ struct SubLevelJoint { id_a: Option, id_b: Option, - pos_a: Vector3, - pos_b: Vector3, - normal_a: Vector3, - normal_b: Vector3, + pos_a: DVec3, + pos_b: DVec3, + normal_a: DVec3, + normal_b: DVec3, rotation_a: Option, rotation_b: Option, @@ -34,20 +33,11 @@ struct SubLevelJoint { fixed: bool, contacts_enabled: bool, } - +#[derive(Default)] pub struct SableJointSet { joints: HashMap, } -impl SableJointSet { - #[must_use] - pub fn new() -> Self { - Self { - joints: HashMap::new(), - } - } -} - pub fn tick(scene_id: jint) { let scene = get_scene_mut_ref(scene_id); // filter the joints @@ -63,43 +53,31 @@ pub fn tick(scene_id: jint) { .unwrap(); impulse_joint.data.contacts_enabled = joint.contacts_enabled; if !joint.fixed && joint.rotation_a.is_none() { - impulse_joint.data.set_local_axis1(Vector::new( - joint.normal_a.x as Real, - joint.normal_a.y as Real, - joint.normal_a.z as Real, - )); + impulse_joint.data.set_local_axis1(joint.normal_a.as_vec3()); } let local_anchor_1 = joint.pos_a - - if let Some(id_a) = joint.id_a { - let rb_a = &scene.level_colliders[&id_a]; - rb_a.center_of_mass.unwrap() - } else { - Vector3::new(0.0, 0.0, 0.0) - }; - impulse_joint.data.set_local_anchor1(Vector::new( - local_anchor_1.x as Real, - local_anchor_1.y as Real, - local_anchor_1.z as Real, - )); + - joint + .id_a + .map(|v| scene.level_colliders[&v].center_of_mass.unwrap()) + .unwrap_or_default(); + + impulse_joint + .data + .set_local_anchor1(local_anchor_1.as_vec3()); + if !joint.fixed && joint.rotation_b.is_none() { - impulse_joint.data.set_local_axis2(Vector::new( - joint.normal_b.x as Real, - joint.normal_b.y as Real, - joint.normal_b.z as Real, - )); + impulse_joint.data.set_local_axis2(joint.normal_b.as_vec3()); } let local_anchor_2 = joint.pos_b - - if let Some(id_b) = joint.id_b { - let rb_b = &scene.level_colliders[&id_b]; - rb_b.center_of_mass.unwrap() - } else { - Vector3::new(0.0, 0.0, 0.0) - }; - impulse_joint.data.set_local_anchor2(Vector::new( - local_anchor_2.x as Real, - local_anchor_2.y as Real, - local_anchor_2.z as Real, - )); + - joint + .id_b + .map(|v| scene.level_colliders[&v].center_of_mass.unwrap()) + .unwrap_or_default(); + + impulse_joint + .data + .set_local_anchor2(local_anchor_2.as_vec3()); + if let Some(rotation_a) = joint.rotation_a { impulse_joint.data.local_frame1.rotation = rotation_a; } @@ -270,10 +248,12 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add }; let revolute = RevoluteJointBuilder::new( - Vector::new(axis_x_a as Real, axis_y_a as Real, axis_z_a as Real).normalize(), + DVec3::new(axis_x_a, axis_y_a, axis_z_a) + .as_vec3() + .normalize(), ) - .local_anchor1(Vector::ZERO) - .local_anchor2(Vector::ZERO) + .local_anchor1(Vec3::ZERO) + .local_anchor2(Vec3::ZERO) .softness(SpringCoefficients::new( JOINT_SPRING_FREQUENCY, JOINT_SPRING_DAMPING_RATIO, @@ -300,11 +280,11 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add Some(id_b as LevelColliderID) }, - pos_a: Vector3::new(local_x_a, local_y_a, local_z_a), - pos_b: Vector3::new(local_x_b, local_y_b, local_z_b), + pos_a: DVec3::new(local_x_a, local_y_a, local_z_a), + pos_b: DVec3::new(local_x_b, local_y_b, local_z_b), - normal_a: Vector3::new(axis_x_a, axis_y_a, axis_z_a), - normal_b: Vector3::new(axis_x_b, axis_y_b, axis_z_b), + normal_a: DVec3::new(axis_x_a, axis_y_a, axis_z_a), + normal_b: DVec3::new(axis_x_b, axis_y_b, axis_z_b), rotation_a: None, rotation_b: None, @@ -353,15 +333,10 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add scene.rigid_bodies[&(id_b as LevelColliderID)] }; - let quat = Quat::from_xyzw( - local_q_x as Real, - local_q_y as Real, - local_q_z as Real, - local_q_w as Real, - ); + let quat = DQuat::from_xyzw(local_q_x, local_q_y, local_q_z, local_q_w).as_quat(); let mut revolute = FixedJointBuilder::new() - .local_anchor1(Vector::ZERO) - .local_anchor2(Vector::ZERO) + .local_anchor1(Vec3::ZERO) + .local_anchor2(Vec3::ZERO) .softness(SpringCoefficients::new( JOINT_SPRING_FREQUENCY, JOINT_SPRING_DAMPING_RATIO, @@ -389,11 +364,11 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add Some(id_b as LevelColliderID) }, - pos_a: Vector3::new(local_x_a, local_y_a, local_z_a), - pos_b: Vector3::new(local_x_b, local_y_b, local_z_b), + pos_a: DVec3::new(local_x_a, local_y_a, local_z_a), + pos_b: DVec3::new(local_x_b, local_y_b, local_z_b), - normal_a: Vector3::new(0.0, 0.0, 0.0), - normal_b: Vector3::new(0.0, 0.0, 0.0), + normal_a: DVec3::ZERO, + normal_b: DVec3::ZERO, rotation_a: None, rotation_b: None, @@ -446,13 +421,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add SpringCoefficients::new(JOINT_SPRING_FREQUENCY, JOINT_SPRING_DAMPING_RATIO), ); - let quat = Quat::from_xyzw( - local_q_x as Real, - local_q_y as Real, - local_q_z as Real, - local_q_w as Real, - ); - joint.0.local_frame1.rotation = quat; + joint.0.local_frame1.rotation = + DQuat::from_xyzw(local_q_x, local_q_y, local_q_z, local_q_w).as_quat(); let handle = scene .impulse_joint_set @@ -475,11 +445,11 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add Some(id_b as LevelColliderID) }, - pos_a: Vector3::new(local_x_a, local_y_a, local_z_a), - pos_b: Vector3::new(local_x_b, local_y_b, local_z_b), + pos_a: DVec3::new(local_x_a, local_y_a, local_z_a), + pos_b: DVec3::new(local_x_b, local_y_b, local_z_b), - normal_a: Vector3::new(0.0, 0.0, 0.0), - normal_b: Vector3::new(0.0, 0.0, 0.0), + normal_a: DVec3::ZERO, + normal_b: DVec3::ZERO, rotation_a: None, rotation_b: None, @@ -535,18 +505,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add let locked_axes = JointAxesMask::from_bits_truncate(locked_axes_mask as u8); - let rotation_a = Quat::from_xyzw( - local_q_x_a as Real, - local_q_y_a as Real, - local_q_z_a as Real, - local_q_w_a as Real, - ); - let rotation_b = Quat::from_xyzw( - local_q_x_b as Real, - local_q_y_b as Real, - local_q_z_b as Real, - local_q_w_b as Real, - ); + let rotation_a = DQuat::from_xyzw(local_q_x_a, local_q_y_a, local_q_z_a, local_q_w_a).as_quat(); + let rotation_b = DQuat::from_xyzw(local_q_x_b, local_q_y_b, local_q_z_b, local_q_w_b).as_quat(); let mut joint = GenericJointBuilder::new(locked_axes).softness(SpringCoefficients::new( JOINT_SPRING_FREQUENCY, @@ -576,11 +536,11 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add Some(id_b as LevelColliderID) }, - pos_a: Vector3::new(local_x_a as f64, local_y_a as f64, local_z_a as f64), - pos_b: Vector3::new(local_x_b as f64, local_y_b as f64, local_z_b as f64), + pos_a: DVec3::new(local_x_a, local_y_a, local_z_a), + pos_b: DVec3::new(local_x_b, local_y_b, local_z_b), - normal_a: Vector3::new(0.0, 0.0, 0.0), - normal_b: Vector3::new(0.0, 0.0, 0.0), + normal_a: DVec3::ZERO, + normal_b: DVec3::ZERO, rotation_a: Some(rotation_a), rotation_b: Some(rotation_b), @@ -617,13 +577,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set return; }; - let position = Vector3::new(local_x as f64, local_y as f64, local_z as f64); - let rotation = Quat::from_xyzw( - local_q_x as Real, - local_q_y as Real, - local_q_z as Real, - local_q_w as Real, - ); + let position = DVec3::new(local_x, local_y, local_z); + let rotation = DQuat::from_xyzw(local_q_x, local_q_y, local_q_z, local_q_w).as_quat(); match side { 0 => { diff --git a/common/src/main/rust/rapier/src/lib.rs b/common/src/main/rust/rapier/src/lib.rs index 6e3f5eb..772817a 100644 --- a/common/src/main/rust/rapier/src/lib.rs +++ b/common/src/main/rust/rapier/src/lib.rs @@ -18,8 +18,7 @@ mod voxel_collider; use jni::objects::{JClass, JDoubleArray, JIntArray}; use jni::sys::{jboolean, jdouble, jint}; use jni::{JNIEnv, JavaVM}; -use rapier3d::glamx::Quat; -use rapier3d::math::Vector; +use rapier3d::glamx::{DMat3, DQuat, DVec3, IVec3}; use std::collections::HashMap; use fern::colors::{Color, ColoredLevelConfig}; @@ -27,23 +26,16 @@ use log::info; use crate::buoyancy::compute_buoyancy; use crate::collider::LevelCollider; -use crate::dispatcher::SableDispatcher; -use crate::event_handler::SableEventHandler; use crate::groups::LEVEL_GROUP; -use crate::joints::SableJointSet; -use crate::rope::RopeMap; use crate::scene::{ChunkAccess, ChunkMap, SableManifoldInfoMap, pack_section_pos}; use crate::voxel_collider::VoxelColliderMap; -use hooks::SablePhysicsHooks; use marten::Real; use marten::level::VoxelPhysicsState::Interior; use marten::level::{ ALL_VOXEL_PHYSICS_STATES, BlockState, CHUNK_SHIFT, ChunkSection, OCTREE_CHUNK_SHIFT, - OCTREE_CHUNK_SIZE, OctreeChunkSection, VoxelPhysicsState, + OCTREE_CHUNK_SIZE, VoxelPhysicsState, }; use marten::octree::SubLevelOctree; -use rapier3d::na::{Matrix3, Vector3 as NaVector3}; -use rapier3d::parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use rapier3d::prelude::*; use scene::{LevelColliderID, PhysicsScene}; @@ -52,27 +44,24 @@ pub struct ActiveLevelColliderInfo { pub collider: ColliderHandle, pub static_mount: Option, pub fake_velocities: Option>, - pub local_bounds_min: Option>, - pub local_bounds_max: Option>, - pub center_of_mass: Option>, + pub local_bounds_min: Option, + pub local_bounds_max: Option, + pub center_of_mass: Option, pub octree: Option, pub chunk_map: Option, pub scene_id: jint, } impl ChunkAccess for ActiveLevelColliderInfo { - fn get_chunk_mut(&mut self, x: i32, y: i32, z: i32) -> Option<&mut ChunkSection> { + fn get_chunk_mut(&mut self, pos: IVec3) -> Option<&mut ChunkSection> { self.chunk_map .as_mut() .unwrap() - .get_mut(&pack_section_pos(x, y, z)) + .get_mut(&pack_section_pos(pos)) } - fn get_chunk(&self, x: i32, y: i32, z: i32) -> Option<&ChunkSection> { - self.chunk_map - .as_ref() - .unwrap() - .get(&pack_section_pos(x, y, z)) + fn get_chunk(&self, pos: IVec3) -> Option<&ChunkSection> { + self.chunk_map.as_ref().unwrap().get(&pack_section_pos(pos)) } } @@ -120,24 +109,16 @@ impl ActiveLevelColliderInfo { } /// Sets the local bounds for the object - pub fn set_local_bounds(&mut self, min: NaVector3, max: NaVector3, scene_id: jint) { + pub fn set_local_bounds(&mut self, min: IVec3, max: IVec3, scene_id: jint) { if Some(min) != self.local_bounds_min || Some(max) != self.local_bounds_max { self.local_bounds_min = Some(min); self.local_bounds_max = Some(max); - let max_axis = (max - min).max() as u32 + 1; + let max_axis = (max - min).max_element() as u32 + 1; let smallest_pow_2_above = max_axis.next_power_of_two(); - let chunk_min = NaVector3::new( - min.x >> CHUNK_SHIFT, - min.y >> CHUNK_SHIFT, - min.z >> CHUNK_SHIFT, - ); - let chunk_max = NaVector3::new( - max.x >> CHUNK_SHIFT, - max.y >> CHUNK_SHIFT, - max.z >> CHUNK_SHIFT, - ); + let chunk_min = min >> CHUNK_SHIFT; + let chunk_max = max >> CHUNK_SHIFT; self.octree = Some(SubLevelOctree::new( smallest_pow_2_above.trailing_zeros() as i32 @@ -155,20 +136,22 @@ impl ActiveLevelColliderInfo { for cx in chunk_min.x..=chunk_max.x { for cy in chunk_min.y..=chunk_max.y { for cz in chunk_min.z..=chunk_max.z { + let cpos = IVec3::new(cx, cy, cz); let chunk = if has_own_chunks { self.chunk_map .as_ref() .unwrap() - .get(&pack_section_pos(cx, cy, cz)) + .get(&pack_section_pos(cpos)) } else { - scene.get_chunk(cx, cy, cz) + scene.get_chunk(cpos) }; if let Some(chunk_section) = chunk { for x in 0..16 { for y in 0..16 { for z in 0..16 { - let block_owned = chunk_section.get_block(x, y, z); + let pos = IVec3::new(x, y, z); + let block_owned = chunk_section.get_block(pos); if block_owned.1 == VoxelPhysicsState::Empty { continue; } @@ -177,9 +160,7 @@ impl ActiveLevelColliderInfo { self.octree.as_mut().unwrap(), &block_owned, false, - (x + (cx << CHUNK_SHIFT)) - min.x, - (y + (cy << CHUNK_SHIFT)) - min.y, - (z + (cz << CHUNK_SHIFT)) - min.z, + (pos + (cpos << CHUNK_SHIFT)) - min, ); } } @@ -198,15 +179,14 @@ impl ActiveLevelColliderInfo { self.local_bounds_max = Some(max); } - fn insert_chunk(&mut self, chunk_section: &ChunkSection, cx: i32, cy: i32, cz: i32) { + fn insert_chunk(&mut self, chunk_section: &ChunkSection, cpos: IVec3) { for x in 0..16 { for y in 0..16 { for z in 0..16 { + let pos = IVec3::new(x, y, z); self.insert_block( - x + (cx << CHUNK_SHIFT), - y + (cy << CHUNK_SHIFT), - z + (cz << CHUNK_SHIFT), - &chunk_section.get_block(x, y, z), + pos + (cpos << CHUNK_SHIFT), + &chunk_section.get_block(pos), false, ); } @@ -214,32 +194,21 @@ impl ActiveLevelColliderInfo { } } - fn insert_block(&mut self, x: i32, y: i32, z: i32, state: &BlockState, remove: bool) { - let local_min = self.local_bounds_min.unwrap(); - let x = x - local_min.x; - let y = y - local_min.y; - let z = z - local_min.z; + fn insert_block(&mut self, mut pos: IVec3, state: &BlockState, remove: bool) { + pos -= self.local_bounds_min.unwrap(); let Some(octree) = &mut self.octree else { panic!("No octree!"); }; - insert_block_octree(octree, state, remove, x, y, z); + insert_block_octree(octree, state, remove, pos); } - fn contains(&self, x: i32, y: i32, z: i32) -> bool { - if self.local_bounds_min.is_none() || self.local_bounds_max.is_none() { - return false; + fn contains(&self, pos: IVec3) -> bool { + if let (Some(min), Some(max)) = &(self.local_bounds_min, self.local_bounds_max) { + pos.cmplt(*max).all() && pos.cmpgt(*min).all() + } else { + false } - - let local_min = self.local_bounds_min.unwrap(); - let local_max = self.local_bounds_max.unwrap(); - - x >= local_min.x - && x <= local_max.x - && y >= local_min.y - && y <= local_max.y - && z >= local_min.z - && z <= local_max.z } } @@ -260,10 +229,10 @@ pub struct PhysicsState { pub struct ReportedCollision { body_a: Option, body_b: Option, - local_point_a: NaVector3, - local_point_b: NaVector3, - local_normal_a: NaVector3, - local_normal_b: NaVector3, + local_point_a: DVec3, + local_point_b: DVec3, + local_normal_a: DVec3, + local_normal_b: DVec3, force_amount: f64, } @@ -312,7 +281,29 @@ pub fn get_rigid_body(scene: &PhysicsScene, id: LevelColliderID) -> &RigidBody { let handle = scene.rigid_bodies.get(&id).expect("No rigid body for id"); &scene.rigid_body_set[*handle] } - +pub trait PoseExt { + fn from_jdouble_array(pose_array: [jdouble; 7]) -> Self; + fn into_jdouble_array(self) -> [jdouble; 7]; +} +impl PoseExt for Pose3 { + #[inline] + fn from_jdouble_array(pose_array: [jdouble; 7]) -> Self { + let translation = + DVec3::from_array(*pose_array.first_chunk().expect("const inbounds")).as_vec3(); + let rotation = + DQuat::from_array(*pose_array[3..].first_chunk().expect("const inbounds")).as_quat(); + Self { + rotation, + translation, + } + } + #[inline] + fn into_jdouble_array(self) -> [jdouble; 7] { + let [x, y, z] = self.translation.as_dvec3().to_array(); + let [rx, ry, rz, rw] = self.rotation.as_dquat().to_array(); + [x, y, z, rx, ry, rz, rw] + } +} #[unsafe(no_mangle)] pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_initialize<'local>( env: JNIEnv<'local>, @@ -323,7 +314,9 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_ini z: jdouble, universal_drag: jdouble, ) { - if unsafe { &PHYSICS_STATE }.is_none() { + let ground = RigidBodyBuilder::fixed(); + + let state = unsafe { &mut PHYSICS_STATE }.get_or_insert_with(|| { let colors = ColoredLevelConfig::new() .info(Color::Green) .error(Color::Red) @@ -344,78 +337,47 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_ini .chain(std::io::stdout()) .apply(); - unsafe { - PHYSICS_STATE = Some(PhysicsState { - integration_parameters: IntegrationParameters { - dt: 1.0 / 20.0, - - max_ccd_substeps: 3, - normalized_prediction_distance: 0.005, - - contact_softness: SpringCoefficients { - natural_frequency: 30.0, - damping_ratio: 5.0, - }, - // joint_softness: SpringCoefficients { - // natural_frequency: 1.0e2, - // damping_ratio: 1.0, - // }, - normalized_max_corrective_velocity: 50.0, - normalized_allowed_linear_error: 0.0025, - - ..IntegrationParameters::default() + PhysicsState { + integration_parameters: IntegrationParameters { + dt: 1.0 / 20.0, + + max_ccd_substeps: 3, + normalized_prediction_distance: 0.005, + + contact_softness: SpringCoefficients { + natural_frequency: 30.0, + damping_ratio: 5.0, }, - voxel_collider_map: VoxelColliderMap::new(), - scenes: HashMap::new(), - }); + // joint_softness: SpringCoefficients { + // natural_frequency: 1.0e2, + // damping_ratio: 1.0, + // }, + normalized_max_corrective_velocity: 50.0, + normalized_allowed_linear_error: 0.0025, + + ..IntegrationParameters::default() + }, + voxel_collider_map: VoxelColliderMap::new(), + scenes: HashMap::new(), } - } + }); - unsafe { - let ground = RigidBodyBuilder::fixed(); + let collider = ColliderBuilder::new(SharedShape::new(LevelCollider::new(None, true, scene_id))) + .collision_groups(LEVEL_GROUP) + .build(); - if let Some(state) = &mut PHYSICS_STATE { - let collider = - ColliderBuilder::new(SharedShape::new(LevelCollider::new(None, true, scene_id))) - .collision_groups(LEVEL_GROUP) - .build(); - - let mut scene = PhysicsScene { - scene_id, - pipeline: PhysicsPipeline::new(), - rigid_body_set: RigidBodySet::new(), - collider_set: ColliderSet::new(), - island_manager: IslandManager::new(), - broad_phase: DefaultBroadPhase::new(), - narrow_phase: NarrowPhase::with_query_dispatcher( - SableDispatcher.chain(DefaultQueryDispatcher), - ), - impulse_joint_set: ImpulseJointSet::new(), - multibody_joint_set: MultibodyJointSet::new(), - ccd_solver: CCDSolver::new(), - physics_hooks: SablePhysicsHooks, - event_handler: SableEventHandler { scene_id }, - main_level_chunks: HashMap::::new(), - octree_chunks: HashMap::::new(), - reported_collisions: Vec::with_capacity(16), - joint_set: SableJointSet::new(), - ground_handle: None, - rope_map: RopeMap::default(), - level_colliders: HashMap::::new(), - rigid_bodies: HashMap::::new(), - current_step_vm: None, - gravity: Vector::new(x as Real, y as Real, z as Real), - universal_drag: universal_drag as Real, - manifold_info_map: SableManifoldInfoMap::default(), - }; + let mut scene = PhysicsScene::new( + scene_id, + universal_drag as Real, + DVec3::new(x, y, z).as_vec3(), + ); - scene.collider_set.insert(collider); - scene.ground_handle = Some(scene.rigid_body_set.insert(ground)); - scene.current_step_vm = - Some(JavaVM::from_raw(env.get_java_vm().unwrap().get_java_vm_pointer()).unwrap()); - state.scenes.insert(scene_id, scene); - } - } + scene.collider_set.insert(collider); + scene.ground_handle = Some(scene.rigid_body_set.insert(ground)); + scene.current_step_vm = Some( + unsafe { JavaVM::from_raw(env.get_java_vm().unwrap().get_java_vm_pointer()) }.unwrap(), + ); + state.scenes.insert(scene_id, scene); info!("Rapier initialized scene {}", scene_id); } @@ -450,34 +412,32 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_ste scene_id: jint, time_step: jdouble, ) { - unsafe { - if let Some(state) = &mut PHYSICS_STATE { - rope::tick(scene_id); - joints::tick(scene_id); - - state.integration_parameters.dt = time_step as f32; + if let Some(state) = unsafe { PHYSICS_STATE.as_mut() } { + rope::tick(scene_id); + joints::tick(scene_id); - let Some(scene) = state.scenes.get_mut(&scene_id) else { - panic!("No scene with given ID!"); - }; + state.integration_parameters.dt = time_step as f32; - scene.manifold_info_map = SableManifoldInfoMap::default(); + let Some(scene) = state.scenes.get_mut(&scene_id) else { + panic!("No scene with given ID!"); + }; - scene.pipeline.step( - scene.gravity, - &state.integration_parameters, - &mut scene.island_manager, - &mut scene.broad_phase, - &mut scene.narrow_phase, - &mut scene.rigid_body_set, - &mut scene.collider_set, - &mut scene.impulse_joint_set, - &mut scene.multibody_joint_set, - &mut scene.ccd_solver, - &scene.physics_hooks, - &scene.event_handler, - ); - } + scene.manifold_info_map = SableManifoldInfoMap::default(); + + scene.pipeline.step( + scene.gravity, + &state.integration_parameters, + &mut scene.island_manager, + &mut scene.broad_phase, + &mut scene.narrow_phase, + &mut scene.rigid_body_set, + &mut scene.collider_set, + &mut scene.impulse_joint_set, + &mut scene.multibody_joint_set, + &mut CCDSolver, + &scene.physics_hooks, + &scene.event_handler, + ); } } @@ -496,17 +456,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_get let rb: &RigidBody = &scene.rigid_body_set[scene.rigid_bodies[&(id as LevelColliderID)]]; - let arr: [jdouble; 7] = [ - rb.translation().x as jdouble, - rb.translation().y as jdouble, - rb.translation().z as jdouble, - rb.rotation().x as jdouble, - rb.rotation().y as jdouble, - rb.rotation().z as jdouble, - rb.rotation().w as jdouble, - ]; - - env.set_double_array_region(&store, 0, &arr).unwrap(); + env.set_double_array_region(&store, 0, &rb.position().into_jdouble_array()) + .unwrap(); } } @@ -532,7 +483,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set .level_colliders .get_mut(&(id as LevelColliderID)) .unwrap() - .center_of_mass = Some(NaVector3::new(x, y, z)); + .center_of_mass = Some(DVec3::new(x, y, z)); } } } @@ -563,8 +514,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set .get_mut(&(id as LevelColliderID)) .unwrap() .set_local_bounds( - NaVector3::new(min_x, min_y, min_z), - NaVector3::new(max_x, max_y, max_z), + IVec3::new(min_x, min_y, min_z), + IVec3::new(max_x, max_y, max_z), scene_id, ); } @@ -584,62 +535,48 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cre let mut pose_arr: [jdouble; 7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; env.get_double_array_region(pose, 0, &mut pose_arr).unwrap(); - let quat = Quat::from_xyzw( - pose_arr[3] as Real, - pose_arr[4] as Real, - pose_arr[5] as Real, - pose_arr[6] as Real, - ); - let mut rigid_body = RigidBodyBuilder::dynamic() .ccd_enabled(true) - .translation(Vector::new( - pose_arr[0] as Real, - pose_arr[1] as Real, - pose_arr[2] as Real, - )) + .pose(Pose3::from_jdouble_array(pose_arr)) .build(); - rigid_body.set_rotation(quat, false); let activation_params = rigid_body.activation_mut(); activation_params.angular_threshold = 0.15; activation_params.normalized_linear_threshold = 0.15; - unsafe { - if let Some(state) = &mut PHYSICS_STATE { - let Some(scene) = state.scenes.get_mut(&scene_id) else { - panic!("No scene with given ID!"); - }; + if let Some(state) = unsafe { &mut PHYSICS_STATE } { + let Some(scene) = state.scenes.get_mut(&scene_id) else { + panic!("No scene with given ID!"); + }; - rigid_body.set_linear_damping(scene.universal_drag); - rigid_body.set_angular_damping(scene.universal_drag); - - let handle = scene.rigid_body_set.insert(rigid_body); - - // make a level collider - let collider = ColliderBuilder::new(SharedShape::new(LevelCollider::new( - Some(id as LevelColliderID), - false, - scene_id, - ))) - .friction(0.525) - .active_events(ActiveEvents::CONTACT_FORCE_EVENTS) - .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) - .density(0.0) - .collision_groups(LEVEL_GROUP) - .build(); - - let collider_handle = - scene - .collider_set - .insert_with_parent(collider, handle, &mut scene.rigid_body_set); - - scene.level_colliders.insert( - id as LevelColliderID, - ActiveLevelColliderInfo::new(collider_handle, scene_id), - ); + rigid_body.set_linear_damping(scene.universal_drag); + rigid_body.set_angular_damping(scene.universal_drag); - scene.rigid_bodies.insert(id as LevelColliderID, handle); - } + let handle = scene.rigid_body_set.insert(rigid_body); + + // make a level collider + let collider = ColliderBuilder::new(SharedShape::new(LevelCollider::new( + Some(id as LevelColliderID), + false, + scene_id, + ))) + .friction(0.525) + .active_events(ActiveEvents::CONTACT_FORCE_EVENTS) + .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) + .density(0.0) + .collision_groups(LEVEL_GROUP) + .build(); + + let collider_handle = + scene + .collider_set + .insert_with_parent(collider, handle, &mut scene.rigid_body_set); + + scene.level_colliders.insert( + id as LevelColliderID, + ActiveLevelColliderInfo::new(collider_handle, scene_id), + ); + + scene.rigid_bodies.insert(id as LevelColliderID, handle); } } @@ -678,9 +615,7 @@ pub fn insert_block_octree( octree: &mut SubLevelOctree, state: &BlockState, remove: bool, - x: i32, - y: i32, - z: i32, + pos: IVec3, ) { let block_collider_id = state.0; let block_collider = if block_collider_id > 0 { @@ -706,13 +641,12 @@ pub fn insert_block_octree( .unwrap() .collision_boxes .is_empty()); - if remove && !solid { - octree.insert(x, y, z, -1); + octree.insert(pos, -1); } if solid { - octree.insert(x, y, z, block_collider_id as i32); + octree.insert(pos, block_collider_id as i32); } } @@ -745,21 +679,18 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add } let chunk = ChunkSection::new(blocks); - + let chunk_pos = IVec3::new(x, y, z); unsafe { if let Some(state) = &mut PHYSICS_STATE { let Some(scene) = state.scenes.get_mut(&scene_id) else { panic!("No scene with given ID!"); }; - - scene + let e = scene .main_level_chunks - .insert(pack_section_pos(x, y, z), chunk); + .entry(pack_section_pos(chunk_pos)) + .insert_entry(chunk); + let chunk = e.get(); - let chunk = scene - .main_level_chunks - .get(&pack_section_pos(x, y, z)) - .unwrap(); if global == 0 { // println!("receving non global physics chunk"); // println!("object id {:?}", object_id); @@ -769,7 +700,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add .get_mut(&(object_id as LevelColliderID)) .unwrap(); - body.insert_chunk(chunk, x, y, z); + body.insert_chunk(chunk, chunk_pos); // println!("inserting blocks to octree"); // println!("post octree {:?}", body.octree); // println!("post min {:?}", body.local_bounds_min); @@ -780,49 +711,27 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add for bx in 0..16 { for by in 0..16 { for bz in 0..16 { - let block = chunk.get_block(bx, by, bz); - let x = bx + (x << CHUNK_SHIFT); - let y = by + (y << CHUNK_SHIFT); - let z = bz + (z << CHUNK_SHIFT); + let bpos = IVec3::new(bx, by, bz); + let block = chunk.get_block(bpos); - // insert into level octree - let ox = x >> OCTREE_CHUNK_SHIFT; - let oy = y >> OCTREE_CHUNK_SHIFT; - let oz = z >> OCTREE_CHUNK_SHIFT; - - let mut octree_chunk = - scene.octree_chunks.get_mut(&pack_section_pos(ox, oy, oz)); - - if octree_chunk.is_none() { - scene.octree_chunks.insert( - pack_section_pos(ox, oy, oz), - OctreeChunkSection::new(), - ); - octree_chunk = - scene.octree_chunks.get_mut(&pack_section_pos(ox, oy, oz)); - } + let pos = bpos + (chunk_pos << CHUNK_SHIFT); - let Some(octree_chunk) = octree_chunk else { - panic!("No octree chunk!") - }; + // insert into level octree + let opos = pos >> OCTREE_CHUNK_SHIFT; + let octree_chunk = scene + .octree_chunks + .entry(pack_section_pos(opos)) + .or_default(); + let iopos = pos & (OCTREE_CHUNK_SIZE - 1); if block.0 == 0 { insert_block_octree( &mut octree_chunk.liquid_octree, &block, false, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), - ); - insert_block_octree( - &mut octree_chunk.octree, - &block, - false, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); + insert_block_octree(&mut octree_chunk.octree, &block, false, iopos); } else { if state.voxel_collider_map.voxel_colliders[(block.0 - 1) as usize] .as_ref() @@ -833,18 +742,14 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add &mut octree_chunk.liquid_octree, &block, false, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); } else { insert_block_octree( &mut octree_chunk.octree, &block, false, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); } } @@ -866,44 +771,39 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_rem z: jint, global: jboolean, ) { + let chunk_pos = IVec3::new(x, y, z); unsafe { if let Some(state) = &mut PHYSICS_STATE { let Some(scene) = state.scenes.get_mut(&scene_id) else { panic!("No scene with given ID!"); }; - scene.main_level_chunks.remove(&pack_section_pos(x, y, z)); + scene.main_level_chunks.remove(&pack_section_pos(chunk_pos)); if global > 0 { let octree_chunk = scene.octree_chunks.get_mut(&pack_section_pos( - (x << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, - (y << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, - (z << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, + (chunk_pos << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, )); if let Some(octree_chunk) = octree_chunk { for bx in 0..16 { for by in 0..16 { for bz in 0..16 { - let x = bx + (x << CHUNK_SHIFT); - let y = by + (y << CHUNK_SHIFT); - let z = bz + (z << CHUNK_SHIFT); + let bpos = IVec3::new(bx, by, bz); + let abs_pos = bpos + (chunk_pos << CHUNK_SHIFT); + let iopos = abs_pos & (OCTREE_CHUNK_SIZE - 1); insert_block_octree( &mut octree_chunk.octree, &(0, VoxelPhysicsState::Empty), true, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); insert_block_octree( &mut octree_chunk.liquid_octree, &(0, VoxelPhysicsState::Empty), true, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); } } @@ -913,9 +813,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_rem && octree_chunk.liquid_octree.buffer[0] == 0 { scene.octree_chunks.remove(&pack_section_pos( - (x << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, - (y << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, - (z << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, + (chunk_pos << CHUNK_SHIFT) >> OCTREE_CHUNK_SHIFT, )); } } @@ -936,6 +834,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cha ) { let block_collider_id = (block >> 16) as u16; let voxel_state_id = (block & 0xFFFF) as u16; + let block_pos = IVec3::new(x, y, z); unsafe { if let Some(state) = &mut PHYSICS_STATE { @@ -945,60 +844,37 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cha let chunk = scene .main_level_chunks - .get_mut(&pack_section_pos(x >> 4, y >> 4, z >> 4)); + .get_mut(&pack_section_pos(block_pos >> 4)); if let Some(chunk) = chunk { let block_state = ( block_collider_id as u32, ALL_VOXEL_PHYSICS_STATES[voxel_state_id as usize], ); - chunk.set_block(x & 15, y & 15, z & 15, block_state); + chunk.set_block(block_pos & 15, block_state); - let mut any = false; - for (_, sable_body) in scene.level_colliders.iter_mut() { - if sable_body.contains(x, y, z) { - sable_body.insert_block(x, y, z, &block_state, true); - any = true; - break; - } - } - - if !any { + if let Some((_, sable_body)) = scene + .level_colliders + .iter_mut() + .find(|(_, body)| body.contains(block_pos)) + { + sable_body.insert_block(block_pos, &block_state, true); + } else { // insert into level octree - let ox = x >> OCTREE_CHUNK_SHIFT; - let oy = y >> OCTREE_CHUNK_SHIFT; - let oz = z >> OCTREE_CHUNK_SHIFT; - - let mut octree_chunk = - scene.octree_chunks.get_mut(&pack_section_pos(ox, oy, oz)); - - if octree_chunk.is_none() { - scene - .octree_chunks - .insert(pack_section_pos(ox, oy, oz), OctreeChunkSection::new()); - octree_chunk = scene.octree_chunks.get_mut(&pack_section_pos(ox, oy, oz)); - } - - let Some(octree_chunk) = octree_chunk else { - panic!("No octree chunk!") - }; + let opos = block_pos >> OCTREE_CHUNK_SHIFT; + let octree_chunk = scene + .octree_chunks + .entry(pack_section_pos(opos)) + .or_default(); + let iopos = block_pos & (OCTREE_CHUNK_SIZE - 1); if block_collider_id == 0 { - insert_block_octree( - &mut octree_chunk.octree, - &block_state, - true, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), - ); + insert_block_octree(&mut octree_chunk.octree, &block_state, true, iopos); insert_block_octree( &mut octree_chunk.liquid_octree, &block_state, true, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); } else { if state @@ -1014,18 +890,14 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cha &mut octree_chunk.liquid_octree, &block_state, false, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); } else { insert_block_octree( &mut octree_chunk.octree, &block_state, false, - x & (OCTREE_CHUNK_SIZE - 1), - y & (OCTREE_CHUNK_SIZE - 1), - z & (OCTREE_CHUNK_SIZE - 1), + iopos, ); } } @@ -1055,24 +927,16 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set env.get_double_array_region(inertia, 0, &mut inertia_arr) .unwrap(); - let inertia_tensor = Matrix3::new( - inertia_arr[0] as Real, - inertia_arr[1] as Real, - inertia_arr[2] as Real, - inertia_arr[3] as Real, - inertia_arr[4] as Real, - inertia_arr[5] as Real, - inertia_arr[6] as Real, - inertia_arr[7] as Real, - inertia_arr[8] as Real, - ); - let scene = get_scene_mut_ref(scene_id); let rb = &mut scene.rigid_body_set[scene.rigid_bodies[&(id as LevelColliderID)]]; rb.set_additional_mass_properties( - MassProperties::with_inertia_matrix(Vector::ZERO, mass as Real, inertia_tensor.into()), + MassProperties::with_inertia_matrix( + Vec3::ZERO, + mass as Real, + DMat3::from_cols_array(&inertia_arr).as_mat3(), + ), true, ); } @@ -1097,10 +961,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_tel let scene = get_scene_mut_ref(scene_id); let rb = &mut scene.rigid_body_set[scene.rigid_bodies[&(id as LevelColliderID)]]; - let mut pose = *rb.position(); - pose.translation = Vector::new(x as Real, y as Real, z as Real); - pose.rotation = Quat::from_xyzw(i as Real, j as Real, k as Real, r as Real); - rb.set_position(pose, true); + rb.set_position(Pose3::from_jdouble_array([x, y, z, i, j, k, r]), true); } /// Wakes up an object. @@ -1142,11 +1003,11 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add } rb.set_linvel( - rb.linvel() + Vector::new(linear_x as Real, linear_y as Real, linear_z as Real), + rb.linvel() + DVec3::new(linear_x, linear_y, linear_z).as_vec3(), wake_up > 0, ); rb.set_angvel( - rb.angvel() + Vector::new(angular_x as Real, angular_y as Real, angular_z as Real), + rb.angvel() + DVec3::new(angular_x, angular_y, angular_z).as_vec3(), wake_up > 0, ); } @@ -1242,12 +1103,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_app return; } - let force: Vector = rb - .rotation() - .mul_vec3(Vector::new(fx as Real, fy as Real, fz as Real)); - let force_pos = rb - .position() - .transform_point(Vector::new(x as Real, y as Real, z as Real)); + let force: Vec3 = rb.rotation().mul_vec3(DVec3::new(fx, fy, fz).as_vec3()); + let force_pos = rb.position().transform_point(DVec3::new(x, y, z).as_vec3()); rb.apply_impulse(force, wake_up > 0); @@ -1289,14 +1146,10 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_app return; } - let force: Vector = rb - .rotation() - .mul_vec3(Vector::new(fx as Real, fy as Real, fz as Real)); + let force: Vec3 = rb.rotation().mul_vec3(DVec3::new(fx, fy, fz).as_vec3()); rb.apply_impulse(force, wake_up > 0); - let torque: Vector = rb - .rotation() - .mul_vec3(Vector::new(tx as Real, ty as Real, tz as Real)); + let torque: Vec3 = rb.rotation().mul_vec3(DVec3::new(tx, ty, tz).as_vec3()); rb.apply_torque_impulse(torque, wake_up > 0); } } diff --git a/common/src/main/rust/rapier/src/rope.rs b/common/src/main/rust/rapier/src/rope.rs index f2758e2..6abb7a7 100644 --- a/common/src/main/rust/rapier/src/rope.rs +++ b/common/src/main/rust/rapier/src/rope.rs @@ -6,8 +6,8 @@ use jni::sys::{jboolean, jdouble, jint, jlong, jsize}; use marten::Real; use rapier3d::dynamics::{GenericJointBuilder, JointAxis, RigidBodyBuilder, SpringCoefficients}; use rapier3d::geometry::{ColliderBuilder, SharedShape}; -use rapier3d::math::Vector; -use rapier3d::na::Vector3; +use rapier3d::glamx::DVec3; +use rapier3d::math::Vec3; use rapier3d::prelude::{ ImpulseJointHandle, ImpulseJointSet, JointAxesMask, RigidBodyHandle, RopeJointBuilder, }; @@ -23,7 +23,7 @@ const MIN_BOUND_DAMPING: Real = 10.0; struct RopeAttachment { joint: ImpulseJointHandle, sub_level_id: Option, - location: Vector3, + location: DVec3, } struct RopeStrand { @@ -55,16 +55,14 @@ pub fn tick(scene_id: jint) { let rb_b = &scene.level_colliders[&id_b]; rb_b.center_of_mass.unwrap() } else { - Vector3::new(0.0, 0.0, 0.0) + DVec3::new(0.0, 0.0, 0.0) }; let impulse_joint = scene .impulse_joint_set .get_mut(attachment.joint, false) .unwrap(); - impulse_joint - .data - .set_local_anchor1(Vector::from(local_anchor.map(|x| x as Real))); + impulse_joint.data.set_local_anchor1(local_anchor.as_vec3()); } } @@ -78,18 +76,14 @@ pub fn tick(scene_id: jint) { let rb_b = &scene.level_colliders[&id_b]; rb_b.center_of_mass.unwrap() } else { - Vector3::new(0.0, 0.0, 0.0) + DVec3::new(0.0, 0.0, 0.0) }; let impulse_joint = scene .impulse_joint_set .get_mut(attachment.joint, false) .unwrap(); - impulse_joint.data.set_local_anchor1(Vector::new( - local_anchor.x as Real, - local_anchor.y as Real, - local_anchor.z as Real, - )); + impulse_joint.data.set_local_anchor1(local_anchor.as_vec3()); } } } @@ -113,7 +107,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_cre let mut vec = Vec::with_capacity(num_points as usize); for i in 0..(num_points as usize) { - let coordinate = Vector::new( + let coordinate = Vec3::new( coordinates[i * 3] as Real, coordinates[i * 3 + 1] as Real, coordinates[i * 3 + 2] as Real, @@ -167,8 +161,8 @@ fn add_rope_joint( length: Real, ) -> (ImpulseJointHandle, ImpulseJointHandle) { let mut joint = RopeJointBuilder::new(length) - .local_anchor1(Vector::ZERO) - .local_anchor2(Vector::ZERO) + .local_anchor1(Vec3::ZERO) + .local_anchor2(Vec3::ZERO) .softness(SpringCoefficients::new( JOINT_SPRING_FREQUENCY, JOINT_SPRING_DAMPING_RATIO, @@ -203,7 +197,7 @@ fn add_rope_joint( (handle, damp_handle) } -fn create_rope_body(scene_id: i32, coordinate: Vector, point_radius: Real) -> RigidBodyHandle { +fn create_rope_body(scene_id: i32, coordinate: Vec3, point_radius: Real) -> RigidBodyHandle { let scene = get_scene_mut_ref(scene_id); let mut rigid_body = RigidBodyBuilder::dynamic() .translation(coordinate) @@ -383,11 +377,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add old_joint.set_limits(JointAxis::LinX, [0.0, 1.0]); old_joint.set_motor_position(JointAxis::LinX, 1.0, MIN_BOUND_STIFFNESS, MIN_BOUND_DAMPING); - let handle = create_rope_body( - scene_id, - Vector::new(x as Real, y as Real, z as Real), - point_radius, - ); + let handle = create_rope_body(scene_id, DVec3::new(x, y, z).as_vec3(), point_radius); strand.joints.insert( 0, add_rope_joint( @@ -458,8 +448,8 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set }; let joint = RopeJointBuilder::new(0.0) - .local_anchor1(Vector::ZERO) - .local_anchor2(Vector::ZERO) + .local_anchor1(Vec3::ZERO) + .local_anchor2(Vec3::ZERO) .softness(SpringCoefficients::new( JOINT_SPRING_FREQUENCY, JOINT_SPRING_DAMPING_RATIO, @@ -491,7 +481,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_set Some(sub_level_id as LevelColliderID) }, joint, - location: Vector3::new(x, y, z), + location: DVec3::new(x, y, z), }; if end > 0 { diff --git a/common/src/main/rust/rapier/src/scene.rs b/common/src/main/rust/rapier/src/scene.rs index 0a0d609..526b0a3 100644 --- a/common/src/main/rust/rapier/src/scene.rs +++ b/common/src/main/rust/rapier/src/scene.rs @@ -1,3 +1,4 @@ +use crate::dispatcher::SableDispatcher; use crate::event_handler::SableEventHandler; use crate::hooks::SablePhysicsHooks; use crate::joints::SableJointSet; @@ -8,11 +9,13 @@ use jni::JavaVM; use marten::Real; use marten::level::{ChunkSection, OctreeChunkSection}; use rapier3d::dynamics::{ - CCDSolver, ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet, + ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyHandle, RigidBodySet, }; use rapier3d::geometry::{ColliderSet, DefaultBroadPhase, NarrowPhase}; -use rapier3d::math::Vector; -use rapier3d::na::Vector3; +use rapier3d::glamx::IVec3; + +use rapier3d::math::Vec3; +use rapier3d::parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use rapier3d::pipeline::PhysicsPipeline; use std::collections::HashMap; use std::sync::atomic::AtomicUsize; @@ -21,12 +24,12 @@ pub type LevelColliderID = usize; pub trait ChunkAccess { #[allow(unused)] - fn get_chunk_mut(&mut self, x: i32, y: i32, z: i32) -> Option<&mut ChunkSection>; - fn get_chunk(&self, x: i32, y: i32, z: i32) -> Option<&ChunkSection>; + fn get_chunk_mut(&mut self, pos: IVec3) -> Option<&mut ChunkSection>; + fn get_chunk(&self, pos: IVec3) -> Option<&ChunkSection>; } #[inline(always)] -pub fn pack_section_pos(i: i32, j: i32, k: i32) -> i64 { +pub fn pack_section_pos(IVec3 { x: i, y: j, z: k }: IVec3) -> i64 { let mut l: i64 = 0; l |= (i as i64 & 4194303i64) << 42; l |= j as i64 & 1048575i64; @@ -47,7 +50,6 @@ pub struct PhysicsScene { pub narrow_phase: NarrowPhase, pub impulse_joint_set: ImpulseJointSet, pub multibody_joint_set: MultibodyJointSet, - pub ccd_solver: CCDSolver, pub physics_hooks: SablePhysicsHooks, pub event_handler: SableEventHandler, @@ -74,7 +76,7 @@ pub struct PhysicsScene { pub current_step_vm: Option, /// The current gravity vector for all bodies. [m/s^2] - pub gravity: Vector, + pub gravity: Vec3, /// Universal linear drag applied to all bodies pub universal_drag: Real, @@ -89,33 +91,57 @@ pub struct SableManifoldInfoMap { } pub struct SableManifoldInfo { - pub pos_a: Vector3, - pub pos_b: Vector3, + pub pos_a: IVec3, + pub pos_b: IVec3, pub col_a: usize, pub col_b: usize, } impl ChunkAccess for PhysicsScene { - fn get_chunk_mut(&mut self, x: i32, y: i32, z: i32) -> Option<&mut ChunkSection> { - self.main_level_chunks.get_mut(&pack_section_pos(x, y, z)) + fn get_chunk_mut(&mut self, pos: IVec3) -> Option<&mut ChunkSection> { + self.main_level_chunks.get_mut(&pack_section_pos(pos)) } - fn get_chunk(&self, x: i32, y: i32, z: i32) -> Option<&ChunkSection> { - self.main_level_chunks.get(&pack_section_pos(x, y, z)) + fn get_chunk(&self, pos: IVec3) -> Option<&ChunkSection> { + self.main_level_chunks.get(&pack_section_pos(pos)) } } impl PhysicsScene { - pub fn get_octree_chunk(&self, x: i32, y: i32, z: i32) -> Option<&OctreeChunkSection> { - self.octree_chunks.get(&pack_section_pos(x, y, z)) + pub fn new(scene_id: i32, universal_drag: f32, gravity: Vec3) -> Self { + Self { + scene_id, + pipeline: PhysicsPipeline::default(), + rigid_body_set: RigidBodySet::default(), + collider_set: ColliderSet::default(), + island_manager: IslandManager::default(), + broad_phase: DefaultBroadPhase::default(), + narrow_phase: NarrowPhase::with_query_dispatcher( + SableDispatcher.chain(DefaultQueryDispatcher), + ), + impulse_joint_set: ImpulseJointSet::default(), + multibody_joint_set: MultibodyJointSet::default(), + physics_hooks: SablePhysicsHooks, + event_handler: SableEventHandler { scene_id }, + main_level_chunks: HashMap::::default(), + octree_chunks: HashMap::::default(), + reported_collisions: Vec::with_capacity(16), + joint_set: SableJointSet::default(), + ground_handle: None, + rope_map: RopeMap::default(), + level_colliders: HashMap::::default(), + rigid_bodies: HashMap::::default(), + current_step_vm: None, + gravity, + universal_drag, + manifold_info_map: SableManifoldInfoMap::default(), + } + } + pub fn get_octree_chunk(&self, pos: IVec3) -> Option<&OctreeChunkSection> { + self.octree_chunks.get(&pack_section_pos(pos)) } - pub fn get_octree_chunk_mut( - &mut self, - x: i32, - y: i32, - z: i32, - ) -> Option<&mut OctreeChunkSection> { - self.octree_chunks.get_mut(&pack_section_pos(x, y, z)) + pub fn get_octree_chunk_mut(&mut self, pos: IVec3) -> Option<&mut OctreeChunkSection> { + self.octree_chunks.get_mut(&pack_section_pos(pos)) } } diff --git a/common/src/main/rust/rapier/src/voxel_collider.rs b/common/src/main/rust/rapier/src/voxel_collider.rs index 2badf7f..edacd39 100644 --- a/common/src/main/rust/rapier/src/voxel_collider.rs +++ b/common/src/main/rust/rapier/src/voxel_collider.rs @@ -4,13 +4,11 @@ use jni::JNIEnv; use jni::objects::{JClass, JDoubleArray, JObject}; use jni::sys::{jboolean, jdouble, jint}; use marten::Real; -use marten::level::{SableMethodID, VoxelColliderData}; -use rapier3d::na::Vector3; +use marten::level::{CollisionBox, SableMethodID, VoxelColliderData}; +use rapier3d::glamx::IVec3; use crate::get_physics_state_mut; -type IVec3 = Vector3; - /// The physics data of a blockstate #[derive(Debug)] pub struct VoxelColliderMap { @@ -113,14 +111,7 @@ pub extern "system" fn Java_dev_ryanhcode_sable_physics_impl_rapier_Rapier3D_add .unwrap(); if let Some(data) = &mut state.voxel_collider_map.voxel_colliders[index as usize] { - data.collision_boxes.push(( - bounds[0] as f32, - bounds[1] as f32, - bounds[2] as f32, - bounds[3] as f32, - bounds[4] as f32, - bounds[5] as f32, - )); + data.collision_boxes.push(CollisionBox::from(bounds)); } }