From ecd308338b189ab569816a38a03e3f8b89669dde Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 17 Mar 2024 21:20:18 +0100 Subject: feat: start experimenting with a glam/bevy version --- Cargo.toml | 21 +- crates/rapier2d/Cargo.toml | 46 +- crates/rapier3d/Cargo.toml | 48 +- crates/rapier_testbed3d/Cargo.toml | 2 + examples3d/Cargo.toml | 2 + examples3d/ccd3.rs | 4 +- examples3d/convex_polyhedron3.rs | 2 +- examples3d/debug_deserialize3.rs | 2 +- examples3d/debug_prismatic3.rs | 2 +- examples3d/joints3.rs | 18 +- examples3d/keva3.rs | 4 +- src/control/character_controller.rs | 112 +- src/control/ray_cast_vehicle_controller.rs | 114 +- src/data/arena.rs | 2 +- src/data/coarena.rs | 21 +- src/data/entity_arena.rs | 145 +++ src/data/mod.rs | 6 + src/dynamics/ccd/ccd_solver.rs | 55 +- src/dynamics/ccd/toi_entry.rs | 12 +- src/dynamics/coefficient_combine_rule.rs | 10 +- src/dynamics/integration_parameters.rs | 2 +- src/dynamics/island_manager.rs | 10 +- src/dynamics/joint/fixed_joint.rs | 26 +- src/dynamics/joint/generic_joint.rs | 65 +- src/dynamics/joint/impulse_joint/impulse_joint.rs | 4 +- .../joint/impulse_joint/impulse_joint_set.rs | 107 +- src/dynamics/joint/motor_model.rs | 2 +- src/dynamics/joint/multibody_joint/multibody.rs | 80 +- .../joint/multibody_joint/multibody_joint.rs | 47 +- .../joint/multibody_joint/multibody_joint_set.rs | 111 +- .../joint/multibody_joint/multibody_link.rs | 28 +- .../joint/multibody_joint/multibody_workspace.rs | 8 +- .../joint/multibody_joint/unit_multibody_joint.rs | 2 +- src/dynamics/joint/prismatic_joint.rs | 30 +- src/dynamics/joint/revolute_joint.rs | 21 +- src/dynamics/joint/rope_joint.rs | 14 +- src/dynamics/joint/spherical_joint.rs | 26 +- src/dynamics/joint/spring_joint.rs | 14 +- src/dynamics/rigid_body.rs | 178 ++-- src/dynamics/rigid_body_components.rs | 302 +++--- src/dynamics/rigid_body_set.rs | 61 +- src/dynamics/solver/categorization.rs | 4 +- .../contact_constraint/contact_constraints_set.rs | 14 +- .../generic_one_body_constraint.rs | 28 +- .../generic_one_body_constraint_element.rs | 17 +- .../generic_two_body_constraint.rs | 48 +- .../generic_two_body_constraint_element.rs | 75 +- .../contact_constraint/one_body_constraint.rs | 64 +- .../one_body_constraint_element.rs | 107 +- .../contact_constraint/one_body_constraint_simd.rs | 86 +- .../contact_constraint/two_body_constraint.rs | 112 +- .../two_body_constraint_element.rs | 143 ++- .../contact_constraint/two_body_constraint_simd.rs | 88 +- src/dynamics/solver/island_solver.rs | 2 +- .../joint_constraint/any_joint_constraint.rs | 2 +- .../joint_constraint/joint_constraint_builder.rs | 1096 +------------------- .../joint_constraint/joint_constraint_helper.rs | 141 +++ .../joint_constraint_helper_simd.rs | 19 + .../joint_constraint_helper_template.rs | 966 +++++++++++++++++ .../joint_constraint/joint_generic_constraint.rs | 24 +- .../joint_generic_constraint_builder.rs | 110 +- .../joint_constraint/joint_velocity_constraint.rs | 106 +- src/dynamics/solver/joint_constraint/mod.rs | 8 +- src/dynamics/solver/parallel_island_solver.rs | 2 +- src/dynamics/solver/parallel_solver_constraints.rs | 2 +- src/dynamics/solver/parallel_velocity_solver.rs | 2 +- src/dynamics/solver/solver_body.rs | 39 +- src/dynamics/solver/solver_constraints_set.rs | 2 +- src/dynamics/solver/solver_vel.rs | 25 +- src/dynamics/solver/velocity_solver.rs | 14 +- src/geometry/broad_phase_multi_sap/broad_phase.rs | 6 +- .../broad_phase_pair_event.rs | 8 +- src/geometry/broad_phase_multi_sap/sap_axis.rs | 2 +- src/geometry/broad_phase_multi_sap/sap_endpoint.rs | 2 +- src/geometry/broad_phase_multi_sap/sap_layer.rs | 36 +- src/geometry/broad_phase_multi_sap/sap_utils.rs | 18 +- src/geometry/broad_phase_qbvh.rs | 2 +- src/geometry/collider.rs | 171 ++- src/geometry/collider_components.rs | 160 ++- src/geometry/collider_set.rs | 95 +- src/geometry/contact_pair.rs | 27 +- src/geometry/interaction_groups.rs | 10 + src/geometry/mod.rs | 24 +- src/geometry/narrow_phase.rs | 61 +- src/lib.rs | 42 + src/linalg/linalg_glam.rs | 279 +++++ src/linalg/linalg_glam_simd.rs | 262 +++++ src/linalg/linalg_nalgebra.rs | 459 ++++++++ src/linalg/mod.rs | 5 + .../debug_render_pipeline/debug_render_backend.rs | 44 +- .../debug_render_pipeline/debug_render_pipeline.rs | 29 +- src/pipeline/debug_render_pipeline/outlines.rs | 6 +- src/pipeline/event_handler.rs | 8 + src/pipeline/physics_hooks.rs | 20 +- src/pipeline/physics_pipeline.rs | 7 +- src/pipeline/query_pipeline.rs | 26 +- src/utils.rs | 534 +++------- src_testbed/debug_render.rs | 4 +- src_testbed/graphics.rs | 6 +- src_testbed/harness/mod.rs | 2 +- src_testbed/objects/node.rs | 8 +- src_testbed/physics/mod.rs | 2 +- src_testbed/testbed.rs | 4 +- 103 files changed, 4350 insertions(+), 3131 deletions(-) create mode 100644 src/data/entity_arena.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_helper.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_helper_simd.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint_helper_template.rs create mode 100644 src/linalg/linalg_glam.rs create mode 100644 src/linalg/linalg_glam_simd.rs create mode 100644 src/linalg/linalg_nalgebra.rs create mode 100644 src/linalg/mod.rs diff --git a/Cargo.toml b/Cargo.toml index 023432e..195f5e4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [workspace] -members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d", - "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d" ] +members = ["crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d", + "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d"] resolver = "2" [patch.crates-io] @@ -12,15 +12,14 @@ resolver = "2" #parry3d = { path = "../parry/crates/parry3d" } #parry2d-f64 = { path = "../parry/crates/parry2d-f64" } #parry3d-f64 = { path = "../parry/crates/parry3d-f64" } -# nalgebra = { path = "../nalgebra" } - - -#kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } -#nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } -#parry2d = { git = "https://github.com/dimforge/parry", branch = "master" } -#parry3d = { git = "https://github.com/dimforge/parry", branch = "master" } -#parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "master" } -#parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "master" } +#nalgebra = { path = "../nalgebra" } + +simba = { git = "https://github.com/dimforge/simba", rev = "7b50b3eb568e9c551431286a95c913a7057de58f" } +nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "25032f089bb2e8a8bd2c1d419f1613911a042895" } +parry2d = { git = "https://github.com/dimforge/parry", rev = "c0e6f8032396396f93b5ad687504bed9d676aaee" } +parry3d = { git = "https://github.com/dimforge/parry", rev = "c0e6f8032396396f93b5ad687504bed9d676aaee" } +parry2d-f64 = { git = "https://github.com/dimforge/parry", rev = "c0e6f8032396396f93b5ad687504bed9d676aaee" } +parry3d-f64 = { git = "https://github.com/dimforge/parry", rev = "c0e6f8032396396f93b5ad687504bed9d676aaee" } [profile.release] #debug = true diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 0d8bc24..f73107d 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -1,14 +1,14 @@ [package] -name = "rapier2d" +name = "rapier2d" version = "0.18.0" -authors = [ "Sébastien Crozet " ] +authors = ["Sébastien Crozet "] description = "2-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier2d" homepage = "https://rapier.rs" repository = "https://github.com/dimforge/rapier" readme = "README.md" -categories = [ "science", "game-development", "mathematics", "simulation", "wasm"] -keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ] +categories = ["science", "game-development", "mathematics", "simulation", "wasm"] +keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"] license = "Apache-2.0" edition = "2021" @@ -16,23 +16,26 @@ edition = "2021" maintenance = { status = "actively-developed" } [features] -default = [ "dim2", "f32" ] -dim2 = [ ] -f32 = [ ] -parallel = [ "rayon" ] -simd-stable = [ "simba/wide", "simd-is-enabled" ] -simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] +default = ["dim2", "f32"] +dim2 = [] +f32 = [] +linalg-nalgebra = ["parry2d/linalg-nalgebra"] +linalg-glam = ["parry2d/linalg-glam", "glam"] + +parallel = ["rayon"] +simd-stable = ["simba/wide", "simd-is-enabled"] +simd-nightly = ["simba/packed_simd", "simd-is-enabled"] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [ "vec_map" ] -wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ] -enhanced-determinism = [ "simba/libm_force", "parry2d/enhanced-determinism" ] -debug-render = [ ] -profiler = [ "instant" ] # Enables the internal profiler. +simd-is-enabled = ["vec_map"] +wasm-bindgen = ["instant/wasm-bindgen"] +serde-serialize = ["nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde"] +enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"] +debug-render = [] +profiler = ["instant"] # Enables the internal profiler. # Feature used for debugging only. -debug-disable-legitimate-fe-exceptions = [ ] +debug-disable-legitimate-fe-exceptions = [] # Feature used for development and debugging only. # Do not enable this unless you are working on the engine internals. @@ -44,12 +47,12 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"] [lib] name = "rapier2d" path = "../../src/lib.rs" -required-features = [ "dim2", "f32" ] +required-features = ["dim2", "f32"] [dependencies] vec_map = { version = "0.8", optional = true } -instant = { version = "0.1", features = [ "now" ], optional = true } +instant = { version = "0.1", features = ["now"], optional = true } num-traits = "0.2" nalgebra = "0.32" parry2d = "0.13.1" @@ -60,11 +63,12 @@ crossbeam = "0.8" arrayvec = "0.7" bit-vec = "0.6" rustc-hash = "1" -serde = { version = "1", features = [ "derive" ], optional = true } +serde = { version = "1", features = ["derive"], optional = true } downcast-rs = "1.2" num-derive = "0.4" bitflags = "1" +glam = { version = "0.25", optional = true } [dev-dependencies] bincode = "1" -serde = { version = "1", features = [ "derive" ] } +serde = { version = "1", features = ["derive"] } diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index 1d3605b..dbe15f5 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -1,14 +1,14 @@ [package] -name = "rapier3d" +name = "rapier3d" version = "0.18.0" -authors = [ "Sébastien Crozet " ] +authors = ["Sébastien Crozet "] description = "3-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier3d" homepage = "https://rapier.rs" repository = "https://github.com/dimforge/rapier" readme = "README.md" -categories = [ "science", "game-development", "mathematics", "simulation", "wasm"] -keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ] +categories = ["science", "game-development", "mathematics", "simulation", "wasm"] +keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"] license = "Apache-2.0" edition = "2021" @@ -16,23 +16,26 @@ edition = "2021" maintenance = { status = "actively-developed" } [features] -default = [ "dim3", "f32" ] -dim3 = [ ] -f32 = [ ] -parallel = [ "rayon" ] -simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ] -simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] +default = ["dim3", "f32"] +dim3 = [] +f32 = [] +linalg-nalgebra = ["parry3d/linalg-nalgebra"] +linalg-glam = ["parry3d/linalg-glam", "glam", "bevy"] # FIXME: don’t enable bevy by default + +parallel = ["rayon"] +simd-stable = ["parry3d/simd-stable", "simba/wide", "simd-is-enabled"] +simd-nightly = ["parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled"] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [ "vec_map" ] -wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde" ] -enhanced-determinism = [ "simba/libm_force", "parry3d/enhanced-determinism" ] -debug-render = [ ] -profiler = [ "instant" ] # Enables the internal profiler. +simd-is-enabled = ["vec_map"] +wasm-bindgen = ["instant/wasm-bindgen"] +serde-serialize = ["nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde"] +enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"] +debug-render = [] +profiler = ["instant"] # Enables the internal profiler. # Feature used for debugging only. -debug-disable-legitimate-fe-exceptions = [ ] +debug-disable-legitimate-fe-exceptions = [] # Feature used for development and debugging only. # Do not enable this unless you are working on the engine internals. @@ -44,12 +47,12 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"] [lib] name = "rapier3d" path = "../../src/lib.rs" -required-features = [ "dim3", "f32" ] +required-features = ["dim3", "f32"] [dependencies] vec_map = { version = "0.8", optional = true } -instant = { version = "0.1", features = [ "now" ], optional = true } +instant = { version = "0.1", features = ["now"], optional = true } num-traits = "0.2" nalgebra = "0.32" parry3d = "0.13.1" @@ -60,11 +63,14 @@ crossbeam = "0.8" arrayvec = "0.7" bit-vec = "0.6" rustc-hash = "1" -serde = { version = "1", features = [ "derive" ], optional = true } +serde = { version = "1", features = ["derive"], optional = true } downcast-rs = "1.2" num-derive = "0.4" bitflags = "1" +glam = { version = "0.25", optional = true } + +bevy = { version = "0.13", default-features = false, optional = true } # FIXME: depend on bevy-ecs only? [dev-dependencies] bincode = "1" -serde = { version = "1", features = [ "derive" ] } +serde = { version = "1", features = ["derive"] } diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml index 3b3bdeb..e8de12f 100644 --- a/crates/rapier_testbed3d/Cargo.toml +++ b/crates/rapier_testbed3d/Cargo.toml @@ -21,6 +21,8 @@ required-features = [ "dim3" ] [features] default = [ "dim3" ] dim3 = [ ] +linalg-nalgebra = [ "rapier/linalg-nalgebra" ] +linalg-glam = [ "rapier/linalg-glam" ] parallel = [ "rapier/parallel", "num_cpus" ] other-backends = [ "physx", "physx-sys", "glam" ] diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 1e44b3d..1edf6fe 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -11,6 +11,8 @@ simd-stable = [ "rapier3d/simd-stable" ] simd-nightly = [ "rapier3d/simd-nightly" ] other-backends = [ "rapier_testbed3d/other-backends" ] enhanced-determinism = [ "rapier3d/enhanced-determinism" ] +linalg-nalgebra = [ "rapier_testbed3d/linalg-nalgebra" ] +linalg-glam = [ "rapier_testbed3d/linalg-glam" ] [dependencies] rand = "0.8" diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs index c1a5176..8a3ea7c 100644 --- a/examples3d/ccd3.rs +++ b/examples3d/ccd3.rs @@ -5,9 +5,9 @@ fn create_wall( testbed: &mut Testbed, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - offset: Vector, + offset: Vector, stack_height: usize, - half_extents: Vector, + half_extents: Vector, ) { let shift = half_extents * 2.0; let mut k = 0; diff --git a/examples3d/convex_polyhedron3.rs b/examples3d/convex_polyhedron3.rs index 980720a..986dea9 100644 --- a/examples3d/convex_polyhedron3.rs +++ b/examples3d/convex_polyhedron3.rs @@ -47,7 +47,7 @@ pub fn init_world(testbed: &mut Testbed) { let mut points = Vec::new(); for _ in 0..10 { - let pt: Point = distribution.sample(&mut rng); + let pt: Point = distribution.sample(&mut rng); points.push(pt * scale); } diff --git a/examples3d/debug_deserialize3.rs b/examples3d/debug_deserialize3.rs index b8b79fb..00eedf4 100644 --- a/examples3d/debug_deserialize3.rs +++ b/examples3d/debug_deserialize3.rs @@ -3,7 +3,7 @@ use rapier_testbed3d::Testbed; #[derive(serde::Deserialize)] struct PhysicsState { - pub gravity: Vector, + pub gravity: Vector, pub integration_parameters: IntegrationParameters, pub islands: IslandManager, pub broad_phase: BroadPhase, diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs index 776c50c..f67c43e 100644 --- a/examples3d/debug_prismatic3.rs +++ b/examples3d/debug_prismatic3.rs @@ -5,7 +5,7 @@ fn prismatic_repro( bodies: &mut RigidBodySet, colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, - box_center: Point, + box_center: Point, ) { let box_rb = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![ box_center.x, diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 73f48ae..2e0ad25 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -6,7 +6,7 @@ fn create_coupled_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, use_articulations: bool, ) { let ground = bodies.insert(RigidBodyBuilder::fixed().translation(origin.coords)); @@ -34,7 +34,7 @@ fn create_prismatic_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, num: usize, use_articulations: bool, ) { @@ -78,7 +78,7 @@ fn create_actuated_prismatic_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, num: usize, use_articulations: bool, ) { @@ -143,7 +143,7 @@ fn create_revolute_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, num: usize, use_articulations: bool, ) { @@ -204,7 +204,7 @@ fn create_revolute_joints_with_limits( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, use_articulations: bool, ) { let ground = bodies.insert(RigidBodyBuilder::fixed().translation(origin.coords)); @@ -275,7 +275,7 @@ fn create_fixed_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, num: usize, use_articulations: bool, ) { @@ -397,7 +397,7 @@ fn create_spherical_joints_with_limits( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, use_articulations: bool, ) { let shift = vector![0.0, 0.0, 3.0]; @@ -438,7 +438,7 @@ fn create_actuated_revolute_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, num: usize, use_articulations: bool, ) { @@ -507,7 +507,7 @@ fn create_actuated_spherical_joints( colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, - origin: Point, + origin: Point, num: usize, use_articulations: bool, ) { diff --git a/examples3d/keva3.rs b/examples3d/keva3.rs index 4f5cd54..fb2c6b2 100644 --- a/examples3d/keva3.rs +++ b/examples3d/keva3.rs @@ -5,8 +5,8 @@ pub fn build_block( testbed: &mut Testbed, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - half_extents: Vector, - shift: Vector, + half_extents: Vector, + shift: Vector, (mut numx, numy, mut numz): (usize, usize, usize), ) { let dimensions = [half_extents.xyz(), half_extents.zyx()]; diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index ca088b2..23964b5 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -1,6 +1,6 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, TOI}; -use crate::math::{Isometry, Point, Real, UnitVector, Vector}; +use crate::math::*; use crate::pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline}; use crate::utils; use na::{RealField, Vector2}; @@ -78,16 +78,16 @@ impl Default for CharacterAutostep { } /// A collision between the character and its environment during its movement. -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialEq)] pub struct CharacterCollision { /// The collider hit by the character. pub handle: ColliderHandle, /// The position of the character when the collider was hit. - pub character_pos: Isometry, + pub character_pos: Isometry, /// The translation that was already applied to the character when the hit happens. - pub translation_applied: Vector, + pub translation_applied: Vector, /// The translations that was still waiting to be applied to the character when the hit happens. - pub translation_remaining: Vector, + pub translation_remaining: Vector, /// Geometric information about the hit. pub toi: TOI, } @@ -97,7 +97,7 @@ pub struct CharacterCollision { #[derive(Copy, Clone, Debug)] pub struct KinematicCharacterController { /// The direction that goes "up". Used to determine where the floor is, and the floor’s angle. - pub up: UnitVector, + pub up: UnitVector, /// A small gap to preserve between the character and its surroundings. /// /// This value should not be too large to avoid visual artifacts, but shouldn’t be too small @@ -135,7 +135,7 @@ impl Default for KinematicCharacterController { /// The effective movement computed by the character controller. pub struct EffectiveCharacterMovement { /// The movement to apply. - pub translation: Vector, + pub translation: Vector, /// Is the character touching the ground after applying `EffectiveKineamticMovement::translation`? pub grounded: bool, /// Is the character sliding down a slope due to slope angle being larger than `min_slope_slide_angle`? @@ -181,8 +181,8 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry, - desired_translation: Vector, + character_pos: &Isometry, + desired_translation: Vector, filter: QueryFilter, mut events: impl FnMut(CharacterCollision), ) -> EffectiveCharacterMovement { @@ -237,8 +237,8 @@ impl KinematicCharacterController { ) { // We hit something, compute the allowed self. let allowed_dist = - (toi.toi - (-toi.normal1.dot(&translation_dir)) * offset).max(0.0); - let allowed_translation = *translation_dir * allowed_dist; + (toi.toi - (-toi.normal1.dot(translation_dir)) * offset).max(0.0); + let allowed_translation = translation_dir.into_inner() * allowed_dist; result.translation += allowed_translation; translation_remaining -= allowed_translation; @@ -315,13 +315,13 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry, + character_pos: &Isometry, dims: &Vector2, filter: QueryFilter, result: &mut EffectiveCharacterMovement, ) -> Option<(ColliderHandle, TOI)> { if let Some(snap_distance) = self.snap_to_ground { - if result.translation.dot(&self.up) < -1.0e-5 { + if result.translation.dot(self.up) < -1.0e-5 { let snap_distance = snap_distance.eval(dims.y); let offset = self.offset.eval(dims.y); if let Some((hit_handle, hit)) = queries.cast_shape( @@ -335,7 +335,7 @@ impl KinematicCharacterController { filter, ) { // Apply the snap. - result.translation -= *self.up * (hit.toi - offset).max(0.0); + result.translation -= self.up.into_inner() * (hit.toi - offset).max(0.0); result.grounded = true; return Some((hit_handle, hit)); } @@ -356,11 +356,11 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry, + character_pos: &Isometry, dims: &Vector2, filter: QueryFilter, - mut kinematic_friction_translation: Option<&mut Vector>, - mut translation_remaining: Option<&mut Vector>, + mut kinematic_friction_translation: Option<&mut Vector>, + mut translation_remaining: Option<&mut Vector>, ) -> bool { let prediction = self.predict_ground(dims.y); @@ -406,19 +406,20 @@ impl KinematicCharacterController { if let Some(kinematic_parent) = kinematic_parent { let mut num_active_contacts = 0; let mut manifold_center = Point::origin(); - let normal = -(character_pos * m.local_n1); + let normal = -(character_pos.rotation * m.local_n1); for contact in &m.points { if contact.dist <= prediction { num_active_contacts += 1; - let contact_point = collider.position() * contact.local_p2; + let contact_point = + collider.position().transform_point(&contact.local_p2); let target_vel = kinematic_parent.velocity_at_point(&contact_point); - let normal_target_mvt = target_vel.dot(&normal) * dt; - let normal_current_mvt = translation_remaining.dot(&normal); + let normal_target_mvt = target_vel.dot(normal) * dt; + let normal_current_mvt = translation_remaining.dot(normal); - manifold_center += contact_point.coords; + manifold_center += contact_point.as_vector(); *translation_remaining += normal * (normal_target_mvt - normal_current_mvt); } @@ -429,7 +430,7 @@ impl KinematicCharacterController { &(manifold_center / num_active_contacts as Real), ); let tangent_platform_mvt = - (target_vel - normal * target_vel.dot(&normal)) * dt; + (target_vel - normal * target_vel.dot(normal)) * dt; kinematic_friction_translation.zip_apply( &tangent_platform_mvt, |y, x| { @@ -463,12 +464,12 @@ impl KinematicCharacterController { fn is_grounded_at_contact_manifold( &self, manifold: &ContactManifold, - character_pos: &Isometry, + character_pos: &Isometry, dims: &Vector2, ) -> bool { - let normal = -(character_pos * manifold.local_n1); + let normal = -(character_pos.rotation * manifold.local_n1); - if normal.dot(&self.up) >= 1.0e-5 { + if normal.dot(self.up) >= 1.0e-5 { let prediction = self.predict_ground(dims.y); for contact in &manifold.points { if contact.dist <= prediction { @@ -482,9 +483,9 @@ impl KinematicCharacterController { fn handle_slopes( &self, hit: &TOI, - translation_remaining: &Vector, + translation_remaining: &Vector, result: &mut EffectiveCharacterMovement, - ) -> Vector { + ) -> Vector { let [vertical_translation, horizontal_translation] = self.split_into_components(translation_remaining); let slope_translation = subtract_hit(*translation_remaining, hit); @@ -498,7 +499,7 @@ impl KinematicCharacterController { // NOTE: part of the slope will already be handled by auto-stepping if it was enabled. // Therefore, `climbing` may not always be `true` when climbing on a slope at // slow speed. - let climbing = self.up.dot(&slope_translation) >= 0.0 && self.up.dot(&hit.normal1) > 0.0; + let climbing = self.up.dot(slope_translation) >= 0.0 && self.up.dot(hit.normal1) > 0.0; if climbing && angle_with_floor >= self.max_slope_climb_angle { // Prevent horizontal movement from pushing through the slope. @@ -513,16 +514,16 @@ impl KinematicCharacterController { } } - fn split_into_components(&self, translation: &Vector) -> [Vector; 2] { - let vertical_translation = *self.up * (self.up.dot(translation)); + fn split_into_components(&self, translation: &Vector) -> [Vector; 2] { + let vertical_translation = self.up.into_inner() * (self.up.dot(*translation)); let horizontal_translation = *translation - vertical_translation; [vertical_translation, horizontal_translation] } fn compute_dims(&self, character_shape: &dyn Shape) -> Vector2 { let extents = character_shape.compute_local_aabb().extents(); - let up_extent = extents.dot(&self.up.abs()); - let side_extent = (extents - (*self.up).abs() * up_extent).norm(); + let up_extent = extents.dot(self.up.abs()); + let side_extent = (extents - self.up.into_inner().abs() * up_extent).norm(); Vector2::new(side_extent, up_extent) } @@ -532,11 +533,11 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry, + character_pos: &Isometry, dims: &Vector2, mut filter: QueryFilter, stair_handle: ColliderHandle, - translation_remaining: &mut Vector, + translation_remaining: &mut Vector, result: &mut EffectiveCharacterMovement, ) -> bool { let autostep = match self.autostep { @@ -563,11 +564,12 @@ impl KinematicCharacterController { filter.flags |= QueryFilterFlags::EXCLUDE_DYNAMIC; } - let shifted_character_pos = Translation::from(*self.up * max_height) * character_pos; + let shifted_character_pos = + Translation::from(self.up.into_inner() * max_height) * character_pos; let horizontal_dir = match (*translation_remaining - - *self.up * translation_remaining.dot(&self.up)) - .try_normalize(1.0e-5) + - self.up.into_inner() * translation_remaining.dot(self.up)) + .try_normalize_eps(1.0e-5) { Some(dir) => dir, None => return false, @@ -626,7 +628,7 @@ impl KinematicCharacterController { let slope_translation = horizontal_slope_translation + vertical_slope_translation; let angle_with_floor = self.up.angle(&hit.normal1); - let climbing = self.up.dot(&slope_translation) >= 0.0; + let climbing = self.up.dot(slope_translation) >= 0.0; if climbing && angle_with_floor > self.max_slope_climb_angle { return false; // The target ramp is too steep. @@ -650,13 +652,13 @@ impl KinematicCharacterController { .unwrap_or(max_height); // Remove the step height from the vertical part of the self. - let step = *self.up * step_height; + let step = self.up.into_inner() * step_height; *translation_remaining -= step; // Advance the collider on the step horizontally, to make sure further // movement won’t just get stuck on its edge. let horizontal_nudge = - horizontal_dir * horizontal_dir.dot(translation_remaining).min(min_width); + horizontal_dir * horizontal_dir.dot(*translation_remaining).min(min_width); *translation_remaining -= horizontal_nudge; result.translation += step + horizontal_nudge; @@ -679,9 +681,9 @@ impl KinematicCharacterController { filter: QueryFilter, ) { let extents = character_shape.compute_local_aabb().extents(); - let up_extent = extents.dot(&self.up.abs()); - let movement_to_transfer = - *collision.toi.normal1 * collision.translation_remaining.dot(&collision.toi.normal1); + let up_extent = extents.dot(self.up.abs()); + let movement_to_transfer = collision.toi.normal1.into_inner() + * collision.translation_remaining.dot(collision.toi.normal1); let prediction = self.predict_ground(up_extent); // TODO: allow custom dispatchers. @@ -712,7 +714,7 @@ impl KinematicCharacterController { for m in &mut manifolds[prev_manifolds_len..] { m.data.rigid_body2 = Some(parent.handle); - m.data.normal = collision.character_pos * m.local_n1; + m.data.normal = collision.character_pos.rotation * m.local_n1; } } } @@ -726,15 +728,23 @@ impl KinematicCharacterController { for manifold in &manifolds { let body_handle = manifold.data.rigid_body2.unwrap(); - let body = &mut bodies[body_handle]; + + // NOTE: we are calling get_mut_internal_with_modification_tracking because this is part + // of the internal rapier codebase (and we use this function to make sure we are + // triggering modification tracking on purpose), but a copy-paste on this character + // controller outside of the rapier codebase could just use + // `&mut bodies[body_handle]` instead. + let Some(body) = bodies.get_mut_internal_with_modification_tracking(body_handle) else { + continue; + }; for pt in &manifold.points { if pt.dist <= prediction { let body_mass = body.mass(); - let contact_point = body.position() * pt.local_p2; + let contact_point = body.position().transform_point(&pt.local_p2); let delta_vel_per_contact = (velocity_to_transfer - body.velocity_at_point(&contact_point)) - .dot(&manifold.data.normal); + .dot(manifold.data.normal); let mass_ratio = body_mass * character_mass / (body_mass + character_mass); body.apply_impulse_at_point( @@ -748,9 +758,9 @@ impl KinematicCharacterController { } } -fn subtract_hit(translation: Vector, hit: &TOI) -> Vector { - let surface_correction = (-translation).dot(&hit.normal1).max(0.0); +fn subtract_hit(translation: Vector, hit: &TOI) -> Vector { + let surface_correction = (-translation).dot(hit.normal1).max(0.0); // This fixes some instances of moving through walls let surface_correction = surface_correction * (1.0 + 1.0e-5); - translation + *hit.normal1 * surface_correction + translation + hit.normal1.into_inner() * surface_correction } diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 00b11eb..c56d987 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -2,15 +2,15 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderHandle, ColliderSet, Ray}; -use crate::math::{Point, Real, Rotation, Vector}; +use crate::math::*; use crate::pipeline::{QueryFilter, QueryPipeline}; use crate::utils::{SimdCross, SimdDot}; /// A character controller to simulate vehicles using ray-casting for the wheels. pub struct DynamicRayCastVehicleController { wheels: Vec, - forward_ws: Vec>, - axle: Vec>, + forward_ws: Vec, + axle: Vec, /// The current forward speed of the vehicle. pub current_vehicle_speed: Real, @@ -65,13 +65,13 @@ impl Default for WheelTuning { /// Objects used to initialize a wheel. struct WheelDesc { /// The position of the wheel, relative to the chassis. - pub chassis_connection_cs: Point, + pub chassis_connection_cs: Point, /// The direction of the wheel’s suspension, relative to the chassis. /// /// The ray-casting will happen following this direction to detect the ground. - pub direction_cs: Vector, + pub direction_cs: Vector, /// The wheel’s axle axis, relative to the chassis. - pub axle_cs: Vector, + pub axle_cs: Vector, /// The rest length of the wheel’s suspension spring. pub suspension_rest_length: Real, /// The maximum distance the suspension can travel before and after its resting length. @@ -105,18 +105,18 @@ struct WheelDesc { pub struct Wheel { raycast_info: RayCastInfo, - center: Point, - wheel_direction_ws: Vector, - wheel_axle_ws: Vector, + center: Point, + wheel_direction_ws: Vector, + wheel_axle_ws: Vector, /// The position of the wheel, relative to the chassis. - pub chassis_connection_point_cs: Point, + pub chassis_connection_point_cs: Point, /// The direction of the wheel’s suspension, relative to the chassis. /// /// The ray-casting will happen following this direction to detect the ground. - pub direction_cs: Vector, + pub direction_cs: Vector, /// The wheel’s axle axis, relative to the chassis. - pub axle_cs: Vector, + pub axle_cs: Vector, /// The rest length of the wheel’s suspension spring. pub suspension_rest_length: Real, /// The maximum distance the suspension can travel before and after its resting length. @@ -207,17 +207,17 @@ impl Wheel { } /// The world-space center of the wheel. - pub fn center(&self) -> Point { + pub fn center(&self) -> Point { self.center } /// The world-space direction of the wheel’s suspension. - pub fn suspension(&self) -> Vector { + pub fn suspension(&self) -> Vector { self.wheel_direction_ws } /// The world-space direction of the wheel’s axle. - pub fn axle(&self) -> Vector { + pub fn axle(&self) -> Vector { self.wheel_axle_ws } } @@ -227,13 +227,13 @@ impl Wheel { #[derive(Copy, Clone, Debug, PartialEq, Default)] pub struct RayCastInfo { /// The (world-space) contact normal between the wheel and the floor. - pub contact_normal_ws: Vector, + pub contact_normal_ws: Vector, /// The (world-space) point hit by the wheel’s ray-cast. - pub contact_point_ws: Point, + pub contact_point_ws: Point, /// The suspension length for the wheel. pub suspension_length: Real, /// The (world-space) starting point of the ray-cast. - pub hard_point_ws: Point, + pub hard_point_ws: Point, /// Is the wheel in contact with the ground? pub is_in_contact: bool, /// The collider hit by the ray-cast. @@ -262,9 +262,9 @@ impl DynamicRayCastVehicleController { /// Adds a wheel to this vehicle. pub fn add_wheel( &mut self, - chassis_connection_cs: Point, - direction_cs: Vector, - axle_cs: Vector, + chassis_connection_cs: Point, + direction_cs: Vector, + axle_cs: Vector, suspension_rest_length: Real, radius: Real, tuning: &WheelTuning, @@ -296,7 +296,7 @@ impl DynamicRayCastVehicleController { let wheel = &mut self.wheels[wheel_index]; wheel.center = (wheel.raycast_info.hard_point_ws + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length) - .coords; + .as_vector(); } #[cfg(feature = "dim3")] @@ -304,8 +304,8 @@ impl DynamicRayCastVehicleController { self.update_wheel_transforms_ws(chassis, wheel_index); let wheel = &mut self.wheels[wheel_index]; - let steering_orn = Rotation::new(-wheel.wheel_direction_ws * wheel.steering); - wheel.wheel_axle_ws = steering_orn * (chassis.position() * wheel.axle_cs); + let steering_orn = Rotation::from_scaled_axis(-wheel.wheel_direction_ws * wheel.steering); + wheel.wheel_axle_ws = steering_orn * (chassis.position().rotation * wheel.axle_cs); wheel.center = wheel.raycast_info.hard_point_ws + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length; } @@ -316,9 +316,10 @@ impl DynamicRayCastVehicleController { let chassis_transform = chassis.position(); - wheel.raycast_info.hard_point_ws = chassis_transform * wheel.chassis_connection_point_cs; - wheel.wheel_direction_ws = chassis_transform * wheel.direction_cs; - wheel.wheel_axle_ws = chassis_transform * wheel.axle_cs; + wheel.raycast_info.hard_point_ws = + chassis_transform.transform_point(&wheel.chassis_connection_point_cs); + wheel.wheel_direction_ws = chassis_transform.rotation * wheel.direction_cs; + wheel.wheel_axle_ws = chassis_transform.rotation * wheel.axle_cs; } fn ray_cast( @@ -377,13 +378,13 @@ impl DynamicRayCastVehicleController { let denominator = wheel .raycast_info .contact_normal_ws - .dot(&wheel.wheel_direction_ws); + .dot(wheel.wheel_direction_ws); let chassis_velocity_at_contact_point = chassis.velocity_at_point(&wheel.raycast_info.contact_point_ws); let proj_vel = wheel .raycast_info .contact_normal_ws - .dot(&chassis_velocity_at_contact_point); + .dot(chassis_velocity_at_contact_point); if denominator >= -0.1 { wheel.suspension_relative_velocity = 0.0; @@ -420,9 +421,9 @@ impl DynamicRayCastVehicleController { self.current_vehicle_speed = chassis.linvel().norm(); - let forward_w = chassis.position() * Vector::ith(self.index_forward_axis, 1.0); + let forward_w = chassis.position().rotation * Vector::ith(self.index_forward_axis, 1.0); - if forward_w.dot(chassis.linvel()) < 0.0 { + if forward_w.dot(*chassis.linvel()) < 0.0 { self.current_vehicle_speed *= -1.0; } @@ -467,11 +468,12 @@ impl DynamicRayCastVehicleController { let vel = chassis.velocity_at_point(&wheel.raycast_info.hard_point_ws); if wheel.raycast_info.is_in_contact { - let mut fwd = chassis.position() * Vector::ith(self.index_forward_axis, 1.0); - let proj = fwd.dot(&wheel.raycast_info.contact_normal_ws); + let mut fwd = + chassis.position().rotation * Vector::ith(self.index_forward_axis, 1.0); + let proj = fwd.dot(wheel.raycast_info.contact_normal_ws); fwd -= wheel.raycast_info.contact_normal_ws * proj; - let proj2 = fwd.dot(&vel); + let proj2 = fwd.dot(vel); wheel.delta_rotation = (proj2 * dt) / (wheel.radius); wheel.rotation += wheel.delta_rotation; @@ -564,14 +566,14 @@ impl DynamicRayCastVehicleController { self.axle[i] = wheel.wheel_axle_ws; let surf_normal_ws = wheel.raycast_info.contact_normal_ws; - let proj = self.axle[i].dot(&surf_normal_ws); + let proj = self.axle[i].dot(surf_normal_ws); self.axle[i] -= surf_normal_ws * proj; self.axle[i] = self.axle[i] - .try_normalize(1.0e-5) + .try_normalize_eps(1.0e-5) .unwrap_or_else(Vector::zeros); self.forward_ws[i] = surf_normal_ws - .cross(&self.axle[i]) - .try_normalize(1.0e-5) + .cross(self.axle[i]) + .try_normalize_eps(1.0e-5) .unwrap_or_else(Vector::zeros); if let Some(ground_body) = ground_object @@ -694,7 +696,7 @@ impl DynamicRayCastVehicleController { let v_chassis_world_up = chassis.position().rotation * Vector::ith(self.index_up_axis, 1.0); impulse_point -= v_chassis_world_up - * (v_chassis_world_up.dot(&(impulse_point - chassis.center_of_mass())) + * (v_chassis_world_up.dot(impulse_point - *chassis.center_of_mass()) * (1.0 - wheel.roll_influence)); chassis.apply_impulse_at_point(side_impulse, impulse_point, false); @@ -715,8 +717,8 @@ impl DynamicRayCastVehicleController { struct WheelContactPoint<'a> { body0: &'a RigidBody, body1: Option<&'a RigidBody>, - friction_position_world: Point, - friction_direction_world: Vector, + friction_position_world: Point, + friction_direction_world: Vector, jac_diag_ab_inv: Real, max_impulse: Real, } @@ -725,18 +727,18 @@ impl<'a> WheelContactPoint<'a> { pub fn new( body0: &'a RigidBody, body1: Option<&'a RigidBody>, - friction_position_world: Point, - friction_direction_world: Vector, + friction_position_world: Point, + friction_direction_world: Vector, max_impulse: Real, ) -> Self { - fn impulse_denominator(body: &RigidBody, pos: &Point, n: &Vector) -> Real { - let dpt = pos - body.center_of_mass(); + fn impulse_denominator(body: &RigidBody, pos: &Point, n: &Vector) -> Real { + let dpt = *pos - *body.center_of_mass(); let gcross = dpt.gcross(*n); let v = (body.mprops.effective_world_inv_inertia_sqrt * (body.mprops.effective_world_inv_inertia_sqrt * gcross)) .gcross(dpt); // TODO: take the effective inv mass into account instead of the inv_mass? - body.mprops.local_mprops.inv_mass + n.dot(&v) + body.mprops.local_mprops.inv_mass + n.dot(v) } let denom0 = impulse_denominator(body0, &friction_position_world, &friction_direction_world); @@ -768,7 +770,7 @@ impl<'a> WheelContactPoint<'a> { .map(|b| b.velocity_at_point(&contact_pos_world)) .unwrap_or_else(Vector::zeros); let vel = vel1 - vel2; - let vrel = self.friction_direction_world.dot(&vel); + let vrel = self.friction_direction_world.dot(vel); // calculate friction that moves us to zero relative velocity (-vrel * self.jac_diag_ab_inv / (num_wheels_on_ground as Real)) @@ -778,17 +780,17 @@ impl<'a> WheelContactPoint<'a> { fn resolve_single_bilateral( body1: &RigidBody, - pt1: &Point, + pt1: &Point, body2: &RigidBody, - pt2: &Point, - normal: &Vector, + pt2: &Point, + normal: &Vector, ) -> Real { let vel1 = body1.velocity_at_point(pt1); let vel2 = body2.velocity_at_point(pt2); let dvel = vel1 - vel2; - let dpt1 = pt1 - body1.center_of_mass(); - let dpt2 = pt2 - body2.center_of_mass(); + let dpt1 = *pt1 - *body1.center_of_mass(); + let dpt2 = *pt2 - *body2.center_of_mass(); let aj = dpt1.gcross(*normal); let bj = dpt2.gcross(-*normal); let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; @@ -800,17 +802,17 @@ fn resolve_single_bilateral( let jac_diag_ab = im1 + im2 + iaj.gdot(iaj) + ibj.gdot(ibj); let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab); - let rel_vel = normal.dot(&dvel); + let rel_vel = normal.dot(dvel); //todo: move this into proper structure let contact_damping = 0.2; -contact_damping * rel_vel * jac_diag_ab_inv } -fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point, normal: &Vector) -> Real { +fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point, normal: &Vector) -> Real { let vel1 = body1.velocity_at_point(pt1); let dvel = vel1; - let dpt1 = pt1 - body1.center_of_mass(); + let dpt1 = *pt1 - *body1.center_of_mass(); let aj = dpt1.gcross(*normal); let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; @@ -818,7 +820,7 @@ fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point, normal: &Vect let im1 = body1.mprops.local_mprops.inv_mass; let jac_diag_ab = im1 + iaj.gdot(iaj); let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab); - let rel_vel = normal.dot(&dvel); + let rel_vel = normal.dot(dvel); //todo: move this into proper structure let contact_damping = 0.2; diff --git a/src/data/arena.rs b/src/data/arena.rs index 57653da..3a8c613 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -76,7 +76,7 @@ impl Index { /// /// Providing arbitrary values will lead to malformed indices and ultimately /// panics. - pub fn from_raw_parts(index: u32, generation: u32) -> Index { + pub const fn from_raw_parts(index: u32, generation: u32) -> Index { Index { index, generation } } diff --git a/src/data/coarena.rs b/src/data/coarena.rs index 18fb055..2015b6f 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -1,12 +1,18 @@ use crate::data::arena::Index; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Debug, Default)] +#[derive(Clone, Debug)] /// A container for data associated to item existing into another Arena. pub struct Coarena { data: Vec<(u32, T)>, } +impl Default for Coarena { + fn default() -> Self { + Self { data: Vec::new() } + } +} + impl Coarena { /// A coarena with no element. pub fn new() -> Self { @@ -68,11 +74,22 @@ impl Coarena { pub fn insert(&mut self, a: Index, value: T) where T: Clone + Default, + { + self.insert_with_default(a, value, T::default()) + } + + /// Inserts an element into this coarena. + /// + /// The provided `default` value is used for any empty slots created + /// by this insertion. + pub fn insert_with_default(&mut self, a: Index, value: T, default: T) + where + T: Clone, { let (i1, g1) = a.into_raw_parts(); if self.data.len() <= i1 as usize { - self.data.resize(i1 as usize + 1, (u32::MAX, T::default())); + self.data.resize(i1 as usize + 1, (u32::MAX, default)); } self.data[i1 as usize] = (g1, value); diff --git a/src/data/entity_arena.rs b/src/data/entity_arena.rs new file mode 100644 index 0000000..ad6e451 --- /dev/null +++ b/src/data/entity_arena.rs @@ -0,0 +1,145 @@ +use crate::data::arena::Index; +use crate::data::Arena; +use crate::data::Coarena; +use bevy::prelude::Entity; +use std::ops::IndexMut; + +impl From for Index { + #[inline] + fn from(value: Entity) -> Self { + Index::from_raw_parts(value.index(), value.generation()) + } +} + +impl From for Entity { + #[inline] + fn from(value: Index) -> Self { + let (id, gen) = value.into_raw_parts(); + Entity::from_bits(id as u64 | ((gen as u64) << u32::BITS)) + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug, Default)] +struct EntityMap { + entity_to_index: Coarena, + index_to_entity: Coarena, // Is this really needed? +} + +impl EntityMap { + pub fn get_index(&self, entity: Entity) -> Option { + self.entity_to_index.get(entity.into()).copied() + } + + pub fn get_entity(&self, index: Index) -> Option { + self.index_to_entity.get(index).copied() + } + + pub fn insert(&mut self, entity: Entity, index: Index) { + self.entity_to_index.insert(entity.into(), index); + self.index_to_entity + .insert_with_default(index, entity, Entity::PLACEHOLDER); + } + + pub fn remove(&mut self, entity: Entity) { + if let Some(index) = self.entity_to_index.remove( + entity.into(), + Index::from_raw_parts(crate::INVALID_U32, crate::INVALID_U32), + ) { + self.index_to_entity.remove(index, Entity::PLACEHOLDER); + } + } +} + +impl std::ops::Index for EntityMap { + type Output = Entity; + + fn index(&self, index: Index) -> &Self::Output { + self.index_to_entity.get(index).expect("Index not found.") + } +} + +impl std::ops::Index for EntityMap { + type Output = Index; + + fn index(&self, entity: Entity) -> &Self::Output { + self.entity_to_index + .get(entity.into()) + .expect("Index not found.") + } +} + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct EntityArena { + entity_map: EntityMap, + elements: Arena, +} + +impl Default for EntityArena { + fn default() -> Self { + Self { + entity_map: EntityMap::default(), + elements: Arena::default(), + } + } +} + +impl EntityArena { + pub fn insert(&mut self, entity: Entity, value: T) { + let index = self.elements.insert(value); + self.entity_map.insert(entity, index); + } + + pub fn remove(&mut self, entity: Entity) -> Option { + let elt = self.elements.remove(self.entity_map.get_index(entity)?)?; + self.entity_map.remove(entity); + Some(elt) + } + + pub fn contains(&self, entity: Entity) -> bool { + self.entity_map.get_index(entity).is_some() + } + + pub fn len(&self) -> usize { + self.elements.len() + } + + pub fn is_empty(&self) -> bool { + self.elements.is_empty() + } + + pub fn get(&self, entity: Entity) -> Option<&T> { + self.elements.get(self.entity_map.get_index(entity)?) + } + + pub fn get_mut(&mut self, entity: Entity) -> Option<&mut T> { + self.elements.get_mut(self.entity_map.get_index(entity)?) + } + + pub fn iter(&self) -> impl ExactSizeIterator { + self.elements + .iter() + .map(|(h, data)| (self.entity_map[h], data)) + } + + pub fn iter_mut(&mut self) -> impl ExactSizeIterator { + self.elements + .iter_mut() + .map(|(h, data)| (self.entity_map[h], data)) + } +} + +impl std::ops::Index for EntityArena { + type Output = T; + + fn index(&self, index: Entity) -> &Self::Output { + &self.elements[self.entity_map[index]] + } +} + +impl IndexMut for EntityArena { + fn index_mut(&mut self, index: Entity) -> &mut Self::Output { + &mut self.elements[self.entity_map[index]] + } +} diff --git a/src/data/mod.rs b/src/data/mod.rs index 6d9e2ce..c729202 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -3,7 +3,13 @@ pub use self::arena::{Arena, Index}; pub use self::coarena::Coarena; +#[cfg(feature = "bevy")] +pub use self::entity_arena::EntityArena; + pub mod arena; mod coarena; pub(crate) mod graph; pub mod pubsub; + +#[cfg(feature = "bevy")] +mod entity_arena; diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 79c4495..064c044 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,7 +1,7 @@ use super::TOIEntry; use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase}; -use crate::math::Real; +use crate::math::*; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; use crate::prelude::{ActiveEvents, CollisionEventFlags}; @@ -92,14 +92,14 @@ impl CCDSolver { for handle in islands.active_dynamic_bodies() { let rb = bodies.index_mut_internal(*handle); - if rb.ccd.ccd_enabled { + if rb.ccd.enabled { let forces = if include_forces { Some(&rb.forces) } else { None }; let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces); - rb.ccd.ccd_active = moving_fast; + rb.ccd.active = moving_fast; ccd_active = ccd_active || moving_fast; } } @@ -129,7 +129,7 @@ impl CCDSolver { for handle in islands.active_dynamic_bodies() { let rb1 = &bodies[*handle]; - if rb1.ccd.ccd_active { + if rb1.ccd.active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, @@ -161,10 +161,7 @@ impl CCDSolver { } if pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) + .insert(SortedPair::new(ch1.index(), ch2.index()), ()) .is_none() { let co1 = &colliders[*ch1]; @@ -253,7 +250,7 @@ impl CCDSolver { for handle in islands.active_dynamic_bodies() { let rb1 = &bodies[*handle]; - if rb1.ccd.ccd_active { + if rb1.ccd.active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, @@ -261,8 +258,8 @@ impl CCDSolver { &rb1.mprops, ); - for ch1 in &rb1.colliders.0 { - let co1 = &colliders[*ch1]; + for ch1 in rb1.colliders.0.iter().copied() { + let co1 = &colliders[ch1]; let co_parent1 = co1 .parent .as_ref() @@ -275,19 +272,16 @@ impl CCDSolver { self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { - if *ch1 == *ch2 { + if ch1 == *ch2 { // Ignore self-intersection. return true; } if pairs_seen - .insert( - SortedPair::new(ch1.into_raw_parts().0, ch2.into_raw_parts().0), - (), - ) + .insert(SortedPair::new(ch1.index(), ch2.index()), ()) .is_none() { - let co1 = &colliders[*ch1]; + let co1 = &colliders[ch1]; let co2 = &colliders[*ch2]; let bh1 = co1.parent.map(|p| p.handle); @@ -301,7 +295,7 @@ impl CCDSolver { } let smallest_dist = narrow_phase - .contact_pair(*ch1, *ch2) + .contact_pair(ch1, *ch2) .and_then(|p| p.find_deepest_contact()) .map(|c| c.1.dist) .unwrap_or(0.0); @@ -311,7 +305,7 @@ impl CCDSolver { if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), - *ch1, + ch1, *ch2, co1, co2, @@ -362,12 +356,10 @@ impl CCDSolver { let rb2 = toi.b2.and_then(|b| bodies.get(b)); let mut colliders_to_check = Vec::new(); - let should_freeze1 = rb1.is_some() - && rb1.unwrap().ccd.ccd_active - && !frozen.contains_key(&toi.b1.unwrap()); - let should_freeze2 = rb2.is_some() - && rb2.unwrap().ccd.ccd_active - && !frozen.contains_key(&toi.b2.unwrap()); + let should_freeze1 = + rb1.is_some() && rb1.unwrap().ccd.active && !frozen.contains_key(&toi.b1.unwrap()); + let should_freeze2 = + rb2.is_some() && rb2.unwrap().ccd.active && !frozen.contains_key(&toi.b2.unwrap()); if !should_freeze1 && !should_freeze2 { continue; @@ -400,8 +392,8 @@