diff --git a/.github/workflows/parry-ci-build.yml b/.github/workflows/parry-ci-build.yml index 129c6cde..4c6681a0 100644 --- a/.github/workflows/parry-ci-build.yml +++ b/.github/workflows/parry-ci-build.yml @@ -40,7 +40,7 @@ jobs: - name: Build parry3d SIMD run: cd crates/parry3d; cargo build --verbose --features simd-stable; - name: Check serialization - run: cargo check --features bytemuck-serialize,serde-serialize,rkyv-serialize; + run: cargo check --features bytemuck-serialize,serde-serialize,rkyv; - name: Check enhanced-determinism run: cargo check --features enhanced-determinism tests: @@ -91,7 +91,7 @@ jobs: with: toolchain: stable - name: cargo doc - run: cargo doc --workspace --features bytemuck-serialize,serde-serialize,rkyv-serialize,parallel --no-deps --document-private-items + run: cargo doc --workspace --features bytemuck-serialize,serde-serialize,parallel --no-deps --document-private-items # TODO: rkyv check-benchmarks: runs-on: ubuntu-latest steps: diff --git a/.gitignore b/.gitignore index 04cb271e..25ca1173 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,5 @@ private Cargo.lock Makefile .vscode -.idea \ No newline at end of file +.idea +.DS_store \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index 39d9e260..621b23c5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,75 @@ +## 0.26.0 + +### Breaking changes + +This release migrates parry from `nalgebra` to `glam` (via the `glamx` crate) for future compatibility with +`rust-gpu`. This is a major breaking change affecting almost all public APIs. + +#### Type renames + +- `Isometry` → `Pose` (using `glamx::Pose2`/`Pose3`) +- `Rotation` → `Rot2`/`Rot3` (using `glamx::Rot2`/`Rot3`) +- `IsometryOps` → `PoseOps` +- `IsometryOpt` → `PoseOpt` + +#### Removed types + +- `Point` - use `Vector` instead. Points and vectors are now unified. +- `UnitVector` - use `Vector` instead. Normalization is no longer encoded in the type. +- `Translation` - use `Vector` for translations. + +#### Math type changes + +- `Vector` is now `glam::Vec2`/`Vec3`/`DVec2`/`DVec3` depending on dimension and precision features +- `Matrix` is now `glam::Mat2`/`Mat3`/`DMat2`/`DMat3` +- The `math` module now re-exports glam types and provides dimension-agnostic aliases + +#### API signature changes + +- Many functions that previously took `&Point` or `&Vector` now take `Vector` by value +- Functions taking `&Isometry` now take `&Pose` or `Pose` by value +- `HalfSpace::new` now takes `Vector` instead of `Unit>` +- Shape constructors like `Segment::new`, `Triangle::new`, `Capsule::new` now take `Vector` instead of `Point` +- `Aabb::mins` and `Aabb::maxs` are now `Vector` instead of `Point` + +#### Migration guide + +If your codebase currently relies on `nalgebra`, note that `nalgebra` and `glamx` provide type conversion. Enable the +corresponding features: +- `nalgebra = { version = "0.34", features = [ "convert-glam030" ] }` +- `glamx = { version = "0.1", features = ["nalgebra"] }` + then you can convert between `glam` and `nalgebra` types using `.into()`. + +```rust +// Before (nalgebra) +use parry3d::na::{Point3, Vector3, Isometry3, Unit}; +let point = Point3::new(1.0, 2.0, 3.0); +let vector = Vector3::new(1.0, 0.0, 0.0); +let normal = Unit::new_normalize(vector); +let pos = Isometry3::translation(1.0, 2.0, 3.0); + +// After (glam) +use parry3d::math::{Vector, Pose}; +let point = Vector::new(1.0, 2.0, 3.0); // Points are now Vector +let vector = Vector::X; +let normal = vector.normalize(); // No Unit wrapper +let pos = Pose::translation(1.0, 2.0, 3.0); +``` + +Common patterns: +- `Point3::origin()` → `Vector::ZERO` +- `Vector3::x()` → `Vector::X` +- `point.coords` → just use the vector directly +- `Unit::new_normalize(v)` → `v.normalize()` +- `Isometry3::identity()` → `Pose::IDENTITY` +- `isometry.translation.vector` → `pose.translation` + +### Modified + +- Re-export `glamx` instead of `nalgebra` as the public linear algebra dependency +- Migrate visual examples from `macroquad` to `kiss3d` +- Renamed `rkyv-serialize` feature to just `rkyv` + ## 0.25.3 - Significantly improve performances of `Voxels::combine_voxel_states`. diff --git a/Cargo.toml b/Cargo.toml index aa82d173..9be7dc4b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -27,6 +27,7 @@ parry3d = { path = "crates/parry3d" } parry2d-f64 = { path = "crates/parry2d-f64" } parry3d-f64 = { path = "crates/parry3d-f64" } +#glamx = { path = "../glamx" } #simba = { path = "../simba" } #simba = { git = "https://github.com/dimforge/simba", rev = "45a5266eb36ed9d25907e9bf9130cd4ac846a748" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "0cf79aef0e6155befc3279a3145f1940822b8377" } diff --git a/Claude.md b/Claude.md index 20eb3cb3..6c0628ee 100644 --- a/Claude.md +++ b/Claude.md @@ -35,7 +35,7 @@ cargo build -p parry2d cd crates/parry3d && cargo build --features simd-stable # Build with all serialization features -cargo build --features bytemuck-serialize,serde-serialize,rkyv-serialize +cargo build --features bytemuck-serialize,serde-serialize,rkyv # Build with enhanced determinism (incompatible with SIMD) cargo build --features enhanced-determinism diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index 954a35a9..2fc37cad 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "parry2d-f64" -version = "0.25.3" +version = "0.26.0" authors = ["Sébastien Crozet "] description = "2 dimensional collision detection library in Rust. 64-bit precision version." @@ -23,7 +23,7 @@ workspace = true default = ["required-features", "std", "spade"] required-features = ["dim2", "f64"] std = [ - "nalgebra/std", + "glamx/std", "slab", "simba/std", "arrayvec/std", @@ -37,23 +37,19 @@ f64 = [] serde-serialize = [ "serde", "serde_arrays", - "nalgebra/serde-serialize", + "glamx/serde", "arrayvec/serde", "bitflags/serde", "hashbrown?/serde", "spade?/serde", ] -rkyv-serialize = [ - "rkyv/validation", - "nalgebra/rkyv-serialize", - "simba/rkyv-serialize", -] -bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] +rkyv = ["dep:rkyv", "glamx/rkyv"] +bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] -enhanced-determinism = ["simba/libm_force", "indexmap"] +enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] parallel = ["rayon"] -alloc = ["nalgebra/alloc", "hashbrown"] +alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] @@ -74,11 +70,11 @@ num-traits = { version = "0.2", default-features = false } slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } simba = { version = "0.9", default-features = false } -nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] } +glamx = { version = "0.1.2", default-features = false, features = ["nostd-libm", "approx"] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive"] } serde_arrays = { version = "0.2", optional = true } -rkyv = { version = "0.7.41", optional = true } +rkyv = { version = "0.8", optional = true, default-features = false, features = ["bytecheck", "alloc"] } num-derive = "0.4" indexmap = { version = "2", features = ["serde"], optional = true } hashbrown = { version = "0.16", optional = true, default-features = false, features = [ diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 69bea874..5e5bf436 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "parry2d" -version = "0.25.3" +version = "0.26.0" authors = ["Sébastien Crozet "] description = "2 dimensional collision detection library in Rust." @@ -24,7 +24,7 @@ default = ["required-features", "std", "spade"] required-features = ["dim2", "f32"] std = [ "alloc", - "nalgebra/std", + "glamx/std", "slab", "simba/std", "arrayvec/std", @@ -37,26 +37,22 @@ f32 = [] serde-serialize = [ "serde", "serde_arrays", - "nalgebra/serde-serialize", + "glamx/serde", "arrayvec/serde", "bitflags/serde", "hashbrown?/serde", "spade?/serde", ] -rkyv-serialize = [ - "rkyv/validation", - "nalgebra/rkyv-serialize", - "simba/rkyv-serialize", -] -bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] +rkyv = ["dep:rkyv", "glamx/rkyv"] +bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] -enhanced-determinism = ["simba/libm_force", "indexmap"] +enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] parallel = ["rayon"] -alloc = ["nalgebra/alloc", "hashbrown"] +alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] -encase = [ "dep:encase", "nalgebra/encase" ] +encase = [ "dep:encase", "glamx/encase" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. @@ -75,11 +71,11 @@ num-traits = { version = "0.2", default-features = false } slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } simba = { version = "0.9", default-features = false } -nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] } +glamx = { version = "0.1.2", default-features = false, features = ["nostd-libm", "approx"] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive"] } serde_arrays = { version = "0.2", optional = true } -rkyv = { version = "0.7.41", optional = true } +rkyv = { version = "0.8", optional = true, default-features = false, features = ["bytecheck", "alloc"] } num-derive = "0.4" indexmap = { version = "2", features = ["serde"], optional = true } hashbrown = { version = "0.16", optional = true, default-features = false, features = [ @@ -101,7 +97,8 @@ simba = { version = "0.9", default-features = false } oorandom = "11" ptree = "0.4.0" rand = { version = "0.9" } -macroquad = "0.4.12" +kiss3d = "0.39" +web-time = "1" [package.metadata.docs.rs] rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"] diff --git a/crates/parry2d/examples/aabb2d.rs b/crates/parry2d/examples/aabb2d.rs index a105f72b..20310542 100644 --- a/crates/parry2d/examples/aabb2d.rs +++ b/crates/parry2d/examples/aabb2d.rs @@ -1,22 +1,23 @@ -mod common_macroquad2d; +mod utils2d; -extern crate nalgebra as na; - -use common_macroquad2d::{draw_polyline, lissajous_2d, mquad_from_na, na_from_mquad}; -use macroquad::prelude::*; -use na::Isometry2; -use parry2d::bounding_volume::{Aabb, BoundingVolume}; +use kiss3d::prelude::*; +use parry2d::bounding_volume::BoundingVolume; +use parry2d::math::Pose; use parry2d::shape::Ball; +use utils2d::{draw_aabb2, draw_circle, lissajous_2d}; const RENDER_SCALE: f32 = 30.0; -#[macroquad::main("aabb2d")] +#[kiss3d::main] async fn main() { - let render_pos = Vec2::new(300.0, 300.0); + let mut window = Window::new("aabb2d").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 4.0); + let mut scene = SceneNode2d::empty(); + + let start_time = web_time::Instant::now(); - loop { - let elapsed_time = get_time() as f32 * 0.7; - clear_background(BLACK); + while window.render_2d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32() * 0.7; /* * Initialize the shapes. @@ -24,14 +25,15 @@ async fn main() { let ball1 = Ball::new(0.5); let ball2 = Ball::new(1.0); - let ball1_pos = na_from_mquad(lissajous_2d(elapsed_time)) * 5f32; - let ball2_pos = Isometry2::identity(); + let ball1_pos = lissajous_2d(elapsed_time) * 5f32; + let ball1_pose = Pose::from_translation(ball1_pos); + let ball2_pose = Pose::identity(); /* * Compute their axis-aligned bounding boxes. */ - let aabb_ball1 = ball1.aabb(&ball1_pos.into()); - let aabb_ball2 = ball2.aabb(&ball2_pos); + let aabb_ball1 = ball1.aabb(&ball1_pose); + let aabb_ball2 = ball2.aabb(&ball2_pose); // Merge the two boxes. let bounding_aabb = aabb_ball1.merged(&aabb_ball2); @@ -50,25 +52,37 @@ async fn main() { assert!(bounding_aabb.contains(&aabb_ball2)); assert!(loose_aabb_ball2.contains(&aabb_ball2)); - let ball1_translation = mquad_from_na(ball1_pos.coords.into()) * RENDER_SCALE + render_pos; draw_circle( - ball1_translation.x, - ball1_translation.y, + &mut window, + ball1_pos * RENDER_SCALE, ball1.radius * RENDER_SCALE, color, ); - let ball2_translation = - mquad_from_na(ball2_pos.translation.vector.into()) * RENDER_SCALE + render_pos; draw_circle( - ball2_translation.x, - ball2_translation.y, + &mut window, + ball2_pose.translation * RENDER_SCALE, ball2.radius * RENDER_SCALE, color, ); - draw_aabb(aabb_ball1, render_pos, color); - draw_aabb(aabb_ball2, render_pos, color); - draw_aabb(bounding_aabb, render_pos, YELLOW); + draw_aabb2( + &mut window, + aabb_ball1.mins * RENDER_SCALE, + aabb_ball1.maxs * RENDER_SCALE, + color, + ); + draw_aabb2( + &mut window, + aabb_ball2.mins * RENDER_SCALE, + aabb_ball2.maxs * RENDER_SCALE, + color, + ); + draw_aabb2( + &mut window, + bounding_aabb.mins * RENDER_SCALE, + bounding_aabb.maxs * RENDER_SCALE, + YELLOW, + ); // Inclusion test let color_included: Color = if loose_aabb_ball2.contains(&aabb_ball1) { @@ -76,26 +90,11 @@ async fn main() { } else { MAGENTA }; - draw_aabb(loose_aabb_ball2, render_pos, color_included); - next_frame().await + draw_aabb2( + &mut window, + loose_aabb_ball2.mins * RENDER_SCALE, + loose_aabb_ball2.maxs * RENDER_SCALE, + color_included, + ); } } - -fn draw_aabb(aabb: Aabb, offset: Vec2, color: Color) { - let mins = mquad_from_na(aabb.mins) * RENDER_SCALE + offset; - let maxs = mquad_from_na(aabb.maxs) * RENDER_SCALE + offset; - - let line = vec![ - Vec2::new(mins.x, mins.y), - Vec2::new(mins.x, maxs.y), - Vec2::new(maxs.x, maxs.y), - Vec2::new(maxs.x, mins.y), - Vec2::new(mins.x, mins.y), - ]; - let drawable_line = line - .iter() - .zip(line.iter().cycle().skip(1).take(line.len())) - .map(|item| (item.0.clone(), item.1.clone())) - .collect(); - draw_polyline(drawable_line, color); -} diff --git a/crates/parry2d/examples/bounding_sphere2d.rs b/crates/parry2d/examples/bounding_sphere2d.rs index 1785496c..9e9aa2f2 100644 --- a/crates/parry2d/examples/bounding_sphere2d.rs +++ b/crates/parry2d/examples/bounding_sphere2d.rs @@ -1,32 +1,33 @@ -mod common_macroquad2d; +mod utils2d; -extern crate nalgebra as na; - -use common_macroquad2d::{draw_polyline, lissajous_2d, mquad_from_na, na_from_mquad}; -use macroquad::prelude::*; -use na::{Isometry2, Vector2}; -use parry2d::bounding_volume::{Aabb, BoundingVolume}; +use kiss3d::prelude::*; +use parry2d::bounding_volume::BoundingVolume; +use parry2d::math::Pose; use parry2d::shape::Cuboid; +use utils2d::{draw_aabb2, draw_circle, lissajous_2d}; const RENDER_SCALE: f32 = 30.0; -#[macroquad::main("bounding_sphere2d")] +#[kiss3d::main] async fn main() { - let render_pos = Vec2::new(300.0, 300.0); + let mut window = Window::new("bounding_sphere2d").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 4.0); + let mut scene = SceneNode2d::empty(); + + let start_time = web_time::Instant::now(); - loop { - let elapsed_time = get_time() as f32 * 0.7; - clear_background(BLACK); + while window.render_2d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32() * 0.7; /* * Initialize the shapes. */ - let cube1: Cuboid = Cuboid::new(Vector2::repeat(0.5)); - let cube2 = Cuboid::new(Vector2::new(1., 0.5)); + let cube1: Cuboid = Cuboid::new(Vec2::splat(0.5)); + let cube2 = Cuboid::new(Vec2::new(1., 0.5)); - let cube1_pos = na_from_mquad(lissajous_2d(elapsed_time)) * 5f32; - let cube1_pos = Isometry2::from(cube1_pos); - let cube2_pos = Isometry2::identity(); + let cube1_pt = lissajous_2d(elapsed_time) * 5f32; + let cube1_pos = Pose::from_translation(cube1_pt); + let cube2_pos = Pose::identity(); /* * Compute their bounding spheres. @@ -55,35 +56,41 @@ async fn main() { // assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube1)); // assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube2)); - assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube1)); + // assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube1)); assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube2)); - let cube1_translation = - mquad_from_na(cube1_pos.translation.vector.into()) * RENDER_SCALE + render_pos; - draw_cuboid(cube1, cube1_translation, color); + // Draw cuboids using their local AABBs + let aabb1 = cube1.local_aabb(); + let aabb2 = cube2.local_aabb(); + draw_aabb2( + &mut window, + aabb1.mins * RENDER_SCALE + cube1_pos.translation * RENDER_SCALE, + aabb1.maxs * RENDER_SCALE + cube1_pos.translation * RENDER_SCALE, + color, + ); + draw_aabb2( + &mut window, + aabb2.mins * RENDER_SCALE + cube2_pos.translation * RENDER_SCALE, + aabb2.maxs * RENDER_SCALE + cube2_pos.translation * RENDER_SCALE, + color, + ); - let cube2_translation = - mquad_from_na(cube2_pos.translation.vector.into()) * RENDER_SCALE + render_pos; - draw_cuboid(cube2, cube2_translation, color); - draw_circle_lines( - bounding_sphere_cube1.center.x * RENDER_SCALE + render_pos.x, - bounding_sphere_cube1.center.y * RENDER_SCALE + render_pos.y, + draw_circle( + &mut window, + bounding_sphere_cube1.center * RENDER_SCALE, bounding_sphere_cube1.radius * RENDER_SCALE, - 2f32, color, ); - draw_circle_lines( - bounding_sphere_cube2.center.x * RENDER_SCALE + render_pos.x, - bounding_sphere_cube2.center.y * RENDER_SCALE + render_pos.y, + draw_circle( + &mut window, + bounding_sphere_cube2.center * RENDER_SCALE, bounding_sphere_cube2.radius * RENDER_SCALE, - 2f32, color, ); - draw_circle_lines( - bounding_bounding_sphere.center.x * RENDER_SCALE + render_pos.x, - bounding_bounding_sphere.center.y * RENDER_SCALE + render_pos.y, + draw_circle( + &mut window, + bounding_bounding_sphere.center * RENDER_SCALE, bounding_bounding_sphere.radius * RENDER_SCALE, - 2f32, YELLOW, ); @@ -94,37 +101,11 @@ async fn main() { } else { MAGENTA }; - draw_circle_lines( - loose_bounding_sphere_cube2.center.x * RENDER_SCALE + render_pos.x, - loose_bounding_sphere_cube2.center.y * RENDER_SCALE + render_pos.y, + draw_circle( + &mut window, + loose_bounding_sphere_cube2.center * RENDER_SCALE, loose_bounding_sphere_cube2.radius * RENDER_SCALE, - 2f32, color_included, ); - next_frame().await } } - -fn draw_cuboid(cuboid: Cuboid, pos: Vec2, color: Color) { - let aabb = cuboid.local_aabb(); - draw_aabb(aabb, pos, color) -} - -fn draw_aabb(aabb: Aabb, offset: Vec2, color: Color) { - let mins = mquad_from_na(aabb.mins) * RENDER_SCALE + offset; - let maxs = mquad_from_na(aabb.maxs) * RENDER_SCALE + offset; - - let line = vec![ - Vec2::new(mins.x, mins.y), - Vec2::new(mins.x, maxs.y), - Vec2::new(maxs.x, maxs.y), - Vec2::new(maxs.x, mins.y), - Vec2::new(mins.x, mins.y), - ]; - let drawable_line = line - .iter() - .zip(line.iter().cycle().skip(1).take(line.len())) - .map(|item| (item.0.clone(), item.1.clone())) - .collect(); - draw_polyline(drawable_line, color); -} diff --git a/crates/parry2d/examples/common_macroquad2d.rs b/crates/parry2d/examples/common_macroquad2d.rs deleted file mode 100644 index aada9964..00000000 --- a/crates/parry2d/examples/common_macroquad2d.rs +++ /dev/null @@ -1,132 +0,0 @@ -use core::f32::consts::{FRAC_PI_2, FRAC_PI_4}; - -use macroquad::prelude::*; -use macroquad::{ - color::{Color, WHITE}, - math::Vec2, - shapes::draw_line, -}; -use nalgebra::Point2; -use parry2d::math::Real; -use parry2d::shape::TriMesh; - -/// As this file is used as a module from other examples, -/// rustc warns about dead code: -/// - `main()` is needed for this file to be included in examples -/// - For other functions, they may be "dead code" for an example, but not for others. -#[allow(dead_code)] -fn main() { - println!( - "This module contains helper functions to use macroquad, - isolated from the rest of the examples for the sake of simplicity." - ); -} - -/// Converts a [`nalgebra::Point2`] to a [`Vec2`], which is used by [`macroquad`] -#[allow(dead_code)] -pub fn mquad_from_na(a: Point2) -> Vec2 { - Vec2::new(a.x, a.y) -} - -/// Converts a [`Vec2`] to a [`nalgebra::Point2`], which is used by [`parry3d`] -#[allow(dead_code)] -pub fn na_from_mquad(a: Vec2) -> Point2 { - Point2::new(a.x, a.y) -} - -/// Uses [`macroquad`] to display the line passed as parameter. -#[allow(dead_code)] -pub fn draw_polyline(polyline: Vec<(Vec2, Vec2)>, color: Color) { - for line in polyline { - let a = line.0; - let b = line.1; - draw_line_2d(a, b, color); - } -} - -/// Draws a text in the top left corner of the screen. -/// -/// This uses a hardcoded position, size, color. -#[allow(dead_code)] -pub fn easy_draw_text(text: &str) { - macroquad::text::draw_text(text, 10.0, 48.0 + 18.0, 30.0, WHITE); -} - -/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates for time `t`. -/// -/// This uses hardcoded parameters to have an arbitrary pleasing trajectory. -#[allow(dead_code)] -pub fn lissajous_2d(t: f32) -> Vec2 { - // Some hardcoded parameters to have a pleasing lissajous trajectory. - lissajous_2d_with_params(t, 3.0, 2.0, FRAC_PI_2, FRAC_PI_4) -} - -/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates. -#[allow(dead_code)] -pub fn lissajous_2d_with_params(t: f32, a: f32, b: f32, delta_x: f32, delta_y: f32) -> Vec2 { - // Some hardcoded parameters to have a pleasing lissajous trajectory. - - let x = (a * t + delta_x).sin(); - let y = (b * t + delta_y).sin(); - Vec2::new(x, y) * 0.75f32 -} - -/// Uses [`macroquad`] to display the line passed as parameter. -#[allow(dead_code)] -pub fn draw_line_2d(a: Vec2, b: Vec2, color: Color) { - draw_line(a.x, a.y, b.x, b.y, 2f32, color); -} - -/// Uses [`macroquad`] to display the line passed as parameter. -#[allow(dead_code)] -pub fn draw_trimesh2(trimesh: &TriMesh, offset: Vec2) { - let vertices = trimesh.vertices(); - for v in trimesh.indices() { - let v0 = mquad_from_na(vertices[v[0] as usize]) + offset; - let v1 = mquad_from_na(vertices[v[1] as usize]) + offset; - let v2 = mquad_from_na(vertices[v[2] as usize]) + offset; - - draw_line(v0.x, v0.y, v1.x, v1.y, 2f32, WHITE); - draw_line(v0.x, v0.y, v2.x, v2.y, 2f32, WHITE); - draw_line(v2.x, v2.y, v1.x, v1.y, 2f32, WHITE); - } -} - -/// Uses [`macroquad`] to display a wireframe of the polygon. -#[allow(dead_code)] -pub fn draw_polygon(polygon: &[Point2], scale: f32, shift: Point2, color: Color) { - for i in 0..polygon.len() { - let a = polygon[i]; - let b = polygon[(i + 1) % polygon.len()]; - draw_line( - a.x * scale + shift.x, - a.y * scale + shift.y, - b.x * scale + shift.x, - b.y * scale + shift.y, - 2.0, - color, - ); - } -} - -/// Uses [`macroquad`] to display the a cross, representing a point. -#[allow(dead_code)] -pub fn draw_point(point: Point2, scale: f32, shift: Point2, color: Color) { - let edge_len = 0.15; - draw_line( - (point.x - edge_len) * scale + shift.x, - point.y * scale + shift.y, - (point.x + edge_len) * scale + shift.x, - point.y * scale + shift.y, - 2.0, - color, - ); - draw_line( - point.x * scale + shift.x, - (point.y - edge_len) * scale + shift.y, - point.x * scale + shift.x, - (point.y + edge_len) * scale + shift.y, - 2.0, - color, - ); -} diff --git a/crates/parry2d/examples/contact_query2d.rs b/crates/parry2d/examples/contact_query2d.rs index 9df160e8..58f29fe0 100644 --- a/crates/parry2d/examples/contact_query2d.rs +++ b/crates/parry2d/examples/contact_query2d.rs @@ -1,18 +1,16 @@ -extern crate nalgebra as na; - -use na::{Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query; use parry2d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); let ball = Ball::new(1.0); let prediction = 1.0; - let cuboid_pos = Isometry2::identity(); - let ball_pos_penetrating = Isometry2::translation(1.0, 1.0); - let ball_pos_in_prediction = Isometry2::translation(2.0, 2.0); - let ball_pos_too_far = Isometry2::translation(3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_penetrating = Pose::translation(1.0, 1.0); + let ball_pos_in_prediction = Pose::translation(2.0, 2.0); + let ball_pos_too_far = Pose::translation(3.0, 3.0); let ctct_penetrating = query::contact( &ball_pos_penetrating, diff --git a/crates/parry2d/examples/convex2d.rs b/crates/parry2d/examples/convex2d.rs index 9bfeb8bb..964bb6d7 100644 --- a/crates/parry2d/examples/convex2d.rs +++ b/crates/parry2d/examples/convex2d.rs @@ -1,16 +1,15 @@ -extern crate nalgebra as na; extern crate num_traits as num; -use na::Point2; +use parry2d::math::Vector; use parry2d::shape::ConvexPolygon; fn main() { let points = [ - Point2::new(-1.0, 1.0), - Point2::new(-0.5, -0.5), - Point2::new(0.0, 0.5), - Point2::new(0.5, -0.5), - Point2::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + Vector::new(-0.5, -0.5), + Vector::new(0.0, 0.5), + Vector::new(0.5, -0.5), + Vector::new(1.0, 1.0), ]; let convex = ConvexPolygon::from_convex_hull(&points).expect("Invalid convex polygon."); diff --git a/crates/parry2d/examples/convex_hull2d.rs b/crates/parry2d/examples/convex_hull2d.rs index 3a5f0ec6..b76f289c 100644 --- a/crates/parry2d/examples/convex_hull2d.rs +++ b/crates/parry2d/examples/convex_hull2d.rs @@ -1,35 +1,39 @@ -mod common_macroquad2d; +mod utils2d; use core::f32::consts::{FRAC_PI_2, FRAC_PI_4}; -use common_macroquad2d::{draw_point, draw_polygon, lissajous_2d_with_params, na_from_mquad}; -use macroquad::prelude::*; -use nalgebra::Point2; +use kiss3d::prelude::*; use parry2d::transformation; +use utils2d::{draw_point, draw_polygon, lissajous_2d_with_params}; const RENDER_SCALE: f32 = 30.0; -#[macroquad::main("convex_hull2d")] +#[kiss3d::main] async fn main() { + let mut window = Window::new("convex_hull2d").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 4.0); + let mut scene = SceneNode2d::empty(); + let count = 9; - let mut pts = vec![Point2::default(); count]; + let mut pts = vec![Vec2::ZERO; count]; + + let render_pos = Vec2::ZERO; - let render_pos = Point2::new(300.0, 300.0); + let start_time = web_time::Instant::now(); - loop { - let elapsed_time = get_time() as f32; + while window.render_2d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32(); let elapsed_time_slow = elapsed_time * 0.2; - clear_background(BLACK); for (i, pt) in pts.iter_mut().enumerate() { - *pt = na_from_mquad(lissajous_2d_with_params( + *pt = lissajous_2d_with_params( (i * i) as f32 + elapsed_time_slow, 2.0 + i as f32 / 3.0, (i as f32 / count as f32) + elapsed_time_slow.cos() * 0.1, (elapsed_time_slow as f32 + i as f32).cos() * 0.1 + FRAC_PI_2, FRAC_PI_4, - )) * 5f32; - draw_point(*pt, RENDER_SCALE, render_pos, RED); + ) * 5f32; + draw_point(&mut window, *pt, RENDER_SCALE, render_pos, RED); } /* @@ -38,7 +42,6 @@ async fn main() { * */ let convex_hull = transformation::convex_hull(&pts); - draw_polygon(&convex_hull, RENDER_SCALE, render_pos, WHITE); - next_frame().await + draw_polygon(&mut window, &convex_hull, RENDER_SCALE, render_pos, WHITE); } } diff --git a/crates/parry2d/examples/convex_try_new2d.rs b/crates/parry2d/examples/convex_try_new2d.rs index 31eb0625..28dd765d 100644 --- a/crates/parry2d/examples/convex_try_new2d.rs +++ b/crates/parry2d/examples/convex_try_new2d.rs @@ -1,14 +1,12 @@ -extern crate nalgebra as na; - -use na::Point2; +use parry2d::math::Vector; use parry2d::shape::ConvexPolygon; fn main() { let points = vec![ - Point2::new(-1.0, 1.0), - Point2::new(-0.5, -0.5), - Point2::new(0.5, -0.5), - Point2::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + Vector::new(-0.5, -0.5), + Vector::new(0.5, -0.5), + Vector::new(1.0, 1.0), ]; let convex = ConvexPolygon::from_convex_polyline(points).expect("Invalid convex polygon."); diff --git a/crates/parry2d/examples/cuboid2d.rs b/crates/parry2d/examples/cuboid2d.rs index cc392b7a..b030d13c 100644 --- a/crates/parry2d/examples/cuboid2d.rs +++ b/crates/parry2d/examples/cuboid2d.rs @@ -1,10 +1,8 @@ -extern crate nalgebra as na; - -use na::Vector2; +use parry2d::math::Vector; use parry2d::shape::Cuboid; fn main() { - let cuboid = Cuboid::new(Vector2::new(2.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(2.0, 1.0)); assert!(cuboid.half_extents.x == 2.0); assert!(cuboid.half_extents.y == 1.0); diff --git a/crates/parry2d/examples/distance_query2d.rs b/crates/parry2d/examples/distance_query2d.rs index b2aef704..65c1105e 100644 --- a/crates/parry2d/examples/distance_query2d.rs +++ b/crates/parry2d/examples/distance_query2d.rs @@ -1,18 +1,17 @@ #[macro_use] extern crate approx; // for relative_eq! -extern crate nalgebra as na; -use na::{Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query; use parry2d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry2::identity(); - let ball_pos_intersecting = Isometry2::translation(0.0, 1.0); - let ball_pos_disjoint = Isometry2::translation(0.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(0.0, 1.0); + let ball_pos_disjoint = Pose::translation(0.0, 3.0); let dist_intersecting = query::distance(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap(); diff --git a/crates/parry2d/examples/plane2d.rs b/crates/parry2d/examples/plane2d.rs index fb801c61..c26b4aa1 100644 --- a/crates/parry2d/examples/plane2d.rs +++ b/crates/parry2d/examples/plane2d.rs @@ -1,10 +1,8 @@ -extern crate nalgebra as na; - -use na::Vector2; +use parry2d::math::Vector; use parry2d::shape::HalfSpace; fn main() { - let halfspace = HalfSpace::new(Vector2::::y_axis()); + let halfspace = HalfSpace::new(Vector::Y); assert!(halfspace.normal.x == 0.0); assert!(halfspace.normal.y == 1.0); diff --git a/crates/parry2d/examples/point_in_poly2d.rs b/crates/parry2d/examples/point_in_poly2d.rs index c0e5cfd7..aade43b1 100644 --- a/crates/parry2d/examples/point_in_poly2d.rs +++ b/crates/parry2d/examples/point_in_poly2d.rs @@ -1,95 +1,105 @@ -mod common_macroquad2d; +mod utils2d; -use common_macroquad2d::{draw_point, draw_polygon}; -use macroquad::prelude::*; -use nalgebra::{Point2, UnitComplex, Vector2}; +use kiss3d::prelude::*; +use parry2d::math::Rotation; use parry2d::utils::point_in_poly2d; +use utils2d::{draw_point, draw_polygon}; const RENDER_SCALE: f32 = 30.0; -#[macroquad::main("points_in_poly2d")] +#[kiss3d::main] async fn main() { + let mut window = Window::new("points_in_poly2d").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 2.0); + let mut scene = SceneNode2d::empty(); + let mut spikes = spikes_polygon(); let mut squares = squares_polygon(); let test_points = grid_points(); - let animation_rotation = UnitComplex::new(0.02); - let polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0); + let animation_rotation = Rotation::new(0.02); + let polygon_render_pos = Vec2::ZERO; + + let start_time = web_time::Instant::now(); + + while window.render_2d(&mut scene, &mut camera).await { + let i = (start_time.elapsed().as_secs_f32() * 60.0) as i32; - for i in 0.. { let polygon = if (i / 350) % 2 == 0 { &mut spikes } else { &mut squares }; - clear_background(BLACK); - polygon .iter_mut() - .for_each(|pt| *pt = animation_rotation * *pt); + .for_each(|pt| *pt = animation_rotation.transform_vector(*pt)); - draw_polygon(&polygon, RENDER_SCALE, polygon_render_pos, BLUE); + draw_polygon( + &mut window, + &polygon, + RENDER_SCALE, + polygon_render_pos, + BLUE, + ); /* * Compute polygon intersections. */ for point in &test_points { - if point_in_poly2d(point, &polygon) { - draw_point(*point, RENDER_SCALE, polygon_render_pos, RED); + if point_in_poly2d(*point, &polygon) { + draw_point(&mut window, *point, RENDER_SCALE, polygon_render_pos, RED); } else { - draw_point(*point, RENDER_SCALE, polygon_render_pos, GREEN); + draw_point(&mut window, *point, RENDER_SCALE, polygon_render_pos, GREEN); } } - - next_frame().await } } -fn spikes_polygon() -> Vec> { +fn spikes_polygon() -> Vec { let teeths = 3; let width = 15.0; let height = 7.5; let tooth_width = width / (teeths as f32); - let center = Vector2::new(width / 2.0, height / 2.0); + let center = Vec2::new(width / 2.0, height / 2.0); let mut polygon = vec![ - Point2::new(width, 0.0) - center, - Point2::new(width, height) - center, - Point2::new(0.0, height) - center, + Vec2::new(width, 0.0) - center, + Vec2::new(width, height) - center, + Vec2::new(0.0, height) - center, ]; for i in 0..teeths { let x = i as f32 * tooth_width; - polygon.push(Point2::new(x, 0.0) - center); - polygon.push(Point2::new(x + tooth_width / 2.0, height * 1.5) - center); + polygon.push(Vec2::new(x, 0.0) - center); + polygon.push(Vec2::new(x + tooth_width / 2.0, height * 1.5) - center); } polygon } -fn squares_polygon() -> Vec> { +fn squares_polygon() -> Vec { let scale = 3.0; [ - Point2::new(-1.0, -1.0) * scale, - Point2::new(0.0, -1.0) * scale, - Point2::new(0.0, 1.0) * scale, - Point2::new(-2.0, 1.0) * scale, - Point2::new(-2.0, -2.0) * scale, - Point2::new(1.0, -2.0) * scale, - Point2::new(1.0, 2.0) * scale, - Point2::new(-1.0, 2.0) * scale, + Vec2::new(-1.0, -1.0) * scale, + Vec2::new(0.0, -1.0) * scale, + Vec2::new(0.0, 1.0) * scale, + Vec2::new(-2.0, 1.0) * scale, + Vec2::new(-2.0, -2.0) * scale, + Vec2::new(1.0, -2.0) * scale, + Vec2::new(1.0, 2.0) * scale, + Vec2::new(-1.0, 2.0) * scale, ] .to_vec() } -fn grid_points() -> Vec> { +fn grid_points() -> Vec { let count = 40; let spacing = 0.6; let mut pts = vec![]; for i in 0..count { for j in 0..count { - pts.push(Point2::new( + pts.push(Vec2::new( (i as f32 - count as f32 / 2.0) * spacing, (j as f32 - count as f32 / 2.0) * spacing, )); diff --git a/crates/parry2d/examples/polygons_intersection2d.rs b/crates/parry2d/examples/polygons_intersection2d.rs index 4cbeb137..fd33510d 100644 --- a/crates/parry2d/examples/polygons_intersection2d.rs +++ b/crates/parry2d/examples/polygons_intersection2d.rs @@ -1,28 +1,35 @@ -mod common_macroquad2d; +mod utils2d; -use common_macroquad2d::draw_polygon; -use macroquad::prelude::*; -use nalgebra::{Point2, UnitComplex, Vector2}; +use kiss3d::prelude::*; +use parry2d::math::Rotation; use parry2d::shape::Ball; use parry2d::transformation::polygons_intersection_points; +use utils2d::draw_polygon; const RENDER_SCALE: f32 = 30.0; -#[macroquad::main("polygons_intersection2d")] +#[kiss3d::main] async fn main() { + let mut window = Window::new("polygons_intersection2d").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 2.0); + let mut scene = SceneNode2d::empty(); + let font = Font::default(); + let spikes = spikes_polygon(); let mut animated_spikes = spikes.clone(); let star = star_polygon(); let animation_scale = 2.0; - let animation_rotation = UnitComplex::new(0.008); + let animation_rotation = Rotation::new(0.008); + + let spikes_render_pos = Vec2::new(-150.0, 0.0); + let star_render_pos = Vec2::new(150.0, 0.0); - let spikes_render_pos = Point2::new(300.0, 300.0); - let star_render_pos = Point2::new(600.0, 300.0); + let start_time = web_time::Instant::now(); - for i in 0.. { - clear_background(BLACK); + while window.render_2d(&mut scene, &mut camera).await { + let i = (start_time.elapsed().as_secs_f32() * 60.0) as i32; /* * @@ -32,14 +39,13 @@ async fn main() { */ animated_spikes .iter_mut() - .for_each(|pt| *pt = animation_rotation * *pt); + .for_each(|pt| *pt = animation_rotation.transform_vector(*pt)); let spikes_intersections = polygons_intersection_points(&spikes, &animated_spikes); let animated_star: Vec<_> = star .iter() .map(|pt| { - animation_rotation.powf(i as f32) - * *pt + animation_rotation.powf(i as f32).transform_vector(*pt) * ((i as f32 / 100.0).sin().abs() * animation_scale) }) .collect(); @@ -51,65 +57,87 @@ async fn main() { * Render the polygons and their intersections. * */ - draw_polygon(&spikes, RENDER_SCALE, spikes_render_pos, BLUE); - draw_polygon(&animated_spikes, RENDER_SCALE, spikes_render_pos, GREEN); - - draw_polygon(&star, RENDER_SCALE, star_render_pos, BLUE); - draw_polygon(&animated_star, RENDER_SCALE, star_render_pos, GREEN); + draw_polygon(&mut window, &spikes, RENDER_SCALE, spikes_render_pos, BLUE); + draw_polygon( + &mut window, + &animated_spikes, + RENDER_SCALE, + spikes_render_pos, + GREEN, + ); + + draw_polygon(&mut window, &star, RENDER_SCALE, star_render_pos, BLUE); + draw_polygon( + &mut window, + &animated_star, + RENDER_SCALE, + star_render_pos, + GREEN, + ); if let Ok(intersections) = spikes_intersections { - draw_text( + window.draw_text( &format!("# spikes intersections: {}", intersections.len()), - 0.0, - 15.0, + Vec2::new(0.0, 15.0), 20.0, + &font, WHITE, ); for intersection in intersections { - draw_polygon(&intersection, RENDER_SCALE, spikes_render_pos, RED); + draw_polygon( + &mut window, + &intersection, + RENDER_SCALE, + spikes_render_pos, + RED, + ); } } if let Ok(intersections) = star_intersections { - draw_text( + window.draw_text( &format!("# star intersections: {}", intersections.len()), - 0.0, - 30.0, + Vec2::new(0.0, 35.0), 20.0, + &font, WHITE, ); for intersection in intersections { - draw_polygon(&intersection, RENDER_SCALE, star_render_pos, RED); + draw_polygon( + &mut window, + &intersection, + RENDER_SCALE, + star_render_pos, + RED, + ); } } - - next_frame().await } } -fn star_polygon() -> Vec> { +fn star_polygon() -> Vec { let mut star = Ball::new(1.5).to_polyline(10); star.iter_mut().step_by(2).for_each(|pt| *pt = *pt * 0.6); star } -fn spikes_polygon() -> Vec> { +fn spikes_polygon() -> Vec { let teeths = 5; let width = 10.0; let height = 5.0; let tooth_width = width / (teeths as f32); - let center = Vector2::new(width / 2.0, height / 2.0); + let center = Vec2::new(width / 2.0, height / 2.0); let mut polygon = vec![ - Point2::new(width, 0.0) - center, - Point2::new(width, height) - center, - Point2::new(0.0, height) - center, + Vec2::new(width, 0.0) - center, + Vec2::new(width, height) - center, + Vec2::new(0.0, height) - center, ]; for i in 0..teeths { let x = i as f32 * tooth_width; - polygon.push(Point2::new(x, 0.0) - center); - polygon.push(Point2::new(x + tooth_width / 2.0, height * 0.8) - center); + polygon.push(Vec2::new(x, 0.0) - center); + polygon.push(Vec2::new(x + tooth_width / 2.0, height * 0.8) - center); } polygon diff --git a/crates/parry2d/examples/polyline2d.rs b/crates/parry2d/examples/polyline2d.rs index e8a08e12..0e40b167 100644 --- a/crates/parry2d/examples/polyline2d.rs +++ b/crates/parry2d/examples/polyline2d.rs @@ -1,14 +1,12 @@ -extern crate nalgebra as na; - -use na::Point2; +use parry2d::math::Vector; use parry2d::shape::Polyline; fn main() { let points = vec![ - Point2::new(0.0, 1.0), - Point2::new(-1.0, -1.0), - Point2::new(0.0, -0.5), - Point2::new(1.0, -1.0), + Vector::new(0.0, 1.0), + Vector::new(-1.0, -1.0), + Vector::new(0.0, -0.5), + Vector::new(1.0, -1.0), ]; let indices = vec![ diff --git a/crates/parry2d/examples/project_point2d.rs b/crates/parry2d/examples/project_point2d.rs index ba615a0f..579824e1 100644 --- a/crates/parry2d/examples/project_point2d.rs +++ b/crates/parry2d/examples/project_point2d.rs @@ -1,42 +1,34 @@ -mod common_macroquad2d; +mod utils2d; -use common_macroquad2d::{draw_line_2d, draw_trimesh2, lissajous_2d, mquad_from_na, na_from_mquad}; -use macroquad::prelude::*; -use nalgebra::{Point3, UnitComplex, Vector2}; -use parry2d::math::{Isometry, Translation}; +use kiss3d::prelude::*; +use parry2d::math::{Pose, Rotation}; use parry2d::query::PointQuery; use parry2d::shape::{Cuboid, TriMesh, TriMeshFlags}; +use utils2d::{draw_circle, draw_line_2d, draw_trimesh2, lissajous_2d}; -#[macroquad::main("project_point2d")] +#[kiss3d::main] async fn main() { - // - // This is useful to test for https://github.com/dimforge/parry/pull/248 - let _points = vec![ - Point3::from([0.0, 0.0, 0.0]), - Point3::from([0.0, 0.0, 1.0]), - Point3::from([1.0, 0.0, 0.0]), - Point3::from([1.0, 0.0, 1.0]), - ]; - let _indices: Vec<[u32; 3]> = vec![[0, 1, 2], [1, 3, 2]]; + let mut window = Window::new("project_point2d").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 4.0); + let mut scene = SceneNode2d::empty(); let scale = 200f32; - let (points, indices) = Cuboid::new(Vector2::new(0.2 * scale, 0.5 * scale)).to_trimesh(); + let (points, indices) = Cuboid::new(Vec2::new(0.2 * scale, 0.5 * scale)).to_trimesh(); let trimesh = TriMesh::with_flags(points, indices, TriMeshFlags::ORIENTED).unwrap(); - for _i in 1.. { - clear_background(BLACK); - let elapsed_time = get_time() as f32; + let start_time = web_time::Instant::now(); + + while window.render_2d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32(); let slow_elapsed_time = elapsed_time / 3.0; - let offset = Vec2::new(screen_width() / 2f32, screen_height() / 2f32); + let offset = Vec2::ZERO; let point_to_project = lissajous_2d(slow_elapsed_time) * scale + offset; - let translation = Translation::new(offset.x, offset.y); - let rot = UnitComplex::identity(); let projected_point = trimesh.project_point( - &Isometry::from_parts(translation, rot), - &na_from_mquad(point_to_project), + &Pose::from_parts(offset, Rotation::identity()), + point_to_project, true, ); @@ -51,39 +43,28 @@ async fn main() { YELLOW }; - draw_line_2d( - point_to_project, - mquad_from_na(projected_point.point), - color, - ); - draw_circle(point_to_project.x, point_to_project.y, 10f32, color); - - draw_line_2d( - point_to_project, - mquad_from_na(projected_point.point), - color, - ); + draw_line_2d(&mut window, point_to_project, projected_point.point, color); + draw_circle(&mut window, point_to_project, 10f32, color); // fixed local point inside the shape let point_to_project = Vec2::ZERO; - let projected_point = trimesh.project_local_point(&na_from_mquad(point_to_project), true); + let projected_point = trimesh.project_local_point(point_to_project, true); let color = if projected_point.is_inside { RED } else { YELLOW }; // convert to "world" space - let point_to_project = point_to_project * scale + offset; - draw_circle(point_to_project.x, point_to_project.y, 10f32, color); + let point_to_project_world = point_to_project * scale + offset; + draw_circle(&mut window, point_to_project_world, 10f32, color); draw_line_2d( - point_to_project, - mquad_from_na(projected_point.point) * scale + offset, + &mut window, + point_to_project_world, + projected_point.point * scale + offset, color, ); // Mesh is rendered in the back, so we can see the other graphics elements - draw_trimesh2(&trimesh, offset); - - next_frame().await + draw_trimesh2(&mut window, &trimesh, offset); } } diff --git a/crates/parry2d/examples/proximity_query2d.rs b/crates/parry2d/examples/proximity_query2d.rs index 5177be75..401946a6 100644 --- a/crates/parry2d/examples/proximity_query2d.rs +++ b/crates/parry2d/examples/proximity_query2d.rs @@ -1,16 +1,14 @@ -extern crate nalgebra as na; - -use na::{Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query; use parry2d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry2::identity(); - let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); - let ball_pos_disjoint = Isometry2::translation(3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(1.0, 1.0); + let ball_pos_disjoint = Pose::translation(3.0, 3.0); assert!(query::intersection_test(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap()); assert!(!query::intersection_test(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid).unwrap()); diff --git a/crates/parry2d/examples/raycasts_animated.rs b/crates/parry2d/examples/raycasts_animated.rs index 3aa85fe8..cd88d98f 100644 --- a/crates/parry2d/examples/raycasts_animated.rs +++ b/crates/parry2d/examples/raycasts_animated.rs @@ -1,39 +1,44 @@ -mod common_macroquad2d; +mod utils2d; -use common_macroquad2d::draw_point; -use macroquad::prelude::*; -use nalgebra::{Isometry2, Point2, UnitComplex, Vector2}; -use parry2d::math::Isometry; +use kiss3d::prelude::*; +use parry2d::math::{Pose, Rotation}; use parry2d::query::{Ray, RayCast}; use parry2d::shape::Cuboid; +use utils2d::draw_point; const RENDER_SCALE: f32 = 30.0; -#[macroquad::main("raycasts_animated")] +#[kiss3d::main] async fn main() { + let mut window = Window::new("raycasts_animated").await; + let mut camera = PanZoomCamera2d::new(Vec2::ZERO, 4.0); + let mut scene = SceneNode2d::empty(); + let animation_scale = 1.4; let animation_rotation = 0.04; - for i in 1.. { - clear_background(BLACK); + let screen_shift = Vec2::ZERO; + let start_time = web_time::Instant::now(); + + while window.render_2d(&mut scene, &mut camera).await { + let i = (start_time.elapsed().as_secs_f32() * 60.0) as i32 + 1; - let screen_shift = Point2::new(screen_width() / 2.0, screen_height() / 2.0); /* * * Compute the scaled cuboid. * */ let cube = - Cuboid::new(Vector2::new(2.0, 2.0) * ((i as f32 / 50.0).sin().abs() * animation_scale)); - let cube_pose = Isometry2::rotation(0.008 * i as f32); + Cuboid::new(Vec2::new(2.0, 2.0) * ((i as f32 / 50.0).sin().abs() * animation_scale)); + let cube_pose = Pose::new(Vec2::ZERO, 0.008 * i as f32); /* * * Prepare a Raycast and compute its result against the shape. * */ let ray = Ray::new( - Point2::new(2.0, 2.0), - UnitComplex::new(animation_rotation * i as f32) * -Vector2::x(), + Vec2::new(2.0, 2.0), + Rotation::new(animation_rotation * i as f32).transform_vector(-Vec2::X), ); let toi = cube.cast_ray(&cube_pose, &ray, f32::MAX, true); @@ -44,9 +49,10 @@ async fn main() { */ if let Some(toi) = toi { if toi == 0f32 { - draw_point(ray.origin, RENDER_SCALE, screen_shift, YELLOW); + draw_point(&mut window, ray.origin, RENDER_SCALE, screen_shift, YELLOW); } else { drawline_from_to( + &mut window, ray.origin, ray.origin + ray.dir * toi, RENDER_SCALE, @@ -56,6 +62,7 @@ async fn main() { } } else { drawline_from_to( + &mut window, ray.origin, ray.origin + ray.dir * 1000f32, RENDER_SCALE, @@ -69,47 +76,39 @@ async fn main() { * Render the cuboid. * */ - draw_polygon( + draw_cuboid_polygon( + &mut window, &cube.to_polyline(), &cube_pose, RENDER_SCALE, screen_shift, GREEN, ); - - next_frame().await } } -fn draw_polygon( - polygon: &[Point2], - pose: &Isometry, +fn draw_cuboid_polygon( + window: &mut Window, + polygon: &[Vec2], + pose: &Pose, scale: f32, - shift: Point2, + shift: Vec2, color: Color, ) { for i in 0..polygon.len() { - let a = pose * (scale * polygon[i]); - let b = pose * (scale * polygon[(i + 1) % polygon.len()]); - draw_line( - a.x + shift.x, - a.y + shift.y, - b.x + shift.x, - b.y + shift.y, - 2.0, - color, - ); + let a = pose * (polygon[i] * scale) + shift; + let b = pose * (polygon[(i + 1) % polygon.len()] * scale) + shift; + window.draw_line_2d(a, b, color, 2.0); } } fn drawline_from_to( - from: Point2, - to: Point2, + window: &mut Window, + from: Vec2, + to: Vec2, scale: f32, - shift: Point2, + shift: Vec2, color: Color, ) { - let from = from * scale + shift.coords; - let to = to * scale + shift.coords; - draw_line(from.x, from.y, to.x, to.y, 2.0, color); + window.draw_line_2d(from * scale + shift, to * scale + shift, color, 2.0); } diff --git a/crates/parry2d/examples/solid_point_query2d.rs b/crates/parry2d/examples/solid_point_query2d.rs index 7885d57a..667f5891 100644 --- a/crates/parry2d/examples/solid_point_query2d.rs +++ b/crates/parry2d/examples/solid_point_query2d.rs @@ -1,33 +1,31 @@ -extern crate nalgebra as na; - -use na::{Isometry2, Point2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query::PointQuery; use parry2d::shape::Cuboid; fn main() { - let cuboid = Cuboid::new(Vector2::new(1.0, 2.0)); - let pt_inside = Point2::origin(); - let pt_outside = Point2::new(2.0, 2.0); + let cuboid = Cuboid::new(Vector::new(1.0, 2.0)); + let pt_inside = Vector::ZERO; + let pt_outside = Vector::new(2.0, 2.0); // Solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_inside, true), + cuboid.distance_to_point(&Pose::identity(), pt_inside, true), 0.0 ); // Non-solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_inside, false), + cuboid.distance_to_point(&Pose::identity(), pt_inside, false), -1.0 ); // The other point is outside of the cuboid so the `solid` flag has no effect. assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_outside, false), + cuboid.distance_to_point(&Pose::identity(), pt_outside, false), 1.0 ); assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_outside, true), + cuboid.distance_to_point(&Pose::identity(), pt_outside, true), 1.0 ); } diff --git a/crates/parry2d/examples/solid_ray_cast2d.rs b/crates/parry2d/examples/solid_ray_cast2d.rs index 78223df9..23712587 100644 --- a/crates/parry2d/examples/solid_ray_cast2d.rs +++ b/crates/parry2d/examples/solid_ray_cast2d.rs @@ -1,18 +1,16 @@ -extern crate nalgebra as na; - -use na::{Isometry2, Point2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query::{Ray, RayCast}; use parry2d::shape::Cuboid; fn main() { - let cuboid = Cuboid::new(Vector2::new(1.0, 2.0)); - let ray_inside = Ray::new(Point2::origin(), Vector2::y()); - let ray_miss = Ray::new(Point2::new(2.0, 2.0), Vector2::new(1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 2.0)); + let ray_inside = Ray::new(Vector::ZERO, Vector::Y); + let ray_miss = Ray::new(Vector::new(2.0, 2.0), Vector::new(1.0, 1.0)); // Solid cast. assert_eq!( cuboid - .cast_ray(&Isometry2::identity(), &ray_inside, f32::MAX, true) + .cast_ray(&Pose::identity(), &ray_inside, f32::MAX, true) .unwrap(), 0.0 ); @@ -20,16 +18,16 @@ fn main() { // Non-solid cast. assert_eq!( cuboid - .cast_ray(&Isometry2::identity(), &ray_inside, f32::MAX, false) + .cast_ray(&Pose::identity(), &ray_inside, f32::MAX, false) .unwrap(), 2.0 ); // The other ray does not intersect this shape. assert!(cuboid - .cast_ray(&Isometry2::identity(), &ray_miss, f32::MAX, false) + .cast_ray(&Pose::identity(), &ray_miss, f32::MAX, false) .is_none()); assert!(cuboid - .cast_ray(&Isometry2::identity(), &ray_miss, f32::MAX, true) + .cast_ray(&Pose::identity(), &ray_miss, f32::MAX, true) .is_none()); } diff --git a/crates/parry2d/examples/time_of_impact_query2d.rs b/crates/parry2d/examples/time_of_impact_query2d.rs index d96e1d07..8ef584d4 100644 --- a/crates/parry2d/examples/time_of_impact_query2d.rs +++ b/crates/parry2d/examples/time_of_impact_query2d.rs @@ -1,51 +1,49 @@ -extern crate nalgebra as na; - -use na::{Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query; use parry2d::query::ShapeCastOptions; use parry2d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry2::identity(); - let ball_pos_intersecting = Isometry2::translation(1.0, 1.0); - let ball_pos_will_touch = Isometry2::translation(2.0, 2.0); - let ball_pos_wont_touch = Isometry2::translation(3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(1.0, 1.0); + let ball_pos_will_touch = Pose::translation(2.0, 2.0); + let ball_pos_wont_touch = Pose::translation(3.0, 3.0); - let box_vel1 = Vector2::new(-1.0, 1.0); - let box_vel2 = Vector2::new(1.0, 1.0); + let box_vel1 = Vector::new(-1.0, 1.0); + let box_vel2 = Vector::new(1.0, 1.0); - let ball_vel1 = Vector2::new(2.0, 2.0); - let ball_vel2 = Vector2::new(-0.5, -0.5); + let ball_vel1 = Vector::new(2.0, 2.0); + let ball_vel2 = Vector::new(-0.5, -0.5); let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &box_vel1, + box_vel1, &cuboid, ShapeCastOptions::default(), ) .unwrap(); let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, - &ball_vel2, + ball_vel2, &ball, &cuboid_pos, - &box_vel2, + box_vel2, &cuboid, ShapeCastOptions::default(), ) .unwrap(); let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &box_vel1, + box_vel1, &cuboid, ShapeCastOptions::default(), ) diff --git a/crates/parry2d/examples/utils2d.rs b/crates/parry2d/examples/utils2d.rs new file mode 100644 index 00000000..07b9a165 --- /dev/null +++ b/crates/parry2d/examples/utils2d.rs @@ -0,0 +1,151 @@ +use core::f32::consts::{FRAC_PI_2, FRAC_PI_4}; +use std::sync::Arc; + +use kiss3d::prelude::*; +use parry2d::shape::TriMesh; + +/// As this file is used as a module from other examples, +/// rustc warns about dead code: +/// - `main()` is needed for this file to be included in examples +/// - For other functions, they may be "dead code" for an example, but not for others. +#[allow(dead_code)] +fn main() { + println!( + "This module contains helper functions to use kiss3d, + isolated from the rest of the examples for the sake of simplicity." + ); +} + +/// Uses [`kiss3d`] to display the line passed as parameter. +#[allow(dead_code)] +pub fn draw_polyline(window: &mut Window, polyline: Vec<(Vec2, Vec2)>, color: Color) { + for line in polyline { + let a = line.0; + let b = line.1; + draw_line_2d(window, a, b, color); + } +} + +/// Draws a text in the top left corner of the screen. +/// +/// This uses a hardcoded position, size, color. +#[allow(dead_code)] +pub fn draw_text(window: &mut Window, font: &Arc, text: &str) { + window.draw_text(text, Vec2::new(10.0, 66.0), 30.0, font, WHITE); +} + +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates for time `t`. +/// +/// This uses hardcoded parameters to have an arbitrary pleasing trajectory. +#[allow(dead_code)] +pub fn lissajous_2d(t: f32) -> Vec2 { + // Some hardcoded parameters to have a pleasing lissajous trajectory. + lissajous_2d_with_params(t, 3.0, 2.0, FRAC_PI_2, FRAC_PI_4) +} + +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates. +#[allow(dead_code)] +pub fn lissajous_2d_with_params(t: f32, a: f32, b: f32, delta_x: f32, delta_y: f32) -> Vec2 { + // Some hardcoded parameters to have a pleasing lissajous trajectory. + + let x = (a * t + delta_x).sin(); + let y = (b * t + delta_y).sin(); + Vec2::new(x, y) * 0.75f32 +} + +/// Uses [`kiss3d`] to display the line passed as parameter. +#[allow(dead_code)] +pub fn draw_line_2d(window: &mut Window, a: Vec2, b: Vec2, color: Color) { + window.draw_line_2d(a, b, color, 2.0); +} + +/// Uses [`kiss3d`] to display the trimesh passed as parameter. +#[allow(dead_code)] +pub fn draw_trimesh2(window: &mut Window, trimesh: &TriMesh, offset: Vec2) { + let vertices = trimesh.vertices(); + for v in trimesh.indices() { + let v0 = vertices[v[0] as usize] + offset; + let v1 = vertices[v[1] as usize] + offset; + let v2 = vertices[v[2] as usize] + offset; + + window.draw_line_2d(v0, v1, WHITE, 2.0); + window.draw_line_2d(v0, v2, WHITE, 2.0); + window.draw_line_2d(v2, v1, WHITE, 2.0); + } +} + +/// Uses [`kiss3d`] to display a wireframe of the polygon. +#[allow(dead_code)] +pub fn draw_polygon(window: &mut Window, polygon: &[Vec2], scale: f32, shift: Vec2, color: Color) { + for i in 0..polygon.len() { + let a = polygon[i]; + let b = polygon[(i + 1) % polygon.len()]; + window.draw_line_2d(a * scale + shift, b * scale + shift, color, 2.0); + } +} + +/// Uses [`kiss3d`] to display a cross, representing a point. +#[allow(dead_code)] +pub fn draw_point(window: &mut Window, point: Vec2, scale: f32, shift: Vec2, color: Color) { + let edge_len = 0.15; + let scaled_point = point * scale + shift; + let edge_offset = Vec2::new(edge_len * scale, 0.0); + let vert_offset = Vec2::new(0.0, edge_len * scale); + window.draw_line_2d( + scaled_point - edge_offset, + scaled_point + edge_offset, + color, + 2.0, + ); + window.draw_line_2d( + scaled_point - vert_offset, + scaled_point + vert_offset, + color, + 2.0, + ); +} + +/// Draws a circle outline. +#[allow(dead_code)] +pub fn draw_circle(window: &mut Window, center: Vec2, radius: f32, color: Color) { + let segments = 32; + let tau = std::f32::consts::TAU; + + for i in 0..segments { + let angle1 = (i as f32 / segments as f32) * tau; + let angle2 = ((i + 1) as f32 / segments as f32) * tau; + + let p1 = center + Vec2::new(radius * angle1.cos(), radius * angle1.sin()); + let p2 = center + Vec2::new(radius * angle2.cos(), radius * angle2.sin()); + window.draw_line_2d(p1, p2, color, 2.0); + } +} + +/// Draws a 2D AABB (axis-aligned bounding box) as a rectangle outline. +#[allow(dead_code)] +pub fn draw_aabb2(window: &mut Window, mins: Vec2, maxs: Vec2, color: Color) { + window.draw_line_2d( + Vec2::new(mins.x, mins.y), + Vec2::new(maxs.x, mins.y), + color, + 2.0, + ); + window.draw_line_2d( + Vec2::new(maxs.x, mins.y), + Vec2::new(maxs.x, maxs.y), + color, + 2.0, + ); + window.draw_line_2d( + Vec2::new(maxs.x, maxs.y), + Vec2::new(mins.x, maxs.y), + color, + 2.0, + ); + window.draw_line_2d( + Vec2::new(mins.x, maxs.y), + Vec2::new(mins.x, mins.y), + color, + 2.0, + ); +} diff --git a/crates/parry2d/tests/geometry/aabb_scale.rs b/crates/parry2d/tests/geometry/aabb_scale.rs index 07c8bf8a..37d7db98 100644 --- a/crates/parry2d/tests/geometry/aabb_scale.rs +++ b/crates/parry2d/tests/geometry/aabb_scale.rs @@ -1,16 +1,16 @@ -use na::{Point2, Vector2}; use parry2d::bounding_volume::Aabb; +use parry2d::math::Vector; #[test] fn test_aabb_scale_wrt_center() { - let aabb = Aabb::from_half_extents(Point2::new(1.0, 2.0), Vector2::new(4.0, 5.0)); - let scale = Vector2::new(10.0, -20.0); - let scaled_aabb = aabb.scaled_wrt_center(&scale); - let scaled_aabb_neg = aabb.scaled_wrt_center(&-scale); - let scaled_aabb_abs = aabb.scaled_wrt_center(&scale.abs()); + let aabb = Aabb::from_half_extents(Vector::new(1.0, 2.0), Vector::new(4.0, 5.0)); + let scale = Vector::new(10.0, -20.0); + let scaled_aabb = aabb.scaled_wrt_center(scale); + let scaled_aabb_neg = aabb.scaled_wrt_center(-scale); + let scaled_aabb_abs = aabb.scaled_wrt_center(scale.abs()); assert_eq!(&scaled_aabb, &scaled_aabb_neg); assert_eq!(&scaled_aabb, &scaled_aabb_abs); assert_eq!(aabb.center(), scaled_aabb.center()); - assert_eq!(scaled_aabb.half_extents(), Vector2::new(40.0, 100.0)); + assert_eq!(scaled_aabb.half_extents(), Vector::new(40.0, 100.0)); } diff --git a/crates/parry2d/tests/geometry/ball_ball_toi.rs b/crates/parry2d/tests/geometry/ball_ball_toi.rs index 5e007b34..8f8bcccc 100644 --- a/crates/parry2d/tests/geometry/ball_ball_toi.rs +++ b/crates/parry2d/tests/geometry/ball_ball_toi.rs @@ -1,6 +1,6 @@ // Issue #35 -use na::{self, Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query; use parry2d::query::details::ShapeCastOptions; use parry2d::shape::Ball; @@ -8,12 +8,12 @@ use parry2d::shape::Ball; #[test] fn test_ball_ball_toi() { let b = Ball::new(0.5); - let m1 = Isometry2::identity(); - let m2 = Isometry2::translation(0.0, 10.0); - let v1 = Vector2::new(0.0, 10.0); - let v2 = Vector2::zeros(); + let m1 = Pose::identity(); + let m2 = Pose::translation(0.0, 10.0); + let v1 = Vector::new(0.0, 10.0); + let v2 = Vector::ZERO; - let cast = query::cast_shapes(&m1, &v1, &b, &m2, &v2, &b, ShapeCastOptions::default()).unwrap(); + let cast = query::cast_shapes(&m1, v1, &b, &m2, v2, &b, ShapeCastOptions::default()).unwrap(); assert_eq!(cast.unwrap().time_of_impact, 0.9); } diff --git a/crates/parry2d/tests/geometry/ball_cuboid_contact.rs b/crates/parry2d/tests/geometry/ball_cuboid_contact.rs index b9edafc7..573127f1 100644 --- a/crates/parry2d/tests/geometry/ball_cuboid_contact.rs +++ b/crates/parry2d/tests/geometry/ball_cuboid_contact.rs @@ -1,4 +1,4 @@ -use nalgebra::{Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query; use parry2d::shape::{Ball, Cuboid}; #[cfg(feature = "improved_fixed_point_support")] @@ -6,10 +6,10 @@ use simba::scalar::FixedI40F24; #[test] fn test_ball_cuboid_query_contact() { - let cuboid = Cuboid::new(Vector2::new(0.5, 0.5)); - let cuboid_pos = Isometry2::translation(0.0, 4.0); + let cuboid = Cuboid::new(Vector::new(0.5, 0.5)); + let cuboid_pos = Pose::translation(0.0, 4.0); let ball = Ball::new(0.5); - let ball_pos = Isometry2::translation(0.0517938, 3.05178815); + let ball_pos = Pose::translation(0.0517938, 3.05178815); let ct = query::contact(&cuboid_pos, &cuboid, &ball_pos, &ball, 0.0).unwrap(); assert!(ct.is_some()); let ct = query::contact(&ball_pos, &ball, &cuboid_pos, &cuboid, 0.0).unwrap(); @@ -19,10 +19,10 @@ fn test_ball_cuboid_query_contact() { #[test] fn test_false_negative() { let contact = query::contact( - &Isometry2::translation(1.0, 1.0), + &Pose::translation(1.0, 1.0), &Ball::new(1.0), - &Isometry2::identity(), - &Cuboid::new(Vector2::new(1.0, 1.0)), + &Pose::identity(), + &Cuboid::new(Vector::new(1.0, 1.0)), 1.0, ) .unwrap() diff --git a/crates/parry2d/tests/geometry/convex_polygons_intersection.rs b/crates/parry2d/tests/geometry/convex_polygons_intersection.rs index 454eb3b7..fcbcc435 100644 --- a/crates/parry2d/tests/geometry/convex_polygons_intersection.rs +++ b/crates/parry2d/tests/geometry/convex_polygons_intersection.rs @@ -1,21 +1,21 @@ -use na::Point2; +use parry2d::math::Vector; use parry2d::transformation; #[test] pub fn disjoint_no_intersection() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::new(1.0, 0.0), - Point2::new(1.1, 0.0), - Point2::new(1.1, 0.1), - Point2::new(1.0, 0.1), + Vector::new(1.0, 0.0), + Vector::new(1.1, 0.0), + Vector::new(1.1, 0.1), + Vector::new(1.0, 0.1), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); @@ -27,25 +27,25 @@ pub fn disjoint_no_intersection() { pub fn full_inclusion() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::new(0.025, 0.025), - Point2::new(0.075, 0.025), - Point2::new(0.075, 0.075), - Point2::new(0.025, 0.075), + Vector::new(0.025, 0.025), + Vector::new(0.075, 0.025), + Vector::new(0.075, 0.075), + Vector::new(0.025, 0.075), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); - assert!(res.contains(&Point2::new(0.025, 0.025))); - assert!(res.contains(&Point2::new(0.075, 0.025))); - assert!(res.contains(&Point2::new(0.075, 0.075))); - assert!(res.contains(&Point2::new(0.025, 0.075))); + assert!(res.contains(Vector::new(0.025, 0.025))); + assert!(res.contains(Vector::new(0.075, 0.025))); + assert!(res.contains(Vector::new(0.075, 0.075))); + assert!(res.contains(Vector::new(0.025, 0.075))); assert_eq!(res.len(), 4); } @@ -53,24 +53,24 @@ pub fn full_inclusion() { pub fn two_edges_overlap() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::new(0.05, 0.05), - Point2::new(0.1, 0.05), - Point2::new(0.1, 0.1), - Point2::new(0.05, 0.1), + Vector::new(0.05, 0.05), + Vector::new(0.1, 0.05), + Vector::new(0.1, 0.1), + Vector::new(0.05, 0.1), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); - assert!(res.contains(&Point2::new(0.1, 0.05))); - assert!(res.contains(&Point2::new(0.1, 0.1))); - assert!(res.contains(&Point2::new(0.05, 0.1))); + assert!(res.contains(Vector::new(0.1, 0.05))); + assert!(res.contains(Vector::new(0.1, 0.1))); + assert!(res.contains(Vector::new(0.05, 0.1))); assert_eq!(res.len(), 3); } @@ -78,36 +78,36 @@ pub fn two_edges_overlap() { pub fn quadrilateral_intersection() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.3, 0.0), - Point2::new(0.3, 0.3), - Point2::new(0.0, 0.3), + Vector::ZERO, + Vector::new(0.3, 0.0), + Vector::new(0.3, 0.3), + Vector::new(0.0, 0.3), ]; let poly2 = [ - Point2::new(0.1, 0.4), - Point2::new(0.5, 0.1), - Point2::new(0.2, -0.1), - Point2::new(0.1, 0.2), + Vector::new(0.1, 0.4), + Vector::new(0.5, 0.1), + Vector::new(0.2, -0.1), + Vector::new(0.1, 0.2), ]; /* let poly2 = [ - Point2::new(0.1, 0.2), - Point2::new(0.2, -0.1), - Point2::new(0.5, 0.1), - Point2::new(0.1, 0.4), + Vector::new(0.1, 0.2), + Vector::new(0.2, -0.1), + Vector::new(0.5, 0.1), + Vector::new(0.1, 0.4), ];*/ transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); /* - Point2::new(0.16666666666666666,0.0), - Point2::new(0.3, 0.0) - Point2::new(0.3, 0.25) - Point2::new(0.23333333333333336, 0.3) - Point2::new(0.09999999999999998, 0.3) - Point2::new(0.1, 0.2) + Vector::new(0.16666666666666666,0.0), + Vector::new(0.3, 0.0) + Vector::new(0.3, 0.25) + Vector::new(0.23333333333333336, 0.3) + Vector::new(0.09999999999999998, 0.3) + Vector::new(0.1, 0.2) */ println!("{:?}", res); assert_eq!(res.len(), 6); @@ -117,23 +117,23 @@ pub fn quadrilateral_intersection() { pub fn single_segment_intersection() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::new(0.0 + 0.1, 0.025), - Point2::new(0.1 + 0.1, 0.025), - Point2::new(0.1 + 0.1, 0.075), - Point2::new(0.0 + 0.1, 0.075), + Vector::new(0.0 + 0.1, 0.025), + Vector::new(0.1 + 0.1, 0.025), + Vector::new(0.1 + 0.1, 0.075), + Vector::new(0.0 + 0.1, 0.075), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); - assert!(res.contains(&Point2::new(0.1, 0.025))); - assert!(res.contains(&Point2::new(0.1, 0.075))); + assert!(res.contains(Vector::new(0.1, 0.025))); + assert!(res.contains(Vector::new(0.1, 0.075))); assert_eq!(res.len(), 2); } @@ -141,23 +141,23 @@ pub fn single_segment_intersection() { pub fn single_edge_intersection() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::new(0.0 + 0.1, 0.0), - Point2::new(0.1 + 0.1, 0.0), - Point2::new(0.1 + 0.1, 0.1), - Point2::new(0.0 + 0.1, 0.1), + Vector::new(0.0 + 0.1, 0.0), + Vector::new(0.1 + 0.1, 0.0), + Vector::new(0.1 + 0.1, 0.1), + Vector::new(0.0 + 0.1, 0.1), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); - assert!(res.contains(&Point2::new(0.1, 0.0))); - assert!(res.contains(&Point2::new(0.1, 0.1))); + assert!(res.contains(Vector::new(0.1, 0.0))); + assert!(res.contains(Vector::new(0.1, 0.1))); assert_eq!(res.len(), 2); } @@ -165,25 +165,25 @@ pub fn single_edge_intersection() { pub fn intersect_vertices_only() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::new(0.05, 0.0), - Point2::new(0.1, 0.05), - Point2::new(0.05, 0.1), - Point2::new(0.0, 0.05), + Vector::new(0.05, 0.0), + Vector::new(0.1, 0.05), + Vector::new(0.05, 0.1), + Vector::new(0.0, 0.05), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); - assert!(res.contains(&Point2::new(0.05, 0.0))); - assert!(res.contains(&Point2::new(0.1, 0.05))); - assert!(res.contains(&Point2::new(0.05, 0.1))); - assert!(res.contains(&Point2::new(0.0, 0.05))); + assert!(res.contains(Vector::new(0.05, 0.0))); + assert!(res.contains(Vector::new(0.1, 0.05))); + assert!(res.contains(Vector::new(0.05, 0.1))); + assert!(res.contains(Vector::new(0.0, 0.05))); assert_eq!(res.len(), 4); } @@ -191,25 +191,25 @@ pub fn intersect_vertices_only() { pub fn partial_overlap() { let mut res = Vec::new(); let poly1 = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; let poly2 = [ - Point2::origin(), - Point2::new(0.05, 0.0), - Point2::new(0.05, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.05, 0.0), + Vector::new(0.05, 0.1), + Vector::new(0.0, 0.1), ]; transformation::convex_polygons_intersection_points(&poly1, &poly2, &mut res); - assert!(res.contains(&Point2::origin())); - assert!(res.contains(&Point2::new(0.05, 0.0))); - assert!(res.contains(&Point2::new(0.05, 0.1))); - assert!(res.contains(&Point2::new(0.0, 0.1))); + assert!(res.contains(Vector::ZERO)); + assert!(res.contains(Vector::new(0.05, 0.0))); + assert!(res.contains(Vector::new(0.05, 0.1))); + assert!(res.contains(Vector::new(0.0, 0.1))); assert_eq!(res.len(), 4); } @@ -217,17 +217,17 @@ pub fn partial_overlap() { pub fn full_overlap() { let mut res = Vec::new(); let poly = [ - Point2::origin(), - Point2::new(0.1, 0.0), - Point2::new(0.1, 0.1), - Point2::new(0.0, 0.1), + Vector::ZERO, + Vector::new(0.1, 0.0), + Vector::new(0.1, 0.1), + Vector::new(0.0, 0.1), ]; transformation::convex_polygons_intersection_points(&poly, &poly, &mut res); - assert!(res.contains(&Point2::origin())); - assert!(res.contains(&Point2::new(0.1, 0.0))); - assert!(res.contains(&Point2::new(0.1, 0.1))); - assert!(res.contains(&Point2::new(0.0, 0.1))); + assert!(res.contains(Vector::ZERO)); + assert!(res.contains(Vector::new(0.1, 0.0))); + assert!(res.contains(Vector::new(0.1, 0.1))); + assert!(res.contains(Vector::new(0.0, 0.1))); assert_eq!(res.len(), 4); } diff --git a/crates/parry2d/tests/geometry/epa2.rs b/crates/parry2d/tests/geometry/epa2.rs index 47727488..6daebdb0 100644 --- a/crates/parry2d/tests/geometry/epa2.rs +++ b/crates/parry2d/tests/geometry/epa2.rs @@ -1,35 +1,35 @@ -use na::{self, Isometry2, Vector2}; +use parry2d::math::{Pose, Vector}; use parry2d::query::{self, ContactManifold, DefaultQueryDispatcher, PersistentQueryDispatcher}; use parry2d::shape::Cuboid; #[test] #[allow(non_snake_case)] fn cuboid_cuboid_EPA() { - let c = Cuboid::new(Vector2::new(2.0, 1.0)); - let m1 = Isometry2::translation(3.5, 0.0); - let m2 = Isometry2::identity(); + let c = Cuboid::new(Vector::new(2.0, 1.0)); + let m1 = Pose::translation(3.5, 0.0); + let m2 = Pose::identity(); let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) .expect("Penetration not found."); assert_eq!(res.dist, -0.5); - assert_eq!(res.normal1, -Vector2::x_axis()); + assert_eq!(res.normal1, -Vector::X); - let m1 = Isometry2::translation(0.0, 0.2); + let m1 = Pose::translation(0.0, 0.2); let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) .expect("Penetration not found."); assert_eq!(res.dist, -1.8); - assert_eq!(res.normal1, -Vector2::y_axis()); + assert_eq!(res.normal1, -Vector::Y); } #[test] fn cuboids_large_size_ratio_issue_181() { - let cuboid_a = Cuboid::new(Vector2::new(10.0, 10.0)); - let cuboid_b = Cuboid::new(Vector2::new(300.0, 1.5)); + let cuboid_a = Cuboid::new(Vector::new(10.0, 10.0)); + let cuboid_b = Cuboid::new(Vector::new(300.0, 1.5)); - let pos_b = Isometry2::new(Vector2::new(5.0, 0.0), 1.5); + let pos_b = Pose::new(Vector::new(5.0, 0.0), 1.5); let dispatcher = DefaultQueryDispatcher; - let mut p = Vector2::new(0.0, 0.0); + let mut p = Vector::new(0.0, 0.0); let mut angle = 0.0; // Used to panic at some point: @@ -38,7 +38,7 @@ fn cuboids_large_size_ratio_issue_181() { p.x += 0.0001; angle += 0.005; - let pos_a = Isometry2::new(p, angle); + let pos_a = Pose::new(p, angle); let pos_ab = pos_a.inv_mul(&pos_b); let mut manifold: ContactManifold<(), ()> = ContactManifold::new(); dispatcher @@ -54,7 +54,7 @@ fn cuboids_large_size_ratio_issue_181() { .unwrap(); if let Some(deepest) = manifold.find_deepest_contact() { - p += pos_a * manifold.local_n1 * deepest.dist; + p += pos_a.rotation * (manifold.local_n1 * deepest.dist); } } } diff --git a/crates/parry2d/tests/geometry/epa_convergence.rs b/crates/parry2d/tests/geometry/epa_convergence.rs index e2206104..e07c14f7 100644 --- a/crates/parry2d/tests/geometry/epa_convergence.rs +++ b/crates/parry2d/tests/geometry/epa_convergence.rs @@ -1,6 +1,5 @@ -use na::Vector2; use parry2d::{ - math::{Isometry, Point, Real}, + math::{Pose, Vector}, query, shape::{Capsule, ConvexPolygon, SharedShape}, }; @@ -9,14 +8,14 @@ use parry2d::{ #[test] fn capsule_convergence() { let shape1 = Capsule::new_y(5.0, 10.0); - let mut vec = Vec::>::with_capacity(3); - vec.push(Point::::new(64.0, 507.0)); - vec.push(Point::::new(440.0, 326.0)); - vec.push(Point::::new(1072.0, 507.0)); + let mut vec = Vec::::with_capacity(3); + vec.push(Vector::new(64.0, 507.0)); + vec.push(Vector::new(440.0, 326.0)); + vec.push(Vector::new(1072.0, 507.0)); let shape2 = ConvexPolygon::from_convex_polyline(vec); let shape2 = shape2.unwrap(); - let transform1 = Isometry::new(Vector2::new(381.592, 348.491), 0.0); - let transform2 = Isometry::new(Vector2::new(0.0, 0.0), 0.0); + let transform1 = Pose::new(Vector::new(381.592, 348.491), 0.0); + let transform2 = Pose::new(Vector::new(0.0, 0.0), 0.0); let _res = query::details::contact_support_map_support_map( &transform1.inv_mul(&transform2), diff --git a/crates/parry2d/tests/geometry/ray_cast.rs b/crates/parry2d/tests/geometry/ray_cast.rs index 751273e2..8dadd58a 100644 --- a/crates/parry2d/tests/geometry/ray_cast.rs +++ b/crates/parry2d/tests/geometry/ray_cast.rs @@ -1,13 +1,12 @@ -use na::{self, Isometry2, Point2, Vector2}; -use parry2d::math::Real; +use parry2d::math::{Pose, Real, Vector}; use parry2d::query::{Ray, RayCast}; use parry2d::shape::{ConvexPolygon, Segment, Triangle}; #[test] fn issue_178_parallel_raycast() { - let m1 = Isometry2::identity(); - let ray = Ray::new(Point2::origin(), Vector2::new(0.0, 1.0)); - let seg = Segment::new(Point2::new(2.0, 1.0), Point2::new(2.0, 0.0)); + let m1 = Pose::identity(); + let ray = Ray::new(Vector::ZERO, Vector::new(0.0, 1.0)); + let seg = Segment::new(Vector::new(2.0, 1.0), Vector::new(2.0, 0.0)); let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); assert!(cast.is_none()); @@ -15,9 +14,9 @@ fn issue_178_parallel_raycast() { #[test] fn parallel_raycast() { - let m1 = Isometry2::identity(); - let ray = Ray::new(Point2::origin(), Vector2::new(0.0, 1.0)); - let seg = Segment::new(Point2::new(2.0, 1.0), Point2::new(2.0, -1.0)); + let m1 = Pose::identity(); + let ray = Ray::new(Vector::ZERO, Vector::new(0.0, 1.0)); + let seg = Segment::new(Vector::new(2.0, 1.0), Vector::new(2.0, -1.0)); let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); assert!(cast.is_none()); @@ -25,9 +24,9 @@ fn parallel_raycast() { #[test] fn collinear_raycast_starting_on_segment() { - let m1 = Isometry2::identity(); - let ray = Ray::new(Point2::origin(), Vector2::new(0.0, 1.0)); - let seg = Segment::new(Point2::new(0.0, 1.0), Point2::new(0.0, -1.0)); + let m1 = Pose::identity(); + let ray = Ray::new(Vector::ZERO, Vector::new(0.0, 1.0)); + let seg = Segment::new(Vector::new(0.0, 1.0), Vector::new(0.0, -1.0)); let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); assert_eq!(cast, Some(0.0)); @@ -35,9 +34,9 @@ fn collinear_raycast_starting_on_segment() { #[test] fn collinear_raycast_starting_below_segment() { - let m1 = Isometry2::identity(); - let ray = Ray::new(Point2::new(0.0, -2.0), Vector2::new(0.0, 1.0)); - let seg = Segment::new(Point2::new(0.0, 1.0), Point2::new(0.0, -1.0)); + let m1 = Pose::identity(); + let ray = Ray::new(Vector::new(0.0, -2.0), Vector::new(0.0, 1.0)); + let seg = Segment::new(Vector::new(0.0, 1.0), Vector::new(0.0, -1.0)); let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); assert_eq!(cast, Some(1.0)); @@ -45,9 +44,9 @@ fn collinear_raycast_starting_below_segment() { #[test] fn collinear_raycast_starting_above_segment() { - let m1 = Isometry2::identity(); - let ray = Ray::new(Point2::new(0.0, 2.0), Vector2::new(0.0, 1.0)); - let seg = Segment::new(Point2::new(0.0, 1.0), Point2::new(0.0, -1.0)); + let m1 = Pose::identity(); + let ray = Ray::new(Vector::new(0.0, 2.0), Vector::new(0.0, 1.0)); + let seg = Segment::new(Vector::new(0.0, 1.0), Vector::new(0.0, -1.0)); let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); assert_eq!(cast, None); @@ -55,22 +54,22 @@ fn collinear_raycast_starting_above_segment() { #[test] fn perpendicular_raycast_starting_behind_segment() { - let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); - let ray = Ray::new(Point2::new(-1.0, 0.0), Vector2::new(1.0, 0.0)); + let segment = Segment::new(Vector::new(0.0f32, -10.0), Vector::new(0.0, 10.0)); + let ray = Ray::new(Vector::new(-1.0, 0.0), Vector::new(1.0, 0.0)); assert!(segment.intersects_local_ray(&ray, f32::MAX)); } #[test] fn perpendicular_raycast_starting_in_front_of_segment() { - let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); - let ray = Ray::new(Point2::new(1.0, 0.0), Vector2::new(1.0, 0.0)); + let segment = Segment::new(Vector::new(0.0f32, -10.0), Vector::new(0.0, 10.0)); + let ray = Ray::new(Vector::new(1.0, 0.0), Vector::new(1.0, 0.0)); assert!(!segment.intersects_local_ray(&ray, f32::MAX)); } #[test] fn perpendicular_raycast_starting_on_segment() { - let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); - let ray = Ray::new(Point2::new(0.0, 3.0), Vector2::new(1.0, 0.0)); + let segment = Segment::new(Vector::new(0.0f32, -10.0), Vector::new(0.0, 10.0)); + let ray = Ray::new(Vector::new(0.0, 3.0), Vector::new(1.0, 0.0)); let cast = segment.cast_local_ray(&ray, f32::MAX, true); assert_eq!(cast, Some(0.0)); @@ -78,26 +77,26 @@ fn perpendicular_raycast_starting_on_segment() { #[test] fn perpendicular_raycast_starting_above_segment() { - let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); - let ray = Ray::new(Point2::new(0.0, 11.0), Vector2::new(1.0, 0.0)); + let segment = Segment::new(Vector::new(0.0f32, -10.0), Vector::new(0.0, 10.0)); + let ray = Ray::new(Vector::new(0.0, 11.0), Vector::new(1.0, 0.0)); assert!(!segment.intersects_local_ray(&ray, f32::MAX)); } #[test] fn perpendicular_raycast_starting_below_segment() { - let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); - let ray = Ray::new(Point2::new(0.0, -11.0), Vector2::new(1.0, 0.0)); + let segment = Segment::new(Vector::new(0.0f32, -10.0), Vector::new(0.0, 10.0)); + let ray = Ray::new(Vector::new(0.0, -11.0), Vector::new(1.0, 0.0)); assert!(!segment.intersects_local_ray(&ray, f32::MAX)); } #[test] fn raycast_starting_outside_of_triangle() { let triangle = Triangle::new( - Point2::new(0.0f32, -10.0), - Point2::new(0.0, 10.0), - Point2::new(10.0, 0.0), + Vector::new(0.0f32, -10.0), + Vector::new(0.0, 10.0), + Vector::new(10.0, 0.0), ); - let ray = Ray::new(Point2::new(-10.0, 0.0), Vector2::new(1.0, 0.0)); + let ray = Ray::new(Vector::new(-10.0, 0.0), Vector::new(1.0, 0.0)); let intersect = triangle .cast_local_ray_and_get_normal(&ray, f32::MAX, true) .expect("No intersection"); @@ -108,11 +107,11 @@ fn raycast_starting_outside_of_triangle() { #[test] fn raycast_starting_inside_of_triangle() { let triangle = Triangle::new( - Point2::new(0.0f32, -10.0), - Point2::new(0.0, 10.0), - Point2::new(10.0, 0.0), + Vector::new(0.0f32, -10.0), + Vector::new(0.0, 10.0), + Vector::new(10.0, 0.0), ); - let ray = Ray::new(Point2::new(2.0, 0.0), Vector2::new(1.0, 0.0)); + let ray = Ray::new(Vector::new(2.0, 0.0), Vector::new(1.0, 0.0)); let intersect = triangle .cast_local_ray_and_get_normal(&ray, f32::MAX, true) .expect("No intersection"); @@ -123,11 +122,11 @@ fn raycast_starting_inside_of_triangle() { #[test] fn raycast_starting_on_edge_of_triangle() { let triangle = Triangle::new( - Point2::new(0.0f32, -10.0), - Point2::new(0.0, 10.0), - Point2::new(10.0, 0.0), + Vector::new(0.0f32, -10.0), + Vector::new(0.0, 10.0), + Vector::new(10.0, 0.0), ); - let ray = Ray::new(Point2::origin(), Vector2::new(1.0, 0.0)); + let ray = Ray::new(Vector::ZERO, Vector::new(1.0, 0.0)); let intersect = triangle .cast_local_ray_and_get_normal(&ray, f32::MAX, true) .expect("No intersection"); @@ -148,18 +147,18 @@ fn raycast_starting_on_edge_of_triangle() { /// Tests the accuracy of raycaster collision detection against a `ConvexPolygon`. #[test] fn convexpoly_raycast_fuzz() { - let vertices: Vec> = vec![[2, 1], [2, 2], [1, 2], [1, 1]] + let vertices: Vec = vec![[2, 1], [2, 2], [1, 2], [1, 1]] .into_iter() - .map(|[x, y]| Point2::new(x as Real, y as Real)) + .map(|[x, y]| Vector::new(x as Real, y as Real)) .collect(); let square = ConvexPolygon::from_convex_polyline(vertices).unwrap(); - let test_raycast = |ray_origin: Point2, ray_look_at: Point2| -> Option { + let test_raycast = |ray_origin: Vector, ray_look_at: Vector| -> Option { let ray_angle = ray_look_at - ray_origin; square.cast_ray( - &Isometry2::identity(), + &Pose::identity(), &Ray::new(ray_origin, ray_angle.normalize()), Real::MAX, true, @@ -167,8 +166,8 @@ fn convexpoly_raycast_fuzz() { }; for i in 0..10_000 { - let ray_origin = Point2::new(3., 1. + (i as Real * 0.0001)); - let ray_look_at = Point2::new(0., 2.); + let ray_origin = Vector::new(3., 1. + (i as Real * 0.0001)); + let ray_look_at = Vector::new(0., 2.); let collision = test_raycast(ray_origin, ray_look_at); let eps = 1.0e-5; diff --git a/crates/parry2d/tests/geometry/time_of_impact2.rs b/crates/parry2d/tests/geometry/time_of_impact2.rs index 35d8a4f2..ef79072c 100644 --- a/crates/parry2d/tests/geometry/time_of_impact2.rs +++ b/crates/parry2d/tests/geometry/time_of_impact2.rs @@ -1,5 +1,4 @@ -use na::{self, Isometry2, Point2, Vector2}; -use parry2d::math::Real; +use parry2d::math::{Pose, Real, Vector}; use parry2d::query; use parry2d::query::details::ShapeCastOptions; use parry2d::shape::{Ball, Cuboid, Polyline, Segment}; @@ -9,19 +8,19 @@ fn ball_ball_intersecting_toi() { let ball1 = Ball::new(1.0); let ball2 = Ball::new(2.0); - let ball1_pos_intersecting = Isometry2::new(Vector2::new(1.0, 1.0), 0.0); - let ball2_pos = Isometry2::identity(); + let ball1_pos_intersecting = Pose::new(Vector::new(1.0, 1.0), 0.0); + let ball2_pos = Pose::identity(); - let ball1_vel_separating = Vector2::new(1.0, 2.0); - let ball1_vel_penetrating = Vector2::new(1.0, -2.0); - let ball2_vel = Vector2::zeros(); + let ball1_vel_separating = Vector::new(1.0, 2.0); + let ball1_vel_penetrating = Vector::new(1.0, -2.0); + let ball2_vel = Vector::ZERO; let toi_separating = query::cast_shapes( &ball1_pos_intersecting, - &ball1_vel_separating, + ball1_vel_separating, &ball1, &ball2_pos, - &ball2_vel, + ball2_vel, &ball2, ShapeCastOptions::default(), ) @@ -29,10 +28,10 @@ fn ball_ball_intersecting_toi() { let toi_penetrating_ignore_pen = query::cast_shapes( &ball1_pos_intersecting, - &ball1_vel_penetrating, + ball1_vel_penetrating, &ball1, &ball2_pos, - &ball2_vel, + ball2_vel, &ball2, ShapeCastOptions { stop_at_penetration: false, @@ -43,10 +42,10 @@ fn ball_ball_intersecting_toi() { let toi_separating_ignore_pen = query::cast_shapes( &ball1_pos_intersecting, - &ball1_vel_separating, + ball1_vel_separating, &ball1, &ball2_pos, - &ball2_vel, + ball2_vel, &ball2, ShapeCastOptions { stop_at_penetration: false, @@ -70,46 +69,46 @@ fn ball_ball_intersecting_toi() { #[test] fn ball_cuboid_toi() { - let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry2::identity(); - let ball_pos_intersecting = Isometry2::new(Vector2::new(1.0, 1.0), 0.0); - let ball_pos_will_touch = Isometry2::new(Vector2::new(2.0, 2.0), 0.0); - let ball_pos_wont_touch = Isometry2::new(Vector2::new(3.0, 3.0), 0.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::new(Vector::new(1.0, 1.0), 0.0); + let ball_pos_will_touch = Pose::new(Vector::new(2.0, 2.0), 0.0); + let ball_pos_wont_touch = Pose::new(Vector::new(3.0, 3.0), 0.0); - let cuboid_vel1 = Vector2::new(-1.0, 1.0); - let cuboid_vel2 = Vector2::new(1.0, 1.0); + let cuboid_vel1 = Vector::new(-1.0, 1.0); + let cuboid_vel2 = Vector::new(1.0, 1.0); - let ball_vel1 = Vector2::new(2.0, 2.0); - let ball_vel2 = Vector2::new(-0.5, -0.5); + let ball_vel1 = Vector::new(2.0, 2.0); + let ball_vel2 = Vector::new(-0.5, -0.5); let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &cuboid_vel1, + cuboid_vel1, &cuboid, ShapeCastOptions::default(), ) .unwrap(); let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, - &ball_vel2, + ball_vel2, &ball, &cuboid_pos, - &cuboid_vel2, + cuboid_vel2, &cuboid, ShapeCastOptions::default(), ) .unwrap(); let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, - &ball_vel2, + ball_vel2, &ball, &cuboid_pos, - &cuboid_vel1, + cuboid_vel1, &cuboid, ShapeCastOptions::default(), ) @@ -118,28 +117,28 @@ fn ball_cuboid_toi() { assert_eq!(toi_intersecting.map(|hit| hit.time_of_impact), Some(0.0)); assert!(relative_eq!( toi_will_touch.unwrap().time_of_impact, - ((2.0 as Real).sqrt() - 1.0) / (ball_vel2 - cuboid_vel2).norm() + ((2.0 as Real).sqrt() - 1.0) / (ball_vel2 - cuboid_vel2).length() )); assert_eq!(toi_wont_touch.map(|hit| hit.time_of_impact), None); } #[test] fn cuboid_cuboid_toi_issue_214() { - let shape1 = Cuboid::new(Vector2::new(1.0, 1.0)); - let shape2 = Cuboid::new(Vector2::new(1.0, 1.5)); + let shape1 = Cuboid::new(Vector::new(1.0, 1.0)); + let shape2 = Cuboid::new(Vector::new(1.0, 1.5)); - let pos1 = Isometry2::new(Vector2::new(0.0, 0.0), 0.0); - let pos2 = Isometry2::new(Vector2::new(10.0, 0.0), 0.0); + let pos1 = Pose::new(Vector::new(0.0, 0.0), 0.0); + let pos2 = Pose::new(Vector::new(10.0, 0.0), 0.0); - let vel1 = Vector2::new(1.0, 0.0); - let vel2 = Vector2::new(0.0, 0.0); + let vel1 = Vector::new(1.0, 0.0); + let vel2 = Vector::new(0.0, 0.0); let hit = query::cast_shapes( &pos1, - &vel1, + vel1, &shape1, &pos2, - &vel2, + vel2, &shape2, ShapeCastOptions::default(), ) @@ -149,28 +148,28 @@ fn cuboid_cuboid_toi_issue_214() { #[test] fn cast_shapes_should_return_toi_for_ball_and_rotated_polyline() { - let ball_isometry = Isometry2::identity(); - let ball_velocity = Vector2::new(1.0, 0.0); + let ball_isometry = Pose::identity(); + let ball_velocity = Vector::new(1.0, 0.0); let ball = Ball::new(0.5); - let polyline_isometry = Isometry2::rotation(-core::f32::consts::FRAC_PI_2); - let polyline_velocity = Vector2::zeros(); - let polyline = Polyline::new(vec![Point2::new(1.0, 1.0), Point2::new(-1.0, 1.0)], None); + let polyline_isometry = Pose::new(Vector::ZERO, -core::f32::consts::FRAC_PI_2); + let polyline_velocity = Vector::ZERO; + let polyline = Polyline::new(vec![Vector::new(1.0, 1.0), Vector::new(-1.0, 1.0)], None); assert_eq!( - polyline_isometry.transform_point(&Point2::new(1.0, 1.0)), - Point2::new(0.99999994, -1.0) + polyline_isometry.transform_point(Vector::new(1.0, 1.0)), + Vector::new(0.99999994, -1.0) ); assert_eq!( - polyline_isometry.transform_point(&Point2::new(-1.0, 1.0)), - Point2::new(1.0, 0.99999994) + polyline_isometry.transform_point(Vector::new(-1.0, 1.0)), + Vector::new(1.0, 0.99999994) ); let hit = query::cast_shapes( &ball_isometry, - &ball_velocity, + ball_velocity, &ball, &polyline_isometry, - &polyline_velocity, + polyline_velocity, &polyline, ShapeCastOptions::with_max_time_of_impact(1.0), ) @@ -181,28 +180,28 @@ fn cast_shapes_should_return_toi_for_ball_and_rotated_polyline() { #[test] fn cast_shapes_should_return_toi_for_ball_and_rotated_segment() { - let ball_isometry = Isometry2::identity(); - let ball_velocity = Vector2::new(1.0, 0.0); + let ball_isometry = Pose::identity(); + let ball_velocity = Vector::new(1.0, 0.0); let ball = Ball::new(0.5); - let segment_isometry = Isometry2::rotation(-core::f32::consts::FRAC_PI_2); - let segment_velocity = Vector2::zeros(); - let segment = Segment::new(Point2::new(1.0, 1.0), Point2::new(-1.0, 1.0)); + let segment_isometry = Pose::new(Vector::ZERO, -core::f32::consts::FRAC_PI_2); + let segment_velocity = Vector::ZERO; + let segment = Segment::new(Vector::new(1.0, 1.0), Vector::new(-1.0, 1.0)); assert_eq!( - segment_isometry.transform_point(&Point2::new(1.0, 1.0)), - Point2::new(0.99999994, -1.0) + segment_isometry.transform_point(Vector::new(1.0, 1.0)), + Vector::new(0.99999994, -1.0) ); assert_eq!( - segment_isometry.transform_point(&Point2::new(-1.0, 1.0)), - Point2::new(1.0, 0.99999994) + segment_isometry.transform_point(Vector::new(-1.0, 1.0)), + Vector::new(1.0, 0.99999994) ); let hit = query::cast_shapes( &ball_isometry, - &ball_velocity, + ball_velocity, &ball, &segment_isometry, - &segment_velocity, + segment_velocity, &segment, ShapeCastOptions::with_max_time_of_impact(1.0), ) @@ -213,28 +212,28 @@ fn cast_shapes_should_return_toi_for_ball_and_rotated_segment() { #[test] fn cast_shapes_should_return_toi_for_rotated_segment_and_ball() { - let ball_isometry = Isometry2::identity(); - let ball_velocity = Vector2::new(1.0, 0.0); + let ball_isometry = Pose::identity(); + let ball_velocity = Vector::new(1.0, 0.0); let ball = Ball::new(0.5); - let segment_isometry = Isometry2::rotation(-core::f32::consts::FRAC_PI_2); - let segment_velocity = Vector2::zeros(); - let segment = Segment::new(Point2::new(1.0, 1.0), Point2::new(-1.0, 1.0)); + let segment_isometry = Pose::new(Vector::ZERO, -core::f32::consts::FRAC_PI_2); + let segment_velocity = Vector::ZERO; + let segment = Segment::new(Vector::new(1.0, 1.0), Vector::new(-1.0, 1.0)); assert_eq!( - segment_isometry.transform_point(&Point2::new(1.0, 1.0)), - Point2::new(0.99999994, -1.0) + segment_isometry.transform_point(Vector::new(1.0, 1.0)), + Vector::new(0.99999994, -1.0) ); assert_eq!( - segment_isometry.transform_point(&Point2::new(-1.0, 1.0)), - Point2::new(1.0, 0.99999994) + segment_isometry.transform_point(Vector::new(-1.0, 1.0)), + Vector::new(1.0, 0.99999994) ); let hit = query::cast_shapes( &segment_isometry, - &segment_velocity, + segment_velocity, &segment, &ball_isometry, - &ball_velocity, + ball_velocity, &ball, ShapeCastOptions::with_max_time_of_impact(1.0), ) diff --git a/crates/parry2d/tests/lib.rs b/crates/parry2d/tests/lib.rs index 4557202e..4aaa2564 100644 --- a/crates/parry2d/tests/lib.rs +++ b/crates/parry2d/tests/lib.rs @@ -1,6 +1,5 @@ #[macro_use] extern crate approx; -extern crate nalgebra as na; extern crate parry2d; mod geometry; diff --git a/crates/parry2d/tests/query/point_composite_shape.rs b/crates/parry2d/tests/query/point_composite_shape.rs index aae5f406..d561a206 100644 --- a/crates/parry2d/tests/query/point_composite_shape.rs +++ b/crates/parry2d/tests/query/point_composite_shape.rs @@ -1,24 +1,24 @@ -use na::Point2; +use parry2d::math::Vector; use parry2d::{query::PointQuery, shape::TriMesh}; #[test] fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { let vertices = vec![ - Point2::new(0.0, 1.0), - Point2::origin(), - Point2::new(1.0, 0.0), - Point2::new(1.0, 1.0), + Vector::new(0.0, 1.0), + Vector::ZERO, + Vector::new(1.0, 0.0), + Vector::new(1.0, 1.0), ]; let mesh = TriMesh::new(vertices, vec![[0, 1, 2], [3, 0, 2]]).unwrap(); - let query_pt = Point2::new(0.6, 0.6); // Inside the top-right triangle (index 1) + let query_pt = Vector::new(0.6, 0.6); // Inside the top-right triangle (index 1) - let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt); + let (proj, feat) = mesh.project_local_point_and_get_feature(query_pt); let correct_tri_idx = 1; let correct_tri = mesh.triangle(correct_tri_idx); - let is_inside_correct = correct_tri.contains_local_point(&query_pt); + let is_inside_correct = correct_tri.contains_local_point(query_pt); assert!(is_inside_correct); assert_eq!(proj.is_inside, is_inside_correct); @@ -28,42 +28,42 @@ fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { #[test] fn project_local_point_and_get_feature_projects_correctly_from_outside() { let vertices = vec![ - Point2::new(0.0, 1.0), - Point2::origin(), - Point2::new(1.0, 0.0), - Point2::new(1.0, 1.0), + Vector::new(0.0, 1.0), + Vector::ZERO, + Vector::new(1.0, 0.0), + Vector::new(1.0, 1.0), ]; let mesh = TriMesh::new(vertices, vec![[0, 1, 2], [3, 0, 2]]).unwrap(); { - let query_pt = Point2::new(-1.0, 0.0); // Left from the bottom-left triangle (index 0) + let query_pt = Vector::new(-1.0, 0.0); // Left from the bottom-left triangle (index 0) - let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt); + let (proj, feat) = mesh.project_local_point_and_get_feature(query_pt); let correct_tri_idx = 0; let correct_tri = mesh.triangle(correct_tri_idx); - let is_inside_correct = correct_tri.contains_local_point(&query_pt); + let is_inside_correct = correct_tri.contains_local_point(query_pt); assert_eq!(is_inside_correct, false); assert_eq!(proj.is_inside, is_inside_correct); - assert_eq!(proj.point, Point2::origin()); + assert_eq!(proj.point, Vector::ZERO); assert_eq!(feat.unwrap_face(), correct_tri_idx); } { - let query_pt = Point2::new(0.5, 2.0); // Above the top-right triangle (index 1) + let query_pt = Vector::new(0.5, 2.0); // Above the top-right triangle (index 1) - let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt); + let (proj, feat) = mesh.project_local_point_and_get_feature(query_pt); let correct_tri_idx = 1; let correct_tri = mesh.triangle(correct_tri_idx); - let is_inside_correct = correct_tri.contains_local_point(&query_pt); + let is_inside_correct = correct_tri.contains_local_point(query_pt); assert_eq!(is_inside_correct, false); assert_eq!(proj.is_inside, is_inside_correct); - assert_eq!(proj.point, Point2::new(0.5, 1.0)); + assert_eq!(proj.point, Vector::new(0.5, 1.0)); assert_eq!(feat.unwrap_face(), correct_tri_idx); } } diff --git a/crates/parry2d/tests/query/point_triangle.rs b/crates/parry2d/tests/query/point_triangle.rs index 75fde225..901dc35b 100644 --- a/crates/parry2d/tests/query/point_triangle.rs +++ b/crates/parry2d/tests/query/point_triangle.rs @@ -1,19 +1,19 @@ -use parry2d::{math::Point, query::PointQuery, shape::Triangle}; +use parry2d::{math::Vector, query::PointQuery, shape::Triangle}; #[test] fn project_local_point_point_on_ab() { let verts = [ - Point::new(2.0, 1.0), - Point::new(0.0, 1.0), - Point::new(1.0, 0.0), + Vector::new(2.0, 1.0), + Vector::new(0.0, 1.0), + Vector::new(1.0, 0.0), ]; let tri1 = Triangle::new(verts[0], verts[1], verts[2]); let tri2 = Triangle::new(verts[2], verts[0], verts[1]); - let query_pt = Point::new(1.4, 1.0); + let query_pt = Vector::new(1.4, 1.0); - let proj1 = tri1.project_local_point(&query_pt, false); // Used to fail on 0.14 and earlier - let proj2 = tri2.project_local_point(&query_pt, false); + let proj1 = tri1.project_local_point(query_pt, false); // Used to fail on 0.14 and earlier + let proj2 = tri2.project_local_point(query_pt, false); assert_eq!(proj1.point, proj2.point); assert_eq!(proj1.point, query_pt); diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index 6f2571e4..6f8a0331 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "parry3d-f64" -version = "0.25.3" +version = "0.26.0" authors = ["Sébastien Crozet "] description = "3 dimensional collision detection library in Rust. 64-bits precision version." @@ -23,7 +23,7 @@ workspace = true default = ["required-features", "std", "spade"] required-features = ["dim3", "f64"] std = [ - "nalgebra/std", + "glamx/std", "slab", "simba/std", "arrayvec/std", @@ -36,24 +36,20 @@ f64 = [] serde-serialize = [ "serde", "serde_arrays", - "nalgebra/serde-serialize", + "glamx/serde", "bitflags/serde", "hashbrown?/serde", "spade?/serde", ] -rkyv-serialize = [ - "rkyv/validation", - "nalgebra/rkyv-serialize", - "simba/rkyv-serialize", -] -bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] +rkyv = ["dep:rkyv", "glamx/rkyv"] +bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] -enhanced-determinism = ["simba/libm_force", "indexmap"] +enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] parallel = ["rayon"] # Adds `TriMesh:to_obj_file` function. wavefront = ["obj"] -alloc = ["nalgebra/alloc", "hashbrown"] +alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] @@ -74,11 +70,11 @@ num-traits = { version = "0.2", default-features = false } slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } simba = { version = "0.9", default-features = false } -nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] } +glamx = { version = "0.1.2", default-features = false, features = ["nostd-libm", "approx"] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive", "rc"] } serde_arrays = { version = "0.2", optional = true } -rkyv = { version = "0.7.41", optional = true } +rkyv = { version = "0.8", optional = true, default-features = false, features = ["bytecheck", "alloc"] } num-derive = "0.4" indexmap = { version = "2", features = ["serde"], optional = true } hashbrown = { version = "0.16", optional = true, default-features = false, features = [ diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index c74dd4ac..d76a155a 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "parry3d" -version = "0.25.3" +version = "0.26.0" authors = ["Sébastien Crozet "] description = "3 dimensional collision detection library in Rust." @@ -23,7 +23,7 @@ workspace = true default = ["required-features", "std", "spade"] required-features = ["dim3", "f32"] std = [ - "nalgebra/std", + "glamx/std", "slab", "simba/std", "arrayvec/std", @@ -36,32 +36,28 @@ f32 = [] serde-serialize = [ "serde", "serde_arrays", - "nalgebra/serde-serialize", + "glamx/serde", "bitflags/serde", "hashbrown?/serde", "spade?/serde", ] -rkyv-serialize = [ - "rkyv/validation", - "nalgebra/rkyv-serialize", - "simba/rkyv-serialize", -] -bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] +rkyv = ["dep:rkyv", "glamx/rkyv"] +bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] -enhanced-determinism = ["simba/libm_force", "indexmap"] +enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] parallel = ["rayon"] # Adds `TriMesh:to_obj_file` function. wavefront = ["obj"] -alloc = ["nalgebra/alloc", "hashbrown"] +alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] -encase = [ "dep:encase", "nalgebra/encase" ] +encase = [ "dep:encase", "glamx/encase" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [ "glam" ] +simd-is-enabled = [] [lib] name = "parry3d" @@ -76,11 +72,11 @@ num-traits = { version = "0.2", default-features = false } slab = { version = "0.4", optional = true } arrayvec = { version = "0.7", default-features = false } simba = { version = "0.9", default-features = false } -nalgebra = { version = "0.34", default-features = false, features = ["libm", "macros"] } +glamx = { version = "0.1.2", default-features = false, features = ["nostd-libm", "approx"] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", optional = true, features = ["derive", "rc"] } serde_arrays = { version = "0.2", optional = true } -rkyv = { version = "0.7.41", optional = true } +rkyv = { version = "0.8", optional = true, default-features = false, features = ["bytecheck", "alloc"] } num-derive = "0.4" indexmap = { version = "2", features = ["serde"], optional = true } hashbrown = { version = "0.16", optional = true, default-features = false, features = [ @@ -100,18 +96,14 @@ static_assertions = "1" foldhash = { version = "0.2", default-features = false } encase = { version = "0.12", optional = true } -# NOTE: needed only for element_min for SIMD BVH ray-casting. -# can be removed once `wide` supports it (and allows filtering-out the -# fourth element). -glam = { version = "0.30.4", optional = true } [dev-dependencies] oorandom = "11" ptree = "0.4.0" rand = { version = "0.9" } -macroquad = "0.4.12" -nalgebra = { version = "0.34", default-features = false, features = ["rand"] } +kiss3d = "0.39" rand_isaac = "0.4" +web-time = "1" [package.metadata.docs.rs] rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"] diff --git a/crates/parry3d/benches/all.rs b/crates/parry3d/benches/all.rs index 5b9f7076..35d79f8e 100644 --- a/crates/parry3d/benches/all.rs +++ b/crates/parry3d/benches/all.rs @@ -1,7 +1,6 @@ #![feature(test)] #![allow(unused_macros)] -extern crate nalgebra as na; extern crate parry3d; extern crate rand; extern crate test; diff --git a/crates/parry3d/benches/bounding_volume/mod.rs b/crates/parry3d/benches/bounding_volume/mod.rs index 8123af9e..3ac8675c 100644 --- a/crates/parry3d/benches/bounding_volume/mod.rs +++ b/crates/parry3d/benches/bounding_volume/mod.rs @@ -1,8 +1,8 @@ use crate::common::generate_trimesh_around_origin; use crate::common::{generate, unref}; -use na::Isometry3; use parry3d::bounding_volume::BoundingVolume; use parry3d::bounding_volume::{Aabb, BoundingSphere}; +use parry3d::math::Pose3; use parry3d::shape::{Ball, Capsule, Cone, Cuboid, Cylinder, Segment, Triangle}; use parry3d::shape::{ConvexPolyhedron, TriMesh}; use rand::SeedableRng; @@ -56,104 +56,104 @@ bench_method!( /* * Bounding volume construction. */ -bench_method!(bench_cuboid_aabb, aabb: Aabb, c: Cuboid, m: Isometry3); +bench_method!(bench_cuboid_aabb, aabb: Aabb, c: Cuboid, m: Pose3); bench_method!( bench_cuboid_bounding_sphere, bounding_sphere: BoundingSphere, c: Cuboid, - m: Isometry3 + m: Pose3 ); -bench_method!(bench_ball_aabb, aabb: Aabb, b: Ball, m: Isometry3); +bench_method!(bench_ball_aabb, aabb: Aabb, b: Ball, m: Pose3); bench_method!( bench_ball_bounding_sphere, bounding_sphere: BoundingSphere, b: Ball, - m: Isometry3 + m: Pose3 ); bench_method!( bench_capsule_aabb, aabb: Aabb, c: Capsule, - m: Isometry3 + m: Pose3 ); bench_method!( bench_capsule_bounding_sphere, bounding_sphere: BoundingSphere, c: Capsule, - m: Isometry3 + m: Pose3 ); -bench_method!(bench_cone_aabb, aabb: Aabb, c: Cone, m: Isometry3); +bench_method!(bench_cone_aabb, aabb: Aabb, c: Cone, m: Pose3); bench_method!( bench_cone_bounding_sphere, bounding_sphere: BoundingSphere, c: Cone, - m: Isometry3 + m: Pose3 ); bench_method!( bench_cylinder_aabb, aabb: Aabb, c: Cylinder, - m: Isometry3 + m: Pose3 ); bench_method!( bench_cylinder_bounding_sphere, bounding_sphere: BoundingSphere, c: Cylinder, - m: Isometry3 + m: Pose3 ); bench_method!( bench_segment_aabb, aabb: Aabb, c: Segment, - m: Isometry3 + m: Pose3 ); bench_method!( bench_segment_bounding_sphere, bounding_sphere: BoundingSphere, c: Segment, - m: Isometry3 + m: Pose3 ); bench_method!( bench_triangle_aabb, aabb: Aabb, c: Triangle, - m: Isometry3 + m: Pose3 ); bench_method!( bench_triangle_bounding_sphere, bounding_sphere: BoundingSphere, c: Triangle, - m: Isometry3 + m: Pose3 ); bench_method!( bench_convex_aabb, aabb: Aabb, c: ConvexPolyhedron, - m: Isometry3 + m: Pose3 ); bench_method!( bench_convex_bounding_sphere, bounding_sphere: BoundingSphere, c: ConvexPolyhedron, - m: Isometry3 + m: Pose3 ); bench_method_gen!( bench_mesh_aabb, aabb: Aabb, mesh: TriMesh = generate_trimesh_around_origin, - m: Isometry3 = generate + m: Pose3 = generate ); bench_method_gen!( bench_mesh_bounding_sphere, bounding_sphere: BoundingSphere, mesh: TriMesh = generate_trimesh_around_origin, - m: Isometry3 = generate + m: Pose3 = generate ); diff --git a/crates/parry3d/benches/common/default_gen.rs b/crates/parry3d/benches/common/default_gen.rs index 527f3766..3fd059fd 100644 --- a/crates/parry3d/benches/common/default_gen.rs +++ b/crates/parry3d/benches/common/default_gen.rs @@ -1,13 +1,8 @@ -use na::{ - self, Isometry2, Isometry3, Matrix2, Matrix3, Matrix4, Point2, Point3, Point4, Vector2, - Vector3, Vector4, -}; use parry3d::bounding_volume::{Aabb, BoundingSphere}; -use parry3d::math::{Point, Real, Vector}; +use parry3d::math::{Pose3, Real, Rot3, Vec3, Vector}; use parry3d::query::Ray; use parry3d::shape::ConvexPolyhedron; use parry3d::shape::{Ball, Capsule, Cone, Cuboid, Cylinder, Segment, Triangle}; -use rand::distr::{Distribution, StandardUniform}; use rand::Rng; pub trait DefaultGen { @@ -28,142 +23,127 @@ macro_rules! impl_rand_default_gen ( } ); -impl_rand_default_gen!(Vector2); -impl_rand_default_gen!(Vector3); -impl_rand_default_gen!(Vector4); -impl_rand_default_gen!(Point2); -impl_rand_default_gen!(Point3); -impl_rand_default_gen!(Point4); -impl_rand_default_gen!(Matrix2); -impl_rand_default_gen!(Matrix3); -impl_rand_default_gen!(Matrix4); -impl_rand_default_gen!(Isometry2); -impl_rand_default_gen!(Isometry3); -impl_rand_default_gen!(Vector2); -impl_rand_default_gen!(Vector3); -impl_rand_default_gen!(Vector4); -impl_rand_default_gen!(Point2); -impl_rand_default_gen!(Point3); -impl_rand_default_gen!(Point4); -impl_rand_default_gen!(Matrix2); -impl_rand_default_gen!(Matrix3); -impl_rand_default_gen!(Matrix4); -impl_rand_default_gen!(Isometry2); -impl_rand_default_gen!(Isometry3); impl_rand_default_gen!(f32); impl_rand_default_gen!(f64); impl_rand_default_gen!(bool); -impl DefaultGen for Ball -where - StandardUniform: Distribution, -{ +// Implement DefaultGen for glam types manually since they don't implement Distribution +impl DefaultGen for Vec3 { + fn generate(rng: &mut R) -> Vec3 { + Vec3::new(rng.random(), rng.random(), rng.random()) + } +} + +impl DefaultGen for Pose3 { + fn generate(rng: &mut R) -> Pose3 { + let translation = Vec3::new(rng.random(), rng.random(), rng.random()); + let axis = Vec3::new( + rng.random::() - 0.5, + rng.random::() - 0.5, + rng.random::() - 0.5, + ) + .normalize_or_zero(); + let angle: f32 = rng.random::() * core::f32::consts::TAU; + let rotation = Rot3::from_axis_angle(axis, angle); + Pose3::from_parts(translation, rotation) + } +} + +impl DefaultGen for Ball { fn generate(rng: &mut R) -> Ball { Ball::new(rng.random::().abs()) } } -impl DefaultGen for Cuboid -where - StandardUniform: Distribution>, -{ +impl DefaultGen for Cuboid { fn generate(rng: &mut R) -> Cuboid { - Cuboid::new(rng.random::>().abs()) + Cuboid::new(Vec3::new( + rng.random::().abs(), + rng.random::().abs(), + rng.random::().abs(), + )) } } -impl DefaultGen for Capsule -where - StandardUniform: Distribution, -{ +impl DefaultGen for Capsule { fn generate(rng: &mut R) -> Capsule { Capsule::new( - rng.random::>(), - rng.random::>(), + Vec3::new(rng.random(), rng.random(), rng.random()), + Vec3::new(rng.random(), rng.random(), rng.random()), rng.random::().abs(), ) } } -impl DefaultGen for Cone -where - StandardUniform: Distribution, -{ +impl DefaultGen for Cone { fn generate(rng: &mut R) -> Cone { Cone::new(rng.random::().abs(), rng.random::().abs()) } } -impl DefaultGen for Cylinder -where - StandardUniform: Distribution, -{ +impl DefaultGen for Cylinder { fn generate(rng: &mut R) -> Cylinder { Cylinder::new(rng.random::().abs(), rng.random::().abs()) } } -impl DefaultGen for Segment -where - StandardUniform: Distribution>, -{ +impl DefaultGen for Segment { fn generate(rng: &mut R) -> Segment { - Segment::new(rng.random(), rng.random()) + Segment::new( + Vec3::new(rng.random(), rng.random(), rng.random()), + Vec3::new(rng.random(), rng.random(), rng.random()), + ) } } -impl DefaultGen for Triangle -where - StandardUniform: Distribution>, -{ +impl DefaultGen for Triangle { fn generate(rng: &mut R) -> Triangle { - Triangle::new(rng.random(), rng.random(), rng.random()) + Triangle::new( + Vec3::new(rng.random(), rng.random(), rng.random()), + Vec3::new(rng.random(), rng.random(), rng.random()), + Vec3::new(rng.random(), rng.random(), rng.random()), + ) } } -impl DefaultGen for ConvexPolyhedron -where - StandardUniform: Distribution, -{ +impl DefaultGen for ConvexPolyhedron { fn generate(rng: &mut R) -> ConvexPolyhedron { // It is recommended to have at most 100 points. // Otherwise, a smarter structure like the DK hierarchy would be needed. - let pts: Vec<_> = (0..100).map(|_| rng.random()).collect(); + let pts: Vec<_> = (0..100) + .map(|_| Vec3::new(rng.random(), rng.random(), rng.random())) + .collect(); ConvexPolyhedron::from_convex_hull(&pts).unwrap() } } -impl DefaultGen for Ray -where - StandardUniform: Distribution>, -{ +impl DefaultGen for Ray { fn generate(rng: &mut R) -> Ray { - // The generate ray will always point to the origin. - let shift = rng.random::>() * na::convert::<_, Real>(10.0f64); - Ray::new(Point::origin() + shift, -shift) + // The generated ray will always point to the origin. + let shift = Vec3::new( + rng.random::() * 10.0, + rng.random::() * 10.0, + rng.random::() * 10.0, + ); + Ray::new(Vector::ZERO + shift, -shift) } } -impl DefaultGen for Aabb -where - StandardUniform: Distribution>, -{ +impl DefaultGen for Aabb { fn generate(rng: &mut R) -> Aabb { // an Aabb centered at the origin. - let half_extents = rng.random::>().abs(); - Aabb::new( - Point::origin() + (-half_extents), - Point::origin() + half_extents, - ) + let half_extents = Vec3::new( + rng.random::().abs(), + rng.random::().abs(), + rng.random::().abs(), + ); + Aabb::new(Vector::ZERO + (-half_extents), Vector::ZERO + half_extents) } } -impl DefaultGen for BoundingSphere -where - StandardUniform: Distribution, -{ +impl DefaultGen for BoundingSphere { fn generate(rng: &mut R) -> BoundingSphere { // a bounding sphere centered at the origin. - BoundingSphere::new(Point::origin(), rng.random::().abs()) + BoundingSphere::new(Vector::ZERO, rng.random::().abs()) } } diff --git a/crates/parry3d/benches/common/generators.rs b/crates/parry3d/benches/common/generators.rs index 76e4e0d4..2b2e24a0 100644 --- a/crates/parry3d/benches/common/generators.rs +++ b/crates/parry3d/benches/common/generators.rs @@ -1,10 +1,16 @@ -use na::Point3; +use parry3d::math::Vec3; use parry3d::shape::TriMesh; use rand::Rng; pub fn generate_trimesh_around_origin(rng: &mut R) -> TriMesh { let pts = (0..3000) - .map(|_| rng.random::>() * 3.0) + .map(|_| { + Vec3::new( + rng.random::() * 3.0, + rng.random::() * 3.0, + rng.random::() * 3.0, + ) + }) .collect(); let indices = (0..1000).map(|i| [i * 3, i * 3 + 1, i * 3 + 2]).collect(); diff --git a/crates/parry3d/benches/common/unref.rs b/crates/parry3d/benches/common/unref.rs index 1ab474c9..06aa53d6 100644 --- a/crates/parry3d/benches/common/unref.rs +++ b/crates/parry3d/benches/common/unref.rs @@ -1,3 +1,5 @@ +use parry3d::math::{Pose3, Vec3}; + pub trait Unref { fn unref(a: Self) -> T; } @@ -23,6 +25,20 @@ impl<'a> Unref for &'a bool { } } +impl<'a> Unref for &'a Vec3 { + #[inline(always)] + fn unref(a: &Vec3) -> Vec3 { + *a + } +} + +impl<'a> Unref for &'a Pose3 { + #[inline(always)] + fn unref(a: &Pose3) -> Pose3 { + *a + } +} + impl<'a, T> Unref<&'a T> for &'a T { #[inline(always)] fn unref(a: &'a T) -> &'a T { diff --git a/crates/parry3d/benches/query/algorithm.rs b/crates/parry3d/benches/query/algorithm.rs index 521fd1b5..6c2b0f25 100644 --- a/crates/parry3d/benches/query/algorithm.rs +++ b/crates/parry3d/benches/query/algorithm.rs @@ -1,13 +1,13 @@ -use na::Point3; -use parry3d::query::gjk::{CSOPoint, VoronoiSimplex}; +use parry3d::math::Vec3; +use parry3d::query::gjk::{CsoPoint, VoronoiSimplex}; use test::Bencher; #[bench] fn bench_johnson_simplex(bh: &mut Bencher) { - let a = CSOPoint::single_point(Point3::new(-0.5f32, -0.5, -0.5)); - let b = CSOPoint::single_point(Point3::new(0.0, 0.5, 0.0)); - let c = CSOPoint::single_point(Point3::new(0.5, -0.5, -0.5)); - let d = CSOPoint::single_point(Point3::new(0.0, -0.5, -0.5)); + let a = CsoPoint::single_point(Vec3::new(-0.5f32, -0.5, -0.5)); + let b = CsoPoint::single_point(Vec3::new(0.0, 0.5, 0.0)); + let c = CsoPoint::single_point(Vec3::new(0.5, -0.5, -0.5)); + let d = CsoPoint::single_point(Vec3::new(0.0, -0.5, -0.5)); bh.iter(|| { let mut spl = VoronoiSimplex::new(); @@ -24,10 +24,10 @@ fn bench_johnson_simplex(bh: &mut Bencher) { #[bench] fn bench_johnson_simplex_tls(bh: &mut Bencher) { - let a = CSOPoint::single_point(Point3::new(-0.5f32, -0.5, -0.5)); - let b = CSOPoint::single_point(Point3::new(0.0, 0.5, 0.0)); - let c = CSOPoint::single_point(Point3::new(0.5, -0.5, -0.5)); - let d = CSOPoint::single_point(Point3::new(0.0, -0.5, -0.5)); + let a = CsoPoint::single_point(Vec3::new(-0.5f32, -0.5, -0.5)); + let b = CsoPoint::single_point(Vec3::new(0.0, 0.5, 0.0)); + let c = CsoPoint::single_point(Vec3::new(0.5, -0.5, -0.5)); + let d = CsoPoint::single_point(Vec3::new(0.0, -0.5, -0.5)); bh.iter(|| { let mut spl = VoronoiSimplex::new(); diff --git a/crates/parry3d/benches/query/contacts.rs b/crates/parry3d/benches/query/contacts.rs index 886049ce..32ac3a6b 100644 --- a/crates/parry3d/benches/query/contacts.rs +++ b/crates/parry3d/benches/query/contacts.rs @@ -1,5 +1,5 @@ use crate::common::{generate, unref}; -use na::Isometry3; +use parry3d::math::Pose3; use parry3d::query; use parry3d::shape::{Ball, Capsule, Cone, Cuboid, Cylinder}; use rand::SeedableRng; @@ -13,9 +13,9 @@ mod macros; bench_free_fn!( bench_ball_against_ball, query::contact, - pos1: Isometry3, + pos1: Pose3, b1: Ball, - pos2: Isometry3, + pos2: Pose3, b2: Ball, prediction: f32 ); @@ -23,9 +23,9 @@ bench_free_fn!( bench_free_fn!( bench_cuboid_against_cuboid, query::contact, - pos1: Isometry3, + pos1: Pose3, b1: Cuboid, - pos2: Isometry3, + pos2: Pose3, b2: Cuboid, prediction: f32 ); @@ -33,9 +33,9 @@ bench_free_fn!( bench_free_fn!( bench_capsule_against_capsule, query::contact, - pos1: Isometry3, + pos1: Pose3, b1: Capsule, - pos2: Isometry3, + pos2: Pose3, b2: Capsule, prediction: f32 ); @@ -43,9 +43,9 @@ bench_free_fn!( bench_free_fn!( bench_cone_against_cone, query::contact, - pos1: Isometry3, + pos1: Pose3, b1: Cone, - pos2: Isometry3, + pos2: Pose3, b2: Cone, prediction: f32 ); @@ -53,9 +53,9 @@ bench_free_fn!( bench_free_fn!( bench_cylinder_against_cylinder, query::contact, - pos1: Isometry3, + pos1: Pose3, b1: Cylinder, - pos2: Isometry3, + pos2: Pose3, b2: Cylinder, prediction: f32 ); diff --git a/crates/parry3d/benches/query/ray.rs b/crates/parry3d/benches/query/ray.rs index 22db6536..4a99bbc0 100644 --- a/crates/parry3d/benches/query/ray.rs +++ b/crates/parry3d/benches/query/ray.rs @@ -1,7 +1,7 @@ use crate::common::generate_trimesh_around_origin; use crate::common::{generate, unref}; -use na::Isometry3; use parry3d::bounding_volume::{Aabb, BoundingSphere}; +use parry3d::math::Pose3; use parry3d::query::{Ray, RayCast}; use parry3d::shape::{Ball, Capsule, Cone, Cuboid, Cylinder, Segment, Triangle}; use parry3d::shape::{ConvexPolyhedron, TriMesh}; @@ -18,7 +18,7 @@ bench_method!( bench_ray_against_ball, cast_ray, b: Ball, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -28,7 +28,7 @@ bench_method!( bench_ray_against_cuboid, cast_ray, c: Cuboid, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -38,7 +38,7 @@ bench_method!( bench_ray_against_capsule, cast_ray, c: Capsule, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -48,7 +48,7 @@ bench_method!( bench_ray_against_cone, cast_ray, c: Cone, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -58,7 +58,7 @@ bench_method!( bench_ray_against_cylinder, cast_ray, c: Cylinder, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -68,7 +68,7 @@ bench_method!( bench_ray_against_aabb, cast_ray, a: Aabb, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -78,7 +78,7 @@ bench_method!( bench_ray_against_bounding_sphere, cast_ray, b: BoundingSphere, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -88,7 +88,7 @@ bench_method!( bench_ray_against_ball_with_normal, cast_ray_and_get_normal, b: Ball, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -98,7 +98,7 @@ bench_method!( bench_ray_against_cuboid_with_normal, cast_ray_and_get_normal, c: Cuboid, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -108,7 +108,7 @@ bench_method!( bench_ray_against_capsule_with_normal, cast_ray_and_get_normal, c: Capsule, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -118,7 +118,7 @@ bench_method!( bench_ray_against_cone_with_normal, cast_ray_and_get_normal, c: Cone, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -128,7 +128,7 @@ bench_method!( bench_ray_against_cylinder_with_normal, cast_ray_and_get_normal, c: Cylinder, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -138,7 +138,7 @@ bench_method!( bench_ray_against_segment_with_normal, cast_ray_and_get_normal, c: Segment, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -148,7 +148,7 @@ bench_method!( bench_ray_against_triangle_with_normal, cast_ray_and_get_normal, c: Triangle, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -158,7 +158,7 @@ bench_method!( bench_ray_against_convex_with_normal, cast_ray_and_get_normal, c: ConvexPolyhedron, - pos: Isometry3, + pos: Pose3, ray: Ray, max_time_of_impact: f32, solid: bool @@ -168,7 +168,7 @@ bench_method_gen!( bench_ray_against_trimesh_with_normal, cast_ray_and_get_normal, m: TriMesh = generate_trimesh_around_origin, - pos: Isometry3 = generate, + pos: Pose3 = generate, ray: Ray = generate, max_time_of_impact: f32 = generate, solid: bool = generate diff --git a/crates/parry3d/benches/support_map/mod.rs b/crates/parry3d/benches/support_map/mod.rs index fbba0fab..7a5d29c5 100644 --- a/crates/parry3d/benches/support_map/mod.rs +++ b/crates/parry3d/benches/support_map/mod.rs @@ -1,5 +1,5 @@ use crate::common::{generate, unref}; -use na::{Isometry3, Vector3}; +use parry3d::math::{Pose3, Vec3}; use parry3d::shape::ConvexPolyhedron; use parry3d::shape::SupportMap; use parry3d::shape::{Ball, Capsule, Cone, Cuboid, Cylinder, Segment, Triangle}; @@ -15,55 +15,55 @@ bench_method!( bench_ball_support_map, support_point, c: Ball, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_cuboid_support_map, support_point, c: Cuboid, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_capsule_support_map, support_point, c: Capsule, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_cone_support_map, support_point, c: Cone, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_cylinder_support_map, support_point, c: Cylinder, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_segment_support_map, support_point, c: Segment, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_triangle_support_map, support_point, c: Triangle, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); bench_method!( bench_convex_support_map, support_point, c: ConvexPolyhedron, - m: Isometry3, - dir: Vector3 + m: Pose3, + dir: Vec3 ); diff --git a/crates/parry3d/examples/aabb3d.rs b/crates/parry3d/examples/aabb3d.rs index ef9c411c..56c71396 100644 --- a/crates/parry3d/examples/aabb3d.rs +++ b/crates/parry3d/examples/aabb3d.rs @@ -1,27 +1,24 @@ -mod common_macroquad3d; +mod utils3d; -extern crate nalgebra as na; - -use common_macroquad3d::{lissajous_3d, mquad_from_na, na_from_mquad}; -use macroquad::prelude::*; -use na::Isometry3; -use parry3d::bounding_volume::{Aabb, BoundingVolume}; +use kiss3d::prelude::*; +use parry3d::bounding_volume::BoundingVolume; +use parry3d::math::Pose; use parry3d::shape::Ball; +use utils3d::{draw_aabb3, lissajous_3d}; -#[macroquad::main("aabb3d")] +#[kiss3d::main] async fn main() { - let camera_pos = Vec3::new(8f32, 8f32, 12f32); + let mut window = Window::new("aabb3d").await; + let mut camera = OrbitCamera3d::new(Vec3::new(8.0, 8.0, 12.0), Vec3::new(0.5, 0.0, 0.5)); + let mut scene = SceneNode3d::empty(); + scene + .add_light(Light::point(100.0)) + .set_position(Vec3::new(0.0, 10.0, 10.0)); + + let start_time = web_time::Instant::now(); - loop { - let elapsed_time = get_time() as f32 * 0.7; - clear_background(BLACK); - // Initialize 3D camera. - set_camera(&Camera3D { - position: camera_pos, - up: Vec3::new(0f32, 1f32, 0f32), - target: Vec3::new(0.5f32, 0f32, 0.5f32), - ..Default::default() - }); + while window.render_3d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32() * 0.7; /* * Initialize the shapes. @@ -29,13 +26,14 @@ async fn main() { let ball1 = Ball::new(0.5); let ball2 = Ball::new(1.0); - let ball1_pos = na_from_mquad(lissajous_3d(elapsed_time)) * 4f32; - let ball2_pos = Isometry3::identity(); + let ball1_translation = lissajous_3d(elapsed_time) * 4f32; + let ball1_pos = Pose::from_translation(ball1_translation); + let ball2_pos = Pose::identity(); /* * Compute their axis-aligned bounding boxes. */ - let aabb_ball1 = ball1.aabb(&ball1_pos.into()); + let aabb_ball1 = ball1.aabb(&ball1_pos); let aabb_ball2 = ball2.aabb(&ball2_pos); // Merge the two boxes. @@ -55,30 +53,23 @@ async fn main() { assert!(bounding_aabb.contains(&aabb_ball2)); assert!(loose_aabb_ball2.contains(&aabb_ball2)); - let ball1_translation = mquad_from_na(ball1_pos.coords.into()); - draw_sphere(ball1_translation, ball1.radius, None, color); - let ball2_translation = mquad_from_na(ball2_pos.translation.vector.into()); - draw_sphere(ball2_translation, ball2.radius, None, color); + window.draw_point(ball1_pos.translation, color, ball1.radius * 50.0); + window.draw_point(ball2_pos.translation, color, ball2.radius * 50.0); - draw_aabb(aabb_ball1, color); - draw_aabb(aabb_ball2, color); - draw_aabb(bounding_aabb, YELLOW); + draw_aabb3(&mut window, aabb_ball1.mins, aabb_ball1.maxs, color); + draw_aabb3(&mut window, aabb_ball2.mins, aabb_ball2.maxs, color); + draw_aabb3(&mut window, bounding_aabb.mins, bounding_aabb.maxs, YELLOW); let color_included: Color = if loose_aabb_ball2.contains(&aabb_ball1) { BLUE } else { MAGENTA }; - draw_aabb(loose_aabb_ball2, color_included); - next_frame().await + draw_aabb3( + &mut window, + loose_aabb_ball2.mins, + loose_aabb_ball2.maxs, + color_included, + ); } } - -fn draw_aabb(aabb: Aabb, color: Color) { - let size = aabb.maxs - aabb.mins; - draw_cube_wires( - mquad_from_na(aabb.maxs - size / 2f32), - mquad_from_na(size.into()), - color, - ); -} diff --git a/crates/parry3d/examples/bounding_sphere3d.rs b/crates/parry3d/examples/bounding_sphere3d.rs index 0d2c181c..1d83bc1f 100644 --- a/crates/parry3d/examples/bounding_sphere3d.rs +++ b/crates/parry3d/examples/bounding_sphere3d.rs @@ -1,39 +1,34 @@ -mod common_macroquad3d; +mod utils3d; -extern crate nalgebra as na; - -use core::ops::Rem; - -use common_macroquad3d::{lissajous_3d, mquad_from_na, na_from_mquad}; -use macroquad::prelude::*; -use na::{Isometry3, Vector3}; +use kiss3d::prelude::*; use parry3d::bounding_volume::BoundingVolume; +use parry3d::math::Pose; use parry3d::shape::Cuboid; +use utils3d::{draw_aabb3, draw_sphere_wires, lissajous_3d}; -#[macroquad::main("bounding_sphere3d")] +#[kiss3d::main] async fn main() { - let camera_pos = Vec3::new(8f32, 8f32, 12f32); + let mut window = Window::new("bounding_sphere3d").await; + let mut camera = OrbitCamera3d::new(Vec3::new(8.0, 8.0, 12.0), Vec3::new(0.5, 0.0, 0.5)); + let mut scene = SceneNode3d::empty(); + scene + .add_light(Light::point(100.0)) + .set_position(Vec3::new(0.0, 10.0, 10.0)); + + let start_time = web_time::Instant::now(); - loop { - let elapsed_time = get_time() as f32 * 0.7; - clear_background(BLACK); - // Initialize 3D camera. - set_camera(&Camera3D { - position: camera_pos, - up: Vec3::new(0f32, 1f32, 0f32), - target: Vec3::new(0.5f32, 0f32, 0.5f32), - ..Default::default() - }); + while window.render_3d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32() * 0.7; /* * Initialize the shapes. */ - let cube1 = Cuboid::new(Vector3::repeat(0.5)); - let cube2 = Cuboid::new(Vector3::new(0.5, 1.0, 0.5)); + let cube1 = Cuboid::new(Vec3::splat(0.5)); + let cube2 = Cuboid::new(Vec3::new(0.5, 1.0, 0.5)); - let cube1_pos = na_from_mquad(lissajous_3d(elapsed_time)) * 4f32; - let cube1_pos = Isometry3::from(cube1_pos); - let cube2_pos = Isometry3::identity(); // Identity matrix. + let cube1_translation = lissajous_3d(elapsed_time) * 4f32; + let cube1_pos = Pose::from_translation(cube1_translation); + let cube2_pos = Pose::identity(); // Identity matrix. /* * Compute their bounding spheres. @@ -48,12 +43,11 @@ async fn main() { let loose_bounding_sphere_cube2 = bounding_sphere_cube2.loosened(3.0); // Intersection and inclusion tests. - let mut color = if bounding_sphere_cube1.intersects(&bounding_sphere_cube2) { + let color = if bounding_sphere_cube1.intersects(&bounding_sphere_cube2) { RED } else { GREEN }; - color.a = 1f32 * (elapsed_time.rem(1f32) - 0.5).abs() * 2f32; // Due to float imprecisions, it's dangerous to assume that both shapes will be // contained in the merged. @@ -64,35 +58,31 @@ async fn main() { //assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube2)); assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube2)); - let cube1_translation = mquad_from_na(cube1_pos.translation.vector.into()); - draw_cube_wires( - cube1_translation, - mquad_from_na(cube1.half_extents.into()) * 2f32, - WHITE, - ); - let cube2_translation = mquad_from_na(cube2_pos.translation.vector.into()); - draw_cube_wires( - cube2_translation, - mquad_from_na(cube2.half_extents.into()) * 2f32, - WHITE, - ); + // Draw cube wireframes + let mins1 = cube1_pos.translation - cube1.half_extents; + let maxs1 = cube1_pos.translation + cube1.half_extents; + draw_aabb3(&mut window, mins1, maxs1, WHITE); + + let mins2 = cube2_pos.translation - cube2.half_extents; + let maxs2 = cube2_pos.translation + cube2.half_extents; + draw_aabb3(&mut window, mins2, maxs2, WHITE); draw_sphere_wires( - mquad_from_na(bounding_sphere_cube1.center), + &mut window, + bounding_sphere_cube1.center, bounding_sphere_cube1.radius, - None, color, ); draw_sphere_wires( - mquad_from_na(bounding_sphere_cube2.center), + &mut window, + bounding_sphere_cube2.center, bounding_sphere_cube2.radius, - None, color, ); draw_sphere_wires( - mquad_from_na(bounding_bounding_sphere.center), + &mut window, + bounding_bounding_sphere.center, bounding_bounding_sphere.radius, - None, YELLOW, ); @@ -103,11 +93,10 @@ async fn main() { MAGENTA }; draw_sphere_wires( - mquad_from_na(loose_bounding_sphere_cube2.center), + &mut window, + loose_bounding_sphere_cube2.center, loose_bounding_sphere_cube2.radius, - None, color_included, ); - next_frame().await } } diff --git a/crates/parry3d/examples/common_macroquad3d.rs b/crates/parry3d/examples/common_macroquad3d.rs deleted file mode 100644 index 57c5be0b..00000000 --- a/crates/parry3d/examples/common_macroquad3d.rs +++ /dev/null @@ -1,164 +0,0 @@ -use core::f32::consts::{FRAC_PI_2, FRAC_PI_4, FRAC_PI_6}; - -use macroquad::{ - color::{Color, WHITE}, - math::{Vec2, Vec3, Vec4}, - models::{draw_line_3d, Mesh}, - ui::Vertex, -}; -use nalgebra::Point3; -use parry3d::math::Real; - -#[allow(dead_code)] -fn main() { - println!( - "This module contains helper functions to use macroquad, - isolated from the rest of the examples for the sake of simplicity." - ); -} - -/// Converts a [`nalgebra::Point3`] to a [`Vec3`], which is used by [`macroquad`] -#[allow(dead_code)] -pub fn mquad_from_na(a: Point3) -> Vec3 { - Vec3::new(a.x, a.y, a.z) -} - -/// Converts a [`Vec3`] to a [`nalgebra::Point3`], which is used by [`parry3d`] -#[allow(dead_code)] -pub fn na_from_mquad(a: Vec3) -> Point3 { - Point3::new(a.x, a.y, a.z) -} - -/// Converts a hue (from 0..=1) to rgb -#[allow(dead_code)] -pub fn hue_to_rgb(h: f32) -> (f32, f32, f32) { - let kr = (5.0 + h * 6.0).rem_euclid(6.0); - let kg = (3.0 + h * 6.0).rem_euclid(6.0); - let kb = (1.0 + h * 6.0).rem_euclid(6.0); - - let r = 1.0 - kr.min(4.0 - kr).clamp(0.0, 1.0); - let g = 1.0 - kg.min(4.0 - kg).clamp(0.0, 1.0); - let b = 1.0 - kb.min(4.0 - kb).clamp(0.0, 1.0); - - (r, g, b) -} - -/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates for time `t`. -/// -/// This uses hardcoded parameters to have an arbitrary pleasing trajectory. -#[allow(dead_code)] -pub fn lissajous_3d(t: f32) -> Vec3 { - // Some hardcoded parameters to have a pleasing lissajous trajectory. - lissajous_3d_with_params(t, 3.0, 2.0, 1.0, FRAC_PI_2, FRAC_PI_4, FRAC_PI_6) -} - -/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates. -#[allow(dead_code)] -pub fn lissajous_3d_with_params( - t: f32, - a: f32, - b: f32, - c: f32, - delta_x: f32, - delta_y: f32, - delta_z: f32, -) -> Vec3 { - let x = (a * t + delta_x).sin(); - let y = (b * t + delta_y).sin(); - let z = (c * t + delta_z).sin(); - Vec3::new(x, y, z) * 0.75f32 -} - -/// Uses [`macroquad`] to display the line passed as parameter. -#[allow(dead_code)] -pub fn draw_polyline(polyline: Vec<(Vec3, Vec3)>, color: Color) { - for line in polyline { - let a = line.0; - let b = line.1; - draw_line_3d(a, b, color); - } -} - -/// Draws a text in the top left corner of the screen. -/// -/// This uses a hardcoded position, size, color. -#[allow(dead_code)] -pub fn easy_draw_text(text: &str) { - macroquad::text::draw_text(text, 10.0, 48.0 + 18.0, 30.0, WHITE); -} - -/// Create a usable mesh for [`macroquad`]. -/// -/// This duplicates the trimesh vertices, computes their normals, -/// and bakes light into its vertices colors using [`mquad_compute_normals_and_bake_light`]. -#[allow(dead_code)] -pub fn mquad_mesh_from_points( - trimesh: &(Vec>, Vec<[u32; 3]>), - light_pos: Vec3, - color: Color, -) -> Mesh { - let (points, indices) = trimesh; - // Transform the parry mesh into a mquad Mesh - let (mquad_points, mquad_indices) = ( - points - .iter() - .map(|p| Vertex { - position: mquad_from_na(*p), - uv: Vec2::new(p.x, p.y), - color: color.into(), - normal: Vec4::ZERO, - }) - .collect(), - indices.iter().flatten().map(|v| *v as u16).collect(), - ); - - // Macroquad does support adding normals to vertices, but we´d have to provide shaders for them. - // so we're baking a color into these vertices. - // See https://github.com/not-fl3/macroquad/issues/321. - - // Compute the normal of each vertex, making them unique - let vertices: Vec = - mquad_compute_normals_and_bake_light(&mquad_points, &mquad_indices, light_pos); - // Regenerate the index for each vertex. - let indices: Vec = (0..vertices.len() * 3).map(|i| i as u16).collect(); - let mesh = Mesh { - vertices, - indices, - texture: None, - }; - mesh -} - -/// Bakes light into vertices, using an hardcoded light strength. -#[allow(dead_code)] -pub fn mquad_compute_normals_and_bake_light( - points: &Vec, - indices: &Vec, - light_pos: Vec3, -) -> Vec { - let mut vertices: Vec = Vec::::new(); - for indices in indices.chunks(3) { - let v0 = &points[indices[0] as usize]; - let v1 = &points[indices[1] as usize]; - let v2 = &points[indices[2] as usize]; - let normal = (v0.position - v2.position) - .cross(v1.position - v2.position) - .normalize(); - let brightness_mod = 0.4 + (0.6 / 2.) * (normal.dot(light_pos) + 1.); - - for &i in indices.iter() { - let mut color = points[i as usize].color; - color[0] = (color[0] as f32 * brightness_mod) as u8; - color[1] = (color[1] as f32 * brightness_mod) as u8; - color[2] = (color[2] as f32 * brightness_mod) as u8; - - vertices.push(Vertex { - position: points[i as usize].position, - uv: Vec2::ZERO, - color: color, - normal: Vec4::ZERO, - }); - } - } - vertices -} diff --git a/crates/parry3d/examples/contact_query3d.rs b/crates/parry3d/examples/contact_query3d.rs index b48c5cfd..14331b1c 100644 --- a/crates/parry3d/examples/contact_query3d.rs +++ b/crates/parry3d/examples/contact_query3d.rs @@ -1,18 +1,16 @@ -extern crate nalgebra as na; - -use na::{Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query; use parry3d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); let ball = Ball::new(1.0); let prediction = 1.0; - let cuboid_pos = Isometry3::identity(); - let ball_pos_penetrating = Isometry3::translation(1.0, 1.0, 1.0); - let ball_pos_in_prediction = Isometry3::translation(2.0, 2.0, 2.0); - let ball_pos_too_far = Isometry3::translation(3.0, 3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_penetrating = Pose::translation(1.0, 1.0, 1.0); + let ball_pos_in_prediction = Pose::translation(2.0, 2.0, 2.0); + let ball_pos_too_far = Pose::translation(3.0, 3.0, 3.0); let ctct_penetrating = query::contact( &ball_pos_penetrating, diff --git a/crates/parry3d/examples/convex3d.rs b/crates/parry3d/examples/convex3d.rs index 0531ef01..6f38e3a9 100644 --- a/crates/parry3d/examples/convex3d.rs +++ b/crates/parry3d/examples/convex3d.rs @@ -1,17 +1,15 @@ -extern crate nalgebra as na; - -use na::Point3; +use parry3d::math::Vector; use parry3d::shape::ConvexPolyhedron; fn main() { let points = [ - Point3::new(0.0f32, 0.0, 1.0), - Point3::new(0.0, 0.0, -1.0), - Point3::new(0.0, 1.0, 0.0), - Point3::new(0.0, -1.0, 0.0), - Point3::new(1.0, 0.0, 0.0), - Point3::new(-1.0, 0.0, 0.0), - Point3::origin(), + Vector::new(0.0f32, 0.0, 1.0), + Vector::new(0.0, 0.0, -1.0), + Vector::new(0.0, 1.0, 0.0), + Vector::new(0.0, -1.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(-1.0, 0.0, 0.0), + Vector::ZERO, ]; let convex = ConvexPolyhedron::from_convex_hull(&points).expect("Invalid convex shape."); diff --git a/crates/parry3d/examples/convex_decomposition.rs b/crates/parry3d/examples/convex_decomposition.rs index 65daafab..924a6a3b 100644 --- a/crates/parry3d/examples/convex_decomposition.rs +++ b/crates/parry3d/examples/convex_decomposition.rs @@ -1,32 +1,36 @@ -mod common_macroquad3d; +mod utils3d; -extern crate nalgebra as na; - -use common_macroquad3d::{easy_draw_text, hue_to_rgb, mquad_mesh_from_points}; -use macroquad::prelude::*; -use na::Point3; -use obj::{Obj, ObjData}; +use kiss3d::prelude::*; use parry3d::{ math::Real, shape::{SharedShape, TriMesh, TriMeshFlags}, }; +use utils3d::{create_mesh_from_trimesh, draw_text, hue_to_rgb}; -#[macroquad::main("convex_decomposition")] +#[kiss3d::main] async fn main() { + let mut window = Window::new("convex_decomposition").await; + let mut scene = SceneNode3d::empty(); + scene + .add_light(Light::point(100.0)) + .set_position(Vec3::new(1.0, 3.0, 3.0)); + + let font = Font::default(); + /* * Initialize the shapes. */ - let Obj { - data: ObjData { + let ::obj::Obj { + data: ::obj::ObjData { position, objects, .. }, .. - } = Obj::load("assets/tests/low_poly_bunny.obj").unwrap(); + } = ::obj::Obj::load("assets/tests/low_poly_bunny.obj").unwrap(); let bunny_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vec3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -36,67 +40,45 @@ async fn main() { TriMeshFlags::all(), ) .unwrap(); - clear_background(BLACK); - easy_draw_text("Please wait while convex decomposition is being computed..."); - #[cfg(debug_assertions)] + // Show loading message + draw_text( + &mut window, + &font, + "Please wait while convex decomposition is being computed...", + ); + + // Dummy render to display the message { - macroquad::text::draw_text( - "Running in debug mode is significantly slower than with `--release`.", - 10.0, - 48.0 + 48.0, - 26.0, - RED, - ); + let mut camera = OrbitCamera3d::new(Vec3::new(5.5, 3.0, 5.5), Vec3::new(0.0, 1.0, 0.0)); + window.render_3d(&mut scene, &mut camera).await; } - next_frame().await; + let mesh_vertices = bunny_mesh.vertices(); let mesh_indices = bunny_mesh.indices(); let convex_mesh = SharedShape::convex_decomposition(&mesh_vertices, &mesh_indices); let trimesh_convex_compound = convex_mesh.as_compound().unwrap(); let shapes_count = trimesh_convex_compound.shapes().len() as u32; - let mut meshes = Vec::new(); + + // Add all convex parts to the scene for (i, s) in trimesh_convex_compound.shapes().iter().enumerate() { - let trimesh_convex = s.1.as_convex_polyhedron().unwrap().to_trimesh(); + let (vertices, indices) = s.1.as_convex_polyhedron().unwrap().to_trimesh(); + let mesh = create_mesh_from_trimesh(vertices, indices); - /* - * Make render meshes out of the shapes. - */ let (r, g, b) = hue_to_rgb(i as f32 / 6 as f32); - let mesh = mquad_mesh_from_points( - &trimesh_convex, - Vec3::new(1f32, 3f32, 3f32), - Color::from_rgba( - (r as f32 * 255.0) as u8, - (g as f32 * 255.0) as u8, - (b as f32 * 255.0) as u8, - 255, - ), - ); - meshes.push(mesh); + scene + .add_mesh(mesh, Vec3::splat(1.0)) + .set_color(Color::new(r, g, b, 1.0)); } - loop { - clear_background(BLACK); - let elapsed_time = get_time() as f32; - let camera_pos = Vec3::new( - 5.5f32 * elapsed_time.sin(), - 3f32, - 5.5f32 * elapsed_time.cos(), + let mut camera = OrbitCamera3d::new(Vec3::new(5.5, 3.0, 5.5), Vec3::new(0.0, 1.0, 0.0)); + + while window.render_3d(&mut scene, &mut camera).await { + draw_text( + &mut window, + &font, + &format!("Number of shapes: {}", shapes_count), ); - // Initialize 3D camera. - set_camera(&Camera3D { - position: camera_pos, - up: Vec3::new(0f32, 1f32, 0f32), - target: Vec3::new(0f32, 1f32, 0f32), - ..Default::default() - }); - for mesh in &meshes { - draw_mesh(mesh); - } - set_default_camera(); - easy_draw_text(&format!("Number of shapes: {}", shapes_count)); - next_frame().await } } diff --git a/crates/parry3d/examples/convex_hull3d.rs b/crates/parry3d/examples/convex_hull3d.rs index fff14fa1..11fa4575 100644 --- a/crates/parry3d/examples/convex_hull3d.rs +++ b/crates/parry3d/examples/convex_hull3d.rs @@ -1,27 +1,32 @@ -mod common_macroquad3d; +mod utils3d; use core::f32::consts::{FRAC_PI_2, FRAC_PI_4, FRAC_PI_6}; -use common_macroquad3d::{ - lissajous_3d_with_params, mquad_from_na, mquad_mesh_from_points, na_from_mquad, -}; -use macroquad::prelude::*; -use nalgebra::Point3; +use kiss3d::prelude::*; use parry3d::transformation; +use utils3d::{create_mesh_from_trimesh, lissajous_3d_with_params}; -#[macroquad::main("convex_hull3d")] +#[kiss3d::main] async fn main() { + let mut window = Window::new("convex_hull3d").await; + let mut camera = OrbitCamera3d::new(Vec3::new(8.0, 8.0, 8.0), Vec3::new(0.5, 0.0, 0.5)); + let mut scene = SceneNode3d::empty(); + scene + .add_light(Light::point(100.0)) + .set_position(Vec3::new(5.0, 10.0, 3.0)); + let count = 9; - let mut pts = vec![Point3::default(); count]; + let mut pts = vec![Vec3::ZERO; count]; + + let start_time = web_time::Instant::now(); + let mut mesh_node: Option = None; - let camera_pos = Vec3::new(8.0, 8.0, 8.0); - loop { - let elapsed_time = get_time() as f32; + while window.render_3d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32(); let elapsed_time_slow = elapsed_time * 0.2; - clear_background(BLACK); for (i, pt) in pts.iter_mut().enumerate() { - *pt = na_from_mquad(lissajous_3d_with_params( + *pt = lissajous_3d_with_params( (i * i) as f32 + elapsed_time_slow, 2.0 + i as f32 / 3.0, 1f32 + (i as f32).sin() * 0.2, @@ -29,24 +34,28 @@ async fn main() { (elapsed_time_slow as f32 + i as f32).cos() * 0.1 + FRAC_PI_2, FRAC_PI_4, FRAC_PI_6, - )) * 5f32; - draw_sphere(mquad_from_na(*pt), 0.1f32, None, RED); + ) * 5f32; + window.draw_point(*pt, RED, 20.0); } - // Initialize 3D camera. - set_camera(&Camera3D { - position: camera_pos, - up: Vec3::new(0f32, 1f32, 0f32), - target: Vec3::new(0.5f32, 0f32, 0.5f32), - ..Default::default() - }); + /* * * Compute the convex hull. * */ let convex_hull = transformation::convex_hull(&pts); - let mesh = mquad_mesh_from_points(&convex_hull, Vec3::new(5.0, 10.0, 3.0), DARKGRAY); - draw_mesh(&mesh); - next_frame().await + + // Remove previous mesh if exists + if let Some(mut node) = mesh_node.take() { + node.remove(); + } + + // Create new mesh from convex hull + let mesh = create_mesh_from_trimesh(convex_hull.0, convex_hull.1); + mesh_node = Some( + scene + .add_mesh(mesh, Vec3::splat(1.0)) + .set_color(LIGHT_GREEN), + ); } } diff --git a/crates/parry3d/examples/convex_try_new3d.rs b/crates/parry3d/examples/convex_try_new3d.rs index f121e7b7..4423b71a 100644 --- a/crates/parry3d/examples/convex_try_new3d.rs +++ b/crates/parry3d/examples/convex_try_new3d.rs @@ -1,16 +1,14 @@ -extern crate nalgebra as na; - -use na::Point3; +use parry3d::math::Vector; use parry3d::shape::ConvexPolyhedron; fn main() { let points = vec![ - Point3::new(0.0f32, 0.0, 1.0), - Point3::new(0.0, 0.0, -1.0), - Point3::new(0.0, 1.0, 0.0), - Point3::new(0.0, -1.0, 0.0), - Point3::new(1.0, 0.0, 0.0), - Point3::new(-1.0, 0.0, 0.0), + Vector::new(0.0f32, 0.0, 1.0), + Vector::new(0.0, 0.0, -1.0), + Vector::new(0.0, 1.0, 0.0), + Vector::new(0.0, -1.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(-1.0, 0.0, 0.0), ]; let indices = vec![ diff --git a/crates/parry3d/examples/cuboid3d.rs b/crates/parry3d/examples/cuboid3d.rs index 128249cd..be1c88fd 100644 --- a/crates/parry3d/examples/cuboid3d.rs +++ b/crates/parry3d/examples/cuboid3d.rs @@ -1,10 +1,8 @@ -extern crate nalgebra as na; - -use na::Vector3; +use parry3d::math::Vector; use parry3d::shape::Cuboid; fn main() { - let cuboid = Cuboid::new(Vector3::new(2.0f32, 1.0, 3.0)); + let cuboid = Cuboid::new(Vector::new(2.0f32, 1.0, 3.0)); assert!(cuboid.half_extents.x == 2.0); assert!(cuboid.half_extents.y == 1.0); diff --git a/crates/parry3d/examples/distance_query3d.rs b/crates/parry3d/examples/distance_query3d.rs index 186b558e..1bf97289 100644 --- a/crates/parry3d/examples/distance_query3d.rs +++ b/crates/parry3d/examples/distance_query3d.rs @@ -1,18 +1,17 @@ #[macro_use] extern crate approx; // for relative_eq! -extern crate nalgebra as na; -use na::{Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query; use parry3d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry3::identity(); - let ball_pos_intersecting = Isometry3::translation(0.0, 1.0, 0.0); - let ball_pos_disjoint = Isometry3::translation(0.0, 3.0, 0.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(0.0, 1.0, 0.0); + let ball_pos_disjoint = Pose::translation(0.0, 3.0, 0.0); let dist_intersecting = query::distance(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap(); diff --git a/crates/parry3d/examples/getting_started.rs b/crates/parry3d/examples/getting_started.rs index 0776f4c2..29887784 100644 --- a/crates/parry3d/examples/getting_started.rs +++ b/crates/parry3d/examples/getting_started.rs @@ -1,12 +1,10 @@ -extern crate nalgebra as na; - -use na::{Isometry3, Point3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::{Ray, RayCast}; use parry3d::shape::Cuboid; fn main() { - let cube = Cuboid::new(Vector3::new(1.0f32, 1.0, 1.0)); - let ray = Ray::new(Point3::new(0.0f32, 0.0, -1.0), Vector3::z()); + let cube = Cuboid::new(Vector::new(1.0f32, 1.0, 1.0)); + let ray = Ray::new(Vector::new(0.0f32, 0.0, -1.0), Vector::Z); - assert!(cube.intersects_ray(&Isometry3::identity(), &ray, f32::MAX)); + assert!(cube.intersects_ray(&Pose::identity(), &ray, f32::MAX)); } diff --git a/crates/parry3d/examples/mesh3d.rs b/crates/parry3d/examples/mesh3d.rs index aeb26eb0..cfad8afa 100644 --- a/crates/parry3d/examples/mesh3d.rs +++ b/crates/parry3d/examples/mesh3d.rs @@ -1,14 +1,12 @@ -extern crate nalgebra as na; - -use na::Point3; +use parry3d::math::Vector; use parry3d::shape::TriMesh; fn main() { let points = vec![ - Point3::new(0.0, 1.0, 0.0), - Point3::new(-1.0, -0.5, 0.0), - Point3::new(0.0, -0.5, -1.0), - Point3::new(1.0, -0.5, 0.0), + Vector::new(0.0, 1.0, 0.0), + Vector::new(-1.0, -0.5, 0.0), + Vector::new(0.0, -0.5, -1.0), + Vector::new(1.0, -0.5, 0.0), ]; let indices = vec![[0u32, 1, 2], [0, 2, 3], [0, 3, 1]]; diff --git a/crates/parry3d/examples/plane3d.rs b/crates/parry3d/examples/plane3d.rs index bf174103..3fc409c7 100644 --- a/crates/parry3d/examples/plane3d.rs +++ b/crates/parry3d/examples/plane3d.rs @@ -1,10 +1,8 @@ -extern crate nalgebra as na; - -use na::Vector3; +use parry3d::math::Vector; use parry3d::shape::HalfSpace; fn main() { - let halfspace = HalfSpace::new(Vector3::::y_axis()); + let halfspace = HalfSpace::new(Vector::Y); assert!(halfspace.normal.x == 0.0); assert!(halfspace.normal.y == 1.0); diff --git a/crates/parry3d/examples/plane_intersection.rs b/crates/parry3d/examples/plane_intersection.rs index c7420787..d5961bce 100644 --- a/crates/parry3d/examples/plane_intersection.rs +++ b/crates/parry3d/examples/plane_intersection.rs @@ -1,25 +1,34 @@ -use macroquad::prelude::*; -use nalgebra::{UnitVector3, Vector3}; +mod utils3d; + +use kiss3d::prelude::*; use parry3d::query::IntersectResult; use parry3d::shape::{Cuboid, TriMesh}; +use utils3d::{create_mesh_from_trimesh, draw_polyline, draw_text}; -mod common_macroquad3d; -use common_macroquad3d::*; - -#[macroquad::main("plane_intersection")] +#[kiss3d::main] async fn main() { - let trimesh = Cuboid::new(Vector3::repeat(1.0)).to_trimesh(); + let mut window = Window::new("plane_intersection").await; + let mut camera = OrbitCamera3d::new(Vec3::new(-1.5, 2.5, -3.0), Vec3::new(0.5, 0.0, 0.5)); + let mut scene = SceneNode3d::empty(); + scene + .add_light(Light::point(100.0)) + .set_position(Vec3::new(-1.0, 3.5, -3.0)); - let light_pos = Vec3::new(-1f32, 3.5f32, -3f32); - let camera_pos = Vec3::new(-1.5f32, 2.5f32, -3f32); + let font = Font::default(); - let mesh = mquad_mesh_from_points(&trimesh, light_pos, DARKGRAY); - let trimesh = TriMesh::new(trimesh.0, trimesh.1).unwrap(); + let (points, indices) = Cuboid::new(Vec3::splat(1.0)).to_trimesh(); + let trimesh = TriMesh::new(points.clone(), indices.clone()).unwrap(); - for _ in 1.. { - clear_background(BLACK); + // Add mesh to scene + let mesh = create_mesh_from_trimesh(points, indices); + scene + .add_mesh(mesh, Vec3::splat(1.0)) + .set_color(Color::new(0.3, 0.3, 0.3, 1.0)); - let elapsed_time = get_time(); + let start_time = web_time::Instant::now(); + + while window.render_3d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f64(); // Animated rotation for the intersection plane. let bias = -1.2 * (elapsed_time as f32 / 3f32).sin(); @@ -27,29 +36,21 @@ async fn main() { let up_plane_vector = rotation * Vec3::Y; // Get the intersection polyline. - let intersection_result = trimesh.intersection_with_local_plane( - &UnitVector3::new_normalize(Vector3::new( - up_plane_vector.x, - up_plane_vector.y, - up_plane_vector.z, - )), - bias, - 0.0005, - ); + let intersection_result = + trimesh.intersection_with_local_plane(up_plane_vector.normalize(), bias, 0.0005); - // Initialize 3D camera. - set_camera(&Camera3D { - position: camera_pos, - up: Vec3::new(0f32, 1f32, 0f32), - target: Vec3::new(0.5f32, 0f32, 0.5f32), - ..Default::default() - }); - - // Draw involved shapes. + // Draw the plane normal let plane_center = up_plane_vector * bias; - draw_line_3d(plane_center, plane_center + up_plane_vector, GREEN); - draw_mesh(&mesh); - draw_grid_ex(10, 0.333, BLUE, RED, plane_center, rotation); + window.draw_line( + plane_center, + plane_center + up_plane_vector, + GREEN, + 4.0, + false, + ); + + // Draw a grid to represent the plane + draw_grid(&mut window, plane_center, rotation, 10, 0.333); /* * @@ -59,24 +60,46 @@ async fn main() { match intersection_result { IntersectResult::Intersect(points) => { draw_polyline( - points - .segments() - .map(|s| (mquad_from_na(s.a), mquad_from_na(s.b))) - .collect(), + &mut window, + points.segments().map(|s| (s.a, s.b)).collect(), Color::new(0f32, 1f32, 0f32, 1f32), ); - set_default_camera(); - easy_draw_text("Intersection found!"); + draw_text(&mut window, &font, "Intersection found!"); } IntersectResult::Negative => { - set_default_camera(); - easy_draw_text("No intersection found, the shape is below the plane."); + draw_text( + &mut window, + &font, + "No intersection found, the shape is below the plane.", + ); } IntersectResult::Positive => { - set_default_camera(); - easy_draw_text("No intersection found, the shape is above the plane."); + draw_text( + &mut window, + &font, + "No intersection found, the shape is above the plane.", + ); } } - next_frame().await + } +} + +fn draw_grid(window: &mut Window, center: Vec3, rotation: Quat, subdivisions: i32, spacing: f32) { + let half_size = subdivisions as f32 * spacing / 2.0; + + for i in 0..=subdivisions { + let offset = -half_size + i as f32 * spacing; + + // Lines along X axis + let start = rotation * Vec3::new(-half_size, 0.0, offset) + center; + let end = rotation * Vec3::new(half_size, 0.0, offset) + center; + let color = if i == subdivisions / 2 { RED } else { BLUE }; + window.draw_line(start, end, color, 2.0, false); + + // Lines along Z axis + let start = rotation * Vec3::new(offset, 0.0, -half_size) + center; + let end = rotation * Vec3::new(offset, 0.0, half_size) + center; + let color = if i == subdivisions / 2 { RED } else { BLUE }; + window.draw_line(start, end, color, 2.0, false); } } diff --git a/crates/parry3d/examples/polyline3d.rs b/crates/parry3d/examples/polyline3d.rs index d118a7d2..0cc4d37e 100644 --- a/crates/parry3d/examples/polyline3d.rs +++ b/crates/parry3d/examples/polyline3d.rs @@ -1,15 +1,13 @@ -extern crate nalgebra as na; - -use na::Point3; +use parry3d::math::Vector; use parry3d::shape::Polyline; fn main() { let points = vec![ - Point3::new(0.0, 1.0, 0.0), - Point3::new(-1.0, -1.0, 1.0), - Point3::new(0.0, -0.5, 0.0), - Point3::new(1.0, -1.0, -1.0), - Point3::new(0.0, 1.0, 0.0), // This forms a loop. + Vector::new(0.0, 1.0, 0.0), + Vector::new(-1.0, -1.0, 1.0), + Vector::new(0.0, -0.5, 0.0), + Vector::new(1.0, -1.0, -1.0), + Vector::new(0.0, 1.0, 0.0), // This forms a loop. ]; // Build the polyline. diff --git a/crates/parry3d/examples/project_point3d.rs b/crates/parry3d/examples/project_point3d.rs index c629030b..e6681164 100644 --- a/crates/parry3d/examples/project_point3d.rs +++ b/crates/parry3d/examples/project_point3d.rs @@ -1,43 +1,37 @@ -use macroquad::prelude::*; -use nalgebra::Vector3; +mod utils3d; + +use kiss3d::prelude::*; use parry3d::query::PointQuery; use parry3d::shape::{Cuboid, TriMesh, TriMeshFlags}; +use utils3d::{create_mesh_from_trimesh, lissajous_3d}; -mod common_macroquad3d; -use common_macroquad3d::*; - -#[macroquad::main("project_point3d")] +#[kiss3d::main] async fn main() { - let trimesh = Cuboid::new(Vector3::new(0.2, 0.5, 1.0)).to_trimesh(); - - let mesh = mquad_mesh_from_points( - &trimesh, - Vec3::new(1f32, 3f32, 3f32), - Color::from_rgba(200, 200, 200, 150), - ); - let (points, indices) = trimesh; - let trimesh = TriMesh::with_flags(points, indices, TriMeshFlags::ORIENTED).unwrap(); - for _i in 1.. { - clear_background(BLACK); - - let elapsed_time = get_time() as f32; + let mut window = Window::new("project_point3d").await; + let mut scene = SceneNode3d::empty(); + scene + .add_light(Light::point(100.0)) + .set_position(Vec3::new(1.0, 3.0, 3.0)); + + let (points, indices) = Cuboid::new(Vec3::new(0.2, 0.5, 1.0)).to_trimesh(); + let trimesh = + TriMesh::with_flags(points.clone(), indices.clone(), TriMeshFlags::ORIENTED).unwrap(); + + // Add the mesh to the scene + let mesh = create_mesh_from_trimesh(points, indices); + scene + .add_mesh(mesh, Vec3::splat(1.0)) + .set_color(Color::new(0.8, 0.8, 0.8, 0.6)); + + let start_time = web_time::Instant::now(); + let mut camera = OrbitCamera3d::new(Vec3::new(3.0, 1.0, 3.0), Vec3::ZERO); + + while window.render_3d(&mut scene, &mut camera).await { + let elapsed_time = start_time.elapsed().as_secs_f32(); let slow_elapsed_time = elapsed_time / 3.0; let point_to_project = lissajous_3d(slow_elapsed_time); - let projected_point = trimesh.project_local_point(&na_from_mquad(point_to_project), true); - - let slow_elapsed_time = slow_elapsed_time * 0.7; - // Setup 3D camera. - set_camera(&Camera3D { - position: Vec3::new( - slow_elapsed_time.sin() * 3.0, - slow_elapsed_time.sin(), - slow_elapsed_time.cos() * 3.0, - ), - up: Vec3::Y, - target: Vec3::ZERO, - ..Default::default() - }); + let projected_point = trimesh.project_local_point(point_to_project, true); /* * @@ -50,37 +44,18 @@ async fn main() { YELLOW }; - draw_line_3d( - point_to_project, - mquad_from_na(projected_point.point), - color, - ); - draw_sphere(point_to_project, 0.1, None, color); - - draw_line_3d( - point_to_project, - mquad_from_na(projected_point.point), - color, - ); + window.draw_line(point_to_project, projected_point.point, color, 2.0, false); + window.draw_point(point_to_project, color, 10.0); // fixed point inside the shape let point_to_project = Vec3::ZERO; - let projected_point = trimesh.project_local_point(&na_from_mquad(point_to_project), true); + let projected_point = trimesh.project_local_point(point_to_project, true); let color = if projected_point.is_inside { RED } else { YELLOW }; - draw_sphere(point_to_project, 0.1, None, color); - - draw_line_3d( - point_to_project, - mquad_from_na(projected_point.point), - color, - ); - // Mesh is rendered in the back, so we can see the other graphics elements - draw_mesh(&mesh); - - next_frame().await + window.draw_point(point_to_project, color, 10.0); + window.draw_line(point_to_project, projected_point.point, color, 2.0, false); } } diff --git a/crates/parry3d/examples/proximity_query3d.rs b/crates/parry3d/examples/proximity_query3d.rs index 9705d4cf..235c96c5 100644 --- a/crates/parry3d/examples/proximity_query3d.rs +++ b/crates/parry3d/examples/proximity_query3d.rs @@ -1,15 +1,13 @@ -extern crate nalgebra as na; - -use na::{Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query; use parry3d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry3::identity(); - let ball_pos_intersecting = Isometry3::translation(1.0, 1.0, 1.0); - let ball_pos_disjoint = Isometry3::translation(3.0, 3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(1.0, 1.0, 1.0); + let ball_pos_disjoint = Pose::translation(3.0, 3.0, 3.0); let intersecting = query::intersection_test(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap(); diff --git a/crates/parry3d/examples/solid_point_query3d.rs b/crates/parry3d/examples/solid_point_query3d.rs index 4d0efed4..cb929289 100644 --- a/crates/parry3d/examples/solid_point_query3d.rs +++ b/crates/parry3d/examples/solid_point_query3d.rs @@ -1,33 +1,31 @@ -extern crate nalgebra as na; - -use na::{Isometry3, Point3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::PointQuery; use parry3d::shape::Cuboid; fn main() { - let cuboid = Cuboid::new(Vector3::new(1.0, 2.0, 2.0)); - let pt_inside = Point3::origin(); - let pt_outside = Point3::new(2.0, 2.0, 2.0); + let cuboid = Cuboid::new(Vector::new(1.0, 2.0, 2.0)); + let pt_inside = Vector::ZERO; + let pt_outside = Vector::new(2.0, 2.0, 2.0); // Solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_inside, true), + cuboid.distance_to_point(&Pose::identity(), pt_inside, true), 0.0 ); // Non-solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_inside, false), + cuboid.distance_to_point(&Pose::identity(), pt_inside, false), -1.0 ); // The other point is outside of the cuboid so the `solid` flag has no effect. assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_outside, false), + cuboid.distance_to_point(&Pose::identity(), pt_outside, false), 1.0 ); assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_outside, true), + cuboid.distance_to_point(&Pose::identity(), pt_outside, true), 1.0 ); } diff --git a/crates/parry3d/examples/solid_ray_cast3d.rs b/crates/parry3d/examples/solid_ray_cast3d.rs index 0dc9afab..ba0ced8e 100644 --- a/crates/parry3d/examples/solid_ray_cast3d.rs +++ b/crates/parry3d/examples/solid_ray_cast3d.rs @@ -1,18 +1,16 @@ -extern crate nalgebra as na; - -use na::{Isometry3, Point3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::{Ray, RayCast}; use parry3d::shape::Cuboid; fn main() { - let cuboid = Cuboid::new(Vector3::new(1.0, 2.0, 1.0)); - let ray_inside = Ray::new(Point3::origin(), Vector3::y()); - let ray_miss = Ray::new(Point3::new(2.0, 2.0, 2.0), Vector3::new(1.0, 1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 2.0, 1.0)); + let ray_inside = Ray::new(Vector::ZERO, Vector::Y); + let ray_miss = Ray::new(Vector::new(2.0, 2.0, 2.0), Vector::new(1.0, 1.0, 1.0)); // Solid cast. assert_eq!( cuboid - .cast_ray(&Isometry3::identity(), &ray_inside, f32::MAX, true) + .cast_ray(&Pose::identity(), &ray_inside, f32::MAX, true) .unwrap(), 0.0 ); @@ -20,16 +18,16 @@ fn main() { // Non-solid cast. assert_eq!( cuboid - .cast_ray(&Isometry3::identity(), &ray_inside, f32::MAX, false) + .cast_ray(&Pose::identity(), &ray_inside, f32::MAX, false) .unwrap(), 2.0 ); // The other ray does not intersect this shape. assert!(cuboid - .cast_ray(&Isometry3::identity(), &ray_miss, f32::MAX, false) + .cast_ray(&Pose::identity(), &ray_miss, f32::MAX, false) .is_none()); assert!(cuboid - .cast_ray(&Isometry3::identity(), &ray_miss, f32::MAX, true) + .cast_ray(&Pose::identity(), &ray_miss, f32::MAX, true) .is_none()); } diff --git a/crates/parry3d/examples/time_of_impact_query3d.rs b/crates/parry3d/examples/time_of_impact_query3d.rs index 20028785..f5d99f4b 100644 --- a/crates/parry3d/examples/time_of_impact_query3d.rs +++ b/crates/parry3d/examples/time_of_impact_query3d.rs @@ -1,50 +1,48 @@ -extern crate nalgebra as na; - -use na::{Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::{Ball, Cuboid}; fn main() { - let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry3::identity(); - let ball_pos_intersecting = Isometry3::translation(1.0, 1.0, 1.0); - let ball_pos_will_touch = Isometry3::translation(2.0, 2.0, 2.0); - let ball_pos_wont_touch = Isometry3::translation(3.0, 3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(1.0, 1.0, 1.0); + let ball_pos_will_touch = Pose::translation(2.0, 2.0, 2.0); + let ball_pos_wont_touch = Pose::translation(3.0, 3.0, 3.0); - let cuboid_vel1 = Vector3::new(-1.0, 1.0, 1.0); - let cuboid_vel2 = Vector3::new(1.0, 1.0, 1.0); + let cuboid_vel1 = Vector::new(-1.0, 1.0, 1.0); + let cuboid_vel2 = Vector::new(1.0, 1.0, 1.0); - let ball_vel1 = Vector3::new(2.0, 2.0, 2.0); - let ball_vel2 = Vector3::new(-0.5, -0.5, -0.5); + let ball_vel1 = Vector::new(2.0, 2.0, 2.0); + let ball_vel2 = Vector::new(-0.5, -0.5, -0.5); let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &cuboid_vel1, + cuboid_vel1, &cuboid, ShapeCastOptions::default(), ) .unwrap(); let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, - &ball_vel2, + ball_vel2, &ball, &cuboid_pos, - &cuboid_vel2, + cuboid_vel2, &cuboid, ShapeCastOptions::default(), ) .unwrap(); let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &cuboid_vel1, + cuboid_vel1, &cuboid, ShapeCastOptions::default(), ) diff --git a/crates/parry3d/examples/utils3d.rs b/crates/parry3d/examples/utils3d.rs new file mode 100644 index 00000000..0ec78d58 --- /dev/null +++ b/crates/parry3d/examples/utils3d.rs @@ -0,0 +1,206 @@ +use core::f32::consts::{FRAC_PI_2, FRAC_PI_4, FRAC_PI_6}; +use std::sync::Arc; + +use kiss3d::prelude::*; +use kiss3d::procedural::{IndexBuffer, RenderMesh}; + +#[allow(dead_code)] +fn main() { + println!( + "This module contains helper functions to use kiss3d, + isolated from the rest of the examples for the sake of simplicity." + ); +} + +/// Converts a hue (from 0..=1) to rgb +#[allow(dead_code)] +pub fn hue_to_rgb(h: f32) -> (f32, f32, f32) { + let kr = (5.0 + h * 6.0).rem_euclid(6.0); + let kg = (3.0 + h * 6.0).rem_euclid(6.0); + let kb = (1.0 + h * 6.0).rem_euclid(6.0); + + let r = 1.0 - kr.min(4.0 - kr).clamp(0.0, 1.0); + let g = 1.0 - kg.min(4.0 - kg).clamp(0.0, 1.0); + let b = 1.0 - kb.min(4.0 - kb).clamp(0.0, 1.0); + + (r, g, b) +} + +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates for time `t`. +/// +/// This uses hardcoded parameters to have an arbitrary pleasing trajectory. +#[allow(dead_code)] +pub fn lissajous_3d(t: f32) -> Vec3 { + // Some hardcoded parameters to have a pleasing lissajous trajectory. + lissajous_3d_with_params(t, 3.0, 2.0, 1.0, FRAC_PI_2, FRAC_PI_4, FRAC_PI_6) +} + +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates. +#[allow(dead_code)] +pub fn lissajous_3d_with_params( + t: f32, + a: f32, + b: f32, + c: f32, + delta_x: f32, + delta_y: f32, + delta_z: f32, +) -> Vec3 { + let x = (a * t + delta_x).sin(); + let y = (b * t + delta_y).sin(); + let z = (c * t + delta_z).sin(); + Vec3::new(x, y, z) * 0.75f32 +} + +/// Uses [`kiss3d`] to display the line passed as parameter. +#[allow(dead_code)] +pub fn draw_polyline(window: &mut Window, polyline: Vec<(Vec3, Vec3)>, color: Color) { + for line in polyline { + let a = line.0; + let b = line.1; + window.draw_line(a, b, color, 2.0, false); + } +} + +/// Draws a text in the top left corner of the screen. +/// +/// This uses a hardcoded position, size, color. +#[allow(dead_code)] +pub fn draw_text(window: &mut Window, font: &Arc, text: &str) { + window.draw_text(text, Vec2::new(10.0, 66.0), 30.0, font, WHITE); +} + +/// Creates a GpuMesh3d from parry3d's trimesh vertices and indices. +#[allow(dead_code)] +pub fn create_mesh_from_trimesh( + vertices: Vec, + indices: Vec<[u32; 3]>, +) -> std::rc::Rc> { + use std::cell::RefCell; + use std::rc::Rc; + + let mut render_mesh = + RenderMesh::new(vertices, None, None, Some(IndexBuffer::Unified(indices))); + render_mesh.replicate_vertices(); + render_mesh.recompute_normals(); + Rc::new(RefCell::new(render_mesh.into())) +} + +/// Draws a 3D AABB (axis-aligned bounding box) as a wireframe. +#[allow(dead_code)] +pub fn draw_aabb3(window: &mut Window, mins: Vec3, maxs: Vec3, color: Color) { + // Bottom face + window.draw_line( + Vec3::new(mins.x, mins.y, mins.z), + Vec3::new(maxs.x, mins.y, mins.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(maxs.x, mins.y, mins.z), + Vec3::new(maxs.x, mins.y, maxs.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(maxs.x, mins.y, maxs.z), + Vec3::new(mins.x, mins.y, maxs.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(mins.x, mins.y, maxs.z), + Vec3::new(mins.x, mins.y, mins.z), + color, + 2.0, + false, + ); + // Top face + window.draw_line( + Vec3::new(mins.x, maxs.y, mins.z), + Vec3::new(maxs.x, maxs.y, mins.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(maxs.x, maxs.y, mins.z), + Vec3::new(maxs.x, maxs.y, maxs.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(maxs.x, maxs.y, maxs.z), + Vec3::new(mins.x, maxs.y, maxs.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(mins.x, maxs.y, maxs.z), + Vec3::new(mins.x, maxs.y, mins.z), + color, + 2.0, + false, + ); + // Vertical edges + window.draw_line( + Vec3::new(mins.x, mins.y, mins.z), + Vec3::new(mins.x, maxs.y, mins.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(maxs.x, mins.y, mins.z), + Vec3::new(maxs.x, maxs.y, mins.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(maxs.x, mins.y, maxs.z), + Vec3::new(maxs.x, maxs.y, maxs.z), + color, + 2.0, + false, + ); + window.draw_line( + Vec3::new(mins.x, mins.y, maxs.z), + Vec3::new(mins.x, maxs.y, maxs.z), + color, + 2.0, + false, + ); +} + +/// Draws a sphere wireframe using 3 orthogonal circles. +#[allow(dead_code)] +pub fn draw_sphere_wires(window: &mut Window, center: Vec3, radius: f32, color: Color) { + let segments = 32; + let tau = std::f32::consts::TAU; + + for i in 0..segments { + let angle1 = (i as f32 / segments as f32) * tau; + let angle2 = ((i + 1) as f32 / segments as f32) * tau; + + // XY plane circle + let p1 = center + Vec3::new(radius * angle1.cos(), radius * angle1.sin(), 0.0); + let p2 = center + Vec3::new(radius * angle2.cos(), radius * angle2.sin(), 0.0); + window.draw_line(p1, p2, color, 2.0, false); + + // XZ plane circle + let p1 = center + Vec3::new(radius * angle1.cos(), 0.0, radius * angle1.sin()); + let p2 = center + Vec3::new(radius * angle2.cos(), 0.0, radius * angle2.sin()); + window.draw_line(p1, p2, color, 2.0, false); + + // YZ plane circle + let p1 = center + Vec3::new(0.0, radius * angle1.cos(), radius * angle1.sin()); + let p2 = center + Vec3::new(0.0, radius * angle2.cos(), radius * angle2.sin()); + window.draw_line(p1, p2, color, 2.0, false); + } +} diff --git a/crates/parry3d/tests/geometry/aabb_scale.rs b/crates/parry3d/tests/geometry/aabb_scale.rs index 30e6c384..d6098aef 100644 --- a/crates/parry3d/tests/geometry/aabb_scale.rs +++ b/crates/parry3d/tests/geometry/aabb_scale.rs @@ -1,16 +1,16 @@ -use na::{Point3, Vector3}; use parry3d::bounding_volume::Aabb; +use parry3d::math::Vector; #[test] fn test_aabb_scale_wrt_center() { - let aabb = Aabb::from_half_extents(Point3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0)); - let scale = Vector3::new(10.0, -20.0, 50.0); - let scaled_aabb = aabb.scaled_wrt_center(&scale); - let scaled_aabb_neg = aabb.scaled_wrt_center(&-scale); - let scaled_aabb_abs = aabb.scaled_wrt_center(&scale.abs()); + let aabb = Aabb::from_half_extents(Vector::new(1.0, 2.0, 3.0), Vector::new(4.0, 5.0, 6.0)); + let scale = Vector::new(10.0, -20.0, 50.0); + let scaled_aabb = aabb.scaled_wrt_center(scale); + let scaled_aabb_neg = aabb.scaled_wrt_center(-scale); + let scaled_aabb_abs = aabb.scaled_wrt_center(scale.abs()); assert_eq!(&scaled_aabb, &scaled_aabb_neg); assert_eq!(&scaled_aabb, &scaled_aabb_abs); assert_eq!(aabb.center(), scaled_aabb.center()); - assert_eq!(scaled_aabb.half_extents(), Vector3::new(40.0, 100.0, 300.0)); + assert_eq!(scaled_aabb.half_extents(), Vector::new(40.0, 100.0, 300.0)); } diff --git a/crates/parry3d/tests/geometry/ball_ball_toi.rs b/crates/parry3d/tests/geometry/ball_ball_toi.rs index 55defdb0..11a77405 100644 --- a/crates/parry3d/tests/geometry/ball_ball_toi.rs +++ b/crates/parry3d/tests/geometry/ball_ball_toi.rs @@ -1,19 +1,19 @@ // Issue #35 -use na::{self, Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::Ball; #[test] fn test_ball_ball_toi() { let b = Ball::new(0.5); - let m1 = Isometry3::identity(); - let m2 = Isometry3::translation(0.0, 10.0, 0.0); - let vel1 = Vector3::new(0.0, 10.0, 0.0); - let vel2 = Vector3::zeros(); + let m1 = Pose::identity(); + let m2 = Pose::translation(0.0, 10.0, 0.0); + let vel1 = Vector::new(0.0, 10.0, 0.0); + let vel2 = Vector::ZERO; let cast = - query::cast_shapes(&m1, &vel1, &b, &m2, &vel2, &b, ShapeCastOptions::default()).unwrap(); + query::cast_shapes(&m1, vel1, &b, &m2, vel2, &b, ShapeCastOptions::default()).unwrap(); assert_eq!(cast.unwrap().time_of_impact, 0.9); } diff --git a/crates/parry3d/tests/geometry/ball_triangle_toi.rs b/crates/parry3d/tests/geometry/ball_triangle_toi.rs index bf57906a..02f807e7 100644 --- a/crates/parry3d/tests/geometry/ball_triangle_toi.rs +++ b/crates/parry3d/tests/geometry/ball_triangle_toi.rs @@ -1,6 +1,6 @@ // Issue #123 -use na::{self, Isometry3, Point3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::{Ball, Triangle}; @@ -8,18 +8,18 @@ use parry3d::shape::{Ball, Triangle}; fn ball_triangle_toi_infinite_loop_issue() { let b = Ball::new(0.375f32); let t = Triangle::new( - Point3::new(0.5, -0.5, 0.0), - Point3::new(-0.5, -0.5, 0.0), - Point3::new(-0.5, 0.5, 0.0), + Vector::new(0.5, -0.5, 0.0), + Vector::new(-0.5, -0.5, 0.0), + Vector::new(-0.5, 0.5, 0.0), ); - let m1 = Isometry3::translation(0.0, 0.0, 0.0); - let m2 = Isometry3::translation(11.5, 5.5, 0.0); - let vel1 = Vector3::new(0.0, 0.000000000000000000000000000000000000000006925, 0.0); - let vel2 = Vector3::zeros(); + let m1 = Pose::translation(0.0, 0.0, 0.0); + let m2 = Pose::translation(11.5, 5.5, 0.0); + let vel1 = Vector::new(0.0, 0.000000000000000000000000000000000000000006925, 0.0); + let vel2 = Vector::ZERO; let cast = - query::cast_shapes(&m1, &vel1, &b, &m2, &vel2, &t, ShapeCastOptions::default()).unwrap(); + query::cast_shapes(&m1, vel1, &b, &m2, vel2, &t, ShapeCastOptions::default()).unwrap(); println!("ShapeCastHit: {:?}", cast); assert!(cast.is_none()); // The provided velocity is too small. diff --git a/crates/parry3d/tests/geometry/convex_hull.rs b/crates/parry3d/tests/geometry/convex_hull.rs index 82dd0e7f..a1e7f411 100644 --- a/crates/parry3d/tests/geometry/convex_hull.rs +++ b/crates/parry3d/tests/geometry/convex_hull.rs @@ -1,4063 +1,4063 @@ -use na::Point3; +use parry3d::math::Vector3; use parry3d::transformation; #[test] fn test_complex_convex_hull() { let input = vec![ - Point3::new(-0.48323253, 0.015908679, -1.3343177), - Point3::new(-0.4485139, -0.10542433, -1.4737657), - Point3::new(-0.42711252, -0.10988713, -1.37545), - Point3::new(-0.4219087, -0.06716395, -1.5995741), - Point3::new(-0.4953231, 0.08068632, -1.4425513), - Point3::new(-0.42424062, 0.07620259, -1.2036741), - Point3::new(-0.47573563, 0.041238867, -1.484446), - Point3::new(-0.4782635, 0.046329774, -1.4790393), - Point3::new(-0.47573563, 0.046329767, -1.4853326), - Point3::new(-0.4842582, 0.02425052, -1.3302914), - Point3::new(-0.48323253, 0.015908679, -1.3343177), - Point3::new(-0.4828088, 0.020500222, -1.3302914), - Point3::new(-0.4874344, 0.046329774, -1.3302914), - Point3::new(-0.48891053, 0.046329774, -1.3851467), - Point3::new(-0.48323253, 0.015908679, -1.3343177), - Point3::new(-0.48635057, 0.03868017, -1.3302914), - Point3::new(-0.48323253, 0.015908679, -1.3343177), - Point3::new(-0.4842582, 0.024250517, -1.3302914), - Point3::new(-0.48635057, 0.038680166, -1.3302914), - Point3::new(-0.4828088, 0.020500222, -1.3302914), - Point3::new(-0.48323253, 0.015908679, -1.3343177), - Point3::new(-0.48141447, 0.017766872, -1.3302914), - Point3::new(-0.48042512, 0.046329774, -1.3076416), - Point3::new(-0.486973, 0.04632978, -1.3196346), - Point3::new(-0.48425823, 0.024250766, -1.3302913), - Point3::new(-0.4828088, 0.020500358, -1.3302913), - Point3::new(-0.4874344, 0.046329774, -1.3302913), - Point3::new(-0.48635066, 0.038680844, -1.3302913), - Point3::new(-0.487398, 0.046329774, -1.3289388), - Point3::new(-0.48425823, 0.024250764, -1.3302913), - Point3::new(-0.486973, 0.046329774, -1.3196346), - Point3::new(-0.487398, 0.046329767, -1.3289388), - Point3::new(-0.48635066, 0.03868084, -1.3302913), - Point3::new(-0.4828088, 0.020500358, -1.3302913), - Point3::new(-0.4814144, 0.017766926, -1.3302913), - Point3::new(-0.47573563, 0.023571035, -1.317715), - Point3::new(-0.47573563, 0.046329774, -1.3008153), - Point3::new(-0.48042512, 0.046329774, -1.3076416), - Point3::new(-0.47573563, 0.046329707, -1.4853326), - Point3::new(-0.47826347, 0.0463297, -1.4790394), - Point3::new(-0.48561266, 0.061130375, -1.4633205), - Point3::new(-0.48923, 0.112844266, -1.4633205), - Point3::new(-0.47573563, 0.112844266, -1.4969156), - Point3::new(-0.4953231, 0.08068632, -1.4425513), - Point3::new(-0.49021298, 0.05330783, -1.396806), - Point3::new(-0.4986484, 0.11284426, -1.396806), - Point3::new(-0.49916852, 0.11284426, -1.4161342), - Point3::new(-0.48923, 0.112844266, -1.4633205), - Point3::new(-0.48561266, 0.061130375, -1.4633205), - Point3::new(-0.4953231, 0.08068632, -1.4425513), - Point3::new(-0.49079263, 0.112844266, -1.4594302), - Point3::new(-0.4953231, 0.08068632, -1.4425513), - Point3::new(-0.49916852, 0.112844266, -1.4161342), - Point3::new(-0.49605638, 0.11284426, -1.4436231), - Point3::new(-0.4953231, 0.08068632, -1.4425513), - Point3::new(-0.49605638, 0.112844266, -1.4436231), - Point3::new(-0.49079263, 0.112844266, -1.4594302), - Point3::new(-0.49021295, 0.053307757, -1.3968059), - Point3::new(-0.48891053, 0.0463297, -1.3851466), - Point3::new(-0.4874344, 0.0463297, -1.3302914), - Point3::new(-0.4968585, 0.112844266, -1.3302914), - Point3::new(-0.4986484, 0.11284426, -1.3968059), - Point3::new(-0.49515137, 0.112844266, -1.2875305), - Point3::new(-0.486973, 0.046329692, -1.3196346), - Point3::new(-0.48042512, 0.0463297, -1.3076417), - Point3::new(-0.4758087, 0.096352905, -1.2637768), - Point3::new(-0.48218226, 0.112844266, -1.2637768), - Point3::new(-0.4968585, 0.112844266, -1.3302913), - Point3::new(-0.4874344, 0.0463297, -1.3302913), - Point3::new(-0.487398, 0.0463297, -1.3289388), - Point3::new(-0.49650565, 0.112844266, -1.3171781), - Point3::new(-0.486973, 0.0463297, -1.3196346), - Point3::new(-0.49515137, 0.112844266, -1.2875305), - Point3::new(-0.49650565, 0.11284426, -1.3171781), - Point3::new(-0.487398, 0.046329707, -1.3289388), - Point3::new(-0.4758087, 0.096352905, -1.2637768), - Point3::new(-0.48042512, 0.0463297, -1.3076417), - Point3::new(-0.47573563, 0.0463297, -1.3008155), - Point3::new(-0.47573563, 0.096209645, -1.2637768), - Point3::new(-0.48218226, 0.112844266, -1.2637768), - Point3::new(-0.4758087, 0.096352905, -1.2637768), - Point3::new(-0.47573563, 0.09714479, -1.2630824), - Point3::new(-0.47573563, 0.112844266, -1.2519693), - Point3::new(-0.4758087, 0.096352905, -1.2637768), - Point3::new(-0.47573563, 0.096209645, -1.2637768), - Point3::new(-0.47573563, 0.09714478, -1.2630824), - Point3::new(-0.47573563, 0.17935875, -1.5084984), - Point3::new(-0.47573563, 0.1128442, -1.4969155), - Point3::new(-0.48922995, 0.11284419, -1.4633205), - Point3::new(-0.48974842, 0.12025622, -1.4633205), - Point3::new(-0.48142195, 0.17935875, -1.494342), - Point3::new(-0.49175212, 0.17935875, -1.4633205), - Point3::new(-0.48142195, 0.17935875, -1.494342), - Point3::new(-0.48974842, 0.12025623, -1.4633205), - Point3::new(-0.5019821, 0.13637298, -1.396806), - Point3::new(-0.49916852, 0.1128442, -1.4161344), - Point3::new(-0.4986484, 0.1128442, -1.396806), - Point3::new(-0.48974842, 0.12025622, -1.4633205), - Point3::new(-0.48922995, 0.11284419, -1.4633205), - Point3::new(-0.49079263, 0.11284419, -1.4594301), - Point3::new(-0.5031245, 0.17935875, -1.396806), - Point3::new(-0.49757308, 0.17935875, -1.4458401), - Point3::new(-0.49605638, 0.1128442, -1.4436231), - Point3::new(-0.49916852, 0.11284419, -1.4161344), - Point3::new(-0.5019821, 0.13637298, -1.396806), - Point3::new(-0.49175212, 0.17935875, -1.4633205), - Point3::new(-0.48974842, 0.12025623, -1.4633205), - Point3::new(-0.49079263, 0.11284419, -1.4594301), - Point3::new(-0.49605638, 0.11284419, -1.4436231), - Point3::new(-0.49757308, 0.17935875, -1.4458401), - Point3::new(-0.5019821, 0.13637313, -1.3968059), - Point3::new(-0.4986484, 0.1128442, -1.3968059), - Point3::new(-0.4968585, 0.11284419, -1.3302914), - Point3::new(-0.5062826, 0.17935875, -1.3302914), - Point3::new(-0.5071223, 0.17935875, -1.3614942), - Point3::new(-0.5031245, 0.17935875, -1.3968059), - Point3::new(-0.5019821, 0.13637313, -1.3968059), - Point3::new(-0.5071223, 0.17935875, -1.3614942), - Point3::new(-0.5012025, 0.16205832, -1.2637768), - Point3::new(-0.49515134, 0.11284419, -1.2875305), - Point3::new(-0.48218226, 0.11284419, -1.2637768), - Point3::new(-0.5062826, 0.17935875, -1.3302913), - Point3::new(-0.4968585, 0.11284419, -1.3302913), - Point3::new(-0.49650565, 0.11284419, -1.3171781), - Point3::new(-0.5056133, 0.17935875, -1.3054174), - Point3::new(-0.49515134, 0.11284419, -1.2875305), - Point3::new(-0.5012025, 0.16205832, -1.2637768), - Point3::new(-0.50371116, 0.17935875, -1.2637768), - Point3::new(-0.5056133, 0.17935875, -1.3054174), - Point3::new(-0.49650565, 0.1128442, -1.3171781), - Point3::new(-0.47573563, 0.17935875, -1.204886), - Point3::new(-0.50332975, 0.17935875, -1.2554265), - Point3::new(-0.5012025, 0.16205832, -1.2637768), - Point3::new(-0.48218226, 0.11284419, -1.2637768), - Point3::new(-0.47573563, 0.11284419, -1.2519693), - Point3::new(-0.5012025, 0.16205832, -1.2637768), - Point3::new(-0.50332975, 0.17935875, -1.2554265), - Point3::new(-0.50371116, 0.17935875, -1.2637768), - Point3::new(-0.41459927, -0.17038356, -1.5298351), - Point3::new(-0.40922108, -0.1806848, -1.5387266), - Point3::new(-0.40922108, -0.18694264, -1.5298351), - Point3::new(-0.40922108, -0.1806848, -1.5387266), - Point3::new(-0.41459927, -0.17038356, -1.5298351), - Point3::new(-0.41921717, -0.15321395, -1.5298351), - Point3::new(-0.40922108, -0.15321395, -1.5639201), - Point3::new(-0.40922108, -0.19245225, -1.5220068), - Point3::new(-0.42693704, -0.15321395, -1.5002563), - Point3::new(-0.42356336, -0.15321395, -1.5150152), - Point3::new(-0.41459933, -0.17038342, -1.529835), - Point3::new(-0.40922108, -0.18694273, -1.529835), - Point3::new(-0.42693704, -0.15321395, -1.5002563), - Point3::new(-0.40922108, -0.19245224, -1.5220068), - Point3::new(-0.40922108, -0.172227, -1.4633205), - Point3::new(-0.4198326, -0.15321395, -1.4633205), - Point3::new(-0.41459933, -0.17038342, -1.529835), - Point3::new(-0.42356336, -0.15321395, -1.5150152), - Point3::new(-0.4192172, -0.15321395, -1.529835), - Point3::new(-0.4198326, -0.15321395, -1.4633205), - Point3::new(-0.40922108, -0.172227, -1.4633205), - Point3::new(-0.40922108, -0.15622416, -1.4168861), - Point3::new(-0.41038337, -0.15321395, -1.4141942), - Point3::new(-0.40922108, -0.15622418, -1.4168862), - Point3::new(-0.40922108, -0.15321395, -1.4124653), - Point3::new(-0.41038337, -0.15321395, -1.4141943), - Point3::new(-0.41552514, -0.08669945, -1.603425), - Point3::new(-0.40922108, -0.1059916, -1.6072279), - Point3::new(-0.40922108, -0.1178531, -1.5963497), - Point3::new(-0.4176001, -0.08669945, -1.5963497), - Point3::new(-0.40922108, -0.1059916, -1.6072279), - Point3::new(-0.41552514, -0.08669945, -1.603425), - Point3::new(-0.40922108, -0.08669945, -1.6126148), - Point3::new(-0.41760013, -0.08669945, -1.5963496), - Point3::new(-0.40922108, -0.117853224, -1.5963496), - Point3::new(-0.40922108, -0.15321401, -1.5639201), - Point3::new(-0.41921714, -0.15321401, -1.5298351), - Point3::new(-0.43665668, -0.08837273, -1.5298351), - Point3::new(-0.43549314, -0.08669945, -1.5353372), - Point3::new(-0.426937, -0.15321401, -1.5002563), - Point3::new(-0.4485139, -0.10542433, -1.4737657), - Point3::new(-0.42356333, -0.15321401, -1.5150152), - Point3::new(-0.4485139, -0.10542433, -1.4737657), - Point3::new(-0.426937, -0.15321401, -1.5002563), - Point3::new(-0.41983256, -0.15321401, -1.4633205), - Point3::new(-0.4462402, -0.10589846, -1.4633205), - Point3::new(-0.4366567, -0.08837277, -1.529835), - Point3::new(-0.41921717, -0.15321401, -1.529835), - Point3::new(-0.42356333, -0.15321401, -1.5150152), - Point3::new(-0.4485139, -0.10542433, -1.4737657), - Point3::new(-0.4462402, -0.10589846, -1.4633205), - Point3::new(-0.41983256, -0.15321401, -1.4633205), - Point3::new(-0.41038334, -0.15321401, -1.4141943), - Point3::new(-0.41789135, -0.13376904, -1.396806), - Point3::new(-0.4317613, -0.10891773, -1.396806), - Point3::new(-0.41789138, -0.13376896, -1.396806), - Point3::new(-0.41038334, -0.15321401, -1.4141945), - Point3::new(-0.40922108, -0.15321401, -1.4124653), - Point3::new(-0.40922108, -0.14255148, -1.396806), - Point3::new(-0.43176126, -0.108917736, -1.3968059), - Point3::new(-0.4178914, -0.1337689, -1.3968059), - Point3::new(-0.42711252, -0.10988713, -1.37545), - Point3::new(-0.40922108, -0.14129017, -1.3949536), - Point3::new(-0.42711252, -0.10988713, -1.37545), - Point3::new(-0.41789144, -0.13376883, -1.3968059), - Point3::new(-0.40922108, -0.14255139, -1.3968059), - Point3::new(-0.40922108, -0.14129019, -1.3949536), - Point3::new(-0.40922108, -0.10430696, -1.35424), - Point3::new(-0.42711252, -0.10988713, -1.37545), - Point3::new(-0.3472593, -0.08498167, -1.2807851), - Point3::new(-0.42259058, -0.06814454, -1.5963497), - Point3::new(-0.4219087, -0.06716395, -1.5995741), - Point3::new(-0.41552514, -0.086699404, -1.603425), - Point3::new(-0.4176001, -0.086699404, -1.5963497), - Point3::new(-0.4219087, -0.06716395, -1.5995741), - Point3::new(-0.42150742, -0.020184837, -1.6004248), - Point3::new(-0.40922108, -0.020184845, -1.6154815), - Point3::new(-0.40922108, -0.07801496, -1.6150396), - Point3::new(-0.4219087, -0.06716395, -1.5995741), - Point3::new(-0.42461768, -0.04695158, -1.5963497), - Point3::new(-0.42477804, -0.020184841, -1.5963497), - Point3::new(-0.42150742, -0.020184845, -1.6004248), - Point3::new(-0.4219087, -0.06716395, -1.5995741), - Point3::new(-0.42341623, -0.06412793, -1.5963497), - Point3::new(-0.42461768, -0.046951585, -1.5963497), - Point3::new(-0.40922108, -0.086699404, -1.6126148), - Point3::new(-0.41552514, -0.086699404, -1.603425), - Point3::new(-0.4219087, -0.06716395, -1.5995741), - Point3::new(-0.40922108, -0.07801497, -1.6150396), - Point3::new(-0.4225906, -0.068144575, -1.5963496), - Point3::new(-0.41760013, -0.086699404, -1.5963496), - Point3::new(-0.4354931, -0.086699404, -1.5353373), - Point3::new(-0.4246178, -0.046950832, -1.5963496), - Point3::new(-0.42820513, -0.020184837, -1.5920798), - Point3::new(-0.42477816, -0.020184841, -1.5963496), - Point3::new(-0.4234163, -0.06412782, -1.5963496), - Point3::new(-0.44523597, -0.020184837, -1.5496805), - Point3::new(-0.42820513, -0.020184845, -1.5920798), - Point3::new(-0.4246178, -0.04695084, -1.5963496), - Point3::new(-0.42150742, -0.020184793, -1.6004248), - Point3::new(-0.42093927, 0.046329774, -1.6016293), - Point3::new(-0.40922108, 0.046329767, -1.6159897), - Point3::new(-0.40922108, -0.020184785, -1.6154815), - Point3::new(-0.42517662, 0.04632977, -1.5963497), - Point3::new(-0.42093927, 0.046329767, -1.6016293), - Point3::new(-0.42150742, -0.020184785, -1.6004248), - Point3::new(-0.42477804, -0.020184789, -1.5963497), - Point3::new(-0.42820513, -0.020184793, -1.5920798), - Point3::new(-0.43711984, 0.046329774, -1.581469), - Point3::new(-0.4251767, 0.04632977, -1.5963496), - Point3::new(-0.42477816, -0.020184789, -1.5963496), - Point3::new(-0.45786002, 0.046329763, -1.5298351), - Point3::new(-0.43711984, 0.04632975, -1.581469), - Point3::new(-0.42820513, -0.020184785, -1.5920798), - Point3::new(-0.445236, -0.020184793, -1.5496804), - Point3::new(-0.45451444, -0.0014987998, -1.5298351), - Point3::new(-0.45786005, 0.046329763, -1.529835), - Point3::new(-0.4545145, -0.0014986861, -1.529835), - Point3::new(-0.47573563, 0.041238867, -1.484446), - Point3::new(-0.47573563, 0.046329774, -1.4853326), - Point3::new(-0.47573563, 0.023571035, -1.317715), - Point3::new(-0.45346835, 0.046329774, -1.2684019), - Point3::new(-0.47573563, 0.046329774, -1.3008153), - Point3::new(-0.40922108, 0.046329774, -1.2177562), - Point3::new(-0.4099734, 0.046329774, -1.2179652), - Point3::new(-0.40922108, 0.044754528, -1.2187189), - Point3::new(-0.42093927, 0.0463297, -1.6016293), - Point3::new(-0.42037112, 0.112844266, -1.6028337), - Point3::new(-0.40922108, 0.11284426, -1.6164979), - Point3::new(-0.40922108, 0.046329707, -1.6159897), - Point3::new(-0.4255752, 0.11284426, -1.5963497), - Point3::new(-0.42037112, 0.11284426, -1.6028337), - Point3::new(-0.42093927, 0.046329707, -1.6016293), - Point3::new(-0.42517662, 0.046329703, -1.5963497), - Point3::new(-0.43711984, 0.0463297, -1.581469), - Point3::new(-0.44603452, 0.112844266, -1.5708584), - Point3::new(-0.4255753, 0.11284426, -1.5963496), - Point3::new(-0.4251767, 0.046329703, -1.5963496), - Point3::new(-0.4625126, 0.112844266, -1.5298351), - Point3::new(-0.44603452, 0.11284426, -1.5708584), - Point3::new(-0.43711984, 0.046329707, -1.581469), - Point3::new(-0.45786002, 0.046329703, -1.5298351), - Point3::new(-0.46251267, 0.112844266, -1.529835), - Point3::new(-0.45786005, 0.046329703, -1.529835), - Point3::new(-0.47573563, 0.0463297, -1.4853326), - Point3::new(-0.47573563, 0.112844266, -1.4969156), - Point3::new(-0.47573563, 0.096209645, -1.2637768), - Point3::new(-0.47573563, 0.0463297, -1.3008155), - Point3::new(-0.4534684, 0.0463297, -1.2684021), - Point3::new(-0.45137987, 0.04846435, -1.2637768), - Point3::new(-0.4742868, 0.112844266, -1.2493156), - Point3::new(-0.47573563, 0.112844266, -1.2519693), - Point3::new(-0.47573563, 0.09714479, -1.2630824), - Point3::new(-0.42944744, 0.095044516, -1.1972623), - Point3::new(-0.42424062, 0.07620259, -1.2036741), - Point3::new(-0.41959578, 0.08458061, -1.1972623), - Point3::new(-0.42424062, 0.07620259, -1.2036741), - Point3::new(-0.40997338, 0.0463297, -1.2179654), - Point3::new(-0.40922108, 0.0463297, -1.2177562), - Point3::new(-0.40922108, 0.07986139, -1.1972623), - Point3::new(-0.41959578, 0.08458061, -1.1972623), - Point3::new(-0.42424062, 0.07620259, -1.2036741), - Point3::new(-0.42944744, 0.095044516, -1.1972623), - Point3::new(-0.4385274, 0.112844266, -1.1972623), - Point3::new(-0.4742868, 0.112844266, -1.2493156), - Point3::new(-0.47573563, 0.09714478, -1.2630824), - Point3::new(-0.47573563, 0.096209645, -1.2637768), - Point3::new(-0.45137987, 0.04846435, -1.2637768), - Point3::new(-0.40922108, 0.112844266, -1.1783407), - Point3::new(-0.4343663, 0.112844266, -1.1912051), - Point3::new(-0.42944756, 0.095044866, -1.1972622), - Point3::new(-0.4195957, 0.084580764, -1.1972622), - Point3::new(-0.40922108, 0.10329369, -1.182941), - Point3::new(-0.4195957, 0.084580764, -1.1972622), - Point3::new(-0.40922108, 0.07986158, -1.1972622), - Point3::new(-0.40922108, 0.10329369, -1.182941), - Point3::new(-0.42944756, 0.095044866, -1.1972622), - Point3::new(-0.4343663, 0.112844266, -1.1912051), - Point3::new(-0.43852732, 0.112844266, -1.1972622), - Point3::new(-0.42037112, 0.11284419, -1.6028337), - Point3::new(-0.41980296, 0.17935875, -1.6040382), - Point3::new(-0.40922108, 0.17935874, -1.6170061), - Point3::new(-0.40922108, 0.1128442, -1.6164979), - Point3::new(-0.42597374, 0.17935875, -1.5963497), - Point3::new(-0.41980296, 0.17935875, -1.6040382), - Point3::new(-0.42037112, 0.1128442, -1.6028337), - Point3::new(-0.4255752, 0.1128442, -1.5963497), - Point3::new(-0.44603452, 0.11284419, -1.5708584), - Point3::new(-0.4549492, 0.17935875, -1.5602477), - Point3::new(-0.42597386, 0.17935875, -1.5963496), - Point3::new(-0.4255753, 0.1128442, -1.5963496), - Point3::new(-0.4671652, 0.17935875, -1.5298351), - Point3::new(-0.4549492, 0.17935875, -1.5602477), - Point3::new(-0.44603452, 0.1128442, -1.5708584), - Point3::new(-0.46251258, 0.11284419, -1.5298351), - Point3::new(-0.46716526, 0.17935875, -1.529835), - Point3::new(-0.46251264, 0.11284419, -1.529835), - Point3::new(-0.47573563, 0.11284419, -1.4969155), - Point3::new(-0.47573563, 0.17935875, -1.5084984), - Point3::new(-0.47573566, 0.11284423, -1.4633205), - Point3::new(-0.40922108, 0.11284423, -1.4633205), - Point3::new(-0.40922108, 0.17935872, -1.4633205), - Point3::new(-0.47573566, 0.17935872, -1.4633205), - Point3::new(-0.47573566, 0.11284423, -1.396806), - Point3::new(-0.40922108, 0.11284423, -1.396806), - Point3::new(-0.40922108, 0.17935872, -1.396806), - Point3::new(-0.47573566, 0.17935872, -1.396806), - Point3::new(-0.47573566, 0.11284423, -1.396806), - Point3::new(-0.40922108, 0.11284423, -1.396806), - Point3::new(-0.40922108, 0.17935872, -1.396806), - Point3::new(-0.47573566, 0.17935872, -1.396806), - Point3::new(-0.47573566, 0.11284423, -1.3302914), - Point3::new(-0.40922108, 0.11284423, -1.3302914), - Point3::new(-0.40922108, 0.17935872, -1.3302914), - Point3::new(-0.47573566, 0.17935872, -1.3302914), - Point3::new(-0.47573566, 0.11284423, -1.3302914), - Point3::new(-0.40922108, 0.11284423, -1.3302914), - Point3::new(-0.40922108, 0.17935872, -1.3302914), - Point3::new(-0.47573566, 0.17935872, -1.3302914), - Point3::new(-0.47573566, 0.11284423, -1.2637768), - Point3::new(-0.40922108, 0.11284423, -1.2637768), - Point3::new(-0.40922108, 0.17935872, -1.2637768), - Point3::new(-0.47573566, 0.17935872, -1.2637768), - Point3::new(-0.47573563, 0.17935875, -1.204886), - Point3::new(-0.47573563, 0.11284419, -1.2519693), - Point3::new(-0.47428682, 0.11284419, -1.2493157), - Point3::new(-0.46880862, 0.17220557, -1.1972623), - Point3::new(-0.47157323, 0.17935875, -1.1972623), - Point3::new(-0.46880862, 0.17220557, -1.1972623), - Point3::new(-0.47428682, 0.11284419, -1.2493157), - Point3::new(-0.43852738, 0.11284419, -1.1972623), - Point3::new(-0.47157314, 0.17935875, -1.1972622), - Point3::new(-0.46880862, 0.17220572, -1.1972622), - Point3::new(-0.4681485, 0.17935875, -1.1909897), - Point3::new(-0.40922108, 0.17935875, -1.1463025), - Point3::new(-0.4527471, 0.17935875, -1.1685705), - Point3::new(-0.43436626, 0.11284419, -1.1912051), - Point3::new(-0.40922108, 0.11284419, -1.1783408), - Point3::new(-0.4527471, 0.17935875, -1.1685705), - Point3::new(-0.4681485, 0.17935875, -1.1909897), - Point3::new(-0.46880862, 0.17220572, -1.1972622), - Point3::new(-0.4385273, 0.11284419, -1.1972622), - Point3::new(-0.43436626, 0.11284419, -1.1912051), - Point3::new(-0.36200994, -0.15321395, -1.6628642), - Point3::new(-0.34270653, -0.15321395, -1.6910036), - Point3::new(-0.34270653, -0.2197285, -1.6724315), - Point3::new(-0.34926963, -0.2197285, -1.6628642), - Point3::new(-0.3997105, -0.15321395, -1.5963497), - Point3::new(-0.39379033, -0.15321395, -1.6165366), - Point3::new(-0.37205553, -0.2197285, -1.6296483), - Point3::new(-0.38182098, -0.2197285, -1.5963497), - Point3::new(-0.36200994, -0.15321395, -1.6628642), - Point3::new(-0.34926963, -0.2197285, -1.6628642), - Point3::new(-0.37205553, -0.2197285, -1.6296483), - Point3::new(-0.39379033, -0.15321395, -1.6165366), - Point3::new(-0.3888367, -0.2197285, -1.572427), - Point3::new(-0.39690596, -0.2197285, -1.5371264), - Point3::new(-0.40284482, -0.2065748, -1.5298351), - Point3::new(-0.40922108, -0.18694264, -1.5298351), - Point3::new(-0.40922108, -0.1806848, -1.5387266), - Point3::new(-0.4028448, -0.20657478, -1.5298351), - Point3::new(-0.39690593, -0.2197285, -1.5371264), - Point3::new(-0.3955035, -0.2197285, -1.5298351), - Point3::new(-0.39971054, -0.15321395, -1.5963496), - Point3::new(-0.381821, -0.2197285, -1.5963496), - Point3::new(-0.38883674, -0.2197285, -1.572427), - Point3::new(-0.40922108, -0.1806848, -1.5387266), - Point3::new(-0.40922108, -0.15321395, -1.5639201), - Point3::new(-0.40922108, -0.18694273, -1.529835), - Point3::new(-0.4028449, -0.20657457, -1.529835), - Point3::new(-0.40922108, -0.19245225, -1.5220068), - Point3::new(-0.4028449, -0.20657457, -1.529835), - Point3::new(-0.39550346, -0.2197285, -1.529835), - Point3::new(-0.3847011, -0.2197285, -1.4736738), - Point3::new(-0.38917148, -0.20815063, -1.4633205), - Point3::new(-0.40922108, -0.172227, -1.4633205), - Point3::new(-0.40922108, -0.19245224, -1.5220068), - Point3::new(-0.38917154, -0.20815049, -1.4633205), - Point3::new(-0.3847011, -0.2197285, -1.4736739), - Point3::new(-0.37774143, -0.2197285, -1.4633205), - Point3::new(-0.40922108, -0.172227, -1.4633205), - Point3::new(-0.38917148, -0.20815063, -1.4633205), - Point3::new(-0.40922108, -0.15622416, -1.4168861), - Point3::new(-0.40922108, -0.15321395, -1.4124653), - Point3::new(-0.40922108, -0.15622418, -1.4168862), - Point3::new(-0.38917154, -0.20815049, -1.4633205), - Point3::new(-0.37774143, -0.2197285, -1.4633205), - Point3::new(-0.36453196, -0.2197285, -1.4436697), - Point3::new(-0.40242767, -0.15321395, -1.4023591), - Point3::new(-0.3958336, -0.15321395, -1.396806), - Point3::new(-0.4024277, -0.15321395, -1.4023591), - Point3::new(-0.364532, -0.2197285, -1.4436697), - Point3::new(-0.34270653, -0.2197285, -1.4252896), - Point3::new(-0.34270653, -0.19385473, -1.396806), - Point3::new(-0.39583346, -0.15321395, -1.3968059), - Point3::new(-0.34270653, -0.19385463, -1.3968059), - Point3::new(-0.34270653, -0.15321395, -1.3520658), - Point3::new(-0.34270653, -0.13490114, -1.6961168), - Point3::new(-0.36998647, -0.11157012, -1.6628642), - Point3::new(-0.37014154, -0.086699456, -1.6628642), - Point3::new(-0.34270653, -0.086699456, -1.696485), - Point3::new(-0.36998647, -0.11157012, -1.6628642), - Point3::new(-0.34270653, -0.13490114, -1.6961168), - Point3::new(-0.34270653, -0.15321401, -1.6910036), - Point3::new(-0.36200994, -0.15321401, -1.6628642), - Point3::new(-0.40922108, -0.1178531, -1.5963497), - Point3::new(-0.40922108, -0.1059916, -1.6072279), - Point3::new(-0.39379033, -0.15321401, -1.6165366), - Point3::new(-0.3997105, -0.15321401, -1.5963497), - Point3::new(-0.36998647, -0.11157012, -1.6628642), - Point3::new(-0.3990667, -0.08669945, -1.6274172), - Point3::new(-0.37014154, -0.086699456, -1.6628642), - Point3::new(-0.36998647, -0.11157012, -1.6628642), - Point3::new(-0.36200994, -0.15321401, -1.6628642), - Point3::new(-0.39379033, -0.15321401, -1.6165366), - Point3::new(-0.40922108, -0.1059916, -1.6072279), - Point3::new(-0.40922108, -0.08669945, -1.6126148), - Point3::new(-0.3990667, -0.08669945, -1.6274172), - Point3::new(-0.40922108, -0.117853224, -1.5963496), - Point3::new(-0.39971054, -0.15321401, -1.5963496), - Point3::new(-0.40922108, -0.15321401, -1.5639201), - Point3::new(-0.40242764, -0.15321401, -1.4023592), - Point3::new(-0.4075218, -0.14427271, -1.396806), - Point3::new(-0.40922108, -0.14255148, -1.396806), - Point3::new(-0.40922108, -0.15321401, -1.4124653), - Point3::new(-0.4075218, -0.14427273, -1.396806), - Point3::new(-0.40242764, -0.15321401, -1.4023592), - Point3::new(-0.3958334, -0.15321401, -1.396806), - Point3::new(-0.40752193, -0.14427252, -1.3968059), - Point3::new(-0.40922108, -0.14129017, -1.3949536), - Point3::new(-0.40922108, -0.14255139, -1.3968059), - Point3::new(-0.40922108, -0.10430696, -1.35424), - Point3::new(-0.40922108, -0.14129019, -1.3949536), - Point3::new(-0.40752193, -0.14427254, -1.3968059), - Point3::new(-0.39583325, -0.15321401, -1.3968059), - Point3::new(-0.34270653, -0.15321401, -1.3520659), - Point3::new(-0.34270653, -0.13343461, -1.3302914), - Point3::new(-0.38901964, -0.09800632, -1.3302914), - Point3::new(-0.34270653, -0.086741775, -1.2821165), - Point3::new(-0.34270653, -0.08669945, -1.2820915), - Point3::new(-0.34281603, -0.08669945, -1.2820845), - Point3::new(-0.38901955, -0.098006286, -1.3302913), - Point3::new(-0.34270653, -0.1334345, -1.3302913), - Point3::new(-0.34270653, -0.10629411, -1.3004133), - Point3::new(-0.34689236, -0.08669945, -1.2823671), - Point3::new(-0.35276693, -0.08669945, -1.2873143), - Point3::new(-0.34281603, -0.08669945, -1.2820845), - Point3::new(-0.34662613, -0.08669945, -1.2822367), - Point3::new(-0.34270653, -0.097332954, -1.2912227), - Point3::new(-0.34270653, -0.086741775, -1.2821165), - Point3::new(-0.34662613, -0.08669945, -1.2822367), - Point3::new(-0.34689236, -0.08669945, -1.2823671), - Point3::new(-0.34270653, -0.106294096, -1.3004133), - Point3::new(-0.34270653, -0.097332954, -1.2912227), - Point3::new(-0.37055624, -0.020184837, -1.6628642), - Point3::new(-0.34270653, -0.020184837, -1.6969932), - Point3::new(-0.34270653, -0.0866994, -1.696485), - Point3::new(-0.3701415, -0.0866994, -1.6628642), - Point3::new(-0.37055624, -0.020184837, -1.6628642), - Point3::new(-0.3701415, -0.0866994, -1.6628642), - Point3::new(-0.39906675, -0.086699404, -1.6274171), - Point3::new(-0.40922108, -0.07801496, -1.6150396), - Point3::new(-0.40922108, -0.020184837, -1.6154815), - Point3::new(-0.40922108, -0.086699404, -1.6126148), - Point3::new(-0.40922108, -0.07801497, -1.6150396), - Point3::new(-0.39906675, -0.086699404, -1.6274171), - Point3::new(-0.3472593, -0.08498167, -1.2807851), - Point3::new(-0.3488198, -0.0564432, -1.2637768), - Point3::new(-0.36423904, -0.049429357, -1.2637768), - Point3::new(-0.34270653, -0.086699404, -1.2820914), - Point3::new(-0.34270653, -0.055789243, -1.2637768), - Point3::new(-0.3488198, -0.056443203, -1.2637768), - Point3::new(-0.3472593, -0.08498167, -1.2807851), - Point3::new(-0.34281614, -0.086699404, -1.2820845), - Point3::new(-0.34689236, -0.086699404, -1.2823671), - Point3::new(-0.3472593, -0.08498167, -1.2807851), - Point3::new(-0.35276678, -0.086699404, -1.2873142), - Point3::new(-0.34281614, -0.086699404, -1.2820845), - Point3::new(-0.3472593, -0.08498167, -1.2807851), - Point3::new(-0.34662613, -0.086699404, -1.2822367), - Point3::new(-0.34662613, -0.086699404, -1.2822367), - Point3::new(-0.3472593, -0.08498167, -1.2807851), - Point3::new(-0.34689236, -0.086699404, -1.2823671), - Point3::new(-0.3488198, -0.0564432, -1.2637768), - Point3::new(-0.35080242, -0.020184837, -1.2421676), - Point3::new(-0.37820616, -0.020184837, -1.2497861), - Point3::new(-0.36423904, -0.049429357, -1.2637768), - Point3::new(-0.34270653, -0.055789243, -1.2637768), - Point3::new(-0.34270653, -0.020184837, -1.2426808), - Point3::new(-0.35080242, -0.020184845, -1.2421676), - Point3::new(-0.3488198, -0.056443203, -1.2637768), - Point3::new(-0.3709709, 0.046329774, -1.6628642), - Point3::new(-0.34270653, 0.046329774, -1.6975014), - Point3::new(-0.34270653, -0.020184793, -1.6969932), - Point3::new(-0.37055624, -0.020184793, -1.6628642), - Point3::new(-0.3709709, 0.046329774, -1.6628642), - Point3::new(-0.37055624, -0.020184793, -1.6628642), - Point3::new(-0.40922108, -0.020184793, -1.6154815), - Point3::new(-0.40922108, 0.046329774, -1.6159897), - Point3::new(-0.35080242, -0.020184793, -1.2421676), - Point3::new(-0.35443944, 0.046329774, -1.2025263), - Point3::new(-0.40922108, 0.046329774, -1.2177562), - Point3::new(-0.40922108, 0.044754528, -1.2187189), - Point3::new(-0.3782062, -0.020184793, -1.249786), - Point3::new(-0.34270653, -0.020184793, -1.2426808), - Point3::new(-0.34270653, 0.046329774, -1.2032701), - Point3::new(-0.35443944, 0.046329767, -1.2025265), - Point3::new(-0.35080242, -0.02018477, -1.2421676), - Point3::new(-0.34755087, 0.112844266, -1.6920731), - Point3::new(-0.34270653, 0.11284426, -1.694334), - Point3::new(-0.34270653, 0.08907384, -1.697828), - Point3::new(-0.34755087, 0.11284426, -1.6920731), - Point3::new(-0.34270653, 0.08907384, -1.697828), - Point3::new(-0.34270653, 0.0463297, -1.6975014), - Point3::new(-0.3709709, 0.046329696, -1.6628642), - Point3::new(-0.37138563, 0.11284426, -1.6628642), - Point3::new(-0.37138563, 0.11284426, -1.6628642), - Point3::new(-0.3709709, 0.046329696, -1.6628642), - Point3::new(-0.40922108, 0.046329692, -1.6159897), - Point3::new(-0.40922108, 0.112844266, -1.6164979), - Point3::new(-0.35443944, 0.0463297, -1.2025265), - Point3::new(-0.3549224, 0.055162504, -1.1972623), - Point3::new(-0.40922108, 0.07986139, -1.1972623), - Point3::new(-0.40922108, 0.0463297, -1.2177562), - Point3::new(-0.35443944, 0.046329707, -1.2025265), - Point3::new(-0.34270653, 0.046329707, -1.2032702), - Point3::new(-0.34270653, 0.056469433, -1.1972623), - Point3::new(-0.3549224, 0.05516251, -1.1972623), - Point3::new(-0.34270653, 0.10713466, -1.1672425), - Point3::new(-0.34270653, 0.112844266, -1.1643773), - Point3::new(-0.34408033, 0.112844266, -1.1637725), - Point3::new(-0.40392616, 0.112844266, -1.1756318), - Point3::new(-0.40922108, 0.112844266, -1.1783407), - Point3::new(-0.40922108, 0.10329369, -1.182941), - Point3::new(-0.35492244, 0.055162705, -1.1972622), - Point3::new(-0.35807648, 0.112844266, -1.1628852), - Point3::new(-0.40392616, 0.112844266, -1.1756318), - Point3::new(-0.40922108, 0.10329369, -1.182941), - Point3::new(-0.40922108, 0.07986158, -1.1972622), - Point3::new(-0.35492244, 0.055162713, -1.1972622), - Point3::new(-0.34270653, 0.056469634, -1.1972622), - Point3::new(-0.34270653, 0.10713467, -1.1672425), - Point3::new(-0.34408033, 0.112844266, -1.1637725), - Point3::new(-0.35807648, 0.112844266, -1.1628852), - Point3::new(-0.34270653, 0.17935875, -1.6845568), - Point3::new(-0.34270653, 0.1128442, -1.694334), - Point3::new(-0.34755087, 0.11284419, -1.6920731), - Point3::new(-0.36110634, 0.17935875, -1.6759695), - Point3::new(-0.37180036, 0.17935874, -1.6628642), - Point3::new(-0.36110634, 0.17935874, -1.6759695), - Point3::new(-0.34755087, 0.1128442, -1.6920731), - Point3::new(-0.37138563, 0.1128442, -1.6628642), - Point3::new(-0.37180036, 0.17935874, -1.6628642), - Point3::new(-0.37138563, 0.1128442, -1.6628642), - Point3::new(-0.40922108, 0.11284419, -1.6164979), - Point3::new(-0.40922108, 0.17935875, -1.6170061), - Point3::new(-0.40922108, 0.11284423, -1.5963496), - Point3::new(-0.34270653, 0.11284423, -1.5963496), - Point3::new(-0.34270653, 0.17935872, -1.5963496), - Point3::new(-0.40922108, 0.17935872, -1.5963496), - Point3::new(-0.40922108, 0.11284423, -1.529835), - Point3::new(-0.34270653, 0.11284423, -1.529835), - Point3::new(-0.34270653, 0.17935872, -1.529835), - Point3::new(-0.40922108, 0.17935872, -1.529835), - Point3::new(-0.40922108, 0.11284423, -1.529835), - Point3::new(-0.34270653, 0.11284423, -1.529835), - Point3::new(-0.34270653, 0.17935872, -1.529835), - Point3::new(-0.40922108, 0.17935872, -1.529835), - Point3::new(-0.40922108, 0.11284423, -1.4633205), - Point3::new(-0.34270653, 0.11284423, -1.4633205), - Point3::new(-0.34270653, 0.17935872, -1.4633205), - Point3::new(-0.40922108, 0.17935872, -1.4633205), - Point3::new(-0.40922108, 0.11284423, -1.4633205), - Point3::new(-0.34270653, 0.11284423, -1.4633205), - Point3::new(-0.34270653, 0.17935872, -1.4633205), - Point3::new(-0.40922108, 0.17935872, -1.4633205), - Point3::new(-0.40922108, 0.11284423, -1.396806), - Point3::new(-0.34270653, 0.11284423, -1.396806), - Point3::new(-0.34270653, 0.17935872, -1.396806), - Point3::new(-0.40922108, 0.17935872, -1.396806), - Point3::new(-0.40922108, 0.11284423, -1.396806), - Point3::new(-0.34270653, 0.11284423, -1.396806), - Point3::new(-0.34270653, 0.17935872, -1.396806), - Point3::new(-0.40922108, 0.17935872, -1.396806), - Point3::new(-0.40922108, 0.11284423, -1.3302914), - Point3::new(-0.34270653, 0.11284423, -1.3302914), - Point3::new(-0.34270653, 0.17935872, -1.3302914), - Point3::new(-0.40922108, 0.17935872, -1.3302914), - Point3::new(-0.40922108, 0.11284423, -1.3302914), - Point3::new(-0.34270653, 0.11284423, -1.3302914), - Point3::new(-0.34270653, 0.17935872, -1.3302914), - Point3::new(-0.40922108, 0.17935872, -1.3302914), - Point3::new(-0.40922108, 0.11284423, -1.2637768), - Point3::new(-0.34270653, 0.11284423, -1.2637768), - Point3::new(-0.34270653, 0.17935872, -1.2637768), - Point3::new(-0.40922108, 0.17935872, -1.2637768), - Point3::new(-0.40922108, 0.11284423, -1.2637768), - Point3::new(-0.34270653, 0.11284423, -1.2637768), - Point3::new(-0.34270653, 0.17935872, -1.2637768), - Point3::new(-0.40922108, 0.17935872, -1.2637768), - Point3::new(-0.40922108, 0.11284423, -1.1972623), - Point3::new(-0.34270653, 0.11284423, -1.1972623), - Point3::new(-0.34270653, 0.17935872, -1.1972623), - Point3::new(-0.40922108, 0.17935872, -1.1972623), - Point3::new(-0.3571547, 0.16718233, -1.1307477), - Point3::new(-0.3440803, 0.11284419, -1.1637725), - Point3::new(-0.34270653, 0.11284419, -1.1643773), - Point3::new(-0.34270653, 0.15000299, -1.1457299), - Point3::new(-0.35324317, 0.17061399, -1.1307477), - Point3::new(-0.34270653, 0.15000299, -1.1457299), - Point3::new(-0.34270653, 0.17935875, -1.1310631), - Point3::new(-0.343416, 0.17935875, -1.1307477), - Point3::new(-0.35324317, 0.17061399, -1.1307477), - Point3::new(-0.40922108, 0.17935875, -1.1463025), - Point3::new(-0.40922108, 0.11284419, -1.1783408), - Point3::new(-0.4039262, 0.11284419, -1.1756319), - Point3::new(-0.37141103, 0.17149258, -1.1307477), - Point3::new(-0.37881705, 0.17935875, -1.1307477), - Point3::new(-0.37141103, 0.17149258, -1.1307477), - Point3::new(-0.4039262, 0.11284419, -1.1756319), - Point3::new(-0.35807645, 0.11284419, -1.1628852), - Point3::new(-0.36102504, 0.16676828, -1.1307477), - Point3::new(-0.36102504, 0.16676828, -1.1307477), - Point3::new(-0.35807645, 0.11284419, -1.1628852), - Point3::new(-0.3440803, 0.11284419, -1.1637725), - Point3::new(-0.3571547, 0.16718233, -1.1307477), - Point3::new(-0.3577136, 0.17935875, -1.1243911), - Point3::new(-0.36008447, 0.17935875, -1.1233473), - Point3::new(-0.35715473, 0.16718253, -1.1307476), - Point3::new(-0.35324326, 0.17061415, -1.1307476), - Point3::new(-0.35324326, 0.17061415, -1.1307476), - Point3::new(-0.34341627, 0.17935875, -1.1307476), - Point3::new(-0.3577136, 0.17935875, -1.1243911), - Point3::new(-0.3788168, 0.17935875, -1.1307476), - Point3::new(-0.37141097, 0.17149273, -1.1307476), - Point3::new(-0.36704996, 0.17935875, -1.1247276), - Point3::new(-0.3617135, 0.17935875, -1.123244), - Point3::new(-0.36704996, 0.17935875, -1.1247276), - Point3::new(-0.37141097, 0.17149273, -1.1307476), - Point3::new(-0.36102507, 0.16676848, -1.1307476), - Point3::new(-0.36102507, 0.16676848, -1.1307476), - Point3::new(-0.35715473, 0.16718253, -1.1307476), - Point3::new(-0.36008447, 0.17935875, -1.1233473), - Point3::new(-0.3617135, 0.17935875, -1.123244), - Point3::new(-0.29332903, -0.15321395, -1.7564877), - Point3::new(-0.2885518, -0.15321395, -1.7587173), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.31541878, -0.15823883, -1.7293788), - Point3::new(-0.31545013, -0.15321395, -1.7293788), - Point3::new(-0.29332903, -0.15321395, -1.7564877), - Point3::new(-0.30393142, -0.21821208, -1.7293788), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.28400028, -0.2197285, -1.7625116), - Point3::new(-0.30376926, -0.2197285, -1.7293788), - Point3::new(-0.31541878, -0.15823883, -1.7293788), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.30393142, -0.21821208, -1.7293788), - Point3::new(-0.23608275, -0.038243636, -1.7570658), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.3002659, 0.14816007, -1.7089504), - Point3::new(-0.27619198, -0.15321395, -1.7627211), - Point3::new(-0.27619198, -0.20534962, -1.7756828), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.27709365, -0.15321395, -1.7622056), - Point3::new(-0.27619198, -0.2197285, -1.7805436), - Point3::new(-0.27641594, -0.2197285, -1.7804569), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.27619198, -0.20534962, -1.7756828), - Point3::new(-0.28400028, -0.2197285, -1.7625116), - Point3::new(-0.28742594, -0.18217954, -1.7635005), - Point3::new(-0.27641594, -0.2197285, -1.7804569), - Point3::new(-0.31541887, -0.15823874, -1.7293787), - Point3::new(-0.32129416, -0.15321395, -1.7222171), - Point3::new(-0.31545022, -0.15321395, -1.7293787), - Point3::new(-0.30393147, -0.21821222, -1.7293787), - Point3::new(-0.30376932, -0.2197285, -1.7293787), - Point3::new(-0.30462605, -0.2197285, -1.7279428), - Point3::new(-0.34270653, -0.2197285, -1.6724315), - Point3::new(-0.34270653, -0.15321395, -1.6910034), - Point3::new(-0.32129416, -0.15321395, -1.7222171), - Point3::new(-0.31541887, -0.15823874, -1.7293787), - Point3::new(-0.30393147, -0.21821222, -1.7293787), - Point3::new(-0.30462605, -0.2197285, -1.7279428), - Point3::new(-0.34270653, -0.21972859, -1.6628642), - Point3::new(-0.27619195, -0.21972859, -1.6628642), - Point3::new(-0.27619195, -0.15321398, -1.6628642), - Point3::new(-0.34270653, -0.15321398, -1.6628642), - Point3::new(-0.34270653, -0.21972859, -1.5963496), - Point3::new(-0.27619195, -0.21972859, -1.5963496), - Point3::new(-0.27619195, -0.15321398, -1.5963496), - Point3::new(-0.34270653, -0.15321398, -1.5963496), - Point3::new(-0.34270653, -0.21972859, -1.5963496), - Point3::new(-0.27619195, -0.21972859, -1.5963496), - Point3::new(-0.27619195, -0.15321398, -1.5963496), - Point3::new(-0.34270653, -0.15321398, -1.5963496), - Point3::new(-0.34270653, -0.21972859, -1.529835), - Point3::new(-0.27619195, -0.21972859, -1.529835), - Point3::new(-0.27619195, -0.15321398, -1.529835), - Point3::new(-0.34270653, -0.15321398, -1.529835), - Point3::new(-0.34270653, -0.21972859, -1.529835), - Point3::new(-0.27619195, -0.21972859, -1.529835), - Point3::new(-0.27619195, -0.15321398, -1.529835), - Point3::new(-0.34270653, -0.15321398, -1.529835), - Point3::new(-0.34270653, -0.21972859, -1.4633205), - Point3::new(-0.27619195, -0.21972859, -1.4633205), - Point3::new(-0.27619195, -0.15321398, -1.4633205), - Point3::new(-0.34270653, -0.15321398, -1.4633205), - Point3::new(-0.34270653, -0.19385463, -1.396806), - Point3::new(-0.34270653, -0.2197285, -1.4252898), - Point3::new(-0.31847456, -0.2197285, -1.4048831), - Point3::new(-0.32034805, -0.21095827, -1.396806), - Point3::new(-0.32034805, -0.21095827, -1.396806), - Point3::new(-0.31847456, -0.2197285, -1.4048831), - Point3::new(-0.30198348, -0.2197285, -1.396806), - Point3::new(-0.27619198, -0.17143622, -1.3522788), - Point3::new(-0.27619198, -0.15321395, -1.3391918), - Point3::new(-0.28043228, -0.15321395, -1.336781), - Point3::new(-0.33268347, -0.15321395, -1.3436251), - Point3::new(-0.34270653, -0.15321395, -1.3520659), - Point3::new(-0.34270653, -0.19385453, -1.3968059), - Point3::new(-0.32034808, -0.21095814, -1.3968059), - Point3::new(-0.27619198, -0.17143622, -1.3522788), - Point3::new(-0.28043228, -0.15321395, -1.336781), - Point3::new(-0.32210836, -0.15321395, -1.3384455), - Point3::new(-0.29759055, -0.2197285, -1.3946544), - Point3::new(-0.27619198, -0.2197285, -1.3937998), - Point3::new(-0.32034808, -0.21095814, -1.3968059), - Point3::new(-0.30198324, -0.2197285, -1.3968059), - Point3::new(-0.29759055, -0.2197285, -1.3946544), - Point3::new(-0.32210836, -0.15321395, -1.3384455), - Point3::new(-0.33268347, -0.15321395, -1.3436251), - Point3::new(-0.29113716, -0.08669944, -1.7477335), - Point3::new(-0.2885518, -0.15321401, -1.7587173), - Point3::new(-0.29332903, -0.15321401, -1.7564877), - Point3::new(-0.3068845, -0.08669945, -1.7403841), - Point3::new(-0.31586492, -0.086699456, -1.7293788), - Point3::new(-0.3068845, -0.086699456, -1.7403841), - Point3::new(-0.29332903, -0.15321401, -1.7564877), - Point3::new(-0.31545013, -0.15321401, -1.7293788), - Point3::new(-0.27619198, -0.15068617, -1.7620926), - Point3::new(-0.27619198, -0.15321401, -1.7627211), - Point3::new(-0.27709368, -0.15321401, -1.7622056), - Point3::new(-0.315865, -0.086699456, -1.7293787), - Point3::new(-0.31545022, -0.15321401, -1.7293787), - Point3::new(-0.32129407, -0.15321401, -1.7222172), - Point3::new(-0.34270653, -0.13490114, -1.6961167), - Point3::new(-0.34270653, -0.08669945, -1.696485), - Point3::new(-0.34270653, -0.15321401, -1.6910034), - Point3::new(-0.34270653, -0.13490114, -1.6961167), - Point3::new(-0.32129407, -0.15321401, -1.7222172), - Point3::new(-0.2912733, -0.106625855, -1.2971584), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.25837165, -0.014966724, -1.2656467), - Point3::new(-0.28220788, -0.14558353, -1.3302914), - Point3::new(-0.28043228, -0.15321401, -1.336781), - Point3::new(-0.27619198, -0.15321401, -1.3391918), - Point3::new(-0.27619198, -0.14082113, -1.3302914), - Point3::new(-0.33268344, -0.15321401, -1.3436252), - Point3::new(-0.33577624, -0.13873605, -1.3302914), - Point3::new(-0.34270653, -0.13343461, -1.3302914), - Point3::new(-0.34270653, -0.15321401, -1.3520659), - Point3::new(-0.32210833, -0.15321401, -1.3384455), - Point3::new(-0.28043228, -0.15321401, -1.336781), - Point3::new(-0.28220788, -0.14558353, -1.3302914), - Point3::new(-0.3256651, -0.1435648, -1.3302914), - Point3::new(-0.33577624, -0.13873605, -1.3302914), - Point3::new(-0.33268344, -0.15321401, -1.3436252), - Point3::new(-0.32210833, -0.15321401, -1.3384455), - Point3::new(-0.3256651, -0.1435648, -1.3302914), - Point3::new(-0.27619198, -0.11314488, -1.3104147), - Point3::new(-0.2912733, -0.106625855, -1.2971584), - Point3::new(-0.2822079, -0.14558339, -1.3302913), - Point3::new(-0.27619198, -0.14082097, -1.3302913), - Point3::new(-0.2912733, -0.106625855, -1.2971584), - Point3::new(-0.2841206, -0.08669945, -1.2903078), - Point3::new(-0.29606783, -0.08669945, -1.2850478), - Point3::new(-0.34270653, -0.086699456, -1.2820915), - Point3::new(-0.34270653, -0.086741775, -1.2821165), - Point3::new(-0.2912733, -0.106625855, -1.2971584), - Point3::new(-0.2960678, -0.08669945, -1.2850478), - Point3::new(-0.33577627, -0.13873592, -1.3302913), - Point3::new(-0.34270653, -0.10629411, -1.3004133), - Point3::new(-0.34270653, -0.1334345, -1.3302913), - Point3::new(-0.32566515, -0.14356467, -1.3302913), - Point3::new(-0.28220794, -0.14558339, -1.3302913), - Point3::new(-0.2912733, -0.106625855, -1.2971584), - Point3::new(-0.34270653, -0.086741775, -1.2821165), - Point3::new(-0.34270653, -0.097332954, -1.2912227), - Point3::new(-0.33577627, -0.13873592, -1.3302913), - Point3::new(-0.32566515, -0.14356467, -1.3302913), - Point3::new(-0.34270653, -0.097332954, -1.2912227), - Point3::new(-0.34270653, -0.106294096, -1.3004133), - Point3::new(-0.2937225, -0.020184845, -1.7367498), - Point3::new(-0.29113716, -0.0866994, -1.7477335), - Point3::new(-0.3068845, -0.086699404, -1.740384), - Point3::new(-0.31614825, -0.04124357, -1.7293788), - Point3::new(-0.3095158, -0.020184841, -1.7293788), - Point3::new(-0.31614825, -0.041243568, -1.7293788), - Point3::new(-0.3068845, -0.0866994, -1.740384), - Point3::new(-0.31586483, -0.0866994, -1.7293788), - Point3::new(-0.2045752, 0.17152071, -1.7329516), - Point3::new(-0.30951604, -0.020184841, -1.7293787), - Point3::new(-0.31614837, -0.04124308, -1.7293787), - Point3::new(-0.32043996, -0.020184837, -1.7242804), - Point3::new(-0.34270653, -0.020184837, -1.6969932), - Point3::new(-0.32043996, -0.020184837, -1.7242804), - Point3::new(-0.31614837, -0.041243076, -1.7293787), - Point3::new(-0.31586492, -0.0866994, -1.7293787), - Point3::new(-0.34270653, -0.086699404, -1.696485), - Point3::new(-0.27619198, -0.06461153, -1.2827141), - Point3::new(-0.27619198, -0.026874736, -1.2637768), - Point3::new(-0.30448896, -0.051700555, -1.2637768), - Point3::new(-0.29606783, -0.086699404, -1.2850478), - Point3::new(-0.28412056, -0.086699404, -1.2903078), - Point3::new(-0.34270653, -0.055789243, -1.2637768), - Point3::new(-0.34270653, -0.0866994, -1.2820914), - Point3::new(-0.29606783, -0.086699404, -1.2850478), - Point3::new(-0.30448896, -0.051700555, -1.2637768), - Point3::new(-0.27619198, -0.026874736, -1.2637768), - Point3::new(-0.27619198, -0.020184837, -1.2604196), - Point3::new(-0.312072, -0.020184837, -1.2446227), - Point3::new(-0.30448896, -0.051700555, -1.2637768), - Point3::new(-0.34270653, -0.055789243, -1.2637768), - Point3::new(-0.30448896, -0.051700555, -1.2637768), - Point3::new(-0.312072, -0.020184837, -1.2446227), - Point3::new(-0.34270653, -0.020184845, -1.2426808), - Point3::new(-0.29545748, 0.024451343, -1.7293788), - Point3::new(-0.2937225, -0.020184785, -1.7367498), - Point3::new(-0.3095158, -0.020184785, -1.7293788), - Point3::new(-0.2954575, 0.024452064, -1.7293787), - Point3::new(-0.30951604, -0.020184785, -1.7293787), - Point3::new(-0.32043996, -0.020184785, -1.7242804), - Point3::new(-0.33399543, 0.046329774, -1.7081767), - Point3::new(-0.29630786, 0.046329774, -1.725766), - Point3::new(-0.34270653, 0.046329774, -1.6975014), - Point3::new(-0.33399543, 0.046329774, -1.7081767), - Point3::new(-0.32043996, -0.020184793, -1.7242804), - Point3::new(-0.34270653, -0.020184785, -1.6969932), - Point3::new(-0.27619198, 0.019892095, -1.2403078), - Point3::new(-0.2897073, 0.046329774, -1.2210902), - Point3::new(-0.32807618, 0.046329774, -1.2041975), - Point3::new(-0.312072, -0.020184793, -1.2446226), - Point3::new(-0.27619198, -0.020184793, -1.2604195), - Point3::new(-0.27619198, 0.019892097, -1.2403078), - Point3::new(-0.27619198, 0.046329774, -1.227099), - Point3::new(-0.2897073, 0.046329774, -1.2210902), - Point3::new(-0.34270653, 0.046329774, -1.2032702), - Point3::new(-0.34270653, -0.020184785, -1.2426808), - Point3::new(-0.312072, -0.020184793, -1.2446226), - Point3::new(-0.32807618, 0.046329774, -1.2041975), - Point3::new(-0.2988932, 0.112844266, -1.7147822), - Point3::new(-0.29630786, 0.0463297, -1.725766), - Point3::new(-0.3339954, 0.046329707, -1.7081767), - Point3::new(-0.34270653, 0.08907384, -1.697828), - Point3::new(-0.34270653, 0.112844266, -1.694334), - Point3::new(-0.34270653, 0.046329707, -1.6975014), - Point3::new(-0.34270653, 0.08907384, -1.697828), - Point3::new(-0.3339954, 0.0463297, -1.7081767), - Point3::new(-0.28970727, 0.0463297, -1.2210902), - Point3::new(-0.30646494, 0.07910983, -1.1972623), - Point3::new(-0.33082178, 0.057740793, -1.1972623), - Point3::new(-0.32807615, 0.0463297, -1.2041975), - Point3::new(-0.30646497, 0.07910992, -1.1972623), - Point3::new(-0.28970724, 0.0463297, -1.2210903), - Point3::new(-0.27619198, 0.0463297, -1.227099), - Point3::new(-0.27619198, 0.10604803, -1.1972623), - Point3::new(-0.34270653, 0.05646943, -1.1972623), - Point3::new(-0.34270653, 0.0463297, -1.2032702), - Point3::new(-0.32807615, 0.046329707, -1.2041975), - Point3::new(-0.33082178, 0.0577408, -1.1972623), - Point3::new(-0.30646503, 0.07911, -1.1972622), - Point3::new(-0.32371047, 0.112844266, -1.1727407), - Point3::new(-0.34270653, 0.112844266, -1.1643773), - Point3::new(-0.34270653, 0.10713466, -1.1672425), - Point3::new(-0.33082184, 0.057740986, -1.1972622), - Point3::new(-0.27619198, 0.112844266, -1.1938667), - Point3::new(-0.32371044, 0.112844266, -1.1727407), - Point3::new(-0.30646503, 0.07911008, -1.1972622), - Point3::new(-0.27619198, 0.10604827, -1.1972622), - Point3::new(-0.34270653, 0.05646963, -1.1972622), - Point3::new(-0.33082184, 0.057740998, -1.1972622), - Point3::new(-0.34270653, 0.10713467, -1.1672425), - Point3::new(-0.31214952, 0.17935875, -1.6988182), - Point3::new(-0.3002659, 0.14816007, -1.7089504), - Point3::new(-0.2988932, 0.11284419, -1.7147822), - Point3::new(-0.34270653, 0.11284419, -1.694334), - Point3::new(-0.34270653, 0.17935875, -1.6845568), - Point3::new(-0.3002659, 0.14816007, -1.7089504), - Point3::new(-0.31214952, 0.17935875, -1.6988182), - Point3::new(-0.3041196, 0.17935875, -1.7055525), - Point3::new(-0.3041196, 0.17935875, -1.7055525), - Point3::new(-0.29330474, 0.17935875, -1.7115203), - Point3::new(-0.3002659, 0.14816007, -1.7089504), - Point3::new(-0.27619198, 0.17935875, -1.7156959), - Point3::new(-0.27619198, 0.15403715, -1.7149886), - Point3::new(-0.3002659, 0.14816007, -1.7089504), - Point3::new(-0.2933047, 0.17935875, -1.7115203), - Point3::new(-0.34270653, 0.11284423, -1.6628642), - Point3::new(-0.27619195, 0.11284423, -1.6628642), - Point3::new(-0.27619195, 0.17935872, -1.6628642), - Point3::new(-0.34270653, 0.17935872, -1.6628642), - Point3::new(-0.34270653, 0.11284423, -1.5963496), - Point3::new(-0.27619195, 0.11284423, -1.5963496), - Point3::new(-0.27619195, 0.17935872, -1.5963496), - Point3::new(-0.34270653, 0.17935872, -1.5963496), - Point3::new(-0.34270653, 0.11284423, -1.5963496), - Point3::new(-0.27619195, 0.11284423, -1.5963496), - Point3::new(-0.27619195, 0.17935872, -1.5963496), - Point3::new(-0.34270653, 0.17935872, -1.5963496), - Point3::new(-0.34270653, 0.11284423, -1.529835), - Point3::new(-0.27619195, 0.11284423, -1.529835), - Point3::new(-0.27619195, 0.17935872, -1.529835), - Point3::new(-0.34270653, 0.17935872, -1.529835), - Point3::new(-0.34270653, 0.11284423, -1.529835), - Point3::new(-0.27619195, 0.11284423, -1.529835), - Point3::new(-0.27619195, 0.17935872, -1.529835), - Point3::new(-0.34270653, 0.17935872, -1.529835), - Point3::new(-0.34270653, 0.11284423, -1.4633205), - Point3::new(-0.27619195, 0.11284423, -1.4633205), - Point3::new(-0.27619195, 0.17935872, -1.4633205), - Point3::new(-0.34270653, 0.17935872, -1.4633205), - Point3::new(-0.34270653, 0.11284423, -1.4633205), - Point3::new(-0.27619195, 0.11284423, -1.4633205), - Point3::new(-0.27619195, 0.17935872, -1.4633205), - Point3::new(-0.34270653, 0.17935872, -1.4633205), - Point3::new(-0.34270653, 0.11284423, -1.396806), - Point3::new(-0.27619195, 0.11284423, -1.396806), - Point3::new(-0.27619195, 0.17935872, -1.396806), - Point3::new(-0.34270653, 0.17935872, -1.396806), - Point3::new(-0.34270653, 0.11284423, -1.396806), - Point3::new(-0.27619195, 0.11284423, -1.396806), - Point3::new(-0.27619195, 0.17935872, -1.396806), - Point3::new(-0.34270653, 0.17935872, -1.396806), - Point3::new(-0.34270653, 0.11284423, -1.3302914), - Point3::new(-0.27619195, 0.11284423, -1.3302914), - Point3::new(-0.27619195, 0.17935872, -1.3302914), - Point3::new(-0.34270653, 0.17935872, -1.3302914), - Point3::new(-0.34270653, 0.11284423, -1.3302914), - Point3::new(-0.27619195, 0.11284423, -1.3302914), - Point3::new(-0.27619195, 0.17935872, -1.3302914), - Point3::new(-0.34270653, 0.17935872, -1.3302914), - Point3::new(-0.34270653, 0.11284423, -1.2637768), - Point3::new(-0.27619195, 0.11284423, -1.2637768), - Point3::new(-0.27619195, 0.17935872, -1.2637768), - Point3::new(-0.34270653, 0.17935872, -1.2637768), - Point3::new(-0.34270653, 0.11284423, -1.2637768), - Point3::new(-0.27619195, 0.11284423, -1.2637768), - Point3::new(-0.27619195, 0.17935872, -1.2637768), - Point3::new(-0.34270653, 0.17935872, -1.2637768), - Point3::new(-0.34270653, 0.11284423, -1.1972623), - Point3::new(-0.27619195, 0.11284423, -1.1972623), - Point3::new(-0.27619195, 0.17935872, -1.1972623), - Point3::new(-0.34270653, 0.17935872, -1.1972623), - Point3::new(-0.3237104, 0.11284419, -1.1727407), - Point3::new(-0.34270653, 0.15000299, -1.1457299), - Point3::new(-0.34270653, 0.11284419, -1.1643773), - Point3::new(-0.34270653, 0.17935875, -1.1310631), - Point3::new(-0.34270653, 0.15000299, -1.1457299), - Point3::new(-0.3237104, 0.11284419, -1.1727407), - Point3::new(-0.27619198, 0.11284419, -1.1938668), - Point3::new(-0.27619198, 0.17935875, -1.1606346), - Point3::new(-0.2096774, -0.18912837, -1.8096728), - Point3::new(-0.21596254, -0.15321395, -1.797151), - Point3::new(-0.2096774, -0.15321395, -1.797623), - Point3::new(-0.2096774, -0.2197285, -1.8172805), - Point3::new(-0.24709052, -0.2197285, -1.7958934), - Point3::new(-0.2181624, -0.15321395, -1.7958934), - Point3::new(-0.21596254, -0.15321395, -1.797151), - Point3::new(-0.2096774, -0.18912835, -1.8096728), - Point3::new(-0.24709073, -0.2197285, -1.7958933), - Point3::new(-0.2692204, -0.2197285, -1.783243), - Point3::new(-0.27619195, -0.20534968, -1.7756828), - Point3::new(-0.27619195, -0.15321395, -1.7627211), - Point3::new(-0.21816261, -0.15321395, -1.7958933), - Point3::new(-0.27619195, -0.2197285, -1.7805436), - Point3::new(-0.27619195, -0.20534968, -1.7756828), - Point3::new(-0.2692204, -0.2197285, -1.783243), - Point3::new(-0.27619195, -0.21972859, -1.7293787), - Point3::new(-0.2096774, -0.21972859, -1.7293787), - Point3::new(-0.2096774, -0.15321398, -1.7293787), - Point3::new(-0.27619195, -0.15321398, -1.7293787), - Point3::new(-0.27619195, -0.21972859, -1.6628642), - Point3::new(-0.2096774, -0.21972859, -1.6628642), - Point3::new(-0.2096774, -0.15321398, -1.6628642), - Point3::new(-0.27619195, -0.15321398, -1.6628642), - Point3::new(-0.27619195, -0.21972859, -1.6628642), - Point3::new(-0.2096774, -0.21972859, -1.6628642), - Point3::new(-0.2096774, -0.15321398, -1.6628642), - Point3::new(-0.27619195, -0.15321398, -1.6628642), - Point3::new(-0.27619195, -0.21972859, -1.5963496), - Point3::new(-0.2096774, -0.21972859, -1.5963496), - Point3::new(-0.2096774, -0.15321398, -1.5963496), - Point3::new(-0.27619195, -0.15321398, -1.5963496), - Point3::new(-0.27619195, -0.21972859, -1.5963496), - Point3::new(-0.2096774, -0.21972859, -1.5963496), - Point3::new(-0.2096774, -0.15321398, -1.5963496), - Point3::new(-0.27619195, -0.15321398, -1.5963496), - Point3::new(-0.27619195, -0.21972859, -1.529835), - Point3::new(-0.2096774, -0.21972859, -1.529835), - Point3::new(-0.2096774, -0.15321398, -1.529835), - Point3::new(-0.27619195, -0.15321398, -1.529835), - Point3::new(-0.27619195, -0.21972859, -1.529835), - Point3::new(-0.2096774, -0.21972859, -1.529835), - Point3::new(-0.2096774, -0.15321398, -1.529835), - Point3::new(-0.27619195, -0.15321398, -1.529835), - Point3::new(-0.27619195, -0.21972859, -1.4633205), - Point3::new(-0.2096774, -0.21972859, -1.4633205), - Point3::new(-0.2096774, -0.15321398, -1.4633205), - Point3::new(-0.27619195, -0.15321398, -1.4633205), - Point3::new(-0.20989056, -0.18094835, -1.396806), - Point3::new(-0.20977727, -0.2197285, -1.4247218), - Point3::new(-0.2096774, -0.2197285, -1.4247513), - Point3::new(-0.2096774, -0.1808609, -1.396806), - Point3::new(-0.25887728, -0.2197285, -1.396806), - Point3::new(-0.20977727, -0.2197285, -1.4247218), - Point3::new(-0.20989056, -0.18094835, -1.396806), - Point3::new(-0.2096774, -0.15321395, -1.3769282), - Point3::new(-0.20997159, -0.15321395, -1.3768414), - Point3::new(-0.20989056, -0.18094818, -1.3968059), - Point3::new(-0.2096774, -0.18086074, -1.3968059), - Point3::new(-0.27619195, -0.15321395, -1.3391919), - Point3::new(-0.27619195, -0.17143634, -1.352279), - Point3::new(-0.26495442, -0.2197285, -1.3933508), - Point3::new(-0.2588775, -0.2197285, -1.3968059), - Point3::new(-0.20989056, -0.1809482, -1.3968059), - Point3::new(-0.20997159, -0.15321395, -1.3768414), - Point3::new(-0.27619195, -0.17143634, -1.352279), - Point3::new(-0.27619195, -0.2197285, -1.3937997), - Point3::new(-0.2649544, -0.2197285, -1.393351), - Point3::new(-0.2096774, -0.14805892, -1.7958934), - Point3::new(-0.2096774, -0.15321401, -1.797623), - Point3::new(-0.21596253, -0.15321401, -1.7971511), - Point3::new(-0.21659379, -0.14960687, -1.7958934), - Point3::new(-0.21659379, -0.14960688, -1.7958934), - Point3::new(-0.21596253, -0.15321401, -1.7971511), - Point3::new(-0.21816258, -0.15321401, -1.7958934), - Point3::new(-0.22760281, -0.08669945, -1.7739602), - Point3::new(-0.2096774, -0.08669945, -1.7753065), - Point3::new(-0.2096774, -0.14805856, -1.7958933), - Point3::new(-0.21659385, -0.14960653, -1.7958933), - Point3::new(-0.21659385, -0.14960653, -1.7958933), - Point3::new(-0.21816279, -0.15321401, -1.7958933), - Point3::new(-0.27619195, -0.15321401, -1.7627211), - Point3::new(-0.27619195, -0.1506861, -1.7620926), - Point3::new(-0.25336736, -0.08669945, -1.759232), - Point3::new(-0.22760281, -0.08669945, -1.7739602), - Point3::new(-0.2096774, -0.1415204, -1.3689709), - Point3::new(-0.2096774, -0.13848218, -1.3677686), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.2096774, -0.14281492, -1.3697803), - Point3::new(-0.2096774, -0.1415204, -1.3689709), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.2096774, -0.08669945, -1.3487011), - Point3::new(-0.22550425, -0.08669945, -1.3302914), - Point3::new(-0.2279999, -0.09458314, -1.3302914), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.2096774, -0.1384822, -1.3677686), - Point3::new(-0.2096774, -0.14527045, -1.3712171), - Point3::new(-0.2096774, -0.14281493, -1.3697803), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.2096774, -0.14527047, -1.371217), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.20997159, -0.15321401, -1.3768414), - Point3::new(-0.2096774, -0.15321401, -1.3769283), - Point3::new(-0.21000507, -0.14175473, -1.3685925), - Point3::new(-0.253579, -0.12291954, -1.3302914), - Point3::new(-0.27619195, -0.14082097, -1.3302914), - Point3::new(-0.27619195, -0.15321401, -1.3391919), - Point3::new(-0.20997159, -0.15321401, -1.3768414), - Point3::new(-0.22550434, -0.08669945, -1.3302913), - Point3::new(-0.23100734, -0.08669945, -1.3238902), - Point3::new(-0.22799996, -0.09458299, -1.3302913), - Point3::new(-0.25357914, -0.12291948, -1.3302913), - Point3::new(-0.27619195, -0.1131449, -1.3104148), - Point3::new(-0.27619195, -0.1408208, -1.3302913), - Point3::new(-0.23608275, -0.038243636, -1.7570658), - Point3::new(-0.2096774, -0.039554533, -1.7594886), - Point3::new(-0.2096774, -0.086699404, -1.7753063), - Point3::new(-0.22760282, -0.086699404, -1.7739602), - Point3::new(-0.25336733, -0.086699404, -1.759232), - Point3::new(-0.23608275, -0.038243636, -1.7570658), - Point3::new(-0.22760282, -0.086699404, -1.7739602), - Point3::new(-0.14053856, -0.042986937, -1.7658323), - Point3::new(-0.22550425, -0.086699404, -1.3302914), - Point3::new(-0.2096774, -0.086699404, -1.3487011), - Point3::new(-0.2096774, -0.036703166, -1.3302914), - Point3::new(-0.2096774, -0.020184837, -1.324209), - Point3::new(-0.25638106, -0.020184837, -1.2698835), - Point3::new(-0.23100735, -0.086699404, -1.3238902), - Point3::new(-0.22550435, -0.086699404, -1.3302913), - Point3::new(-0.2096774, -0.036702838, -1.3302913), - Point3::new(-0.27619195, -0.06461145, -1.2827141), - Point3::new(-0.26024473, -0.020184837, -1.2674407), - Point3::new(-0.26856667, -0.020184835, -1.2637768), - Point3::new(-0.27619195, -0.026874721, -1.2637768), - Point3::new(-0.27619195, -0.026874721, -1.2637768), - Point3::new(-0.26856667, -0.020184835, -1.2637768), - Point3::new(-0.27619195, -0.020184835, -1.2604196), - Point3::new(-0.25837165, -0.014966724, -1.2656467), - Point3::new(-0.23523012, 0.046329774, -1.269994), - Point3::new(-0.23869139, 0.046329774, -1.2637768), - Point3::new(-0.2575794, -0.009543579, -1.2637768), - Point3::new(-0.23523012, 0.046329774, -1.269994), - Point3::new(-0.25837165, -0.014966724, -1.2656467), - Point3::new(-0.2563811, -0.020184793, -1.2698835), - Point3::new(-0.2096774, -0.020184793, -1.324209), - Point3::new(-0.2096774, 0.046329774, -1.2997168), - Point3::new(-0.26856652, -0.02018479, -1.2637768), - Point3::new(-0.2602447, -0.020184793, -1.2674407), - Point3::new(-0.25837165, -0.014966724, -1.2656467), - Point3::new(-0.25968674, -0.0123942755, -1.2637768), - Point3::new(-0.25837165, -0.014966724, -1.2656467), - Point3::new(-0.2585028, -0.011340788, -1.2637768), - Point3::new(-0.2596867, -0.012394279, -1.2637768), - Point3::new(-0.2585028, -0.0113407895, -1.2637768), - Point3::new(-0.25837165, -0.014966724, -1.2656467), - Point3::new(-0.2575794, -0.009543577, -1.2637768), - Point3::new(-0.2575794, -0.009543579, -1.2637768), - Point3::new(-0.23869139, 0.046329774, -1.2637768), - Point3::new(-0.2494168, 0.046329767, -1.2445115), - Point3::new(-0.26856652, -0.02018479, -1.2637768), - Point3::new(-0.25968674, -0.0123942755, -1.2637768), - Point3::new(-0.27619195, 0.019892039, -1.2403078), - Point3::new(-0.27619195, -0.02018479, -1.2604195), - Point3::new(-0.2585028, -0.011340788, -1.2637768), - Point3::new(-0.26058903, 0.046329774, -1.2340357), - Point3::new(-0.27619195, 0.046329767, -1.227099), - Point3::new(-0.27619195, 0.019892037, -1.2403078), - Point3::new(-0.2596867, -0.012394279, -1.2637768), - Point3::new(-0.2494168, 0.046329774, -1.2445115), - Point3::new(-0.26058903, 0.04632978, -1.2340357), - Point3::new(-0.2585028, -0.0113407895, -1.2637768), - Point3::new(-0.2575794, -0.009543577, -1.2637768), - Point3::new(-0.23523015, 0.0463297, -1.269994), - Point3::new(-0.21011862, 0.112844266, -1.2747115), - Point3::new(-0.21620622, 0.112844266, -1.2637768), - Point3::new(-0.23869143, 0.0463297, -1.2637768), - Point3::new(-0.2096774, 0.112844266, -1.2752247), - Point3::new(-0.2101186, 0.112844266, -1.2747115), - Point3::new(-0.23523015, 0.0463297, -1.269994), - Point3::new(-0.2096774, 0.046329692, -1.2997168), - Point3::new(-0.23869143, 0.0463297, -1.2637768), - Point3::new(-0.21620622, 0.112844266, -1.2637768), - Point3::new(-0.23969963, 0.11284426, -1.2215773), - Point3::new(-0.2494168, 0.046329707, -1.2445116), - Point3::new(-0.26299515, 0.112844266, -1.199734), - Point3::new(-0.26855454, 0.112844266, -1.1972623), - Point3::new(-0.27619195, 0.106048025, -1.1972623), - Point3::new(-0.27619195, 0.046329707, -1.227099), - Point3::new(-0.26058903, 0.0463297, -1.2340358), - Point3::new(-0.26299515, 0.11284427, -1.199734), - Point3::new(-0.26058903, 0.046329692, -1.2340358), - Point3::new(-0.24941681, 0.0463297, -1.2445116), - Point3::new(-0.23969965, 0.112844266, -1.2215773), - Point3::new(-0.2685548, 0.112844266, -1.1972622), - Point3::new(-0.27619195, 0.11284426, -1.1938667), - Point3::new(-0.27619195, 0.10604826, -1.1972622), - Point3::new(-0.2096774, 0.17935875, -1.7319256), - Point3::new(-0.2096774, 0.17027512, -1.7316719), - Point3::new(-0.21881983, 0.16804321, -1.7293788), - Point3::new(-0.220115, 0.17935875, -1.7293788), - Point3::new(-0.22011548, 0.17935875, -1.7293787), - Point3::new(-0.2188203, 0.16804309, -1.7293787), - Point3::new(-0.27619195, 0.15403716, -1.7149887), - Point3::new(-0.27619195, 0.17935875, -1.715696), - Point3::new(-0.27619195, 0.11284423, -1.6628642), - Point3::new(-0.2096774, 0.11284423, -1.6628642), - Point3::new(-0.2096774, 0.17935872, -1.6628642), - Point3::new(-0.27619195, 0.17935872, -1.6628642), - Point3::new(-0.27619195, 0.11284423, -1.5963496), - Point3::new(-0.2096774, 0.11284423, -1.5963496), - Point3::new(-0.2096774, 0.17935872, -1.5963496), - Point3::new(-0.27619195, 0.17935872, -1.5963496), - Point3::new(-0.27619195, 0.11284423, -1.5963496), - Point3::new(-0.2096774, 0.11284423, -1.5963496), - Point3::new(-0.2096774, 0.17935872, -1.5963496), - Point3::new(-0.27619195, 0.17935872, -1.5963496), - Point3::new(-0.27619195, 0.11284423, -1.529835), - Point3::new(-0.2096774, 0.11284423, -1.529835), - Point3::new(-0.2096774, 0.17935872, -1.529835), - Point3::new(-0.27619195, 0.17935872, -1.529835), - Point3::new(-0.27619195, 0.11284423, -1.529835), - Point3::new(-0.2096774, 0.11284423, -1.529835), - Point3::new(-0.2096774, 0.17935872, -1.529835), - Point3::new(-0.27619195, 0.17935872, -1.529835), - Point3::new(-0.27619195, 0.11284423, -1.4633205), - Point3::new(-0.2096774, 0.11284423, -1.4633205), - Point3::new(-0.2096774, 0.17935872, -1.4633205), - Point3::new(-0.27619195, 0.17935872, -1.4633205), - Point3::new(-0.27619195, 0.11284423, -1.4633205), - Point3::new(-0.2096774, 0.11284423, -1.4633205), - Point3::new(-0.2096774, 0.17935872, -1.4633205), - Point3::new(-0.27619195, 0.17935872, -1.4633205), - Point3::new(-0.27619195, 0.11284423, -1.396806), - Point3::new(-0.2096774, 0.11284423, -1.396806), - Point3::new(-0.2096774, 0.17935872, -1.396806), - Point3::new(-0.27619195, 0.17935872, -1.396806), - Point3::new(-0.27619195, 0.11284423, -1.396806), - Point3::new(-0.2096774, 0.11284423, -1.396806), - Point3::new(-0.2096774, 0.17935872, -1.396806), - Point3::new(-0.27619195, 0.17935872, -1.396806), - Point3::new(-0.27619195, 0.11284423, -1.3302914), - Point3::new(-0.2096774, 0.11284423, -1.3302914), - Point3::new(-0.2096774, 0.17935872, -1.3302914), - Point3::new(-0.27619195, 0.17935872, -1.3302914), - Point3::new(-0.21620624, 0.112844184, -1.2637768), - Point3::new(-0.21011865, 0.112844184, -1.2747115), - Point3::new(-0.2096774, 0.11401294, -1.2747943), - Point3::new(-0.2096774, 0.13215737, -1.2637768), - Point3::new(-0.2096774, 0.112844184, -1.2752247), - Point3::new(-0.2096774, 0.11401294, -1.2747943), - Point3::new(-0.21011864, 0.11284419, -1.2747115), - Point3::new(-0.21620624, 0.112844184, -1.2637768), - Point3::new(-0.2096774, 0.13215737, -1.2637768), - Point3::new(-0.2096774, 0.17935875, -1.2351154), - Point3::new(-0.22998248, 0.17935875, -1.1986428), - Point3::new(-0.23969965, 0.1128442, -1.2215772), - Point3::new(-0.26855466, 0.11284419, -1.1972623), - Point3::new(-0.26299515, 0.112844184, -1.199734), - Point3::new(-0.26316854, 0.117637016, -1.1972623), - Point3::new(-0.26316854, 0.11763701, -1.1972623), - Point3::new(-0.26299515, 0.112844184, -1.199734), - Point3::new(-0.23969965, 0.112844184, -1.2215772), - Point3::new(-0.22998248, 0.17935875, -1.1986428), - Point3::new(-0.23145483, 0.17935875, -1.1972623), - Point3::new(-0.2654013, 0.17935875, -1.1654321), - Point3::new(-0.27619195, 0.17935875, -1.1606346), - Point3::new(-0.27619195, 0.1128442, -1.1938668), - Point3::new(-0.26855493, 0.11284419, -1.1972622), - Point3::new(-0.26316854, 0.11763725, -1.1972622), - Point3::new(-0.26316854, 0.11763725, -1.1972622), - Point3::new(-0.23145495, 0.17935875, -1.1972622), - Point3::new(-0.2654013, 0.17935875, -1.1654321), - Point3::new(-0.1482815, -0.15321395, -1.8022337), - Point3::new(-0.15295385, -0.2197285, -1.8241994), - Point3::new(-0.20432228, -0.2197285, -1.8203418), - Point3::new(-0.20967737, -0.18912855, -1.809673), - Point3::new(-0.20967737, -0.15321395, -1.797623), - Point3::new(-0.20967737, -0.2197285, -1.8172807), - Point3::new(-0.20967737, -0.18912852, -1.809673), - Point3::new(-0.20432226, -0.2197285, -1.8203418), - Point3::new(-0.14316282, -0.15321395, -1.8006383), - Point3::new(-0.14316282, -0.2197285, -1.8211478), - Point3::new(-0.15295385, -0.21972848, -1.8241994), - Point3::new(-0.1482815, -0.15321395, -1.8022337), - Point3::new(-0.2096774, -0.21972859, -1.7958933), - Point3::new(-0.14316282, -0.21972859, -1.7958933), - Point3::new(-0.14316282, -0.15321398, -1.7958933), - Point3::new(-0.2096774, -0.15321398, -1.7958933), - Point3::new(-0.2096774, -0.21972859, -1.7293787), - Point3::new(-0.14316282, -0.21972859, -1.7293787), - Point3::new(-0.14316282, -0.15321398, -1.7293787), - Point3::new(-0.2096774, -0.15321398, -1.7293787), - Point3::new(-0.2096774, -0.21972859, -1.7293787), - Point3::new(-0.14316282, -0.21972859, -1.7293787), - Point3::new(-0.14316282, -0.15321398, -1.7293787), - Point3::new(-0.2096774, -0.15321398, -1.7293787), - Point3::new(-0.2096774, -0.21972859, -1.6628642), - Point3::new(-0.14316282, -0.21972859, -1.6628642), - Point3::new(-0.14316282, -0.15321398, -1.6628642), - Point3::new(-0.2096774, -0.15321398, -1.6628642), - Point3::new(-0.2096774, -0.21972859, -1.6628642), - Point3::new(-0.14316282, -0.21972859, -1.6628642), - Point3::new(-0.14316282, -0.15321398, -1.6628642), - Point3::new(-0.2096774, -0.15321398, -1.6628642), - Point3::new(-0.2096774, -0.21972859, -1.5963496), - Point3::new(-0.14316282, -0.21972859, -1.5963496), - Point3::new(-0.14316282, -0.15321398, -1.5963496), - Point3::new(-0.2096774, -0.15321398, -1.5963496), - Point3::new(-0.2096774, -0.21972859, -1.5963496), - Point3::new(-0.14316282, -0.21972859, -1.5963496), - Point3::new(-0.14316282, -0.15321398, -1.5963496), - Point3::new(-0.2096774, -0.15321398, -1.5963496), - Point3::new(-0.2096774, -0.21972859, -1.529835), - Point3::new(-0.14316282, -0.21972859, -1.529835), - Point3::new(-0.14316282, -0.15321398, -1.529835), - Point3::new(-0.2096774, -0.15321398, -1.529835), - Point3::new(-0.18130249, -0.2197285, -1.4633205), - Point3::new(-0.14316282, -0.2197285, -1.5244126), - Point3::new(-0.14316282, -0.15321395, -1.4828252), - Point3::new(-0.15533954, -0.15321395, -1.4633205), - Point3::new(-0.20222148, -0.16693833, -1.396806), - Point3::new(-0.18590543, -0.2197285, -1.4559475), - Point3::new(-0.18130249, -0.2197285, -1.4633205), - Point3::new(-0.15533954, -0.15321395, -1.4633205), - Point3::new(-0.19686438, -0.15321395, -1.396806), - Point3::new(-0.20648265, -0.17954761, -1.396806), - Point3::new(-0.20273766, -0.2197285, -1.4268022), - Point3::new(-0.18590543, -0.2197285, -1.4559475), - Point3::new(-0.20222148, -0.16693833, -1.396806), - Point3::new(-0.20967737, -0.18086074, -1.396806), - Point3::new(-0.20967737, -0.2197285, -1.4247514), - Point3::new(-0.20273766, -0.2197285, -1.426802), - Point3::new(-0.20648263, -0.17954776, -1.396806), - Point3::new(-0.20222151, -0.16693823, -1.3968059), - Point3::new(-0.19686446, -0.15321395, -1.3968059), - Point3::new(-0.20646334, -0.15321395, -1.3814304), - Point3::new(-0.20646334, -0.15321395, -1.3814304), - Point3::new(-0.20893703, -0.15321395, -1.3771472), - Point3::new(-0.20648266, -0.17954744, -1.3968059), - Point3::new(-0.20222151, -0.16693823, -1.3968059), - Point3::new(-0.20893703, -0.15321395, -1.3771471), - Point3::new(-0.20967737, -0.15321395, -1.3769283), - Point3::new(-0.20967737, -0.18086058, -1.3968059), - Point3::new(-0.20648265, -0.17954761, -1.3968059), - Point3::new(-0.14693287, -0.13401513, -1.7958934), - Point3::new(-0.1482815, -0.15321401, -1.8022337), - Point3::new(-0.20967737, -0.15321401, -1.797623), - Point3::new(-0.20967737, -0.1480589, -1.7958934), - Point3::new(-0.14316282, -0.13782589, -1.7958934), - Point3::new(-0.14316282, -0.15321401, -1.8006383), - Point3::new(-0.1482815, -0.15321401, -1.8022337), - Point3::new(-0.14693287, -0.13401513, -1.7958934), - Point3::new(-0.20967737, -0.08669945, -1.7753065), - Point3::new(-0.14360917, -0.08669945, -1.7802678), - Point3::new(-0.14693284, -0.13401476, -1.7958933), - Point3::new(-0.20967737, -0.14805856, -1.7958933), - Point3::new(-0.14316282, -0.1378255, -1.7958933), - Point3::new(-0.14693284, -0.13401477, -1.7958933), - Point3::new(-0.14360917, -0.08669945, -1.7802678), - Point3::new(-0.14316282, -0.08669945, -1.7801287), - Point3::new(-0.1553396, -0.15321401, -1.4633205), - Point3::new(-0.14316282, -0.15321401, -1.4828253), - Point3::new(-0.14316282, -0.122018315, -1.4633205), - Point3::new(-0.14316282, -0.093953855, -1.4457735), - Point3::new(-0.14316282, -0.08669945, -1.4429029), - Point3::new(-0.17522681, -0.086699456, -1.396806), - Point3::new(-0.18557088, -0.124281116, -1.396806), - Point3::new(-0.19686438, -0.15321401, -1.396806), - Point3::new(-0.1553396, -0.15321401, -1.4633205), - Point3::new(-0.14316282, -0.122018315, -1.4633205), - Point3::new(-0.14316282, -0.093953855, -1.4457735), - Point3::new(-0.18557088, -0.124281116, -1.396806), - Point3::new(-0.18557099, -0.12428119, -1.3968059), - Point3::new(-0.1752269, -0.086699456, -1.3968059), - Point3::new(-0.20449243, -0.086699456, -1.3547322), - Point3::new(-0.20967737, -0.13848189, -1.3677685), - Point3::new(-0.20967737, -0.14152038, -1.3689709), - Point3::new(-0.20967737, -0.14281502, -1.3697803), - Point3::new(-0.20646332, -0.15321401, -1.3814304), - Point3::new(-0.19686446, -0.15321401, -1.3968059), - Point3::new(-0.18557099, -0.12428119, -1.3968059), - Point3::new(-0.20967737, -0.14152038, -1.3689709), - Point3::new(-0.20449243, -0.08669945, -1.3547322), - Point3::new(-0.20967737, -0.086699456, -1.3487011), - Point3::new(-0.20967737, -0.1384819, -1.3677685), - Point3::new(-0.20967737, -0.14281502, -1.3697803), - Point3::new(-0.20967737, -0.14527076, -1.3712174), - Point3::new(-0.20893703, -0.15321401, -1.3771472), - Point3::new(-0.2064633, -0.15321401, -1.3814304), - Point3::new(-0.20893703, -0.15321404, -1.3771471), - Point3::new(-0.20967737, -0.14527076, -1.3712173), - Point3::new(-0.20967737, -0.15321401, -1.3769283), - Point3::new(-0.14316282, -0.042856656, -1.7655915), - Point3::new(-0.14316282, -0.08034536, -1.7781695), - Point3::new(-0.14360917, -0.086699404, -1.7802678), - Point3::new(-0.20967737, -0.086699404, -1.7753065), - Point3::new(-0.20967737, -0.039554533, -1.7594886), - Point3::new(-0.14316282, -0.08034536, -1.7781695), - Point3::new(-0.14316282, -0.086699404, -1.7801287), - Point3::new(-0.14360917, -0.0866994, -1.7802678), - Point3::new(-0.1353515, 0.16140196, -1.7255667), - Point3::new(-0.17522681, -0.0866994, -1.396806), - Point3::new(-0.14316282, -0.086699404, -1.4429029), - Point3::new(-0.14316282, -0.032543883, -1.4214734), - Point3::new(-0.14483222, -0.020184837, -1.4141828), - Point3::new(-0.15691914, -0.02018484, -1.396806), - Point3::new(-0.14316282, -0.032543883, -1.4214734), - Point3::new(-0.14316282, -0.020184837, -1.4174645), - Point3::new(-0.14483222, -0.020184845, -1.4141828), - Point3::new(-0.1752269, -0.0866994, -1.3968059), - Point3::new(-0.15691921, -0.02018484, -1.3968059), - Point3::new(-0.19783239, -0.020184845, -1.337987), - Point3::new(-0.20449243, -0.0866994, -1.3547322), - Point3::new(-0.20967737, -0.03670317, -1.3302914), - Point3::new(-0.20967737, -0.0866994, -1.3487011), - Point3::new(-0.20449243, -0.08669941, -1.3547322), - Point3::new(-0.19783239, -0.020184837, -1.337987), - Point3::new(-0.2044483, -0.020184841, -1.3302914), - Point3::new(-0.20967737, -0.03670284, -1.3302913), - Point3::new(-0.20444839, -0.020184841, -1.3302913), - Point3::new(-0.20967737, -0.020184845, -1.324209), - Point3::new(-0.15691915, -0.02018479, -1.396806), - Point3::new(-0.14483224, -0.020184793, -1.4141828), - Point3::new(-0.14881122, 0.009272579, -1.396806), - Point3::new(-0.14881122, 0.009272579, -1.396806), - Point3::new(-0.14483224, -0.020184785, -1.4141828), - Point3::new(-0.14316282, -0.020184793, -1.4174645), - Point3::new(-0.14316282, 0.043503255, -1.396806), - Point3::new(-0.19477163, 0.010383148, -1.3302914), - Point3::new(-0.19783238, -0.020184785, -1.337987), - Point3::new(-0.15691923, -0.02018479, -1.3968059), - Point3::new(-0.14881125, 0.00927278, -1.3968059), - Point3::new(-0.15381677, 0.046329774, -1.3749461), - Point3::new(-0.1848776, 0.046329767, -1.3302914), - Point3::new(-0.14316282, 0.046329774, -1.3958892), - Point3::new(-0.15381677, 0.046329767, -1.3749461), - Point3::new(-0.14881125, 0.00927278, -1.3968059), - Point3::new(-0.14316282, 0.04350362, -1.3968059), - Point3::new(-0.20444828, -0.020184789, -1.3302914), - Point3::new(-0.19783238, -0.020184793, -1.337987), - Point3::new(-0.19477163, 0.010383146, -1.3302914), - Point3::new(-0.19477159, 0.010383621, -1.3302913), - Point3::new(-0.18487768, 0.046329767, -1.3302913), - Point3::new(-0.19117233, 0.046329767, -1.3212417), - Point3::new(-0.20967737, 0.046329774, -1.2997168), - Point3::new(-0.20967737, -0.020184785, -1.324209), - Point3::new(-0.20444839, -0.020184789, -1.3302913), - Point3::new(-0.19477159, 0.010383619, -1.3302913), - Point3::new(-0.19117233, 0.046329774, -1.3212417), - Point3::new(-0.1848776, 0.046329707, -1.3302914), - Point3::new(-0.15381676, 0.0463297, -1.3749461), - Point3::new(-0.1628013, 0.112844266, -1.3357095), - Point3::new(-0.16656998, 0.112844266, -1.3302914), - Point3::new(-0.14334899, 0.112844266, -1.373948), - Point3::new(-0.1628013, 0.112844266, -1.3357095), - Point3::new(-0.15381676, 0.046329707, -1.3749461), - Point3::new(-0.14316282, 0.0463297, -1.3958893), - Point3::new(-0.14316282, 0.11230591, -1.3744886), - Point3::new(-0.14316282, 0.112844266, -1.3743398), - Point3::new(-0.14334898, 0.112844266, -1.373948), - Point3::new(-0.14316282, 0.11230593, -1.3744886), - Point3::new(-0.1845123, 0.112844266, -1.3044965), - Point3::new(-0.19117235, 0.046329707, -1.3212417), - Point3::new(-0.1848777, 0.046329707, -1.3302913), - Point3::new(-0.16657007, 0.112844266, -1.3302913), - Point3::new(-0.20967737, 0.112844266, -1.2752247), - Point3::new(-0.20967737, 0.0463297, -1.2997168), - Point3::new(-0.19117235, 0.046329692, -1.3212417), - Point3::new(-0.1845123, 0.112844266, -1.3044965), - Point3::new(-0.2045752, 0.17152071, -1.7329516), - Point3::new(-0.20536505, 0.17935875, -1.7329779), - Point3::new(-0.2029104, 0.17935875, -1.7331995), - Point3::new(-0.2045752, 0.17152071, -1.7329516), - Point3::new(-0.20291038, 0.17935875, -1.7331995), - Point3::new(-0.16431268, 0.17935875, -1.7293788), - Point3::new(-0.17108496, 0.16662528, -1.7293788), - Point3::new(-0.20967737, 0.17935875, -1.7319256), - Point3::new(-0.20536505, 0.17935875, -1.7329779), - Point3::new(-0.2045752, 0.17152071, -1.7329516), - Point3::new(-0.20967737, 0.17027514, -1.7316719), - Point3::new(-0.17108384, 0.16662511, -1.7293787), - Point3::new(-0.16431147, 0.17935875, -1.7293787), - Point3::new(-0.14316282, 0.17935875, -1.7272853), - Point3::new(-0.14316282, 0.16254377, -1.7264), - Point3::new(-0.2096774, 0.11284423, -1.6628642), - Point3::new(-0.14316282, 0.11284423, -1.6628642), - Point3::new(-0.14316282, 0.17935872, -1.6628642), - Point3::new(-0.2096774, 0.17935872, -1.6628642), - Point3::new(-0.2096774, 0.11284423, -1.5963496), - Point3::new(-0.14316282, 0.11284423, -1.5963496), - Point3::new(-0.14316282, 0.17935872, -1.5963496), - Point3::new(-0.2096774, 0.17935872, -1.5963496), - Point3::new(-0.2096774, 0.11284423, -1.5963496), - Point3::new(-0.14316282, 0.11284423, -1.5963496), - Point3::new(-0.14316282, 0.17935872, -1.5963496), - Point3::new(-0.2096774, 0.17935872, -1.5963496), - Point3::new(-0.2096774, 0.11284423, -1.529835), - Point3::new(-0.14316282, 0.11284423, -1.529835), - Point3::new(-0.14316282, 0.17935872, -1.529835), - Point3::new(-0.2096774, 0.17935872, -1.529835), - Point3::new(-0.2096774, 0.11284423, -1.529835), - Point3::new(-0.14316282, 0.11284423, -1.529835), - Point3::new(-0.14316282, 0.17935872, -1.529835), - Point3::new(-0.2096774, 0.17935872, -1.529835), - Point3::new(-0.2096774, 0.11284423, -1.4633205), - Point3::new(-0.14316282, 0.11284423, -1.4633205), - Point3::new(-0.14316282, 0.17935872, -1.4633205), - Point3::new(-0.2096774, 0.17935872, -1.4633205), - Point3::new(-0.2096774, 0.11284423, -1.4633205), - Point3::new(-0.14316282, 0.11284423, -1.4633205), - Point3::new(-0.14316282, 0.17935872, -1.4633205), - Point3::new(-0.2096774, 0.17935872, -1.4633205), - Point3::new(-0.2096774, 0.11284423, -1.396806), - Point3::new(-0.14316282, 0.11284423, -1.396806), - Point3::new(-0.14316282, 0.17935872, -1.396806), - Point3::new(-0.2096774, 0.17935872, -1.396806), - Point3::new(-0.16656998, 0.112844184, -1.3302914), - Point3::new(-0.1628013, 0.112844184, -1.3357095), - Point3::new(-0.16404194, 0.12202896, -1.3302914), - Point3::new(-0.16404194, 0.12202897, -1.3302914), - Point3::new(-0.1628013, 0.11284419, -1.3357095), - Point3::new(-0.14334896, 0.11284419, -1.3739481), - Point3::new(-0.15838355, 0.15632033, -1.3302914), - Point3::new(-0.15838355, 0.15632033, -1.3302914), - Point3::new(-0.14334896, 0.11284419, -1.3739481), - Point3::new(-0.14316282, 0.11284419, -1.3743399), - Point3::new(-0.14316282, 0.17935875, -1.3559638), - Point3::new(-0.15535964, 0.17935875, -1.3302914), - Point3::new(-0.17785226, 0.17935875, -1.2877513), - Point3::new(-0.1845123, 0.11284419, -1.3044965), - Point3::new(-0.16657007, 0.112844184, -1.3302913), - Point3::new(-0.16404197, 0.12202916, -1.3302913), - Point3::new(-0.17178583, 0.17935875, -1.2964728), - Point3::new(-0.16635051, 0.17935875, -1.3071573), - Point3::new(-0.17178583, 0.17935875, -1.2964728), - Point3::new(-0.16404197, 0.12202917, -1.3302913), - Point3::new(-0.15838358, 0.15632045, -1.3302913), - Point3::new(-0.15838358, 0.15632045, -1.3302913), - Point3::new(-0.15535969, 0.17935875, -1.3302913), - Point3::new(-0.16635051, 0.17935875, -1.3071573), - Point3::new(-0.18500711, 0.17935875, -1.2794288), - Point3::new(-0.19372095, 0.17935875, -1.2637768), - Point3::new(-0.20967737, 0.13215743, -1.2637768), - Point3::new(-0.20967737, 0.11401303, -1.2747943), - Point3::new(-0.20967737, 0.114013016, -1.2747943), - Point3::new(-0.20967737, 0.11284419, -1.2752247), - Point3::new(-0.1845123, 0.112844184, -1.3044965), - Point3::new(-0.17785226, 0.17935875, -1.2877513), - Point3::new(-0.18500711, 0.17935875, -1.2794288), - Point3::new(-0.19372095, 0.17935875, -1.2637768), - Point3::new(-0.20967737, 0.17935875, -1.2351154), - Point3::new(-0.20967737, 0.13215743, -1.2637768), - Point3::new(-0.13414398, -0.15321395, -1.7978274), - Point3::new(-0.1302853, -0.2197285, -1.8171343), - Point3::new(-0.14316282, -0.2197285, -1.8211478), - Point3::new(-0.14316282, -0.15321395, -1.8006383), - Point3::new(-0.10625099, -0.2197285, -1.7988122), - Point3::new(-0.104408056, -0.21520284, -1.7958934), - Point3::new(-0.10402099, -0.2197285, -1.7958934), - Point3::new(-0.11945202, -0.2197285, -1.8134716), - Point3::new(-0.112069346, -0.17105782, -1.7958934), - Point3::new(-0.104408056, -0.21520284, -1.7958934), - Point3::new(-0.10625099, -0.2197285, -1.7988122), - Point3::new(-0.1302853, -0.2197285, -1.8171343), - Point3::new(-0.13414398, -0.15321395, -1.7978274), - Point3::new(-0.12842394, -0.15321395, -1.7958934), - Point3::new(-0.11206935, -0.17105782, -1.7958934), - Point3::new(-0.11945203, -0.2197285, -1.8134716), - Point3::new(-0.077586554, -0.2197285, -1.7484213), - Point3::new(-0.07664826, -0.2152715, -1.7458801), - Point3::new(-0.07664826, -0.2197285, -1.7424797), - Point3::new(-0.07664826, -0.21527147, -1.7458801), - Point3::new(-0.07758656, -0.2197285, -1.7484213), - Point3::new(-0.078665845, -0.2197285, -1.7627075), - Point3::new(-0.07664826, -0.20939292, -1.7589098), - Point3::new(-0.10440798, -0.21520264, -1.7958933), - Point3::new(-0.07916506, -0.15321395, -1.7559152), - Point3::new(-0.07664826, -0.15321395, -1.7526212), - Point3::new(-0.07664826, -0.20939294, -1.7589098), - Point3::new(-0.078665845, -0.2197285, -1.7627075), - Point3::new(-0.1040209, -0.2197285, -1.7958933), - Point3::new(-0.112069294, -0.17105749, -1.7958933), - Point3::new(-0.10936268, -0.15321395, -1.7894489), - Point3::new(-0.07916506, -0.15321395, -1.7559152), - Point3::new(-0.10440798, -0.21520266, -1.7958933), - Point3::new(-0.1120693, -0.17105749, -1.7958933), - Point3::new(-0.12842359, -0.15321395, -1.7958933), - Point3::new(-0.109362684, -0.15321395, -1.7894489), - Point3::new(-0.08346283, -0.2197285, -1.6628642), - Point3::new(-0.07664826, -0.2197285, -1.685307), - Point3::new(-0.07664826, -0.19288324, -1.6628642), - Point3::new(-0.07664826, -0.15321395, -1.6297004), - Point3::new(-0.08166675, -0.15321395, -1.6131728), - Point3::new(-0.08588588, -0.2197285, -1.6548842), - Point3::new(-0.08346283, -0.2197285, -1.6628642), - Point3::new(-0.07664826, -0.19288324, -1.6628642), - Point3::new(-0.099511996, -0.2197285, -1.6080378), - Point3::new(-0.09313648, -0.17996536, -1.5963497), - Point3::new(-0.105319, -0.2197285, -1.5963497), - Point3::new(-0.09313648, -0.17996536, -1.5963497), - Point3::new(-0.099511996, -0.2197285, -1.6080378), - Point3::new(-0.08588588, -0.21972848, -1.6548842), - Point3::new(-0.08166675, -0.15321395, -1.6131728), - Point3::new(-0.086560026, -0.15321395, -1.5963497), - Point3::new(-0.13977757, -0.2197285, -1.5298351), - Point3::new(-0.13286145, -0.2197285, -1.5409133), - Point3::new(-0.13321237, -0.2029091, -1.5298351), - Point3::new(-0.093136415, -0.17996496, -1.5963496), - Point3::new(-0.08884723, -0.15321395, -1.5884863), - Point3::new(-0.1179869, -0.15321395, -1.5298351), - Point3::new(-0.13321237, -0.2029091, -1.5298351), - Point3::new(-0.13286145, -0.2197285, -1.5409133), - Point3::new(-0.10531906, -0.2197285, -1.5963496), - Point3::new(-0.093136415, -0.17996496, -1.5963496), - Point3::new(-0.086560056, -0.15321395, -1.5963496), - Point3::new(-0.08884723, -0.15321395, -1.5884863), - Point3::new(-0.14316282, -0.15321395, -1.4828252), - Point3::new(-0.14316282, -0.2197285, -1.5244126), - Point3::new(-0.13977765, -0.2197285, -1.529835), - Point3::new(-0.13321237, -0.20290892, -1.529835), - Point3::new(-0.13424921, -0.15321395, -1.497103), - Point3::new(-0.13321237, -0.20290892, -1.529835), - Point3::new(-0.11798696, -0.15321395, -1.529835), - Point3::new(-0.13424921, -0.15321395, -1.497103), - Point3::new(-0.13453051, -0.14655097, -1.7958934), - Point3::new(-0.13414398, -0.15321401, -1.7978275), - Point3::new(-0.14316282, -0.15321401, -1.8006383), - Point3::new(-0.14316282, -0.13782589, -1.7958934), - Point3::new(-0.13414398, -0.15321401, -1.7978275), - Point3::new(-0.13453051, -0.14655097, -1.7958934), - Point3::new(-0.12842368, -0.15321401, -1.7958934), - Point3::new(-0.13453054, -0.14655057, -1.7958933), - Point3::new(-0.14316282, -0.1378255, -1.7958933), - Point3::new(-0.14316282, -0.08669945, -1.7801287), - Point3::new(-0.13800268, -0.08669945, -1.7785206), - Point3::new(-0.07916508, -0.15321401, -1.7559153), - Point3::new(-0.07664826, -0.14703348, -1.7519293), - Point3::new(-0.07664826, -0.15321401, -1.7526212), - Point3::new(-0.10936269, -0.15321401, -1.7894489), - Point3::new(-0.09927335, -0.08669945, -1.7654262), - Point3::new(-0.07664826, -0.086699456, -1.7403016), - Point3::new(-0.07664826, -0.14703348, -1.7519293), - Point3::new(-0.07916508, -0.15321401, -1.7559153), - Point3::new(-0.13453054, -0.14655057, -1.7958933), - Point3::new(-0.13800268, -0.08669945, -1.7785206), - Point3::new(-0.09927335, -0.08669945, -1.7654262), - Point3::new(-0.10936269, -0.15321401, -1.7894489), - Point3::new(-0.12842332, -0.15321401, -1.7958933), - Point3::new(-0.0799651, -0.12638737, -1.5963497), - Point3::new(-0.08166676, -0.15321401, -1.6131728), - Point3::new(-0.07664826, -0.15321401, -1.6297005), - Point3::new(-0.07664826, -0.11332099, -1.5963497), - Point3::new(-0.086560056, -0.15321401, -1.5963497), - Point3::new(-0.08166676, -0.15321401, -1.6131728), - Point3::new(-0.0799651, -0.12638737, -1.5963497), - Point3::new(-0.07664826, -0.08669945, -1.5740939), - Point3::new(-0.07744763, -0.08669945, -1.5714613), - Point3::new(-0.07996509, -0.12638718, -1.5963496), - Point3::new(-0.07664826, -0.11332084, -1.5963496), - Point3::new(-0.088847235, -0.15321401, -1.5884864), - Point3::new(-0.07818247, -0.08669945, -1.5689349), - Point3::new(-0.09760841, -0.08669945, -1.5298351), - Point3::new(-0.11798693, -0.15321401, -1.5298351), - Point3::new(-0.07818247, -0.08669945, -1.5689349), - Point3::new(-0.088847235, -0.15321401, -1.5884864), - Point3::new(-0.086560085, -0.15321401, -1.5963496), - Point3::new(-0.07996509, -0.12638718, -1.5963496), - Point3::new(-0.07744763, -0.08669945, -1.5714613), - Point3::new(-0.14316282, -0.122018404, -1.4633205), - Point3::new(-0.14316282, -0.15321401, -1.4828252), - Point3::new(-0.13424921, -0.15321401, -1.497103), - Point3::new(-0.13531932, -0.101924114, -1.4633205), - Point3::new(-0.11798699, -0.15321401, -1.529835), - Point3::new(-0.09760847, -0.08669945, -1.529835), - Point3::new(-0.12412503, -0.08669945, -1.4764634), - Point3::new(-0.1309932, -0.08780385, -1.4633205), - Point3::new(-0.13531932, -0.101924114, -1.4633205), - Point3::new(-0.13424921, -0.15321401, -1.497103), - Point3::new(-0.1309932, -0.08780385, -1.4633205), - Point3::new(-0.12412503, -0.08669945, -1.4764634), - Point3::new(-0.13081099, -0.08669945, -1.4633205), - Point3::new(-0.13584769, -0.08669945, -1.4534196), - Point3::new(-0.14316282, -0.08669945, -1.4429029), - Point3::new(-0.14316282, -0.093953855, -1.4457735), - Point3::new(-0.13559848, -0.088544376, -1.4545078), - Point3::new(-0.14316282, -0.122018404, -1.4633205), - Point3::new(-0.13531932, -0.101924114, -1.4633205), - Point3::new(-0.13559848, -0.088544376, -1.4545078), - Point3::new(-0.14316282, -0.093953855, -1.4457735), - Point3::new(-0.13531932, -0.101924114, -1.4633205), - Point3::new(-0.1309932, -0.08780385, -1.4633205), - Point3::new(-0.13559848, -0.088544376, -1.4545078), - Point3::new(-0.1309932, -0.08780385, -1.4633205), - Point3::new(-0.13081099, -0.08669945, -1.4633205), - Point3::new(-0.13584769, -0.08669945, -1.4534196), - Point3::new(-0.13559848, -0.088544376, -1.4545078), - Point3::new(-0.14316282, -0.08034536, -1.7781695), - Point3::new(-0.14316282, -0.042856656, -1.7655915), - Point3::new(-0.14053856, -0.042986937, -1.7658323), - Point3::new(-0.14053856, -0.042986937, -1.7658323), - Point3::new(-0.13800268, -0.086699404, -1.7785206), - Point3::new(-0.14316282, -0.0866994, -1.7801287), - Point3::new(-0.14316282, -0.08034536, -1.7781695), - Point3::new(-0.08918399, -0.020184837, -1.7414033), - Point3::new(-0.07835567, -0.020184832, -1.7293788), - Point3::new(-0.07664826, -0.030022964, -1.7293788), - Point3::new(-0.07664826, -0.0866994, -1.7403016), - Point3::new(-0.09927334, -0.0866994, -1.765426), - Point3::new(-0.14053856, -0.042986937, -1.7658323), - Point3::new(-0.13078228, -0.020184837, -1.7554678), - Point3::new(-0.08918399, -0.020184837, -1.7414033), - Point3::new(-0.09927334, -0.086699404, -1.765426), - Point3::new(-0.13800268, -0.0866994, -1.7785206), - Point3::new(-0.06633711, 0.13043445, -1.6870049), - Point3::new(-0.07835556, -0.020184832, -1.7293787), - Point3::new(-0.07664826, -0.02018483, -1.7274828), - Point3::new(-0.07664826, -0.030022345, -1.7293787), - Point3::new(-0.07664826, -0.07897442, -1.5676358), - Point3::new(-0.07696662, -0.07911639, -1.566706), - Point3::new(-0.07744762, -0.086699404, -1.5714613), - Point3::new(-0.07664826, -0.0866994, -1.5740939), - Point3::new(-0.07818246, -0.0866994, -1.5689349), - Point3::new(-0.07696662, -0.07911639, -1.566706), - Point3::new(-0.09623438, -0.08221464, -1.5298351), - Point3::new(-0.09760841, -0.0866994, -1.5298351), - Point3::new(-0.07696662, -0.07911639, -1.566706), - Point3::new(-0.07818246, -0.086699404, -1.5689349), - Point3::new(-0.07744762, -0.0866994, -1.5714613), - Point3::new(-0.07696662, -0.07911639, -1.566706), - Point3::new(-0.08966427, -0.04239806, -1.5298351), - Point3::new(-0.09623438, -0.08221464, -1.5298351), - Point3::new(-0.07664826, -0.076576024, -1.5666742), - Point3::new(-0.07664826, -0.020184841, -1.5510949), - Point3::new(-0.08674866, -0.02018484, -1.5298351), - Point3::new(-0.089664266, -0.042398065, -1.5298351), - Point3::new(-0.07696662, -0.07911639, -1.566706), - Point3::new(-0.07664826, -0.07897442, -1.5676358), - Point3::new(-0.07664826, -0.076576024, -1.5666742), - Point3::new(-0.07696662, -0.07911639, -1.566706), - Point3::new(-0.09760847, -0.0866994, -1.529835), - Point3::new(-0.09623445, -0.08221465, -1.529835), - Point3::new(-0.12412475, -0.086699404, -1.476464), - Point3::new(-0.08966432, -0.042397942, -1.529835), - Point3::new(-0.09734588, -0.020184837, -1.5075296), - Point3::new(-0.11983546, -0.020184841, -1.4633205), - Point3::new(-0.13081096, -0.086699404, -1.4633205), - Point3::new(-0.12412475, -0.086699404, -1.476464), - Point3::new(-0.09623445, -0.08221465, -1.529835), - Point3::new(-0.08966431, -0.042397946, -1.529835), - Point3::new(-0.08674872, -0.02018484, -1.529835), - Point3::new(-0.09734587, -0.020184837, -1.5075296), - Point3::new(-0.13584769, -0.086699404, -1.4534194), - Point3::new(-0.14316282, -0.032543883, -1.4214734), - Point3::new(-0.14316282, -0.086699404, -1.4429029), - Point3::new(-0.13081096, -0.086699404, -1.4633205), - Point3::new(-0.11983546, -0.020184841, -1.4633205), - Point3::new(-0.14316282, -0.020184845, -1.4174645), - Point3::new(-0.14316282, -0.032543883, -1.4214734), - Point3::new(-0.13584769, -0.086699404, -1.4534194), - Point3::new(-0.07835566, -0.02018479, -1.7293788), - Point3::new(-0.089183986, -0.020184785, -1.7414033), - Point3::new(-0.084133804, 0.013108801, -1.7293788), - Point3::new(-0.13078225, -0.020184793, -1.7554678), - Point3::new(-0.106224254, 0.037211422, -1.7293788), - Point3::new(-0.0841338, 0.013108797, -1.7293788), - Point3::new(-0.08918398, -0.020184793, -1.7414033), - Point3::new(-0.07909463, 0.046329774, -1.7173805), - Point3::new(-0.07664826, 0.046329774, -1.714664), - Point3::new(-0.07664826, -0.020184793, -1.7274828), - Point3::new(-0.07835556, -0.02018479, -1.7293787), - Point3::new(-0.08413375, 0.013109133, -1.7293787), - Point3::new(-0.10622414, 0.037211683, -1.7293787), - Point3::new(-0.1023228, 0.046329774, -1.7252342), - Point3::new(-0.07909463, 0.046329774, -1.7173805), - Point3::new(-0.08413375, 0.013109129, -1.7293787), - Point3::new(-0.07664826, -0.020184796, -1.5510949), - Point3::new(-0.07664826, 0.04632977, -1.5327188), - Point3::new(-0.07801828, 0.04632977, -1.5298351), - Point3::new(-0.08674867, -0.020184794, -1.5298351), - Point3::new(-0.119835466, -0.020184789, -1.4633205), - Point3::new(-0.097345896, -0.020184793, -1.5075296), - Point3::new(-0.112570725, 0.023841474, -1.4633205), - Point3::new(-0.08674873, -0.020184794, -1.529835), - Point3::new(-0.07801833, 0.04632977, -1.529835), - Point3::new(-0.10961901, 0.046329774, -1.4633205), - Point3::new(-0.11257072, 0.023841478, -1.4633205), - Point3::new(-0.09734589, -0.020184793, -1.5075296), - Point3::new(-0.120347455, 0.046329774, -1.4407388), - Point3::new(-0.14269641, 0.046329774, -1.396806), - Point3::new(-0.14316282, 0.043503255, -1.396806), - Point3::new(-0.14316282, -0.020184785, -1.4174645), - Point3::new(-0.119835466, -0.020184789, -1.4633205), - Point3::new(-0.112570725, 0.023841474, -1.4633205), - Point3::new(-0.11257072, 0.023841478, -1.4633205), - Point3::new(-0.10961901, 0.046329774, -1.4633205), - Point3::new(-0.12034745, 0.046329774, -1.4407388), - Point3::new(-0.14269647, 0.046329774, -1.3968059), - Point3::new(-0.14316282, 0.046329774, -1.3958892), - Point3::new(-0.14316282, 0.04350362, -1.3968059), - Point3::new(-0.07909465, 0.046329707, -1.7173805), - Point3::new(-0.07664826, 0.06245765, -1.7115557), - Point3::new(-0.07664826, 0.0463297, -1.714664), - Point3::new(-0.07909465, 0.0463297, -1.7173805), - Point3::new(-0.10232283, 0.0463297, -1.7252342), - Point3::new(-0.07664826, 0.10633554, -1.697959), - Point3::new(-0.07664826, 0.062457643, -1.7115557), - Point3::new(-0.07664826, 0.0463297, -1.5327188), - Point3::new(-0.07664826, 0.0567675, -1.5298351), - Point3::new(-0.07801828, 0.0463297, -1.5298351), - Point3::new(-0.07664826, 0.056767933, -1.529835), - Point3::new(-0.07664826, 0.112844266, -1.5143427), - Point3::new(-0.1008886, 0.112844266, -1.4633205), - Point3::new(-0.10961899, 0.0463297, -1.4633205), - Point3::new(-0.07801833, 0.0463297, -1.529835), - Point3::new(-0.120347425, 0.046329692, -1.4407388), - Point3::new(-0.1354771, 0.0900808, -1.396806), - Point3::new(-0.14269647, 0.0463297, -1.396806), - Point3::new(-0.10961899, 0.0463297, -1.4633205), - Point3::new(-0.1008886, 0.112844266, -1.4633205), - Point3::new(-0.13248926, 0.112844266, -1.396806), - Point3::new(-0.1354771, 0.09008081, -1.396806), - Point3::new(-0.120347425, 0.0463297, -1.4407388), - Point3::new(-0.13547714, 0.09008092, -1.3968059), - Point3::new(-0.14316282, 0.11230591, -1.3744886), - Point3::new(-0.14316282, 0.0463297, -1.3958893), - Point3::new(-0.14269653, 0.0463297, -1.3968059), - Point3::new(-0.13547714, 0.09008093, -1.3968059), - Point3::new(-0.13248932, 0.112844266, -1.3968059), - Point3::new(-0.14316282, 0.112844266, -1.3743398), - Point3::new(-0.14316282, 0.11230593, -1.3744886), - Point3::new(-0.14316282, 0.17935875, -1.7272853), - Point3::new(-0.13450867, 0.17935875, -1.7264287), - Point3::new(-0.1353515, 0.16140196, -1.7255667), - Point3::new(-0.14316282, 0.16254377, -1.7264), - Point3::new(-0.1353515, 0.16140196, -1.7255667), - Point3::new(-0.1320768, 0.17935875, -1.7248962), - Point3::new(-0.07664826, 0.17935875, -1.6954094), - Point3::new(-0.07664826, 0.13506117, -1.6927663), - Point3::new(-0.1353515, 0.16140196, -1.7255667), - Point3::new(-0.13450867, 0.17935875, -1.7264287), - Point3::new(-0.1320768, 0.17935875, -1.7248962), - Point3::new(-0.14316282, 0.11284423, -1.6628642), - Point3::new(-0.076648235, 0.11284423, -1.6628642), - Point3::new(-0.076648235, 0.17935872, -1.6628642), - Point3::new(-0.14316282, 0.17935872, -1.6628642), - Point3::new(-0.14316282, 0.11284423, -1.5963496), - Point3::new(-0.076648235, 0.11284423, -1.5963496), - Point3::new(-0.076648235, 0.17935872, -1.5963496), - Point3::new(-0.14316282, 0.17935872, -1.5963496), - Point3::new(-0.14316282, 0.11284423, -1.5963496), - Point3::new(-0.076648235, 0.11284423, -1.5963496), - Point3::new(-0.076648235, 0.17935872, -1.5963496), - Point3::new(-0.14316282, 0.17935872, -1.5963496), - Point3::new(-0.14316282, 0.11284423, -1.529835), - Point3::new(-0.076648235, 0.11284423, -1.529835), - Point3::new(-0.076648235, 0.17935872, -1.529835), - Point3::new(-0.14316282, 0.17935872, -1.529835), - Point3::new(-0.07664826, 0.112844184, -1.5143427), - Point3::new(-0.07664826, 0.17935875, -1.4959666), - Point3::new(-0.09215823, 0.17935875, -1.4633205), - Point3::new(-0.100888625, 0.112844184, -1.4633205), - Point3::new(-0.100888625, 0.112844184, -1.4633205), - Point3::new(-0.09215823, 0.17935875, -1.4633205), - Point3::new(-0.12375891, 0.17935875, -1.396806), - Point3::new(-0.13248931, 0.11284419, -1.396806), - Point3::new(-0.13248937, 0.11284419, -1.3968059), - Point3::new(-0.12375897, 0.17935875, -1.3968059), - Point3::new(-0.14316282, 0.17935875, -1.3559638), - Point3::new(-0.14316282, 0.11284419, -1.3743399), - Point3::new(-0.07457939, -0.2197285, -1.7293788), - Point3::new(-0.07664827, -0.2197285, -1.7424798), - Point3::new(-0.07664827, -0.21527156, -1.7458802), - Point3::new(-0.07055528, -0.1863293, -1.7293788), - Point3::new(-0.06568167, -0.15321395, -1.7382675), - Point3::new(-0.06501015, -0.15321395, -1.7293788), - Point3::new(-0.070555285, -0.1863293, -1.7293788), - Point3::new(-0.07664827, -0.21527156, -1.7458802), - Point3::new(-0.07664827, -0.209393, -1.7589098), - Point3::new(-0.07664827, -0.209393, -1.7589098), - Point3::new(-0.07664827, -0.15321395, -1.7526212), - Point3::new(-0.06568167, -0.15321395, -1.7382675), - Point3::new(-0.07664827, -0.19288324, -1.6628642), - Point3::new(-0.07664827, -0.2197285, -1.685307), - Point3::new(-0.07070869, -0.2197285, -1.7048682), - Point3::new(-0.059659734, -0.15321395, -1.6856498), - Point3::new(-0.06657837, -0.15321395, -1.6628642), - Point3::new(-0.063583754, -0.15321395, -1.7104981), - Point3::new(-0.059659734, -0.15321395, -1.6856498), - Point3::new(-0.07070869, -0.2197285, -1.7048681), - Point3::new(-0.074579366, -0.2197285, -1.7293787), - Point3::new(-0.07055523, -0.1863291, -1.7293787), - Point3::new(-0.065010145, -0.15321395, -1.7293787), - Point3::new(-0.06358376, -0.15321395, -1.7104981), - Point3::new(-0.07055524, -0.1863291, -1.7293787), - Point3::new(-0.07664827, -0.19288324, -1.6628642), - Point3::new(-0.06657837, -0.15321395, -1.6628642), - Point3::new(-0.07664827, -0.15321395, -1.6297004), - Point3::new(-0.06501015, -0.15321401, -1.7293788), - Point3::new(-0.06568169, -0.15321401, -1.7382677), - Point3::new(-0.060959324, -0.12902257, -1.7293788), - Point3::new(-0.06568169, -0.15321401, -1.7382677), - Point3::new(-0.07664827, -0.15321401, -1.7526213), - Point3::new(-0.07664827, -0.14703351, -1.7519294), - Point3::new(-0.062409427, -0.11206737, -1.7293788), - Point3::new(-0.060959324, -0.12902257, -1.7293788), - Point3::new(-0.07664827, -0.086699456, -1.7403017), - Point3::new(-0.06681198, -0.086699456, -1.7293788), - Point3::new(-0.062409427, -0.11206737, -1.7293788), - Point3::new(-0.07664827, -0.14703351, -1.7519294), - Point3::new(-0.06657838, -0.15321401, -1.6628642), - Point3::new(-0.059659746, -0.15321401, -1.6856498), - Point3::new(-0.048610788, -0.08669945, -1.6664314), - Point3::new(-0.049693942, -0.08669945, -1.6628642), - Point3::new(-0.049580965, -0.08669945, -1.6725749), - Point3::new(-0.048610788, -0.08669945, -1.6664314), - Point3::new(-0.059659746, -0.15321401, -1.6856498), - Point3::new(-0.06358377, -0.15321401, -1.7104982), - Point3::new(-0.05326687, -0.08961612, -1.7148994), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.052291032, -0.08669945, -1.7084472), - Point3::new(-0.049580965, -0.08669945, -1.6725749), - Point3::new(-0.06358377, -0.15321401, -1.7104982), - Point3::new(-0.065010145, -0.15321401, -1.7293787), - Point3::new(-0.06095926, -0.12902224, -1.7293787), - Point3::new(-0.05326687, -0.08961612, -1.7148994), - Point3::new(-0.06095926, -0.12902224, -1.7293787), - Point3::new(-0.062409353, -0.112067185, -1.7293787), - Point3::new(-0.05326687, -0.08961612, -1.7148994), - Point3::new(-0.066811875, -0.086699456, -1.7293787), - Point3::new(-0.053440113, -0.086699456, -1.7145298), - Point3::new(-0.05326687, -0.08961612, -1.7148994), - Point3::new(-0.06240935, -0.112067185, -1.7293787), - Point3::new(-0.07664827, -0.113321126, -1.5963497), - Point3::new(-0.07664827, -0.15321401, -1.6297004), - Point3::new(-0.06657838, -0.15321401, -1.6628642), - Point3::new(-0.049693942, -0.08669945, -1.6628642), - Point3::new(-0.06989047, -0.08669945, -1.5963497), - Point3::new(-0.07664827, -0.113320984, -1.5963496), - Point3::new(-0.06989051, -0.08669945, -1.5963496), - Point3::new(-0.07664827, -0.08669945, -1.5740938), - Point3::new(-0.021179304, -0.02406057, -1.8624079), - Point3::new(-0.023803987, -0.020184837, -1.861124), - Point3::new(-0.021124221, -0.020184837, -1.8624079), - Point3::new(-0.015632318, -0.020184837, -1.8624079), - Point3::new(-0.023803983, -0.020184837, -1.861124), - Point3::new(-0.021179302, -0.02406057, -1.8624079), - Point3::new(-0.06681203, -0.0866994, -1.7293788), - Point3::new(-0.07664827, -0.0866994, -1.7403017), - Point3::new(-0.07664827, -0.030022353, -1.7293788), - Point3::new(-0.049693897, -0.0866994, -1.6628642), - Point3::new(-0.048610777, -0.0866994, -1.6664313), - Point3::new(-0.04655998, -0.074353606, -1.6628642), - Point3::new(-0.049866408, -0.020184837, -1.6662687), - Point3::new(-0.047395226, -0.020184841, -1.6628642), - Point3::new(-0.047162194, -0.044913992, -1.6628642), - Point3::new(-0.049580954, -0.0866994, -1.6725749), - Point3::new(-0.045995377, -0.06966759, -1.6628642), - Point3::new(-0.04655998, -0.07435361, -1.6628642), - Point3::new(-0.04861078, -0.086699404, -1.6664313), - Point3::new(-0.052291017, -0.086699404, -1.7084471), - Point3::new(-0.045396965, -0.066093884, -1.6628642), - Point3::new(-0.04599538, -0.06966759, -1.6628642), - Point3::new(-0.049580958, -0.086699404, -1.6725749), - Point3::new(-0.07664827, -0.020184837, -1.7274829), - Point3::new(-0.057390846, -0.020184845, -1.706098), - Point3::new(-0.053440116, -0.0866994, -1.7145296), - Point3::new(-0.06681192, -0.0866994, -1.7293787), - Point3::new(-0.07664827, -0.030021735, -1.7293787), - Point3::new(-0.069890454, -0.086699404, -1.5963497), - Point3::new(-0.049693897, -0.0866994, -1.6628642), - Point3::new(-0.04655998, -0.074353606, -1.6628642), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.06681654, -0.07459004, -1.5963497), - Point3::new(-0.047395226, -0.020184841, -1.6628642), - Point3::new(-0.04431176, -0.020184845, -1.6586162), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.047162194, -0.044913992, -1.6628642), - Point3::new(-0.045995377, -0.06966759, -1.6628642), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.04655998, -0.07435361, -1.6628642), - Point3::new(-0.043273855, -0.020184834, -1.647505), - Point3::new(-0.059779156, -0.020184841, -1.5963497), - Point3::new(-0.06681654, -0.07459004, -1.5963497), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.04431176, -0.020184837, -1.6586162), - Point3::new(-0.04244908, -0.020184815, -1.6544119), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.042449083, -0.020184837, -1.6544119), - Point3::new(-0.04327385, -0.020184845, -1.647505), - Point3::new(-0.045396965, -0.066093884, -1.6628642), - Point3::new(-0.0449818, -0.064853005, -1.6601192), - Point3::new(-0.04599538, -0.06966759, -1.6628642), - Point3::new(-0.07664827, -0.078974426, -1.5676357), - Point3::new(-0.07664827, -0.086699404, -1.5740938), - Point3::new(-0.06989049, -0.086699404, -1.5963496), - Point3::new(-0.06681658, -0.07459006, -1.5963496), - Point3::new(-0.06958121, -0.020184834, -1.56597), - Point3::new(-0.07664827, -0.020184845, -1.5510949), - Point3::new(-0.07664827, -0.07657614, -1.5666742), - Point3::new(-0.059779197, -0.020184841, -1.5963496), - Point3::new(-0.06958121, -0.020184845, -1.56597), - Point3::new(-0.07664827, -0.07657617, -1.5666742), - Point3::new(-0.07664827, -0.078974426, -1.5676357), - Point3::new(-0.06681658, -0.07459006, -1.5963496), - Point3::new(-0.030793766, -0.009863387, -1.857705), - Point3::new(-0.031695616, 0.046329774, -1.8540797), - Point3::new(-0.020150661, 0.046329774, -1.8624079), - Point3::new(-0.020185381, 0.045856625, -1.8624079), - Point3::new(-0.021124247, -0.020184793, -1.8624079), - Point3::new(-0.023804016, -0.020184793, -1.861124), - Point3::new(-0.030793766, -0.009863387, -1.857705), - Point3::new(-0.020185381, 0.045856625, -1.8624079), - Point3::new(-0.030793766, -0.009863387, -1.857705), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(-0.033531733, 0.16073631, -1.8466988), - Point3::new(-0.030793766, -0.009863387, -1.857705), - Point3::new(-0.023804015, -0.020184793, -1.861124), - Point3::new(-0.015632331, -0.020184793, -1.8624079), - Point3::new(-0.010133706, -0.016342914, -1.8624079), - Point3::new(-0.010133706, 0.028591737, -1.8523036), - Point3::new(-0.047395177, -0.020184789, -1.6628642), - Point3::new(-0.04986641, -0.020184793, -1.6662688), - Point3::new(-0.057139993, 0.046329774, -1.675426), - Point3::new(-0.048021954, 0.04632977, -1.6628642), - Point3::new(-0.07664827, 0.046329774, -1.714664), - Point3::new(-0.061341584, 0.046329767, -1.6976664), - Point3::new(-0.05739085, -0.020184785, -1.7060981), - Point3::new(-0.07664827, -0.020184785, -1.7274828), - Point3::new(-0.043314017, 0.046329767, -1.6563781), - Point3::new(-0.04431176, -0.020184785, -1.6586162), - Point3::new(-0.047395177, -0.020184789, -1.6628642), - Point3::new(-0.048021954, 0.04632977, -1.6628642), - Point3::new(-0.040730577, 0.046329774, -1.6287216), - Point3::new(-0.05117538, 0.046329778, -1.5963497), - Point3::new(-0.059779152, -0.020184787, -1.5963497), - Point3::new(-0.04327385, -0.020184793, -1.647505), - Point3::new(-0.043314017, 0.046329774, -1.6563781), - Point3::new(-0.038677655, 0.046329767, -1.6459134), - Point3::new(-0.04244908, -0.020184815, -1.6544119), - Point3::new(-0.04431176, -0.020184793, -1.6586162), - Point3::new(-0.038677655, 0.04632977, -1.6459134), - Point3::new(-0.04073058, 0.04632978, -1.6287216), - Point3::new(-0.04327385, -0.020184785, -1.647505), - Point3::new(-0.04244908, -0.020184796, -1.6544119), - Point3::new(-0.07664827, 0.046329767, -1.5327188), - Point3::new(-0.07664827, -0.020184785, -1.5510949), - Point3::new(-0.06958121, -0.020184793, -1.56597), - Point3::new(-0.061245486, 0.046329774, -1.5651392), - Point3::new(-0.05117542, 0.046329778, -1.5963496), - Point3::new(-0.06124548, 0.04632978, -1.5651392), - Point3::new(-0.0695812, -0.020184785, -1.56597), - Point3::new(-0.05977919, -0.020184787, -1.5963496), - Point3::new(-0.03169561, 0.0463297, -1.8540797), - Point3::new(-0.03276311, 0.112844266, -1.8497885), - Point3::new(-0.015269473, 0.112844266, -1.8624079), - Point3::new(-0.020150675, 0.0463297, -1.8624079), - Point3::new(-0.04802195, 0.046329703, -1.6628642), - Point3::new(-0.057139985, 0.0463297, -1.675426), - Point3::new(-0.06441356, 0.112844266, -1.6845832), - Point3::new(-0.048648708, 0.11284426, -1.6628642), - Point3::new(-0.069005296, 0.112844266, -1.693358), - Point3::new(-0.06529231, 0.112844266, -1.6892347), - Point3::new(-0.06134158, 0.0463297, -1.6976664), - Point3::new(-0.07664827, 0.046329707, -1.714664), - Point3::new(-0.07664827, 0.06245753, -1.7115558), - Point3::new(-0.069005296, 0.112844266, -1.693358), - Point3::new(-0.07664827, 0.062457547, -1.7115558), - Point3::new(-0.07664827, 0.10633551, -1.697959), - Point3::new(-0.07386339, 0.112844266, -1.6950004), - Point3::new(-0.042316273, 0.11284426, -1.6541401), - Point3::new(-0.043314017, 0.046329707, -1.6563781), - Point3::new(-0.04802195, 0.046329703, -1.6628642), - Point3::new(-0.048648708, 0.11284426, -1.6628642), - Point3::new(-0.03818731, 0.112844266, -1.6099383), - Point3::new(-0.04257166, 0.112844266, -1.5963497), - Point3::new(-0.051175404, 0.046329707, -1.5963497), - Point3::new(-0.04073058, 0.046329707, -1.6287217), - Point3::new(-0.042316273, 0.112844266, -1.6541401), - Point3::new(-0.034906235, 0.11284426, -1.6374148), - Point3::new(-0.03867766, 0.046329707, -1.6459134), - Point3::new(-0.043314017, 0.0463297, -1.6563781), - Point3::new(-0.034906235, 0.112844266, -1.6374148), - Point3::new(-0.03818731, 0.11284426, -1.6099383), - Point3::new(-0.04073058, 0.046329707, -1.6287217), - Point3::new(-0.03867766, 0.046329692, -1.6459134), - Point3::new(-0.07664827, 0.05676751, -1.5298351), - Point3::new(-0.07664827, 0.046329707, -1.5327188), - Point3::new(-0.061245494, 0.0463297, -1.5651392), - Point3::new(-0.05290977, 0.112844266, -1.5643085), - Point3::new(-0.069287896, 0.11284426, -1.5298351), - Point3::new(-0.042571697, 0.112844266, -1.5963496), - Point3::new(-0.05290976, 0.11284426, -1.5643085), - Point3::new(-0.061245486, 0.046329707, -1.5651392), - Point3::new(-0.051175445, 0.046329707, -1.5963496), - Point3::new(-0.07664827, 0.05676794, -1.529835), - Point3::new(-0.069287956, 0.11284426, -1.529835), - Point3::new(-0.07664827, 0.11284426, -1.5143427), - Point3::new(-0.015269473, 0.11284419, -1.8624079), - Point3::new(-0.03276311, 0.1128442, -1.8497885), - Point3::new(-0.033531733, 0.16073631, -1.8466988), - Point3::new(-0.010635661, 0.17598689, -1.8624079), - Point3::new(-0.011457793, 0.17935875, -1.8624079), - Point3::new(-0.010635662, 0.17598689, -1.8624079), - Point3::new(-0.033531733, 0.16073631, -1.8466988), - Point3::new(-0.03173334, 0.17935875, -1.8504404), - Point3::new(-0.010133706, 0.1447646, -1.8445529), - Point3::new(-0.010133706, 0.17935875, -1.8480864), - Point3::new(-0.033573695, 0.17935874, -1.8486018), - Point3::new(-0.033531733, 0.16073631, -1.8466988), - Point3::new(-0.033531733, 0.16073631, -1.8466988), - Point3::new(-0.033573695, 0.17935875, -1.8486018), - Point3::new(-0.03173334, 0.17935875, -1.8504404), - Point3::new(-0.04864877, 0.1128442, -1.6628642), - Point3::new(-0.064413555, 0.11284419, -1.6845832), - Point3::new(-0.06633711, 0.13043445, -1.6870049), - Point3::new(-0.06087463, 0.17935875, -1.6788442), - Point3::new(-0.049275506, 0.17935875, -1.6628642), - Point3::new(-0.06633711, 0.13043445, -1.6870049), - Point3::new(-0.06676162, 0.17935875, -1.69015), - Point3::new(-0.060874626, 0.17935875, -1.6788442), - Point3::new(-0.06900531, 0.11284419, -1.693358), - Point3::new(-0.06633711, 0.13043445, -1.6870049), - Point3::new(-0.065292306, 0.11284419, -1.6892347), - Point3::new(-0.06633711, 0.13043445, -1.6870049), - Point3::new(-0.06900531, 0.11284419, -1.693358), - Point3::new(-0.07386342, 0.11284419, -1.6950004), - Point3::new(-0.07664827, 0.17935875, -1.6954094), - Point3::new(-0.06676162, 0.17935875, -1.69015), - Point3::new(-0.06633711, 0.13043445, -1.6870049), - Point3::new(-0.07664827, 0.13506119, -1.6927663), - Point3::new(-0.041318532, 0.17935875, -1.651902), - Point3::new(-0.042316277, 0.1128442, -1.65414), - Point3::new(-0.04864877, 0.1128442, -1.6628642), - Point3::new(-0.049275506, 0.17935875, -1.6628642), - Point3::new(-0.042571664, 0.11284419, -1.5963497), - Point3::new(-0.03818731, 0.11284419, -1.6099383), - Point3::new(-0.036347426, 0.16096294, -1.5963497), - Point3::new(-0.041318532, 0.17935875, -1.651902), - Point3::new(-0.031134814, 0.17935875, -1.6289163), - Point3::new(-0.03490624, 0.1128442, -1.6374148), - Point3::new(-0.042316277, 0.11284419, -1.65414), - Point3::new(-0.031134814, 0.17935875, -1.6289163), - Point3::new(-0.035023697, 0.17935875, -1.5963497), - Point3::new(-0.036347426, 0.16096295, -1.5963497), - Point3::new(-0.03818731, 0.1128442, -1.6099383), - Point3::new(-0.03490624, 0.11284419, -1.6374148), - Point3::new(-0.069287896, 0.1128442, -1.5298351), - Point3::new(-0.052909777, 0.1128442, -1.5643085), - Point3::new(-0.044574052, 0.17935875, -1.5634778), - Point3::new(-0.060557496, 0.17935875, -1.5298351), - Point3::new(-0.03564404, 0.17935875, -1.5911548), - Point3::new(-0.044574052, 0.17935875, -1.5634778), - Point3::new(-0.052909773, 0.1128442, -1.5643085), - Point3::new(-0.0425717, 0.11284419, -1.5963496), - Point3::new(-0.03634741, 0.16096337, -1.5963496), - Point3::new(-0.03502371, 0.17935875, -1.5963496), - Point3::new(-0.03564404, 0.17935875, -1.5911548), - Point3::new(-0.03634741, 0.16096337, -1.5963496), - Point3::new(-0.076648265, 0.17935875, -1.4959666), - Point3::new(-0.076648265, 0.1128442, -1.5143427), - Point3::new(-0.069287956, 0.1128442, -1.529835), - Point3::new(-0.06055755, 0.17935875, -1.529835), - Point3::new(0.056380846, -0.12297553, -1.7159696), - Point3::new(0.049570914, -0.08669945, -1.7050503), - Point3::new(0.049927697, -0.08669945, -1.7057998), - Point3::new(0.056380846, -0.104652956, -1.7176802), - Point3::new(0.05365365, -0.08669945, -1.6628642), - Point3::new(0.049570914, -0.08669945, -1.7050503), - Point3::new(0.056380846, -0.12297552, -1.7159696), - Point3::new(0.056380846, -0.099275514, -1.6628642), - Point3::new(0.056380846, -0.10465294, -1.7176802), - Point3::new(0.049927697, -0.08669945, -1.7057998), - Point3::new(0.056380846, -0.08669945, -1.7127628), - Point3::new(0.056380846, -0.08669945, -1.6338606), - Point3::new(0.055560432, -0.08669945, -1.6356738), - Point3::new(0.056380846, -0.09205388, -1.6371784), - Point3::new(0.082199745, 0.00933588, -1.5234594), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.15582763, -0.12761694, -1.4394239), - Point3::new(0.056380846, -0.090703234, -1.643656), - Point3::new(0.0557327, -0.08669945, -1.6413817), - Point3::new(0.05365365, -0.08669945, -1.6628642), - Point3::new(0.056380846, -0.099275514, -1.6628642), - Point3::new(0.056380846, -0.09205389, -1.6371784), - Point3::new(0.055560432, -0.08669945, -1.6356738), - Point3::new(0.055732697, -0.08669945, -1.6413817), - Point3::new(0.056380846, -0.09070324, -1.643656), - Point3::new(0.03944725, 0.13982476, -1.6501261), - Point3::new(0.11484603, 0.11130635, -1.7216146), - Point3::new(0.049180854, -0.08462163, -1.7044249), - Point3::new(0.049570903, -0.086699404, -1.7050502), - Point3::new(0.049180854, -0.08462163, -1.7044249), - Point3::new(0.04992768, -0.086699404, -1.7057998), - Point3::new(0.05104391, -0.07466504, -1.6628642), - Point3::new(0.049180854, -0.08462163, -1.7044249), - Point3::new(0.049570903, -0.086699404, -1.7050502), - Point3::new(0.053653635, -0.086699404, -1.6628642), - Point3::new(0.056380846, -0.086699404, -1.7127628), - Point3::new(0.04992768, -0.086699404, -1.7057998), - Point3::new(0.049180854, -0.08462163, -1.7044249), - Point3::new(0.056380846, -0.06313871, -1.7063097), - Point3::new(0.056380846, -0.05689171, -1.6161941), - Point3::new(0.056380846, -0.020184837, -1.6050887), - Point3::new(0.049929477, -0.020184815, -1.6232537), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.049929474, -0.020184837, -1.6232537), - Point3::new(0.049345437, -0.020184815, -1.6278259), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.055560425, -0.086699404, -1.6356738), - Point3::new(0.056380846, -0.086699404, -1.6338605), - Point3::new(0.056380846, -0.06908849, -1.6229479), - Point3::new(0.049345437, -0.020184837, -1.6278259), - Point3::new(0.04956496, -0.02018483, -1.6346076), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.05104391, -0.07466504, -1.6628642), - Point3::new(0.053653635, -0.086699404, -1.6628642), - Point3::new(0.055732694, -0.086699404, -1.6413817), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.055560425, -0.086699404, -1.6356738), - Point3::new(0.0525133, -0.06681228, -1.6300853), - Point3::new(0.055732694, -0.086699404, -1.6413817), - Point3::new(0.018538032, 0.046329774, -1.8624079), - Point3::new(0.025458898, 0.046329774, -1.8539069), - Point3::new(0.01913499, 0.0041060485, -1.8624079), - Point3::new(0.019134987, 0.0041060466, -1.8624079), - Point3::new(0.025458895, 0.046329774, -1.8539069), - Point3::new(-0.00060392544, 0.046329774, -1.8498122), - Point3::new(-0.010133721, 0.028591715, -1.8523036), - Point3::new(-0.010133721, -0.016342916, -1.8624079), - Point3::new(0.056380846, -0.020184793, -1.6050887), - Point3::new(0.056380846, 0.008700218, -1.5963497), - Point3::new(0.052337565, 0.04632978, -1.5963497), - Point3::new(0.046243615, 0.046329796, -1.6135083), - Point3::new(0.049929477, -0.020184815, -1.6232537), - Point3::new(0.04482644, 0.046329796, -1.6246027), - Point3::new(0.049345437, -0.020184815, -1.6278259), - Point3::new(0.049929474, -0.020184793, -1.6232537), - Point3::new(0.046243615, 0.046329774, -1.6135083), - Point3::new(0.045359116, 0.046329774, -1.6410584), - Point3::new(0.049564958, -0.0201848, -1.6346076), - Point3::new(0.049345434, -0.020184793, -1.6278259), - Point3::new(0.04482644, 0.046329774, -1.6246027), - Point3::new(0.056380846, 0.008700613, -1.5963496), - Point3::new(0.056380846, 0.046329774, -1.5849651), - Point3::new(0.052337606, 0.04632978, -1.5963496), - Point3::new(0.017597593, 0.112844266, -1.8624079), - Point3::new(0.035420865, 0.112844266, -1.8405151), - Point3::new(0.025458887, 0.0463297, -1.8539069), - Point3::new(0.018538024, 0.0463297, -1.8624079), - Point3::new(0.035420865, 0.112844266, -1.8405151), - Point3::new(0.035131067, 0.112844266, -1.8404696), - Point3::new(-0.0006039664, 0.0463297, -1.8498122), - Point3::new(0.025458885, 0.0463297, -1.8539069), - Point3::new(0.04519064, 0.11284426, -1.5963497), - Point3::new(0.04255776, 0.11284426, -1.6037631), - Point3::new(0.04624362, 0.046329707, -1.6135085), - Point3::new(0.05233758, 0.046329703, -1.5963497), - Point3::new(0.04030746, 0.11284426, -1.6213795), - Point3::new(0.04482645, 0.046329677, -1.6246027), - Point3::new(0.04624362, 0.0463297, -1.6135085), - Point3::new(0.04255776, 0.112844266, -1.6037631), - Point3::new(0.04115328, 0.112844266, -1.6475093), - Point3::new(0.04535912, 0.0463297, -1.6410584), - Point3::new(0.044826448, 0.0463297, -1.6246027), - Point3::new(0.040307455, 0.112844266, -1.6213795), - Point3::new(0.056380846, 0.0463297, -1.5849651), - Point3::new(0.056380846, 0.112844266, -1.5648415), - Point3::new(0.045190684, 0.11284426, -1.5963496), - Point3::new(0.052337624, 0.046329703, -1.5963496), - Point3::new(0.016908769, 0.16156493, -1.8624079), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(0.035420854, 0.11284419, -1.8405151), - Point3::new(0.017597588, 0.1128442, -1.8624079), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(0.033428412, 0.17935875, -1.8455174), - Point3::new(0.02062426, 0.17935875, -1.8474098), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(0.035209637, 0.17935875, -1.8489724), - Point3::new(0.03342841, 0.17935875, -1.8455174), - Point3::new(0.019537896, 0.17935875, -1.8624079), - Point3::new(0.035209637, 0.17935875, -1.8489724), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(0.016908769, 0.16156493, -1.8624079), - Point3::new(-0.010133721, 0.17935875, -1.8480864), - Point3::new(-0.010133721, 0.14476462, -1.8445529), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(0.02062426, 0.17935875, -1.8474098), - Point3::new(0.035420854, 0.1128442, -1.8405151), - Point3::new(0.03553288, 0.11359217, -1.8403646), - Point3::new(0.03513103, 0.11284419, -1.8404696), - Point3::new(0.05102139, 0.17935875, -1.6628642), - Point3::new(0.056380846, 0.17935875, -1.6680272), - Point3::new(0.056380846, 0.1334199, -1.6661814), - Point3::new(0.05288215, 0.13474323, -1.6628642), - Point3::new(0.039753903, 0.16344245, -1.5963497), - Point3::new(0.04255777, 0.1128442, -1.6037631), - Point3::new(0.045190655, 0.1128442, -1.5963497), - Point3::new(0.035788465, 0.17935875, -1.6181563), - Point3::new(0.04030746, 0.1128442, -1.6213795), - Point3::new(0.04255777, 0.1128442, -1.6037631), - Point3::new(0.039753903, 0.16344245, -1.5963497), - Point3::new(0.038574025, 0.17935875, -1.5963497), - Point3::new(0.03667778, 0.17935875, -1.6456296), - Point3::new(0.042568896, 0.17935875, -1.6547216), - Point3::new(0.03944725, 0.13982476, -1.6501261), - Point3::new(0.03944725, 0.13982476, -1.6501261), - Point3::new(0.041153286, 0.11284419, -1.6475093), - Point3::new(0.040307462, 0.112844184, -1.6213795), - Point3::new(0.03578847, 0.17935875, -1.6181563), - Point3::new(0.036677778, 0.17935875, -1.6456296), - Point3::new(0.03944725, 0.13982476, -1.6501261), - Point3::new(0.042568896, 0.17935875, -1.6547216), - Point3::new(0.05102139, 0.17935875, -1.6628642), - Point3::new(0.05288215, 0.13474323, -1.6628642), - Point3::new(0.056380846, 0.1128442, -1.5648416), - Point3::new(0.056380846, 0.17935875, -1.544718), - Point3::new(0.038871914, 0.17935875, -1.5940177), - Point3::new(0.039753858, 0.16344327, -1.5963496), - Point3::new(0.045190696, 0.1128442, -1.5963496), - Point3::new(0.03857404, 0.17935875, -1.5963496), - Point3::new(0.039753858, 0.16344327, -1.5963496), - Point3::new(0.03887191, 0.17935875, -1.5940177), - Point3::new(0.12289546, -0.16181482, -1.7958934), - Point3::new(0.12289546, -0.2197285, -1.8116077), - Point3::new(0.114046395, -0.2197285, -1.8114208), - Point3::new(0.114184655, -0.16249311, -1.7958934), - Point3::new(0.114184655, -0.16249311, -1.7958934), - Point3::new(0.114046395, -0.2197285, -1.8114208), - Point3::new(0.09965607, -0.2197285, -1.7958934), - Point3::new(0.11420707, -0.15321395, -1.7933761), - Point3::new(0.12289546, -0.15321395, -1.7935597), - Point3::new(0.12289546, -0.16181439, -1.7958933), - Point3::new(0.114184655, -0.16249268, -1.7958933), - Point3::new(0.06410769, -0.15321395, -1.7293788), - Point3::new(0.07383545, -0.15321395, -1.7498146), - Point3::new(0.09774321, -0.2197285, -1.7938294), - Point3::new(0.07454382, -0.2197285, -1.7450926), - Point3::new(0.06474366, -0.16752368, -1.7293788), - Point3::new(0.06474366, -0.16752367, -1.7293788), - Point3::new(0.07454382, -0.2197285, -1.7450926), - Point3::new(0.07606459, -0.2197285, -1.7293788), - Point3::new(0.07383545, -0.15321395, -1.7498146), - Point3::new(0.11420707, -0.15321395, -1.7933761), - Point3::new(0.114184655, -0.16249268, -1.7958933), - Point3::new(0.099655956, -0.2197285, -1.7958933), - Point3::new(0.09774322, -0.2197285, -1.7938294), - Point3::new(0.07038619, -0.1834601, -1.6628642), - Point3::new(0.07594328, -0.2197285, -1.6730559), - Point3::new(0.080554605, -0.2197285, -1.6628642), - Point3::new(0.064107634, -0.15321395, -1.7293787), - Point3::new(0.064743586, -0.16752328, -1.7293787), - Point3::new(0.06205736, -0.15321395, -1.7250715), - Point3::new(0.06650028, -0.15321395, -1.6791637), - Point3::new(0.06205736, -0.15321395, -1.7250715), - Point3::new(0.064743586, -0.16752328, -1.7293787), - Point3::new(0.0760646, -0.2197285, -1.7293787), - Point3::new(0.07726786, -0.2197285, -1.7169456), - Point3::new(0.07594328, -0.2197285, -1.6730559), - Point3::new(0.07038619, -0.1834601, -1.6628642), - Point3::new(0.06600836, -0.15321395, -1.6628642), - Point3::new(0.06650028, -0.15321395, -1.6791637), - Point3::new(0.07726786, -0.2197285, -1.7169456), - Point3::new(0.0920014, -0.15321395, -1.5963497), - Point3::new(0.06575185, -0.15321395, -1.6543648), - Point3::new(0.07038619, -0.1834601, -1.6628642), - Point3::new(0.080554605, -0.2197285, -1.6628642), - Point3::new(0.110649765, -0.2197285, -1.5963497), - Point3::new(0.07038619, -0.1834601, -1.6628642), - Point3::new(0.06575185, -0.15321395, -1.6543648), - Point3::new(0.06600836, -0.15321395, -1.6628642), - Point3::new(0.12289546, -0.2197285, -1.569285), - Point3::new(0.12289546, -0.15606314, -1.5298351), - Point3::new(0.12209664, -0.15321395, -1.5298351), - Point3::new(0.09200145, -0.15321395, -1.5963496), - Point3::new(0.110649824, -0.2197285, -1.5963496), - Point3::new(0.12289546, -0.15606296, -1.529835), - Point3::new(0.12289546, -0.15321395, -1.5280696), - Point3::new(0.122096695, -0.15321395, -1.529835), - Point3::new(0.11436774, -0.08669945, -1.7753314), - Point3::new(0.12289546, -0.08669945, -1.7755116), - Point3::new(0.12289546, -0.15321401, -1.7935597), - Point3::new(0.11420707, -0.15321401, -1.7933761), - Point3::new(0.06273523, -0.12233167, -1.7293788), - Point3::new(0.07383547, -0.15321401, -1.7498146), - Point3::new(0.0641077, -0.15321401, -1.7293788), - Point3::new(0.071780056, -0.08669945, -1.7293788), - Point3::new(0.11436774, -0.08669945, -1.7753314), - Point3::new(0.11420707, -0.15321401, -1.7933761), - Point3::new(0.07383547, -0.15321401, -1.7498146), - Point3::new(0.06273523, -0.12233168, -1.7293788), - Point3::new(0.05638089, -0.12297577, -1.7159697), - Point3::new(0.05638089, -0.104653075, -1.7176803), - Point3::new(0.06273516, -0.12233149, -1.7293787), - Point3::new(0.06410764, -0.15321401, -1.7293787), - Point3::new(0.06205737, -0.15321401, -1.7250715), - Point3::new(0.05638089, -0.09927574, -1.6628642), - Point3::new(0.05638089, -0.12297576, -1.7159697), - Point3::new(0.062057372, -0.15321401, -1.7250715), - Point3::new(0.066500284, -0.15321401, -1.6791637), - Point3::new(0.06185505, -0.12451902, -1.6628642), - Point3::new(0.066500284, -0.15321401, -1.6791637), - Point3::new(0.066008374, -0.15321401, -1.6628642), - Point3::new(0.061855048, -0.12451902, -1.6628642), - Point3::new(0.07177995, -0.08669945, -1.7293787), - Point3::new(0.06273516, -0.12233149, -1.7293787), - Point3::new(0.05638089, -0.104653075, -1.7176803), - Point3::new(0.05638089, -0.08669945, -1.712763), - Point3::new(0.065751866, -0.15321401, -1.6543648), - Point3::new(0.09200141, -0.15321401, -1.5963497), - Point3::new(0.07335302, -0.08669945, -1.5963497), - Point3::new(0.05638089, -0.08669945, -1.6338605), - Point3::new(0.05638089, -0.09205417, -1.6371785), - Point3::new(0.05638089, -0.09927574, -1.6628642), - Point3::new(0.06185505, -0.12451902, -1.6628642), - Point3::new(0.05638089, -0.09070352, -1.6436561), - Point3::new(0.066008374, -0.15321401, -1.6628642), - Point3::new(0.065751866, -0.15321401, -1.6543648), - Point3::new(0.05638089, -0.09205416, -1.6371785), - Point3::new(0.05638089, -0.09070352, -1.6436561), - Point3::new(0.061855048, -0.12451902, -1.6628642), - Point3::new(0.09200147, -0.15321401, -1.5963496), - Point3::new(0.12209664, -0.15321401, -1.5298351), - Point3::new(0.10683623, -0.09878352, -1.5298351), - Point3::new(0.08630396, -0.08669945, -1.5677264), - Point3::new(0.07335308, -0.08669945, -1.5963496), - Point3::new(0.19847965, 0.05531616, -1.2937757), - Point3::new(0.122096695, -0.15321401, -1.529835), - Point3::new(0.12289546, -0.15321401, -1.5280696), - Point3::new(0.12289546, -0.10823503, -1.5001986), - Point3::new(0.10683629, -0.09878356, -1.529835), - Point3::new(0.11452841, -0.020184837, -1.7572867), - Point3::new(0.12289546, -0.020184837, -1.7574636), - Point3::new(0.12289546, -0.086699404, -1.7755116), - Point3::new(0.11436774, -0.0866994, -1.7753314), - Point3::new(0.20694056, -0.04075135, -1.7648193), - Point3::new(0.19531466, 0.13716632, -1.7400897), - Point3::new(0.08866407, -0.020184837, -1.7293788), - Point3::new(0.11452841, -0.020184837, -1.7572868), - Point3::new(0.11436774, -0.08669942, -1.7753315), - Point3::new(0.07178003, -0.08669941, -1.7293788), - Point3::new(0.08866396, -0.020184837, -1.7293787), - Point3::new(0.07177992, -0.08669941, -1.7293787), - Point3::new(0.05638089, -0.086699404, -1.712763), - Point3::new(0.05638089, -0.063138574, -1.7063097), - Point3::new(0.07077682, -0.020184837, -1.7100782), - Point3::new(0.05638089, -0.056891594, -1.616194), - Point3::new(0.061905872, -0.042719565, -1.5963497), - Point3::new(0.059484527, -0.020184832, -1.5963497), - Point3::new(0.05638089, -0.02018483, -1.6050886), - Point3::new(0.05638089, -0.06908851, -1.6229479), - Point3::new(0.05638089, -0.086699404, -1.6338605), - Point3::new(0.073353015, -0.086699404, -1.5963497), - Point3::new(0.07079374, -0.07757106, -1.5963497), - Point3::new(0.061905906, -0.04271948, -1.5963496), - Point3::new(0.07069106, -0.020184841, -1.5647956), - Point3::new(0.05948457, -0.020184832, -1.5963496), - Point3::new(0.0707938, -0.077571094, -1.5963496), - Point3::new(0.07335307, -0.086699404, -1.5963496), - Point3::new(0.08630388, -0.086699404, -1.5677265), - Point3::new(0.114689074, 0.046329774, -1.7392421), - Point3::new(0.12289546, 0.046329774, -1.7394154), - Point3::new(0.12289546, -0.020184793, -1.7574635), - Point3::new(0.1145284, -0.020184785, -1.7572868), - Point3::new(0.105548084, 0.046329774, -1.7293788), - Point3::new(0.114689074, 0.046329774, -1.7392421), - Point3::new(0.1145284, -0.020184793, -1.7572868), - Point3::new(0.08866408, -0.020184793, -1.7293788), - Point3::new(0.10554797, 0.046329774, -1.7293787), - Point3::new(0.088663965, -0.020184793, -1.7293787), - Point3::new(0.070776835, -0.020184793, -1.7100782), - Point3::new(0.09306916, 0.046329774, -1.7159139), - Point3::new(0.05638089, -0.0201848, -1.6050886), - Point3::new(0.05948453, -0.020184798, -1.5963497), - Point3::new(0.05638089, 0.008699823, -1.5963497), - Point3::new(0.059484575, -0.020184798, -1.5963496), - Point3::new(0.07069108, -0.020184793, -1.5647956), - Point3::new(0.08042465, 0.0047826264, -1.5298351), - Point3::new(0.075960435, 0.046329774, -1.5298351), - Point3::new(0.05638089, 0.04632978, -1.584965), - Point3::new(0.05638089, 0.008700218, -1.5963496), - Point3::new(0.07596047, 0.046329774, -1.529835), - Point3::new(0.08042469, 0.00478271, -1.529835), - Point3::new(0.082199745, 0.00933588, -1.5234594), - Point3::new(0.077658, 0.046329774, -1.5250553), - Point3::new(0.082199745, 0.00933588, -1.5234594), - Point3::new(0.07995127, 0.046329774, -1.5189065), - Point3::new(0.077658, 0.046329767, -1.5250553), - Point3::new(0.10949655, 0.04632977, -1.4633205), - Point3::new(0.07995127, 0.046329767, -1.5189065), - Point3::new(0.082199745, 0.00933588, -1.5234594), - Point3::new(0.112645745, 0.021375068, -1.4633205), - Point3::new(0.12289546, 0.025428087, -1.4430746), - Point3::new(0.12289546, 0.046329774, -1.438112), - Point3::new(0.10949655, 0.04632977, -1.4633205), - Point3::new(0.112645745, 0.021375068, -1.4633205), - Point3::new(0.12289546, 0.08331877, -1.7293788), - Point3::new(0.12289546, 0.046329696, -1.7394154), - Point3::new(0.114689074, 0.046329707, -1.7392421), - Point3::new(0.114776894, 0.082686655, -1.7293788), - Point3::new(0.114776894, 0.082686655, -1.7293788), - Point3::new(0.114689074, 0.0463297, -1.7392421), - Point3::new(0.10554807, 0.046329696, -1.7293788), - Point3::new(0.11484603, 0.11130635, -1.7216146), - Point3::new(0.12289546, 0.0980159, -1.7253909), - Point3::new(0.12289546, 0.08331921, -1.7293787), - Point3::new(0.114776894, 0.082687095, -1.7293787), - Point3::new(0.11484603, 0.11130635, -1.7216146), - Point3::new(0.11446581, 0.112844266, -1.7213101), - Point3::new(0.11483169, 0.11284426, -1.7216887), - Point3::new(0.11484603, 0.11130635, -1.7216146), - Point3::new(0.11483169, 0.112844266, -1.7216887), - Point3::new(0.11963159, 0.112844266, -1.7227134), - Point3::new(0.093069136, 0.046329692, -1.7159139), - Point3::new(0.11484603, 0.11130635, -1.7216146), - Point3::new(0.114776894, 0.082687095, -1.7293787), - Point3::new(0.105547965, 0.046329696, -1.7293787), - Point3::new(0.11484603, 0.11130635, -1.7216146), - Point3::new(0.11077998, 0.112844266, -1.7177594), - Point3::new(0.11446581, 0.11284426, -1.7213101), - Point3::new(0.05638089, 0.11284426, -1.5648414), - Point3::new(0.05638089, 0.046329692, -1.584965), - Point3::new(0.07596044, 0.0463297, -1.5298351), - Point3::new(0.06881351, 0.112844266, -1.5298351), - Point3::new(0.068813555, 0.112844266, -1.529835), - Point3::new(0.07596049, 0.0463297, -1.529835), - Point3::new(0.07765801, 0.0463297, -1.5250553), - Point3::new(0.06949201, 0.112844266, -1.5279247), - Point3::new(0.07995127, 0.0463297, -1.5189065), - Point3::new(0.07590854, 0.112844266, -1.5107203), - Point3::new(0.06949201, 0.112844266, -1.5279247), - Point3::new(0.07765801, 0.046329707, -1.5250553), - Point3::new(0.10110265, 0.112844266, -1.4633205), - Point3::new(0.07590854, 0.11284426, -1.5107203), - Point3::new(0.07995127, 0.046329707, -1.5189065), - Point3::new(0.10949655, 0.046329703, -1.4633205), - Point3::new(0.12289546, 0.0463297, -1.438112), - Point3::new(0.12289546, 0.112844266, -1.4223199), - Point3::new(0.10110265, 0.112844266, -1.4633205), - Point3::new(0.10949655, 0.046329703, -1.4633205), - Point3::new(0.09802142, 0.17935875, -1.7081411), - Point3::new(0.11421156, 0.17935875, -1.7248966), - Point3::new(0.11483169, 0.1128442, -1.7216887), - Point3::new(0.114465825, 0.11284419, -1.7213101), - Point3::new(0.12289546, 0.17935875, -1.7267503), - Point3::new(0.12289546, 0.113893166, -1.7234627), - Point3::new(0.11963135, 0.11284419, -1.7227132), - Point3::new(0.11483169, 0.11284419, -1.7216887), - Point3::new(0.11421156, 0.17935875, -1.7248966), - Point3::new(0.05638089, 0.17935875, -1.6680273), - Point3::new(0.098021425, 0.17935875, -1.7081411), - Point3::new(0.114465825, 0.1128442, -1.7213101), - Point3::new(0.11078018, 0.11284419, -1.7177596), - Point3::new(0.05638089, 0.13341987, -1.6661816), - Point3::new(0.056380868, 0.11284423, -1.6628642), - Point3::new(0.12289542, 0.11284423, -1.6628642), - Point3::new(0.12289542, 0.17935872, -1.6628642), - Point3::new(0.056380868, 0.17935872, -1.6628642), - Point3::new(0.056380868, 0.11284423, -1.5963496), - Point3::new(0.12289542, 0.11284423, -1.5963496), - Point3::new(0.12289542, 0.17935872, -1.5963496), - Point3::new(0.056380868, 0.17935872, -1.5963496), - Point3::new(0.05638089, 0.17935875, -1.5447178), - Point3::new(0.05638089, 0.11284419, -1.5648414), - Point3::new(0.06881347, 0.11284419, -1.5298351), - Point3::new(0.06405469, 0.1571329, -1.5298351), - Point3::new(0.06132602, 0.17935875, -1.5307939), - Point3::new(0.061683614, 0.17935875, -1.5298351), - Point3::new(0.06132602, 0.17935875, -1.5307939), - Point3::new(0.06405469, 0.1571329, -1.5298351), - Point3::new(0.06405503, 0.15713012, -1.529835), - Point3::new(0.06881352, 0.11284419, -1.529835), - Point3::new(0.06949202, 0.11284419, -1.5279245), - Point3::new(0.07590855, 0.11284419, -1.5107204), - Point3::new(0.07186582, 0.17935875, -1.5025342), - Point3::new(0.06168366, 0.17935875, -1.529835), - Point3::new(0.06405503, 0.15713012, -1.529835), - Point3::new(0.06949202, 0.1128442, -1.5279245), - Point3::new(0.09270881, 0.17935875, -1.4633205), - Point3::new(0.07186581, 0.17935875, -1.5025342), - Point3::new(0.07590854, 0.1128442, -1.5107204), - Point3::new(0.10110272, 0.11284419, -1.4633205), - Point3::new(0.12289546, 0.11284419, -1.42232), - Point3::new(0.12289546, 0.17935875, -1.4065279), - Point3::new(0.09270881, 0.17935875, -1.4633205), - Point3::new(0.10110272, 0.11284419, -1.4633205), - Point3::new(0.18941, -0.19789436, -1.820354), - Point3::new(0.18941, -0.2197285, -1.8267576), - Point3::new(0.18697423, -0.2197285, -1.8280702), - Point3::new(0.1578116, -0.15909654, -1.7958934), - Point3::new(0.13264129, -0.2197285, -1.8118136), - Point3::new(0.12289544, -0.2197285, -1.8116077), - Point3::new(0.12289544, -0.16181482, -1.7958934), - Point3::new(0.18941, -0.19789436, -1.820354), - Point3::new(0.18697423, -0.2197285, -1.8280703), - Point3::new(0.13264129, -0.2197285, -1.8118135), - Point3::new(0.15781142, -0.15909699, -1.7958934), - Point3::new(0.16541637, -0.15321395, -1.7958934), - Point3::new(0.18941, -0.15321395, -1.8030725), - Point3::new(0.12289544, -0.15321395, -1.7935597), - Point3::new(0.16025366, -0.15321395, -1.7943488), - Point3::new(0.15781179, -0.15909609, -1.7958933), - Point3::new(0.12289544, -0.16181439, -1.7958933), - Point3::new(0.16541599, -0.15321395, -1.7958933), - Point3::new(0.1578116, -0.15909654, -1.7958933), - Point3::new(0.16025366, -0.15321395, -1.7943487), - Point3::new(0.12289542, -0.21972859, -1.7293787), - Point3::new(0.18941003, -0.21972859, -1.7293787), - Point3::new(0.18941003, -0.15321398, -1.7293787), - Point3::new(0.12289542, -0.15321398, -1.7293787), - Point3::new(0.12289542, -0.21972859, -1.6628642), - Point3::new(0.18941003, -0.21972859, -1.6628642), - Point3::new(0.18941003, -0.15321398, -1.6628642), - Point3::new(0.12289542, -0.15321398, -1.6628642), - Point3::new(0.12289542, -0.21972859, -1.6628642), - Point3::new(0.18941003, -0.21972859, -1.6628642), - Point3::new(0.18941003, -0.15321398, -1.6628642), - Point3::new(0.12289542, -0.15321398, -1.6628642), - Point3::new(0.12289542, -0.21972859, -1.5963496), - Point3::new(0.18941003, -0.21972859, -1.5963496), - Point3::new(0.18941003, -0.15321398, -1.5963496), - Point3::new(0.12289542, -0.15321398, -1.5963496), - Point3::new(0.12289544, -0.15606314, -1.5298351), - Point3::new(0.12289544, -0.2197285, -1.5692852), - Point3::new(0.13997748, -0.2197285, -1.5315315), - Point3::new(0.1402694, -0.21803208, -1.5298351), - Point3::new(0.1402694, -0.21803208, -1.5298351), - Point3::new(0.13997748, -0.2197285, -1.5315315), - Point3::new(0.14072663, -0.2197285, -1.5298351), - Point3::new(0.12289544, -0.15606295, -1.529835), - Point3::new(0.14026941, -0.21803196, -1.529835), - Point3::new(0.151423, -0.15321395, -1.4650198), - Point3::new(0.12289544, -0.15321395, -1.5280696), - Point3::new(0.15217347, -0.15321395, -1.4633205), - Point3::new(0.151423, -0.15321395, -1.4650198), - Point3::new(0.14026941, -0.21803196, -1.529835), - Point3::new(0.14072669, -0.2197285, -1.529835), - Point3::new(0.15683821, -0.2197285, -1.4933524), - Point3::new(0.15627542, -0.16843303, -1.4633205), - Point3::new(0.15627542, -0.16843303, -1.4633205), - Point3::new(0.15683821, -0.2197285, -1.4933524), - Point3::new(0.18408938, -0.2197285, -1.4633205), - Point3::new(0.15217347, -0.15321395, -1.4633205), - Point3::new(0.15627542, -0.16843303, -1.4633205), - Point3::new(0.15610845, -0.15321395, -1.4544102), - Point3::new(0.18941, -0.15321395, -1.4177105), - Point3::new(0.15610845, -0.15321395, -1.4544102), - Point3::new(0.15627542, -0.16843303, -1.4633205), - Point3::new(0.18408938, -0.2197285, -1.4633205), - Point3::new(0.18941, -0.2197285, -1.457457), - Point3::new(0.18941, -0.13465314, -1.7958934), - Point3::new(0.18941, -0.15321401, -1.8030725), - Point3::new(0.16541636, -0.15321401, -1.7958934), - Point3::new(0.12289544, -0.08669945, -1.7755116), - Point3::new(0.187866, -0.08669945, -1.776884), - Point3::new(0.16025363, -0.15321401, -1.7943487), - Point3::new(0.12289544, -0.15321401, -1.7935597), - Point3::new(0.18941, -0.13465282, -1.7958933), - Point3::new(0.16541596, -0.15321401, -1.7958933), - Point3::new(0.16025363, -0.15321401, -1.7943487), - Point3::new(0.187866, -0.08669945, -1.7768838), - Point3::new(0.18941, -0.08669945, -1.7773458), - Point3::new(0.12289544, -0.10823502, -1.5001986), - Point3::new(0.12289544, -0.15321401, -1.5280696), - Point3::new(0.151423, -0.15321401, -1.4650198), - Point3::new(0.15171543, -0.1515146, -1.4633205), - Point3::new(0.14287871, -0.11999598, -1.4633205), - Point3::new(0.15171541, -0.1515146, -1.4633205), - Point3::new(0.15142299, -0.15321401, -1.4650198), - Point3::new(0.15217346, -0.15321401, -1.4633205), - Point3::new(0.26476443, -0.12245499, -1.3162864), - Point3::new(0.14287871, -0.11999598, -1.4633205), - Point3::new(0.15171543, -0.1515146, -1.4633205), - Point3::new(0.15582763, -0.12761694, -1.4394239), - Point3::new(0.15610845, -0.15321401, -1.4544102), - Point3::new(0.15582763, -0.12761694, -1.4394239), - Point3::new(0.15171541, -0.1515146, -1.4633205), - Point3::new(0.15217346, -0.15321401, -1.4633205), - Point3::new(0.18941, -0.12602565, -1.4014639), - Point3::new(0.15582763, -0.12761694, -1.4394239), - Point3::new(0.15610845, -0.15321401, -1.4544102), - Point3::new(0.18941, -0.15321401, -1.4177105), - Point3::new(0.18941, -0.020184837, -1.7588685), - Point3::new(0.18941, -0.082980156, -1.7759074), - Point3::new(0.18786602, -0.086699404, -1.776884), - Point3::new(0.12289544, -0.08669941, -1.7755116), - Point3::new(0.12289544, -0.020184837, -1.7574636), - Point3::new(0.18941, -0.082980156, -1.7759073), - Point3::new(0.18941, -0.086699404, -1.7773458), - Point3::new(0.18786603, -0.0866994, -1.7768838), - Point3::new(0.15419942, 0.046329774, -1.7400767), - Point3::new(0.18941, -0.011806563, -1.7565951), - Point3::new(0.18941, -0.020184793, -1.7588685), - Point3::new(0.12289544, -0.0201848, -1.7574635), - Point3::new(0.12289544, 0.046329774, -1.7394154), - Point3::new(0.12289544, 0.025428083, -1.4430746), - Point3::new(0.14631943, 0.034690574, -1.396806), - Point3::new(0.14485061, 0.04632977, -1.396806), - Point3::new(0.12289544, 0.046329767, -1.438112), - Point3::new(0.1463195, 0.034690596, -1.3968059), - Point3::new(0.1757539, 0.046329774, -1.3386651), - Point3::new(0.14485067, 0.04632977, -1.3968059), - Point3::new(0.12289544, 0.046329692, -1.7394154), - Point3::new(0.12289544, 0.083318785, -1.7293788), - Point3::new(0.13139604, 0.08398056, -1.7293788), - Point3::new(0.15419948, 0.0463297, -1.7400767), - Point3::new(0.12289544, 0.08331923, -1.7293787), - Point3::new(0.12289544, 0.09801593, -1.7253909), - Point3::new(0.13139579, 0.08398098, -1.7293787), - Point3::new(0.1364567, 0.112844266, -1.396806), - Point3::new(0.12289544, 0.112844266, -1.4223199), - Point3::new(0.12289544, 0.046329707, -1.438112), - Point3::new(0.1448506, 0.046329703, -1.396806), - Point3::new(0.17575371, 0.0463297, -1.3386655), - Point3::new(0.1799932, 0.048006106, -1.3302914), - Point3::new(0.17181085, 0.112844266, -1.3302914), - Point3::new(0.13645676, 0.112844266, -1.3968059), - Point3::new(0.14485067, 0.046329703, -1.3968059), - Point3::new(0.17999326, 0.04800613, -1.3302913), - Point3::new(0.18941, 0.051729772, -1.3116907), - Point3::new(0.18941, 0.07574098, -1.3059899), - Point3::new(0.17293426, 0.112844266, -1.3281778), - Point3::new(0.17181091, 0.112844266, -1.3302913), - Point3::new(0.18941, 0.07574098, -1.3059899), - Point3::new(0.18941, 0.112844266, -1.2902151), - Point3::new(0.17293426, 0.112844266, -1.3281778), - Point3::new(0.18529011, 0.17935875, -1.7400687), - Point3::new(0.18941, 0.17935875, -1.7406108), - Point3::new(0.18941, 0.16201851, -1.7400773), - Point3::new(0.18529011, 0.17935875, -1.7400687), - Point3::new(0.18941, 0.16201851, -1.7400773), - Point3::new(0.18941, 0.13526876, -1.738734), - Point3::new(0.14866325, 0.12217409, -1.7293788), - Point3::new(0.13520984, 0.17935875, -1.7293788), - Point3::new(0.13520929, 0.17935875, -1.7293787), - Point3::new(0.14866273, 0.12217393, -1.7293787), - Point3::new(0.12289544, 0.113893166, -1.7234627), - Point3::new(0.12289544, 0.17935875, -1.7267503), - Point3::new(0.12289542, 0.11284423, -1.6628642), - Point3::new(0.18941003, 0.11284423, -1.6628642), - Point3::new(0.18941003, 0.17935872, -1.6628642), - Point3::new(0.12289542, 0.17935872, -1.6628642), - Point3::new(0.12289542, 0.11284423, -1.5963496), - Point3::new(0.18941003, 0.11284423, -1.5963496), - Point3::new(0.18941003, 0.17935872, -1.5963496), - Point3::new(0.12289542, 0.17935872, -1.5963496), - Point3::new(0.12289542, 0.11284423, -1.5963496), - Point3::new(0.18941003, 0.11284423, -1.5963496), - Point3::new(0.18941003, 0.17935872, -1.5963496), - Point3::new(0.12289542, 0.17935872, -1.5963496), - Point3::new(0.12289542, 0.11284423, -1.529835), - Point3::new(0.18941003, 0.11284423, -1.529835), - Point3::new(0.18941003, 0.17935872, -1.529835), - Point3::new(0.12289542, 0.17935872, -1.529835), - Point3::new(0.12289542, 0.11284423, -1.529835), - Point3::new(0.18941003, 0.11284423, -1.529835), - Point3::new(0.18941003, 0.17935872, -1.529835), - Point3::new(0.12289542, 0.17935872, -1.529835), - Point3::new(0.12289542, 0.11284423, -1.4633205), - Point3::new(0.18941003, 0.11284423, -1.4633205), - Point3::new(0.18941003, 0.17935872, -1.4633205), - Point3::new(0.12289542, 0.17935872, -1.4633205), - Point3::new(0.12289544, 0.17935875, -1.4065279), - Point3::new(0.12289544, 0.11284419, -1.42232), - Point3::new(0.13645676, 0.11284419, -1.396806), - Point3::new(0.12806286, 0.17935875, -1.396806), - Point3::new(0.12806292, 0.17935875, -1.3968059), - Point3::new(0.13645682, 0.11284419, -1.3968059), - Point3::new(0.17181088, 0.11284419, -1.3302914), - Point3::new(0.17136484, 0.116378576, -1.3302914), - Point3::new(0.14339846, 0.17935875, -1.3679539), - Point3::new(0.15974389, 0.17935875, -1.3302914), - Point3::new(0.14339845, 0.17935875, -1.3679539), - Point3::new(0.17136484, 0.116378576, -1.3302914), - Point3::new(0.17136493, 0.116378374, -1.3302913), - Point3::new(0.17181094, 0.11284419, -1.3302913), - Point3::new(0.1729343, 0.11284419, -1.3281778), - Point3::new(0.18941, 0.11284419, -1.2902151), - Point3::new(0.18941, 0.175029, -1.2637768), - Point3::new(0.18861109, 0.17935875, -1.2637768), - Point3::new(0.15974393, 0.17935875, -1.3302913), - Point3::new(0.17136493, 0.116378374, -1.3302913), - Point3::new(0.1729343, 0.11284419, -1.3281778), - Point3::new(0.18941, 0.175029, -1.2637768), - Point3::new(0.18941, 0.17935875, -1.261936), - Point3::new(0.18861109, 0.17935875, -1.2637768), - Point3::new(0.24324219, -0.21341467, -1.7958934), - Point3::new(0.24456964, -0.2197285, -1.7970297), - Point3::new(0.18941, -0.2197285, -1.8267574), - Point3::new(0.18941, -0.19789436, -1.820354), - Point3::new(0.19439445, -0.15321395, -1.8045638), - Point3::new(0.21048221, -0.15321395, -1.7958934), - Point3::new(0.2504267, -0.2197285, -1.7958934), - Point3::new(0.24456964, -0.2197285, -1.7970297), - Point3::new(0.24324219, -0.21341465, -1.7958934), - Point3::new(0.18941, -0.19789436, -1.820354), - Point3::new(0.18941, -0.15321395, -1.8030725), - Point3::new(0.19439445, -0.15321395, -1.8045638), - Point3::new(0.24324206, -0.213414, -1.7958933), - Point3::new(0.21048243, -0.15321395, -1.7958933), - Point3::new(0.23058528, -0.15321395, -1.7850591), - Point3::new(0.25592455, -0.15321395, -1.7801431), - Point3::new(0.25592455, -0.21021768, -1.7927272), - Point3::new(0.25472918, -0.2197285, -1.7950587), - Point3::new(0.25042734, -0.2197285, -1.7958933), - Point3::new(0.24324206, -0.213414, -1.7958933), - Point3::new(0.23058528, -0.15321395, -1.7850591), - Point3::new(0.25592455, -0.21021768, -1.7927272), - Point3::new(0.25592455, -0.2197285, -1.7938863), - Point3::new(0.25472918, -0.2197285, -1.7950587), - Point3::new(0.18941003, -0.21972859, -1.7293787), - Point3::new(0.25592458, -0.21972859, -1.7293787), - Point3::new(0.25592458, -0.15321398, -1.7293787), - Point3::new(0.18941003, -0.15321398, -1.7293787), - Point3::new(0.18941003, -0.21972859, -1.6628642), - Point3::new(0.25592458, -0.21972859, -1.6628642), - Point3::new(0.25592458, -0.15321398, -1.6628642), - Point3::new(0.18941003, -0.15321398, -1.6628642), - Point3::new(0.18941003, -0.21972859, -1.6628642), - Point3::new(0.25592458, -0.21972859, -1.6628642), - Point3::new(0.25592458, -0.15321398, -1.6628642), - Point3::new(0.18941003, -0.15321398, -1.6628642), - Point3::new(0.18941003, -0.21972859, -1.5963496), - Point3::new(0.25592458, -0.21972859, -1.5963496), - Point3::new(0.25592458, -0.15321398, -1.5963496), - Point3::new(0.18941003, -0.15321398, -1.5963496), - Point3::new(0.18941003, -0.21972859, -1.5963496), - Point3::new(0.25592458, -0.21972859, -1.5963496), - Point3::new(0.25592458, -0.15321398, -1.5963496), - Point3::new(0.18941003, -0.15321398, -1.5963496), - Point3::new(0.18941003, -0.21972859, -1.529835), - Point3::new(0.25592458, -0.21972859, -1.529835), - Point3::new(0.25592458, -0.15321398, -1.529835), - Point3::new(0.18941003, -0.15321398, -1.529835), - Point3::new(0.18941003, -0.21972859, -1.529835), - Point3::new(0.25592458, -0.21972859, -1.529835), - Point3::new(0.25592458, -0.15321398, -1.529835), - Point3::new(0.18941003, -0.15321398, -1.529835), - Point3::new(0.18941003, -0.21972859, -1.4633205), - Point3::new(0.25592458, -0.21972859, -1.4633205), - Point3::new(0.25592458, -0.15321398, -1.4633205), - Point3::new(0.18941003, -0.15321398, -1.4633205), - Point3::new(0.247539, -0.21914232, -1.396806), - Point3::new(0.24743457, -0.2197285, -1.3972942), - Point3::new(0.25583023, -0.2197285, -1.396806), - Point3::new(0.23635623, -0.20481087, -1.396806), - Point3::new(0.23121047, -0.2197285, -1.411391), - Point3::new(0.24743459, -0.2197285, -1.3972942), - Point3::new(0.24753901, -0.21914232, -1.396806), - Point3::new(0.23121047, -0.2197285, -1.411391), - Point3::new(0.23635623, -0.20481087, -1.396806), - Point3::new(0.20837888, -0.15321395, -1.396806), - Point3::new(0.18941, -0.15321395, -1.4177105), - Point3::new(0.18941, -0.2197285, -1.457457), - Point3::new(0.24753903, -0.21914217, -1.3968059), - Point3::new(0.25583228, -0.2197285, -1.3968059), - Point3::new(0.25592455, -0.2197285, -1.3968005), - Point3::new(0.25592455, -0.17207375, -1.3576081), - Point3::new(0.25592455, -0.15321395, -1.3448215), - Point3::new(0.2541543, -0.15321395, -1.3463595), - Point3::new(0.23635626, -0.20481075, -1.3968059), - Point3::new(0.24753904, -0.21914217, -1.3968059), - Point3::new(0.25592455, -0.17207375, -1.3576081), - Point3::new(0.23635626, -0.20481075, -1.3968059), - Point3::new(0.2541543, -0.15321395, -1.3463595), - Point3::new(0.208379, -0.15321395, -1.3968059), - Point3::new(0.21048245, -0.15321401, -1.7958934), - Point3::new(0.19439445, -0.15321401, -1.8045639), - Point3::new(0.19713144, -0.1286798, -1.7958934), - Point3::new(0.18941, -0.15321401, -1.8030725), - Point3::new(0.18941, -0.13465305, -1.7958934), - Point3::new(0.19713144, -0.1286798, -1.7958934), - Point3::new(0.19439445, -0.15321401, -1.8045639), - Point3::new(0.20181468, -0.08669945, -1.7810575), - Point3::new(0.21660092, -0.08669945, -1.7730886), - Point3::new(0.23058529, -0.15321401, -1.7850592), - Point3::new(0.21048266, -0.15321401, -1.7958933), - Point3::new(0.19713148, -0.12867945, -1.7958933), - Point3::new(0.21660092, -0.08669945, -1.7730886), - Point3::new(0.25592455, -0.086699456, -1.7654594), - Point3::new(0.25592455, -0.15321401, -1.7801431), - Point3::new(0.23058529, -0.15321401, -1.7850592), - Point3::new(0.18941, -0.13465275, -1.7958933), - Point3::new(0.18941, -0.08669945, -1.7773459), - Point3::new(0.20181468, -0.08669945, -1.7810575), - Point3::new(0.19713148, -0.12867945, -1.7958933), - Point3::new(0.18941, -0.15321401, -1.4177105), - Point3::new(0.20837891, -0.15321401, -1.396806), - Point3::new(0.19353071, -0.12583038, -1.396806), - Point3::new(0.18941, -0.12602565, -1.4014639), - Point3::new(0.25592455, -0.15321401, -1.3448215), - Point3::new(0.25592455, -0.14808194, -1.341342), - Point3::new(0.25415426, -0.15321401, -1.3463596), - Point3::new(0.20837902, -0.15321401, -1.3968059), - Point3::new(0.25415426, -0.15321401, -1.3463596), - Point3::new(0.25592455, -0.14808196, -1.341342), - Point3::new(0.25592455, -0.12958905, -1.3302914), - Point3::new(0.25237462, -0.123042084, -1.3302914), - Point3::new(0.19353081, -0.12583038, -1.3968059), - Point3::new(0.25592455, -0.09874706, -1.3132844), - Point3::new(0.25592455, -0.08669945, -1.3077292), - Point3::new(0.25143242, -0.08669944, -1.3117589), - Point3::new(0.2523747, -0.123042084, -1.3302913), - Point3::new(0.25592455, -0.12958884, -1.3302913), - Point3::new(0.25592455, -0.12287387, -1.3262787), - Point3::new(0.20181468, -0.08669941, -1.7810574), - Point3::new(0.20694056, -0.04075135, -1.7648193), - Point3::new(0.21660092, -0.086699404, -1.7730886), - Point3::new(0.18941, -0.020184837, -1.7588685), - Point3::new(0.19448435, -0.020184837, -1.7589756), - Point3::new(0.20694056, -0.04075135, -1.7648193), - Point3::new(0.18941, -0.082980156, -1.7759074), - Point3::new(0.22242229, -0.020184837, -1.7572755), - Point3::new(0.25592455, -0.020184837, -1.7507757), - Point3::new(0.25592455, -0.08669941, -1.7654594), - Point3::new(0.21660091, -0.0866994, -1.7730886), - Point3::new(0.20694056, -0.04075135, -1.7648193), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.20181468, -0.086699404, -1.7810574), - Point3::new(0.18941, -0.086699404, -1.7773459), - Point3::new(0.18941, -0.082980156, -1.7759074), - Point3::new(0.20694056, -0.04075135, -1.7648193), - Point3::new(0.25592455, -0.020184837, -1.2770597), - Point3::new(0.22663139, -0.020184834, -1.3033363), - Point3::new(0.2514324, -0.0866994, -1.3117589), - Point3::new(0.25592455, -0.086699404, -1.3077292), - Point3::new(0.18941, -0.020184793, -1.7588685), - Point3::new(0.18941, -0.011806555, -1.7565951), - Point3::new(0.19448432, -0.020184793, -1.7589756), - Point3::new(0.22242233, -0.020184793, -1.7572755), - Point3::new(0.25592455, 0.02432083, -1.7409507), - Point3::new(0.25592455, -0.020184793, -1.7507757), - Point3::new(0.23654184, 0.046329774, -1.2637768), - Point3::new(0.20183037, 0.046329774, -1.2949136), - Point3::new(0.22663137, -0.020184789, -1.3033363), - Point3::new(0.25592455, -0.020184793, -1.2770597), - Point3::new(0.25592455, 0.008622458, -1.2637768), - Point3::new(0.23654184, 0.046329774, -1.2637768), - Point3::new(0.25592455, 0.008622458, -1.2637768), - Point3::new(0.25592455, 0.046329774, -1.2463901), - Point3::new(0.25592455, 0.09559846, -1.7297466), - Point3::new(0.23077853, 0.112844266, -1.7340378), - Point3::new(0.25592455, 0.112844266, -1.729814), - Point3::new(0.19847965, 0.05531616, -1.2937757), - Point3::new(0.21850373, 0.081421204, -1.2637768), - Point3::new(0.20192681, 0.10719351, -1.2637768), - Point3::new(0.21850373, 0.081421204, -1.2637768), - Point3::new(0.19847965, 0.05531616, -1.2937757), - Point3::new(0.2018304, 0.0463297, -1.2949136), - Point3::new(0.23654194, 0.046329696, -1.2637768), - Point3::new(0.18941, 0.07574098, -1.3059899), - Point3::new(0.18941, 0.05172977, -1.3116907), - Point3::new(0.19847965, 0.05531616, -1.2937757), - Point3::new(0.19847965, 0.05531616, -1.2937757), - Point3::new(0.20192681, 0.1071935, -1.2637768), - Point3::new(0.20088415, 0.112844266, -1.2637768), - Point3::new(0.18941, 0.11284426, -1.2902151), - Point3::new(0.18941, 0.07574096, -1.3059899), - Point3::new(0.21850373, 0.081421204, -1.2637768), - Point3::new(0.24260704, 0.112844266, -1.2276666), - Point3::new(0.20230229, 0.11284427, -1.2605091), - Point3::new(0.20192681, 0.10719351, -1.2637768), - Point3::new(0.21850373, 0.081421204, -1.2637768), - Point3::new(0.23654194, 0.046329696, -1.2637768), - Point3::new(0.25592455, 0.046329692, -1.2463902), - Point3::new(0.25592455, 0.112844266, -1.2157207), - Point3::new(0.24260704, 0.112844266, -1.2276666), - Point3::new(0.20192681, 0.1071935, -1.2637768), - Point3::new(0.20230229, 0.112844266, -1.2605091), - Point3::new(0.20088415, 0.112844266, -1.2637768), - Point3::new(0.18941, 0.16201851, -1.7400773), - Point3::new(0.18941, 0.17935875, -1.7406108), - Point3::new(0.19153188, 0.17935875, -1.7408901), - Point3::new(0.19531466, 0.13716632, -1.7400897), - Point3::new(0.25592455, 0.11284419, -1.729814), - Point3::new(0.23077863, 0.11284419, -1.7340378), - Point3::new(0.19531466, 0.13716632, -1.7400897), - Point3::new(0.19153188, 0.17935875, -1.7408901), - Point3::new(0.25592455, 0.17935875, -1.7300744), - Point3::new(0.18941, 0.13526876, -1.738734), - Point3::new(0.18941, 0.16201851, -1.7400773), - Point3::new(0.19531466, 0.13716632, -1.7400897), - Point3::new(0.18941003, 0.11284423, -1.7293787), - Point3::new(0.25592458, 0.11284423, -1.7293787), - Point3::new(0.25592458, 0.17935872, -1.7293787), - Point3::new(0.18941003, 0.17935872, -1.7293787), - Point3::new(0.18941003, 0.11284423, -1.6628642), - Point3::new(0.25592458, 0.11284423, -1.6628642), - Point3::new(0.25592458, 0.17935872, -1.6628642), - Point3::new(0.18941003, 0.17935872, -1.6628642), - Point3::new(0.18941003, 0.11284423, -1.6628642), - Point3::new(0.25592458, 0.11284423, -1.6628642), - Point3::new(0.25592458, 0.17935872, -1.6628642), - Point3::new(0.18941003, 0.17935872, -1.6628642), - Point3::new(0.18941003, 0.11284423, -1.5963496), - Point3::new(0.25592458, 0.11284423, -1.5963496), - Point3::new(0.25592458, 0.17935872, -1.5963496), - Point3::new(0.18941003, 0.17935872, -1.5963496), - Point3::new(0.18941003, 0.11284423, -1.5963496), - Point3::new(0.25592458, 0.11284423, -1.5963496), - Point3::new(0.25592458, 0.17935872, -1.5963496), - Point3::new(0.18941003, 0.17935872, -1.5963496), - Point3::new(0.18941003, 0.11284423, -1.529835), - Point3::new(0.25592458, 0.11284423, -1.529835), - Point3::new(0.25592458, 0.17935872, -1.529835), - Point3::new(0.18941003, 0.17935872, -1.529835), - Point3::new(0.18941003, 0.11284423, -1.529835), - Point3::new(0.25592458, 0.11284423, -1.529835), - Point3::new(0.25592458, 0.17935872, -1.529835), - Point3::new(0.18941003, 0.17935872, -1.529835), - Point3::new(0.18941003, 0.11284423, -1.4633205), - Point3::new(0.25592458, 0.11284423, -1.4633205), - Point3::new(0.25592458, 0.17935872, -1.4633205), - Point3::new(0.18941003, 0.17935872, -1.4633205), - Point3::new(0.18941003, 0.11284423, -1.4633205), - Point3::new(0.25592458, 0.11284423, -1.4633205), - Point3::new(0.25592458, 0.17935872, -1.4633205), - Point3::new(0.18941003, 0.17935872, -1.4633205), - Point3::new(0.18941003, 0.11284423, -1.396806), - Point3::new(0.25592458, 0.11284423, -1.396806), - Point3::new(0.25592458, 0.17935872, -1.396806), - Point3::new(0.18941003, 0.17935872, -1.396806), - Point3::new(0.18941003, 0.11284423, -1.396806), - Point3::new(0.25592458, 0.11284423, -1.396806), - Point3::new(0.25592458, 0.17935872, -1.396806), - Point3::new(0.18941003, 0.17935872, -1.396806), - Point3::new(0.18941003, 0.11284423, -1.3302914), - Point3::new(0.25592458, 0.11284423, -1.3302914), - Point3::new(0.25592458, 0.17935872, -1.3302914), - Point3::new(0.18941003, 0.17935872, -1.3302914), - Point3::new(0.18941, 0.175029, -1.2637768), - Point3::new(0.18941, 0.1128442, -1.2902151), - Point3::new(0.20088415, 0.1128442, -1.2637768), - Point3::new(0.25592455, 0.13020608, -1.2077152), - Point3::new(0.25592455, 0.15014967, -1.1972623), - Point3::new(0.23713702, 0.17935875, -1.1972623), - Point3::new(0.20672207, 0.17935875, -1.222046), - Point3::new(0.20230229, 0.112844184, -1.2605091), - Point3::new(0.24260698, 0.11284419, -1.2276667), - Point3::new(0.25592455, 0.112844184, -1.2157207), - Point3::new(0.25592455, 0.13020608, -1.2077152), - Point3::new(0.24260698, 0.11284419, -1.2276667), - Point3::new(0.20230229, 0.1128442, -1.2605091), - Point3::new(0.20672207, 0.17935875, -1.222046), - Point3::new(0.18941, 0.17935875, -1.261936), - Point3::new(0.18941, 0.175029, -1.2637768), - Point3::new(0.20088415, 0.1128442, -1.2637768), - Point3::new(0.25592455, 0.15014991, -1.1972622), - Point3::new(0.25592455, 0.17935875, -1.1819532), - Point3::new(0.23713715, 0.17935875, -1.1972622), - Point3::new(0.2559246, -0.21021721, -1.7927271), - Point3::new(0.2559246, -0.15321395, -1.7801431), - Point3::new(0.2630891, -0.15321395, -1.7787532), - Point3::new(0.28173494, -0.15321395, -1.7604647), - Point3::new(0.2788983, -0.2197285, -1.7713528), - Point3::new(0.2559246, -0.2197285, -1.7938862), - Point3::new(0.2559246, -0.21021721, -1.7927271), - Point3::new(0.2630891, -0.15321395, -1.7787532), - Point3::new(0.28173494, -0.15321395, -1.7604647), - Point3::new(0.30298996, -0.15321395, -1.7469393), - Point3::new(0.28517705, -0.2197285, -1.7673575), - Point3::new(0.2788983, -0.2197285, -1.7713529), - Point3::new(0.28517705, -0.2197285, -1.7673575), - Point3::new(0.30298996, -0.15321395, -1.7469393), - Point3::new(0.3175517, -0.15321395, -1.7293788), - Point3::new(0.31705132, -0.19096869, -1.7293788), - Point3::new(0.29707673, -0.2197285, -1.7530072), - Point3::new(0.31142703, -0.2197285, -1.7293788), - Point3::new(0.29707673, -0.2197285, -1.7530072), - Point3::new(0.31705132, -0.19096869, -1.7293788), - Point3::new(0.3170514, -0.19096854, -1.7293787), - Point3::new(0.3175518, -0.15321395, -1.7293787), - Point3::new(0.32243916, -0.15321395, -1.7234849), - Point3::new(0.32243916, -0.18321115, -1.7230054), - Point3::new(0.32243916, -0.18321115, -1.7230054), - Point3::new(0.32243916, -0.2197285, -1.7112468), - Point3::new(0.3114271, -0.2197285, -1.7293787), - Point3::new(0.3170514, -0.19096854, -1.7293787), - Point3::new(0.25592458, -0.21972859, -1.6628642), - Point3::new(0.32243913, -0.21972859, -1.6628642), - Point3::new(0.32243913, -0.15321398, -1.6628642), - Point3::new(0.25592458, -0.15321398, -1.6628642), - Point3::new(0.25592458, -0.21972859, -1.5963496), - Point3::new(0.32243913, -0.21972859, -1.5963496), - Point3::new(0.32243913, -0.15321398, -1.5963496), - Point3::new(0.25592458, -0.15321398, -1.5963496), - Point3::new(0.25592458, -0.21972859, -1.5963496), - Point3::new(0.32243913, -0.21972859, -1.5963496), - Point3::new(0.32243913, -0.15321398, -1.5963496), - Point3::new(0.25592458, -0.15321398, -1.5963496), - Point3::new(0.25592458, -0.21972859, -1.529835), - Point3::new(0.32243913, -0.21972859, -1.529835), - Point3::new(0.32243913, -0.15321398, -1.529835), - Point3::new(0.25592458, -0.15321398, -1.529835), - Point3::new(0.25592458, -0.21972859, -1.529835), - Point3::new(0.32243913, -0.21972859, -1.529835), - Point3::new(0.32243913, -0.15321398, -1.529835), - Point3::new(0.25592458, -0.15321398, -1.529835), - Point3::new(0.25592458, -0.21972859, -1.4633205), - Point3::new(0.32243913, -0.21972859, -1.4633205), - Point3::new(0.32243913, -0.15321398, -1.4633205), - Point3::new(0.25592458, -0.15321398, -1.4633205), - Point3::new(0.32243916, -0.2197285, -1.4047158), - Point3::new(0.32243916, -0.21268144, -1.396806), - Point3::new(0.31990728, -0.21465531, -1.396806), - Point3::new(0.31870013, -0.2197285, -1.401444), - Point3::new(0.30893937, -0.2197285, -1.396806), - Point3::new(0.31870013, -0.2197285, -1.4014438), - Point3::new(0.31990725, -0.21465544, -1.396806), - Point3::new(0.32243916, -0.21268134, -1.3968059), - Point3::new(0.32243916, -0.20401484, -1.3870784), - Point3::new(0.3199073, -0.21465518, -1.3968059), - Point3::new(0.32243916, -0.16951019, -1.3516326), - Point3::new(0.32243916, -0.15321395, -1.3382301), - Point3::new(0.25928453, -0.15321395, -1.341902), - Point3::new(0.2559246, -0.17207341, -1.3576078), - Point3::new(0.2559246, -0.2197285, -1.3968005), - Point3::new(0.3031494, -0.2197285, -1.3940549), - Point3::new(0.32243916, -0.20401485, -1.3870783), - Point3::new(0.32243916, -0.16951019, -1.3516326), - Point3::new(0.3031494, -0.2197285, -1.3940549), - Point3::new(0.30893913, -0.2197285, -1.3968059), - Point3::new(0.31990728, -0.21465531, -1.3968059), - Point3::new(0.2559246, -0.17207342, -1.3576078), - Point3::new(0.25928453, -0.15321395, -1.341902), - Point3::new(0.2559246, -0.15321395, -1.3448215), - Point3::new(0.2559246, -0.08669945, -1.7654594), - Point3::new(0.27144903, -0.086699456, -1.7624476), - Point3::new(0.2630891, -0.15321401, -1.7787532), - Point3::new(0.2559246, -0.15321401, -1.7801431), - Point3::new(0.27144903, -0.08669945, -1.7624476), - Point3::new(0.2845716, -0.086699456, -1.7495764), - Point3::new(0.28173494, -0.15321401, -1.7604647), - Point3::new(0.2630891, -0.15321401, -1.7787532), - Point3::new(0.2845716, -0.08669945, -1.7495764), - Point3::new(0.31631216, -0.08669945, -1.7293788), - Point3::new(0.31830987, -0.09600836, -1.7293788), - Point3::new(0.30298993, -0.15321401, -1.7469393), - Point3::new(0.28173494, -0.15321401, -1.7604647), - Point3::new(0.3175517, -0.15321401, -1.7293788), - Point3::new(0.30298993, -0.15321401, -1.7469393), - Point3::new(0.31830987, -0.09600836, -1.7293788), - Point3::new(0.31631237, -0.08669945, -1.7293787), - Point3::new(0.32080284, -0.08669945, -1.7265213), - Point3::new(0.31830996, -0.096007966, -1.7293787), - Point3::new(0.32080284, -0.08669945, -1.7265213), - Point3::new(0.32243916, -0.08669945, -1.724548), - Point3::new(0.32243916, -0.15321401, -1.7234849), - Point3::new(0.3175518, -0.15321401, -1.7293787), - Point3::new(0.31830996, -0.09600797, -1.7293787), - Point3::new(0.26176837, -0.13927203, -1.3302914), - Point3::new(0.25928453, -0.15321401, -1.341902), - Point3::new(0.32243916, -0.15321401, -1.3382303), - Point3::new(0.32243916, -0.14356102, -1.3302914), - Point3::new(0.2559246, -0.15321401, -1.3448215), - Point3::new(0.25928453, -0.15321401, -1.341902), - Point3::new(0.26176837, -0.13927203, -1.3302914), - Point3::new(0.25982332, -0.13677931, -1.3302914), - Point3::new(0.2559246, -0.14808176, -1.3413419), - Point3::new(0.2559246, -0.1295892, -1.3302914), - Point3::new(0.2559246, -0.14808178, -1.3413419), - Point3::new(0.25982332, -0.13677931, -1.3302914), - Point3::new(0.32243916, -0.11582225, -1.3074783), - Point3::new(0.26476443, -0.12245499, -1.3162864), - Point3::new(0.2617684, -0.1392719, -1.3302913), - Point3::new(0.32243916, -0.14356089, -1.3302913), - Point3::new(0.34401476, -0.113340996, -1.3041832), - Point3::new(0.3887098, 0.049111772, -1.1970468), - Point3::new(0.26476443, -0.12245499, -1.3162864), - Point3::new(0.29059538, -0.08669945, -1.2914362), - Point3::new(0.27026275, -0.086699456, -1.2948676), - Point3::new(0.25982338, -0.13677919, -1.3302913), - Point3::new(0.2617684, -0.1392719, -1.3302913), - Point3::new(0.26476443, -0.12245499, -1.3162864), - Point3::new(0.26476443, -0.12245499, -1.3162864), - Point3::new(0.27026275, -0.08669945, -1.2948676), - Point3::new(0.2559246, -0.08669944, -1.3077292), - Point3::new(0.2559246, -0.09874722, -1.3132844), - Point3::new(0.26476443, -0.12245499, -1.3162864), - Point3::new(0.2559246, -0.122873865, -1.3262786), - Point3::new(0.2559246, -0.12958899, -1.3302913), - Point3::new(0.25982338, -0.13677919, -1.3302913), - Point3::new(0.2559246, -0.020184837, -1.7507757), - Point3::new(0.27980897, -0.020184837, -1.7461419), - Point3::new(0.27144903, -0.0866994, -1.7624476), - Point3::new(0.2559246, -0.08669941, -1.7654594), - Point3::new(0.27980897, -0.020184837, -1.7461419), - Point3::new(0.2874082, -0.020184837, -1.7386883), - Point3::new(0.2845716, -0.0866994, -1.7495764), - Point3::new(0.27144903, -0.086699404, -1.7624476), - Point3::new(0.3163122, -0.086699404, -1.7293788), - Point3::new(0.28457156, -0.08669941, -1.7495764), - Point3::new(0.2874082, -0.020184837, -1.7386883), - Point3::new(0.30203813, -0.020184837, -1.7293788), - Point3::new(0.32243916, -0.020184837, -1.7163969), - Point3::new(0.32243916, -0.080589324, -1.7246456), - Point3::new(0.32080287, -0.086699404, -1.7265213), - Point3::new(0.31631237, -0.086699404, -1.7293787), - Point3::new(0.3020383, -0.020184837, -1.7293787), - Point3::new(0.32080287, -0.08669941, -1.7265213), - Point3::new(0.32243916, -0.08058934, -1.7246456), - Point3::new(0.32243916, -0.086699404, -1.724548), - Point3::new(0.27824393, -0.034797788, -1.2637768), - Point3::new(0.27026275, -0.0866994, -1.2948676), - Point3::new(0.2905954, -0.086699404, -1.2914362), - Point3::new(0.3193464, -0.04690194, -1.2637768), - Point3::new(0.27073243, -0.020184834, -1.2637768), - Point3::new(0.2559246, -0.02018483, -1.2770597), - Point3::new(0.2559246, -0.08669941, -1.3077292), - Point3::new(0.27026275, -0.086699404, -1.2948676), - Point3::new(0.27824393, -0.034797784, -1.2637768), - Point3::new(0.32243916, -0.042620897, -1.2608014), - Point3::new(0.32243916, -0.020184837, -1.2479438), - Point3::new(0.28049102, -0.020184845, -1.2550231), - Point3::new(0.27824393, -0.034797788, -1.2637768), - Point3::new(0.3193464, -0.04690194, -1.2637768), - Point3::new(0.27073243, -0.020184834, -1.2637768), - Point3::new(0.27824393, -0.034797784, -1.2637768), - Point3::new(0.28049102, -0.020184837, -1.2550231), - Point3::new(0.2724921, 0.046329774, -1.7328777), - Point3::new(0.28816888, 0.046329774, -1.7298363), - Point3::new(0.27980897, -0.020184793, -1.7461419), - Point3::new(0.2559246, -0.020184785, -1.7507757), - Point3::new(0.2559246, 0.024320912, -1.7409506), - Point3::new(0.28816888, 0.046329774, -1.7298363), - Point3::new(0.28863534, 0.046329774, -1.7293788), - Point3::new(0.28983358, 0.03668579, -1.7293788), - Point3::new(0.2874082, -0.020184793, -1.7386883), - Point3::new(0.27980897, -0.020184785, -1.7461419), - Point3::new(0.30203813, -0.020184796, -1.7293788), - Point3::new(0.2874082, -0.0201848, -1.7386883), - Point3::new(0.28983358, 0.03668579, -1.7293788), - Point3::new(0.28863546, 0.046329774, -1.7293787), - Point3::new(0.29024485, 0.046329774, -1.7278001), - Point3::new(0.2898336, 0.03668652, -1.7293787), - Point3::new(0.29024485, 0.046329774, -1.7278001), - Point3::new(0.32243916, 0.046329774, -1.7073138), - Point3::new(0.32243916, -0.020184793, -1.7163969), - Point3::new(0.3020383, -0.020184796, -1.7293787), - Point3::new(0.2898336, 0.036686517, -1.7293787), - Point3::new(0.2559246, 0.008622456, -1.2637768), - Point3::new(0.2559246, -0.0201848, -1.2770597), - Point3::new(0.27073243, -0.020184796, -1.2637768), - Point3::new(0.32243916, -0.020184793, -1.2479438), - Point3::new(0.32243916, 0.046329774, -1.2098254), - Point3::new(0.2907193, 0.046329767, -1.2151786), - Point3::new(0.28049102, -0.0201848, -1.2550231), - Point3::new(0.2907193, 0.046329774, -1.2151786), - Point3::new(0.2559246, 0.04632978, -1.2463901), - Point3::new(0.2559246, 0.008622456, -1.2637768), - Point3::new(0.27073243, -0.020184796, -1.2637768), - Point3::new(0.28049102, -0.020184793, -1.2550231), - Point3::new(0.2559246, 0.112844266, -1.7298139), - Point3::new(0.25851518, 0.112844266, -1.7293788), - Point3::new(0.258079, 0.0941209, -1.7293788), - Point3::new(0.2559246, 0.09559842, -1.7297465), - Point3::new(0.27249205, 0.0463297, -1.7328777), - Point3::new(0.27967268, 0.055868708, -1.7293788), - Point3::new(0.28840345, 0.048196055, -1.7293788), - Point3::new(0.28816888, 0.0463297, -1.7298363), - Point3::new(0.28816888, 0.046329707, -1.7298363), - Point3::new(0.28840345, 0.04819606, -1.7293788), - Point3::new(0.28863534, 0.046329707, -1.7293788), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.2998039, 0.112844266, -1.717168), - Point3::new(0.32243916, 0.112844266, -1.7004257), - Point3::new(0.32243916, 0.09600361, -1.7005304), - Point3::new(0.2585159, 0.112844266, -1.7293787), - Point3::new(0.2798679, 0.112844266, -1.7257924), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.25807968, 0.09412042, -1.7293787), - Point3::new(0.2798679, 0.112844266, -1.7257924), - Point3::new(0.29007727, 0.11284426, -1.7226921), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.29007727, 0.112844266, -1.7226921), - Point3::new(0.2998039, 0.112844266, -1.717168), - Point3::new(0.27967292, 0.055869035, -1.7293787), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.2884035, 0.04819654, -1.7293787), - Point3::new(0.2884035, 0.048196547, -1.7293787), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.29024485, 0.0463297, -1.7278001), - Point3::new(0.28863546, 0.046329707, -1.7293787), - Point3::new(0.291311, 0.071329504, -1.7237078), - Point3::new(0.32243916, 0.09600361, -1.7005304), - Point3::new(0.32243916, 0.0463297, -1.7073138), - Point3::new(0.29024485, 0.046329692, -1.7278001), - Point3::new(0.32243916, 0.0463297, -1.2098254), - Point3::new(0.32243916, 0.06825167, -1.1972623), - Point3::new(0.2953185, 0.07623838, -1.1972623), - Point3::new(0.2907193, 0.046329707, -1.2151786), - Point3::new(0.27650195, 0.112844266, -1.1972623), - Point3::new(0.2559246, 0.112844266, -1.2157205), - Point3::new(0.2559246, 0.0463297, -1.2463901), - Point3::new(0.2907193, 0.0463297, -1.2151786), - Point3::new(0.2953185, 0.07623837, -1.1972623), - Point3::new(0.32243916, 0.06825188, -1.1972622), - Point3::new(0.32243916, 0.112844266, -1.171707), - Point3::new(0.30094758, 0.112844266, -1.1753341), - Point3::new(0.29531854, 0.07623857, -1.1972622), - Point3::new(0.27650207, 0.112844266, -1.1972622), - Point3::new(0.29531854, 0.07623857, -1.1972622), - Point3::new(0.30094758, 0.112844266, -1.1753341), - Point3::new(0.2559246, 0.17935875, -1.7300744), - Point3::new(0.26006553, 0.17935875, -1.7293788), - Point3::new(0.25851518, 0.11284419, -1.7293788), - Point3::new(0.2559246, 0.11284419, -1.7298139), - Point3::new(0.31341112, 0.17935875, -1.7066901), - Point3::new(0.32243916, 0.17935875, -1.7000124), - Point3::new(0.32243916, 0.11284419, -1.7004257), - Point3::new(0.29980388, 0.11284419, -1.7171681), - Point3::new(0.26006624, 0.17935875, -1.7293787), - Point3::new(0.26153383, 0.17935875, -1.7291322), - Point3::new(0.2798679, 0.11284419, -1.7257924), - Point3::new(0.2585159, 0.11284419, -1.7293787), - Point3::new(0.28810057, 0.17935875, -1.7210648), - Point3::new(0.29007727, 0.1128442, -1.7226921), - Point3::new(0.2798679, 0.11284419, -1.7257924), - Point3::new(0.26153383, 0.17935875, -1.7291322), - Point3::new(0.29007727, 0.11284419, -1.7226921), - Point3::new(0.28810057, 0.17935875, -1.7210648), - Point3::new(0.31341112, 0.17935875, -1.7066901), - Point3::new(0.29980388, 0.1128442, -1.7171681), - Point3::new(0.25592458, 0.11284423, -1.6628642), - Point3::new(0.32243913, 0.11284423, -1.6628642), - Point3::new(0.32243913, 0.17935872, -1.6628642), - Point3::new(0.25592458, 0.17935872, -1.6628642), - Point3::new(0.25592458, 0.11284423, -1.5963496), - Point3::new(0.32243913, 0.11284423, -1.5963496), - Point3::new(0.32243913, 0.17935872, -1.5963496), - Point3::new(0.25592458, 0.17935872, -1.5963496), - Point3::new(0.25592458, 0.11284423, -1.5963496), - Point3::new(0.32243913, 0.11284423, -1.5963496), - Point3::new(0.32243913, 0.17935872, -1.5963496), - Point3::new(0.25592458, 0.17935872, -1.5963496), - Point3::new(0.25592458, 0.11284423, -1.529835), - Point3::new(0.32243913, 0.11284423, -1.529835), - Point3::new(0.32243913, 0.17935872, -1.529835), - Point3::new(0.25592458, 0.17935872, -1.529835), - Point3::new(0.25592458, 0.11284423, -1.529835), - Point3::new(0.32243913, 0.11284423, -1.529835), - Point3::new(0.32243913, 0.17935872, -1.529835), - Point3::new(0.25592458, 0.17935872, -1.529835), - Point3::new(0.25592458, 0.11284423, -1.4633205), - Point3::new(0.32243913, 0.11284423, -1.4633205), - Point3::new(0.32243913, 0.17935872, -1.4633205), - Point3::new(0.25592458, 0.17935872, -1.4633205), - Point3::new(0.25592458, 0.11284423, -1.4633205), - Point3::new(0.32243913, 0.11284423, -1.4633205), - Point3::new(0.32243913, 0.17935872, -1.4633205), - Point3::new(0.25592458, 0.17935872, -1.4633205), - Point3::new(0.25592458, 0.11284423, -1.396806), - Point3::new(0.32243913, 0.11284423, -1.396806), - Point3::new(0.32243913, 0.17935872, -1.396806), - Point3::new(0.25592458, 0.17935872, -1.396806), - Point3::new(0.25592458, 0.11284423, -1.396806), - Point3::new(0.32243913, 0.11284423, -1.396806), - Point3::new(0.32243913, 0.17935872, -1.396806), - Point3::new(0.25592458, 0.17935872, -1.396806), - Point3::new(0.25592458, 0.11284423, -1.3302914), - Point3::new(0.32243913, 0.11284423, -1.3302914), - Point3::new(0.32243913, 0.17935872, -1.3302914), - Point3::new(0.25592458, 0.17935872, -1.3302914), - Point3::new(0.25592458, 0.11284423, -1.3302914), - Point3::new(0.32243913, 0.11284423, -1.3302914), - Point3::new(0.32243913, 0.17935872, -1.3302914), - Point3::new(0.25592458, 0.17935872, -1.3302914), - Point3::new(0.25592458, 0.11284423, -1.2637768), - Point3::new(0.32243913, 0.11284423, -1.2637768), - Point3::new(0.32243913, 0.17935872, -1.2637768), - Point3::new(0.25592458, 0.17935872, -1.2637768), - Point3::new(0.2559246, 0.13020615, -1.207715), - Point3::new(0.26290175, 0.13930213, -1.1972623), - Point3::new(0.2559246, 0.15014958, -1.1972623), - Point3::new(0.26290175, 0.13930213, -1.1972623), - Point3::new(0.2559246, 0.13020615, -1.207715), - Point3::new(0.2559246, 0.11284419, -1.2157205), - Point3::new(0.276502, 0.11284419, -1.1972623), - Point3::new(0.26290184, 0.13930224, -1.1972622), - Point3::new(0.2936275, 0.17935875, -1.1512308), - Point3::new(0.2559246, 0.17935875, -1.1819532), - Point3::new(0.2559246, 0.15014982, -1.1972622), - Point3::new(0.32243916, 0.1128442, -1.1717072), - Point3::new(0.32243916, 0.17935875, -1.1335888), - Point3::new(0.31117585, 0.17935875, -1.1354897), - Point3::new(0.30094758, 0.11284419, -1.1753342), - Point3::new(0.26290184, 0.13930224, -1.1972622), - Point3::new(0.27650213, 0.11284419, -1.1972622), - Point3::new(0.30094758, 0.11284419, -1.1753342), - Point3::new(0.31117585, 0.17935875, -1.1354897), - Point3::new(0.2936275, 0.17935875, -1.1512308), - Point3::new(0.32243916, -0.18321115, -1.7230054), - Point3::new(0.32243916, -0.15321395, -1.7234849), - Point3::new(0.34327316, -0.15321395, -1.6983604), - Point3::new(0.34327316, -0.15321395, -1.6983604), - Point3::new(0.36483112, -0.15321395, -1.6628642), - Point3::new(0.35182345, -0.2197285, -1.6628642), - Point3::new(0.32243916, -0.2197285, -1.7112468), - Point3::new(0.32243916, -0.18321115, -1.7230054), - Point3::new(0.38536644, -0.2197285, -1.5963497), - Point3::new(0.36939064, -0.2197285, -1.633939), - Point3::new(0.38697082, -0.15321395, -1.6264101), - Point3::new(0.38895372, -0.15321395, -1.6217446), - Point3::new(0.38895372, -0.20313594, -1.5963497), - Point3::new(0.36483112, -0.15321395, -1.6628642), - Point3::new(0.38697082, -0.15321395, -1.6264101), - Point3::new(0.36939064, -0.2197285, -1.633939), - Point3::new(0.35182345, -0.2197285, -1.6628642), - Point3::new(0.38886338, -0.2197285, -1.5615234), - Point3::new(0.38895372, -0.21955214, -1.5613409), - Point3::new(0.38895372, -0.2197285, -1.5610058), - Point3::new(0.38895372, -0.21955213, -1.5613409), - Point3::new(0.38886338, -0.2197285, -1.5615236), - Point3::new(0.38743, -0.2197285, -1.5914943), - Point3::new(0.38895372, -0.21664403, -1.5894783), - Point3::new(0.38895372, -0.21664403, -1.5894783), - Point3::new(0.38743, -0.2197285, -1.5914943), - Point3::new(0.38536647, -0.2197285, -1.5963496), - Point3::new(0.38895372, -0.20313618, -1.5963496), - Point3::new(0.38432705, -0.2197285, -1.4633205), - Point3::new(0.38523936, -0.2197285, -1.4643732), - Point3::new(0.38572723, -0.21845657, -1.4633205), - Point3::new(0.38895372, -0.2197285, -1.4771357), - Point3::new(0.38895372, -0.21328956, -1.4633205), - Point3::new(0.38572723, -0.21845657, -1.4633205), - Point3::new(0.38523936, -0.2197285, -1.4643732), - Point3::new(0.38895372, -0.21004458, -1.4563582), - Point3::new(0.38895372, -0.18089001, -1.419324), - Point3::new(0.3683665, -0.2197285, -1.4449029), - Point3::new(0.38432705, -0.2197285, -1.4633205), - Point3::new(0.38572723, -0.21845657, -1.4633205), - Point3::new(0.32243916, -0.21268144, -1.396806), - Point3::new(0.32243916, -0.2197285, -1.4047158), - Point3::new(0.3683665, -0.2197285, -1.4449029), - Point3::new(0.38895372, -0.18089001, -1.419324), - Point3::new(0.38895372, -0.16082796, -1.396806), - Point3::new(0.38895372, -0.21328956, -1.4633205), - Point3::new(0.38895372, -0.2100446, -1.4563582), - Point3::new(0.38572723, -0.21845657, -1.4633205), - Point3::new(0.32243916, -0.21268134, -1.3968059), - Point3::new(0.38895372, -0.16082785, -1.3968059), - Point3::new(0.38895372, -0.15321395, -1.3882599), - Point3::new(0.3345271, -0.15321395, -1.3406355), - Point3::new(0.32243916, -0.20401484, -1.3870784), - Point3::new(0.32243916, -0.16951019, -1.3516326), - Point3::new(0.32869884, -0.15321395, -1.3378662), - Point3::new(0.32243916, -0.15321395, -1.3382301), - Point3::new(0.32869884, -0.15321395, -1.3378662), - Point3::new(0.32243916, -0.16951019, -1.3516326), - Point3::new(0.32243916, -0.20401485, -1.3870783), - Point3::new(0.3345271, -0.15321395, -1.3406355), - Point3::new(0.37328035, -0.11000898, -1.6628642), - Point3::new(0.3432731, -0.15321401, -1.6983604), - Point3::new(0.32243916, -0.15321401, -1.7234849), - Point3::new(0.32243916, -0.08669945, -1.724548), - Point3::new(0.37358934, -0.08669945, -1.6628642), - Point3::new(0.36483112, -0.15321401, -1.6628642), - Point3::new(0.3432731, -0.15321401, -1.6983604), - Point3::new(0.37328035, -0.110008985, -1.6628642), - Point3::new(0.38895372, -0.08669945, -1.6443357), - Point3::new(0.38895372, -0.08744214, -1.6443238), - Point3::new(0.37328035, -0.11000898, -1.6628642), - Point3::new(0.37358934, -0.08669945, -1.6628642), - Point3::new(0.38895372, -0.14571172, -1.625561), - Point3::new(0.38895372, -0.15321401, -1.6217446), - Point3::new(0.38697082, -0.15321401, -1.6264102), - Point3::new(0.38895372, -0.087442145, -1.6443238), - Point3::new(0.38895372, -0.14571172, -1.625561), - Point3::new(0.38697082, -0.15321401, -1.6264102), - Point3::new(0.36483112, -0.15321401, -1.6628642), - Point3::new(0.37328035, -0.110008985, -1.6628642), - Point3::new(0.4294109, -0.1045661, -1.3690573), - Point3::new(0.44130057, -0.00047307758, -1.2766529), - Point3::new(0.38895372, -0.10872328, -1.3383226), - Point3::new(0.37838188, -0.10980959, -1.3302914), - Point3::new(0.33721945, -0.14189906, -1.3302914), - Point3::new(0.33452708, -0.15321401, -1.3406357), - Point3::new(0.38895372, -0.15321401, -1.3882599), - Point3::new(0.32243916, -0.15321401, -1.3382303), - Point3::new(0.3286988, -0.15321401, -1.3378663), - Point3::new(0.3321432, -0.14424704, -1.3302914), - Point3::new(0.32243916, -0.14356102, -1.3302914), - Point3::new(0.3321432, -0.14424706, -1.3302914), - Point3::new(0.3286988, -0.15321401, -1.3378663), - Point3::new(0.33452708, -0.15321401, -1.3406355), - Point3::new(0.33721942, -0.14189915, -1.3302914), - Point3::new(0.37838173, -0.10980961, -1.3302913), - Point3::new(0.34401476, -0.113340996, -1.3041832), - Point3::new(0.33721948, -0.14189893, -1.3302913), - Point3::new(0.32243916, -0.14356089, -1.3302913), - Point3::new(0.33214325, -0.1442469, -1.3302913), - Point3::new(0.34401476, -0.113340996, -1.3041832), - Point3::new(0.32243916, -0.11582225, -1.3074783), - Point3::new(0.33214325, -0.1442469, -1.3302913), - Point3::new(0.33721945, -0.14189902, -1.3302913), - Point3::new(0.34401476, -0.113340996, -1.3041832), - Point3::new(0.32243916, -0.080589324, -1.7246456), - Point3::new(0.32243916, -0.020184837, -1.7163969), - Point3::new(0.33861578, -0.020184845, -1.7061031), - Point3::new(0.33861578, -0.020184837, -1.7061031), - Point3::new(0.37447086, -0.020184837, -1.6628642), - Point3::new(0.37358934, -0.086699404, -1.6628642), - Point3::new(0.32243916, -0.086699404, -1.724548), - Point3::new(0.32243916, -0.08058934, -1.7246456), - Point3::new(0.37447086, -0.020184837, -1.6628642), - Point3::new(0.38895372, -0.020184837, -1.6453989), - Point3::new(0.38895372, -0.086699404, -1.6443357), - Point3::new(0.37358934, -0.086699404, -1.6628642), - Point3::new(0.32243916, -0.042620897, -1.2608014), - Point3::new(0.3386477, -0.020184837, -1.2452083), - Point3::new(0.32243916, -0.020184845, -1.2479438), - Point3::new(0.32243916, 0.046329774, -1.7073138), - Point3::new(0.35642868, 0.046329774, -1.6856849), - Point3::new(0.33861578, -0.020184785, -1.7061031), - Point3::new(0.32243916, -0.020184793, -1.7163969), - Point3::new(0.35642868, 0.046329774, -1.6856849), - Point3::new(0.37535235, 0.046329774, -1.6628642), - Point3::new(0.3744708, -0.020184793, -1.6628642), - Point3::new(0.33861578, -0.020184793, -1.7061031), - Point3::new(0.37535235, 0.046329774, -1.6628642), - Point3::new(0.38895372, 0.046329774, -1.6464618), - Point3::new(0.38895372, -0.020184793, -1.6453987), - Point3::new(0.3744708, -0.020184793, -1.6628642), - Point3::new(0.48844048, 0.10279102, -1.2567252), - Point3::new(0.33864772, -0.020184793, -1.2452083), - Point3::new(0.3867, 0.046329774, -1.1989802), - Point3::new(0.32243916, 0.046329767, -1.2098254), - Point3::new(0.32243916, -0.020184785, -1.2479436), - Point3::new(0.32243916, 0.09600361, -1.7005304), - Point3::new(0.32243916, 0.112844266, -1.7004257), - Point3::new(0.34368488, 0.112844266, -1.6847112), - Point3::new(0.34368488, 0.112844266, -1.6847112), - Point3::new(0.3742416, 0.112844266, -1.6652669), - Point3::new(0.35642868, 0.0463297, -1.685685), - Point3::new(0.32243916, 0.046329707, -1.7073138), - Point3::new(0.32243916, 0.09600361, -1.7005304), - Point3::new(0.37535238, 0.046329696, -1.6628642), - Point3::new(0.35642865, 0.046329692, -1.685685), - Point3::new(0.37424156, 0.112844266, -1.6652669), - Point3::new(0.37623394, 0.112844266, -1.6628642), - Point3::new(0.38741195, 0.112844266, -1.6493843), - Point3::new(0.38895372, 0.10323957, -1.6473714), - Point3::new(0.38895372, 0.0463297, -1.6464618), - Point3::new(0.37535238, 0.046329696, -1.6628642), - Point3::new(0.37623394, 0.112844266, -1.6628642), - Point3::new(0.38895372, 0.10323958, -1.6473714), - Point3::new(0.38741195, 0.112844266, -1.6493843), - Point3::new(0.38895372, 0.112844266, -1.6473188), - Point3::new(0.32243916, 0.046329707, -1.2098254), - Point3::new(0.38669994, 0.0463297, -1.1989803), - Point3::new(0.3884858, 0.04880166, -1.1972623), - Point3::new(0.32243916, 0.068251744, -1.1972623), - Point3::new(0.38895372, 0.04924305, -1.1971927), - Point3::new(0.38895372, 0.05021119, -1.1965392), - Point3::new(0.3887098, 0.049111772, -1.1970468), - Point3::new(0.32243916, 0.06825195, -1.1972622), - Point3::new(0.3884859, 0.048801832, -1.1972622), - Point3::new(0.3887098, 0.049111772, -1.1970468), - Point3::new(0.35936964, 0.11284426, -1.1654744), - Point3::new(0.32243916, 0.112844266, -1.1717072), - Point3::new(0.38895372, 0.050211195, -1.1965392), - Point3::new(0.38895372, 0.11284426, -1.1669341), - Point3::new(0.35936964, 0.112844266, -1.1654744), - Point3::new(0.3887098, 0.049111772, -1.1970468), - Point3::new(0.37302625, 0.13610204, -1.6628642), - Point3::new(0.3436848, 0.11284419, -1.6847113), - Point3::new(0.32243916, 0.11284419, -1.7004257), - Point3::new(0.32243916, 0.17935875, -1.7000124), - Point3::new(0.3726629, 0.17935875, -1.6628642), - Point3::new(0.3436848, 0.11284419, -1.6847113), - Point3::new(0.37302625, 0.13610205, -1.6628642), - Point3::new(0.37633768, 0.12067119, -1.6628642), - Point3::new(0.37424156, 0.11284419, -1.6652669), - Point3::new(0.37424156, 0.112844184, -1.6652669), - Point3::new(0.37633768, 0.12067119, -1.6628642), - Point3::new(0.37623394, 0.112844184, -1.6628642), - Point3::new(0.3778704, 0.17935875, -1.6590124), - Point3::new(0.38247612, 0.1435926, -1.655828), - Point3::new(0.37302625, 0.13610204, -1.6628642), - Point3::new(0.3726629, 0.17935875, -1.6628642), - Point3::new(0.37302625, 0.13610205, -1.6628642), - Point3::new(0.38247612, 0.1435926, -1.655828), - Point3::new(0.37633768, 0.12067119, -1.6628642), - Point3::new(0.38247612, 0.1435926, -1.655828), - Point3::new(0.3778704, 0.17935875, -1.6590124), - Point3::new(0.38842443, 0.17935875, -1.6479617), - Point3::new(0.38895372, 0.17935875, -1.6472442), - Point3::new(0.38895372, 0.15302485, -1.6470989), - Point3::new(0.38247612, 0.1435926, -1.655828), - Point3::new(0.38842443, 0.17935875, -1.6479617), - Point3::new(0.37633768, 0.12067119, -1.6628642), - Point3::new(0.38247612, 0.1435926, -1.655828), - Point3::new(0.38741195, 0.11284419, -1.6493843), - Point3::new(0.37623394, 0.112844184, -1.6628642), - Point3::new(0.38895372, 0.11284419, -1.6473188), - Point3::new(0.38741195, 0.11284419, -1.6493843), - Point3::new(0.38247612, 0.1435926, -1.655828), - Point3::new(0.38895372, 0.15302485, -1.6470989), - Point3::new(0.32243913, 0.11284423, -1.5963496), - Point3::new(0.38895375, 0.11284423, -1.5963496), - Point3::new(0.38895375, 0.17935872, -1.5963496), - Point3::new(0.32243913, 0.17935872, -1.5963496), - Point3::new(0.32243913, 0.11284423, -1.529835), - Point3::new(0.38895375, 0.11284423, -1.529835), - Point3::new(0.38895375, 0.17935872, -1.529835), - Point3::new(0.32243913, 0.17935872, -1.529835), - Point3::new(0.32243913, 0.11284423, -1.529835), - Point3::new(0.38895375, 0.11284423, -1.529835), - Point3::new(0.38895375, 0.17935872, -1.529835), - Point3::new(0.32243913, 0.17935872, -1.529835), - Point3::new(0.32243913, 0.11284423, -1.4633205), - Point3::new(0.38895375, 0.11284423, -1.4633205), - Point3::new(0.38895375, 0.17935872, -1.4633205), - Point3::new(0.32243913, 0.17935872, -1.4633205), - Point3::new(0.32243913, 0.11284423, -1.4633205), - Point3::new(0.38895375, 0.11284423, -1.4633205), - Point3::new(0.38895375, 0.17935872, -1.4633205), - Point3::new(0.32243913, 0.17935872, -1.4633205), - Point3::new(0.32243913, 0.11284423, -1.396806), - Point3::new(0.38895375, 0.11284423, -1.396806), - Point3::new(0.38895375, 0.17935872, -1.396806), - Point3::new(0.32243913, 0.17935872, -1.396806), - Point3::new(0.32243913, 0.11284423, -1.396806), - Point3::new(0.38895375, 0.11284423, -1.396806), - Point3::new(0.38895375, 0.17935872, -1.396806), - Point3::new(0.32243913, 0.17935872, -1.396806), - Point3::new(0.32243913, 0.11284423, -1.3302914), - Point3::new(0.38895375, 0.11284423, -1.3302914), - Point3::new(0.38895375, 0.17935872, -1.3302914), - Point3::new(0.32243913, 0.17935872, -1.3302914), - Point3::new(0.32243913, 0.11284423, -1.3302914), - Point3::new(0.38895375, 0.11284423, -1.3302914), - Point3::new(0.38895375, 0.17935872, -1.3302914), - Point3::new(0.32243913, 0.17935872, -1.3302914), - Point3::new(0.32243913, 0.11284423, -1.2637768), - Point3::new(0.38895375, 0.11284423, -1.2637768), - Point3::new(0.38895375, 0.17935872, -1.2637768), - Point3::new(0.32243913, 0.17935872, -1.2637768), - Point3::new(0.32243913, 0.11284423, -1.2637768), - Point3::new(0.38895375, 0.11284423, -1.2637768), - Point3::new(0.38895375, 0.17935872, -1.2637768), - Point3::new(0.32243913, 0.17935872, -1.2637768), - Point3::new(0.32243913, 0.11284423, -1.1972623), - Point3::new(0.38895375, 0.11284423, -1.1972623), - Point3::new(0.38895375, 0.17935872, -1.1972623), - Point3::new(0.32243913, 0.17935872, -1.1972623), - Point3::new(0.32243916, 0.17935875, -1.1335888), - Point3::new(0.32243916, 0.11284419, -1.1717072), - Point3::new(0.35936967, 0.112844184, -1.1654745), - Point3::new(0.3287487, 0.17935875, -1.132524), - Point3::new(0.38895372, 0.112844184, -1.1669343), - Point3::new(0.38895372, 0.17935875, -1.1354945), - Point3::new(0.3287487, 0.17935875, -1.132524), - Point3::new(0.35936967, 0.11284419, -1.1654745), - Point3::new(0.38895372, -0.15321395, -1.6217446), - Point3::new(0.39974678, -0.15321395, -1.5963497), - Point3::new(0.38895372, -0.20313594, -1.5963497), - Point3::new(0.38895372, -0.21955214, -1.5613409), - Point3::new(0.40454316, -0.18911855, -1.5298351), - Point3::new(0.39439303, -0.2197285, -1.5298351), - Point3::new(0.38895372, -0.2197285, -1.5610058), - Point3::new(0.38895372, -0.21664403, -1.5894783), - Point3::new(0.42028785, -0.15321395, -1.5480187), - Point3::new(0.42115748, -0.15321395, -1.5298351), - Point3::new(0.40454316, -0.18911853, -1.5298351), - Point3::new(0.38895372, -0.21955213, -1.5613409), - Point3::new(0.3997468, -0.15321395, -1.5963496), - Point3::new(0.42028785, -0.15321395, -1.5480187), - Point3::new(0.38895372, -0.21664403, -1.5894783), - Point3::new(0.38895372, -0.20313618, -1.5963496), - Point3::new(0.39810324, -0.2197285, -1.5085732), - Point3::new(0.42746043, -0.15321395, -1.4667326), - Point3::new(0.4264674, -0.15321395, -1.4633205), - Point3::new(0.38895372, -0.21328956, -1.4633205), - Point3::new(0.38895372, -0.2197285, -1.4771357), - Point3::new(0.40454322, -0.18911843, -1.529835), - Point3::new(0.42293513, -0.15321395, -1.4926655), - Point3::new(0.42746043, -0.15321395, -1.4667326), - Point3::new(0.39810324, -0.2197285, -1.5085732), - Point3::new(0.39439303, -0.2197285, -1.529835), - Point3::new(0.40454322, -0.18911843, -1.529835), - Point3::new(0.42115748, -0.15321395, -1.529835), - Point3::new(0.42293513, -0.15321395, -1.4926655), - Point3::new(0.38895372, -0.21004458, -1.4563582), - Point3::new(0.4107516, -0.15321395, -1.4093214), - Point3::new(0.40362403, -0.15321395, -1.4010967), - Point3::new(0.38895372, -0.18089, -1.419324), - Point3::new(0.40362403, -0.15321395, -1.4010967), - Point3::new(0.39872047, -0.15321395, -1.396806), - Point3::new(0.38895372, -0.16082796, -1.396806), - Point3::new(0.38895372, -0.18089001, -1.419324), - Point3::new(0.38895372, -0.21328956, -1.4633205), - Point3::new(0.4264674, -0.15321395, -1.4633205), - Point3::new(0.4107516, -0.15321395, -1.4093214), - Point3::new(0.38895372, -0.2100446, -1.4563582), - Point3::new(0.39872035, -0.15321395, -1.3968059), - Point3::new(0.38895372, -0.15321395, -1.3882599), - Point3::new(0.38895372, -0.16082785, -1.3968059), - Point3::new(0.4138162, -0.05164461, -1.6149133), - Point3::new(0.49487513, 0.08750193, -1.4567895), - Point3::new(0.4522901, -0.08843134, -1.5056752), - Point3::new(0.38895372, -0.087442145, -1.6443238), - Point3::new(0.38895372, -0.08669945, -1.6443357), - Point3::new(0.38946953, -0.08669945, -1.6437136), - Point3::new(0.39974678, -0.15321401, -1.5963497), - Point3::new(0.38895372, -0.15321401, -1.6217446), - Point3::new(0.38895372, -0.14571172, -1.625561), - Point3::new(0.404551, -0.08669945, -1.6188812), - Point3::new(0.41412708, -0.08669945, -1.5963497), - Point3::new(0.404551, -0.08669945, -1.6188812), - Point3::new(0.38895372, -0.14571172, -1.625561), - Point3::new(0.38895372, -0.087442145, -1.6443238), - Point3::new(0.38946953, -0.08669945, -1.6437136), - Point3::new(0.4202878, -0.15321401, -1.5480188), - Point3::new(0.4340306, -0.12539425, -1.5298351), - Point3::new(0.42115745, -0.15321401, -1.5298351), - Point3::new(0.43403062, -0.12539425, -1.5298351), - Point3::new(0.42028782, -0.15321401, -1.5480188), - Point3::new(0.39974684, -0.15321401, -1.5963496), - Point3::new(0.41412714, -0.08669945, -1.5963496), - Point3::new(0.44239634, -0.08669945, -1.5298351), - Point3::new(0.5041985, 0.082197644, -1.3501678), - Point3::new(0.42646736, -0.15321401, -1.4633205), - Point3::new(0.42746043, -0.15321401, -1.4667326), - Point3::new(0.42985454, -0.1477897, -1.4633205), - Point3::new(0.4274604, -0.15321401, -1.4667326), - Point3::new(0.4229351, -0.15321401, -1.4926656), - Point3::new(0.43745542, -0.12486756, -1.4633205), - Point3::new(0.4298545, -0.1477897, -1.4633205), - Point3::new(0.43403068, -0.12539408, -1.529835), - Point3::new(0.4522901, -0.08843134, -1.5056752), - Point3::new(0.45408338, -0.08893345, -1.4633205), - Point3::new(0.43745542, -0.12486756, -1.4633205), - Point3::new(0.4229351, -0.15321401, -1.4926656), - Point3::new(0.42115745, -0.15321401, -1.529835), - Point3::new(0.45563933, -0.08936912, -1.4265714), - Point3::new(0.4340307, -0.12539408, -1.529835), - Point3::new(0.4423964, -0.08669945, -1.529835), - Point3::new(0.45047876, -0.08669945, -1.510818), - Point3::new(0.4522901, -0.08843134, -1.5056752), - Point3::new(0.41075158, -0.15321401, -1.4093215), - Point3::new(0.41655156, -0.13809252, -1.396806), - Point3::new(0.40707737, -0.1466991, -1.396806), - Point3::new(0.403624, -0.15321401, -1.4010967), - Point3::new(0.39872047, -0.15321401, -1.396806), - Point3::new(0.403624, -0.15321401, -1.4010967), - Point3::new(0.40707737, -0.1466991, -1.396806), - Point3::new(0.45546827, -0.08946823, -1.4261962), - Point3::new(0.4420653, -0.09723403, -1.396806), - Point3::new(0.41655156, -0.13809252, -1.396806), - Point3::new(0.41075158, -0.15321401, -1.4093215), - Point3::new(0.42646736, -0.15321401, -1.4633205), - Point3::new(0.42985454, -0.1477897, -1.4633205), - Point3::new(0.45546827, -0.0897567, -1.4268152), - Point3::new(0.4298545, -0.1477897, -1.4633205), - Point3::new(0.43745542, -0.12486756, -1.4633205), - Point3::new(0.45546827, -0.08970307, -1.4269171), - Point3::new(0.45546827, -0.0897567, -1.4268152), - Point3::new(0.43745542, -0.12486756, -1.4633205), - Point3::new(0.45408338, -0.08893345, -1.4633205), - Point3::new(0.45546827, -0.08932122, -1.4306116), - Point3::new(0.45546827, -0.08970307, -1.4269171), - Point3::new(0.41655162, -0.13809238, -1.3968059), - Point3::new(0.4294109, -0.1045661, -1.3690573), - Point3::new(0.40707746, -0.14669892, -1.3968059), - Point3::new(0.38895372, -0.10872328, -1.3383226), - Point3::new(0.38895372, -0.15321401, -1.3882599), - Point3::new(0.39872032, -0.15321401, -1.3968059), - Point3::new(0.40707746, -0.14669892, -1.3968059), - Point3::new(0.4294109, -0.1045661, -1.3690573), - Point3::new(0.44206524, -0.09723406, -1.3968059), - Point3::new(0.4294109, -0.1045661, -1.3690573), - Point3::new(0.41655162, -0.13809238, -1.3968059), - Point3::new(0.419506, -0.020184837, -1.607119), - Point3::new(0.424374, -0.020184837, -1.5963497), - Point3::new(0.42333248, -0.035308912, -1.5963497), - Point3::new(0.4138162, -0.05164461, -1.6149133), - Point3::new(0.38895372, -0.086699404, -1.6443357), - Point3::new(0.38895372, -0.020184837, -1.6453989), - Point3::new(0.40876618, -0.020184837, -1.6215062), - Point3::new(0.4138162, -0.05164461, -1.6149133), - Point3::new(0.38946956, -0.086699404, -1.6437136), - Point3::new(0.4138162, -0.05164461, -1.6149133), - Point3::new(0.40876618, -0.020184837, -1.6215062), - Point3::new(0.419506, -0.020184845, -1.607119), - Point3::new(0.40455103, -0.086699404, -1.6188812), - Point3::new(0.4138162, -0.05164461, -1.6149133), - Point3::new(0.42035437, -0.057896048, -1.5963497), - Point3::new(0.4141271, -0.086699404, -1.5963497), - Point3::new(0.4138162, -0.05164461, -1.6149133), - Point3::new(0.40455103, -0.086699404, -1.6188812), - Point3::new(0.38946956, -0.086699404, -1.6437136), - Point3::new(0.42437407, -0.020184837, -1.5963496), - Point3::new(0.4321429, -0.020184837, -1.579163), - Point3::new(0.42333254, -0.035308808, -1.5963496), - Point3::new(0.41412717, -0.086699404, -1.5963496), - Point3::new(0.4203544, -0.05789609, -1.5963496), - Point3::new(0.44378093, -0.08029534, -1.5298351), - Point3::new(0.44239637, -0.086699404, -1.5298351), - Point3::new(0.44239643, -0.086699404, -1.529835), - Point3::new(0.44378096, -0.08029538, -1.529835), - Point3::new(0.4504787, -0.086699404, -1.5108182), - Point3::new(0.424374, -0.020184793, -1.5963497), - Point3::new(0.419506, -0.020184793, -1.607119), - Point3::new(0.42736745, 0.023282386, -1.5963497), - Point3::new(0.38895372, 0.046329774, -1.6464618), - Point3::new(0.39808905, 0.046329774, -1.6354452), - Point3::new(0.40876618, -0.020184793, -1.6215062), - Point3::new(0.38895372, -0.020184793, -1.6453987), - Point3::new(0.39808905, 0.046329774, -1.6354452), - Point3::new(0.42727327, 0.046329767, -1.5963497), - Point3::new(0.42736745, 0.023282384, -1.5963497), - Point3::new(0.419506, -0.020184785, -1.607119), - Point3::new(0.40876618, -0.020184793, -1.6215062), - Point3::new(0.45546827, 0.019855715, -1.5336616), - Point3::new(0.4321429, -0.020184793, -1.579163), - Point3::new(0.42437407, -0.020184793, -1.5963496), - Point3::new(0.42736754, 0.023282867, -1.5963496), - Point3::new(0.43153578, 0.046329774, -1.5906396), - Point3::new(0.45546827, 0.046329767, -1.5376949), - Point3::new(0.42727333, 0.046329767, -1.5963496), - Point3::new(0.43153578, 0.046329767, -1.5906396), - Point3::new(0.42736754, 0.023282867, -1.5963496), - Point3::new(0.38895372, 0.046329692, -1.6464618), - Point3::new(0.38895372, 0.10323957, -1.6473714), - Point3::new(0.39808908, 0.0463297, -1.6354452), - Point3::new(0.38895372, 0.112844266, -1.6473188), - Point3::new(0.42700142, 0.112844266, -1.5963497), - Point3::new(0.42727327, 0.046329707, -1.5963497), - Point3::new(0.39808908, 0.0463297, -1.6354452), - Point3::new(0.38895372, 0.10323958, -1.6473714), - Point3::new(0.45546827, 0.112844266, -1.5478284), - Point3::new(0.45546827, 0.046329707, -1.5376949), - Point3::new(0.43153575, 0.0463297, -1.5906396), - Point3::new(0.44356552, 0.112844266, -1.5741602), - Point3::new(0.4270015, 0.112844266, -1.5963496), - Point3::new(0.44356555, 0.112844266, -1.5741602), - Point3::new(0.43153578, 0.046329707, -1.5906396), - Point3::new(0.42727336, 0.046329707, -1.5963496), - Point3::new(0.38907006, 0.04930567, -1.1972623), - Point3::new(0.45546827, 0.08504398, -1.2369947), - Point3::new(0.45546827, 0.112844266, -1.218227), - Point3::new(0.4336699, 0.112844266, -1.1972623), - Point3::new(0.4028486, 0.112844266, -1.1676197), - Point3::new(0.38895372, 0.05021119, -1.1965392), - Point3::new(0.38895372, 0.04924305, -1.1971927), - Point3::new(0.38906986, 0.049305566, -1.1972622), - Point3::new(0.43366978, 0.112844266, -1.1972622), - Point3::new(0.38895372, 0.050211195, -1.1965392), - Point3::new(0.4028486, 0.11284426, -1.1676197), - Point3::new(0.38895372, 0.11284426, -1.1669341), - Point3::new(0.38895372, 0.17935875, -1.6472442), - Point3::new(0.4070385, 0.17935875, -1.6227281), - Point3::new(0.38895372, 0.15302485, -1.6470989), - Point3::new(0.4270014, 0.11284419, -1.5963497), - Point3::new(0.38895372, 0.11284419, -1.6473188), - Point3::new(0.38895372, 0.15302485, -1.6470989), - Point3::new(0.4070385, 0.17935875, -1.6227281), - Point3::new(0.4267296, 0.17935875, -1.5963497), - Point3::new(0.45546827, 0.17865649, -1.5578549), - Point3::new(0.45546827, 0.11284419, -1.5478284), - Point3::new(0.44356552, 0.112844184, -1.5741603), - Point3::new(0.45546827, 0.17935875, -1.5578511), - Point3::new(0.45546827, 0.17865646, -1.5578549), - Point3::new(0.44356552, 0.11284419, -1.5741602), - Point3::new(0.42700148, 0.11284419, -1.5963496), - Point3::new(0.42672968, 0.17935875, -1.5963496), - Point3::new(0.38895375, 0.11284423, -1.529835), - Point3::new(0.45546824, 0.11284423, -1.529835), - Point3::new(0.45546824, 0.17935872, -1.529835), - Point3::new(0.38895375, 0.17935872, -1.529835), - Point3::new(0.38895375, 0.11284423, -1.4633205), - Point3::new(0.45546824, 0.11284423, -1.4633205), - Point3::new(0.45546824, 0.17935872, -1.4633205), - Point3::new(0.38895375, 0.17935872, -1.4633205), - Point3::new(0.38895375, 0.11284423, -1.4633205), - Point3::new(0.45546824, 0.11284423, -1.4633205), - Point3::new(0.45546824, 0.17935872, -1.4633205), - Point3::new(0.38895375, 0.17935872, -1.4633205), - Point3::new(0.38895375, 0.11284423, -1.396806), - Point3::new(0.45546824, 0.11284423, -1.396806), - Point3::new(0.45546824, 0.17935872, -1.396806), - Point3::new(0.38895375, 0.17935872, -1.396806), - Point3::new(0.38895375, 0.11284423, -1.396806), - Point3::new(0.45546824, 0.11284423, -1.396806), - Point3::new(0.45546824, 0.17935872, -1.396806), - Point3::new(0.38895375, 0.17935872, -1.396806), - Point3::new(0.38895375, 0.11284423, -1.3302914), - Point3::new(0.45546824, 0.11284423, -1.3302914), - Point3::new(0.45546824, 0.17935872, -1.3302914), - Point3::new(0.38895375, 0.17935872, -1.3302914), - Point3::new(0.38895375, 0.11284423, -1.3302914), - Point3::new(0.45546824, 0.11284423, -1.3302914), - Point3::new(0.45546824, 0.17935872, -1.3302914), - Point3::new(0.38895375, 0.17935872, -1.3302914), - Point3::new(0.38895375, 0.11284423, -1.2637768), - Point3::new(0.45546824, 0.11284423, -1.2637768), - Point3::new(0.45546824, 0.17935872, -1.2637768), - Point3::new(0.38895375, 0.17935872, -1.2637768), - Point3::new(0.4336698, 0.11284419, -1.1972623), - Point3::new(0.45546827, 0.11284419, -1.2182271), - Point3::new(0.45546827, 0.14389904, -1.1972623), - Point3::new(0.45546827, 0.14711037, -1.1950943), - Point3::new(0.45546827, 0.17935875, -1.1796435), - Point3::new(0.4314765, 0.17935875, -1.1502495), - Point3::new(0.4314765, 0.17935875, -1.1502495), - Point3::new(0.41760457, 0.17935875, -1.136908), - Point3::new(0.40284857, 0.11284419, -1.1676197), - Point3::new(0.4336697, 0.11284419, -1.1972622), - Point3::new(0.45546827, 0.14389922, -1.1972622), - Point3::new(0.45546827, 0.14711037, -1.1950943), - Point3::new(0.40284857, 0.112844184, -1.1676197), - Point3::new(0.41760457, 0.17935875, -1.136908), - Point3::new(0.38895372, 0.17935875, -1.1354945), - Point3::new(0.38895372, 0.11284419, -1.1669343), - Point3::new(0.45546833, -0.0894682, -1.4261963), - Point3::new(0.45546833, -0.08975658, -1.426815), - Point3::new(0.45563933, -0.08936912, -1.4265714), - Point3::new(0.45563933, -0.08936912, -1.4265714), - Point3::new(0.45546833, -0.08975656, -1.426815), - Point3::new(0.45546833, -0.08970295, -1.426917), - Point3::new(0.45563933, -0.08936912, -1.4265714), - Point3::new(0.45546833, -0.08970296, -1.426917), - Point3::new(0.45546833, -0.08932124, -1.4306103), - Point3::new(0.45546833, 0.046329774, -1.5376947), - Point3::new(0.4590211, 0.046329774, -1.5298351), - Point3::new(0.4574298, 0.023222877, -1.5298351), - Point3::new(0.45546833, 0.01985582, -1.5336614), - Point3::new(0.45902115, 0.046329774, -1.529835), - Point3::new(0.47089055, 0.046329774, -1.5035769), - Point3::new(0.45742986, 0.023222983, -1.529835), - Point3::new(0.4590211, 0.0463297, -1.5298351), - Point3::new(0.45546833, 0.0463297, -1.5376947), - Point3::new(0.45546833, 0.112844266, -1.5478283), - Point3::new(0.46360174, 0.112844266, -1.5298351), - Point3::new(0.49152714, 0.081754744, -1.4633205), - Point3::new(0.47089052, 0.0463297, -1.5035769), - Point3::new(0.45902112, 0.0463297, -1.529835), - Point3::new(0.4636018, 0.112844266, -1.529835), - Point3::new(0.49191773, 0.11284426, -1.4671929), - Point3::new(0.49301854, 0.103411235, -1.4633205), - Point3::new(0.49301854, 0.103411235, -1.4633205), - Point3::new(0.49191773, 0.112844266, -1.4671929), - Point3::new(0.49302983, 0.112844266, -1.4633205), - Point3::new(0.49152714, 0.081754744, -1.4633205), - Point3::new(0.49301854, 0.103411235, -1.4633205), - Point3::new(0.49487513, 0.08750193, -1.4567895), - Point3::new(0.5001203, 0.08451783, -1.396806), - Point3::new(0.49487513, 0.08750193, -1.4567895), - Point3::new(0.49677816, 0.112844266, -1.4502685), - Point3::new(0.5015912, 0.11284426, -1.396806), - Point3::new(0.49301854, 0.103411235, -1.4633205), - Point3::new(0.49302983, 0.112844266, -1.4633205), - Point3::new(0.49677816, 0.11284426, -1.4502685), - Point3::new(0.49487513, 0.08750193, -1.4567895), - Point3::new(0.50084656, 0.0865781, -1.3302914), - Point3::new(0.5041985, 0.082197644, -1.3501678), - Point3::new(0.5063298, 0.112844266, -1.334446), - Point3::new(0.5054681, 0.112844266, -1.3302914), - Point3::new(0.5041985, 0.082197644, -1.3501678), - Point3::new(0.5051708, 0.112844266, -1.3570441), - Point3::new(0.5063298, 0.11284426, -1.334446), - Point3::new(0.5051708, 0.11284426, -1.3570441), - Point3::new(0.5041985, 0.082197644, -1.3501678), - Point3::new(0.5001203, 0.08451783, -1.3968059), - Point3::new(0.5015912, 0.11284426, -1.3968059), - Point3::new(0.48962966, 0.101236954, -1.2637768), - Point3::new(0.50084656, 0.08657813, -1.3302913), - Point3::new(0.505468, 0.112844266, -1.3302913), - Point3::new(0.49167192, 0.11284426, -1.2637768), - Point3::new(0.48991573, 0.11284426, -1.2553097), - Point3::new(0.48844048, 0.10279102, -1.2567252), - Point3::new(0.48962966, 0.101236954, -1.2637768), - Point3::new(0.49167192, 0.11284426, -1.2637768), - Point3::new(0.48991573, 0.112844266, -1.2553097), - Point3::new(0.4888348, 0.11284426, -1.2523916), - Point3::new(0.48844048, 0.10279102, -1.2567252), - Point3::new(0.4888348, 0.112844266, -1.2523916), - Point3::new(0.48096117, 0.112844266, -1.242745), - Point3::new(0.48844048, 0.10279102, -1.2567252), - Point3::new(0.48096117, 0.112844266, -1.242745), - Point3::new(0.45546833, 0.112844266, -1.2182271), - Point3::new(0.45546833, 0.08504401, -1.2369947), - Point3::new(0.48844048, 0.10279102, -1.2567252), - Point3::new(0.46360177, 0.11284419, -1.5298351), - Point3::new(0.45546833, 0.11284419, -1.5478283), - Point3::new(0.45546833, 0.17865682, -1.5578549), - Point3::new(0.45559528, 0.17935875, -1.557681), - Point3::new(0.46818238, 0.17935875, -1.5298351), - Point3::new(0.45546833, 0.1786568, -1.5578549), - Point3::new(0.45546833, 0.17935875, -1.5578511), - Point3::new(0.45559528, 0.17935875, -1.557681), - Point3::new(0.48415565, 0.17935875, -1.4944981), - Point3::new(0.49191776, 0.1128442, -1.4671929), - Point3::new(0.4636018, 0.11284419, -1.529835), - Point3::new(0.46818244, 0.17935875, -1.529835), - Point3::new(0.48415565, 0.17935875, -1.4944981), - Point3::new(0.49310938, 0.17935875, -1.4633205), - Point3::new(0.49302983, 0.11284419, -1.4633205), - Point3::new(0.49191776, 0.11284419, -1.4671929), - Point3::new(0.5015912, 0.1128442, -1.396806), - Point3::new(0.49677816, 0.11284419, -1.4502685), - Point3::new(0.50177294, 0.17935875, -1.4331533), - Point3::new(0.5050451, 0.17935875, -1.396806), - Point3::new(0.49310938, 0.17935875, -1.4633205), - Point3::new(0.50177294, 0.17935875, -1.4331533), - Point3::new(0.49677816, 0.1128442, -1.4502685), - Point3::new(0.49302983, 0.11284419, -1.4633205), - Point3::new(0.505468, 0.11284419, -1.3302914), - Point3::new(0.5063298, 0.11284419, -1.3344461), - Point3::new(0.506893, 0.12094295, -1.3302914), - Point3::new(0.5051708, 0.11284419, -1.3570441), - Point3::new(0.5072812, 0.17935875, -1.3719683), - Point3::new(0.50941855, 0.17935875, -1.3302914), - Point3::new(0.506893, 0.12094273, -1.3302914), - Point3::new(0.5063298, 0.1128442, -1.334446), - Point3::new(0.5072812, 0.17935875, -1.3719683), - Point3::new(0.5051708, 0.1128442, -1.3570441), - Point3::new(0.5015912, 0.1128442, -1.3968059), - Point3::new(0.5050452, 0.17935875, -1.3968059), - Point3::new(0.49167192, 0.1128442, -1.2637768), - Point3::new(0.505468, 0.11284419, -1.3302913), - Point3::new(0.50689304, 0.12094319, -1.3302913), - Point3::new(0.51095545, 0.17935875, -1.300324), - Point3::new(0.50337505, 0.17935875, -1.2637768), - Point3::new(0.506893, 0.120942965, -1.3302913), - Point3::new(0.5094186, 0.17935875, -1.3302913), - Point3::new(0.51095545, 0.17935875, -1.3003238), - Point3::new(0.49967632, 0.17935875, -1.2459443), - Point3::new(0.48991573, 0.1128442, -1.2553097), - Point3::new(0.49167192, 0.1128442, -1.2637768), - Point3::new(0.50337505, 0.17935875, -1.2637768), - Point3::new(0.4914436, 0.17935875, -1.2237192), - Point3::new(0.4888348, 0.1128442, -1.2523916), - Point3::new(0.48991573, 0.11284419, -1.2553097), - Point3::new(0.49967632, 0.17935875, -1.2459443), - Point3::new(0.4566281, 0.1455514, -1.1972623), - Point3::new(0.48096123, 0.11284419, -1.2427452), - Point3::new(0.4888348, 0.11284419, -1.2523916), - Point3::new(0.4914436, 0.17935875, -1.2237192), - Point3::new(0.46984905, 0.17935875, -1.1972623), - Point3::new(0.48096123, 0.11284419, -1.2427452), - Point3::new(0.4566281, 0.1455514, -1.1972623), - Point3::new(0.45546833, 0.14389914, -1.1972623), - Point3::new(0.45546833, 0.11284419, -1.2182271), - Point3::new(0.45662802, 0.14555149, -1.1972622), - Point3::new(0.46984893, 0.17935875, -1.1972622), - Point3::new(0.45546833, 0.17935875, -1.1796435), - Point3::new(0.45546833, 0.1471103, -1.1950945), - Point3::new(0.45662802, 0.14555147, -1.1972622), - Point3::new(0.45546833, 0.1471103, -1.1950945), - Point3::new(0.45546833, 0.1438993, -1.1972622), + Vector3::new(-0.48323253, 0.015908679, -1.3343177), + Vector3::new(-0.4485139, -0.10542433, -1.4737657), + Vector3::new(-0.42711252, -0.10988713, -1.37545), + Vector3::new(-0.4219087, -0.06716395, -1.5995741), + Vector3::new(-0.4953231, 0.08068632, -1.4425513), + Vector3::new(-0.42424062, 0.07620259, -1.2036741), + Vector3::new(-0.47573563, 0.041238867, -1.484446), + Vector3::new(-0.4782635, 0.046329774, -1.4790393), + Vector3::new(-0.47573563, 0.046329767, -1.4853326), + Vector3::new(-0.4842582, 0.02425052, -1.3302914), + Vector3::new(-0.48323253, 0.015908679, -1.3343177), + Vector3::new(-0.4828088, 0.020500222, -1.3302914), + Vector3::new(-0.4874344, 0.046329774, -1.3302914), + Vector3::new(-0.48891053, 0.046329774, -1.3851467), + Vector3::new(-0.48323253, 0.015908679, -1.3343177), + Vector3::new(-0.48635057, 0.03868017, -1.3302914), + Vector3::new(-0.48323253, 0.015908679, -1.3343177), + Vector3::new(-0.4842582, 0.024250517, -1.3302914), + Vector3::new(-0.48635057, 0.038680166, -1.3302914), + Vector3::new(-0.4828088, 0.020500222, -1.3302914), + Vector3::new(-0.48323253, 0.015908679, -1.3343177), + Vector3::new(-0.48141447, 0.017766872, -1.3302914), + Vector3::new(-0.48042512, 0.046329774, -1.3076416), + Vector3::new(-0.486973, 0.04632978, -1.3196346), + Vector3::new(-0.48425823, 0.024250766, -1.3302913), + Vector3::new(-0.4828088, 0.020500358, -1.3302913), + Vector3::new(-0.4874344, 0.046329774, -1.3302913), + Vector3::new(-0.48635066, 0.038680844, -1.3302913), + Vector3::new(-0.487398, 0.046329774, -1.3289388), + Vector3::new(-0.48425823, 0.024250764, -1.3302913), + Vector3::new(-0.486973, 0.046329774, -1.3196346), + Vector3::new(-0.487398, 0.046329767, -1.3289388), + Vector3::new(-0.48635066, 0.03868084, -1.3302913), + Vector3::new(-0.4828088, 0.020500358, -1.3302913), + Vector3::new(-0.4814144, 0.017766926, -1.3302913), + Vector3::new(-0.47573563, 0.023571035, -1.317715), + Vector3::new(-0.47573563, 0.046329774, -1.3008153), + Vector3::new(-0.48042512, 0.046329774, -1.3076416), + Vector3::new(-0.47573563, 0.046329707, -1.4853326), + Vector3::new(-0.47826347, 0.0463297, -1.4790394), + Vector3::new(-0.48561266, 0.061130375, -1.4633205), + Vector3::new(-0.48923, 0.112844266, -1.4633205), + Vector3::new(-0.47573563, 0.112844266, -1.4969156), + Vector3::new(-0.4953231, 0.08068632, -1.4425513), + Vector3::new(-0.49021298, 0.05330783, -1.396806), + Vector3::new(-0.4986484, 0.11284426, -1.396806), + Vector3::new(-0.49916852, 0.11284426, -1.4161342), + Vector3::new(-0.48923, 0.112844266, -1.4633205), + Vector3::new(-0.48561266, 0.061130375, -1.4633205), + Vector3::new(-0.4953231, 0.08068632, -1.4425513), + Vector3::new(-0.49079263, 0.112844266, -1.4594302), + Vector3::new(-0.4953231, 0.08068632, -1.4425513), + Vector3::new(-0.49916852, 0.112844266, -1.4161342), + Vector3::new(-0.49605638, 0.11284426, -1.4436231), + Vector3::new(-0.4953231, 0.08068632, -1.4425513), + Vector3::new(-0.49605638, 0.112844266, -1.4436231), + Vector3::new(-0.49079263, 0.112844266, -1.4594302), + Vector3::new(-0.49021295, 0.053307757, -1.3968059), + Vector3::new(-0.48891053, 0.0463297, -1.3851466), + Vector3::new(-0.4874344, 0.0463297, -1.3302914), + Vector3::new(-0.4968585, 0.112844266, -1.3302914), + Vector3::new(-0.4986484, 0.11284426, -1.3968059), + Vector3::new(-0.49515137, 0.112844266, -1.2875305), + Vector3::new(-0.486973, 0.046329692, -1.3196346), + Vector3::new(-0.48042512, 0.0463297, -1.3076417), + Vector3::new(-0.4758087, 0.096352905, -1.2637768), + Vector3::new(-0.48218226, 0.112844266, -1.2637768), + Vector3::new(-0.4968585, 0.112844266, -1.3302913), + Vector3::new(-0.4874344, 0.0463297, -1.3302913), + Vector3::new(-0.487398, 0.0463297, -1.3289388), + Vector3::new(-0.49650565, 0.112844266, -1.3171781), + Vector3::new(-0.486973, 0.0463297, -1.3196346), + Vector3::new(-0.49515137, 0.112844266, -1.2875305), + Vector3::new(-0.49650565, 0.11284426, -1.3171781), + Vector3::new(-0.487398, 0.046329707, -1.3289388), + Vector3::new(-0.4758087, 0.096352905, -1.2637768), + Vector3::new(-0.48042512, 0.0463297, -1.3076417), + Vector3::new(-0.47573563, 0.0463297, -1.3008155), + Vector3::new(-0.47573563, 0.096209645, -1.2637768), + Vector3::new(-0.48218226, 0.112844266, -1.2637768), + Vector3::new(-0.4758087, 0.096352905, -1.2637768), + Vector3::new(-0.47573563, 0.09714479, -1.2630824), + Vector3::new(-0.47573563, 0.112844266, -1.2519693), + Vector3::new(-0.4758087, 0.096352905, -1.2637768), + Vector3::new(-0.47573563, 0.096209645, -1.2637768), + Vector3::new(-0.47573563, 0.09714478, -1.2630824), + Vector3::new(-0.47573563, 0.17935875, -1.5084984), + Vector3::new(-0.47573563, 0.1128442, -1.4969155), + Vector3::new(-0.48922995, 0.11284419, -1.4633205), + Vector3::new(-0.48974842, 0.12025622, -1.4633205), + Vector3::new(-0.48142195, 0.17935875, -1.494342), + Vector3::new(-0.49175212, 0.17935875, -1.4633205), + Vector3::new(-0.48142195, 0.17935875, -1.494342), + Vector3::new(-0.48974842, 0.12025623, -1.4633205), + Vector3::new(-0.5019821, 0.13637298, -1.396806), + Vector3::new(-0.49916852, 0.1128442, -1.4161344), + Vector3::new(-0.4986484, 0.1128442, -1.396806), + Vector3::new(-0.48974842, 0.12025622, -1.4633205), + Vector3::new(-0.48922995, 0.11284419, -1.4633205), + Vector3::new(-0.49079263, 0.11284419, -1.4594301), + Vector3::new(-0.5031245, 0.17935875, -1.396806), + Vector3::new(-0.49757308, 0.17935875, -1.4458401), + Vector3::new(-0.49605638, 0.1128442, -1.4436231), + Vector3::new(-0.49916852, 0.11284419, -1.4161344), + Vector3::new(-0.5019821, 0.13637298, -1.396806), + Vector3::new(-0.49175212, 0.17935875, -1.4633205), + Vector3::new(-0.48974842, 0.12025623, -1.4633205), + Vector3::new(-0.49079263, 0.11284419, -1.4594301), + Vector3::new(-0.49605638, 0.11284419, -1.4436231), + Vector3::new(-0.49757308, 0.17935875, -1.4458401), + Vector3::new(-0.5019821, 0.13637313, -1.3968059), + Vector3::new(-0.4986484, 0.1128442, -1.3968059), + Vector3::new(-0.4968585, 0.11284419, -1.3302914), + Vector3::new(-0.5062826, 0.17935875, -1.3302914), + Vector3::new(-0.5071223, 0.17935875, -1.3614942), + Vector3::new(-0.5031245, 0.17935875, -1.3968059), + Vector3::new(-0.5019821, 0.13637313, -1.3968059), + Vector3::new(-0.5071223, 0.17935875, -1.3614942), + Vector3::new(-0.5012025, 0.16205832, -1.2637768), + Vector3::new(-0.49515134, 0.11284419, -1.2875305), + Vector3::new(-0.48218226, 0.11284419, -1.2637768), + Vector3::new(-0.5062826, 0.17935875, -1.3302913), + Vector3::new(-0.4968585, 0.11284419, -1.3302913), + Vector3::new(-0.49650565, 0.11284419, -1.3171781), + Vector3::new(-0.5056133, 0.17935875, -1.3054174), + Vector3::new(-0.49515134, 0.11284419, -1.2875305), + Vector3::new(-0.5012025, 0.16205832, -1.2637768), + Vector3::new(-0.50371116, 0.17935875, -1.2637768), + Vector3::new(-0.5056133, 0.17935875, -1.3054174), + Vector3::new(-0.49650565, 0.1128442, -1.3171781), + Vector3::new(-0.47573563, 0.17935875, -1.204886), + Vector3::new(-0.50332975, 0.17935875, -1.2554265), + Vector3::new(-0.5012025, 0.16205832, -1.2637768), + Vector3::new(-0.48218226, 0.11284419, -1.2637768), + Vector3::new(-0.47573563, 0.11284419, -1.2519693), + Vector3::new(-0.5012025, 0.16205832, -1.2637768), + Vector3::new(-0.50332975, 0.17935875, -1.2554265), + Vector3::new(-0.50371116, 0.17935875, -1.2637768), + Vector3::new(-0.41459927, -0.17038356, -1.5298351), + Vector3::new(-0.40922108, -0.1806848, -1.5387266), + Vector3::new(-0.40922108, -0.18694264, -1.5298351), + Vector3::new(-0.40922108, -0.1806848, -1.5387266), + Vector3::new(-0.41459927, -0.17038356, -1.5298351), + Vector3::new(-0.41921717, -0.15321395, -1.5298351), + Vector3::new(-0.40922108, -0.15321395, -1.5639201), + Vector3::new(-0.40922108, -0.19245225, -1.5220068), + Vector3::new(-0.42693704, -0.15321395, -1.5002563), + Vector3::new(-0.42356336, -0.15321395, -1.5150152), + Vector3::new(-0.41459933, -0.17038342, -1.529835), + Vector3::new(-0.40922108, -0.18694273, -1.529835), + Vector3::new(-0.42693704, -0.15321395, -1.5002563), + Vector3::new(-0.40922108, -0.19245224, -1.5220068), + Vector3::new(-0.40922108, -0.172227, -1.4633205), + Vector3::new(-0.4198326, -0.15321395, -1.4633205), + Vector3::new(-0.41459933, -0.17038342, -1.529835), + Vector3::new(-0.42356336, -0.15321395, -1.5150152), + Vector3::new(-0.4192172, -0.15321395, -1.529835), + Vector3::new(-0.4198326, -0.15321395, -1.4633205), + Vector3::new(-0.40922108, -0.172227, -1.4633205), + Vector3::new(-0.40922108, -0.15622416, -1.4168861), + Vector3::new(-0.41038337, -0.15321395, -1.4141942), + Vector3::new(-0.40922108, -0.15622418, -1.4168862), + Vector3::new(-0.40922108, -0.15321395, -1.4124653), + Vector3::new(-0.41038337, -0.15321395, -1.4141943), + Vector3::new(-0.41552514, -0.08669945, -1.603425), + Vector3::new(-0.40922108, -0.1059916, -1.6072279), + Vector3::new(-0.40922108, -0.1178531, -1.5963497), + Vector3::new(-0.4176001, -0.08669945, -1.5963497), + Vector3::new(-0.40922108, -0.1059916, -1.6072279), + Vector3::new(-0.41552514, -0.08669945, -1.603425), + Vector3::new(-0.40922108, -0.08669945, -1.6126148), + Vector3::new(-0.41760013, -0.08669945, -1.5963496), + Vector3::new(-0.40922108, -0.117853224, -1.5963496), + Vector3::new(-0.40922108, -0.15321401, -1.5639201), + Vector3::new(-0.41921714, -0.15321401, -1.5298351), + Vector3::new(-0.43665668, -0.08837273, -1.5298351), + Vector3::new(-0.43549314, -0.08669945, -1.5353372), + Vector3::new(-0.426937, -0.15321401, -1.5002563), + Vector3::new(-0.4485139, -0.10542433, -1.4737657), + Vector3::new(-0.42356333, -0.15321401, -1.5150152), + Vector3::new(-0.4485139, -0.10542433, -1.4737657), + Vector3::new(-0.426937, -0.15321401, -1.5002563), + Vector3::new(-0.41983256, -0.15321401, -1.4633205), + Vector3::new(-0.4462402, -0.10589846, -1.4633205), + Vector3::new(-0.4366567, -0.08837277, -1.529835), + Vector3::new(-0.41921717, -0.15321401, -1.529835), + Vector3::new(-0.42356333, -0.15321401, -1.5150152), + Vector3::new(-0.4485139, -0.10542433, -1.4737657), + Vector3::new(-0.4462402, -0.10589846, -1.4633205), + Vector3::new(-0.41983256, -0.15321401, -1.4633205), + Vector3::new(-0.41038334, -0.15321401, -1.4141943), + Vector3::new(-0.41789135, -0.13376904, -1.396806), + Vector3::new(-0.4317613, -0.10891773, -1.396806), + Vector3::new(-0.41789138, -0.13376896, -1.396806), + Vector3::new(-0.41038334, -0.15321401, -1.4141945), + Vector3::new(-0.40922108, -0.15321401, -1.4124653), + Vector3::new(-0.40922108, -0.14255148, -1.396806), + Vector3::new(-0.43176126, -0.108917736, -1.3968059), + Vector3::new(-0.4178914, -0.1337689, -1.3968059), + Vector3::new(-0.42711252, -0.10988713, -1.37545), + Vector3::new(-0.40922108, -0.14129017, -1.3949536), + Vector3::new(-0.42711252, -0.10988713, -1.37545), + Vector3::new(-0.41789144, -0.13376883, -1.3968059), + Vector3::new(-0.40922108, -0.14255139, -1.3968059), + Vector3::new(-0.40922108, -0.14129019, -1.3949536), + Vector3::new(-0.40922108, -0.10430696, -1.35424), + Vector3::new(-0.42711252, -0.10988713, -1.37545), + Vector3::new(-0.3472593, -0.08498167, -1.2807851), + Vector3::new(-0.42259058, -0.06814454, -1.5963497), + Vector3::new(-0.4219087, -0.06716395, -1.5995741), + Vector3::new(-0.41552514, -0.086699404, -1.603425), + Vector3::new(-0.4176001, -0.086699404, -1.5963497), + Vector3::new(-0.4219087, -0.06716395, -1.5995741), + Vector3::new(-0.42150742, -0.020184837, -1.6004248), + Vector3::new(-0.40922108, -0.020184845, -1.6154815), + Vector3::new(-0.40922108, -0.07801496, -1.6150396), + Vector3::new(-0.4219087, -0.06716395, -1.5995741), + Vector3::new(-0.42461768, -0.04695158, -1.5963497), + Vector3::new(-0.42477804, -0.020184841, -1.5963497), + Vector3::new(-0.42150742, -0.020184845, -1.6004248), + Vector3::new(-0.4219087, -0.06716395, -1.5995741), + Vector3::new(-0.42341623, -0.06412793, -1.5963497), + Vector3::new(-0.42461768, -0.046951585, -1.5963497), + Vector3::new(-0.40922108, -0.086699404, -1.6126148), + Vector3::new(-0.41552514, -0.086699404, -1.603425), + Vector3::new(-0.4219087, -0.06716395, -1.5995741), + Vector3::new(-0.40922108, -0.07801497, -1.6150396), + Vector3::new(-0.4225906, -0.068144575, -1.5963496), + Vector3::new(-0.41760013, -0.086699404, -1.5963496), + Vector3::new(-0.4354931, -0.086699404, -1.5353373), + Vector3::new(-0.4246178, -0.046950832, -1.5963496), + Vector3::new(-0.42820513, -0.020184837, -1.5920798), + Vector3::new(-0.42477816, -0.020184841, -1.5963496), + Vector3::new(-0.4234163, -0.06412782, -1.5963496), + Vector3::new(-0.44523597, -0.020184837, -1.5496805), + Vector3::new(-0.42820513, -0.020184845, -1.5920798), + Vector3::new(-0.4246178, -0.04695084, -1.5963496), + Vector3::new(-0.42150742, -0.020184793, -1.6004248), + Vector3::new(-0.42093927, 0.046329774, -1.6016293), + Vector3::new(-0.40922108, 0.046329767, -1.6159897), + Vector3::new(-0.40922108, -0.020184785, -1.6154815), + Vector3::new(-0.42517662, 0.04632977, -1.5963497), + Vector3::new(-0.42093927, 0.046329767, -1.6016293), + Vector3::new(-0.42150742, -0.020184785, -1.6004248), + Vector3::new(-0.42477804, -0.020184789, -1.5963497), + Vector3::new(-0.42820513, -0.020184793, -1.5920798), + Vector3::new(-0.43711984, 0.046329774, -1.581469), + Vector3::new(-0.4251767, 0.04632977, -1.5963496), + Vector3::new(-0.42477816, -0.020184789, -1.5963496), + Vector3::new(-0.45786002, 0.046329763, -1.5298351), + Vector3::new(-0.43711984, 0.04632975, -1.581469), + Vector3::new(-0.42820513, -0.020184785, -1.5920798), + Vector3::new(-0.445236, -0.020184793, -1.5496804), + Vector3::new(-0.45451444, -0.0014987998, -1.5298351), + Vector3::new(-0.45786005, 0.046329763, -1.529835), + Vector3::new(-0.4545145, -0.0014986861, -1.529835), + Vector3::new(-0.47573563, 0.041238867, -1.484446), + Vector3::new(-0.47573563, 0.046329774, -1.4853326), + Vector3::new(-0.47573563, 0.023571035, -1.317715), + Vector3::new(-0.45346835, 0.046329774, -1.2684019), + Vector3::new(-0.47573563, 0.046329774, -1.3008153), + Vector3::new(-0.40922108, 0.046329774, -1.2177562), + Vector3::new(-0.4099734, 0.046329774, -1.2179652), + Vector3::new(-0.40922108, 0.044754528, -1.2187189), + Vector3::new(-0.42093927, 0.0463297, -1.6016293), + Vector3::new(-0.42037112, 0.112844266, -1.6028337), + Vector3::new(-0.40922108, 0.11284426, -1.6164979), + Vector3::new(-0.40922108, 0.046329707, -1.6159897), + Vector3::new(-0.4255752, 0.11284426, -1.5963497), + Vector3::new(-0.42037112, 0.11284426, -1.6028337), + Vector3::new(-0.42093927, 0.046329707, -1.6016293), + Vector3::new(-0.42517662, 0.046329703, -1.5963497), + Vector3::new(-0.43711984, 0.0463297, -1.581469), + Vector3::new(-0.44603452, 0.112844266, -1.5708584), + Vector3::new(-0.4255753, 0.11284426, -1.5963496), + Vector3::new(-0.4251767, 0.046329703, -1.5963496), + Vector3::new(-0.4625126, 0.112844266, -1.5298351), + Vector3::new(-0.44603452, 0.11284426, -1.5708584), + Vector3::new(-0.43711984, 0.046329707, -1.581469), + Vector3::new(-0.45786002, 0.046329703, -1.5298351), + Vector3::new(-0.46251267, 0.112844266, -1.529835), + Vector3::new(-0.45786005, 0.046329703, -1.529835), + Vector3::new(-0.47573563, 0.0463297, -1.4853326), + Vector3::new(-0.47573563, 0.112844266, -1.4969156), + Vector3::new(-0.47573563, 0.096209645, -1.2637768), + Vector3::new(-0.47573563, 0.0463297, -1.3008155), + Vector3::new(-0.4534684, 0.0463297, -1.2684021), + Vector3::new(-0.45137987, 0.04846435, -1.2637768), + Vector3::new(-0.4742868, 0.112844266, -1.2493156), + Vector3::new(-0.47573563, 0.112844266, -1.2519693), + Vector3::new(-0.47573563, 0.09714479, -1.2630824), + Vector3::new(-0.42944744, 0.095044516, -1.1972623), + Vector3::new(-0.42424062, 0.07620259, -1.2036741), + Vector3::new(-0.41959578, 0.08458061, -1.1972623), + Vector3::new(-0.42424062, 0.07620259, -1.2036741), + Vector3::new(-0.40997338, 0.0463297, -1.2179654), + Vector3::new(-0.40922108, 0.0463297, -1.2177562), + Vector3::new(-0.40922108, 0.07986139, -1.1972623), + Vector3::new(-0.41959578, 0.08458061, -1.1972623), + Vector3::new(-0.42424062, 0.07620259, -1.2036741), + Vector3::new(-0.42944744, 0.095044516, -1.1972623), + Vector3::new(-0.4385274, 0.112844266, -1.1972623), + Vector3::new(-0.4742868, 0.112844266, -1.2493156), + Vector3::new(-0.47573563, 0.09714478, -1.2630824), + Vector3::new(-0.47573563, 0.096209645, -1.2637768), + Vector3::new(-0.45137987, 0.04846435, -1.2637768), + Vector3::new(-0.40922108, 0.112844266, -1.1783407), + Vector3::new(-0.4343663, 0.112844266, -1.1912051), + Vector3::new(-0.42944756, 0.095044866, -1.1972622), + Vector3::new(-0.4195957, 0.084580764, -1.1972622), + Vector3::new(-0.40922108, 0.10329369, -1.182941), + Vector3::new(-0.4195957, 0.084580764, -1.1972622), + Vector3::new(-0.40922108, 0.07986158, -1.1972622), + Vector3::new(-0.40922108, 0.10329369, -1.182941), + Vector3::new(-0.42944756, 0.095044866, -1.1972622), + Vector3::new(-0.4343663, 0.112844266, -1.1912051), + Vector3::new(-0.43852732, 0.112844266, -1.1972622), + Vector3::new(-0.42037112, 0.11284419, -1.6028337), + Vector3::new(-0.41980296, 0.17935875, -1.6040382), + Vector3::new(-0.40922108, 0.17935874, -1.6170061), + Vector3::new(-0.40922108, 0.1128442, -1.6164979), + Vector3::new(-0.42597374, 0.17935875, -1.5963497), + Vector3::new(-0.41980296, 0.17935875, -1.6040382), + Vector3::new(-0.42037112, 0.1128442, -1.6028337), + Vector3::new(-0.4255752, 0.1128442, -1.5963497), + Vector3::new(-0.44603452, 0.11284419, -1.5708584), + Vector3::new(-0.4549492, 0.17935875, -1.5602477), + Vector3::new(-0.42597386, 0.17935875, -1.5963496), + Vector3::new(-0.4255753, 0.1128442, -1.5963496), + Vector3::new(-0.4671652, 0.17935875, -1.5298351), + Vector3::new(-0.4549492, 0.17935875, -1.5602477), + Vector3::new(-0.44603452, 0.1128442, -1.5708584), + Vector3::new(-0.46251258, 0.11284419, -1.5298351), + Vector3::new(-0.46716526, 0.17935875, -1.529835), + Vector3::new(-0.46251264, 0.11284419, -1.529835), + Vector3::new(-0.47573563, 0.11284419, -1.4969155), + Vector3::new(-0.47573563, 0.17935875, -1.5084984), + Vector3::new(-0.47573566, 0.11284423, -1.4633205), + Vector3::new(-0.40922108, 0.11284423, -1.4633205), + Vector3::new(-0.40922108, 0.17935872, -1.4633205), + Vector3::new(-0.47573566, 0.17935872, -1.4633205), + Vector3::new(-0.47573566, 0.11284423, -1.396806), + Vector3::new(-0.40922108, 0.11284423, -1.396806), + Vector3::new(-0.40922108, 0.17935872, -1.396806), + Vector3::new(-0.47573566, 0.17935872, -1.396806), + Vector3::new(-0.47573566, 0.11284423, -1.396806), + Vector3::new(-0.40922108, 0.11284423, -1.396806), + Vector3::new(-0.40922108, 0.17935872, -1.396806), + Vector3::new(-0.47573566, 0.17935872, -1.396806), + Vector3::new(-0.47573566, 0.11284423, -1.3302914), + Vector3::new(-0.40922108, 0.11284423, -1.3302914), + Vector3::new(-0.40922108, 0.17935872, -1.3302914), + Vector3::new(-0.47573566, 0.17935872, -1.3302914), + Vector3::new(-0.47573566, 0.11284423, -1.3302914), + Vector3::new(-0.40922108, 0.11284423, -1.3302914), + Vector3::new(-0.40922108, 0.17935872, -1.3302914), + Vector3::new(-0.47573566, 0.17935872, -1.3302914), + Vector3::new(-0.47573566, 0.11284423, -1.2637768), + Vector3::new(-0.40922108, 0.11284423, -1.2637768), + Vector3::new(-0.40922108, 0.17935872, -1.2637768), + Vector3::new(-0.47573566, 0.17935872, -1.2637768), + Vector3::new(-0.47573563, 0.17935875, -1.204886), + Vector3::new(-0.47573563, 0.11284419, -1.2519693), + Vector3::new(-0.47428682, 0.11284419, -1.2493157), + Vector3::new(-0.46880862, 0.17220557, -1.1972623), + Vector3::new(-0.47157323, 0.17935875, -1.1972623), + Vector3::new(-0.46880862, 0.17220557, -1.1972623), + Vector3::new(-0.47428682, 0.11284419, -1.2493157), + Vector3::new(-0.43852738, 0.11284419, -1.1972623), + Vector3::new(-0.47157314, 0.17935875, -1.1972622), + Vector3::new(-0.46880862, 0.17220572, -1.1972622), + Vector3::new(-0.4681485, 0.17935875, -1.1909897), + Vector3::new(-0.40922108, 0.17935875, -1.1463025), + Vector3::new(-0.4527471, 0.17935875, -1.1685705), + Vector3::new(-0.43436626, 0.11284419, -1.1912051), + Vector3::new(-0.40922108, 0.11284419, -1.1783408), + Vector3::new(-0.4527471, 0.17935875, -1.1685705), + Vector3::new(-0.4681485, 0.17935875, -1.1909897), + Vector3::new(-0.46880862, 0.17220572, -1.1972622), + Vector3::new(-0.4385273, 0.11284419, -1.1972622), + Vector3::new(-0.43436626, 0.11284419, -1.1912051), + Vector3::new(-0.36200994, -0.15321395, -1.6628642), + Vector3::new(-0.34270653, -0.15321395, -1.6910036), + Vector3::new(-0.34270653, -0.2197285, -1.6724315), + Vector3::new(-0.34926963, -0.2197285, -1.6628642), + Vector3::new(-0.3997105, -0.15321395, -1.5963497), + Vector3::new(-0.39379033, -0.15321395, -1.6165366), + Vector3::new(-0.37205553, -0.2197285, -1.6296483), + Vector3::new(-0.38182098, -0.2197285, -1.5963497), + Vector3::new(-0.36200994, -0.15321395, -1.6628642), + Vector3::new(-0.34926963, -0.2197285, -1.6628642), + Vector3::new(-0.37205553, -0.2197285, -1.6296483), + Vector3::new(-0.39379033, -0.15321395, -1.6165366), + Vector3::new(-0.3888367, -0.2197285, -1.572427), + Vector3::new(-0.39690596, -0.2197285, -1.5371264), + Vector3::new(-0.40284482, -0.2065748, -1.5298351), + Vector3::new(-0.40922108, -0.18694264, -1.5298351), + Vector3::new(-0.40922108, -0.1806848, -1.5387266), + Vector3::new(-0.4028448, -0.20657478, -1.5298351), + Vector3::new(-0.39690593, -0.2197285, -1.5371264), + Vector3::new(-0.3955035, -0.2197285, -1.5298351), + Vector3::new(-0.39971054, -0.15321395, -1.5963496), + Vector3::new(-0.381821, -0.2197285, -1.5963496), + Vector3::new(-0.38883674, -0.2197285, -1.572427), + Vector3::new(-0.40922108, -0.1806848, -1.5387266), + Vector3::new(-0.40922108, -0.15321395, -1.5639201), + Vector3::new(-0.40922108, -0.18694273, -1.529835), + Vector3::new(-0.4028449, -0.20657457, -1.529835), + Vector3::new(-0.40922108, -0.19245225, -1.5220068), + Vector3::new(-0.4028449, -0.20657457, -1.529835), + Vector3::new(-0.39550346, -0.2197285, -1.529835), + Vector3::new(-0.3847011, -0.2197285, -1.4736738), + Vector3::new(-0.38917148, -0.20815063, -1.4633205), + Vector3::new(-0.40922108, -0.172227, -1.4633205), + Vector3::new(-0.40922108, -0.19245224, -1.5220068), + Vector3::new(-0.38917154, -0.20815049, -1.4633205), + Vector3::new(-0.3847011, -0.2197285, -1.4736739), + Vector3::new(-0.37774143, -0.2197285, -1.4633205), + Vector3::new(-0.40922108, -0.172227, -1.4633205), + Vector3::new(-0.38917148, -0.20815063, -1.4633205), + Vector3::new(-0.40922108, -0.15622416, -1.4168861), + Vector3::new(-0.40922108, -0.15321395, -1.4124653), + Vector3::new(-0.40922108, -0.15622418, -1.4168862), + Vector3::new(-0.38917154, -0.20815049, -1.4633205), + Vector3::new(-0.37774143, -0.2197285, -1.4633205), + Vector3::new(-0.36453196, -0.2197285, -1.4436697), + Vector3::new(-0.40242767, -0.15321395, -1.4023591), + Vector3::new(-0.3958336, -0.15321395, -1.396806), + Vector3::new(-0.4024277, -0.15321395, -1.4023591), + Vector3::new(-0.364532, -0.2197285, -1.4436697), + Vector3::new(-0.34270653, -0.2197285, -1.4252896), + Vector3::new(-0.34270653, -0.19385473, -1.396806), + Vector3::new(-0.39583346, -0.15321395, -1.3968059), + Vector3::new(-0.34270653, -0.19385463, -1.3968059), + Vector3::new(-0.34270653, -0.15321395, -1.3520658), + Vector3::new(-0.34270653, -0.13490114, -1.6961168), + Vector3::new(-0.36998647, -0.11157012, -1.6628642), + Vector3::new(-0.37014154, -0.086699456, -1.6628642), + Vector3::new(-0.34270653, -0.086699456, -1.696485), + Vector3::new(-0.36998647, -0.11157012, -1.6628642), + Vector3::new(-0.34270653, -0.13490114, -1.6961168), + Vector3::new(-0.34270653, -0.15321401, -1.6910036), + Vector3::new(-0.36200994, -0.15321401, -1.6628642), + Vector3::new(-0.40922108, -0.1178531, -1.5963497), + Vector3::new(-0.40922108, -0.1059916, -1.6072279), + Vector3::new(-0.39379033, -0.15321401, -1.6165366), + Vector3::new(-0.3997105, -0.15321401, -1.5963497), + Vector3::new(-0.36998647, -0.11157012, -1.6628642), + Vector3::new(-0.3990667, -0.08669945, -1.6274172), + Vector3::new(-0.37014154, -0.086699456, -1.6628642), + Vector3::new(-0.36998647, -0.11157012, -1.6628642), + Vector3::new(-0.36200994, -0.15321401, -1.6628642), + Vector3::new(-0.39379033, -0.15321401, -1.6165366), + Vector3::new(-0.40922108, -0.1059916, -1.6072279), + Vector3::new(-0.40922108, -0.08669945, -1.6126148), + Vector3::new(-0.3990667, -0.08669945, -1.6274172), + Vector3::new(-0.40922108, -0.117853224, -1.5963496), + Vector3::new(-0.39971054, -0.15321401, -1.5963496), + Vector3::new(-0.40922108, -0.15321401, -1.5639201), + Vector3::new(-0.40242764, -0.15321401, -1.4023592), + Vector3::new(-0.4075218, -0.14427271, -1.396806), + Vector3::new(-0.40922108, -0.14255148, -1.396806), + Vector3::new(-0.40922108, -0.15321401, -1.4124653), + Vector3::new(-0.4075218, -0.14427273, -1.396806), + Vector3::new(-0.40242764, -0.15321401, -1.4023592), + Vector3::new(-0.3958334, -0.15321401, -1.396806), + Vector3::new(-0.40752193, -0.14427252, -1.3968059), + Vector3::new(-0.40922108, -0.14129017, -1.3949536), + Vector3::new(-0.40922108, -0.14255139, -1.3968059), + Vector3::new(-0.40922108, -0.10430696, -1.35424), + Vector3::new(-0.40922108, -0.14129019, -1.3949536), + Vector3::new(-0.40752193, -0.14427254, -1.3968059), + Vector3::new(-0.39583325, -0.15321401, -1.3968059), + Vector3::new(-0.34270653, -0.15321401, -1.3520659), + Vector3::new(-0.34270653, -0.13343461, -1.3302914), + Vector3::new(-0.38901964, -0.09800632, -1.3302914), + Vector3::new(-0.34270653, -0.086741775, -1.2821165), + Vector3::new(-0.34270653, -0.08669945, -1.2820915), + Vector3::new(-0.34281603, -0.08669945, -1.2820845), + Vector3::new(-0.38901955, -0.098006286, -1.3302913), + Vector3::new(-0.34270653, -0.1334345, -1.3302913), + Vector3::new(-0.34270653, -0.10629411, -1.3004133), + Vector3::new(-0.34689236, -0.08669945, -1.2823671), + Vector3::new(-0.35276693, -0.08669945, -1.2873143), + Vector3::new(-0.34281603, -0.08669945, -1.2820845), + Vector3::new(-0.34662613, -0.08669945, -1.2822367), + Vector3::new(-0.34270653, -0.097332954, -1.2912227), + Vector3::new(-0.34270653, -0.086741775, -1.2821165), + Vector3::new(-0.34662613, -0.08669945, -1.2822367), + Vector3::new(-0.34689236, -0.08669945, -1.2823671), + Vector3::new(-0.34270653, -0.106294096, -1.3004133), + Vector3::new(-0.34270653, -0.097332954, -1.2912227), + Vector3::new(-0.37055624, -0.020184837, -1.6628642), + Vector3::new(-0.34270653, -0.020184837, -1.6969932), + Vector3::new(-0.34270653, -0.0866994, -1.696485), + Vector3::new(-0.3701415, -0.0866994, -1.6628642), + Vector3::new(-0.37055624, -0.020184837, -1.6628642), + Vector3::new(-0.3701415, -0.0866994, -1.6628642), + Vector3::new(-0.39906675, -0.086699404, -1.6274171), + Vector3::new(-0.40922108, -0.07801496, -1.6150396), + Vector3::new(-0.40922108, -0.020184837, -1.6154815), + Vector3::new(-0.40922108, -0.086699404, -1.6126148), + Vector3::new(-0.40922108, -0.07801497, -1.6150396), + Vector3::new(-0.39906675, -0.086699404, -1.6274171), + Vector3::new(-0.3472593, -0.08498167, -1.2807851), + Vector3::new(-0.3488198, -0.0564432, -1.2637768), + Vector3::new(-0.36423904, -0.049429357, -1.2637768), + Vector3::new(-0.34270653, -0.086699404, -1.2820914), + Vector3::new(-0.34270653, -0.055789243, -1.2637768), + Vector3::new(-0.3488198, -0.056443203, -1.2637768), + Vector3::new(-0.3472593, -0.08498167, -1.2807851), + Vector3::new(-0.34281614, -0.086699404, -1.2820845), + Vector3::new(-0.34689236, -0.086699404, -1.2823671), + Vector3::new(-0.3472593, -0.08498167, -1.2807851), + Vector3::new(-0.35276678, -0.086699404, -1.2873142), + Vector3::new(-0.34281614, -0.086699404, -1.2820845), + Vector3::new(-0.3472593, -0.08498167, -1.2807851), + Vector3::new(-0.34662613, -0.086699404, -1.2822367), + Vector3::new(-0.34662613, -0.086699404, -1.2822367), + Vector3::new(-0.3472593, -0.08498167, -1.2807851), + Vector3::new(-0.34689236, -0.086699404, -1.2823671), + Vector3::new(-0.3488198, -0.0564432, -1.2637768), + Vector3::new(-0.35080242, -0.020184837, -1.2421676), + Vector3::new(-0.37820616, -0.020184837, -1.2497861), + Vector3::new(-0.36423904, -0.049429357, -1.2637768), + Vector3::new(-0.34270653, -0.055789243, -1.2637768), + Vector3::new(-0.34270653, -0.020184837, -1.2426808), + Vector3::new(-0.35080242, -0.020184845, -1.2421676), + Vector3::new(-0.3488198, -0.056443203, -1.2637768), + Vector3::new(-0.3709709, 0.046329774, -1.6628642), + Vector3::new(-0.34270653, 0.046329774, -1.6975014), + Vector3::new(-0.34270653, -0.020184793, -1.6969932), + Vector3::new(-0.37055624, -0.020184793, -1.6628642), + Vector3::new(-0.3709709, 0.046329774, -1.6628642), + Vector3::new(-0.37055624, -0.020184793, -1.6628642), + Vector3::new(-0.40922108, -0.020184793, -1.6154815), + Vector3::new(-0.40922108, 0.046329774, -1.6159897), + Vector3::new(-0.35080242, -0.020184793, -1.2421676), + Vector3::new(-0.35443944, 0.046329774, -1.2025263), + Vector3::new(-0.40922108, 0.046329774, -1.2177562), + Vector3::new(-0.40922108, 0.044754528, -1.2187189), + Vector3::new(-0.3782062, -0.020184793, -1.249786), + Vector3::new(-0.34270653, -0.020184793, -1.2426808), + Vector3::new(-0.34270653, 0.046329774, -1.2032701), + Vector3::new(-0.35443944, 0.046329767, -1.2025265), + Vector3::new(-0.35080242, -0.02018477, -1.2421676), + Vector3::new(-0.34755087, 0.112844266, -1.6920731), + Vector3::new(-0.34270653, 0.11284426, -1.694334), + Vector3::new(-0.34270653, 0.08907384, -1.697828), + Vector3::new(-0.34755087, 0.11284426, -1.6920731), + Vector3::new(-0.34270653, 0.08907384, -1.697828), + Vector3::new(-0.34270653, 0.0463297, -1.6975014), + Vector3::new(-0.3709709, 0.046329696, -1.6628642), + Vector3::new(-0.37138563, 0.11284426, -1.6628642), + Vector3::new(-0.37138563, 0.11284426, -1.6628642), + Vector3::new(-0.3709709, 0.046329696, -1.6628642), + Vector3::new(-0.40922108, 0.046329692, -1.6159897), + Vector3::new(-0.40922108, 0.112844266, -1.6164979), + Vector3::new(-0.35443944, 0.0463297, -1.2025265), + Vector3::new(-0.3549224, 0.055162504, -1.1972623), + Vector3::new(-0.40922108, 0.07986139, -1.1972623), + Vector3::new(-0.40922108, 0.0463297, -1.2177562), + Vector3::new(-0.35443944, 0.046329707, -1.2025265), + Vector3::new(-0.34270653, 0.046329707, -1.2032702), + Vector3::new(-0.34270653, 0.056469433, -1.1972623), + Vector3::new(-0.3549224, 0.05516251, -1.1972623), + Vector3::new(-0.34270653, 0.10713466, -1.1672425), + Vector3::new(-0.34270653, 0.112844266, -1.1643773), + Vector3::new(-0.34408033, 0.112844266, -1.1637725), + Vector3::new(-0.40392616, 0.112844266, -1.1756318), + Vector3::new(-0.40922108, 0.112844266, -1.1783407), + Vector3::new(-0.40922108, 0.10329369, -1.182941), + Vector3::new(-0.35492244, 0.055162705, -1.1972622), + Vector3::new(-0.35807648, 0.112844266, -1.1628852), + Vector3::new(-0.40392616, 0.112844266, -1.1756318), + Vector3::new(-0.40922108, 0.10329369, -1.182941), + Vector3::new(-0.40922108, 0.07986158, -1.1972622), + Vector3::new(-0.35492244, 0.055162713, -1.1972622), + Vector3::new(-0.34270653, 0.056469634, -1.1972622), + Vector3::new(-0.34270653, 0.10713467, -1.1672425), + Vector3::new(-0.34408033, 0.112844266, -1.1637725), + Vector3::new(-0.35807648, 0.112844266, -1.1628852), + Vector3::new(-0.34270653, 0.17935875, -1.6845568), + Vector3::new(-0.34270653, 0.1128442, -1.694334), + Vector3::new(-0.34755087, 0.11284419, -1.6920731), + Vector3::new(-0.36110634, 0.17935875, -1.6759695), + Vector3::new(-0.37180036, 0.17935874, -1.6628642), + Vector3::new(-0.36110634, 0.17935874, -1.6759695), + Vector3::new(-0.34755087, 0.1128442, -1.6920731), + Vector3::new(-0.37138563, 0.1128442, -1.6628642), + Vector3::new(-0.37180036, 0.17935874, -1.6628642), + Vector3::new(-0.37138563, 0.1128442, -1.6628642), + Vector3::new(-0.40922108, 0.11284419, -1.6164979), + Vector3::new(-0.40922108, 0.17935875, -1.6170061), + Vector3::new(-0.40922108, 0.11284423, -1.5963496), + Vector3::new(-0.34270653, 0.11284423, -1.5963496), + Vector3::new(-0.34270653, 0.17935872, -1.5963496), + Vector3::new(-0.40922108, 0.17935872, -1.5963496), + Vector3::new(-0.40922108, 0.11284423, -1.529835), + Vector3::new(-0.34270653, 0.11284423, -1.529835), + Vector3::new(-0.34270653, 0.17935872, -1.529835), + Vector3::new(-0.40922108, 0.17935872, -1.529835), + Vector3::new(-0.40922108, 0.11284423, -1.529835), + Vector3::new(-0.34270653, 0.11284423, -1.529835), + Vector3::new(-0.34270653, 0.17935872, -1.529835), + Vector3::new(-0.40922108, 0.17935872, -1.529835), + Vector3::new(-0.40922108, 0.11284423, -1.4633205), + Vector3::new(-0.34270653, 0.11284423, -1.4633205), + Vector3::new(-0.34270653, 0.17935872, -1.4633205), + Vector3::new(-0.40922108, 0.17935872, -1.4633205), + Vector3::new(-0.40922108, 0.11284423, -1.4633205), + Vector3::new(-0.34270653, 0.11284423, -1.4633205), + Vector3::new(-0.34270653, 0.17935872, -1.4633205), + Vector3::new(-0.40922108, 0.17935872, -1.4633205), + Vector3::new(-0.40922108, 0.11284423, -1.396806), + Vector3::new(-0.34270653, 0.11284423, -1.396806), + Vector3::new(-0.34270653, 0.17935872, -1.396806), + Vector3::new(-0.40922108, 0.17935872, -1.396806), + Vector3::new(-0.40922108, 0.11284423, -1.396806), + Vector3::new(-0.34270653, 0.11284423, -1.396806), + Vector3::new(-0.34270653, 0.17935872, -1.396806), + Vector3::new(-0.40922108, 0.17935872, -1.396806), + Vector3::new(-0.40922108, 0.11284423, -1.3302914), + Vector3::new(-0.34270653, 0.11284423, -1.3302914), + Vector3::new(-0.34270653, 0.17935872, -1.3302914), + Vector3::new(-0.40922108, 0.17935872, -1.3302914), + Vector3::new(-0.40922108, 0.11284423, -1.3302914), + Vector3::new(-0.34270653, 0.11284423, -1.3302914), + Vector3::new(-0.34270653, 0.17935872, -1.3302914), + Vector3::new(-0.40922108, 0.17935872, -1.3302914), + Vector3::new(-0.40922108, 0.11284423, -1.2637768), + Vector3::new(-0.34270653, 0.11284423, -1.2637768), + Vector3::new(-0.34270653, 0.17935872, -1.2637768), + Vector3::new(-0.40922108, 0.17935872, -1.2637768), + Vector3::new(-0.40922108, 0.11284423, -1.2637768), + Vector3::new(-0.34270653, 0.11284423, -1.2637768), + Vector3::new(-0.34270653, 0.17935872, -1.2637768), + Vector3::new(-0.40922108, 0.17935872, -1.2637768), + Vector3::new(-0.40922108, 0.11284423, -1.1972623), + Vector3::new(-0.34270653, 0.11284423, -1.1972623), + Vector3::new(-0.34270653, 0.17935872, -1.1972623), + Vector3::new(-0.40922108, 0.17935872, -1.1972623), + Vector3::new(-0.3571547, 0.16718233, -1.1307477), + Vector3::new(-0.3440803, 0.11284419, -1.1637725), + Vector3::new(-0.34270653, 0.11284419, -1.1643773), + Vector3::new(-0.34270653, 0.15000299, -1.1457299), + Vector3::new(-0.35324317, 0.17061399, -1.1307477), + Vector3::new(-0.34270653, 0.15000299, -1.1457299), + Vector3::new(-0.34270653, 0.17935875, -1.1310631), + Vector3::new(-0.343416, 0.17935875, -1.1307477), + Vector3::new(-0.35324317, 0.17061399, -1.1307477), + Vector3::new(-0.40922108, 0.17935875, -1.1463025), + Vector3::new(-0.40922108, 0.11284419, -1.1783408), + Vector3::new(-0.4039262, 0.11284419, -1.1756319), + Vector3::new(-0.37141103, 0.17149258, -1.1307477), + Vector3::new(-0.37881705, 0.17935875, -1.1307477), + Vector3::new(-0.37141103, 0.17149258, -1.1307477), + Vector3::new(-0.4039262, 0.11284419, -1.1756319), + Vector3::new(-0.35807645, 0.11284419, -1.1628852), + Vector3::new(-0.36102504, 0.16676828, -1.1307477), + Vector3::new(-0.36102504, 0.16676828, -1.1307477), + Vector3::new(-0.35807645, 0.11284419, -1.1628852), + Vector3::new(-0.3440803, 0.11284419, -1.1637725), + Vector3::new(-0.3571547, 0.16718233, -1.1307477), + Vector3::new(-0.3577136, 0.17935875, -1.1243911), + Vector3::new(-0.36008447, 0.17935875, -1.1233473), + Vector3::new(-0.35715473, 0.16718253, -1.1307476), + Vector3::new(-0.35324326, 0.17061415, -1.1307476), + Vector3::new(-0.35324326, 0.17061415, -1.1307476), + Vector3::new(-0.34341627, 0.17935875, -1.1307476), + Vector3::new(-0.3577136, 0.17935875, -1.1243911), + Vector3::new(-0.3788168, 0.17935875, -1.1307476), + Vector3::new(-0.37141097, 0.17149273, -1.1307476), + Vector3::new(-0.36704996, 0.17935875, -1.1247276), + Vector3::new(-0.3617135, 0.17935875, -1.123244), + Vector3::new(-0.36704996, 0.17935875, -1.1247276), + Vector3::new(-0.37141097, 0.17149273, -1.1307476), + Vector3::new(-0.36102507, 0.16676848, -1.1307476), + Vector3::new(-0.36102507, 0.16676848, -1.1307476), + Vector3::new(-0.35715473, 0.16718253, -1.1307476), + Vector3::new(-0.36008447, 0.17935875, -1.1233473), + Vector3::new(-0.3617135, 0.17935875, -1.123244), + Vector3::new(-0.29332903, -0.15321395, -1.7564877), + Vector3::new(-0.2885518, -0.15321395, -1.7587173), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.31541878, -0.15823883, -1.7293788), + Vector3::new(-0.31545013, -0.15321395, -1.7293788), + Vector3::new(-0.29332903, -0.15321395, -1.7564877), + Vector3::new(-0.30393142, -0.21821208, -1.7293788), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.28400028, -0.2197285, -1.7625116), + Vector3::new(-0.30376926, -0.2197285, -1.7293788), + Vector3::new(-0.31541878, -0.15823883, -1.7293788), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.30393142, -0.21821208, -1.7293788), + Vector3::new(-0.23608275, -0.038243636, -1.7570658), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.3002659, 0.14816007, -1.7089504), + Vector3::new(-0.27619198, -0.15321395, -1.7627211), + Vector3::new(-0.27619198, -0.20534962, -1.7756828), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.27709365, -0.15321395, -1.7622056), + Vector3::new(-0.27619198, -0.2197285, -1.7805436), + Vector3::new(-0.27641594, -0.2197285, -1.7804569), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.27619198, -0.20534962, -1.7756828), + Vector3::new(-0.28400028, -0.2197285, -1.7625116), + Vector3::new(-0.28742594, -0.18217954, -1.7635005), + Vector3::new(-0.27641594, -0.2197285, -1.7804569), + Vector3::new(-0.31541887, -0.15823874, -1.7293787), + Vector3::new(-0.32129416, -0.15321395, -1.7222171), + Vector3::new(-0.31545022, -0.15321395, -1.7293787), + Vector3::new(-0.30393147, -0.21821222, -1.7293787), + Vector3::new(-0.30376932, -0.2197285, -1.7293787), + Vector3::new(-0.30462605, -0.2197285, -1.7279428), + Vector3::new(-0.34270653, -0.2197285, -1.6724315), + Vector3::new(-0.34270653, -0.15321395, -1.6910034), + Vector3::new(-0.32129416, -0.15321395, -1.7222171), + Vector3::new(-0.31541887, -0.15823874, -1.7293787), + Vector3::new(-0.30393147, -0.21821222, -1.7293787), + Vector3::new(-0.30462605, -0.2197285, -1.7279428), + Vector3::new(-0.34270653, -0.21972859, -1.6628642), + Vector3::new(-0.27619195, -0.21972859, -1.6628642), + Vector3::new(-0.27619195, -0.15321398, -1.6628642), + Vector3::new(-0.34270653, -0.15321398, -1.6628642), + Vector3::new(-0.34270653, -0.21972859, -1.5963496), + Vector3::new(-0.27619195, -0.21972859, -1.5963496), + Vector3::new(-0.27619195, -0.15321398, -1.5963496), + Vector3::new(-0.34270653, -0.15321398, -1.5963496), + Vector3::new(-0.34270653, -0.21972859, -1.5963496), + Vector3::new(-0.27619195, -0.21972859, -1.5963496), + Vector3::new(-0.27619195, -0.15321398, -1.5963496), + Vector3::new(-0.34270653, -0.15321398, -1.5963496), + Vector3::new(-0.34270653, -0.21972859, -1.529835), + Vector3::new(-0.27619195, -0.21972859, -1.529835), + Vector3::new(-0.27619195, -0.15321398, -1.529835), + Vector3::new(-0.34270653, -0.15321398, -1.529835), + Vector3::new(-0.34270653, -0.21972859, -1.529835), + Vector3::new(-0.27619195, -0.21972859, -1.529835), + Vector3::new(-0.27619195, -0.15321398, -1.529835), + Vector3::new(-0.34270653, -0.15321398, -1.529835), + Vector3::new(-0.34270653, -0.21972859, -1.4633205), + Vector3::new(-0.27619195, -0.21972859, -1.4633205), + Vector3::new(-0.27619195, -0.15321398, -1.4633205), + Vector3::new(-0.34270653, -0.15321398, -1.4633205), + Vector3::new(-0.34270653, -0.19385463, -1.396806), + Vector3::new(-0.34270653, -0.2197285, -1.4252898), + Vector3::new(-0.31847456, -0.2197285, -1.4048831), + Vector3::new(-0.32034805, -0.21095827, -1.396806), + Vector3::new(-0.32034805, -0.21095827, -1.396806), + Vector3::new(-0.31847456, -0.2197285, -1.4048831), + Vector3::new(-0.30198348, -0.2197285, -1.396806), + Vector3::new(-0.27619198, -0.17143622, -1.3522788), + Vector3::new(-0.27619198, -0.15321395, -1.3391918), + Vector3::new(-0.28043228, -0.15321395, -1.336781), + Vector3::new(-0.33268347, -0.15321395, -1.3436251), + Vector3::new(-0.34270653, -0.15321395, -1.3520659), + Vector3::new(-0.34270653, -0.19385453, -1.3968059), + Vector3::new(-0.32034808, -0.21095814, -1.3968059), + Vector3::new(-0.27619198, -0.17143622, -1.3522788), + Vector3::new(-0.28043228, -0.15321395, -1.336781), + Vector3::new(-0.32210836, -0.15321395, -1.3384455), + Vector3::new(-0.29759055, -0.2197285, -1.3946544), + Vector3::new(-0.27619198, -0.2197285, -1.3937998), + Vector3::new(-0.32034808, -0.21095814, -1.3968059), + Vector3::new(-0.30198324, -0.2197285, -1.3968059), + Vector3::new(-0.29759055, -0.2197285, -1.3946544), + Vector3::new(-0.32210836, -0.15321395, -1.3384455), + Vector3::new(-0.33268347, -0.15321395, -1.3436251), + Vector3::new(-0.29113716, -0.08669944, -1.7477335), + Vector3::new(-0.2885518, -0.15321401, -1.7587173), + Vector3::new(-0.29332903, -0.15321401, -1.7564877), + Vector3::new(-0.3068845, -0.08669945, -1.7403841), + Vector3::new(-0.31586492, -0.086699456, -1.7293788), + Vector3::new(-0.3068845, -0.086699456, -1.7403841), + Vector3::new(-0.29332903, -0.15321401, -1.7564877), + Vector3::new(-0.31545013, -0.15321401, -1.7293788), + Vector3::new(-0.27619198, -0.15068617, -1.7620926), + Vector3::new(-0.27619198, -0.15321401, -1.7627211), + Vector3::new(-0.27709368, -0.15321401, -1.7622056), + Vector3::new(-0.315865, -0.086699456, -1.7293787), + Vector3::new(-0.31545022, -0.15321401, -1.7293787), + Vector3::new(-0.32129407, -0.15321401, -1.7222172), + Vector3::new(-0.34270653, -0.13490114, -1.6961167), + Vector3::new(-0.34270653, -0.08669945, -1.696485), + Vector3::new(-0.34270653, -0.15321401, -1.6910034), + Vector3::new(-0.34270653, -0.13490114, -1.6961167), + Vector3::new(-0.32129407, -0.15321401, -1.7222172), + Vector3::new(-0.2912733, -0.106625855, -1.2971584), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.25837165, -0.014966724, -1.2656467), + Vector3::new(-0.28220788, -0.14558353, -1.3302914), + Vector3::new(-0.28043228, -0.15321401, -1.336781), + Vector3::new(-0.27619198, -0.15321401, -1.3391918), + Vector3::new(-0.27619198, -0.14082113, -1.3302914), + Vector3::new(-0.33268344, -0.15321401, -1.3436252), + Vector3::new(-0.33577624, -0.13873605, -1.3302914), + Vector3::new(-0.34270653, -0.13343461, -1.3302914), + Vector3::new(-0.34270653, -0.15321401, -1.3520659), + Vector3::new(-0.32210833, -0.15321401, -1.3384455), + Vector3::new(-0.28043228, -0.15321401, -1.336781), + Vector3::new(-0.28220788, -0.14558353, -1.3302914), + Vector3::new(-0.3256651, -0.1435648, -1.3302914), + Vector3::new(-0.33577624, -0.13873605, -1.3302914), + Vector3::new(-0.33268344, -0.15321401, -1.3436252), + Vector3::new(-0.32210833, -0.15321401, -1.3384455), + Vector3::new(-0.3256651, -0.1435648, -1.3302914), + Vector3::new(-0.27619198, -0.11314488, -1.3104147), + Vector3::new(-0.2912733, -0.106625855, -1.2971584), + Vector3::new(-0.2822079, -0.14558339, -1.3302913), + Vector3::new(-0.27619198, -0.14082097, -1.3302913), + Vector3::new(-0.2912733, -0.106625855, -1.2971584), + Vector3::new(-0.2841206, -0.08669945, -1.2903078), + Vector3::new(-0.29606783, -0.08669945, -1.2850478), + Vector3::new(-0.34270653, -0.086699456, -1.2820915), + Vector3::new(-0.34270653, -0.086741775, -1.2821165), + Vector3::new(-0.2912733, -0.106625855, -1.2971584), + Vector3::new(-0.2960678, -0.08669945, -1.2850478), + Vector3::new(-0.33577627, -0.13873592, -1.3302913), + Vector3::new(-0.34270653, -0.10629411, -1.3004133), + Vector3::new(-0.34270653, -0.1334345, -1.3302913), + Vector3::new(-0.32566515, -0.14356467, -1.3302913), + Vector3::new(-0.28220794, -0.14558339, -1.3302913), + Vector3::new(-0.2912733, -0.106625855, -1.2971584), + Vector3::new(-0.34270653, -0.086741775, -1.2821165), + Vector3::new(-0.34270653, -0.097332954, -1.2912227), + Vector3::new(-0.33577627, -0.13873592, -1.3302913), + Vector3::new(-0.32566515, -0.14356467, -1.3302913), + Vector3::new(-0.34270653, -0.097332954, -1.2912227), + Vector3::new(-0.34270653, -0.106294096, -1.3004133), + Vector3::new(-0.2937225, -0.020184845, -1.7367498), + Vector3::new(-0.29113716, -0.0866994, -1.7477335), + Vector3::new(-0.3068845, -0.086699404, -1.740384), + Vector3::new(-0.31614825, -0.04124357, -1.7293788), + Vector3::new(-0.3095158, -0.020184841, -1.7293788), + Vector3::new(-0.31614825, -0.041243568, -1.7293788), + Vector3::new(-0.3068845, -0.0866994, -1.740384), + Vector3::new(-0.31586483, -0.0866994, -1.7293788), + Vector3::new(-0.2045752, 0.17152071, -1.7329516), + Vector3::new(-0.30951604, -0.020184841, -1.7293787), + Vector3::new(-0.31614837, -0.04124308, -1.7293787), + Vector3::new(-0.32043996, -0.020184837, -1.7242804), + Vector3::new(-0.34270653, -0.020184837, -1.6969932), + Vector3::new(-0.32043996, -0.020184837, -1.7242804), + Vector3::new(-0.31614837, -0.041243076, -1.7293787), + Vector3::new(-0.31586492, -0.0866994, -1.7293787), + Vector3::new(-0.34270653, -0.086699404, -1.696485), + Vector3::new(-0.27619198, -0.06461153, -1.2827141), + Vector3::new(-0.27619198, -0.026874736, -1.2637768), + Vector3::new(-0.30448896, -0.051700555, -1.2637768), + Vector3::new(-0.29606783, -0.086699404, -1.2850478), + Vector3::new(-0.28412056, -0.086699404, -1.2903078), + Vector3::new(-0.34270653, -0.055789243, -1.2637768), + Vector3::new(-0.34270653, -0.0866994, -1.2820914), + Vector3::new(-0.29606783, -0.086699404, -1.2850478), + Vector3::new(-0.30448896, -0.051700555, -1.2637768), + Vector3::new(-0.27619198, -0.026874736, -1.2637768), + Vector3::new(-0.27619198, -0.020184837, -1.2604196), + Vector3::new(-0.312072, -0.020184837, -1.2446227), + Vector3::new(-0.30448896, -0.051700555, -1.2637768), + Vector3::new(-0.34270653, -0.055789243, -1.2637768), + Vector3::new(-0.30448896, -0.051700555, -1.2637768), + Vector3::new(-0.312072, -0.020184837, -1.2446227), + Vector3::new(-0.34270653, -0.020184845, -1.2426808), + Vector3::new(-0.29545748, 0.024451343, -1.7293788), + Vector3::new(-0.2937225, -0.020184785, -1.7367498), + Vector3::new(-0.3095158, -0.020184785, -1.7293788), + Vector3::new(-0.2954575, 0.024452064, -1.7293787), + Vector3::new(-0.30951604, -0.020184785, -1.7293787), + Vector3::new(-0.32043996, -0.020184785, -1.7242804), + Vector3::new(-0.33399543, 0.046329774, -1.7081767), + Vector3::new(-0.29630786, 0.046329774, -1.725766), + Vector3::new(-0.34270653, 0.046329774, -1.6975014), + Vector3::new(-0.33399543, 0.046329774, -1.7081767), + Vector3::new(-0.32043996, -0.020184793, -1.7242804), + Vector3::new(-0.34270653, -0.020184785, -1.6969932), + Vector3::new(-0.27619198, 0.019892095, -1.2403078), + Vector3::new(-0.2897073, 0.046329774, -1.2210902), + Vector3::new(-0.32807618, 0.046329774, -1.2041975), + Vector3::new(-0.312072, -0.020184793, -1.2446226), + Vector3::new(-0.27619198, -0.020184793, -1.2604195), + Vector3::new(-0.27619198, 0.019892097, -1.2403078), + Vector3::new(-0.27619198, 0.046329774, -1.227099), + Vector3::new(-0.2897073, 0.046329774, -1.2210902), + Vector3::new(-0.34270653, 0.046329774, -1.2032702), + Vector3::new(-0.34270653, -0.020184785, -1.2426808), + Vector3::new(-0.312072, -0.020184793, -1.2446226), + Vector3::new(-0.32807618, 0.046329774, -1.2041975), + Vector3::new(-0.2988932, 0.112844266, -1.7147822), + Vector3::new(-0.29630786, 0.0463297, -1.725766), + Vector3::new(-0.3339954, 0.046329707, -1.7081767), + Vector3::new(-0.34270653, 0.08907384, -1.697828), + Vector3::new(-0.34270653, 0.112844266, -1.694334), + Vector3::new(-0.34270653, 0.046329707, -1.6975014), + Vector3::new(-0.34270653, 0.08907384, -1.697828), + Vector3::new(-0.3339954, 0.0463297, -1.7081767), + Vector3::new(-0.28970727, 0.0463297, -1.2210902), + Vector3::new(-0.30646494, 0.07910983, -1.1972623), + Vector3::new(-0.33082178, 0.057740793, -1.1972623), + Vector3::new(-0.32807615, 0.0463297, -1.2041975), + Vector3::new(-0.30646497, 0.07910992, -1.1972623), + Vector3::new(-0.28970724, 0.0463297, -1.2210903), + Vector3::new(-0.27619198, 0.0463297, -1.227099), + Vector3::new(-0.27619198, 0.10604803, -1.1972623), + Vector3::new(-0.34270653, 0.05646943, -1.1972623), + Vector3::new(-0.34270653, 0.0463297, -1.2032702), + Vector3::new(-0.32807615, 0.046329707, -1.2041975), + Vector3::new(-0.33082178, 0.0577408, -1.1972623), + Vector3::new(-0.30646503, 0.07911, -1.1972622), + Vector3::new(-0.32371047, 0.112844266, -1.1727407), + Vector3::new(-0.34270653, 0.112844266, -1.1643773), + Vector3::new(-0.34270653, 0.10713466, -1.1672425), + Vector3::new(-0.33082184, 0.057740986, -1.1972622), + Vector3::new(-0.27619198, 0.112844266, -1.1938667), + Vector3::new(-0.32371044, 0.112844266, -1.1727407), + Vector3::new(-0.30646503, 0.07911008, -1.1972622), + Vector3::new(-0.27619198, 0.10604827, -1.1972622), + Vector3::new(-0.34270653, 0.05646963, -1.1972622), + Vector3::new(-0.33082184, 0.057740998, -1.1972622), + Vector3::new(-0.34270653, 0.10713467, -1.1672425), + Vector3::new(-0.31214952, 0.17935875, -1.6988182), + Vector3::new(-0.3002659, 0.14816007, -1.7089504), + Vector3::new(-0.2988932, 0.11284419, -1.7147822), + Vector3::new(-0.34270653, 0.11284419, -1.694334), + Vector3::new(-0.34270653, 0.17935875, -1.6845568), + Vector3::new(-0.3002659, 0.14816007, -1.7089504), + Vector3::new(-0.31214952, 0.17935875, -1.6988182), + Vector3::new(-0.3041196, 0.17935875, -1.7055525), + Vector3::new(-0.3041196, 0.17935875, -1.7055525), + Vector3::new(-0.29330474, 0.17935875, -1.7115203), + Vector3::new(-0.3002659, 0.14816007, -1.7089504), + Vector3::new(-0.27619198, 0.17935875, -1.7156959), + Vector3::new(-0.27619198, 0.15403715, -1.7149886), + Vector3::new(-0.3002659, 0.14816007, -1.7089504), + Vector3::new(-0.2933047, 0.17935875, -1.7115203), + Vector3::new(-0.34270653, 0.11284423, -1.6628642), + Vector3::new(-0.27619195, 0.11284423, -1.6628642), + Vector3::new(-0.27619195, 0.17935872, -1.6628642), + Vector3::new(-0.34270653, 0.17935872, -1.6628642), + Vector3::new(-0.34270653, 0.11284423, -1.5963496), + Vector3::new(-0.27619195, 0.11284423, -1.5963496), + Vector3::new(-0.27619195, 0.17935872, -1.5963496), + Vector3::new(-0.34270653, 0.17935872, -1.5963496), + Vector3::new(-0.34270653, 0.11284423, -1.5963496), + Vector3::new(-0.27619195, 0.11284423, -1.5963496), + Vector3::new(-0.27619195, 0.17935872, -1.5963496), + Vector3::new(-0.34270653, 0.17935872, -1.5963496), + Vector3::new(-0.34270653, 0.11284423, -1.529835), + Vector3::new(-0.27619195, 0.11284423, -1.529835), + Vector3::new(-0.27619195, 0.17935872, -1.529835), + Vector3::new(-0.34270653, 0.17935872, -1.529835), + Vector3::new(-0.34270653, 0.11284423, -1.529835), + Vector3::new(-0.27619195, 0.11284423, -1.529835), + Vector3::new(-0.27619195, 0.17935872, -1.529835), + Vector3::new(-0.34270653, 0.17935872, -1.529835), + Vector3::new(-0.34270653, 0.11284423, -1.4633205), + Vector3::new(-0.27619195, 0.11284423, -1.4633205), + Vector3::new(-0.27619195, 0.17935872, -1.4633205), + Vector3::new(-0.34270653, 0.17935872, -1.4633205), + Vector3::new(-0.34270653, 0.11284423, -1.4633205), + Vector3::new(-0.27619195, 0.11284423, -1.4633205), + Vector3::new(-0.27619195, 0.17935872, -1.4633205), + Vector3::new(-0.34270653, 0.17935872, -1.4633205), + Vector3::new(-0.34270653, 0.11284423, -1.396806), + Vector3::new(-0.27619195, 0.11284423, -1.396806), + Vector3::new(-0.27619195, 0.17935872, -1.396806), + Vector3::new(-0.34270653, 0.17935872, -1.396806), + Vector3::new(-0.34270653, 0.11284423, -1.396806), + Vector3::new(-0.27619195, 0.11284423, -1.396806), + Vector3::new(-0.27619195, 0.17935872, -1.396806), + Vector3::new(-0.34270653, 0.17935872, -1.396806), + Vector3::new(-0.34270653, 0.11284423, -1.3302914), + Vector3::new(-0.27619195, 0.11284423, -1.3302914), + Vector3::new(-0.27619195, 0.17935872, -1.3302914), + Vector3::new(-0.34270653, 0.17935872, -1.3302914), + Vector3::new(-0.34270653, 0.11284423, -1.3302914), + Vector3::new(-0.27619195, 0.11284423, -1.3302914), + Vector3::new(-0.27619195, 0.17935872, -1.3302914), + Vector3::new(-0.34270653, 0.17935872, -1.3302914), + Vector3::new(-0.34270653, 0.11284423, -1.2637768), + Vector3::new(-0.27619195, 0.11284423, -1.2637768), + Vector3::new(-0.27619195, 0.17935872, -1.2637768), + Vector3::new(-0.34270653, 0.17935872, -1.2637768), + Vector3::new(-0.34270653, 0.11284423, -1.2637768), + Vector3::new(-0.27619195, 0.11284423, -1.2637768), + Vector3::new(-0.27619195, 0.17935872, -1.2637768), + Vector3::new(-0.34270653, 0.17935872, -1.2637768), + Vector3::new(-0.34270653, 0.11284423, -1.1972623), + Vector3::new(-0.27619195, 0.11284423, -1.1972623), + Vector3::new(-0.27619195, 0.17935872, -1.1972623), + Vector3::new(-0.34270653, 0.17935872, -1.1972623), + Vector3::new(-0.3237104, 0.11284419, -1.1727407), + Vector3::new(-0.34270653, 0.15000299, -1.1457299), + Vector3::new(-0.34270653, 0.11284419, -1.1643773), + Vector3::new(-0.34270653, 0.17935875, -1.1310631), + Vector3::new(-0.34270653, 0.15000299, -1.1457299), + Vector3::new(-0.3237104, 0.11284419, -1.1727407), + Vector3::new(-0.27619198, 0.11284419, -1.1938668), + Vector3::new(-0.27619198, 0.17935875, -1.1606346), + Vector3::new(-0.2096774, -0.18912837, -1.8096728), + Vector3::new(-0.21596254, -0.15321395, -1.797151), + Vector3::new(-0.2096774, -0.15321395, -1.797623), + Vector3::new(-0.2096774, -0.2197285, -1.8172805), + Vector3::new(-0.24709052, -0.2197285, -1.7958934), + Vector3::new(-0.2181624, -0.15321395, -1.7958934), + Vector3::new(-0.21596254, -0.15321395, -1.797151), + Vector3::new(-0.2096774, -0.18912835, -1.8096728), + Vector3::new(-0.24709073, -0.2197285, -1.7958933), + Vector3::new(-0.2692204, -0.2197285, -1.783243), + Vector3::new(-0.27619195, -0.20534968, -1.7756828), + Vector3::new(-0.27619195, -0.15321395, -1.7627211), + Vector3::new(-0.21816261, -0.15321395, -1.7958933), + Vector3::new(-0.27619195, -0.2197285, -1.7805436), + Vector3::new(-0.27619195, -0.20534968, -1.7756828), + Vector3::new(-0.2692204, -0.2197285, -1.783243), + Vector3::new(-0.27619195, -0.21972859, -1.7293787), + Vector3::new(-0.2096774, -0.21972859, -1.7293787), + Vector3::new(-0.2096774, -0.15321398, -1.7293787), + Vector3::new(-0.27619195, -0.15321398, -1.7293787), + Vector3::new(-0.27619195, -0.21972859, -1.6628642), + Vector3::new(-0.2096774, -0.21972859, -1.6628642), + Vector3::new(-0.2096774, -0.15321398, -1.6628642), + Vector3::new(-0.27619195, -0.15321398, -1.6628642), + Vector3::new(-0.27619195, -0.21972859, -1.6628642), + Vector3::new(-0.2096774, -0.21972859, -1.6628642), + Vector3::new(-0.2096774, -0.15321398, -1.6628642), + Vector3::new(-0.27619195, -0.15321398, -1.6628642), + Vector3::new(-0.27619195, -0.21972859, -1.5963496), + Vector3::new(-0.2096774, -0.21972859, -1.5963496), + Vector3::new(-0.2096774, -0.15321398, -1.5963496), + Vector3::new(-0.27619195, -0.15321398, -1.5963496), + Vector3::new(-0.27619195, -0.21972859, -1.5963496), + Vector3::new(-0.2096774, -0.21972859, -1.5963496), + Vector3::new(-0.2096774, -0.15321398, -1.5963496), + Vector3::new(-0.27619195, -0.15321398, -1.5963496), + Vector3::new(-0.27619195, -0.21972859, -1.529835), + Vector3::new(-0.2096774, -0.21972859, -1.529835), + Vector3::new(-0.2096774, -0.15321398, -1.529835), + Vector3::new(-0.27619195, -0.15321398, -1.529835), + Vector3::new(-0.27619195, -0.21972859, -1.529835), + Vector3::new(-0.2096774, -0.21972859, -1.529835), + Vector3::new(-0.2096774, -0.15321398, -1.529835), + Vector3::new(-0.27619195, -0.15321398, -1.529835), + Vector3::new(-0.27619195, -0.21972859, -1.4633205), + Vector3::new(-0.2096774, -0.21972859, -1.4633205), + Vector3::new(-0.2096774, -0.15321398, -1.4633205), + Vector3::new(-0.27619195, -0.15321398, -1.4633205), + Vector3::new(-0.20989056, -0.18094835, -1.396806), + Vector3::new(-0.20977727, -0.2197285, -1.4247218), + Vector3::new(-0.2096774, -0.2197285, -1.4247513), + Vector3::new(-0.2096774, -0.1808609, -1.396806), + Vector3::new(-0.25887728, -0.2197285, -1.396806), + Vector3::new(-0.20977727, -0.2197285, -1.4247218), + Vector3::new(-0.20989056, -0.18094835, -1.396806), + Vector3::new(-0.2096774, -0.15321395, -1.3769282), + Vector3::new(-0.20997159, -0.15321395, -1.3768414), + Vector3::new(-0.20989056, -0.18094818, -1.3968059), + Vector3::new(-0.2096774, -0.18086074, -1.3968059), + Vector3::new(-0.27619195, -0.15321395, -1.3391919), + Vector3::new(-0.27619195, -0.17143634, -1.352279), + Vector3::new(-0.26495442, -0.2197285, -1.3933508), + Vector3::new(-0.2588775, -0.2197285, -1.3968059), + Vector3::new(-0.20989056, -0.1809482, -1.3968059), + Vector3::new(-0.20997159, -0.15321395, -1.3768414), + Vector3::new(-0.27619195, -0.17143634, -1.352279), + Vector3::new(-0.27619195, -0.2197285, -1.3937997), + Vector3::new(-0.2649544, -0.2197285, -1.393351), + Vector3::new(-0.2096774, -0.14805892, -1.7958934), + Vector3::new(-0.2096774, -0.15321401, -1.797623), + Vector3::new(-0.21596253, -0.15321401, -1.7971511), + Vector3::new(-0.21659379, -0.14960687, -1.7958934), + Vector3::new(-0.21659379, -0.14960688, -1.7958934), + Vector3::new(-0.21596253, -0.15321401, -1.7971511), + Vector3::new(-0.21816258, -0.15321401, -1.7958934), + Vector3::new(-0.22760281, -0.08669945, -1.7739602), + Vector3::new(-0.2096774, -0.08669945, -1.7753065), + Vector3::new(-0.2096774, -0.14805856, -1.7958933), + Vector3::new(-0.21659385, -0.14960653, -1.7958933), + Vector3::new(-0.21659385, -0.14960653, -1.7958933), + Vector3::new(-0.21816279, -0.15321401, -1.7958933), + Vector3::new(-0.27619195, -0.15321401, -1.7627211), + Vector3::new(-0.27619195, -0.1506861, -1.7620926), + Vector3::new(-0.25336736, -0.08669945, -1.759232), + Vector3::new(-0.22760281, -0.08669945, -1.7739602), + Vector3::new(-0.2096774, -0.1415204, -1.3689709), + Vector3::new(-0.2096774, -0.13848218, -1.3677686), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.2096774, -0.14281492, -1.3697803), + Vector3::new(-0.2096774, -0.1415204, -1.3689709), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.2096774, -0.08669945, -1.3487011), + Vector3::new(-0.22550425, -0.08669945, -1.3302914), + Vector3::new(-0.2279999, -0.09458314, -1.3302914), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.2096774, -0.1384822, -1.3677686), + Vector3::new(-0.2096774, -0.14527045, -1.3712171), + Vector3::new(-0.2096774, -0.14281493, -1.3697803), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.2096774, -0.14527047, -1.371217), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.20997159, -0.15321401, -1.3768414), + Vector3::new(-0.2096774, -0.15321401, -1.3769283), + Vector3::new(-0.21000507, -0.14175473, -1.3685925), + Vector3::new(-0.253579, -0.12291954, -1.3302914), + Vector3::new(-0.27619195, -0.14082097, -1.3302914), + Vector3::new(-0.27619195, -0.15321401, -1.3391919), + Vector3::new(-0.20997159, -0.15321401, -1.3768414), + Vector3::new(-0.22550434, -0.08669945, -1.3302913), + Vector3::new(-0.23100734, -0.08669945, -1.3238902), + Vector3::new(-0.22799996, -0.09458299, -1.3302913), + Vector3::new(-0.25357914, -0.12291948, -1.3302913), + Vector3::new(-0.27619195, -0.1131449, -1.3104148), + Vector3::new(-0.27619195, -0.1408208, -1.3302913), + Vector3::new(-0.23608275, -0.038243636, -1.7570658), + Vector3::new(-0.2096774, -0.039554533, -1.7594886), + Vector3::new(-0.2096774, -0.086699404, -1.7753063), + Vector3::new(-0.22760282, -0.086699404, -1.7739602), + Vector3::new(-0.25336733, -0.086699404, -1.759232), + Vector3::new(-0.23608275, -0.038243636, -1.7570658), + Vector3::new(-0.22760282, -0.086699404, -1.7739602), + Vector3::new(-0.14053856, -0.042986937, -1.7658323), + Vector3::new(-0.22550425, -0.086699404, -1.3302914), + Vector3::new(-0.2096774, -0.086699404, -1.3487011), + Vector3::new(-0.2096774, -0.036703166, -1.3302914), + Vector3::new(-0.2096774, -0.020184837, -1.324209), + Vector3::new(-0.25638106, -0.020184837, -1.2698835), + Vector3::new(-0.23100735, -0.086699404, -1.3238902), + Vector3::new(-0.22550435, -0.086699404, -1.3302913), + Vector3::new(-0.2096774, -0.036702838, -1.3302913), + Vector3::new(-0.27619195, -0.06461145, -1.2827141), + Vector3::new(-0.26024473, -0.020184837, -1.2674407), + Vector3::new(-0.26856667, -0.020184835, -1.2637768), + Vector3::new(-0.27619195, -0.026874721, -1.2637768), + Vector3::new(-0.27619195, -0.026874721, -1.2637768), + Vector3::new(-0.26856667, -0.020184835, -1.2637768), + Vector3::new(-0.27619195, -0.020184835, -1.2604196), + Vector3::new(-0.25837165, -0.014966724, -1.2656467), + Vector3::new(-0.23523012, 0.046329774, -1.269994), + Vector3::new(-0.23869139, 0.046329774, -1.2637768), + Vector3::new(-0.2575794, -0.009543579, -1.2637768), + Vector3::new(-0.23523012, 0.046329774, -1.269994), + Vector3::new(-0.25837165, -0.014966724, -1.2656467), + Vector3::new(-0.2563811, -0.020184793, -1.2698835), + Vector3::new(-0.2096774, -0.020184793, -1.324209), + Vector3::new(-0.2096774, 0.046329774, -1.2997168), + Vector3::new(-0.26856652, -0.02018479, -1.2637768), + Vector3::new(-0.2602447, -0.020184793, -1.2674407), + Vector3::new(-0.25837165, -0.014966724, -1.2656467), + Vector3::new(-0.25968674, -0.0123942755, -1.2637768), + Vector3::new(-0.25837165, -0.014966724, -1.2656467), + Vector3::new(-0.2585028, -0.011340788, -1.2637768), + Vector3::new(-0.2596867, -0.012394279, -1.2637768), + Vector3::new(-0.2585028, -0.0113407895, -1.2637768), + Vector3::new(-0.25837165, -0.014966724, -1.2656467), + Vector3::new(-0.2575794, -0.009543577, -1.2637768), + Vector3::new(-0.2575794, -0.009543579, -1.2637768), + Vector3::new(-0.23869139, 0.046329774, -1.2637768), + Vector3::new(-0.2494168, 0.046329767, -1.2445115), + Vector3::new(-0.26856652, -0.02018479, -1.2637768), + Vector3::new(-0.25968674, -0.0123942755, -1.2637768), + Vector3::new(-0.27619195, 0.019892039, -1.2403078), + Vector3::new(-0.27619195, -0.02018479, -1.2604195), + Vector3::new(-0.2585028, -0.011340788, -1.2637768), + Vector3::new(-0.26058903, 0.046329774, -1.2340357), + Vector3::new(-0.27619195, 0.046329767, -1.227099), + Vector3::new(-0.27619195, 0.019892037, -1.2403078), + Vector3::new(-0.2596867, -0.012394279, -1.2637768), + Vector3::new(-0.2494168, 0.046329774, -1.2445115), + Vector3::new(-0.26058903, 0.04632978, -1.2340357), + Vector3::new(-0.2585028, -0.0113407895, -1.2637768), + Vector3::new(-0.2575794, -0.009543577, -1.2637768), + Vector3::new(-0.23523015, 0.0463297, -1.269994), + Vector3::new(-0.21011862, 0.112844266, -1.2747115), + Vector3::new(-0.21620622, 0.112844266, -1.2637768), + Vector3::new(-0.23869143, 0.0463297, -1.2637768), + Vector3::new(-0.2096774, 0.112844266, -1.2752247), + Vector3::new(-0.2101186, 0.112844266, -1.2747115), + Vector3::new(-0.23523015, 0.0463297, -1.269994), + Vector3::new(-0.2096774, 0.046329692, -1.2997168), + Vector3::new(-0.23869143, 0.0463297, -1.2637768), + Vector3::new(-0.21620622, 0.112844266, -1.2637768), + Vector3::new(-0.23969963, 0.11284426, -1.2215773), + Vector3::new(-0.2494168, 0.046329707, -1.2445116), + Vector3::new(-0.26299515, 0.112844266, -1.199734), + Vector3::new(-0.26855454, 0.112844266, -1.1972623), + Vector3::new(-0.27619195, 0.106048025, -1.1972623), + Vector3::new(-0.27619195, 0.046329707, -1.227099), + Vector3::new(-0.26058903, 0.0463297, -1.2340358), + Vector3::new(-0.26299515, 0.11284427, -1.199734), + Vector3::new(-0.26058903, 0.046329692, -1.2340358), + Vector3::new(-0.24941681, 0.0463297, -1.2445116), + Vector3::new(-0.23969965, 0.112844266, -1.2215773), + Vector3::new(-0.2685548, 0.112844266, -1.1972622), + Vector3::new(-0.27619195, 0.11284426, -1.1938667), + Vector3::new(-0.27619195, 0.10604826, -1.1972622), + Vector3::new(-0.2096774, 0.17935875, -1.7319256), + Vector3::new(-0.2096774, 0.17027512, -1.7316719), + Vector3::new(-0.21881983, 0.16804321, -1.7293788), + Vector3::new(-0.220115, 0.17935875, -1.7293788), + Vector3::new(-0.22011548, 0.17935875, -1.7293787), + Vector3::new(-0.2188203, 0.16804309, -1.7293787), + Vector3::new(-0.27619195, 0.15403716, -1.7149887), + Vector3::new(-0.27619195, 0.17935875, -1.715696), + Vector3::new(-0.27619195, 0.11284423, -1.6628642), + Vector3::new(-0.2096774, 0.11284423, -1.6628642), + Vector3::new(-0.2096774, 0.17935872, -1.6628642), + Vector3::new(-0.27619195, 0.17935872, -1.6628642), + Vector3::new(-0.27619195, 0.11284423, -1.5963496), + Vector3::new(-0.2096774, 0.11284423, -1.5963496), + Vector3::new(-0.2096774, 0.17935872, -1.5963496), + Vector3::new(-0.27619195, 0.17935872, -1.5963496), + Vector3::new(-0.27619195, 0.11284423, -1.5963496), + Vector3::new(-0.2096774, 0.11284423, -1.5963496), + Vector3::new(-0.2096774, 0.17935872, -1.5963496), + Vector3::new(-0.27619195, 0.17935872, -1.5963496), + Vector3::new(-0.27619195, 0.11284423, -1.529835), + Vector3::new(-0.2096774, 0.11284423, -1.529835), + Vector3::new(-0.2096774, 0.17935872, -1.529835), + Vector3::new(-0.27619195, 0.17935872, -1.529835), + Vector3::new(-0.27619195, 0.11284423, -1.529835), + Vector3::new(-0.2096774, 0.11284423, -1.529835), + Vector3::new(-0.2096774, 0.17935872, -1.529835), + Vector3::new(-0.27619195, 0.17935872, -1.529835), + Vector3::new(-0.27619195, 0.11284423, -1.4633205), + Vector3::new(-0.2096774, 0.11284423, -1.4633205), + Vector3::new(-0.2096774, 0.17935872, -1.4633205), + Vector3::new(-0.27619195, 0.17935872, -1.4633205), + Vector3::new(-0.27619195, 0.11284423, -1.4633205), + Vector3::new(-0.2096774, 0.11284423, -1.4633205), + Vector3::new(-0.2096774, 0.17935872, -1.4633205), + Vector3::new(-0.27619195, 0.17935872, -1.4633205), + Vector3::new(-0.27619195, 0.11284423, -1.396806), + Vector3::new(-0.2096774, 0.11284423, -1.396806), + Vector3::new(-0.2096774, 0.17935872, -1.396806), + Vector3::new(-0.27619195, 0.17935872, -1.396806), + Vector3::new(-0.27619195, 0.11284423, -1.396806), + Vector3::new(-0.2096774, 0.11284423, -1.396806), + Vector3::new(-0.2096774, 0.17935872, -1.396806), + Vector3::new(-0.27619195, 0.17935872, -1.396806), + Vector3::new(-0.27619195, 0.11284423, -1.3302914), + Vector3::new(-0.2096774, 0.11284423, -1.3302914), + Vector3::new(-0.2096774, 0.17935872, -1.3302914), + Vector3::new(-0.27619195, 0.17935872, -1.3302914), + Vector3::new(-0.21620624, 0.112844184, -1.2637768), + Vector3::new(-0.21011865, 0.112844184, -1.2747115), + Vector3::new(-0.2096774, 0.11401294, -1.2747943), + Vector3::new(-0.2096774, 0.13215737, -1.2637768), + Vector3::new(-0.2096774, 0.112844184, -1.2752247), + Vector3::new(-0.2096774, 0.11401294, -1.2747943), + Vector3::new(-0.21011864, 0.11284419, -1.2747115), + Vector3::new(-0.21620624, 0.112844184, -1.2637768), + Vector3::new(-0.2096774, 0.13215737, -1.2637768), + Vector3::new(-0.2096774, 0.17935875, -1.2351154), + Vector3::new(-0.22998248, 0.17935875, -1.1986428), + Vector3::new(-0.23969965, 0.1128442, -1.2215772), + Vector3::new(-0.26855466, 0.11284419, -1.1972623), + Vector3::new(-0.26299515, 0.112844184, -1.199734), + Vector3::new(-0.26316854, 0.117637016, -1.1972623), + Vector3::new(-0.26316854, 0.11763701, -1.1972623), + Vector3::new(-0.26299515, 0.112844184, -1.199734), + Vector3::new(-0.23969965, 0.112844184, -1.2215772), + Vector3::new(-0.22998248, 0.17935875, -1.1986428), + Vector3::new(-0.23145483, 0.17935875, -1.1972623), + Vector3::new(-0.2654013, 0.17935875, -1.1654321), + Vector3::new(-0.27619195, 0.17935875, -1.1606346), + Vector3::new(-0.27619195, 0.1128442, -1.1938668), + Vector3::new(-0.26855493, 0.11284419, -1.1972622), + Vector3::new(-0.26316854, 0.11763725, -1.1972622), + Vector3::new(-0.26316854, 0.11763725, -1.1972622), + Vector3::new(-0.23145495, 0.17935875, -1.1972622), + Vector3::new(-0.2654013, 0.17935875, -1.1654321), + Vector3::new(-0.1482815, -0.15321395, -1.8022337), + Vector3::new(-0.15295385, -0.2197285, -1.8241994), + Vector3::new(-0.20432228, -0.2197285, -1.8203418), + Vector3::new(-0.20967737, -0.18912855, -1.809673), + Vector3::new(-0.20967737, -0.15321395, -1.797623), + Vector3::new(-0.20967737, -0.2197285, -1.8172807), + Vector3::new(-0.20967737, -0.18912852, -1.809673), + Vector3::new(-0.20432226, -0.2197285, -1.8203418), + Vector3::new(-0.14316282, -0.15321395, -1.8006383), + Vector3::new(-0.14316282, -0.2197285, -1.8211478), + Vector3::new(-0.15295385, -0.21972848, -1.8241994), + Vector3::new(-0.1482815, -0.15321395, -1.8022337), + Vector3::new(-0.2096774, -0.21972859, -1.7958933), + Vector3::new(-0.14316282, -0.21972859, -1.7958933), + Vector3::new(-0.14316282, -0.15321398, -1.7958933), + Vector3::new(-0.2096774, -0.15321398, -1.7958933), + Vector3::new(-0.2096774, -0.21972859, -1.7293787), + Vector3::new(-0.14316282, -0.21972859, -1.7293787), + Vector3::new(-0.14316282, -0.15321398, -1.7293787), + Vector3::new(-0.2096774, -0.15321398, -1.7293787), + Vector3::new(-0.2096774, -0.21972859, -1.7293787), + Vector3::new(-0.14316282, -0.21972859, -1.7293787), + Vector3::new(-0.14316282, -0.15321398, -1.7293787), + Vector3::new(-0.2096774, -0.15321398, -1.7293787), + Vector3::new(-0.2096774, -0.21972859, -1.6628642), + Vector3::new(-0.14316282, -0.21972859, -1.6628642), + Vector3::new(-0.14316282, -0.15321398, -1.6628642), + Vector3::new(-0.2096774, -0.15321398, -1.6628642), + Vector3::new(-0.2096774, -0.21972859, -1.6628642), + Vector3::new(-0.14316282, -0.21972859, -1.6628642), + Vector3::new(-0.14316282, -0.15321398, -1.6628642), + Vector3::new(-0.2096774, -0.15321398, -1.6628642), + Vector3::new(-0.2096774, -0.21972859, -1.5963496), + Vector3::new(-0.14316282, -0.21972859, -1.5963496), + Vector3::new(-0.14316282, -0.15321398, -1.5963496), + Vector3::new(-0.2096774, -0.15321398, -1.5963496), + Vector3::new(-0.2096774, -0.21972859, -1.5963496), + Vector3::new(-0.14316282, -0.21972859, -1.5963496), + Vector3::new(-0.14316282, -0.15321398, -1.5963496), + Vector3::new(-0.2096774, -0.15321398, -1.5963496), + Vector3::new(-0.2096774, -0.21972859, -1.529835), + Vector3::new(-0.14316282, -0.21972859, -1.529835), + Vector3::new(-0.14316282, -0.15321398, -1.529835), + Vector3::new(-0.2096774, -0.15321398, -1.529835), + Vector3::new(-0.18130249, -0.2197285, -1.4633205), + Vector3::new(-0.14316282, -0.2197285, -1.5244126), + Vector3::new(-0.14316282, -0.15321395, -1.4828252), + Vector3::new(-0.15533954, -0.15321395, -1.4633205), + Vector3::new(-0.20222148, -0.16693833, -1.396806), + Vector3::new(-0.18590543, -0.2197285, -1.4559475), + Vector3::new(-0.18130249, -0.2197285, -1.4633205), + Vector3::new(-0.15533954, -0.15321395, -1.4633205), + Vector3::new(-0.19686438, -0.15321395, -1.396806), + Vector3::new(-0.20648265, -0.17954761, -1.396806), + Vector3::new(-0.20273766, -0.2197285, -1.4268022), + Vector3::new(-0.18590543, -0.2197285, -1.4559475), + Vector3::new(-0.20222148, -0.16693833, -1.396806), + Vector3::new(-0.20967737, -0.18086074, -1.396806), + Vector3::new(-0.20967737, -0.2197285, -1.4247514), + Vector3::new(-0.20273766, -0.2197285, -1.426802), + Vector3::new(-0.20648263, -0.17954776, -1.396806), + Vector3::new(-0.20222151, -0.16693823, -1.3968059), + Vector3::new(-0.19686446, -0.15321395, -1.3968059), + Vector3::new(-0.20646334, -0.15321395, -1.3814304), + Vector3::new(-0.20646334, -0.15321395, -1.3814304), + Vector3::new(-0.20893703, -0.15321395, -1.3771472), + Vector3::new(-0.20648266, -0.17954744, -1.3968059), + Vector3::new(-0.20222151, -0.16693823, -1.3968059), + Vector3::new(-0.20893703, -0.15321395, -1.3771471), + Vector3::new(-0.20967737, -0.15321395, -1.3769283), + Vector3::new(-0.20967737, -0.18086058, -1.3968059), + Vector3::new(-0.20648265, -0.17954761, -1.3968059), + Vector3::new(-0.14693287, -0.13401513, -1.7958934), + Vector3::new(-0.1482815, -0.15321401, -1.8022337), + Vector3::new(-0.20967737, -0.15321401, -1.797623), + Vector3::new(-0.20967737, -0.1480589, -1.7958934), + Vector3::new(-0.14316282, -0.13782589, -1.7958934), + Vector3::new(-0.14316282, -0.15321401, -1.8006383), + Vector3::new(-0.1482815, -0.15321401, -1.8022337), + Vector3::new(-0.14693287, -0.13401513, -1.7958934), + Vector3::new(-0.20967737, -0.08669945, -1.7753065), + Vector3::new(-0.14360917, -0.08669945, -1.7802678), + Vector3::new(-0.14693284, -0.13401476, -1.7958933), + Vector3::new(-0.20967737, -0.14805856, -1.7958933), + Vector3::new(-0.14316282, -0.1378255, -1.7958933), + Vector3::new(-0.14693284, -0.13401477, -1.7958933), + Vector3::new(-0.14360917, -0.08669945, -1.7802678), + Vector3::new(-0.14316282, -0.08669945, -1.7801287), + Vector3::new(-0.1553396, -0.15321401, -1.4633205), + Vector3::new(-0.14316282, -0.15321401, -1.4828253), + Vector3::new(-0.14316282, -0.122018315, -1.4633205), + Vector3::new(-0.14316282, -0.093953855, -1.4457735), + Vector3::new(-0.14316282, -0.08669945, -1.4429029), + Vector3::new(-0.17522681, -0.086699456, -1.396806), + Vector3::new(-0.18557088, -0.124281116, -1.396806), + Vector3::new(-0.19686438, -0.15321401, -1.396806), + Vector3::new(-0.1553396, -0.15321401, -1.4633205), + Vector3::new(-0.14316282, -0.122018315, -1.4633205), + Vector3::new(-0.14316282, -0.093953855, -1.4457735), + Vector3::new(-0.18557088, -0.124281116, -1.396806), + Vector3::new(-0.18557099, -0.12428119, -1.3968059), + Vector3::new(-0.1752269, -0.086699456, -1.3968059), + Vector3::new(-0.20449243, -0.086699456, -1.3547322), + Vector3::new(-0.20967737, -0.13848189, -1.3677685), + Vector3::new(-0.20967737, -0.14152038, -1.3689709), + Vector3::new(-0.20967737, -0.14281502, -1.3697803), + Vector3::new(-0.20646332, -0.15321401, -1.3814304), + Vector3::new(-0.19686446, -0.15321401, -1.3968059), + Vector3::new(-0.18557099, -0.12428119, -1.3968059), + Vector3::new(-0.20967737, -0.14152038, -1.3689709), + Vector3::new(-0.20449243, -0.08669945, -1.3547322), + Vector3::new(-0.20967737, -0.086699456, -1.3487011), + Vector3::new(-0.20967737, -0.1384819, -1.3677685), + Vector3::new(-0.20967737, -0.14281502, -1.3697803), + Vector3::new(-0.20967737, -0.14527076, -1.3712174), + Vector3::new(-0.20893703, -0.15321401, -1.3771472), + Vector3::new(-0.2064633, -0.15321401, -1.3814304), + Vector3::new(-0.20893703, -0.15321404, -1.3771471), + Vector3::new(-0.20967737, -0.14527076, -1.3712173), + Vector3::new(-0.20967737, -0.15321401, -1.3769283), + Vector3::new(-0.14316282, -0.042856656, -1.7655915), + Vector3::new(-0.14316282, -0.08034536, -1.7781695), + Vector3::new(-0.14360917, -0.086699404, -1.7802678), + Vector3::new(-0.20967737, -0.086699404, -1.7753065), + Vector3::new(-0.20967737, -0.039554533, -1.7594886), + Vector3::new(-0.14316282, -0.08034536, -1.7781695), + Vector3::new(-0.14316282, -0.086699404, -1.7801287), + Vector3::new(-0.14360917, -0.0866994, -1.7802678), + Vector3::new(-0.1353515, 0.16140196, -1.7255667), + Vector3::new(-0.17522681, -0.0866994, -1.396806), + Vector3::new(-0.14316282, -0.086699404, -1.4429029), + Vector3::new(-0.14316282, -0.032543883, -1.4214734), + Vector3::new(-0.14483222, -0.020184837, -1.4141828), + Vector3::new(-0.15691914, -0.02018484, -1.396806), + Vector3::new(-0.14316282, -0.032543883, -1.4214734), + Vector3::new(-0.14316282, -0.020184837, -1.4174645), + Vector3::new(-0.14483222, -0.020184845, -1.4141828), + Vector3::new(-0.1752269, -0.0866994, -1.3968059), + Vector3::new(-0.15691921, -0.02018484, -1.3968059), + Vector3::new(-0.19783239, -0.020184845, -1.337987), + Vector3::new(-0.20449243, -0.0866994, -1.3547322), + Vector3::new(-0.20967737, -0.03670317, -1.3302914), + Vector3::new(-0.20967737, -0.0866994, -1.3487011), + Vector3::new(-0.20449243, -0.08669941, -1.3547322), + Vector3::new(-0.19783239, -0.020184837, -1.337987), + Vector3::new(-0.2044483, -0.020184841, -1.3302914), + Vector3::new(-0.20967737, -0.03670284, -1.3302913), + Vector3::new(-0.20444839, -0.020184841, -1.3302913), + Vector3::new(-0.20967737, -0.020184845, -1.324209), + Vector3::new(-0.15691915, -0.02018479, -1.396806), + Vector3::new(-0.14483224, -0.020184793, -1.4141828), + Vector3::new(-0.14881122, 0.009272579, -1.396806), + Vector3::new(-0.14881122, 0.009272579, -1.396806), + Vector3::new(-0.14483224, -0.020184785, -1.4141828), + Vector3::new(-0.14316282, -0.020184793, -1.4174645), + Vector3::new(-0.14316282, 0.043503255, -1.396806), + Vector3::new(-0.19477163, 0.010383148, -1.3302914), + Vector3::new(-0.19783238, -0.020184785, -1.337987), + Vector3::new(-0.15691923, -0.02018479, -1.3968059), + Vector3::new(-0.14881125, 0.00927278, -1.3968059), + Vector3::new(-0.15381677, 0.046329774, -1.3749461), + Vector3::new(-0.1848776, 0.046329767, -1.3302914), + Vector3::new(-0.14316282, 0.046329774, -1.3958892), + Vector3::new(-0.15381677, 0.046329767, -1.3749461), + Vector3::new(-0.14881125, 0.00927278, -1.3968059), + Vector3::new(-0.14316282, 0.04350362, -1.3968059), + Vector3::new(-0.20444828, -0.020184789, -1.3302914), + Vector3::new(-0.19783238, -0.020184793, -1.337987), + Vector3::new(-0.19477163, 0.010383146, -1.3302914), + Vector3::new(-0.19477159, 0.010383621, -1.3302913), + Vector3::new(-0.18487768, 0.046329767, -1.3302913), + Vector3::new(-0.19117233, 0.046329767, -1.3212417), + Vector3::new(-0.20967737, 0.046329774, -1.2997168), + Vector3::new(-0.20967737, -0.020184785, -1.324209), + Vector3::new(-0.20444839, -0.020184789, -1.3302913), + Vector3::new(-0.19477159, 0.010383619, -1.3302913), + Vector3::new(-0.19117233, 0.046329774, -1.3212417), + Vector3::new(-0.1848776, 0.046329707, -1.3302914), + Vector3::new(-0.15381676, 0.0463297, -1.3749461), + Vector3::new(-0.1628013, 0.112844266, -1.3357095), + Vector3::new(-0.16656998, 0.112844266, -1.3302914), + Vector3::new(-0.14334899, 0.112844266, -1.373948), + Vector3::new(-0.1628013, 0.112844266, -1.3357095), + Vector3::new(-0.15381676, 0.046329707, -1.3749461), + Vector3::new(-0.14316282, 0.0463297, -1.3958893), + Vector3::new(-0.14316282, 0.11230591, -1.3744886), + Vector3::new(-0.14316282, 0.112844266, -1.3743398), + Vector3::new(-0.14334898, 0.112844266, -1.373948), + Vector3::new(-0.14316282, 0.11230593, -1.3744886), + Vector3::new(-0.1845123, 0.112844266, -1.3044965), + Vector3::new(-0.19117235, 0.046329707, -1.3212417), + Vector3::new(-0.1848777, 0.046329707, -1.3302913), + Vector3::new(-0.16657007, 0.112844266, -1.3302913), + Vector3::new(-0.20967737, 0.112844266, -1.2752247), + Vector3::new(-0.20967737, 0.0463297, -1.2997168), + Vector3::new(-0.19117235, 0.046329692, -1.3212417), + Vector3::new(-0.1845123, 0.112844266, -1.3044965), + Vector3::new(-0.2045752, 0.17152071, -1.7329516), + Vector3::new(-0.20536505, 0.17935875, -1.7329779), + Vector3::new(-0.2029104, 0.17935875, -1.7331995), + Vector3::new(-0.2045752, 0.17152071, -1.7329516), + Vector3::new(-0.20291038, 0.17935875, -1.7331995), + Vector3::new(-0.16431268, 0.17935875, -1.7293788), + Vector3::new(-0.17108496, 0.16662528, -1.7293788), + Vector3::new(-0.20967737, 0.17935875, -1.7319256), + Vector3::new(-0.20536505, 0.17935875, -1.7329779), + Vector3::new(-0.2045752, 0.17152071, -1.7329516), + Vector3::new(-0.20967737, 0.17027514, -1.7316719), + Vector3::new(-0.17108384, 0.16662511, -1.7293787), + Vector3::new(-0.16431147, 0.17935875, -1.7293787), + Vector3::new(-0.14316282, 0.17935875, -1.7272853), + Vector3::new(-0.14316282, 0.16254377, -1.7264), + Vector3::new(-0.2096774, 0.11284423, -1.6628642), + Vector3::new(-0.14316282, 0.11284423, -1.6628642), + Vector3::new(-0.14316282, 0.17935872, -1.6628642), + Vector3::new(-0.2096774, 0.17935872, -1.6628642), + Vector3::new(-0.2096774, 0.11284423, -1.5963496), + Vector3::new(-0.14316282, 0.11284423, -1.5963496), + Vector3::new(-0.14316282, 0.17935872, -1.5963496), + Vector3::new(-0.2096774, 0.17935872, -1.5963496), + Vector3::new(-0.2096774, 0.11284423, -1.5963496), + Vector3::new(-0.14316282, 0.11284423, -1.5963496), + Vector3::new(-0.14316282, 0.17935872, -1.5963496), + Vector3::new(-0.2096774, 0.17935872, -1.5963496), + Vector3::new(-0.2096774, 0.11284423, -1.529835), + Vector3::new(-0.14316282, 0.11284423, -1.529835), + Vector3::new(-0.14316282, 0.17935872, -1.529835), + Vector3::new(-0.2096774, 0.17935872, -1.529835), + Vector3::new(-0.2096774, 0.11284423, -1.529835), + Vector3::new(-0.14316282, 0.11284423, -1.529835), + Vector3::new(-0.14316282, 0.17935872, -1.529835), + Vector3::new(-0.2096774, 0.17935872, -1.529835), + Vector3::new(-0.2096774, 0.11284423, -1.4633205), + Vector3::new(-0.14316282, 0.11284423, -1.4633205), + Vector3::new(-0.14316282, 0.17935872, -1.4633205), + Vector3::new(-0.2096774, 0.17935872, -1.4633205), + Vector3::new(-0.2096774, 0.11284423, -1.4633205), + Vector3::new(-0.14316282, 0.11284423, -1.4633205), + Vector3::new(-0.14316282, 0.17935872, -1.4633205), + Vector3::new(-0.2096774, 0.17935872, -1.4633205), + Vector3::new(-0.2096774, 0.11284423, -1.396806), + Vector3::new(-0.14316282, 0.11284423, -1.396806), + Vector3::new(-0.14316282, 0.17935872, -1.396806), + Vector3::new(-0.2096774, 0.17935872, -1.396806), + Vector3::new(-0.16656998, 0.112844184, -1.3302914), + Vector3::new(-0.1628013, 0.112844184, -1.3357095), + Vector3::new(-0.16404194, 0.12202896, -1.3302914), + Vector3::new(-0.16404194, 0.12202897, -1.3302914), + Vector3::new(-0.1628013, 0.11284419, -1.3357095), + Vector3::new(-0.14334896, 0.11284419, -1.3739481), + Vector3::new(-0.15838355, 0.15632033, -1.3302914), + Vector3::new(-0.15838355, 0.15632033, -1.3302914), + Vector3::new(-0.14334896, 0.11284419, -1.3739481), + Vector3::new(-0.14316282, 0.11284419, -1.3743399), + Vector3::new(-0.14316282, 0.17935875, -1.3559638), + Vector3::new(-0.15535964, 0.17935875, -1.3302914), + Vector3::new(-0.17785226, 0.17935875, -1.2877513), + Vector3::new(-0.1845123, 0.11284419, -1.3044965), + Vector3::new(-0.16657007, 0.112844184, -1.3302913), + Vector3::new(-0.16404197, 0.12202916, -1.3302913), + Vector3::new(-0.17178583, 0.17935875, -1.2964728), + Vector3::new(-0.16635051, 0.17935875, -1.3071573), + Vector3::new(-0.17178583, 0.17935875, -1.2964728), + Vector3::new(-0.16404197, 0.12202917, -1.3302913), + Vector3::new(-0.15838358, 0.15632045, -1.3302913), + Vector3::new(-0.15838358, 0.15632045, -1.3302913), + Vector3::new(-0.15535969, 0.17935875, -1.3302913), + Vector3::new(-0.16635051, 0.17935875, -1.3071573), + Vector3::new(-0.18500711, 0.17935875, -1.2794288), + Vector3::new(-0.19372095, 0.17935875, -1.2637768), + Vector3::new(-0.20967737, 0.13215743, -1.2637768), + Vector3::new(-0.20967737, 0.11401303, -1.2747943), + Vector3::new(-0.20967737, 0.114013016, -1.2747943), + Vector3::new(-0.20967737, 0.11284419, -1.2752247), + Vector3::new(-0.1845123, 0.112844184, -1.3044965), + Vector3::new(-0.17785226, 0.17935875, -1.2877513), + Vector3::new(-0.18500711, 0.17935875, -1.2794288), + Vector3::new(-0.19372095, 0.17935875, -1.2637768), + Vector3::new(-0.20967737, 0.17935875, -1.2351154), + Vector3::new(-0.20967737, 0.13215743, -1.2637768), + Vector3::new(-0.13414398, -0.15321395, -1.7978274), + Vector3::new(-0.1302853, -0.2197285, -1.8171343), + Vector3::new(-0.14316282, -0.2197285, -1.8211478), + Vector3::new(-0.14316282, -0.15321395, -1.8006383), + Vector3::new(-0.10625099, -0.2197285, -1.7988122), + Vector3::new(-0.104408056, -0.21520284, -1.7958934), + Vector3::new(-0.10402099, -0.2197285, -1.7958934), + Vector3::new(-0.11945202, -0.2197285, -1.8134716), + Vector3::new(-0.112069346, -0.17105782, -1.7958934), + Vector3::new(-0.104408056, -0.21520284, -1.7958934), + Vector3::new(-0.10625099, -0.2197285, -1.7988122), + Vector3::new(-0.1302853, -0.2197285, -1.8171343), + Vector3::new(-0.13414398, -0.15321395, -1.7978274), + Vector3::new(-0.12842394, -0.15321395, -1.7958934), + Vector3::new(-0.11206935, -0.17105782, -1.7958934), + Vector3::new(-0.11945203, -0.2197285, -1.8134716), + Vector3::new(-0.077586554, -0.2197285, -1.7484213), + Vector3::new(-0.07664826, -0.2152715, -1.7458801), + Vector3::new(-0.07664826, -0.2197285, -1.7424797), + Vector3::new(-0.07664826, -0.21527147, -1.7458801), + Vector3::new(-0.07758656, -0.2197285, -1.7484213), + Vector3::new(-0.078665845, -0.2197285, -1.7627075), + Vector3::new(-0.07664826, -0.20939292, -1.7589098), + Vector3::new(-0.10440798, -0.21520264, -1.7958933), + Vector3::new(-0.07916506, -0.15321395, -1.7559152), + Vector3::new(-0.07664826, -0.15321395, -1.7526212), + Vector3::new(-0.07664826, -0.20939294, -1.7589098), + Vector3::new(-0.078665845, -0.2197285, -1.7627075), + Vector3::new(-0.1040209, -0.2197285, -1.7958933), + Vector3::new(-0.112069294, -0.17105749, -1.7958933), + Vector3::new(-0.10936268, -0.15321395, -1.7894489), + Vector3::new(-0.07916506, -0.15321395, -1.7559152), + Vector3::new(-0.10440798, -0.21520266, -1.7958933), + Vector3::new(-0.1120693, -0.17105749, -1.7958933), + Vector3::new(-0.12842359, -0.15321395, -1.7958933), + Vector3::new(-0.109362684, -0.15321395, -1.7894489), + Vector3::new(-0.08346283, -0.2197285, -1.6628642), + Vector3::new(-0.07664826, -0.2197285, -1.685307), + Vector3::new(-0.07664826, -0.19288324, -1.6628642), + Vector3::new(-0.07664826, -0.15321395, -1.6297004), + Vector3::new(-0.08166675, -0.15321395, -1.6131728), + Vector3::new(-0.08588588, -0.2197285, -1.6548842), + Vector3::new(-0.08346283, -0.2197285, -1.6628642), + Vector3::new(-0.07664826, -0.19288324, -1.6628642), + Vector3::new(-0.099511996, -0.2197285, -1.6080378), + Vector3::new(-0.09313648, -0.17996536, -1.5963497), + Vector3::new(-0.105319, -0.2197285, -1.5963497), + Vector3::new(-0.09313648, -0.17996536, -1.5963497), + Vector3::new(-0.099511996, -0.2197285, -1.6080378), + Vector3::new(-0.08588588, -0.21972848, -1.6548842), + Vector3::new(-0.08166675, -0.15321395, -1.6131728), + Vector3::new(-0.086560026, -0.15321395, -1.5963497), + Vector3::new(-0.13977757, -0.2197285, -1.5298351), + Vector3::new(-0.13286145, -0.2197285, -1.5409133), + Vector3::new(-0.13321237, -0.2029091, -1.5298351), + Vector3::new(-0.093136415, -0.17996496, -1.5963496), + Vector3::new(-0.08884723, -0.15321395, -1.5884863), + Vector3::new(-0.1179869, -0.15321395, -1.5298351), + Vector3::new(-0.13321237, -0.2029091, -1.5298351), + Vector3::new(-0.13286145, -0.2197285, -1.5409133), + Vector3::new(-0.10531906, -0.2197285, -1.5963496), + Vector3::new(-0.093136415, -0.17996496, -1.5963496), + Vector3::new(-0.086560056, -0.15321395, -1.5963496), + Vector3::new(-0.08884723, -0.15321395, -1.5884863), + Vector3::new(-0.14316282, -0.15321395, -1.4828252), + Vector3::new(-0.14316282, -0.2197285, -1.5244126), + Vector3::new(-0.13977765, -0.2197285, -1.529835), + Vector3::new(-0.13321237, -0.20290892, -1.529835), + Vector3::new(-0.13424921, -0.15321395, -1.497103), + Vector3::new(-0.13321237, -0.20290892, -1.529835), + Vector3::new(-0.11798696, -0.15321395, -1.529835), + Vector3::new(-0.13424921, -0.15321395, -1.497103), + Vector3::new(-0.13453051, -0.14655097, -1.7958934), + Vector3::new(-0.13414398, -0.15321401, -1.7978275), + Vector3::new(-0.14316282, -0.15321401, -1.8006383), + Vector3::new(-0.14316282, -0.13782589, -1.7958934), + Vector3::new(-0.13414398, -0.15321401, -1.7978275), + Vector3::new(-0.13453051, -0.14655097, -1.7958934), + Vector3::new(-0.12842368, -0.15321401, -1.7958934), + Vector3::new(-0.13453054, -0.14655057, -1.7958933), + Vector3::new(-0.14316282, -0.1378255, -1.7958933), + Vector3::new(-0.14316282, -0.08669945, -1.7801287), + Vector3::new(-0.13800268, -0.08669945, -1.7785206), + Vector3::new(-0.07916508, -0.15321401, -1.7559153), + Vector3::new(-0.07664826, -0.14703348, -1.7519293), + Vector3::new(-0.07664826, -0.15321401, -1.7526212), + Vector3::new(-0.10936269, -0.15321401, -1.7894489), + Vector3::new(-0.09927335, -0.08669945, -1.7654262), + Vector3::new(-0.07664826, -0.086699456, -1.7403016), + Vector3::new(-0.07664826, -0.14703348, -1.7519293), + Vector3::new(-0.07916508, -0.15321401, -1.7559153), + Vector3::new(-0.13453054, -0.14655057, -1.7958933), + Vector3::new(-0.13800268, -0.08669945, -1.7785206), + Vector3::new(-0.09927335, -0.08669945, -1.7654262), + Vector3::new(-0.10936269, -0.15321401, -1.7894489), + Vector3::new(-0.12842332, -0.15321401, -1.7958933), + Vector3::new(-0.0799651, -0.12638737, -1.5963497), + Vector3::new(-0.08166676, -0.15321401, -1.6131728), + Vector3::new(-0.07664826, -0.15321401, -1.6297005), + Vector3::new(-0.07664826, -0.11332099, -1.5963497), + Vector3::new(-0.086560056, -0.15321401, -1.5963497), + Vector3::new(-0.08166676, -0.15321401, -1.6131728), + Vector3::new(-0.0799651, -0.12638737, -1.5963497), + Vector3::new(-0.07664826, -0.08669945, -1.5740939), + Vector3::new(-0.07744763, -0.08669945, -1.5714613), + Vector3::new(-0.07996509, -0.12638718, -1.5963496), + Vector3::new(-0.07664826, -0.11332084, -1.5963496), + Vector3::new(-0.088847235, -0.15321401, -1.5884864), + Vector3::new(-0.07818247, -0.08669945, -1.5689349), + Vector3::new(-0.09760841, -0.08669945, -1.5298351), + Vector3::new(-0.11798693, -0.15321401, -1.5298351), + Vector3::new(-0.07818247, -0.08669945, -1.5689349), + Vector3::new(-0.088847235, -0.15321401, -1.5884864), + Vector3::new(-0.086560085, -0.15321401, -1.5963496), + Vector3::new(-0.07996509, -0.12638718, -1.5963496), + Vector3::new(-0.07744763, -0.08669945, -1.5714613), + Vector3::new(-0.14316282, -0.122018404, -1.4633205), + Vector3::new(-0.14316282, -0.15321401, -1.4828252), + Vector3::new(-0.13424921, -0.15321401, -1.497103), + Vector3::new(-0.13531932, -0.101924114, -1.4633205), + Vector3::new(-0.11798699, -0.15321401, -1.529835), + Vector3::new(-0.09760847, -0.08669945, -1.529835), + Vector3::new(-0.12412503, -0.08669945, -1.4764634), + Vector3::new(-0.1309932, -0.08780385, -1.4633205), + Vector3::new(-0.13531932, -0.101924114, -1.4633205), + Vector3::new(-0.13424921, -0.15321401, -1.497103), + Vector3::new(-0.1309932, -0.08780385, -1.4633205), + Vector3::new(-0.12412503, -0.08669945, -1.4764634), + Vector3::new(-0.13081099, -0.08669945, -1.4633205), + Vector3::new(-0.13584769, -0.08669945, -1.4534196), + Vector3::new(-0.14316282, -0.08669945, -1.4429029), + Vector3::new(-0.14316282, -0.093953855, -1.4457735), + Vector3::new(-0.13559848, -0.088544376, -1.4545078), + Vector3::new(-0.14316282, -0.122018404, -1.4633205), + Vector3::new(-0.13531932, -0.101924114, -1.4633205), + Vector3::new(-0.13559848, -0.088544376, -1.4545078), + Vector3::new(-0.14316282, -0.093953855, -1.4457735), + Vector3::new(-0.13531932, -0.101924114, -1.4633205), + Vector3::new(-0.1309932, -0.08780385, -1.4633205), + Vector3::new(-0.13559848, -0.088544376, -1.4545078), + Vector3::new(-0.1309932, -0.08780385, -1.4633205), + Vector3::new(-0.13081099, -0.08669945, -1.4633205), + Vector3::new(-0.13584769, -0.08669945, -1.4534196), + Vector3::new(-0.13559848, -0.088544376, -1.4545078), + Vector3::new(-0.14316282, -0.08034536, -1.7781695), + Vector3::new(-0.14316282, -0.042856656, -1.7655915), + Vector3::new(-0.14053856, -0.042986937, -1.7658323), + Vector3::new(-0.14053856, -0.042986937, -1.7658323), + Vector3::new(-0.13800268, -0.086699404, -1.7785206), + Vector3::new(-0.14316282, -0.0866994, -1.7801287), + Vector3::new(-0.14316282, -0.08034536, -1.7781695), + Vector3::new(-0.08918399, -0.020184837, -1.7414033), + Vector3::new(-0.07835567, -0.020184832, -1.7293788), + Vector3::new(-0.07664826, -0.030022964, -1.7293788), + Vector3::new(-0.07664826, -0.0866994, -1.7403016), + Vector3::new(-0.09927334, -0.0866994, -1.765426), + Vector3::new(-0.14053856, -0.042986937, -1.7658323), + Vector3::new(-0.13078228, -0.020184837, -1.7554678), + Vector3::new(-0.08918399, -0.020184837, -1.7414033), + Vector3::new(-0.09927334, -0.086699404, -1.765426), + Vector3::new(-0.13800268, -0.0866994, -1.7785206), + Vector3::new(-0.06633711, 0.13043445, -1.6870049), + Vector3::new(-0.07835556, -0.020184832, -1.7293787), + Vector3::new(-0.07664826, -0.02018483, -1.7274828), + Vector3::new(-0.07664826, -0.030022345, -1.7293787), + Vector3::new(-0.07664826, -0.07897442, -1.5676358), + Vector3::new(-0.07696662, -0.07911639, -1.566706), + Vector3::new(-0.07744762, -0.086699404, -1.5714613), + Vector3::new(-0.07664826, -0.0866994, -1.5740939), + Vector3::new(-0.07818246, -0.0866994, -1.5689349), + Vector3::new(-0.07696662, -0.07911639, -1.566706), + Vector3::new(-0.09623438, -0.08221464, -1.5298351), + Vector3::new(-0.09760841, -0.0866994, -1.5298351), + Vector3::new(-0.07696662, -0.07911639, -1.566706), + Vector3::new(-0.07818246, -0.086699404, -1.5689349), + Vector3::new(-0.07744762, -0.0866994, -1.5714613), + Vector3::new(-0.07696662, -0.07911639, -1.566706), + Vector3::new(-0.08966427, -0.04239806, -1.5298351), + Vector3::new(-0.09623438, -0.08221464, -1.5298351), + Vector3::new(-0.07664826, -0.076576024, -1.5666742), + Vector3::new(-0.07664826, -0.020184841, -1.5510949), + Vector3::new(-0.08674866, -0.02018484, -1.5298351), + Vector3::new(-0.089664266, -0.042398065, -1.5298351), + Vector3::new(-0.07696662, -0.07911639, -1.566706), + Vector3::new(-0.07664826, -0.07897442, -1.5676358), + Vector3::new(-0.07664826, -0.076576024, -1.5666742), + Vector3::new(-0.07696662, -0.07911639, -1.566706), + Vector3::new(-0.09760847, -0.0866994, -1.529835), + Vector3::new(-0.09623445, -0.08221465, -1.529835), + Vector3::new(-0.12412475, -0.086699404, -1.476464), + Vector3::new(-0.08966432, -0.042397942, -1.529835), + Vector3::new(-0.09734588, -0.020184837, -1.5075296), + Vector3::new(-0.11983546, -0.020184841, -1.4633205), + Vector3::new(-0.13081096, -0.086699404, -1.4633205), + Vector3::new(-0.12412475, -0.086699404, -1.476464), + Vector3::new(-0.09623445, -0.08221465, -1.529835), + Vector3::new(-0.08966431, -0.042397946, -1.529835), + Vector3::new(-0.08674872, -0.02018484, -1.529835), + Vector3::new(-0.09734587, -0.020184837, -1.5075296), + Vector3::new(-0.13584769, -0.086699404, -1.4534194), + Vector3::new(-0.14316282, -0.032543883, -1.4214734), + Vector3::new(-0.14316282, -0.086699404, -1.4429029), + Vector3::new(-0.13081096, -0.086699404, -1.4633205), + Vector3::new(-0.11983546, -0.020184841, -1.4633205), + Vector3::new(-0.14316282, -0.020184845, -1.4174645), + Vector3::new(-0.14316282, -0.032543883, -1.4214734), + Vector3::new(-0.13584769, -0.086699404, -1.4534194), + Vector3::new(-0.07835566, -0.02018479, -1.7293788), + Vector3::new(-0.089183986, -0.020184785, -1.7414033), + Vector3::new(-0.084133804, 0.013108801, -1.7293788), + Vector3::new(-0.13078225, -0.020184793, -1.7554678), + Vector3::new(-0.106224254, 0.037211422, -1.7293788), + Vector3::new(-0.0841338, 0.013108797, -1.7293788), + Vector3::new(-0.08918398, -0.020184793, -1.7414033), + Vector3::new(-0.07909463, 0.046329774, -1.7173805), + Vector3::new(-0.07664826, 0.046329774, -1.714664), + Vector3::new(-0.07664826, -0.020184793, -1.7274828), + Vector3::new(-0.07835556, -0.02018479, -1.7293787), + Vector3::new(-0.08413375, 0.013109133, -1.7293787), + Vector3::new(-0.10622414, 0.037211683, -1.7293787), + Vector3::new(-0.1023228, 0.046329774, -1.7252342), + Vector3::new(-0.07909463, 0.046329774, -1.7173805), + Vector3::new(-0.08413375, 0.013109129, -1.7293787), + Vector3::new(-0.07664826, -0.020184796, -1.5510949), + Vector3::new(-0.07664826, 0.04632977, -1.5327188), + Vector3::new(-0.07801828, 0.04632977, -1.5298351), + Vector3::new(-0.08674867, -0.020184794, -1.5298351), + Vector3::new(-0.119835466, -0.020184789, -1.4633205), + Vector3::new(-0.097345896, -0.020184793, -1.5075296), + Vector3::new(-0.112570725, 0.023841474, -1.4633205), + Vector3::new(-0.08674873, -0.020184794, -1.529835), + Vector3::new(-0.07801833, 0.04632977, -1.529835), + Vector3::new(-0.10961901, 0.046329774, -1.4633205), + Vector3::new(-0.11257072, 0.023841478, -1.4633205), + Vector3::new(-0.09734589, -0.020184793, -1.5075296), + Vector3::new(-0.120347455, 0.046329774, -1.4407388), + Vector3::new(-0.14269641, 0.046329774, -1.396806), + Vector3::new(-0.14316282, 0.043503255, -1.396806), + Vector3::new(-0.14316282, -0.020184785, -1.4174645), + Vector3::new(-0.119835466, -0.020184789, -1.4633205), + Vector3::new(-0.112570725, 0.023841474, -1.4633205), + Vector3::new(-0.11257072, 0.023841478, -1.4633205), + Vector3::new(-0.10961901, 0.046329774, -1.4633205), + Vector3::new(-0.12034745, 0.046329774, -1.4407388), + Vector3::new(-0.14269647, 0.046329774, -1.3968059), + Vector3::new(-0.14316282, 0.046329774, -1.3958892), + Vector3::new(-0.14316282, 0.04350362, -1.3968059), + Vector3::new(-0.07909465, 0.046329707, -1.7173805), + Vector3::new(-0.07664826, 0.06245765, -1.7115557), + Vector3::new(-0.07664826, 0.0463297, -1.714664), + Vector3::new(-0.07909465, 0.0463297, -1.7173805), + Vector3::new(-0.10232283, 0.0463297, -1.7252342), + Vector3::new(-0.07664826, 0.10633554, -1.697959), + Vector3::new(-0.07664826, 0.062457643, -1.7115557), + Vector3::new(-0.07664826, 0.0463297, -1.5327188), + Vector3::new(-0.07664826, 0.0567675, -1.5298351), + Vector3::new(-0.07801828, 0.0463297, -1.5298351), + Vector3::new(-0.07664826, 0.056767933, -1.529835), + Vector3::new(-0.07664826, 0.112844266, -1.5143427), + Vector3::new(-0.1008886, 0.112844266, -1.4633205), + Vector3::new(-0.10961899, 0.0463297, -1.4633205), + Vector3::new(-0.07801833, 0.0463297, -1.529835), + Vector3::new(-0.120347425, 0.046329692, -1.4407388), + Vector3::new(-0.1354771, 0.0900808, -1.396806), + Vector3::new(-0.14269647, 0.0463297, -1.396806), + Vector3::new(-0.10961899, 0.0463297, -1.4633205), + Vector3::new(-0.1008886, 0.112844266, -1.4633205), + Vector3::new(-0.13248926, 0.112844266, -1.396806), + Vector3::new(-0.1354771, 0.09008081, -1.396806), + Vector3::new(-0.120347425, 0.0463297, -1.4407388), + Vector3::new(-0.13547714, 0.09008092, -1.3968059), + Vector3::new(-0.14316282, 0.11230591, -1.3744886), + Vector3::new(-0.14316282, 0.0463297, -1.3958893), + Vector3::new(-0.14269653, 0.0463297, -1.3968059), + Vector3::new(-0.13547714, 0.09008093, -1.3968059), + Vector3::new(-0.13248932, 0.112844266, -1.3968059), + Vector3::new(-0.14316282, 0.112844266, -1.3743398), + Vector3::new(-0.14316282, 0.11230593, -1.3744886), + Vector3::new(-0.14316282, 0.17935875, -1.7272853), + Vector3::new(-0.13450867, 0.17935875, -1.7264287), + Vector3::new(-0.1353515, 0.16140196, -1.7255667), + Vector3::new(-0.14316282, 0.16254377, -1.7264), + Vector3::new(-0.1353515, 0.16140196, -1.7255667), + Vector3::new(-0.1320768, 0.17935875, -1.7248962), + Vector3::new(-0.07664826, 0.17935875, -1.6954094), + Vector3::new(-0.07664826, 0.13506117, -1.6927663), + Vector3::new(-0.1353515, 0.16140196, -1.7255667), + Vector3::new(-0.13450867, 0.17935875, -1.7264287), + Vector3::new(-0.1320768, 0.17935875, -1.7248962), + Vector3::new(-0.14316282, 0.11284423, -1.6628642), + Vector3::new(-0.076648235, 0.11284423, -1.6628642), + Vector3::new(-0.076648235, 0.17935872, -1.6628642), + Vector3::new(-0.14316282, 0.17935872, -1.6628642), + Vector3::new(-0.14316282, 0.11284423, -1.5963496), + Vector3::new(-0.076648235, 0.11284423, -1.5963496), + Vector3::new(-0.076648235, 0.17935872, -1.5963496), + Vector3::new(-0.14316282, 0.17935872, -1.5963496), + Vector3::new(-0.14316282, 0.11284423, -1.5963496), + Vector3::new(-0.076648235, 0.11284423, -1.5963496), + Vector3::new(-0.076648235, 0.17935872, -1.5963496), + Vector3::new(-0.14316282, 0.17935872, -1.5963496), + Vector3::new(-0.14316282, 0.11284423, -1.529835), + Vector3::new(-0.076648235, 0.11284423, -1.529835), + Vector3::new(-0.076648235, 0.17935872, -1.529835), + Vector3::new(-0.14316282, 0.17935872, -1.529835), + Vector3::new(-0.07664826, 0.112844184, -1.5143427), + Vector3::new(-0.07664826, 0.17935875, -1.4959666), + Vector3::new(-0.09215823, 0.17935875, -1.4633205), + Vector3::new(-0.100888625, 0.112844184, -1.4633205), + Vector3::new(-0.100888625, 0.112844184, -1.4633205), + Vector3::new(-0.09215823, 0.17935875, -1.4633205), + Vector3::new(-0.12375891, 0.17935875, -1.396806), + Vector3::new(-0.13248931, 0.11284419, -1.396806), + Vector3::new(-0.13248937, 0.11284419, -1.3968059), + Vector3::new(-0.12375897, 0.17935875, -1.3968059), + Vector3::new(-0.14316282, 0.17935875, -1.3559638), + Vector3::new(-0.14316282, 0.11284419, -1.3743399), + Vector3::new(-0.07457939, -0.2197285, -1.7293788), + Vector3::new(-0.07664827, -0.2197285, -1.7424798), + Vector3::new(-0.07664827, -0.21527156, -1.7458802), + Vector3::new(-0.07055528, -0.1863293, -1.7293788), + Vector3::new(-0.06568167, -0.15321395, -1.7382675), + Vector3::new(-0.06501015, -0.15321395, -1.7293788), + Vector3::new(-0.070555285, -0.1863293, -1.7293788), + Vector3::new(-0.07664827, -0.21527156, -1.7458802), + Vector3::new(-0.07664827, -0.209393, -1.7589098), + Vector3::new(-0.07664827, -0.209393, -1.7589098), + Vector3::new(-0.07664827, -0.15321395, -1.7526212), + Vector3::new(-0.06568167, -0.15321395, -1.7382675), + Vector3::new(-0.07664827, -0.19288324, -1.6628642), + Vector3::new(-0.07664827, -0.2197285, -1.685307), + Vector3::new(-0.07070869, -0.2197285, -1.7048682), + Vector3::new(-0.059659734, -0.15321395, -1.6856498), + Vector3::new(-0.06657837, -0.15321395, -1.6628642), + Vector3::new(-0.063583754, -0.15321395, -1.7104981), + Vector3::new(-0.059659734, -0.15321395, -1.6856498), + Vector3::new(-0.07070869, -0.2197285, -1.7048681), + Vector3::new(-0.074579366, -0.2197285, -1.7293787), + Vector3::new(-0.07055523, -0.1863291, -1.7293787), + Vector3::new(-0.065010145, -0.15321395, -1.7293787), + Vector3::new(-0.06358376, -0.15321395, -1.7104981), + Vector3::new(-0.07055524, -0.1863291, -1.7293787), + Vector3::new(-0.07664827, -0.19288324, -1.6628642), + Vector3::new(-0.06657837, -0.15321395, -1.6628642), + Vector3::new(-0.07664827, -0.15321395, -1.6297004), + Vector3::new(-0.06501015, -0.15321401, -1.7293788), + Vector3::new(-0.06568169, -0.15321401, -1.7382677), + Vector3::new(-0.060959324, -0.12902257, -1.7293788), + Vector3::new(-0.06568169, -0.15321401, -1.7382677), + Vector3::new(-0.07664827, -0.15321401, -1.7526213), + Vector3::new(-0.07664827, -0.14703351, -1.7519294), + Vector3::new(-0.062409427, -0.11206737, -1.7293788), + Vector3::new(-0.060959324, -0.12902257, -1.7293788), + Vector3::new(-0.07664827, -0.086699456, -1.7403017), + Vector3::new(-0.06681198, -0.086699456, -1.7293788), + Vector3::new(-0.062409427, -0.11206737, -1.7293788), + Vector3::new(-0.07664827, -0.14703351, -1.7519294), + Vector3::new(-0.06657838, -0.15321401, -1.6628642), + Vector3::new(-0.059659746, -0.15321401, -1.6856498), + Vector3::new(-0.048610788, -0.08669945, -1.6664314), + Vector3::new(-0.049693942, -0.08669945, -1.6628642), + Vector3::new(-0.049580965, -0.08669945, -1.6725749), + Vector3::new(-0.048610788, -0.08669945, -1.6664314), + Vector3::new(-0.059659746, -0.15321401, -1.6856498), + Vector3::new(-0.06358377, -0.15321401, -1.7104982), + Vector3::new(-0.05326687, -0.08961612, -1.7148994), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.052291032, -0.08669945, -1.7084472), + Vector3::new(-0.049580965, -0.08669945, -1.6725749), + Vector3::new(-0.06358377, -0.15321401, -1.7104982), + Vector3::new(-0.065010145, -0.15321401, -1.7293787), + Vector3::new(-0.06095926, -0.12902224, -1.7293787), + Vector3::new(-0.05326687, -0.08961612, -1.7148994), + Vector3::new(-0.06095926, -0.12902224, -1.7293787), + Vector3::new(-0.062409353, -0.112067185, -1.7293787), + Vector3::new(-0.05326687, -0.08961612, -1.7148994), + Vector3::new(-0.066811875, -0.086699456, -1.7293787), + Vector3::new(-0.053440113, -0.086699456, -1.7145298), + Vector3::new(-0.05326687, -0.08961612, -1.7148994), + Vector3::new(-0.06240935, -0.112067185, -1.7293787), + Vector3::new(-0.07664827, -0.113321126, -1.5963497), + Vector3::new(-0.07664827, -0.15321401, -1.6297004), + Vector3::new(-0.06657838, -0.15321401, -1.6628642), + Vector3::new(-0.049693942, -0.08669945, -1.6628642), + Vector3::new(-0.06989047, -0.08669945, -1.5963497), + Vector3::new(-0.07664827, -0.113320984, -1.5963496), + Vector3::new(-0.06989051, -0.08669945, -1.5963496), + Vector3::new(-0.07664827, -0.08669945, -1.5740938), + Vector3::new(-0.021179304, -0.02406057, -1.8624079), + Vector3::new(-0.023803987, -0.020184837, -1.861124), + Vector3::new(-0.021124221, -0.020184837, -1.8624079), + Vector3::new(-0.015632318, -0.020184837, -1.8624079), + Vector3::new(-0.023803983, -0.020184837, -1.861124), + Vector3::new(-0.021179302, -0.02406057, -1.8624079), + Vector3::new(-0.06681203, -0.0866994, -1.7293788), + Vector3::new(-0.07664827, -0.0866994, -1.7403017), + Vector3::new(-0.07664827, -0.030022353, -1.7293788), + Vector3::new(-0.049693897, -0.0866994, -1.6628642), + Vector3::new(-0.048610777, -0.0866994, -1.6664313), + Vector3::new(-0.04655998, -0.074353606, -1.6628642), + Vector3::new(-0.049866408, -0.020184837, -1.6662687), + Vector3::new(-0.047395226, -0.020184841, -1.6628642), + Vector3::new(-0.047162194, -0.044913992, -1.6628642), + Vector3::new(-0.049580954, -0.0866994, -1.6725749), + Vector3::new(-0.045995377, -0.06966759, -1.6628642), + Vector3::new(-0.04655998, -0.07435361, -1.6628642), + Vector3::new(-0.04861078, -0.086699404, -1.6664313), + Vector3::new(-0.052291017, -0.086699404, -1.7084471), + Vector3::new(-0.045396965, -0.066093884, -1.6628642), + Vector3::new(-0.04599538, -0.06966759, -1.6628642), + Vector3::new(-0.049580958, -0.086699404, -1.6725749), + Vector3::new(-0.07664827, -0.020184837, -1.7274829), + Vector3::new(-0.057390846, -0.020184845, -1.706098), + Vector3::new(-0.053440116, -0.0866994, -1.7145296), + Vector3::new(-0.06681192, -0.0866994, -1.7293787), + Vector3::new(-0.07664827, -0.030021735, -1.7293787), + Vector3::new(-0.069890454, -0.086699404, -1.5963497), + Vector3::new(-0.049693897, -0.0866994, -1.6628642), + Vector3::new(-0.04655998, -0.074353606, -1.6628642), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.06681654, -0.07459004, -1.5963497), + Vector3::new(-0.047395226, -0.020184841, -1.6628642), + Vector3::new(-0.04431176, -0.020184845, -1.6586162), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.047162194, -0.044913992, -1.6628642), + Vector3::new(-0.045995377, -0.06966759, -1.6628642), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.04655998, -0.07435361, -1.6628642), + Vector3::new(-0.043273855, -0.020184834, -1.647505), + Vector3::new(-0.059779156, -0.020184841, -1.5963497), + Vector3::new(-0.06681654, -0.07459004, -1.5963497), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.04431176, -0.020184837, -1.6586162), + Vector3::new(-0.04244908, -0.020184815, -1.6544119), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.042449083, -0.020184837, -1.6544119), + Vector3::new(-0.04327385, -0.020184845, -1.647505), + Vector3::new(-0.045396965, -0.066093884, -1.6628642), + Vector3::new(-0.0449818, -0.064853005, -1.6601192), + Vector3::new(-0.04599538, -0.06966759, -1.6628642), + Vector3::new(-0.07664827, -0.078974426, -1.5676357), + Vector3::new(-0.07664827, -0.086699404, -1.5740938), + Vector3::new(-0.06989049, -0.086699404, -1.5963496), + Vector3::new(-0.06681658, -0.07459006, -1.5963496), + Vector3::new(-0.06958121, -0.020184834, -1.56597), + Vector3::new(-0.07664827, -0.020184845, -1.5510949), + Vector3::new(-0.07664827, -0.07657614, -1.5666742), + Vector3::new(-0.059779197, -0.020184841, -1.5963496), + Vector3::new(-0.06958121, -0.020184845, -1.56597), + Vector3::new(-0.07664827, -0.07657617, -1.5666742), + Vector3::new(-0.07664827, -0.078974426, -1.5676357), + Vector3::new(-0.06681658, -0.07459006, -1.5963496), + Vector3::new(-0.030793766, -0.009863387, -1.857705), + Vector3::new(-0.031695616, 0.046329774, -1.8540797), + Vector3::new(-0.020150661, 0.046329774, -1.8624079), + Vector3::new(-0.020185381, 0.045856625, -1.8624079), + Vector3::new(-0.021124247, -0.020184793, -1.8624079), + Vector3::new(-0.023804016, -0.020184793, -1.861124), + Vector3::new(-0.030793766, -0.009863387, -1.857705), + Vector3::new(-0.020185381, 0.045856625, -1.8624079), + Vector3::new(-0.030793766, -0.009863387, -1.857705), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(-0.033531733, 0.16073631, -1.8466988), + Vector3::new(-0.030793766, -0.009863387, -1.857705), + Vector3::new(-0.023804015, -0.020184793, -1.861124), + Vector3::new(-0.015632331, -0.020184793, -1.8624079), + Vector3::new(-0.010133706, -0.016342914, -1.8624079), + Vector3::new(-0.010133706, 0.028591737, -1.8523036), + Vector3::new(-0.047395177, -0.020184789, -1.6628642), + Vector3::new(-0.04986641, -0.020184793, -1.6662688), + Vector3::new(-0.057139993, 0.046329774, -1.675426), + Vector3::new(-0.048021954, 0.04632977, -1.6628642), + Vector3::new(-0.07664827, 0.046329774, -1.714664), + Vector3::new(-0.061341584, 0.046329767, -1.6976664), + Vector3::new(-0.05739085, -0.020184785, -1.7060981), + Vector3::new(-0.07664827, -0.020184785, -1.7274828), + Vector3::new(-0.043314017, 0.046329767, -1.6563781), + Vector3::new(-0.04431176, -0.020184785, -1.6586162), + Vector3::new(-0.047395177, -0.020184789, -1.6628642), + Vector3::new(-0.048021954, 0.04632977, -1.6628642), + Vector3::new(-0.040730577, 0.046329774, -1.6287216), + Vector3::new(-0.05117538, 0.046329778, -1.5963497), + Vector3::new(-0.059779152, -0.020184787, -1.5963497), + Vector3::new(-0.04327385, -0.020184793, -1.647505), + Vector3::new(-0.043314017, 0.046329774, -1.6563781), + Vector3::new(-0.038677655, 0.046329767, -1.6459134), + Vector3::new(-0.04244908, -0.020184815, -1.6544119), + Vector3::new(-0.04431176, -0.020184793, -1.6586162), + Vector3::new(-0.038677655, 0.04632977, -1.6459134), + Vector3::new(-0.04073058, 0.04632978, -1.6287216), + Vector3::new(-0.04327385, -0.020184785, -1.647505), + Vector3::new(-0.04244908, -0.020184796, -1.6544119), + Vector3::new(-0.07664827, 0.046329767, -1.5327188), + Vector3::new(-0.07664827, -0.020184785, -1.5510949), + Vector3::new(-0.06958121, -0.020184793, -1.56597), + Vector3::new(-0.061245486, 0.046329774, -1.5651392), + Vector3::new(-0.05117542, 0.046329778, -1.5963496), + Vector3::new(-0.06124548, 0.04632978, -1.5651392), + Vector3::new(-0.0695812, -0.020184785, -1.56597), + Vector3::new(-0.05977919, -0.020184787, -1.5963496), + Vector3::new(-0.03169561, 0.0463297, -1.8540797), + Vector3::new(-0.03276311, 0.112844266, -1.8497885), + Vector3::new(-0.015269473, 0.112844266, -1.8624079), + Vector3::new(-0.020150675, 0.0463297, -1.8624079), + Vector3::new(-0.04802195, 0.046329703, -1.6628642), + Vector3::new(-0.057139985, 0.0463297, -1.675426), + Vector3::new(-0.06441356, 0.112844266, -1.6845832), + Vector3::new(-0.048648708, 0.11284426, -1.6628642), + Vector3::new(-0.069005296, 0.112844266, -1.693358), + Vector3::new(-0.06529231, 0.112844266, -1.6892347), + Vector3::new(-0.06134158, 0.0463297, -1.6976664), + Vector3::new(-0.07664827, 0.046329707, -1.714664), + Vector3::new(-0.07664827, 0.06245753, -1.7115558), + Vector3::new(-0.069005296, 0.112844266, -1.693358), + Vector3::new(-0.07664827, 0.062457547, -1.7115558), + Vector3::new(-0.07664827, 0.10633551, -1.697959), + Vector3::new(-0.07386339, 0.112844266, -1.6950004), + Vector3::new(-0.042316273, 0.11284426, -1.6541401), + Vector3::new(-0.043314017, 0.046329707, -1.6563781), + Vector3::new(-0.04802195, 0.046329703, -1.6628642), + Vector3::new(-0.048648708, 0.11284426, -1.6628642), + Vector3::new(-0.03818731, 0.112844266, -1.6099383), + Vector3::new(-0.04257166, 0.112844266, -1.5963497), + Vector3::new(-0.051175404, 0.046329707, -1.5963497), + Vector3::new(-0.04073058, 0.046329707, -1.6287217), + Vector3::new(-0.042316273, 0.112844266, -1.6541401), + Vector3::new(-0.034906235, 0.11284426, -1.6374148), + Vector3::new(-0.03867766, 0.046329707, -1.6459134), + Vector3::new(-0.043314017, 0.0463297, -1.6563781), + Vector3::new(-0.034906235, 0.112844266, -1.6374148), + Vector3::new(-0.03818731, 0.11284426, -1.6099383), + Vector3::new(-0.04073058, 0.046329707, -1.6287217), + Vector3::new(-0.03867766, 0.046329692, -1.6459134), + Vector3::new(-0.07664827, 0.05676751, -1.5298351), + Vector3::new(-0.07664827, 0.046329707, -1.5327188), + Vector3::new(-0.061245494, 0.0463297, -1.5651392), + Vector3::new(-0.05290977, 0.112844266, -1.5643085), + Vector3::new(-0.069287896, 0.11284426, -1.5298351), + Vector3::new(-0.042571697, 0.112844266, -1.5963496), + Vector3::new(-0.05290976, 0.11284426, -1.5643085), + Vector3::new(-0.061245486, 0.046329707, -1.5651392), + Vector3::new(-0.051175445, 0.046329707, -1.5963496), + Vector3::new(-0.07664827, 0.05676794, -1.529835), + Vector3::new(-0.069287956, 0.11284426, -1.529835), + Vector3::new(-0.07664827, 0.11284426, -1.5143427), + Vector3::new(-0.015269473, 0.11284419, -1.8624079), + Vector3::new(-0.03276311, 0.1128442, -1.8497885), + Vector3::new(-0.033531733, 0.16073631, -1.8466988), + Vector3::new(-0.010635661, 0.17598689, -1.8624079), + Vector3::new(-0.011457793, 0.17935875, -1.8624079), + Vector3::new(-0.010635662, 0.17598689, -1.8624079), + Vector3::new(-0.033531733, 0.16073631, -1.8466988), + Vector3::new(-0.03173334, 0.17935875, -1.8504404), + Vector3::new(-0.010133706, 0.1447646, -1.8445529), + Vector3::new(-0.010133706, 0.17935875, -1.8480864), + Vector3::new(-0.033573695, 0.17935874, -1.8486018), + Vector3::new(-0.033531733, 0.16073631, -1.8466988), + Vector3::new(-0.033531733, 0.16073631, -1.8466988), + Vector3::new(-0.033573695, 0.17935875, -1.8486018), + Vector3::new(-0.03173334, 0.17935875, -1.8504404), + Vector3::new(-0.04864877, 0.1128442, -1.6628642), + Vector3::new(-0.064413555, 0.11284419, -1.6845832), + Vector3::new(-0.06633711, 0.13043445, -1.6870049), + Vector3::new(-0.06087463, 0.17935875, -1.6788442), + Vector3::new(-0.049275506, 0.17935875, -1.6628642), + Vector3::new(-0.06633711, 0.13043445, -1.6870049), + Vector3::new(-0.06676162, 0.17935875, -1.69015), + Vector3::new(-0.060874626, 0.17935875, -1.6788442), + Vector3::new(-0.06900531, 0.11284419, -1.693358), + Vector3::new(-0.06633711, 0.13043445, -1.6870049), + Vector3::new(-0.065292306, 0.11284419, -1.6892347), + Vector3::new(-0.06633711, 0.13043445, -1.6870049), + Vector3::new(-0.06900531, 0.11284419, -1.693358), + Vector3::new(-0.07386342, 0.11284419, -1.6950004), + Vector3::new(-0.07664827, 0.17935875, -1.6954094), + Vector3::new(-0.06676162, 0.17935875, -1.69015), + Vector3::new(-0.06633711, 0.13043445, -1.6870049), + Vector3::new(-0.07664827, 0.13506119, -1.6927663), + Vector3::new(-0.041318532, 0.17935875, -1.651902), + Vector3::new(-0.042316277, 0.1128442, -1.65414), + Vector3::new(-0.04864877, 0.1128442, -1.6628642), + Vector3::new(-0.049275506, 0.17935875, -1.6628642), + Vector3::new(-0.042571664, 0.11284419, -1.5963497), + Vector3::new(-0.03818731, 0.11284419, -1.6099383), + Vector3::new(-0.036347426, 0.16096294, -1.5963497), + Vector3::new(-0.041318532, 0.17935875, -1.651902), + Vector3::new(-0.031134814, 0.17935875, -1.6289163), + Vector3::new(-0.03490624, 0.1128442, -1.6374148), + Vector3::new(-0.042316277, 0.11284419, -1.65414), + Vector3::new(-0.031134814, 0.17935875, -1.6289163), + Vector3::new(-0.035023697, 0.17935875, -1.5963497), + Vector3::new(-0.036347426, 0.16096295, -1.5963497), + Vector3::new(-0.03818731, 0.1128442, -1.6099383), + Vector3::new(-0.03490624, 0.11284419, -1.6374148), + Vector3::new(-0.069287896, 0.1128442, -1.5298351), + Vector3::new(-0.052909777, 0.1128442, -1.5643085), + Vector3::new(-0.044574052, 0.17935875, -1.5634778), + Vector3::new(-0.060557496, 0.17935875, -1.5298351), + Vector3::new(-0.03564404, 0.17935875, -1.5911548), + Vector3::new(-0.044574052, 0.17935875, -1.5634778), + Vector3::new(-0.052909773, 0.1128442, -1.5643085), + Vector3::new(-0.0425717, 0.11284419, -1.5963496), + Vector3::new(-0.03634741, 0.16096337, -1.5963496), + Vector3::new(-0.03502371, 0.17935875, -1.5963496), + Vector3::new(-0.03564404, 0.17935875, -1.5911548), + Vector3::new(-0.03634741, 0.16096337, -1.5963496), + Vector3::new(-0.076648265, 0.17935875, -1.4959666), + Vector3::new(-0.076648265, 0.1128442, -1.5143427), + Vector3::new(-0.069287956, 0.1128442, -1.529835), + Vector3::new(-0.06055755, 0.17935875, -1.529835), + Vector3::new(0.056380846, -0.12297553, -1.7159696), + Vector3::new(0.049570914, -0.08669945, -1.7050503), + Vector3::new(0.049927697, -0.08669945, -1.7057998), + Vector3::new(0.056380846, -0.104652956, -1.7176802), + Vector3::new(0.05365365, -0.08669945, -1.6628642), + Vector3::new(0.049570914, -0.08669945, -1.7050503), + Vector3::new(0.056380846, -0.12297552, -1.7159696), + Vector3::new(0.056380846, -0.099275514, -1.6628642), + Vector3::new(0.056380846, -0.10465294, -1.7176802), + Vector3::new(0.049927697, -0.08669945, -1.7057998), + Vector3::new(0.056380846, -0.08669945, -1.7127628), + Vector3::new(0.056380846, -0.08669945, -1.6338606), + Vector3::new(0.055560432, -0.08669945, -1.6356738), + Vector3::new(0.056380846, -0.09205388, -1.6371784), + Vector3::new(0.082199745, 0.00933588, -1.5234594), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.15582763, -0.12761694, -1.4394239), + Vector3::new(0.056380846, -0.090703234, -1.643656), + Vector3::new(0.0557327, -0.08669945, -1.6413817), + Vector3::new(0.05365365, -0.08669945, -1.6628642), + Vector3::new(0.056380846, -0.099275514, -1.6628642), + Vector3::new(0.056380846, -0.09205389, -1.6371784), + Vector3::new(0.055560432, -0.08669945, -1.6356738), + Vector3::new(0.055732697, -0.08669945, -1.6413817), + Vector3::new(0.056380846, -0.09070324, -1.643656), + Vector3::new(0.03944725, 0.13982476, -1.6501261), + Vector3::new(0.11484603, 0.11130635, -1.7216146), + Vector3::new(0.049180854, -0.08462163, -1.7044249), + Vector3::new(0.049570903, -0.086699404, -1.7050502), + Vector3::new(0.049180854, -0.08462163, -1.7044249), + Vector3::new(0.04992768, -0.086699404, -1.7057998), + Vector3::new(0.05104391, -0.07466504, -1.6628642), + Vector3::new(0.049180854, -0.08462163, -1.7044249), + Vector3::new(0.049570903, -0.086699404, -1.7050502), + Vector3::new(0.053653635, -0.086699404, -1.6628642), + Vector3::new(0.056380846, -0.086699404, -1.7127628), + Vector3::new(0.04992768, -0.086699404, -1.7057998), + Vector3::new(0.049180854, -0.08462163, -1.7044249), + Vector3::new(0.056380846, -0.06313871, -1.7063097), + Vector3::new(0.056380846, -0.05689171, -1.6161941), + Vector3::new(0.056380846, -0.020184837, -1.6050887), + Vector3::new(0.049929477, -0.020184815, -1.6232537), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.049929474, -0.020184837, -1.6232537), + Vector3::new(0.049345437, -0.020184815, -1.6278259), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.055560425, -0.086699404, -1.6356738), + Vector3::new(0.056380846, -0.086699404, -1.6338605), + Vector3::new(0.056380846, -0.06908849, -1.6229479), + Vector3::new(0.049345437, -0.020184837, -1.6278259), + Vector3::new(0.04956496, -0.02018483, -1.6346076), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.05104391, -0.07466504, -1.6628642), + Vector3::new(0.053653635, -0.086699404, -1.6628642), + Vector3::new(0.055732694, -0.086699404, -1.6413817), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.055560425, -0.086699404, -1.6356738), + Vector3::new(0.0525133, -0.06681228, -1.6300853), + Vector3::new(0.055732694, -0.086699404, -1.6413817), + Vector3::new(0.018538032, 0.046329774, -1.8624079), + Vector3::new(0.025458898, 0.046329774, -1.8539069), + Vector3::new(0.01913499, 0.0041060485, -1.8624079), + Vector3::new(0.019134987, 0.0041060466, -1.8624079), + Vector3::new(0.025458895, 0.046329774, -1.8539069), + Vector3::new(-0.00060392544, 0.046329774, -1.8498122), + Vector3::new(-0.010133721, 0.028591715, -1.8523036), + Vector3::new(-0.010133721, -0.016342916, -1.8624079), + Vector3::new(0.056380846, -0.020184793, -1.6050887), + Vector3::new(0.056380846, 0.008700218, -1.5963497), + Vector3::new(0.052337565, 0.04632978, -1.5963497), + Vector3::new(0.046243615, 0.046329796, -1.6135083), + Vector3::new(0.049929477, -0.020184815, -1.6232537), + Vector3::new(0.04482644, 0.046329796, -1.6246027), + Vector3::new(0.049345437, -0.020184815, -1.6278259), + Vector3::new(0.049929474, -0.020184793, -1.6232537), + Vector3::new(0.046243615, 0.046329774, -1.6135083), + Vector3::new(0.045359116, 0.046329774, -1.6410584), + Vector3::new(0.049564958, -0.0201848, -1.6346076), + Vector3::new(0.049345434, -0.020184793, -1.6278259), + Vector3::new(0.04482644, 0.046329774, -1.6246027), + Vector3::new(0.056380846, 0.008700613, -1.5963496), + Vector3::new(0.056380846, 0.046329774, -1.5849651), + Vector3::new(0.052337606, 0.04632978, -1.5963496), + Vector3::new(0.017597593, 0.112844266, -1.8624079), + Vector3::new(0.035420865, 0.112844266, -1.8405151), + Vector3::new(0.025458887, 0.0463297, -1.8539069), + Vector3::new(0.018538024, 0.0463297, -1.8624079), + Vector3::new(0.035420865, 0.112844266, -1.8405151), + Vector3::new(0.035131067, 0.112844266, -1.8404696), + Vector3::new(-0.0006039664, 0.0463297, -1.8498122), + Vector3::new(0.025458885, 0.0463297, -1.8539069), + Vector3::new(0.04519064, 0.11284426, -1.5963497), + Vector3::new(0.04255776, 0.11284426, -1.6037631), + Vector3::new(0.04624362, 0.046329707, -1.6135085), + Vector3::new(0.05233758, 0.046329703, -1.5963497), + Vector3::new(0.04030746, 0.11284426, -1.6213795), + Vector3::new(0.04482645, 0.046329677, -1.6246027), + Vector3::new(0.04624362, 0.0463297, -1.6135085), + Vector3::new(0.04255776, 0.112844266, -1.6037631), + Vector3::new(0.04115328, 0.112844266, -1.6475093), + Vector3::new(0.04535912, 0.0463297, -1.6410584), + Vector3::new(0.044826448, 0.0463297, -1.6246027), + Vector3::new(0.040307455, 0.112844266, -1.6213795), + Vector3::new(0.056380846, 0.0463297, -1.5849651), + Vector3::new(0.056380846, 0.112844266, -1.5648415), + Vector3::new(0.045190684, 0.11284426, -1.5963496), + Vector3::new(0.052337624, 0.046329703, -1.5963496), + Vector3::new(0.016908769, 0.16156493, -1.8624079), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(0.035420854, 0.11284419, -1.8405151), + Vector3::new(0.017597588, 0.1128442, -1.8624079), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(0.033428412, 0.17935875, -1.8455174), + Vector3::new(0.02062426, 0.17935875, -1.8474098), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(0.035209637, 0.17935875, -1.8489724), + Vector3::new(0.03342841, 0.17935875, -1.8455174), + Vector3::new(0.019537896, 0.17935875, -1.8624079), + Vector3::new(0.035209637, 0.17935875, -1.8489724), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(0.016908769, 0.16156493, -1.8624079), + Vector3::new(-0.010133721, 0.17935875, -1.8480864), + Vector3::new(-0.010133721, 0.14476462, -1.8445529), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(0.02062426, 0.17935875, -1.8474098), + Vector3::new(0.035420854, 0.1128442, -1.8405151), + Vector3::new(0.03553288, 0.11359217, -1.8403646), + Vector3::new(0.03513103, 0.11284419, -1.8404696), + Vector3::new(0.05102139, 0.17935875, -1.6628642), + Vector3::new(0.056380846, 0.17935875, -1.6680272), + Vector3::new(0.056380846, 0.1334199, -1.6661814), + Vector3::new(0.05288215, 0.13474323, -1.6628642), + Vector3::new(0.039753903, 0.16344245, -1.5963497), + Vector3::new(0.04255777, 0.1128442, -1.6037631), + Vector3::new(0.045190655, 0.1128442, -1.5963497), + Vector3::new(0.035788465, 0.17935875, -1.6181563), + Vector3::new(0.04030746, 0.1128442, -1.6213795), + Vector3::new(0.04255777, 0.1128442, -1.6037631), + Vector3::new(0.039753903, 0.16344245, -1.5963497), + Vector3::new(0.038574025, 0.17935875, -1.5963497), + Vector3::new(0.03667778, 0.17935875, -1.6456296), + Vector3::new(0.042568896, 0.17935875, -1.6547216), + Vector3::new(0.03944725, 0.13982476, -1.6501261), + Vector3::new(0.03944725, 0.13982476, -1.6501261), + Vector3::new(0.041153286, 0.11284419, -1.6475093), + Vector3::new(0.040307462, 0.112844184, -1.6213795), + Vector3::new(0.03578847, 0.17935875, -1.6181563), + Vector3::new(0.036677778, 0.17935875, -1.6456296), + Vector3::new(0.03944725, 0.13982476, -1.6501261), + Vector3::new(0.042568896, 0.17935875, -1.6547216), + Vector3::new(0.05102139, 0.17935875, -1.6628642), + Vector3::new(0.05288215, 0.13474323, -1.6628642), + Vector3::new(0.056380846, 0.1128442, -1.5648416), + Vector3::new(0.056380846, 0.17935875, -1.544718), + Vector3::new(0.038871914, 0.17935875, -1.5940177), + Vector3::new(0.039753858, 0.16344327, -1.5963496), + Vector3::new(0.045190696, 0.1128442, -1.5963496), + Vector3::new(0.03857404, 0.17935875, -1.5963496), + Vector3::new(0.039753858, 0.16344327, -1.5963496), + Vector3::new(0.03887191, 0.17935875, -1.5940177), + Vector3::new(0.12289546, -0.16181482, -1.7958934), + Vector3::new(0.12289546, -0.2197285, -1.8116077), + Vector3::new(0.114046395, -0.2197285, -1.8114208), + Vector3::new(0.114184655, -0.16249311, -1.7958934), + Vector3::new(0.114184655, -0.16249311, -1.7958934), + Vector3::new(0.114046395, -0.2197285, -1.8114208), + Vector3::new(0.09965607, -0.2197285, -1.7958934), + Vector3::new(0.11420707, -0.15321395, -1.7933761), + Vector3::new(0.12289546, -0.15321395, -1.7935597), + Vector3::new(0.12289546, -0.16181439, -1.7958933), + Vector3::new(0.114184655, -0.16249268, -1.7958933), + Vector3::new(0.06410769, -0.15321395, -1.7293788), + Vector3::new(0.07383545, -0.15321395, -1.7498146), + Vector3::new(0.09774321, -0.2197285, -1.7938294), + Vector3::new(0.07454382, -0.2197285, -1.7450926), + Vector3::new(0.06474366, -0.16752368, -1.7293788), + Vector3::new(0.06474366, -0.16752367, -1.7293788), + Vector3::new(0.07454382, -0.2197285, -1.7450926), + Vector3::new(0.07606459, -0.2197285, -1.7293788), + Vector3::new(0.07383545, -0.15321395, -1.7498146), + Vector3::new(0.11420707, -0.15321395, -1.7933761), + Vector3::new(0.114184655, -0.16249268, -1.7958933), + Vector3::new(0.099655956, -0.2197285, -1.7958933), + Vector3::new(0.09774322, -0.2197285, -1.7938294), + Vector3::new(0.07038619, -0.1834601, -1.6628642), + Vector3::new(0.07594328, -0.2197285, -1.6730559), + Vector3::new(0.080554605, -0.2197285, -1.6628642), + Vector3::new(0.064107634, -0.15321395, -1.7293787), + Vector3::new(0.064743586, -0.16752328, -1.7293787), + Vector3::new(0.06205736, -0.15321395, -1.7250715), + Vector3::new(0.06650028, -0.15321395, -1.6791637), + Vector3::new(0.06205736, -0.15321395, -1.7250715), + Vector3::new(0.064743586, -0.16752328, -1.7293787), + Vector3::new(0.0760646, -0.2197285, -1.7293787), + Vector3::new(0.07726786, -0.2197285, -1.7169456), + Vector3::new(0.07594328, -0.2197285, -1.6730559), + Vector3::new(0.07038619, -0.1834601, -1.6628642), + Vector3::new(0.06600836, -0.15321395, -1.6628642), + Vector3::new(0.06650028, -0.15321395, -1.6791637), + Vector3::new(0.07726786, -0.2197285, -1.7169456), + Vector3::new(0.0920014, -0.15321395, -1.5963497), + Vector3::new(0.06575185, -0.15321395, -1.6543648), + Vector3::new(0.07038619, -0.1834601, -1.6628642), + Vector3::new(0.080554605, -0.2197285, -1.6628642), + Vector3::new(0.110649765, -0.2197285, -1.5963497), + Vector3::new(0.07038619, -0.1834601, -1.6628642), + Vector3::new(0.06575185, -0.15321395, -1.6543648), + Vector3::new(0.06600836, -0.15321395, -1.6628642), + Vector3::new(0.12289546, -0.2197285, -1.569285), + Vector3::new(0.12289546, -0.15606314, -1.5298351), + Vector3::new(0.12209664, -0.15321395, -1.5298351), + Vector3::new(0.09200145, -0.15321395, -1.5963496), + Vector3::new(0.110649824, -0.2197285, -1.5963496), + Vector3::new(0.12289546, -0.15606296, -1.529835), + Vector3::new(0.12289546, -0.15321395, -1.5280696), + Vector3::new(0.122096695, -0.15321395, -1.529835), + Vector3::new(0.11436774, -0.08669945, -1.7753314), + Vector3::new(0.12289546, -0.08669945, -1.7755116), + Vector3::new(0.12289546, -0.15321401, -1.7935597), + Vector3::new(0.11420707, -0.15321401, -1.7933761), + Vector3::new(0.06273523, -0.12233167, -1.7293788), + Vector3::new(0.07383547, -0.15321401, -1.7498146), + Vector3::new(0.0641077, -0.15321401, -1.7293788), + Vector3::new(0.071780056, -0.08669945, -1.7293788), + Vector3::new(0.11436774, -0.08669945, -1.7753314), + Vector3::new(0.11420707, -0.15321401, -1.7933761), + Vector3::new(0.07383547, -0.15321401, -1.7498146), + Vector3::new(0.06273523, -0.12233168, -1.7293788), + Vector3::new(0.05638089, -0.12297577, -1.7159697), + Vector3::new(0.05638089, -0.104653075, -1.7176803), + Vector3::new(0.06273516, -0.12233149, -1.7293787), + Vector3::new(0.06410764, -0.15321401, -1.7293787), + Vector3::new(0.06205737, -0.15321401, -1.7250715), + Vector3::new(0.05638089, -0.09927574, -1.6628642), + Vector3::new(0.05638089, -0.12297576, -1.7159697), + Vector3::new(0.062057372, -0.15321401, -1.7250715), + Vector3::new(0.066500284, -0.15321401, -1.6791637), + Vector3::new(0.06185505, -0.12451902, -1.6628642), + Vector3::new(0.066500284, -0.15321401, -1.6791637), + Vector3::new(0.066008374, -0.15321401, -1.6628642), + Vector3::new(0.061855048, -0.12451902, -1.6628642), + Vector3::new(0.07177995, -0.08669945, -1.7293787), + Vector3::new(0.06273516, -0.12233149, -1.7293787), + Vector3::new(0.05638089, -0.104653075, -1.7176803), + Vector3::new(0.05638089, -0.08669945, -1.712763), + Vector3::new(0.065751866, -0.15321401, -1.6543648), + Vector3::new(0.09200141, -0.15321401, -1.5963497), + Vector3::new(0.07335302, -0.08669945, -1.5963497), + Vector3::new(0.05638089, -0.08669945, -1.6338605), + Vector3::new(0.05638089, -0.09205417, -1.6371785), + Vector3::new(0.05638089, -0.09927574, -1.6628642), + Vector3::new(0.06185505, -0.12451902, -1.6628642), + Vector3::new(0.05638089, -0.09070352, -1.6436561), + Vector3::new(0.066008374, -0.15321401, -1.6628642), + Vector3::new(0.065751866, -0.15321401, -1.6543648), + Vector3::new(0.05638089, -0.09205416, -1.6371785), + Vector3::new(0.05638089, -0.09070352, -1.6436561), + Vector3::new(0.061855048, -0.12451902, -1.6628642), + Vector3::new(0.09200147, -0.15321401, -1.5963496), + Vector3::new(0.12209664, -0.15321401, -1.5298351), + Vector3::new(0.10683623, -0.09878352, -1.5298351), + Vector3::new(0.08630396, -0.08669945, -1.5677264), + Vector3::new(0.07335308, -0.08669945, -1.5963496), + Vector3::new(0.19847965, 0.05531616, -1.2937757), + Vector3::new(0.122096695, -0.15321401, -1.529835), + Vector3::new(0.12289546, -0.15321401, -1.5280696), + Vector3::new(0.12289546, -0.10823503, -1.5001986), + Vector3::new(0.10683629, -0.09878356, -1.529835), + Vector3::new(0.11452841, -0.020184837, -1.7572867), + Vector3::new(0.12289546, -0.020184837, -1.7574636), + Vector3::new(0.12289546, -0.086699404, -1.7755116), + Vector3::new(0.11436774, -0.0866994, -1.7753314), + Vector3::new(0.20694056, -0.04075135, -1.7648193), + Vector3::new(0.19531466, 0.13716632, -1.7400897), + Vector3::new(0.08866407, -0.020184837, -1.7293788), + Vector3::new(0.11452841, -0.020184837, -1.7572868), + Vector3::new(0.11436774, -0.08669942, -1.7753315), + Vector3::new(0.07178003, -0.08669941, -1.7293788), + Vector3::new(0.08866396, -0.020184837, -1.7293787), + Vector3::new(0.07177992, -0.08669941, -1.7293787), + Vector3::new(0.05638089, -0.086699404, -1.712763), + Vector3::new(0.05638089, -0.063138574, -1.7063097), + Vector3::new(0.07077682, -0.020184837, -1.7100782), + Vector3::new(0.05638089, -0.056891594, -1.616194), + Vector3::new(0.061905872, -0.042719565, -1.5963497), + Vector3::new(0.059484527, -0.020184832, -1.5963497), + Vector3::new(0.05638089, -0.02018483, -1.6050886), + Vector3::new(0.05638089, -0.06908851, -1.6229479), + Vector3::new(0.05638089, -0.086699404, -1.6338605), + Vector3::new(0.073353015, -0.086699404, -1.5963497), + Vector3::new(0.07079374, -0.07757106, -1.5963497), + Vector3::new(0.061905906, -0.04271948, -1.5963496), + Vector3::new(0.07069106, -0.020184841, -1.5647956), + Vector3::new(0.05948457, -0.020184832, -1.5963496), + Vector3::new(0.0707938, -0.077571094, -1.5963496), + Vector3::new(0.07335307, -0.086699404, -1.5963496), + Vector3::new(0.08630388, -0.086699404, -1.5677265), + Vector3::new(0.114689074, 0.046329774, -1.7392421), + Vector3::new(0.12289546, 0.046329774, -1.7394154), + Vector3::new(0.12289546, -0.020184793, -1.7574635), + Vector3::new(0.1145284, -0.020184785, -1.7572868), + Vector3::new(0.105548084, 0.046329774, -1.7293788), + Vector3::new(0.114689074, 0.046329774, -1.7392421), + Vector3::new(0.1145284, -0.020184793, -1.7572868), + Vector3::new(0.08866408, -0.020184793, -1.7293788), + Vector3::new(0.10554797, 0.046329774, -1.7293787), + Vector3::new(0.088663965, -0.020184793, -1.7293787), + Vector3::new(0.070776835, -0.020184793, -1.7100782), + Vector3::new(0.09306916, 0.046329774, -1.7159139), + Vector3::new(0.05638089, -0.0201848, -1.6050886), + Vector3::new(0.05948453, -0.020184798, -1.5963497), + Vector3::new(0.05638089, 0.008699823, -1.5963497), + Vector3::new(0.059484575, -0.020184798, -1.5963496), + Vector3::new(0.07069108, -0.020184793, -1.5647956), + Vector3::new(0.08042465, 0.0047826264, -1.5298351), + Vector3::new(0.075960435, 0.046329774, -1.5298351), + Vector3::new(0.05638089, 0.04632978, -1.584965), + Vector3::new(0.05638089, 0.008700218, -1.5963496), + Vector3::new(0.07596047, 0.046329774, -1.529835), + Vector3::new(0.08042469, 0.00478271, -1.529835), + Vector3::new(0.082199745, 0.00933588, -1.5234594), + Vector3::new(0.077658, 0.046329774, -1.5250553), + Vector3::new(0.082199745, 0.00933588, -1.5234594), + Vector3::new(0.07995127, 0.046329774, -1.5189065), + Vector3::new(0.077658, 0.046329767, -1.5250553), + Vector3::new(0.10949655, 0.04632977, -1.4633205), + Vector3::new(0.07995127, 0.046329767, -1.5189065), + Vector3::new(0.082199745, 0.00933588, -1.5234594), + Vector3::new(0.112645745, 0.021375068, -1.4633205), + Vector3::new(0.12289546, 0.025428087, -1.4430746), + Vector3::new(0.12289546, 0.046329774, -1.438112), + Vector3::new(0.10949655, 0.04632977, -1.4633205), + Vector3::new(0.112645745, 0.021375068, -1.4633205), + Vector3::new(0.12289546, 0.08331877, -1.7293788), + Vector3::new(0.12289546, 0.046329696, -1.7394154), + Vector3::new(0.114689074, 0.046329707, -1.7392421), + Vector3::new(0.114776894, 0.082686655, -1.7293788), + Vector3::new(0.114776894, 0.082686655, -1.7293788), + Vector3::new(0.114689074, 0.0463297, -1.7392421), + Vector3::new(0.10554807, 0.046329696, -1.7293788), + Vector3::new(0.11484603, 0.11130635, -1.7216146), + Vector3::new(0.12289546, 0.0980159, -1.7253909), + Vector3::new(0.12289546, 0.08331921, -1.7293787), + Vector3::new(0.114776894, 0.082687095, -1.7293787), + Vector3::new(0.11484603, 0.11130635, -1.7216146), + Vector3::new(0.11446581, 0.112844266, -1.7213101), + Vector3::new(0.11483169, 0.11284426, -1.7216887), + Vector3::new(0.11484603, 0.11130635, -1.7216146), + Vector3::new(0.11483169, 0.112844266, -1.7216887), + Vector3::new(0.11963159, 0.112844266, -1.7227134), + Vector3::new(0.093069136, 0.046329692, -1.7159139), + Vector3::new(0.11484603, 0.11130635, -1.7216146), + Vector3::new(0.114776894, 0.082687095, -1.7293787), + Vector3::new(0.105547965, 0.046329696, -1.7293787), + Vector3::new(0.11484603, 0.11130635, -1.7216146), + Vector3::new(0.11077998, 0.112844266, -1.7177594), + Vector3::new(0.11446581, 0.11284426, -1.7213101), + Vector3::new(0.05638089, 0.11284426, -1.5648414), + Vector3::new(0.05638089, 0.046329692, -1.584965), + Vector3::new(0.07596044, 0.0463297, -1.5298351), + Vector3::new(0.06881351, 0.112844266, -1.5298351), + Vector3::new(0.068813555, 0.112844266, -1.529835), + Vector3::new(0.07596049, 0.0463297, -1.529835), + Vector3::new(0.07765801, 0.0463297, -1.5250553), + Vector3::new(0.06949201, 0.112844266, -1.5279247), + Vector3::new(0.07995127, 0.0463297, -1.5189065), + Vector3::new(0.07590854, 0.112844266, -1.5107203), + Vector3::new(0.06949201, 0.112844266, -1.5279247), + Vector3::new(0.07765801, 0.046329707, -1.5250553), + Vector3::new(0.10110265, 0.112844266, -1.4633205), + Vector3::new(0.07590854, 0.11284426, -1.5107203), + Vector3::new(0.07995127, 0.046329707, -1.5189065), + Vector3::new(0.10949655, 0.046329703, -1.4633205), + Vector3::new(0.12289546, 0.0463297, -1.438112), + Vector3::new(0.12289546, 0.112844266, -1.4223199), + Vector3::new(0.10110265, 0.112844266, -1.4633205), + Vector3::new(0.10949655, 0.046329703, -1.4633205), + Vector3::new(0.09802142, 0.17935875, -1.7081411), + Vector3::new(0.11421156, 0.17935875, -1.7248966), + Vector3::new(0.11483169, 0.1128442, -1.7216887), + Vector3::new(0.114465825, 0.11284419, -1.7213101), + Vector3::new(0.12289546, 0.17935875, -1.7267503), + Vector3::new(0.12289546, 0.113893166, -1.7234627), + Vector3::new(0.11963135, 0.11284419, -1.7227132), + Vector3::new(0.11483169, 0.11284419, -1.7216887), + Vector3::new(0.11421156, 0.17935875, -1.7248966), + Vector3::new(0.05638089, 0.17935875, -1.6680273), + Vector3::new(0.098021425, 0.17935875, -1.7081411), + Vector3::new(0.114465825, 0.1128442, -1.7213101), + Vector3::new(0.11078018, 0.11284419, -1.7177596), + Vector3::new(0.05638089, 0.13341987, -1.6661816), + Vector3::new(0.056380868, 0.11284423, -1.6628642), + Vector3::new(0.12289542, 0.11284423, -1.6628642), + Vector3::new(0.12289542, 0.17935872, -1.6628642), + Vector3::new(0.056380868, 0.17935872, -1.6628642), + Vector3::new(0.056380868, 0.11284423, -1.5963496), + Vector3::new(0.12289542, 0.11284423, -1.5963496), + Vector3::new(0.12289542, 0.17935872, -1.5963496), + Vector3::new(0.056380868, 0.17935872, -1.5963496), + Vector3::new(0.05638089, 0.17935875, -1.5447178), + Vector3::new(0.05638089, 0.11284419, -1.5648414), + Vector3::new(0.06881347, 0.11284419, -1.5298351), + Vector3::new(0.06405469, 0.1571329, -1.5298351), + Vector3::new(0.06132602, 0.17935875, -1.5307939), + Vector3::new(0.061683614, 0.17935875, -1.5298351), + Vector3::new(0.06132602, 0.17935875, -1.5307939), + Vector3::new(0.06405469, 0.1571329, -1.5298351), + Vector3::new(0.06405503, 0.15713012, -1.529835), + Vector3::new(0.06881352, 0.11284419, -1.529835), + Vector3::new(0.06949202, 0.11284419, -1.5279245), + Vector3::new(0.07590855, 0.11284419, -1.5107204), + Vector3::new(0.07186582, 0.17935875, -1.5025342), + Vector3::new(0.06168366, 0.17935875, -1.529835), + Vector3::new(0.06405503, 0.15713012, -1.529835), + Vector3::new(0.06949202, 0.1128442, -1.5279245), + Vector3::new(0.09270881, 0.17935875, -1.4633205), + Vector3::new(0.07186581, 0.17935875, -1.5025342), + Vector3::new(0.07590854, 0.1128442, -1.5107204), + Vector3::new(0.10110272, 0.11284419, -1.4633205), + Vector3::new(0.12289546, 0.11284419, -1.42232), + Vector3::new(0.12289546, 0.17935875, -1.4065279), + Vector3::new(0.09270881, 0.17935875, -1.4633205), + Vector3::new(0.10110272, 0.11284419, -1.4633205), + Vector3::new(0.18941, -0.19789436, -1.820354), + Vector3::new(0.18941, -0.2197285, -1.8267576), + Vector3::new(0.18697423, -0.2197285, -1.8280702), + Vector3::new(0.1578116, -0.15909654, -1.7958934), + Vector3::new(0.13264129, -0.2197285, -1.8118136), + Vector3::new(0.12289544, -0.2197285, -1.8116077), + Vector3::new(0.12289544, -0.16181482, -1.7958934), + Vector3::new(0.18941, -0.19789436, -1.820354), + Vector3::new(0.18697423, -0.2197285, -1.8280703), + Vector3::new(0.13264129, -0.2197285, -1.8118135), + Vector3::new(0.15781142, -0.15909699, -1.7958934), + Vector3::new(0.16541637, -0.15321395, -1.7958934), + Vector3::new(0.18941, -0.15321395, -1.8030725), + Vector3::new(0.12289544, -0.15321395, -1.7935597), + Vector3::new(0.16025366, -0.15321395, -1.7943488), + Vector3::new(0.15781179, -0.15909609, -1.7958933), + Vector3::new(0.12289544, -0.16181439, -1.7958933), + Vector3::new(0.16541599, -0.15321395, -1.7958933), + Vector3::new(0.1578116, -0.15909654, -1.7958933), + Vector3::new(0.16025366, -0.15321395, -1.7943487), + Vector3::new(0.12289542, -0.21972859, -1.7293787), + Vector3::new(0.18941003, -0.21972859, -1.7293787), + Vector3::new(0.18941003, -0.15321398, -1.7293787), + Vector3::new(0.12289542, -0.15321398, -1.7293787), + Vector3::new(0.12289542, -0.21972859, -1.6628642), + Vector3::new(0.18941003, -0.21972859, -1.6628642), + Vector3::new(0.18941003, -0.15321398, -1.6628642), + Vector3::new(0.12289542, -0.15321398, -1.6628642), + Vector3::new(0.12289542, -0.21972859, -1.6628642), + Vector3::new(0.18941003, -0.21972859, -1.6628642), + Vector3::new(0.18941003, -0.15321398, -1.6628642), + Vector3::new(0.12289542, -0.15321398, -1.6628642), + Vector3::new(0.12289542, -0.21972859, -1.5963496), + Vector3::new(0.18941003, -0.21972859, -1.5963496), + Vector3::new(0.18941003, -0.15321398, -1.5963496), + Vector3::new(0.12289542, -0.15321398, -1.5963496), + Vector3::new(0.12289544, -0.15606314, -1.5298351), + Vector3::new(0.12289544, -0.2197285, -1.5692852), + Vector3::new(0.13997748, -0.2197285, -1.5315315), + Vector3::new(0.1402694, -0.21803208, -1.5298351), + Vector3::new(0.1402694, -0.21803208, -1.5298351), + Vector3::new(0.13997748, -0.2197285, -1.5315315), + Vector3::new(0.14072663, -0.2197285, -1.5298351), + Vector3::new(0.12289544, -0.15606295, -1.529835), + Vector3::new(0.14026941, -0.21803196, -1.529835), + Vector3::new(0.151423, -0.15321395, -1.4650198), + Vector3::new(0.12289544, -0.15321395, -1.5280696), + Vector3::new(0.15217347, -0.15321395, -1.4633205), + Vector3::new(0.151423, -0.15321395, -1.4650198), + Vector3::new(0.14026941, -0.21803196, -1.529835), + Vector3::new(0.14072669, -0.2197285, -1.529835), + Vector3::new(0.15683821, -0.2197285, -1.4933524), + Vector3::new(0.15627542, -0.16843303, -1.4633205), + Vector3::new(0.15627542, -0.16843303, -1.4633205), + Vector3::new(0.15683821, -0.2197285, -1.4933524), + Vector3::new(0.18408938, -0.2197285, -1.4633205), + Vector3::new(0.15217347, -0.15321395, -1.4633205), + Vector3::new(0.15627542, -0.16843303, -1.4633205), + Vector3::new(0.15610845, -0.15321395, -1.4544102), + Vector3::new(0.18941, -0.15321395, -1.4177105), + Vector3::new(0.15610845, -0.15321395, -1.4544102), + Vector3::new(0.15627542, -0.16843303, -1.4633205), + Vector3::new(0.18408938, -0.2197285, -1.4633205), + Vector3::new(0.18941, -0.2197285, -1.457457), + Vector3::new(0.18941, -0.13465314, -1.7958934), + Vector3::new(0.18941, -0.15321401, -1.8030725), + Vector3::new(0.16541636, -0.15321401, -1.7958934), + Vector3::new(0.12289544, -0.08669945, -1.7755116), + Vector3::new(0.187866, -0.08669945, -1.776884), + Vector3::new(0.16025363, -0.15321401, -1.7943487), + Vector3::new(0.12289544, -0.15321401, -1.7935597), + Vector3::new(0.18941, -0.13465282, -1.7958933), + Vector3::new(0.16541596, -0.15321401, -1.7958933), + Vector3::new(0.16025363, -0.15321401, -1.7943487), + Vector3::new(0.187866, -0.08669945, -1.7768838), + Vector3::new(0.18941, -0.08669945, -1.7773458), + Vector3::new(0.12289544, -0.10823502, -1.5001986), + Vector3::new(0.12289544, -0.15321401, -1.5280696), + Vector3::new(0.151423, -0.15321401, -1.4650198), + Vector3::new(0.15171543, -0.1515146, -1.4633205), + Vector3::new(0.14287871, -0.11999598, -1.4633205), + Vector3::new(0.15171541, -0.1515146, -1.4633205), + Vector3::new(0.15142299, -0.15321401, -1.4650198), + Vector3::new(0.15217346, -0.15321401, -1.4633205), + Vector3::new(0.26476443, -0.12245499, -1.3162864), + Vector3::new(0.14287871, -0.11999598, -1.4633205), + Vector3::new(0.15171543, -0.1515146, -1.4633205), + Vector3::new(0.15582763, -0.12761694, -1.4394239), + Vector3::new(0.15610845, -0.15321401, -1.4544102), + Vector3::new(0.15582763, -0.12761694, -1.4394239), + Vector3::new(0.15171541, -0.1515146, -1.4633205), + Vector3::new(0.15217346, -0.15321401, -1.4633205), + Vector3::new(0.18941, -0.12602565, -1.4014639), + Vector3::new(0.15582763, -0.12761694, -1.4394239), + Vector3::new(0.15610845, -0.15321401, -1.4544102), + Vector3::new(0.18941, -0.15321401, -1.4177105), + Vector3::new(0.18941, -0.020184837, -1.7588685), + Vector3::new(0.18941, -0.082980156, -1.7759074), + Vector3::new(0.18786602, -0.086699404, -1.776884), + Vector3::new(0.12289544, -0.08669941, -1.7755116), + Vector3::new(0.12289544, -0.020184837, -1.7574636), + Vector3::new(0.18941, -0.082980156, -1.7759073), + Vector3::new(0.18941, -0.086699404, -1.7773458), + Vector3::new(0.18786603, -0.0866994, -1.7768838), + Vector3::new(0.15419942, 0.046329774, -1.7400767), + Vector3::new(0.18941, -0.011806563, -1.7565951), + Vector3::new(0.18941, -0.020184793, -1.7588685), + Vector3::new(0.12289544, -0.0201848, -1.7574635), + Vector3::new(0.12289544, 0.046329774, -1.7394154), + Vector3::new(0.12289544, 0.025428083, -1.4430746), + Vector3::new(0.14631943, 0.034690574, -1.396806), + Vector3::new(0.14485061, 0.04632977, -1.396806), + Vector3::new(0.12289544, 0.046329767, -1.438112), + Vector3::new(0.1463195, 0.034690596, -1.3968059), + Vector3::new(0.1757539, 0.046329774, -1.3386651), + Vector3::new(0.14485067, 0.04632977, -1.3968059), + Vector3::new(0.12289544, 0.046329692, -1.7394154), + Vector3::new(0.12289544, 0.083318785, -1.7293788), + Vector3::new(0.13139604, 0.08398056, -1.7293788), + Vector3::new(0.15419948, 0.0463297, -1.7400767), + Vector3::new(0.12289544, 0.08331923, -1.7293787), + Vector3::new(0.12289544, 0.09801593, -1.7253909), + Vector3::new(0.13139579, 0.08398098, -1.7293787), + Vector3::new(0.1364567, 0.112844266, -1.396806), + Vector3::new(0.12289544, 0.112844266, -1.4223199), + Vector3::new(0.12289544, 0.046329707, -1.438112), + Vector3::new(0.1448506, 0.046329703, -1.396806), + Vector3::new(0.17575371, 0.0463297, -1.3386655), + Vector3::new(0.1799932, 0.048006106, -1.3302914), + Vector3::new(0.17181085, 0.112844266, -1.3302914), + Vector3::new(0.13645676, 0.112844266, -1.3968059), + Vector3::new(0.14485067, 0.046329703, -1.3968059), + Vector3::new(0.17999326, 0.04800613, -1.3302913), + Vector3::new(0.18941, 0.051729772, -1.3116907), + Vector3::new(0.18941, 0.07574098, -1.3059899), + Vector3::new(0.17293426, 0.112844266, -1.3281778), + Vector3::new(0.17181091, 0.112844266, -1.3302913), + Vector3::new(0.18941, 0.07574098, -1.3059899), + Vector3::new(0.18941, 0.112844266, -1.2902151), + Vector3::new(0.17293426, 0.112844266, -1.3281778), + Vector3::new(0.18529011, 0.17935875, -1.7400687), + Vector3::new(0.18941, 0.17935875, -1.7406108), + Vector3::new(0.18941, 0.16201851, -1.7400773), + Vector3::new(0.18529011, 0.17935875, -1.7400687), + Vector3::new(0.18941, 0.16201851, -1.7400773), + Vector3::new(0.18941, 0.13526876, -1.738734), + Vector3::new(0.14866325, 0.12217409, -1.7293788), + Vector3::new(0.13520984, 0.17935875, -1.7293788), + Vector3::new(0.13520929, 0.17935875, -1.7293787), + Vector3::new(0.14866273, 0.12217393, -1.7293787), + Vector3::new(0.12289544, 0.113893166, -1.7234627), + Vector3::new(0.12289544, 0.17935875, -1.7267503), + Vector3::new(0.12289542, 0.11284423, -1.6628642), + Vector3::new(0.18941003, 0.11284423, -1.6628642), + Vector3::new(0.18941003, 0.17935872, -1.6628642), + Vector3::new(0.12289542, 0.17935872, -1.6628642), + Vector3::new(0.12289542, 0.11284423, -1.5963496), + Vector3::new(0.18941003, 0.11284423, -1.5963496), + Vector3::new(0.18941003, 0.17935872, -1.5963496), + Vector3::new(0.12289542, 0.17935872, -1.5963496), + Vector3::new(0.12289542, 0.11284423, -1.5963496), + Vector3::new(0.18941003, 0.11284423, -1.5963496), + Vector3::new(0.18941003, 0.17935872, -1.5963496), + Vector3::new(0.12289542, 0.17935872, -1.5963496), + Vector3::new(0.12289542, 0.11284423, -1.529835), + Vector3::new(0.18941003, 0.11284423, -1.529835), + Vector3::new(0.18941003, 0.17935872, -1.529835), + Vector3::new(0.12289542, 0.17935872, -1.529835), + Vector3::new(0.12289542, 0.11284423, -1.529835), + Vector3::new(0.18941003, 0.11284423, -1.529835), + Vector3::new(0.18941003, 0.17935872, -1.529835), + Vector3::new(0.12289542, 0.17935872, -1.529835), + Vector3::new(0.12289542, 0.11284423, -1.4633205), + Vector3::new(0.18941003, 0.11284423, -1.4633205), + Vector3::new(0.18941003, 0.17935872, -1.4633205), + Vector3::new(0.12289542, 0.17935872, -1.4633205), + Vector3::new(0.12289544, 0.17935875, -1.4065279), + Vector3::new(0.12289544, 0.11284419, -1.42232), + Vector3::new(0.13645676, 0.11284419, -1.396806), + Vector3::new(0.12806286, 0.17935875, -1.396806), + Vector3::new(0.12806292, 0.17935875, -1.3968059), + Vector3::new(0.13645682, 0.11284419, -1.3968059), + Vector3::new(0.17181088, 0.11284419, -1.3302914), + Vector3::new(0.17136484, 0.116378576, -1.3302914), + Vector3::new(0.14339846, 0.17935875, -1.3679539), + Vector3::new(0.15974389, 0.17935875, -1.3302914), + Vector3::new(0.14339845, 0.17935875, -1.3679539), + Vector3::new(0.17136484, 0.116378576, -1.3302914), + Vector3::new(0.17136493, 0.116378374, -1.3302913), + Vector3::new(0.17181094, 0.11284419, -1.3302913), + Vector3::new(0.1729343, 0.11284419, -1.3281778), + Vector3::new(0.18941, 0.11284419, -1.2902151), + Vector3::new(0.18941, 0.175029, -1.2637768), + Vector3::new(0.18861109, 0.17935875, -1.2637768), + Vector3::new(0.15974393, 0.17935875, -1.3302913), + Vector3::new(0.17136493, 0.116378374, -1.3302913), + Vector3::new(0.1729343, 0.11284419, -1.3281778), + Vector3::new(0.18941, 0.175029, -1.2637768), + Vector3::new(0.18941, 0.17935875, -1.261936), + Vector3::new(0.18861109, 0.17935875, -1.2637768), + Vector3::new(0.24324219, -0.21341467, -1.7958934), + Vector3::new(0.24456964, -0.2197285, -1.7970297), + Vector3::new(0.18941, -0.2197285, -1.8267574), + Vector3::new(0.18941, -0.19789436, -1.820354), + Vector3::new(0.19439445, -0.15321395, -1.8045638), + Vector3::new(0.21048221, -0.15321395, -1.7958934), + Vector3::new(0.2504267, -0.2197285, -1.7958934), + Vector3::new(0.24456964, -0.2197285, -1.7970297), + Vector3::new(0.24324219, -0.21341465, -1.7958934), + Vector3::new(0.18941, -0.19789436, -1.820354), + Vector3::new(0.18941, -0.15321395, -1.8030725), + Vector3::new(0.19439445, -0.15321395, -1.8045638), + Vector3::new(0.24324206, -0.213414, -1.7958933), + Vector3::new(0.21048243, -0.15321395, -1.7958933), + Vector3::new(0.23058528, -0.15321395, -1.7850591), + Vector3::new(0.25592455, -0.15321395, -1.7801431), + Vector3::new(0.25592455, -0.21021768, -1.7927272), + Vector3::new(0.25472918, -0.2197285, -1.7950587), + Vector3::new(0.25042734, -0.2197285, -1.7958933), + Vector3::new(0.24324206, -0.213414, -1.7958933), + Vector3::new(0.23058528, -0.15321395, -1.7850591), + Vector3::new(0.25592455, -0.21021768, -1.7927272), + Vector3::new(0.25592455, -0.2197285, -1.7938863), + Vector3::new(0.25472918, -0.2197285, -1.7950587), + Vector3::new(0.18941003, -0.21972859, -1.7293787), + Vector3::new(0.25592458, -0.21972859, -1.7293787), + Vector3::new(0.25592458, -0.15321398, -1.7293787), + Vector3::new(0.18941003, -0.15321398, -1.7293787), + Vector3::new(0.18941003, -0.21972859, -1.6628642), + Vector3::new(0.25592458, -0.21972859, -1.6628642), + Vector3::new(0.25592458, -0.15321398, -1.6628642), + Vector3::new(0.18941003, -0.15321398, -1.6628642), + Vector3::new(0.18941003, -0.21972859, -1.6628642), + Vector3::new(0.25592458, -0.21972859, -1.6628642), + Vector3::new(0.25592458, -0.15321398, -1.6628642), + Vector3::new(0.18941003, -0.15321398, -1.6628642), + Vector3::new(0.18941003, -0.21972859, -1.5963496), + Vector3::new(0.25592458, -0.21972859, -1.5963496), + Vector3::new(0.25592458, -0.15321398, -1.5963496), + Vector3::new(0.18941003, -0.15321398, -1.5963496), + Vector3::new(0.18941003, -0.21972859, -1.5963496), + Vector3::new(0.25592458, -0.21972859, -1.5963496), + Vector3::new(0.25592458, -0.15321398, -1.5963496), + Vector3::new(0.18941003, -0.15321398, -1.5963496), + Vector3::new(0.18941003, -0.21972859, -1.529835), + Vector3::new(0.25592458, -0.21972859, -1.529835), + Vector3::new(0.25592458, -0.15321398, -1.529835), + Vector3::new(0.18941003, -0.15321398, -1.529835), + Vector3::new(0.18941003, -0.21972859, -1.529835), + Vector3::new(0.25592458, -0.21972859, -1.529835), + Vector3::new(0.25592458, -0.15321398, -1.529835), + Vector3::new(0.18941003, -0.15321398, -1.529835), + Vector3::new(0.18941003, -0.21972859, -1.4633205), + Vector3::new(0.25592458, -0.21972859, -1.4633205), + Vector3::new(0.25592458, -0.15321398, -1.4633205), + Vector3::new(0.18941003, -0.15321398, -1.4633205), + Vector3::new(0.247539, -0.21914232, -1.396806), + Vector3::new(0.24743457, -0.2197285, -1.3972942), + Vector3::new(0.25583023, -0.2197285, -1.396806), + Vector3::new(0.23635623, -0.20481087, -1.396806), + Vector3::new(0.23121047, -0.2197285, -1.411391), + Vector3::new(0.24743459, -0.2197285, -1.3972942), + Vector3::new(0.24753901, -0.21914232, -1.396806), + Vector3::new(0.23121047, -0.2197285, -1.411391), + Vector3::new(0.23635623, -0.20481087, -1.396806), + Vector3::new(0.20837888, -0.15321395, -1.396806), + Vector3::new(0.18941, -0.15321395, -1.4177105), + Vector3::new(0.18941, -0.2197285, -1.457457), + Vector3::new(0.24753903, -0.21914217, -1.3968059), + Vector3::new(0.25583228, -0.2197285, -1.3968059), + Vector3::new(0.25592455, -0.2197285, -1.3968005), + Vector3::new(0.25592455, -0.17207375, -1.3576081), + Vector3::new(0.25592455, -0.15321395, -1.3448215), + Vector3::new(0.2541543, -0.15321395, -1.3463595), + Vector3::new(0.23635626, -0.20481075, -1.3968059), + Vector3::new(0.24753904, -0.21914217, -1.3968059), + Vector3::new(0.25592455, -0.17207375, -1.3576081), + Vector3::new(0.23635626, -0.20481075, -1.3968059), + Vector3::new(0.2541543, -0.15321395, -1.3463595), + Vector3::new(0.208379, -0.15321395, -1.3968059), + Vector3::new(0.21048245, -0.15321401, -1.7958934), + Vector3::new(0.19439445, -0.15321401, -1.8045639), + Vector3::new(0.19713144, -0.1286798, -1.7958934), + Vector3::new(0.18941, -0.15321401, -1.8030725), + Vector3::new(0.18941, -0.13465305, -1.7958934), + Vector3::new(0.19713144, -0.1286798, -1.7958934), + Vector3::new(0.19439445, -0.15321401, -1.8045639), + Vector3::new(0.20181468, -0.08669945, -1.7810575), + Vector3::new(0.21660092, -0.08669945, -1.7730886), + Vector3::new(0.23058529, -0.15321401, -1.7850592), + Vector3::new(0.21048266, -0.15321401, -1.7958933), + Vector3::new(0.19713148, -0.12867945, -1.7958933), + Vector3::new(0.21660092, -0.08669945, -1.7730886), + Vector3::new(0.25592455, -0.086699456, -1.7654594), + Vector3::new(0.25592455, -0.15321401, -1.7801431), + Vector3::new(0.23058529, -0.15321401, -1.7850592), + Vector3::new(0.18941, -0.13465275, -1.7958933), + Vector3::new(0.18941, -0.08669945, -1.7773459), + Vector3::new(0.20181468, -0.08669945, -1.7810575), + Vector3::new(0.19713148, -0.12867945, -1.7958933), + Vector3::new(0.18941, -0.15321401, -1.4177105), + Vector3::new(0.20837891, -0.15321401, -1.396806), + Vector3::new(0.19353071, -0.12583038, -1.396806), + Vector3::new(0.18941, -0.12602565, -1.4014639), + Vector3::new(0.25592455, -0.15321401, -1.3448215), + Vector3::new(0.25592455, -0.14808194, -1.341342), + Vector3::new(0.25415426, -0.15321401, -1.3463596), + Vector3::new(0.20837902, -0.15321401, -1.3968059), + Vector3::new(0.25415426, -0.15321401, -1.3463596), + Vector3::new(0.25592455, -0.14808196, -1.341342), + Vector3::new(0.25592455, -0.12958905, -1.3302914), + Vector3::new(0.25237462, -0.123042084, -1.3302914), + Vector3::new(0.19353081, -0.12583038, -1.3968059), + Vector3::new(0.25592455, -0.09874706, -1.3132844), + Vector3::new(0.25592455, -0.08669945, -1.3077292), + Vector3::new(0.25143242, -0.08669944, -1.3117589), + Vector3::new(0.2523747, -0.123042084, -1.3302913), + Vector3::new(0.25592455, -0.12958884, -1.3302913), + Vector3::new(0.25592455, -0.12287387, -1.3262787), + Vector3::new(0.20181468, -0.08669941, -1.7810574), + Vector3::new(0.20694056, -0.04075135, -1.7648193), + Vector3::new(0.21660092, -0.086699404, -1.7730886), + Vector3::new(0.18941, -0.020184837, -1.7588685), + Vector3::new(0.19448435, -0.020184837, -1.7589756), + Vector3::new(0.20694056, -0.04075135, -1.7648193), + Vector3::new(0.18941, -0.082980156, -1.7759074), + Vector3::new(0.22242229, -0.020184837, -1.7572755), + Vector3::new(0.25592455, -0.020184837, -1.7507757), + Vector3::new(0.25592455, -0.08669941, -1.7654594), + Vector3::new(0.21660091, -0.0866994, -1.7730886), + Vector3::new(0.20694056, -0.04075135, -1.7648193), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.20181468, -0.086699404, -1.7810574), + Vector3::new(0.18941, -0.086699404, -1.7773459), + Vector3::new(0.18941, -0.082980156, -1.7759074), + Vector3::new(0.20694056, -0.04075135, -1.7648193), + Vector3::new(0.25592455, -0.020184837, -1.2770597), + Vector3::new(0.22663139, -0.020184834, -1.3033363), + Vector3::new(0.2514324, -0.0866994, -1.3117589), + Vector3::new(0.25592455, -0.086699404, -1.3077292), + Vector3::new(0.18941, -0.020184793, -1.7588685), + Vector3::new(0.18941, -0.011806555, -1.7565951), + Vector3::new(0.19448432, -0.020184793, -1.7589756), + Vector3::new(0.22242233, -0.020184793, -1.7572755), + Vector3::new(0.25592455, 0.02432083, -1.7409507), + Vector3::new(0.25592455, -0.020184793, -1.7507757), + Vector3::new(0.23654184, 0.046329774, -1.2637768), + Vector3::new(0.20183037, 0.046329774, -1.2949136), + Vector3::new(0.22663137, -0.020184789, -1.3033363), + Vector3::new(0.25592455, -0.020184793, -1.2770597), + Vector3::new(0.25592455, 0.008622458, -1.2637768), + Vector3::new(0.23654184, 0.046329774, -1.2637768), + Vector3::new(0.25592455, 0.008622458, -1.2637768), + Vector3::new(0.25592455, 0.046329774, -1.2463901), + Vector3::new(0.25592455, 0.09559846, -1.7297466), + Vector3::new(0.23077853, 0.112844266, -1.7340378), + Vector3::new(0.25592455, 0.112844266, -1.729814), + Vector3::new(0.19847965, 0.05531616, -1.2937757), + Vector3::new(0.21850373, 0.081421204, -1.2637768), + Vector3::new(0.20192681, 0.10719351, -1.2637768), + Vector3::new(0.21850373, 0.081421204, -1.2637768), + Vector3::new(0.19847965, 0.05531616, -1.2937757), + Vector3::new(0.2018304, 0.0463297, -1.2949136), + Vector3::new(0.23654194, 0.046329696, -1.2637768), + Vector3::new(0.18941, 0.07574098, -1.3059899), + Vector3::new(0.18941, 0.05172977, -1.3116907), + Vector3::new(0.19847965, 0.05531616, -1.2937757), + Vector3::new(0.19847965, 0.05531616, -1.2937757), + Vector3::new(0.20192681, 0.1071935, -1.2637768), + Vector3::new(0.20088415, 0.112844266, -1.2637768), + Vector3::new(0.18941, 0.11284426, -1.2902151), + Vector3::new(0.18941, 0.07574096, -1.3059899), + Vector3::new(0.21850373, 0.081421204, -1.2637768), + Vector3::new(0.24260704, 0.112844266, -1.2276666), + Vector3::new(0.20230229, 0.11284427, -1.2605091), + Vector3::new(0.20192681, 0.10719351, -1.2637768), + Vector3::new(0.21850373, 0.081421204, -1.2637768), + Vector3::new(0.23654194, 0.046329696, -1.2637768), + Vector3::new(0.25592455, 0.046329692, -1.2463902), + Vector3::new(0.25592455, 0.112844266, -1.2157207), + Vector3::new(0.24260704, 0.112844266, -1.2276666), + Vector3::new(0.20192681, 0.1071935, -1.2637768), + Vector3::new(0.20230229, 0.112844266, -1.2605091), + Vector3::new(0.20088415, 0.112844266, -1.2637768), + Vector3::new(0.18941, 0.16201851, -1.7400773), + Vector3::new(0.18941, 0.17935875, -1.7406108), + Vector3::new(0.19153188, 0.17935875, -1.7408901), + Vector3::new(0.19531466, 0.13716632, -1.7400897), + Vector3::new(0.25592455, 0.11284419, -1.729814), + Vector3::new(0.23077863, 0.11284419, -1.7340378), + Vector3::new(0.19531466, 0.13716632, -1.7400897), + Vector3::new(0.19153188, 0.17935875, -1.7408901), + Vector3::new(0.25592455, 0.17935875, -1.7300744), + Vector3::new(0.18941, 0.13526876, -1.738734), + Vector3::new(0.18941, 0.16201851, -1.7400773), + Vector3::new(0.19531466, 0.13716632, -1.7400897), + Vector3::new(0.18941003, 0.11284423, -1.7293787), + Vector3::new(0.25592458, 0.11284423, -1.7293787), + Vector3::new(0.25592458, 0.17935872, -1.7293787), + Vector3::new(0.18941003, 0.17935872, -1.7293787), + Vector3::new(0.18941003, 0.11284423, -1.6628642), + Vector3::new(0.25592458, 0.11284423, -1.6628642), + Vector3::new(0.25592458, 0.17935872, -1.6628642), + Vector3::new(0.18941003, 0.17935872, -1.6628642), + Vector3::new(0.18941003, 0.11284423, -1.6628642), + Vector3::new(0.25592458, 0.11284423, -1.6628642), + Vector3::new(0.25592458, 0.17935872, -1.6628642), + Vector3::new(0.18941003, 0.17935872, -1.6628642), + Vector3::new(0.18941003, 0.11284423, -1.5963496), + Vector3::new(0.25592458, 0.11284423, -1.5963496), + Vector3::new(0.25592458, 0.17935872, -1.5963496), + Vector3::new(0.18941003, 0.17935872, -1.5963496), + Vector3::new(0.18941003, 0.11284423, -1.5963496), + Vector3::new(0.25592458, 0.11284423, -1.5963496), + Vector3::new(0.25592458, 0.17935872, -1.5963496), + Vector3::new(0.18941003, 0.17935872, -1.5963496), + Vector3::new(0.18941003, 0.11284423, -1.529835), + Vector3::new(0.25592458, 0.11284423, -1.529835), + Vector3::new(0.25592458, 0.17935872, -1.529835), + Vector3::new(0.18941003, 0.17935872, -1.529835), + Vector3::new(0.18941003, 0.11284423, -1.529835), + Vector3::new(0.25592458, 0.11284423, -1.529835), + Vector3::new(0.25592458, 0.17935872, -1.529835), + Vector3::new(0.18941003, 0.17935872, -1.529835), + Vector3::new(0.18941003, 0.11284423, -1.4633205), + Vector3::new(0.25592458, 0.11284423, -1.4633205), + Vector3::new(0.25592458, 0.17935872, -1.4633205), + Vector3::new(0.18941003, 0.17935872, -1.4633205), + Vector3::new(0.18941003, 0.11284423, -1.4633205), + Vector3::new(0.25592458, 0.11284423, -1.4633205), + Vector3::new(0.25592458, 0.17935872, -1.4633205), + Vector3::new(0.18941003, 0.17935872, -1.4633205), + Vector3::new(0.18941003, 0.11284423, -1.396806), + Vector3::new(0.25592458, 0.11284423, -1.396806), + Vector3::new(0.25592458, 0.17935872, -1.396806), + Vector3::new(0.18941003, 0.17935872, -1.396806), + Vector3::new(0.18941003, 0.11284423, -1.396806), + Vector3::new(0.25592458, 0.11284423, -1.396806), + Vector3::new(0.25592458, 0.17935872, -1.396806), + Vector3::new(0.18941003, 0.17935872, -1.396806), + Vector3::new(0.18941003, 0.11284423, -1.3302914), + Vector3::new(0.25592458, 0.11284423, -1.3302914), + Vector3::new(0.25592458, 0.17935872, -1.3302914), + Vector3::new(0.18941003, 0.17935872, -1.3302914), + Vector3::new(0.18941, 0.175029, -1.2637768), + Vector3::new(0.18941, 0.1128442, -1.2902151), + Vector3::new(0.20088415, 0.1128442, -1.2637768), + Vector3::new(0.25592455, 0.13020608, -1.2077152), + Vector3::new(0.25592455, 0.15014967, -1.1972623), + Vector3::new(0.23713702, 0.17935875, -1.1972623), + Vector3::new(0.20672207, 0.17935875, -1.222046), + Vector3::new(0.20230229, 0.112844184, -1.2605091), + Vector3::new(0.24260698, 0.11284419, -1.2276667), + Vector3::new(0.25592455, 0.112844184, -1.2157207), + Vector3::new(0.25592455, 0.13020608, -1.2077152), + Vector3::new(0.24260698, 0.11284419, -1.2276667), + Vector3::new(0.20230229, 0.1128442, -1.2605091), + Vector3::new(0.20672207, 0.17935875, -1.222046), + Vector3::new(0.18941, 0.17935875, -1.261936), + Vector3::new(0.18941, 0.175029, -1.2637768), + Vector3::new(0.20088415, 0.1128442, -1.2637768), + Vector3::new(0.25592455, 0.15014991, -1.1972622), + Vector3::new(0.25592455, 0.17935875, -1.1819532), + Vector3::new(0.23713715, 0.17935875, -1.1972622), + Vector3::new(0.2559246, -0.21021721, -1.7927271), + Vector3::new(0.2559246, -0.15321395, -1.7801431), + Vector3::new(0.2630891, -0.15321395, -1.7787532), + Vector3::new(0.28173494, -0.15321395, -1.7604647), + Vector3::new(0.2788983, -0.2197285, -1.7713528), + Vector3::new(0.2559246, -0.2197285, -1.7938862), + Vector3::new(0.2559246, -0.21021721, -1.7927271), + Vector3::new(0.2630891, -0.15321395, -1.7787532), + Vector3::new(0.28173494, -0.15321395, -1.7604647), + Vector3::new(0.30298996, -0.15321395, -1.7469393), + Vector3::new(0.28517705, -0.2197285, -1.7673575), + Vector3::new(0.2788983, -0.2197285, -1.7713529), + Vector3::new(0.28517705, -0.2197285, -1.7673575), + Vector3::new(0.30298996, -0.15321395, -1.7469393), + Vector3::new(0.3175517, -0.15321395, -1.7293788), + Vector3::new(0.31705132, -0.19096869, -1.7293788), + Vector3::new(0.29707673, -0.2197285, -1.7530072), + Vector3::new(0.31142703, -0.2197285, -1.7293788), + Vector3::new(0.29707673, -0.2197285, -1.7530072), + Vector3::new(0.31705132, -0.19096869, -1.7293788), + Vector3::new(0.3170514, -0.19096854, -1.7293787), + Vector3::new(0.3175518, -0.15321395, -1.7293787), + Vector3::new(0.32243916, -0.15321395, -1.7234849), + Vector3::new(0.32243916, -0.18321115, -1.7230054), + Vector3::new(0.32243916, -0.18321115, -1.7230054), + Vector3::new(0.32243916, -0.2197285, -1.7112468), + Vector3::new(0.3114271, -0.2197285, -1.7293787), + Vector3::new(0.3170514, -0.19096854, -1.7293787), + Vector3::new(0.25592458, -0.21972859, -1.6628642), + Vector3::new(0.32243913, -0.21972859, -1.6628642), + Vector3::new(0.32243913, -0.15321398, -1.6628642), + Vector3::new(0.25592458, -0.15321398, -1.6628642), + Vector3::new(0.25592458, -0.21972859, -1.5963496), + Vector3::new(0.32243913, -0.21972859, -1.5963496), + Vector3::new(0.32243913, -0.15321398, -1.5963496), + Vector3::new(0.25592458, -0.15321398, -1.5963496), + Vector3::new(0.25592458, -0.21972859, -1.5963496), + Vector3::new(0.32243913, -0.21972859, -1.5963496), + Vector3::new(0.32243913, -0.15321398, -1.5963496), + Vector3::new(0.25592458, -0.15321398, -1.5963496), + Vector3::new(0.25592458, -0.21972859, -1.529835), + Vector3::new(0.32243913, -0.21972859, -1.529835), + Vector3::new(0.32243913, -0.15321398, -1.529835), + Vector3::new(0.25592458, -0.15321398, -1.529835), + Vector3::new(0.25592458, -0.21972859, -1.529835), + Vector3::new(0.32243913, -0.21972859, -1.529835), + Vector3::new(0.32243913, -0.15321398, -1.529835), + Vector3::new(0.25592458, -0.15321398, -1.529835), + Vector3::new(0.25592458, -0.21972859, -1.4633205), + Vector3::new(0.32243913, -0.21972859, -1.4633205), + Vector3::new(0.32243913, -0.15321398, -1.4633205), + Vector3::new(0.25592458, -0.15321398, -1.4633205), + Vector3::new(0.32243916, -0.2197285, -1.4047158), + Vector3::new(0.32243916, -0.21268144, -1.396806), + Vector3::new(0.31990728, -0.21465531, -1.396806), + Vector3::new(0.31870013, -0.2197285, -1.401444), + Vector3::new(0.30893937, -0.2197285, -1.396806), + Vector3::new(0.31870013, -0.2197285, -1.4014438), + Vector3::new(0.31990725, -0.21465544, -1.396806), + Vector3::new(0.32243916, -0.21268134, -1.3968059), + Vector3::new(0.32243916, -0.20401484, -1.3870784), + Vector3::new(0.3199073, -0.21465518, -1.3968059), + Vector3::new(0.32243916, -0.16951019, -1.3516326), + Vector3::new(0.32243916, -0.15321395, -1.3382301), + Vector3::new(0.25928453, -0.15321395, -1.341902), + Vector3::new(0.2559246, -0.17207341, -1.3576078), + Vector3::new(0.2559246, -0.2197285, -1.3968005), + Vector3::new(0.3031494, -0.2197285, -1.3940549), + Vector3::new(0.32243916, -0.20401485, -1.3870783), + Vector3::new(0.32243916, -0.16951019, -1.3516326), + Vector3::new(0.3031494, -0.2197285, -1.3940549), + Vector3::new(0.30893913, -0.2197285, -1.3968059), + Vector3::new(0.31990728, -0.21465531, -1.3968059), + Vector3::new(0.2559246, -0.17207342, -1.3576078), + Vector3::new(0.25928453, -0.15321395, -1.341902), + Vector3::new(0.2559246, -0.15321395, -1.3448215), + Vector3::new(0.2559246, -0.08669945, -1.7654594), + Vector3::new(0.27144903, -0.086699456, -1.7624476), + Vector3::new(0.2630891, -0.15321401, -1.7787532), + Vector3::new(0.2559246, -0.15321401, -1.7801431), + Vector3::new(0.27144903, -0.08669945, -1.7624476), + Vector3::new(0.2845716, -0.086699456, -1.7495764), + Vector3::new(0.28173494, -0.15321401, -1.7604647), + Vector3::new(0.2630891, -0.15321401, -1.7787532), + Vector3::new(0.2845716, -0.08669945, -1.7495764), + Vector3::new(0.31631216, -0.08669945, -1.7293788), + Vector3::new(0.31830987, -0.09600836, -1.7293788), + Vector3::new(0.30298993, -0.15321401, -1.7469393), + Vector3::new(0.28173494, -0.15321401, -1.7604647), + Vector3::new(0.3175517, -0.15321401, -1.7293788), + Vector3::new(0.30298993, -0.15321401, -1.7469393), + Vector3::new(0.31830987, -0.09600836, -1.7293788), + Vector3::new(0.31631237, -0.08669945, -1.7293787), + Vector3::new(0.32080284, -0.08669945, -1.7265213), + Vector3::new(0.31830996, -0.096007966, -1.7293787), + Vector3::new(0.32080284, -0.08669945, -1.7265213), + Vector3::new(0.32243916, -0.08669945, -1.724548), + Vector3::new(0.32243916, -0.15321401, -1.7234849), + Vector3::new(0.3175518, -0.15321401, -1.7293787), + Vector3::new(0.31830996, -0.09600797, -1.7293787), + Vector3::new(0.26176837, -0.13927203, -1.3302914), + Vector3::new(0.25928453, -0.15321401, -1.341902), + Vector3::new(0.32243916, -0.15321401, -1.3382303), + Vector3::new(0.32243916, -0.14356102, -1.3302914), + Vector3::new(0.2559246, -0.15321401, -1.3448215), + Vector3::new(0.25928453, -0.15321401, -1.341902), + Vector3::new(0.26176837, -0.13927203, -1.3302914), + Vector3::new(0.25982332, -0.13677931, -1.3302914), + Vector3::new(0.2559246, -0.14808176, -1.3413419), + Vector3::new(0.2559246, -0.1295892, -1.3302914), + Vector3::new(0.2559246, -0.14808178, -1.3413419), + Vector3::new(0.25982332, -0.13677931, -1.3302914), + Vector3::new(0.32243916, -0.11582225, -1.3074783), + Vector3::new(0.26476443, -0.12245499, -1.3162864), + Vector3::new(0.2617684, -0.1392719, -1.3302913), + Vector3::new(0.32243916, -0.14356089, -1.3302913), + Vector3::new(0.34401476, -0.113340996, -1.3041832), + Vector3::new(0.3887098, 0.049111772, -1.1970468), + Vector3::new(0.26476443, -0.12245499, -1.3162864), + Vector3::new(0.29059538, -0.08669945, -1.2914362), + Vector3::new(0.27026275, -0.086699456, -1.2948676), + Vector3::new(0.25982338, -0.13677919, -1.3302913), + Vector3::new(0.2617684, -0.1392719, -1.3302913), + Vector3::new(0.26476443, -0.12245499, -1.3162864), + Vector3::new(0.26476443, -0.12245499, -1.3162864), + Vector3::new(0.27026275, -0.08669945, -1.2948676), + Vector3::new(0.2559246, -0.08669944, -1.3077292), + Vector3::new(0.2559246, -0.09874722, -1.3132844), + Vector3::new(0.26476443, -0.12245499, -1.3162864), + Vector3::new(0.2559246, -0.122873865, -1.3262786), + Vector3::new(0.2559246, -0.12958899, -1.3302913), + Vector3::new(0.25982338, -0.13677919, -1.3302913), + Vector3::new(0.2559246, -0.020184837, -1.7507757), + Vector3::new(0.27980897, -0.020184837, -1.7461419), + Vector3::new(0.27144903, -0.0866994, -1.7624476), + Vector3::new(0.2559246, -0.08669941, -1.7654594), + Vector3::new(0.27980897, -0.020184837, -1.7461419), + Vector3::new(0.2874082, -0.020184837, -1.7386883), + Vector3::new(0.2845716, -0.0866994, -1.7495764), + Vector3::new(0.27144903, -0.086699404, -1.7624476), + Vector3::new(0.3163122, -0.086699404, -1.7293788), + Vector3::new(0.28457156, -0.08669941, -1.7495764), + Vector3::new(0.2874082, -0.020184837, -1.7386883), + Vector3::new(0.30203813, -0.020184837, -1.7293788), + Vector3::new(0.32243916, -0.020184837, -1.7163969), + Vector3::new(0.32243916, -0.080589324, -1.7246456), + Vector3::new(0.32080287, -0.086699404, -1.7265213), + Vector3::new(0.31631237, -0.086699404, -1.7293787), + Vector3::new(0.3020383, -0.020184837, -1.7293787), + Vector3::new(0.32080287, -0.08669941, -1.7265213), + Vector3::new(0.32243916, -0.08058934, -1.7246456), + Vector3::new(0.32243916, -0.086699404, -1.724548), + Vector3::new(0.27824393, -0.034797788, -1.2637768), + Vector3::new(0.27026275, -0.0866994, -1.2948676), + Vector3::new(0.2905954, -0.086699404, -1.2914362), + Vector3::new(0.3193464, -0.04690194, -1.2637768), + Vector3::new(0.27073243, -0.020184834, -1.2637768), + Vector3::new(0.2559246, -0.02018483, -1.2770597), + Vector3::new(0.2559246, -0.08669941, -1.3077292), + Vector3::new(0.27026275, -0.086699404, -1.2948676), + Vector3::new(0.27824393, -0.034797784, -1.2637768), + Vector3::new(0.32243916, -0.042620897, -1.2608014), + Vector3::new(0.32243916, -0.020184837, -1.2479438), + Vector3::new(0.28049102, -0.020184845, -1.2550231), + Vector3::new(0.27824393, -0.034797788, -1.2637768), + Vector3::new(0.3193464, -0.04690194, -1.2637768), + Vector3::new(0.27073243, -0.020184834, -1.2637768), + Vector3::new(0.27824393, -0.034797784, -1.2637768), + Vector3::new(0.28049102, -0.020184837, -1.2550231), + Vector3::new(0.2724921, 0.046329774, -1.7328777), + Vector3::new(0.28816888, 0.046329774, -1.7298363), + Vector3::new(0.27980897, -0.020184793, -1.7461419), + Vector3::new(0.2559246, -0.020184785, -1.7507757), + Vector3::new(0.2559246, 0.024320912, -1.7409506), + Vector3::new(0.28816888, 0.046329774, -1.7298363), + Vector3::new(0.28863534, 0.046329774, -1.7293788), + Vector3::new(0.28983358, 0.03668579, -1.7293788), + Vector3::new(0.2874082, -0.020184793, -1.7386883), + Vector3::new(0.27980897, -0.020184785, -1.7461419), + Vector3::new(0.30203813, -0.020184796, -1.7293788), + Vector3::new(0.2874082, -0.0201848, -1.7386883), + Vector3::new(0.28983358, 0.03668579, -1.7293788), + Vector3::new(0.28863546, 0.046329774, -1.7293787), + Vector3::new(0.29024485, 0.046329774, -1.7278001), + Vector3::new(0.2898336, 0.03668652, -1.7293787), + Vector3::new(0.29024485, 0.046329774, -1.7278001), + Vector3::new(0.32243916, 0.046329774, -1.7073138), + Vector3::new(0.32243916, -0.020184793, -1.7163969), + Vector3::new(0.3020383, -0.020184796, -1.7293787), + Vector3::new(0.2898336, 0.036686517, -1.7293787), + Vector3::new(0.2559246, 0.008622456, -1.2637768), + Vector3::new(0.2559246, -0.0201848, -1.2770597), + Vector3::new(0.27073243, -0.020184796, -1.2637768), + Vector3::new(0.32243916, -0.020184793, -1.2479438), + Vector3::new(0.32243916, 0.046329774, -1.2098254), + Vector3::new(0.2907193, 0.046329767, -1.2151786), + Vector3::new(0.28049102, -0.0201848, -1.2550231), + Vector3::new(0.2907193, 0.046329774, -1.2151786), + Vector3::new(0.2559246, 0.04632978, -1.2463901), + Vector3::new(0.2559246, 0.008622456, -1.2637768), + Vector3::new(0.27073243, -0.020184796, -1.2637768), + Vector3::new(0.28049102, -0.020184793, -1.2550231), + Vector3::new(0.2559246, 0.112844266, -1.7298139), + Vector3::new(0.25851518, 0.112844266, -1.7293788), + Vector3::new(0.258079, 0.0941209, -1.7293788), + Vector3::new(0.2559246, 0.09559842, -1.7297465), + Vector3::new(0.27249205, 0.0463297, -1.7328777), + Vector3::new(0.27967268, 0.055868708, -1.7293788), + Vector3::new(0.28840345, 0.048196055, -1.7293788), + Vector3::new(0.28816888, 0.0463297, -1.7298363), + Vector3::new(0.28816888, 0.046329707, -1.7298363), + Vector3::new(0.28840345, 0.04819606, -1.7293788), + Vector3::new(0.28863534, 0.046329707, -1.7293788), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.2998039, 0.112844266, -1.717168), + Vector3::new(0.32243916, 0.112844266, -1.7004257), + Vector3::new(0.32243916, 0.09600361, -1.7005304), + Vector3::new(0.2585159, 0.112844266, -1.7293787), + Vector3::new(0.2798679, 0.112844266, -1.7257924), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.25807968, 0.09412042, -1.7293787), + Vector3::new(0.2798679, 0.112844266, -1.7257924), + Vector3::new(0.29007727, 0.11284426, -1.7226921), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.29007727, 0.112844266, -1.7226921), + Vector3::new(0.2998039, 0.112844266, -1.717168), + Vector3::new(0.27967292, 0.055869035, -1.7293787), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.2884035, 0.04819654, -1.7293787), + Vector3::new(0.2884035, 0.048196547, -1.7293787), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.29024485, 0.0463297, -1.7278001), + Vector3::new(0.28863546, 0.046329707, -1.7293787), + Vector3::new(0.291311, 0.071329504, -1.7237078), + Vector3::new(0.32243916, 0.09600361, -1.7005304), + Vector3::new(0.32243916, 0.0463297, -1.7073138), + Vector3::new(0.29024485, 0.046329692, -1.7278001), + Vector3::new(0.32243916, 0.0463297, -1.2098254), + Vector3::new(0.32243916, 0.06825167, -1.1972623), + Vector3::new(0.2953185, 0.07623838, -1.1972623), + Vector3::new(0.2907193, 0.046329707, -1.2151786), + Vector3::new(0.27650195, 0.112844266, -1.1972623), + Vector3::new(0.2559246, 0.112844266, -1.2157205), + Vector3::new(0.2559246, 0.0463297, -1.2463901), + Vector3::new(0.2907193, 0.0463297, -1.2151786), + Vector3::new(0.2953185, 0.07623837, -1.1972623), + Vector3::new(0.32243916, 0.06825188, -1.1972622), + Vector3::new(0.32243916, 0.112844266, -1.171707), + Vector3::new(0.30094758, 0.112844266, -1.1753341), + Vector3::new(0.29531854, 0.07623857, -1.1972622), + Vector3::new(0.27650207, 0.112844266, -1.1972622), + Vector3::new(0.29531854, 0.07623857, -1.1972622), + Vector3::new(0.30094758, 0.112844266, -1.1753341), + Vector3::new(0.2559246, 0.17935875, -1.7300744), + Vector3::new(0.26006553, 0.17935875, -1.7293788), + Vector3::new(0.25851518, 0.11284419, -1.7293788), + Vector3::new(0.2559246, 0.11284419, -1.7298139), + Vector3::new(0.31341112, 0.17935875, -1.7066901), + Vector3::new(0.32243916, 0.17935875, -1.7000124), + Vector3::new(0.32243916, 0.11284419, -1.7004257), + Vector3::new(0.29980388, 0.11284419, -1.7171681), + Vector3::new(0.26006624, 0.17935875, -1.7293787), + Vector3::new(0.26153383, 0.17935875, -1.7291322), + Vector3::new(0.2798679, 0.11284419, -1.7257924), + Vector3::new(0.2585159, 0.11284419, -1.7293787), + Vector3::new(0.28810057, 0.17935875, -1.7210648), + Vector3::new(0.29007727, 0.1128442, -1.7226921), + Vector3::new(0.2798679, 0.11284419, -1.7257924), + Vector3::new(0.26153383, 0.17935875, -1.7291322), + Vector3::new(0.29007727, 0.11284419, -1.7226921), + Vector3::new(0.28810057, 0.17935875, -1.7210648), + Vector3::new(0.31341112, 0.17935875, -1.7066901), + Vector3::new(0.29980388, 0.1128442, -1.7171681), + Vector3::new(0.25592458, 0.11284423, -1.6628642), + Vector3::new(0.32243913, 0.11284423, -1.6628642), + Vector3::new(0.32243913, 0.17935872, -1.6628642), + Vector3::new(0.25592458, 0.17935872, -1.6628642), + Vector3::new(0.25592458, 0.11284423, -1.5963496), + Vector3::new(0.32243913, 0.11284423, -1.5963496), + Vector3::new(0.32243913, 0.17935872, -1.5963496), + Vector3::new(0.25592458, 0.17935872, -1.5963496), + Vector3::new(0.25592458, 0.11284423, -1.5963496), + Vector3::new(0.32243913, 0.11284423, -1.5963496), + Vector3::new(0.32243913, 0.17935872, -1.5963496), + Vector3::new(0.25592458, 0.17935872, -1.5963496), + Vector3::new(0.25592458, 0.11284423, -1.529835), + Vector3::new(0.32243913, 0.11284423, -1.529835), + Vector3::new(0.32243913, 0.17935872, -1.529835), + Vector3::new(0.25592458, 0.17935872, -1.529835), + Vector3::new(0.25592458, 0.11284423, -1.529835), + Vector3::new(0.32243913, 0.11284423, -1.529835), + Vector3::new(0.32243913, 0.17935872, -1.529835), + Vector3::new(0.25592458, 0.17935872, -1.529835), + Vector3::new(0.25592458, 0.11284423, -1.4633205), + Vector3::new(0.32243913, 0.11284423, -1.4633205), + Vector3::new(0.32243913, 0.17935872, -1.4633205), + Vector3::new(0.25592458, 0.17935872, -1.4633205), + Vector3::new(0.25592458, 0.11284423, -1.4633205), + Vector3::new(0.32243913, 0.11284423, -1.4633205), + Vector3::new(0.32243913, 0.17935872, -1.4633205), + Vector3::new(0.25592458, 0.17935872, -1.4633205), + Vector3::new(0.25592458, 0.11284423, -1.396806), + Vector3::new(0.32243913, 0.11284423, -1.396806), + Vector3::new(0.32243913, 0.17935872, -1.396806), + Vector3::new(0.25592458, 0.17935872, -1.396806), + Vector3::new(0.25592458, 0.11284423, -1.396806), + Vector3::new(0.32243913, 0.11284423, -1.396806), + Vector3::new(0.32243913, 0.17935872, -1.396806), + Vector3::new(0.25592458, 0.17935872, -1.396806), + Vector3::new(0.25592458, 0.11284423, -1.3302914), + Vector3::new(0.32243913, 0.11284423, -1.3302914), + Vector3::new(0.32243913, 0.17935872, -1.3302914), + Vector3::new(0.25592458, 0.17935872, -1.3302914), + Vector3::new(0.25592458, 0.11284423, -1.3302914), + Vector3::new(0.32243913, 0.11284423, -1.3302914), + Vector3::new(0.32243913, 0.17935872, -1.3302914), + Vector3::new(0.25592458, 0.17935872, -1.3302914), + Vector3::new(0.25592458, 0.11284423, -1.2637768), + Vector3::new(0.32243913, 0.11284423, -1.2637768), + Vector3::new(0.32243913, 0.17935872, -1.2637768), + Vector3::new(0.25592458, 0.17935872, -1.2637768), + Vector3::new(0.2559246, 0.13020615, -1.207715), + Vector3::new(0.26290175, 0.13930213, -1.1972623), + Vector3::new(0.2559246, 0.15014958, -1.1972623), + Vector3::new(0.26290175, 0.13930213, -1.1972623), + Vector3::new(0.2559246, 0.13020615, -1.207715), + Vector3::new(0.2559246, 0.11284419, -1.2157205), + Vector3::new(0.276502, 0.11284419, -1.1972623), + Vector3::new(0.26290184, 0.13930224, -1.1972622), + Vector3::new(0.2936275, 0.17935875, -1.1512308), + Vector3::new(0.2559246, 0.17935875, -1.1819532), + Vector3::new(0.2559246, 0.15014982, -1.1972622), + Vector3::new(0.32243916, 0.1128442, -1.1717072), + Vector3::new(0.32243916, 0.17935875, -1.1335888), + Vector3::new(0.31117585, 0.17935875, -1.1354897), + Vector3::new(0.30094758, 0.11284419, -1.1753342), + Vector3::new(0.26290184, 0.13930224, -1.1972622), + Vector3::new(0.27650213, 0.11284419, -1.1972622), + Vector3::new(0.30094758, 0.11284419, -1.1753342), + Vector3::new(0.31117585, 0.17935875, -1.1354897), + Vector3::new(0.2936275, 0.17935875, -1.1512308), + Vector3::new(0.32243916, -0.18321115, -1.7230054), + Vector3::new(0.32243916, -0.15321395, -1.7234849), + Vector3::new(0.34327316, -0.15321395, -1.6983604), + Vector3::new(0.34327316, -0.15321395, -1.6983604), + Vector3::new(0.36483112, -0.15321395, -1.6628642), + Vector3::new(0.35182345, -0.2197285, -1.6628642), + Vector3::new(0.32243916, -0.2197285, -1.7112468), + Vector3::new(0.32243916, -0.18321115, -1.7230054), + Vector3::new(0.38536644, -0.2197285, -1.5963497), + Vector3::new(0.36939064, -0.2197285, -1.633939), + Vector3::new(0.38697082, -0.15321395, -1.6264101), + Vector3::new(0.38895372, -0.15321395, -1.6217446), + Vector3::new(0.38895372, -0.20313594, -1.5963497), + Vector3::new(0.36483112, -0.15321395, -1.6628642), + Vector3::new(0.38697082, -0.15321395, -1.6264101), + Vector3::new(0.36939064, -0.2197285, -1.633939), + Vector3::new(0.35182345, -0.2197285, -1.6628642), + Vector3::new(0.38886338, -0.2197285, -1.5615234), + Vector3::new(0.38895372, -0.21955214, -1.5613409), + Vector3::new(0.38895372, -0.2197285, -1.5610058), + Vector3::new(0.38895372, -0.21955213, -1.5613409), + Vector3::new(0.38886338, -0.2197285, -1.5615236), + Vector3::new(0.38743, -0.2197285, -1.5914943), + Vector3::new(0.38895372, -0.21664403, -1.5894783), + Vector3::new(0.38895372, -0.21664403, -1.5894783), + Vector3::new(0.38743, -0.2197285, -1.5914943), + Vector3::new(0.38536647, -0.2197285, -1.5963496), + Vector3::new(0.38895372, -0.20313618, -1.5963496), + Vector3::new(0.38432705, -0.2197285, -1.4633205), + Vector3::new(0.38523936, -0.2197285, -1.4643732), + Vector3::new(0.38572723, -0.21845657, -1.4633205), + Vector3::new(0.38895372, -0.2197285, -1.4771357), + Vector3::new(0.38895372, -0.21328956, -1.4633205), + Vector3::new(0.38572723, -0.21845657, -1.4633205), + Vector3::new(0.38523936, -0.2197285, -1.4643732), + Vector3::new(0.38895372, -0.21004458, -1.4563582), + Vector3::new(0.38895372, -0.18089001, -1.419324), + Vector3::new(0.3683665, -0.2197285, -1.4449029), + Vector3::new(0.38432705, -0.2197285, -1.4633205), + Vector3::new(0.38572723, -0.21845657, -1.4633205), + Vector3::new(0.32243916, -0.21268144, -1.396806), + Vector3::new(0.32243916, -0.2197285, -1.4047158), + Vector3::new(0.3683665, -0.2197285, -1.4449029), + Vector3::new(0.38895372, -0.18089001, -1.419324), + Vector3::new(0.38895372, -0.16082796, -1.396806), + Vector3::new(0.38895372, -0.21328956, -1.4633205), + Vector3::new(0.38895372, -0.2100446, -1.4563582), + Vector3::new(0.38572723, -0.21845657, -1.4633205), + Vector3::new(0.32243916, -0.21268134, -1.3968059), + Vector3::new(0.38895372, -0.16082785, -1.3968059), + Vector3::new(0.38895372, -0.15321395, -1.3882599), + Vector3::new(0.3345271, -0.15321395, -1.3406355), + Vector3::new(0.32243916, -0.20401484, -1.3870784), + Vector3::new(0.32243916, -0.16951019, -1.3516326), + Vector3::new(0.32869884, -0.15321395, -1.3378662), + Vector3::new(0.32243916, -0.15321395, -1.3382301), + Vector3::new(0.32869884, -0.15321395, -1.3378662), + Vector3::new(0.32243916, -0.16951019, -1.3516326), + Vector3::new(0.32243916, -0.20401485, -1.3870783), + Vector3::new(0.3345271, -0.15321395, -1.3406355), + Vector3::new(0.37328035, -0.11000898, -1.6628642), + Vector3::new(0.3432731, -0.15321401, -1.6983604), + Vector3::new(0.32243916, -0.15321401, -1.7234849), + Vector3::new(0.32243916, -0.08669945, -1.724548), + Vector3::new(0.37358934, -0.08669945, -1.6628642), + Vector3::new(0.36483112, -0.15321401, -1.6628642), + Vector3::new(0.3432731, -0.15321401, -1.6983604), + Vector3::new(0.37328035, -0.110008985, -1.6628642), + Vector3::new(0.38895372, -0.08669945, -1.6443357), + Vector3::new(0.38895372, -0.08744214, -1.6443238), + Vector3::new(0.37328035, -0.11000898, -1.6628642), + Vector3::new(0.37358934, -0.08669945, -1.6628642), + Vector3::new(0.38895372, -0.14571172, -1.625561), + Vector3::new(0.38895372, -0.15321401, -1.6217446), + Vector3::new(0.38697082, -0.15321401, -1.6264102), + Vector3::new(0.38895372, -0.087442145, -1.6443238), + Vector3::new(0.38895372, -0.14571172, -1.625561), + Vector3::new(0.38697082, -0.15321401, -1.6264102), + Vector3::new(0.36483112, -0.15321401, -1.6628642), + Vector3::new(0.37328035, -0.110008985, -1.6628642), + Vector3::new(0.4294109, -0.1045661, -1.3690573), + Vector3::new(0.44130057, -0.00047307758, -1.2766529), + Vector3::new(0.38895372, -0.10872328, -1.3383226), + Vector3::new(0.37838188, -0.10980959, -1.3302914), + Vector3::new(0.33721945, -0.14189906, -1.3302914), + Vector3::new(0.33452708, -0.15321401, -1.3406357), + Vector3::new(0.38895372, -0.15321401, -1.3882599), + Vector3::new(0.32243916, -0.15321401, -1.3382303), + Vector3::new(0.3286988, -0.15321401, -1.3378663), + Vector3::new(0.3321432, -0.14424704, -1.3302914), + Vector3::new(0.32243916, -0.14356102, -1.3302914), + Vector3::new(0.3321432, -0.14424706, -1.3302914), + Vector3::new(0.3286988, -0.15321401, -1.3378663), + Vector3::new(0.33452708, -0.15321401, -1.3406355), + Vector3::new(0.33721942, -0.14189915, -1.3302914), + Vector3::new(0.37838173, -0.10980961, -1.3302913), + Vector3::new(0.34401476, -0.113340996, -1.3041832), + Vector3::new(0.33721948, -0.14189893, -1.3302913), + Vector3::new(0.32243916, -0.14356089, -1.3302913), + Vector3::new(0.33214325, -0.1442469, -1.3302913), + Vector3::new(0.34401476, -0.113340996, -1.3041832), + Vector3::new(0.32243916, -0.11582225, -1.3074783), + Vector3::new(0.33214325, -0.1442469, -1.3302913), + Vector3::new(0.33721945, -0.14189902, -1.3302913), + Vector3::new(0.34401476, -0.113340996, -1.3041832), + Vector3::new(0.32243916, -0.080589324, -1.7246456), + Vector3::new(0.32243916, -0.020184837, -1.7163969), + Vector3::new(0.33861578, -0.020184845, -1.7061031), + Vector3::new(0.33861578, -0.020184837, -1.7061031), + Vector3::new(0.37447086, -0.020184837, -1.6628642), + Vector3::new(0.37358934, -0.086699404, -1.6628642), + Vector3::new(0.32243916, -0.086699404, -1.724548), + Vector3::new(0.32243916, -0.08058934, -1.7246456), + Vector3::new(0.37447086, -0.020184837, -1.6628642), + Vector3::new(0.38895372, -0.020184837, -1.6453989), + Vector3::new(0.38895372, -0.086699404, -1.6443357), + Vector3::new(0.37358934, -0.086699404, -1.6628642), + Vector3::new(0.32243916, -0.042620897, -1.2608014), + Vector3::new(0.3386477, -0.020184837, -1.2452083), + Vector3::new(0.32243916, -0.020184845, -1.2479438), + Vector3::new(0.32243916, 0.046329774, -1.7073138), + Vector3::new(0.35642868, 0.046329774, -1.6856849), + Vector3::new(0.33861578, -0.020184785, -1.7061031), + Vector3::new(0.32243916, -0.020184793, -1.7163969), + Vector3::new(0.35642868, 0.046329774, -1.6856849), + Vector3::new(0.37535235, 0.046329774, -1.6628642), + Vector3::new(0.3744708, -0.020184793, -1.6628642), + Vector3::new(0.33861578, -0.020184793, -1.7061031), + Vector3::new(0.37535235, 0.046329774, -1.6628642), + Vector3::new(0.38895372, 0.046329774, -1.6464618), + Vector3::new(0.38895372, -0.020184793, -1.6453987), + Vector3::new(0.3744708, -0.020184793, -1.6628642), + Vector3::new(0.48844048, 0.10279102, -1.2567252), + Vector3::new(0.33864772, -0.020184793, -1.2452083), + Vector3::new(0.3867, 0.046329774, -1.1989802), + Vector3::new(0.32243916, 0.046329767, -1.2098254), + Vector3::new(0.32243916, -0.020184785, -1.2479436), + Vector3::new(0.32243916, 0.09600361, -1.7005304), + Vector3::new(0.32243916, 0.112844266, -1.7004257), + Vector3::new(0.34368488, 0.112844266, -1.6847112), + Vector3::new(0.34368488, 0.112844266, -1.6847112), + Vector3::new(0.3742416, 0.112844266, -1.6652669), + Vector3::new(0.35642868, 0.0463297, -1.685685), + Vector3::new(0.32243916, 0.046329707, -1.7073138), + Vector3::new(0.32243916, 0.09600361, -1.7005304), + Vector3::new(0.37535238, 0.046329696, -1.6628642), + Vector3::new(0.35642865, 0.046329692, -1.685685), + Vector3::new(0.37424156, 0.112844266, -1.6652669), + Vector3::new(0.37623394, 0.112844266, -1.6628642), + Vector3::new(0.38741195, 0.112844266, -1.6493843), + Vector3::new(0.38895372, 0.10323957, -1.6473714), + Vector3::new(0.38895372, 0.0463297, -1.6464618), + Vector3::new(0.37535238, 0.046329696, -1.6628642), + Vector3::new(0.37623394, 0.112844266, -1.6628642), + Vector3::new(0.38895372, 0.10323958, -1.6473714), + Vector3::new(0.38741195, 0.112844266, -1.6493843), + Vector3::new(0.38895372, 0.112844266, -1.6473188), + Vector3::new(0.32243916, 0.046329707, -1.2098254), + Vector3::new(0.38669994, 0.0463297, -1.1989803), + Vector3::new(0.3884858, 0.04880166, -1.1972623), + Vector3::new(0.32243916, 0.068251744, -1.1972623), + Vector3::new(0.38895372, 0.04924305, -1.1971927), + Vector3::new(0.38895372, 0.05021119, -1.1965392), + Vector3::new(0.3887098, 0.049111772, -1.1970468), + Vector3::new(0.32243916, 0.06825195, -1.1972622), + Vector3::new(0.3884859, 0.048801832, -1.1972622), + Vector3::new(0.3887098, 0.049111772, -1.1970468), + Vector3::new(0.35936964, 0.11284426, -1.1654744), + Vector3::new(0.32243916, 0.112844266, -1.1717072), + Vector3::new(0.38895372, 0.050211195, -1.1965392), + Vector3::new(0.38895372, 0.11284426, -1.1669341), + Vector3::new(0.35936964, 0.112844266, -1.1654744), + Vector3::new(0.3887098, 0.049111772, -1.1970468), + Vector3::new(0.37302625, 0.13610204, -1.6628642), + Vector3::new(0.3436848, 0.11284419, -1.6847113), + Vector3::new(0.32243916, 0.11284419, -1.7004257), + Vector3::new(0.32243916, 0.17935875, -1.7000124), + Vector3::new(0.3726629, 0.17935875, -1.6628642), + Vector3::new(0.3436848, 0.11284419, -1.6847113), + Vector3::new(0.37302625, 0.13610205, -1.6628642), + Vector3::new(0.37633768, 0.12067119, -1.6628642), + Vector3::new(0.37424156, 0.11284419, -1.6652669), + Vector3::new(0.37424156, 0.112844184, -1.6652669), + Vector3::new(0.37633768, 0.12067119, -1.6628642), + Vector3::new(0.37623394, 0.112844184, -1.6628642), + Vector3::new(0.3778704, 0.17935875, -1.6590124), + Vector3::new(0.38247612, 0.1435926, -1.655828), + Vector3::new(0.37302625, 0.13610204, -1.6628642), + Vector3::new(0.3726629, 0.17935875, -1.6628642), + Vector3::new(0.37302625, 0.13610205, -1.6628642), + Vector3::new(0.38247612, 0.1435926, -1.655828), + Vector3::new(0.37633768, 0.12067119, -1.6628642), + Vector3::new(0.38247612, 0.1435926, -1.655828), + Vector3::new(0.3778704, 0.17935875, -1.6590124), + Vector3::new(0.38842443, 0.17935875, -1.6479617), + Vector3::new(0.38895372, 0.17935875, -1.6472442), + Vector3::new(0.38895372, 0.15302485, -1.6470989), + Vector3::new(0.38247612, 0.1435926, -1.655828), + Vector3::new(0.38842443, 0.17935875, -1.6479617), + Vector3::new(0.37633768, 0.12067119, -1.6628642), + Vector3::new(0.38247612, 0.1435926, -1.655828), + Vector3::new(0.38741195, 0.11284419, -1.6493843), + Vector3::new(0.37623394, 0.112844184, -1.6628642), + Vector3::new(0.38895372, 0.11284419, -1.6473188), + Vector3::new(0.38741195, 0.11284419, -1.6493843), + Vector3::new(0.38247612, 0.1435926, -1.655828), + Vector3::new(0.38895372, 0.15302485, -1.6470989), + Vector3::new(0.32243913, 0.11284423, -1.5963496), + Vector3::new(0.38895375, 0.11284423, -1.5963496), + Vector3::new(0.38895375, 0.17935872, -1.5963496), + Vector3::new(0.32243913, 0.17935872, -1.5963496), + Vector3::new(0.32243913, 0.11284423, -1.529835), + Vector3::new(0.38895375, 0.11284423, -1.529835), + Vector3::new(0.38895375, 0.17935872, -1.529835), + Vector3::new(0.32243913, 0.17935872, -1.529835), + Vector3::new(0.32243913, 0.11284423, -1.529835), + Vector3::new(0.38895375, 0.11284423, -1.529835), + Vector3::new(0.38895375, 0.17935872, -1.529835), + Vector3::new(0.32243913, 0.17935872, -1.529835), + Vector3::new(0.32243913, 0.11284423, -1.4633205), + Vector3::new(0.38895375, 0.11284423, -1.4633205), + Vector3::new(0.38895375, 0.17935872, -1.4633205), + Vector3::new(0.32243913, 0.17935872, -1.4633205), + Vector3::new(0.32243913, 0.11284423, -1.4633205), + Vector3::new(0.38895375, 0.11284423, -1.4633205), + Vector3::new(0.38895375, 0.17935872, -1.4633205), + Vector3::new(0.32243913, 0.17935872, -1.4633205), + Vector3::new(0.32243913, 0.11284423, -1.396806), + Vector3::new(0.38895375, 0.11284423, -1.396806), + Vector3::new(0.38895375, 0.17935872, -1.396806), + Vector3::new(0.32243913, 0.17935872, -1.396806), + Vector3::new(0.32243913, 0.11284423, -1.396806), + Vector3::new(0.38895375, 0.11284423, -1.396806), + Vector3::new(0.38895375, 0.17935872, -1.396806), + Vector3::new(0.32243913, 0.17935872, -1.396806), + Vector3::new(0.32243913, 0.11284423, -1.3302914), + Vector3::new(0.38895375, 0.11284423, -1.3302914), + Vector3::new(0.38895375, 0.17935872, -1.3302914), + Vector3::new(0.32243913, 0.17935872, -1.3302914), + Vector3::new(0.32243913, 0.11284423, -1.3302914), + Vector3::new(0.38895375, 0.11284423, -1.3302914), + Vector3::new(0.38895375, 0.17935872, -1.3302914), + Vector3::new(0.32243913, 0.17935872, -1.3302914), + Vector3::new(0.32243913, 0.11284423, -1.2637768), + Vector3::new(0.38895375, 0.11284423, -1.2637768), + Vector3::new(0.38895375, 0.17935872, -1.2637768), + Vector3::new(0.32243913, 0.17935872, -1.2637768), + Vector3::new(0.32243913, 0.11284423, -1.2637768), + Vector3::new(0.38895375, 0.11284423, -1.2637768), + Vector3::new(0.38895375, 0.17935872, -1.2637768), + Vector3::new(0.32243913, 0.17935872, -1.2637768), + Vector3::new(0.32243913, 0.11284423, -1.1972623), + Vector3::new(0.38895375, 0.11284423, -1.1972623), + Vector3::new(0.38895375, 0.17935872, -1.1972623), + Vector3::new(0.32243913, 0.17935872, -1.1972623), + Vector3::new(0.32243916, 0.17935875, -1.1335888), + Vector3::new(0.32243916, 0.11284419, -1.1717072), + Vector3::new(0.35936967, 0.112844184, -1.1654745), + Vector3::new(0.3287487, 0.17935875, -1.132524), + Vector3::new(0.38895372, 0.112844184, -1.1669343), + Vector3::new(0.38895372, 0.17935875, -1.1354945), + Vector3::new(0.3287487, 0.17935875, -1.132524), + Vector3::new(0.35936967, 0.11284419, -1.1654745), + Vector3::new(0.38895372, -0.15321395, -1.6217446), + Vector3::new(0.39974678, -0.15321395, -1.5963497), + Vector3::new(0.38895372, -0.20313594, -1.5963497), + Vector3::new(0.38895372, -0.21955214, -1.5613409), + Vector3::new(0.40454316, -0.18911855, -1.5298351), + Vector3::new(0.39439303, -0.2197285, -1.5298351), + Vector3::new(0.38895372, -0.2197285, -1.5610058), + Vector3::new(0.38895372, -0.21664403, -1.5894783), + Vector3::new(0.42028785, -0.15321395, -1.5480187), + Vector3::new(0.42115748, -0.15321395, -1.5298351), + Vector3::new(0.40454316, -0.18911853, -1.5298351), + Vector3::new(0.38895372, -0.21955213, -1.5613409), + Vector3::new(0.3997468, -0.15321395, -1.5963496), + Vector3::new(0.42028785, -0.15321395, -1.5480187), + Vector3::new(0.38895372, -0.21664403, -1.5894783), + Vector3::new(0.38895372, -0.20313618, -1.5963496), + Vector3::new(0.39810324, -0.2197285, -1.5085732), + Vector3::new(0.42746043, -0.15321395, -1.4667326), + Vector3::new(0.4264674, -0.15321395, -1.4633205), + Vector3::new(0.38895372, -0.21328956, -1.4633205), + Vector3::new(0.38895372, -0.2197285, -1.4771357), + Vector3::new(0.40454322, -0.18911843, -1.529835), + Vector3::new(0.42293513, -0.15321395, -1.4926655), + Vector3::new(0.42746043, -0.15321395, -1.4667326), + Vector3::new(0.39810324, -0.2197285, -1.5085732), + Vector3::new(0.39439303, -0.2197285, -1.529835), + Vector3::new(0.40454322, -0.18911843, -1.529835), + Vector3::new(0.42115748, -0.15321395, -1.529835), + Vector3::new(0.42293513, -0.15321395, -1.4926655), + Vector3::new(0.38895372, -0.21004458, -1.4563582), + Vector3::new(0.4107516, -0.15321395, -1.4093214), + Vector3::new(0.40362403, -0.15321395, -1.4010967), + Vector3::new(0.38895372, -0.18089, -1.419324), + Vector3::new(0.40362403, -0.15321395, -1.4010967), + Vector3::new(0.39872047, -0.15321395, -1.396806), + Vector3::new(0.38895372, -0.16082796, -1.396806), + Vector3::new(0.38895372, -0.18089001, -1.419324), + Vector3::new(0.38895372, -0.21328956, -1.4633205), + Vector3::new(0.4264674, -0.15321395, -1.4633205), + Vector3::new(0.4107516, -0.15321395, -1.4093214), + Vector3::new(0.38895372, -0.2100446, -1.4563582), + Vector3::new(0.39872035, -0.15321395, -1.3968059), + Vector3::new(0.38895372, -0.15321395, -1.3882599), + Vector3::new(0.38895372, -0.16082785, -1.3968059), + Vector3::new(0.4138162, -0.05164461, -1.6149133), + Vector3::new(0.49487513, 0.08750193, -1.4567895), + Vector3::new(0.4522901, -0.08843134, -1.5056752), + Vector3::new(0.38895372, -0.087442145, -1.6443238), + Vector3::new(0.38895372, -0.08669945, -1.6443357), + Vector3::new(0.38946953, -0.08669945, -1.6437136), + Vector3::new(0.39974678, -0.15321401, -1.5963497), + Vector3::new(0.38895372, -0.15321401, -1.6217446), + Vector3::new(0.38895372, -0.14571172, -1.625561), + Vector3::new(0.404551, -0.08669945, -1.6188812), + Vector3::new(0.41412708, -0.08669945, -1.5963497), + Vector3::new(0.404551, -0.08669945, -1.6188812), + Vector3::new(0.38895372, -0.14571172, -1.625561), + Vector3::new(0.38895372, -0.087442145, -1.6443238), + Vector3::new(0.38946953, -0.08669945, -1.6437136), + Vector3::new(0.4202878, -0.15321401, -1.5480188), + Vector3::new(0.4340306, -0.12539425, -1.5298351), + Vector3::new(0.42115745, -0.15321401, -1.5298351), + Vector3::new(0.43403062, -0.12539425, -1.5298351), + Vector3::new(0.42028782, -0.15321401, -1.5480188), + Vector3::new(0.39974684, -0.15321401, -1.5963496), + Vector3::new(0.41412714, -0.08669945, -1.5963496), + Vector3::new(0.44239634, -0.08669945, -1.5298351), + Vector3::new(0.5041985, 0.082197644, -1.3501678), + Vector3::new(0.42646736, -0.15321401, -1.4633205), + Vector3::new(0.42746043, -0.15321401, -1.4667326), + Vector3::new(0.42985454, -0.1477897, -1.4633205), + Vector3::new(0.4274604, -0.15321401, -1.4667326), + Vector3::new(0.4229351, -0.15321401, -1.4926656), + Vector3::new(0.43745542, -0.12486756, -1.4633205), + Vector3::new(0.4298545, -0.1477897, -1.4633205), + Vector3::new(0.43403068, -0.12539408, -1.529835), + Vector3::new(0.4522901, -0.08843134, -1.5056752), + Vector3::new(0.45408338, -0.08893345, -1.4633205), + Vector3::new(0.43745542, -0.12486756, -1.4633205), + Vector3::new(0.4229351, -0.15321401, -1.4926656), + Vector3::new(0.42115745, -0.15321401, -1.529835), + Vector3::new(0.45563933, -0.08936912, -1.4265714), + Vector3::new(0.4340307, -0.12539408, -1.529835), + Vector3::new(0.4423964, -0.08669945, -1.529835), + Vector3::new(0.45047876, -0.08669945, -1.510818), + Vector3::new(0.4522901, -0.08843134, -1.5056752), + Vector3::new(0.41075158, -0.15321401, -1.4093215), + Vector3::new(0.41655156, -0.13809252, -1.396806), + Vector3::new(0.40707737, -0.1466991, -1.396806), + Vector3::new(0.403624, -0.15321401, -1.4010967), + Vector3::new(0.39872047, -0.15321401, -1.396806), + Vector3::new(0.403624, -0.15321401, -1.4010967), + Vector3::new(0.40707737, -0.1466991, -1.396806), + Vector3::new(0.45546827, -0.08946823, -1.4261962), + Vector3::new(0.4420653, -0.09723403, -1.396806), + Vector3::new(0.41655156, -0.13809252, -1.396806), + Vector3::new(0.41075158, -0.15321401, -1.4093215), + Vector3::new(0.42646736, -0.15321401, -1.4633205), + Vector3::new(0.42985454, -0.1477897, -1.4633205), + Vector3::new(0.45546827, -0.0897567, -1.4268152), + Vector3::new(0.4298545, -0.1477897, -1.4633205), + Vector3::new(0.43745542, -0.12486756, -1.4633205), + Vector3::new(0.45546827, -0.08970307, -1.4269171), + Vector3::new(0.45546827, -0.0897567, -1.4268152), + Vector3::new(0.43745542, -0.12486756, -1.4633205), + Vector3::new(0.45408338, -0.08893345, -1.4633205), + Vector3::new(0.45546827, -0.08932122, -1.4306116), + Vector3::new(0.45546827, -0.08970307, -1.4269171), + Vector3::new(0.41655162, -0.13809238, -1.3968059), + Vector3::new(0.4294109, -0.1045661, -1.3690573), + Vector3::new(0.40707746, -0.14669892, -1.3968059), + Vector3::new(0.38895372, -0.10872328, -1.3383226), + Vector3::new(0.38895372, -0.15321401, -1.3882599), + Vector3::new(0.39872032, -0.15321401, -1.3968059), + Vector3::new(0.40707746, -0.14669892, -1.3968059), + Vector3::new(0.4294109, -0.1045661, -1.3690573), + Vector3::new(0.44206524, -0.09723406, -1.3968059), + Vector3::new(0.4294109, -0.1045661, -1.3690573), + Vector3::new(0.41655162, -0.13809238, -1.3968059), + Vector3::new(0.419506, -0.020184837, -1.607119), + Vector3::new(0.424374, -0.020184837, -1.5963497), + Vector3::new(0.42333248, -0.035308912, -1.5963497), + Vector3::new(0.4138162, -0.05164461, -1.6149133), + Vector3::new(0.38895372, -0.086699404, -1.6443357), + Vector3::new(0.38895372, -0.020184837, -1.6453989), + Vector3::new(0.40876618, -0.020184837, -1.6215062), + Vector3::new(0.4138162, -0.05164461, -1.6149133), + Vector3::new(0.38946956, -0.086699404, -1.6437136), + Vector3::new(0.4138162, -0.05164461, -1.6149133), + Vector3::new(0.40876618, -0.020184837, -1.6215062), + Vector3::new(0.419506, -0.020184845, -1.607119), + Vector3::new(0.40455103, -0.086699404, -1.6188812), + Vector3::new(0.4138162, -0.05164461, -1.6149133), + Vector3::new(0.42035437, -0.057896048, -1.5963497), + Vector3::new(0.4141271, -0.086699404, -1.5963497), + Vector3::new(0.4138162, -0.05164461, -1.6149133), + Vector3::new(0.40455103, -0.086699404, -1.6188812), + Vector3::new(0.38946956, -0.086699404, -1.6437136), + Vector3::new(0.42437407, -0.020184837, -1.5963496), + Vector3::new(0.4321429, -0.020184837, -1.579163), + Vector3::new(0.42333254, -0.035308808, -1.5963496), + Vector3::new(0.41412717, -0.086699404, -1.5963496), + Vector3::new(0.4203544, -0.05789609, -1.5963496), + Vector3::new(0.44378093, -0.08029534, -1.5298351), + Vector3::new(0.44239637, -0.086699404, -1.5298351), + Vector3::new(0.44239643, -0.086699404, -1.529835), + Vector3::new(0.44378096, -0.08029538, -1.529835), + Vector3::new(0.4504787, -0.086699404, -1.5108182), + Vector3::new(0.424374, -0.020184793, -1.5963497), + Vector3::new(0.419506, -0.020184793, -1.607119), + Vector3::new(0.42736745, 0.023282386, -1.5963497), + Vector3::new(0.38895372, 0.046329774, -1.6464618), + Vector3::new(0.39808905, 0.046329774, -1.6354452), + Vector3::new(0.40876618, -0.020184793, -1.6215062), + Vector3::new(0.38895372, -0.020184793, -1.6453987), + Vector3::new(0.39808905, 0.046329774, -1.6354452), + Vector3::new(0.42727327, 0.046329767, -1.5963497), + Vector3::new(0.42736745, 0.023282384, -1.5963497), + Vector3::new(0.419506, -0.020184785, -1.607119), + Vector3::new(0.40876618, -0.020184793, -1.6215062), + Vector3::new(0.45546827, 0.019855715, -1.5336616), + Vector3::new(0.4321429, -0.020184793, -1.579163), + Vector3::new(0.42437407, -0.020184793, -1.5963496), + Vector3::new(0.42736754, 0.023282867, -1.5963496), + Vector3::new(0.43153578, 0.046329774, -1.5906396), + Vector3::new(0.45546827, 0.046329767, -1.5376949), + Vector3::new(0.42727333, 0.046329767, -1.5963496), + Vector3::new(0.43153578, 0.046329767, -1.5906396), + Vector3::new(0.42736754, 0.023282867, -1.5963496), + Vector3::new(0.38895372, 0.046329692, -1.6464618), + Vector3::new(0.38895372, 0.10323957, -1.6473714), + Vector3::new(0.39808908, 0.0463297, -1.6354452), + Vector3::new(0.38895372, 0.112844266, -1.6473188), + Vector3::new(0.42700142, 0.112844266, -1.5963497), + Vector3::new(0.42727327, 0.046329707, -1.5963497), + Vector3::new(0.39808908, 0.0463297, -1.6354452), + Vector3::new(0.38895372, 0.10323958, -1.6473714), + Vector3::new(0.45546827, 0.112844266, -1.5478284), + Vector3::new(0.45546827, 0.046329707, -1.5376949), + Vector3::new(0.43153575, 0.0463297, -1.5906396), + Vector3::new(0.44356552, 0.112844266, -1.5741602), + Vector3::new(0.4270015, 0.112844266, -1.5963496), + Vector3::new(0.44356555, 0.112844266, -1.5741602), + Vector3::new(0.43153578, 0.046329707, -1.5906396), + Vector3::new(0.42727336, 0.046329707, -1.5963496), + Vector3::new(0.38907006, 0.04930567, -1.1972623), + Vector3::new(0.45546827, 0.08504398, -1.2369947), + Vector3::new(0.45546827, 0.112844266, -1.218227), + Vector3::new(0.4336699, 0.112844266, -1.1972623), + Vector3::new(0.4028486, 0.112844266, -1.1676197), + Vector3::new(0.38895372, 0.05021119, -1.1965392), + Vector3::new(0.38895372, 0.04924305, -1.1971927), + Vector3::new(0.38906986, 0.049305566, -1.1972622), + Vector3::new(0.43366978, 0.112844266, -1.1972622), + Vector3::new(0.38895372, 0.050211195, -1.1965392), + Vector3::new(0.4028486, 0.11284426, -1.1676197), + Vector3::new(0.38895372, 0.11284426, -1.1669341), + Vector3::new(0.38895372, 0.17935875, -1.6472442), + Vector3::new(0.4070385, 0.17935875, -1.6227281), + Vector3::new(0.38895372, 0.15302485, -1.6470989), + Vector3::new(0.4270014, 0.11284419, -1.5963497), + Vector3::new(0.38895372, 0.11284419, -1.6473188), + Vector3::new(0.38895372, 0.15302485, -1.6470989), + Vector3::new(0.4070385, 0.17935875, -1.6227281), + Vector3::new(0.4267296, 0.17935875, -1.5963497), + Vector3::new(0.45546827, 0.17865649, -1.5578549), + Vector3::new(0.45546827, 0.11284419, -1.5478284), + Vector3::new(0.44356552, 0.112844184, -1.5741603), + Vector3::new(0.45546827, 0.17935875, -1.5578511), + Vector3::new(0.45546827, 0.17865646, -1.5578549), + Vector3::new(0.44356552, 0.11284419, -1.5741602), + Vector3::new(0.42700148, 0.11284419, -1.5963496), + Vector3::new(0.42672968, 0.17935875, -1.5963496), + Vector3::new(0.38895375, 0.11284423, -1.529835), + Vector3::new(0.45546824, 0.11284423, -1.529835), + Vector3::new(0.45546824, 0.17935872, -1.529835), + Vector3::new(0.38895375, 0.17935872, -1.529835), + Vector3::new(0.38895375, 0.11284423, -1.4633205), + Vector3::new(0.45546824, 0.11284423, -1.4633205), + Vector3::new(0.45546824, 0.17935872, -1.4633205), + Vector3::new(0.38895375, 0.17935872, -1.4633205), + Vector3::new(0.38895375, 0.11284423, -1.4633205), + Vector3::new(0.45546824, 0.11284423, -1.4633205), + Vector3::new(0.45546824, 0.17935872, -1.4633205), + Vector3::new(0.38895375, 0.17935872, -1.4633205), + Vector3::new(0.38895375, 0.11284423, -1.396806), + Vector3::new(0.45546824, 0.11284423, -1.396806), + Vector3::new(0.45546824, 0.17935872, -1.396806), + Vector3::new(0.38895375, 0.17935872, -1.396806), + Vector3::new(0.38895375, 0.11284423, -1.396806), + Vector3::new(0.45546824, 0.11284423, -1.396806), + Vector3::new(0.45546824, 0.17935872, -1.396806), + Vector3::new(0.38895375, 0.17935872, -1.396806), + Vector3::new(0.38895375, 0.11284423, -1.3302914), + Vector3::new(0.45546824, 0.11284423, -1.3302914), + Vector3::new(0.45546824, 0.17935872, -1.3302914), + Vector3::new(0.38895375, 0.17935872, -1.3302914), + Vector3::new(0.38895375, 0.11284423, -1.3302914), + Vector3::new(0.45546824, 0.11284423, -1.3302914), + Vector3::new(0.45546824, 0.17935872, -1.3302914), + Vector3::new(0.38895375, 0.17935872, -1.3302914), + Vector3::new(0.38895375, 0.11284423, -1.2637768), + Vector3::new(0.45546824, 0.11284423, -1.2637768), + Vector3::new(0.45546824, 0.17935872, -1.2637768), + Vector3::new(0.38895375, 0.17935872, -1.2637768), + Vector3::new(0.4336698, 0.11284419, -1.1972623), + Vector3::new(0.45546827, 0.11284419, -1.2182271), + Vector3::new(0.45546827, 0.14389904, -1.1972623), + Vector3::new(0.45546827, 0.14711037, -1.1950943), + Vector3::new(0.45546827, 0.17935875, -1.1796435), + Vector3::new(0.4314765, 0.17935875, -1.1502495), + Vector3::new(0.4314765, 0.17935875, -1.1502495), + Vector3::new(0.41760457, 0.17935875, -1.136908), + Vector3::new(0.40284857, 0.11284419, -1.1676197), + Vector3::new(0.4336697, 0.11284419, -1.1972622), + Vector3::new(0.45546827, 0.14389922, -1.1972622), + Vector3::new(0.45546827, 0.14711037, -1.1950943), + Vector3::new(0.40284857, 0.112844184, -1.1676197), + Vector3::new(0.41760457, 0.17935875, -1.136908), + Vector3::new(0.38895372, 0.17935875, -1.1354945), + Vector3::new(0.38895372, 0.11284419, -1.1669343), + Vector3::new(0.45546833, -0.0894682, -1.4261963), + Vector3::new(0.45546833, -0.08975658, -1.426815), + Vector3::new(0.45563933, -0.08936912, -1.4265714), + Vector3::new(0.45563933, -0.08936912, -1.4265714), + Vector3::new(0.45546833, -0.08975656, -1.426815), + Vector3::new(0.45546833, -0.08970295, -1.426917), + Vector3::new(0.45563933, -0.08936912, -1.4265714), + Vector3::new(0.45546833, -0.08970296, -1.426917), + Vector3::new(0.45546833, -0.08932124, -1.4306103), + Vector3::new(0.45546833, 0.046329774, -1.5376947), + Vector3::new(0.4590211, 0.046329774, -1.5298351), + Vector3::new(0.4574298, 0.023222877, -1.5298351), + Vector3::new(0.45546833, 0.01985582, -1.5336614), + Vector3::new(0.45902115, 0.046329774, -1.529835), + Vector3::new(0.47089055, 0.046329774, -1.5035769), + Vector3::new(0.45742986, 0.023222983, -1.529835), + Vector3::new(0.4590211, 0.0463297, -1.5298351), + Vector3::new(0.45546833, 0.0463297, -1.5376947), + Vector3::new(0.45546833, 0.112844266, -1.5478283), + Vector3::new(0.46360174, 0.112844266, -1.5298351), + Vector3::new(0.49152714, 0.081754744, -1.4633205), + Vector3::new(0.47089052, 0.0463297, -1.5035769), + Vector3::new(0.45902112, 0.0463297, -1.529835), + Vector3::new(0.4636018, 0.112844266, -1.529835), + Vector3::new(0.49191773, 0.11284426, -1.4671929), + Vector3::new(0.49301854, 0.103411235, -1.4633205), + Vector3::new(0.49301854, 0.103411235, -1.4633205), + Vector3::new(0.49191773, 0.112844266, -1.4671929), + Vector3::new(0.49302983, 0.112844266, -1.4633205), + Vector3::new(0.49152714, 0.081754744, -1.4633205), + Vector3::new(0.49301854, 0.103411235, -1.4633205), + Vector3::new(0.49487513, 0.08750193, -1.4567895), + Vector3::new(0.5001203, 0.08451783, -1.396806), + Vector3::new(0.49487513, 0.08750193, -1.4567895), + Vector3::new(0.49677816, 0.112844266, -1.4502685), + Vector3::new(0.5015912, 0.11284426, -1.396806), + Vector3::new(0.49301854, 0.103411235, -1.4633205), + Vector3::new(0.49302983, 0.112844266, -1.4633205), + Vector3::new(0.49677816, 0.11284426, -1.4502685), + Vector3::new(0.49487513, 0.08750193, -1.4567895), + Vector3::new(0.50084656, 0.0865781, -1.3302914), + Vector3::new(0.5041985, 0.082197644, -1.3501678), + Vector3::new(0.5063298, 0.112844266, -1.334446), + Vector3::new(0.5054681, 0.112844266, -1.3302914), + Vector3::new(0.5041985, 0.082197644, -1.3501678), + Vector3::new(0.5051708, 0.112844266, -1.3570441), + Vector3::new(0.5063298, 0.11284426, -1.334446), + Vector3::new(0.5051708, 0.11284426, -1.3570441), + Vector3::new(0.5041985, 0.082197644, -1.3501678), + Vector3::new(0.5001203, 0.08451783, -1.3968059), + Vector3::new(0.5015912, 0.11284426, -1.3968059), + Vector3::new(0.48962966, 0.101236954, -1.2637768), + Vector3::new(0.50084656, 0.08657813, -1.3302913), + Vector3::new(0.505468, 0.112844266, -1.3302913), + Vector3::new(0.49167192, 0.11284426, -1.2637768), + Vector3::new(0.48991573, 0.11284426, -1.2553097), + Vector3::new(0.48844048, 0.10279102, -1.2567252), + Vector3::new(0.48962966, 0.101236954, -1.2637768), + Vector3::new(0.49167192, 0.11284426, -1.2637768), + Vector3::new(0.48991573, 0.112844266, -1.2553097), + Vector3::new(0.4888348, 0.11284426, -1.2523916), + Vector3::new(0.48844048, 0.10279102, -1.2567252), + Vector3::new(0.4888348, 0.112844266, -1.2523916), + Vector3::new(0.48096117, 0.112844266, -1.242745), + Vector3::new(0.48844048, 0.10279102, -1.2567252), + Vector3::new(0.48096117, 0.112844266, -1.242745), + Vector3::new(0.45546833, 0.112844266, -1.2182271), + Vector3::new(0.45546833, 0.08504401, -1.2369947), + Vector3::new(0.48844048, 0.10279102, -1.2567252), + Vector3::new(0.46360177, 0.11284419, -1.5298351), + Vector3::new(0.45546833, 0.11284419, -1.5478283), + Vector3::new(0.45546833, 0.17865682, -1.5578549), + Vector3::new(0.45559528, 0.17935875, -1.557681), + Vector3::new(0.46818238, 0.17935875, -1.5298351), + Vector3::new(0.45546833, 0.1786568, -1.5578549), + Vector3::new(0.45546833, 0.17935875, -1.5578511), + Vector3::new(0.45559528, 0.17935875, -1.557681), + Vector3::new(0.48415565, 0.17935875, -1.4944981), + Vector3::new(0.49191776, 0.1128442, -1.4671929), + Vector3::new(0.4636018, 0.11284419, -1.529835), + Vector3::new(0.46818244, 0.17935875, -1.529835), + Vector3::new(0.48415565, 0.17935875, -1.4944981), + Vector3::new(0.49310938, 0.17935875, -1.4633205), + Vector3::new(0.49302983, 0.11284419, -1.4633205), + Vector3::new(0.49191776, 0.11284419, -1.4671929), + Vector3::new(0.5015912, 0.1128442, -1.396806), + Vector3::new(0.49677816, 0.11284419, -1.4502685), + Vector3::new(0.50177294, 0.17935875, -1.4331533), + Vector3::new(0.5050451, 0.17935875, -1.396806), + Vector3::new(0.49310938, 0.17935875, -1.4633205), + Vector3::new(0.50177294, 0.17935875, -1.4331533), + Vector3::new(0.49677816, 0.1128442, -1.4502685), + Vector3::new(0.49302983, 0.11284419, -1.4633205), + Vector3::new(0.505468, 0.11284419, -1.3302914), + Vector3::new(0.5063298, 0.11284419, -1.3344461), + Vector3::new(0.506893, 0.12094295, -1.3302914), + Vector3::new(0.5051708, 0.11284419, -1.3570441), + Vector3::new(0.5072812, 0.17935875, -1.3719683), + Vector3::new(0.50941855, 0.17935875, -1.3302914), + Vector3::new(0.506893, 0.12094273, -1.3302914), + Vector3::new(0.5063298, 0.1128442, -1.334446), + Vector3::new(0.5072812, 0.17935875, -1.3719683), + Vector3::new(0.5051708, 0.1128442, -1.3570441), + Vector3::new(0.5015912, 0.1128442, -1.3968059), + Vector3::new(0.5050452, 0.17935875, -1.3968059), + Vector3::new(0.49167192, 0.1128442, -1.2637768), + Vector3::new(0.505468, 0.11284419, -1.3302913), + Vector3::new(0.50689304, 0.12094319, -1.3302913), + Vector3::new(0.51095545, 0.17935875, -1.300324), + Vector3::new(0.50337505, 0.17935875, -1.2637768), + Vector3::new(0.506893, 0.120942965, -1.3302913), + Vector3::new(0.5094186, 0.17935875, -1.3302913), + Vector3::new(0.51095545, 0.17935875, -1.3003238), + Vector3::new(0.49967632, 0.17935875, -1.2459443), + Vector3::new(0.48991573, 0.1128442, -1.2553097), + Vector3::new(0.49167192, 0.1128442, -1.2637768), + Vector3::new(0.50337505, 0.17935875, -1.2637768), + Vector3::new(0.4914436, 0.17935875, -1.2237192), + Vector3::new(0.4888348, 0.1128442, -1.2523916), + Vector3::new(0.48991573, 0.11284419, -1.2553097), + Vector3::new(0.49967632, 0.17935875, -1.2459443), + Vector3::new(0.4566281, 0.1455514, -1.1972623), + Vector3::new(0.48096123, 0.11284419, -1.2427452), + Vector3::new(0.4888348, 0.11284419, -1.2523916), + Vector3::new(0.4914436, 0.17935875, -1.2237192), + Vector3::new(0.46984905, 0.17935875, -1.1972623), + Vector3::new(0.48096123, 0.11284419, -1.2427452), + Vector3::new(0.4566281, 0.1455514, -1.1972623), + Vector3::new(0.45546833, 0.14389914, -1.1972623), + Vector3::new(0.45546833, 0.11284419, -1.2182271), + Vector3::new(0.45662802, 0.14555149, -1.1972622), + Vector3::new(0.46984893, 0.17935875, -1.1972622), + Vector3::new(0.45546833, 0.17935875, -1.1796435), + Vector3::new(0.45546833, 0.1471103, -1.1950945), + Vector3::new(0.45662802, 0.14555147, -1.1972622), + Vector3::new(0.45546833, 0.1471103, -1.1950945), + Vector3::new(0.45546833, 0.1438993, -1.1972622), ]; let (vertices, indices) = transformation::convex_hull(&input); @@ -4069,14 +4069,14 @@ fn test_complex_convex_hull() { #[test] fn test_planar_convex_hull() { let input = vec![ - Point3::new(0.73077667, -0.52849126, 0.9227245), - Point3::new(0.73077667, -0.52849126, -0.66274846), - Point3::new(0.73077667, -1.145064, -0.92699397), - Point3::new(0.73077667, -1.5854732, 0.9227245), - Point3::new(0.73077667, -2.5543733, -0.92699397), - Point3::new(0.73077667, -2.730537, 0.83464265), - Point3::new(0.73077667, -2.8186188, 0.7465608), - Point3::new(0.73077667, -2.8186188, -0.8389121), + Vector3::new(0.73077667, -0.52849126, 0.9227245), + Vector3::new(0.73077667, -0.52849126, -0.66274846), + Vector3::new(0.73077667, -1.145064, -0.92699397), + Vector3::new(0.73077667, -1.5854732, 0.9227245), + Vector3::new(0.73077667, -2.5543733, -0.92699397), + Vector3::new(0.73077667, -2.730537, 0.83464265), + Vector3::new(0.73077667, -2.8186188, 0.7465608), + Vector3::new(0.73077667, -2.8186188, -0.8389121), ]; let (vertices, indices) = transformation::convex_hull(&input); diff --git a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs index 7f8f9e43..bf2cdd9a 100644 --- a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs +++ b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs @@ -1,6 +1,6 @@ // https://github.com/dimforge/parry/issues/242 -use na::{Isometry3, Point3, Translation3, UnitQuaternion, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::Ray; use parry3d::shape::{Ball, Cuboid, Shape}; @@ -11,24 +11,23 @@ where let mut rng = oorandom::Rand32::new(42); for _ in 0..1000 { - let ray_origin = Point3::from(Vector3::from_fn(|_, _| rng.rand_float()).normalize() * 5.0); - let ray = Ray::new(ray_origin, Point3::origin() - ray_origin); + let ray_origin = Vector::from( + Vector::new(rng.rand_float(), rng.rand_float(), rng.rand_float()).normalize() * 5.0, + ); + let ray = Ray::new(ray_origin, Vector::ZERO - ray_origin); let rotation = if rng.rand_float() < 0.01 { - UnitQuaternion::identity() + parry3d::glamx::Quat::IDENTITY } else { - na::Unit::try_new( - na::Quaternion::new( - rng.rand_float(), - rng.rand_float(), - rng.rand_float(), - rng.rand_float(), - ), - 1.0e-5, + parry3d::glamx::Quat::from_xyzw( + rng.rand_float(), + rng.rand_float(), + rng.rand_float(), + rng.rand_float(), ) - .unwrap_or(UnitQuaternion::identity()) + .normalize() }; - let position = Isometry3::from_parts(Translation3::identity(), rotation); + let position = Pose::from_parts(Vector::ZERO.into(), rotation); let intersection = shape .cast_ray_and_get_normal(&position, &ray, f32::MAX, true) @@ -42,18 +41,18 @@ where let point_nudged_out = point + intersection.normal * 0.001; assert!( - shape.contains_point(&position, &point_nudged_in), + shape.contains_point(&position, point_nudged_in), "Shape {} rotated with {:#?} does not contain point nudged in {:#?}", name, - rotation.axis(), + rotation, point_nudged_in, ); assert!( - !shape.contains_point(&position, &point_nudged_out), + !shape.contains_point(&position, point_nudged_out), "Shape {} rotated with {:#?} does contains point nudged out {:#?}", name, - rotation.axis(), + rotation, point_nudged_out, ); @@ -80,11 +79,11 @@ fn shape_ray_cast_points_to_surface() { run_test("ball with radius 1", Ball::new(1.0)); run_test( "cube with half-side 1", - Cuboid::new(Vector3::new(1.0, 1.0, 1.0)), + Cuboid::new(Vector::new(1.0, 1.0, 1.0)), ); - run_test("tall rectangle", Cuboid::new(Vector3::new(1.0, 1.0, 0.5))); + run_test("tall rectangle", Cuboid::new(Vector::new(1.0, 1.0, 0.5))); run_test( "tall and slim rectangle", - Cuboid::new(Vector3::new(0.5, 1.0, 0.5)), + Cuboid::new(Vector::new(0.5, 1.0, 0.5)), ); } diff --git a/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs b/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs index e4817b20..0d410dd2 100644 --- a/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs +++ b/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs @@ -1,4 +1,4 @@ -use na::{self, Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query; use parry3d::shape::{Cuboid, Cylinder}; @@ -6,9 +6,9 @@ use parry3d::shape::{Cuboid, Cylinder}; #[test] fn cylinder_cuboid_contact() { let cyl = Cylinder::new(0.925, 0.5); - let cyl_at = Isometry3::translation(10.97, 0.925, 61.02); - let cuboid = Cuboid::new(Vector3::new(0.05, 0.75, 0.5)); - let cuboid_at = Isometry3::translation(11.50, 0.75, 60.5); + let cyl_at = Pose::translation(10.97, 0.925, 61.02); + let cuboid = Cuboid::new(Vector::new(0.05, 0.75, 0.5)); + let cuboid_at = Pose::translation(11.50, 0.75, 60.5); let distance = query::details::distance_support_map_support_map( &cyl_at.inv_mul(&cuboid_at), &cyl, diff --git a/crates/parry3d/tests/geometry/epa3.rs b/crates/parry3d/tests/geometry/epa3.rs index e77c38f3..5489ab49 100644 --- a/crates/parry3d/tests/geometry/epa3.rs +++ b/crates/parry3d/tests/geometry/epa3.rs @@ -1,4 +1,4 @@ -use na::{self, Isometry3, Point3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query; use parry3d::query::gjk::VoronoiSimplex; use parry3d::shape::{Cuboid, Triangle}; @@ -6,20 +6,20 @@ use parry3d::shape::{Cuboid, Triangle}; #[test] #[allow(non_snake_case)] fn cuboid_cuboid_EPA() { - let c = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)); - let m1 = Isometry3::translation(3.5, 0.0, 0.0); - let m2 = Isometry3::identity(); + let c = Cuboid::new(Vector::new(2.0, 1.0, 1.0)); + let m1 = Pose::translation(3.5, 0.0, 0.0); + let m2 = Pose::identity(); let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) .expect("Penetration not found."); assert_eq!(res.dist, -0.5); - assert_eq!(res.normal1, -Vector3::x_axis()); + assert_eq!(res.normal1, -Vector::X); - let m1 = Isometry3::translation(0.0, 0.2, 0.0); + let m1 = Pose::translation(0.0, 0.2, 0.0); let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) .expect("Penetration not found."); assert_eq!(res.dist, -1.8); - assert_eq!(res.normal1, -Vector3::y_axis()); + assert_eq!(res.normal1, -Vector::Y); } #[test] @@ -29,18 +29,18 @@ fn triangle_vertex_touches_triangle_edge_epa() { // https://github.com/dimforge/parry/issues/246 let mesh1 = Triangle::new( - Point3::new(-13.174434, 1.0, 8.736801), - Point3::new(3.5251038, 1.0, 12.1), - Point3::new(3.2048466, 1.0, 12.218325), + Vector::new(-13.174434, 1.0, 8.736801), + Vector::new(3.5251038, 1.0, 12.1), + Vector::new(3.2048466, 1.0, 12.218325), ); let mesh2 = Triangle::new( - Point3::new(-1.63, 0.0, 11.19), - Point3::new(-2.349647, 0.0, 11.037681), - Point3::new(-2.349647, 1.0, 11.037681), + Vector::new(-1.63, 0.0, 11.19), + Vector::new(-2.349647, 0.0, 11.037681), + Vector::new(-2.349647, 1.0, 11.037681), ); let gjk_result = query::details::contact_support_map_support_map_with_params( - &Isometry3::identity(), + &Pose::identity(), &mesh1, &mesh2, 0.00999999977, diff --git a/crates/parry3d/tests/geometry/still_objects_toi.rs b/crates/parry3d/tests/geometry/still_objects_toi.rs index 3c9bad02..09b870ff 100644 --- a/crates/parry3d/tests/geometry/still_objects_toi.rs +++ b/crates/parry3d/tests/geometry/still_objects_toi.rs @@ -1,4 +1,4 @@ -use na::{self, Isometry3, Vector3}; +use parry3d::math::{Pose, Vector}; use parry3d::query::{cast_shapes, ShapeCastOptions}; use parry3d::shape::Cuboid; @@ -19,18 +19,18 @@ use parry3d::shape::Cuboid; * with box 1 having the provided v_y. */ fn collide(v_y: f32) -> Option { - let pos1 = Isometry3::translation(0.0, 1.1, 0.0); - let pos2 = Isometry3::identity(); - let vel1 = Vector3::y() * v_y; - let vel2 = Vector3::zeros(); - let cuboid = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); + let pos1 = Pose::translation(0.0, 1.1, 0.0); + let pos2 = Pose::identity(); + let vel1 = Vector::Y * v_y; + let vel2 = Vector::ZERO; + let cuboid = Cuboid::new(Vector::new(0.5, 0.5, 0.5)); cast_shapes( &pos1, - &vel1, + vel1, &cuboid, &pos2, - &vel2, + vel2, &cuboid, ShapeCastOptions::default(), ) diff --git a/crates/parry3d/tests/geometry/time_of_impact3.rs b/crates/parry3d/tests/geometry/time_of_impact3.rs index 71cbcf45..8bf314a0 100644 --- a/crates/parry3d/tests/geometry/time_of_impact3.rs +++ b/crates/parry3d/tests/geometry/time_of_impact3.rs @@ -1,30 +1,29 @@ -use na::{self, Isometry3, Vector3}; -use parry3d::math::Real; +use parry3d::math::{Pose, Real, Vector}; use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::{Ball, Cuboid}; #[test] fn ball_cuboid_toi() { - let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); let ball = Ball::new(1.0); - let cuboid_pos = Isometry3::identity(); - let ball_pos_intersecting = Isometry3::translation(1.0, 1.0, 1.0); - let ball_pos_will_touch = Isometry3::translation(2.0, 2.0, 2.0); - let ball_pos_wont_touch = Isometry3::translation(3.0, 3.0, 3.0); + let cuboid_pos = Pose::identity(); + let ball_pos_intersecting = Pose::translation(1.0, 1.0, 1.0); + let ball_pos_will_touch = Pose::translation(2.0, 2.0, 2.0); + let ball_pos_wont_touch = Pose::translation(3.0, 3.0, 3.0); - let cuboid_vel1 = Vector3::new(-1.0, 1.0, 1.0); - let cuboid_vel2 = Vector3::new(1.0, 1.0, 1.0); + let cuboid_vel1 = Vector::new(-1.0, 1.0, 1.0); + let cuboid_vel2 = Vector::new(1.0, 1.0, 1.0); - let ball_vel1 = Vector3::new(2.0, 2.0, 2.0); - let ball_vel2 = Vector3::new(-0.5, -0.5, -0.5); + let ball_vel1 = Vector::new(2.0, 2.0, 2.0); + let ball_vel2 = Vector::new(-0.5, -0.5, -0.5); let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &cuboid_vel1, + cuboid_vel1, &cuboid, ShapeCastOptions::default(), ) @@ -32,10 +31,10 @@ fn ball_cuboid_toi() { .map(|hit| hit.time_of_impact); let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, - &ball_vel2, + ball_vel2, &ball, &cuboid_pos, - &cuboid_vel2, + cuboid_vel2, &cuboid, ShapeCastOptions::default(), ) @@ -43,10 +42,10 @@ fn ball_cuboid_toi() { .map(|hit| hit.time_of_impact); let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, - &ball_vel1, + ball_vel1, &ball, &cuboid_pos, - &cuboid_vel1, + cuboid_vel1, &cuboid, ShapeCastOptions::default(), ) @@ -56,7 +55,7 @@ fn ball_cuboid_toi() { assert_eq!(toi_intersecting, Some(0.0)); assert!(relative_eq!( toi_will_touch.unwrap(), - ((3.0 as Real).sqrt() - 1.0) / (ball_vel2 - cuboid_vel2).norm() + ((3.0 as Real).sqrt() - 1.0) / (ball_vel2 - cuboid_vel2).length() )); assert_eq!(toi_wont_touch, None); } diff --git a/crates/parry3d/tests/geometry/trimesh_connected_components.rs b/crates/parry3d/tests/geometry/trimesh_connected_components.rs index 03216020..d5863fe5 100644 --- a/crates/parry3d/tests/geometry/trimesh_connected_components.rs +++ b/crates/parry3d/tests/geometry/trimesh_connected_components.rs @@ -1,4 +1,4 @@ -use parry3d::math::Point; +use parry3d::math::Vector; use parry3d::shape::{TriMesh, TriMeshFlags}; #[test] @@ -6,13 +6,13 @@ use parry3d::shape::{TriMesh, TriMeshFlags}; fn mesh_connected_components_grouped_faces() { let verts = vec![ // Face 0 - Point::new(15.82, 6.455, -0.15), // <- Vertex shared with face 1. - Point::new(9.915, 6.455, -0.15), - Point::new(9.915, 6.4, 0.0), // <- Vertex shared with face 1. + Vector::new(15.82, 6.455, -0.15), // <- Vertex shared with face 1. + Vector::new(9.915, 6.455, -0.15), + Vector::new(9.915, 6.4, 0.0), // <- Vertex shared with face 1. // Face1 - Point::new(15.82, 6.455, -0.15), // <- Vertex shared with face 0. - Point::new(9.915, 6.4, 0.0), // <- Vertex shared with face 0. - Point::new(15.82, 6.4, 0.0), + Vector::new(15.82, 6.455, -0.15), // <- Vertex shared with face 0. + Vector::new(9.915, 6.4, 0.0), // <- Vertex shared with face 0. + Vector::new(15.82, 6.4, 0.0), ]; let mut roof = TriMesh::new(verts, vec![[0, 1, 2], [3, 4, 5]]).unwrap(); diff --git a/crates/parry3d/tests/geometry/trimesh_intersection.rs b/crates/parry3d/tests/geometry/trimesh_intersection.rs index fd195e4c..8317a154 100644 --- a/crates/parry3d/tests/geometry/trimesh_intersection.rs +++ b/crates/parry3d/tests/geometry/trimesh_intersection.rs @@ -1,16 +1,15 @@ -use na::{Point3, Vector3}; -use parry3d::math::{Isometry, Real}; +use parry3d::math::{Pose, Vector}; use parry3d::query::IntersectResult; use parry3d::shape::TriMesh; -fn build_diamond(position: &Isometry) -> TriMesh { +fn build_diamond(position: &Pose) -> TriMesh { // Two tetrahedrons sharing a face let points = vec![ - position * Point3::new(0.0, 2.0, 0.0), - position * Point3::new(-2.0, -1.0, 0.0), - position * Point3::new(0.0, 0.0, 2.0), - position * Point3::new(2.0, -1.0, 0.0), - position * Point3::new(0.0, 0.0, -2.0), + position * Vector::new(0.0, 2.0, 0.0), + position * Vector::new(-2.0, -1.0, 0.0), + position * Vector::new(0.0, 0.0, 2.0), + position * Vector::new(2.0, -1.0, 0.0), + position * Vector::new(0.0, 0.0, -2.0), ]; let indices = vec![ @@ -27,9 +26,9 @@ fn build_diamond(position: &Isometry) -> TriMesh { #[test] fn trimesh_plane_edge_intersection() { - let mesh = build_diamond(&Isometry::identity()); + let mesh = build_diamond(&Pose::identity()); - let result = mesh.intersection_with_local_plane(&Vector3::ith_axis(2), 0.5, core::f32::EPSILON); + let result = mesh.intersection_with_local_plane(Vector::Z, 0.5, core::f32::EPSILON); assert!(matches!(result, IntersectResult::Intersect(_))); @@ -37,17 +36,17 @@ fn trimesh_plane_edge_intersection() { // Need to check points individually since order is not guaranteed let vertices = line.vertices(); assert_eq!(vertices.len(), 3); - assert!(vertices.contains(&Point3::new(-1.5, -0.75, 0.5))); - assert!(vertices.contains(&Point3::new(1.5, -0.75, 0.5))); - assert!(vertices.contains(&Point3::new(0.0, 1.5, 0.5))); + assert!(vertices.contains(&Vector::new(-1.5, -0.75, 0.5))); + assert!(vertices.contains(&Vector::new(1.5, -0.75, 0.5))); + assert!(vertices.contains(&Vector::new(0.0, 1.5, 0.5))); } } #[test] fn trimesh_plane_vertex_intersection() { - let mesh = build_diamond(&Isometry::identity()); + let mesh = build_diamond(&Pose::identity()); - let result = mesh.intersection_with_local_plane(&Vector3::ith_axis(2), 0.0, core::f32::EPSILON); + let result = mesh.intersection_with_local_plane(Vector::Z, 0.0, core::f32::EPSILON); assert!(matches!(result, IntersectResult::Intersect(_))); @@ -55,17 +54,17 @@ fn trimesh_plane_vertex_intersection() { // Need to check points individually since order is not guaranteed let vertices = line.vertices(); assert_eq!(vertices.len(), 3); - assert!(vertices.contains(&Point3::new(-2.0, -1.0, 0.0))); - assert!(vertices.contains(&Point3::new(2.0, -1.0, 0.0))); - assert!(vertices.contains(&Point3::new(0.0, 2.0, 0.0))); + assert!(vertices.contains(&Vector::new(-2.0, -1.0, 0.0))); + assert!(vertices.contains(&Vector::new(2.0, -1.0, 0.0))); + assert!(vertices.contains(&Vector::new(0.0, 2.0, 0.0))); } } #[test] fn trimesh_plane_mixed_intersection() { - let mesh = build_diamond(&Isometry::identity()); + let mesh = build_diamond(&Pose::identity()); - let result = mesh.intersection_with_local_plane(&Vector3::ith_axis(0), 0.0, core::f32::EPSILON); + let result = mesh.intersection_with_local_plane(Vector::X, 0.0, core::f32::EPSILON); assert!(matches!(result, IntersectResult::Intersect(_))); @@ -73,19 +72,19 @@ fn trimesh_plane_mixed_intersection() { // Need to check points individually since order is not guaranteed let vertices = line.vertices(); assert_eq!(vertices.len(), 4); - assert!(vertices.contains(&Point3::new(0.0, 2.0, 0.0))); - assert!(vertices.contains(&Point3::new(0.0, 0.0, 2.0))); - assert!(vertices.contains(&Point3::new(0.0, -1.0, 0.0))); - assert!(vertices.contains(&Point3::new(0.0, 0.0, -2.0))); + assert!(vertices.contains(&Vector::new(0.0, 2.0, 0.0))); + assert!(vertices.contains(&Vector::new(0.0, 0.0, 2.0))); + assert!(vertices.contains(&Vector::new(0.0, -1.0, 0.0))); + assert!(vertices.contains(&Vector::new(0.0, 0.0, -2.0))); } } #[test] fn trimesh_plane_multi_intersection() { - let mut mesh = build_diamond(&Isometry::identity()); - mesh.append(&build_diamond(&Isometry::translation(-5.0, 0.0, 0.0))); + let mut mesh = build_diamond(&Pose::identity()); + mesh.append(&build_diamond(&Pose::translation(-5.0, 0.0, 0.0))); - let result = mesh.intersection_with_local_plane(&Vector3::ith_axis(2), 0.5, core::f32::EPSILON); + let result = mesh.intersection_with_local_plane(Vector::Z, 0.5, core::f32::EPSILON); assert!(matches!(result, IntersectResult::Intersect(_))); @@ -94,31 +93,30 @@ fn trimesh_plane_multi_intersection() { let vertices = line.vertices(); assert_eq!(vertices.len(), 6); - assert!(vertices.contains(&Point3::new(-1.5, -0.75, 0.5))); - assert!(vertices.contains(&Point3::new(1.5, -0.75, 0.5))); - assert!(vertices.contains(&Point3::new(0.0, 1.5, 0.5))); + assert!(vertices.contains(&Vector::new(-1.5, -0.75, 0.5))); + assert!(vertices.contains(&Vector::new(1.5, -0.75, 0.5))); + assert!(vertices.contains(&Vector::new(0.0, 1.5, 0.5))); - assert!(vertices.contains(&Point3::new(-6.5, -0.75, 0.5))); - assert!(vertices.contains(&Point3::new(-3.5, -0.75, 0.5))); - assert!(vertices.contains(&Point3::new(-5.0, 1.5, 0.5))); + assert!(vertices.contains(&Vector::new(-6.5, -0.75, 0.5))); + assert!(vertices.contains(&Vector::new(-3.5, -0.75, 0.5))); + assert!(vertices.contains(&Vector::new(-5.0, 1.5, 0.5))); } } #[test] fn trimesh_plane_above() { - let mesh = build_diamond(&Isometry::identity()); + let mesh = build_diamond(&Pose::identity()); - let result = - mesh.intersection_with_local_plane(&Vector3::ith_axis(2), -5.0, core::f32::EPSILON); + let result = mesh.intersection_with_local_plane(Vector::Z, -5.0, core::f32::EPSILON); assert!(matches!(result, IntersectResult::Positive)); } #[test] fn trimesh_plane_below() { - let mesh = build_diamond(&Isometry::identity()); + let mesh = build_diamond(&Pose::identity()); - let result = mesh.intersection_with_local_plane(&Vector3::ith_axis(2), 5.0, core::f32::EPSILON); + let result = mesh.intersection_with_local_plane(Vector::Z, 5.0, core::f32::EPSILON); assert!(matches!(result, IntersectResult::Negative)); } diff --git a/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs b/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs index ae08c636..7a036c42 100644 --- a/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs +++ b/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs @@ -1,16 +1,15 @@ // Issue #194 -use na::{zero, Isometry3, Point3, Vector3}; -use parry3d::math::Real; +use parry3d::math::{Pose, Real, Vector}; use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::TriMesh; fn build_pyramid() -> TriMesh { let points = vec![ - Point3::new(0.0, 1.0, 0.0), - Point3::new(-1.0, -0.5, 0.0), - Point3::new(0.0, -0.5, -1.0), - Point3::new(1.0, -0.5, 0.0), + Vector::new(0.0, 1.0, 0.0), + Vector::new(-1.0, -0.5, 0.0), + Vector::new(0.0, -0.5, -1.0), + Vector::new(1.0, -0.5, 0.0), ]; let indices = vec![[0u32, 1, 2], [0, 2, 3], [0, 3, 1]]; @@ -24,21 +23,21 @@ fn do_toi_test() -> Option { let shape_one = build_pyramid(); let shape_two = build_pyramid(); - let pos_one = Vector3::new(0.0, 0.0, 0.0); - let pos_two = Vector3::new(1000.0, 0.0, 0.0); + let pos_one = Vector::new(0.0, 0.0, 0.0); + let pos_two = Vector::new(1000.0, 0.0, 0.0); - let transform_one = Isometry3::new(pos_one, zero()); - let transform_two = Isometry3::new(pos_two, zero()); + let transform_one = Pose::from_parts(pos_one.into(), parry3d::glamx::Quat::IDENTITY); + let transform_two = Pose::from_parts(pos_two.into(), parry3d::glamx::Quat::IDENTITY); - let vel_one = Vector3::new(SPEED, 0.0, 0.0); - let vel_two = Vector3::new(0.0, 0.0, 0.0); + let vel_one = Vector::new(SPEED, 0.0, 0.0); + let vel_two = Vector::new(0.0, 0.0, 0.0); query::cast_shapes( &transform_one, - &vel_one, + vel_one, &shape_one, &transform_two, - &vel_two, + vel_two, &shape_two, ShapeCastOptions::default(), ) diff --git a/crates/parry3d/tests/lib.rs b/crates/parry3d/tests/lib.rs index 8f4fb260..fefc2c53 100644 --- a/crates/parry3d/tests/lib.rs +++ b/crates/parry3d/tests/lib.rs @@ -1,6 +1,5 @@ #[macro_use] extern crate approx; -extern crate nalgebra as na; extern crate parry3d; mod geometry; diff --git a/run_visual_examples.sh b/run_visual_examples.sh new file mode 100755 index 00000000..c1d3de27 --- /dev/null +++ b/run_visual_examples.sh @@ -0,0 +1,60 @@ +#!/bin/bash +# Run all parry 2D and 3D examples that use kiss3d for visualization. +# Close each window to proceed to the next example. + +set -e + +cd "$(dirname "$0")" + +# 3D examples using kiss3d +EXAMPLES_3D=( + aabb3d + bounding_sphere3d + convex_hull3d + plane_intersection + project_point3d +) + +# 3D examples requiring the wavefront feature +EXAMPLES_3D_WAVEFRONT=( + convex_decomposition +) + +# 2D examples using kiss3d +EXAMPLES_2D=( + aabb2d + bounding_sphere2d + convex_hull2d + project_point2d + point_in_poly2d + polygons_intersection2d + raycasts_animated +) + +TOTAL=$((${#EXAMPLES_3D[@]} + ${#EXAMPLES_3D_WAVEFRONT[@]} + ${#EXAMPLES_2D[@]})) +echo "Running $TOTAL parry examples with kiss3d visualization..." +echo "Close each window to proceed to the next example." +echo "" + +echo "=== Running 3D examples ===" +for example in "${EXAMPLES_3D[@]}"; do + echo "=== Running: $example (parry3d) ===" + cargo run --release -p parry3d --example "$example" + echo "" +done + +echo "=== Running 3D examples (with wavefront feature) ===" +for example in "${EXAMPLES_3D_WAVEFRONT[@]}"; do + echo "=== Running: $example (parry3d) ===" + cargo run --release -p parry3d --features wavefront --example "$example" + echo "" +done + +echo "=== Running 2D examples ===" +for example in "${EXAMPLES_2D[@]}"; do + echo "=== Running: $example (parry2d) ===" + cargo run --release -p parry2d --example "$example" + echo "" +done + +echo "All examples completed!" diff --git a/src/bounding_volume/aabb.rs b/src/bounding_volume/aabb.rs index 2fcce281..35c8f0d8 100644 --- a/src/bounding_volume/aabb.rs +++ b/src/bounding_volume/aabb.rs @@ -1,20 +1,13 @@ //! Axis Aligned Bounding Box. use crate::bounding_volume::{BoundingSphere, BoundingVolume}; -use crate::math::{Isometry, Point, Real, UnitVector, Vector, DIM, TWO_DIM}; +use crate::math::{Pose, Real, Vector, DIM, TWO_DIM}; use crate::shape::{Cuboid, SupportMap}; -use crate::utils::IsometryOps; +use crate::utils::PoseOps; use arrayvec::ArrayVec; -use na; use num::Bounded; -#[cfg(all(feature = "dim3", not(feature = "std")))] -use na::ComplexField; -// for .sin_cos() - use crate::query::{Ray, RayCast}; -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// An Axis-Aligned Bounding Box (AABB). /// @@ -63,19 +56,19 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create an AABB for a unit cube centered at origin -/// let mins = Point3::new(-0.5, -0.5, -0.5); -/// let maxs = Point3::new(0.5, 0.5, 0.5); +/// let mins = Vector::new(-0.5, -0.5, -0.5); +/// let maxs = Vector::new(0.5, 0.5, 0.5); /// let aabb = Aabb::new(mins, maxs); /// /// // Check if a point is inside -/// let point = Point3::origin(); -/// assert!(aabb.contains_local_point(&point)); +/// let point = Vector::ZERO; +/// assert!(aabb.contains_local_point(point)); /// /// // Get center and extents -/// assert_eq!(aabb.center(), Point3::origin()); +/// assert_eq!(aabb.center(), Vector::ZERO); /// assert_eq!(aabb.extents().x, 1.0); // Full width /// assert_eq!(aabb.half_extents().x, 0.5); // Half width /// # } @@ -84,27 +77,26 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create from a set of points /// let points = vec![ -/// Point3::new(1.0, 2.0, 3.0), -/// Point3::new(-1.0, 4.0, 2.0), -/// Point3::new(0.0, 0.0, 5.0), +/// Vector::new(1.0, 2.0, 3.0), +/// Vector::new(-1.0, 4.0, 2.0), +/// Vector::new(0.0, 0.0, 5.0), /// ]; /// let aabb = Aabb::from_points(points); /// /// // The AABB encloses all points -/// assert_eq!(aabb.mins, Point3::new(-1.0, 0.0, 2.0)); -/// assert_eq!(aabb.maxs, Point3::new(1.0, 4.0, 5.0)); +/// assert_eq!(aabb.mins, Vector::new(-1.0, 0.0, 2.0)); +/// assert_eq!(aabb.maxs, Vector::new(1.0, 4.0, 5.0)); /// # } /// ``` #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Debug, PartialEq, Copy, Clone)] #[repr(C)] @@ -113,13 +105,13 @@ pub struct Aabb { /// /// Each component (`x`, `y`, `z`) should be less than or equal to the /// corresponding component in `maxs`. - pub mins: Point, + pub mins: Vector, /// The point with maximum coordinates (top-right-front corner). /// /// Each component (`x`, `y`, `z`) should be greater than or equal to the /// corresponding component in `mins`. - pub maxs: Point, + pub maxs: Vector, } impl Aabb { @@ -223,20 +215,20 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a 2x2x2 cube centered at origin /// let aabb = Aabb::new( - /// Point3::new(-1.0, -1.0, -1.0), - /// Point3::new(1.0, 1.0, 1.0) + /// Vector::new(-1.0, -1.0, -1.0), + /// Vector::new(1.0, 1.0, 1.0) /// ); /// - /// assert_eq!(aabb.center(), Point3::origin()); - /// assert_eq!(aabb.extents(), nalgebra::Vector3::new(2.0, 2.0, 2.0)); + /// assert_eq!(aabb.center(), Vector::ZERO); + /// assert_eq!(aabb.extents(), Vector::new(2.0, 2.0, 2.0)); /// # } /// ``` #[inline] - pub fn new(mins: Point, maxs: Point) -> Aabb { + pub fn new(mins: Vector, maxs: Vector) -> Aabb { Aabb { mins, maxs } } @@ -251,24 +243,24 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{Aabb, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut aabb = Aabb::new_invalid(); /// /// // Merge with actual points to build proper AABB - /// aabb.merge(&Aabb::new(Point3::new(1.0, 2.0, 3.0), Point3::new(1.0, 2.0, 3.0))); - /// aabb.merge(&Aabb::new(Point3::new(-1.0, 0.0, 2.0), Point3::new(-1.0, 0.0, 2.0))); + /// aabb.merge(&Aabb::new(Vector::new(1.0, 2.0, 3.0), Vector::new(1.0, 2.0, 3.0))); + /// aabb.merge(&Aabb::new(Vector::new(-1.0, 0.0, 2.0), Vector::new(-1.0, 0.0, 2.0))); /// /// // Now contains the merged bounds - /// assert_eq!(aabb.mins, Point3::new(-1.0, 0.0, 2.0)); - /// assert_eq!(aabb.maxs, Point3::new(1.0, 2.0, 3.0)); + /// assert_eq!(aabb.mins, Vector::new(-1.0, 0.0, 2.0)); + /// assert_eq!(aabb.maxs, Vector::new(1.0, 2.0, 3.0)); /// # } /// ``` #[inline] pub fn new_invalid() -> Self { Self::new( - Vector::repeat(Real::max_value()).into(), - Vector::repeat(-Real::max_value()).into(), + Vector::splat(Real::max_value()), + Vector::splat(-Real::max_value()), ) } @@ -286,21 +278,21 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// // Create a 10x6x8 box centered at (5, 0, 0) /// let aabb = Aabb::from_half_extents( - /// Point3::new(5.0, 0.0, 0.0), - /// Vector3::new(5.0, 3.0, 4.0) + /// Vector::new(5.0, 0.0, 0.0), + /// Vector::new(5.0, 3.0, 4.0) /// ); /// - /// assert_eq!(aabb.mins, Point3::new(0.0, -3.0, -4.0)); - /// assert_eq!(aabb.maxs, Point3::new(10.0, 3.0, 4.0)); - /// assert_eq!(aabb.center(), Point3::new(5.0, 0.0, 0.0)); + /// assert_eq!(aabb.mins, Vector::new(0.0, -3.0, -4.0)); + /// assert_eq!(aabb.maxs, Vector::new(10.0, 3.0, 4.0)); + /// assert_eq!(aabb.center(), Vector::new(5.0, 0.0, 0.0)); /// # } /// ``` #[inline] - pub fn from_half_extents(center: Point, half_extents: Vector) -> Self { + pub fn from_half_extents(center: Vector, half_extents: Vector) -> Self { Self::new(center - half_extents, center + half_extents) } @@ -317,22 +309,22 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let points = vec![ - /// Point3::new(1.0, 2.0, 3.0), - /// Point3::new(-1.0, 4.0, 2.0), - /// Point3::new(0.0, 0.0, 5.0), + /// Vector::new(1.0, 2.0, 3.0), + /// Vector::new(-1.0, 4.0, 2.0), + /// Vector::new(0.0, 0.0, 5.0), /// ]; /// /// let aabb = Aabb::from_points_ref(&points); - /// assert_eq!(aabb.mins, Point3::new(-1.0, 0.0, 2.0)); - /// assert_eq!(aabb.maxs, Point3::new(1.0, 4.0, 5.0)); + /// assert_eq!(aabb.mins, Vector::new(-1.0, 0.0, 2.0)); + /// assert_eq!(aabb.maxs, Vector::new(1.0, 4.0, 5.0)); /// # } /// ``` pub fn from_points_ref<'a, I>(pts: I) -> Self where - I: IntoIterator>, + I: IntoIterator, { super::aabb_utils::local_point_cloud_aabb(pts.into_iter().copied()) } @@ -350,21 +342,21 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabb = Aabb::from_points(vec![ - /// Point3::new(1.0, 2.0, 3.0), - /// Point3::new(-1.0, 4.0, 2.0), - /// Point3::new(0.0, 0.0, 5.0), + /// Vector::new(1.0, 2.0, 3.0), + /// Vector::new(-1.0, 4.0, 2.0), + /// Vector::new(0.0, 0.0, 5.0), /// ]); /// - /// assert_eq!(aabb.mins, Point3::new(-1.0, 0.0, 2.0)); - /// assert_eq!(aabb.maxs, Point3::new(1.0, 4.0, 5.0)); + /// assert_eq!(aabb.mins, Vector::new(-1.0, 0.0, 2.0)); + /// assert_eq!(aabb.maxs, Vector::new(1.0, 4.0, 5.0)); /// # } /// ``` pub fn from_points(pts: I) -> Self where - I: IntoIterator>, + I: IntoIterator, { super::aabb_utils::local_point_cloud_aabb(pts) } @@ -378,19 +370,19 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabb = Aabb::new( - /// Point3::new(-2.0, -3.0, -4.0), - /// Point3::new(2.0, 3.0, 4.0) + /// Vector::new(-2.0, -3.0, -4.0), + /// Vector::new(2.0, 3.0, 4.0) /// ); /// - /// assert_eq!(aabb.center(), Point3::origin()); + /// assert_eq!(aabb.center(), Vector::ZERO); /// # } /// ``` #[inline] - pub fn center(&self) -> Point { - na::center(&self.mins, &self.maxs) + pub fn center(&self) -> Vector { + self.mins.midpoint(self.maxs) } /// Returns the half-extents of this AABB. @@ -402,24 +394,24 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let aabb = Aabb::new( - /// Point3::new(-5.0, -3.0, -2.0), - /// Point3::new(5.0, 3.0, 2.0) + /// Vector::new(-5.0, -3.0, -2.0), + /// Vector::new(5.0, 3.0, 2.0) /// ); /// /// let half = aabb.half_extents(); - /// assert_eq!(half, Vector3::new(5.0, 3.0, 2.0)); + /// assert_eq!(half, Vector::new(5.0, 3.0, 2.0)); /// /// // Full dimensions are 2 * half_extents /// let full = aabb.extents(); - /// assert_eq!(full, Vector3::new(10.0, 6.0, 4.0)); + /// assert_eq!(full, Vector::new(10.0, 6.0, 4.0)); /// # } /// ``` #[inline] - pub fn half_extents(&self) -> Vector { - let half: Real = na::convert::(0.5); + pub fn half_extents(&self) -> Vector { + let half: Real = 0.5; (self.maxs - self.mins) * half } @@ -433,12 +425,12 @@ impl Aabb { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // A 2x3x4 box /// let aabb = Aabb::new( - /// Point3::origin(), - /// Point3::new(2.0, 3.0, 4.0) + /// Vector::ZERO, + /// Vector::new(2.0, 3.0, 4.0) /// ); /// /// assert_eq!(aabb.volume(), 24.0); // 2 * 3 * 4 @@ -477,41 +469,41 @@ impl Aabb { /// The extents of this `Aabb`. #[inline] - pub fn extents(&self) -> Vector { + pub fn extents(&self) -> Vector { self.maxs - self.mins } /// Enlarges this `Aabb` so it also contains the point `pt`. - pub fn take_point(&mut self, pt: Point) { - self.mins = self.mins.coords.inf(&pt.coords).into(); - self.maxs = self.maxs.coords.sup(&pt.coords).into(); + pub fn take_point(&mut self, pt: Vector) { + self.mins = self.mins.min(pt); + self.maxs = self.maxs.max(pt); } /// Computes the `Aabb` bounding `self` transformed by `m`. #[inline] - pub fn transform_by(&self, m: &Isometry) -> Self { + pub fn transform_by(&self, m: &Pose) -> Self { let ls_center = self.center(); let center = m * ls_center; - let ws_half_extents = m.absolute_transform_vector(&self.half_extents()); + let ws_half_extents = m.absolute_transform_vector(self.half_extents()); Aabb::new(center + (-ws_half_extents), center + ws_half_extents) } /// Computes the Aabb bounding `self` translated by `translation`. #[inline] - pub fn translated(mut self, translation: &Vector) -> Self { + pub fn translated(mut self, translation: Vector) -> Self { self.mins += translation; self.maxs += translation; self } #[inline] - pub fn scaled(self, scale: &Vector) -> Self { - let a = self.mins.coords.component_mul(scale); - let b = self.maxs.coords.component_mul(scale); + pub fn scaled(self, scale: Vector) -> Self { + let a = self.mins * scale; + let b = self.maxs * scale; Self { - mins: a.inf(&b).into(), - maxs: a.sup(&b).into(), + mins: a.min(b), + maxs: a.max(b), } } @@ -523,12 +515,12 @@ impl Aabb { /// by its absolute value. #[inline] #[must_use] - pub fn scaled_wrt_center(self, scale: &Vector) -> Self { + pub fn scaled_wrt_center(self, scale: Vector) -> Self { let center = self.center(); // Multiply the extents by the scale. Negative scaling might modify the half-extent // sign, so we take the absolute value. The AABB being symmetric that absolute value // is valid. - let half_extents = self.half_extents().component_mul(scale).abs(); + let half_extents = self.half_extents() * scale.abs(); Self::from_half_extents(center, half_extents) } @@ -536,13 +528,13 @@ impl Aabb { #[inline] pub fn bounding_sphere(&self) -> BoundingSphere { let center = self.center(); - let radius = na::distance(&self.mins, &self.maxs) * 0.5; + let radius = self.mins.distance(self.maxs) * 0.5; BoundingSphere::new(center, radius) } /// Does this AABB contains a point expressed in the same coordinate frame as `self`? #[inline] - pub fn contains_local_point(&self, point: &Point) -> bool { + pub fn contains_local_point(&self, point: Vector) -> bool { for i in 0..DIM { if point[i] < self.mins[i] || point[i] > self.maxs[i] { return false; @@ -554,22 +546,18 @@ impl Aabb { /// Computes the distance between the origin and this AABB. pub fn distance_to_origin(&self) -> Real { - self.mins - .coords - .sup(&-self.maxs.coords) - .sup(&Vector::zeros()) - .norm() + self.mins.max(-self.maxs).max(Vector::ZERO).length() } /// Does this AABB intersects an AABB `aabb2` moving at velocity `vel12` relative to `self`? #[inline] - pub fn intersects_moving_aabb(&self, aabb2: &Self, vel12: Vector) -> bool { + pub fn intersects_moving_aabb(&self, aabb2: &Self, vel12: Vector) -> bool { // Minkowski sum. let msum = Aabb { - mins: self.mins - aabb2.maxs.coords, - maxs: self.maxs - aabb2.mins.coords, + mins: self.mins - aabb2.maxs, + maxs: self.maxs - aabb2.mins, }; - let ray = Ray::new(Point::origin(), vel12); + let ray = Ray::new(Vector::ZERO, vel12); msum.intersects_local_ray(&ray, 1.0) } @@ -577,8 +565,8 @@ impl Aabb { /// Computes the intersection of this `Aabb` and another one. pub fn intersection(&self, other: &Aabb) -> Option { let result = Aabb { - mins: Point::from(self.mins.coords.sup(&other.mins.coords)), - maxs: Point::from(self.maxs.coords.inf(&other.maxs.coords)), + mins: self.mins.max(other.mins), + maxs: self.maxs.min(other.maxs), }; for i in 0..DIM { @@ -594,11 +582,7 @@ impl Aabb { /// /// This method returns two AABBs: the first is expressed in the local-space of `self`, /// and the second is expressed in the local-space of `aabb2`. - pub fn aligned_intersections( - &self, - pos12: &Isometry, - aabb2: &Self, - ) -> Option<(Aabb, Aabb)> { + pub fn aligned_intersections(&self, pos12: &Pose, aabb2: &Self) -> Option<(Aabb, Aabb)> { let pos21 = pos12.inverse(); let aabb2_1 = aabb2.transform_by(pos12); @@ -691,12 +675,12 @@ impl Aabb { /// ``` #[inline] #[cfg(feature = "dim2")] - pub fn vertices(&self) -> [Point; 4] { + pub fn vertices(&self) -> [Vector; 4] { [ - Point::new(self.mins.x, self.mins.y), - Point::new(self.maxs.x, self.mins.y), - Point::new(self.maxs.x, self.maxs.y), - Point::new(self.mins.x, self.maxs.y), + Vector::new(self.mins.x, self.mins.y), + Vector::new(self.maxs.x, self.mins.y), + Vector::new(self.maxs.x, self.maxs.y), + Vector::new(self.mins.x, self.maxs.y), ] } @@ -712,16 +696,16 @@ impl Aabb { /// ``` #[inline] #[cfg(feature = "dim3")] - pub fn vertices(&self) -> [Point; 8] { + pub fn vertices(&self) -> [Vector; 8] { [ - Point::new(self.mins.x, self.mins.y, self.mins.z), - Point::new(self.maxs.x, self.mins.y, self.mins.z), - Point::new(self.maxs.x, self.maxs.y, self.mins.z), - Point::new(self.mins.x, self.maxs.y, self.mins.z), - Point::new(self.mins.x, self.mins.y, self.maxs.z), - Point::new(self.maxs.x, self.mins.y, self.maxs.z), - Point::new(self.maxs.x, self.maxs.y, self.maxs.z), - Point::new(self.mins.x, self.maxs.y, self.maxs.z), + Vector::new(self.mins.x, self.mins.y, self.mins.z), + Vector::new(self.maxs.x, self.mins.y, self.mins.z), + Vector::new(self.maxs.x, self.maxs.y, self.mins.z), + Vector::new(self.mins.x, self.maxs.y, self.mins.z), + Vector::new(self.mins.x, self.mins.y, self.maxs.z), + Vector::new(self.maxs.x, self.mins.y, self.maxs.z), + Vector::new(self.maxs.x, self.maxs.y, self.maxs.z), + Vector::new(self.mins.x, self.maxs.y, self.maxs.z), ] } @@ -734,13 +718,13 @@ impl Aabb { [ Aabb::new(self.mins, center), Aabb::new( - Point::new(center.x, self.mins.y), - Point::new(self.maxs.x, center.y), + Vector::new(center.x, self.mins.y), + Vector::new(self.maxs.x, center.y), ), Aabb::new(center, self.maxs), Aabb::new( - Point::new(self.mins.x, center.y), - Point::new(center.x, self.maxs.y), + Vector::new(self.mins.x, center.y), + Vector::new(center.x, self.maxs.y), ), ] } @@ -753,43 +737,43 @@ impl Aabb { [ Aabb::new( - Point::new(self.mins.x, self.mins.y, self.mins.z), - Point::new(center.x, center.y, center.z), + Vector::new(self.mins.x, self.mins.y, self.mins.z), + Vector::new(center.x, center.y, center.z), ), Aabb::new( - Point::new(center.x, self.mins.y, self.mins.z), - Point::new(self.maxs.x, center.y, center.z), + Vector::new(center.x, self.mins.y, self.mins.z), + Vector::new(self.maxs.x, center.y, center.z), ), Aabb::new( - Point::new(center.x, center.y, self.mins.z), - Point::new(self.maxs.x, self.maxs.y, center.z), + Vector::new(center.x, center.y, self.mins.z), + Vector::new(self.maxs.x, self.maxs.y, center.z), ), Aabb::new( - Point::new(self.mins.x, center.y, self.mins.z), - Point::new(center.x, self.maxs.y, center.z), + Vector::new(self.mins.x, center.y, self.mins.z), + Vector::new(center.x, self.maxs.y, center.z), ), Aabb::new( - Point::new(self.mins.x, self.mins.y, center.z), - Point::new(center.x, center.y, self.maxs.z), + Vector::new(self.mins.x, self.mins.y, center.z), + Vector::new(center.x, center.y, self.maxs.z), ), Aabb::new( - Point::new(center.x, self.mins.y, center.z), - Point::new(self.maxs.x, center.y, self.maxs.z), + Vector::new(center.x, self.mins.y, center.z), + Vector::new(self.maxs.x, center.y, self.maxs.z), ), Aabb::new( - Point::new(center.x, center.y, center.z), - Point::new(self.maxs.x, self.maxs.y, self.maxs.z), + Vector::new(center.x, center.y, center.z), + Vector::new(self.maxs.x, self.maxs.y, self.maxs.z), ), Aabb::new( - Point::new(self.mins.x, center.y, center.z), - Point::new(center.x, self.maxs.y, self.maxs.z), + Vector::new(self.mins.x, center.y, center.z), + Vector::new(center.x, self.maxs.y, self.maxs.z), ), ] } /// Enlarges this AABB on each side by the given `half_extents`. #[must_use] - pub fn add_half_extents(&self, half_extents: &Vector) -> Self { + pub fn add_half_extents(&self, half_extents: Vector) -> Self { Self { mins: self.mins - half_extents, maxs: self.maxs + half_extents, @@ -797,14 +781,10 @@ impl Aabb { } /// Projects every point of `Aabb` on an arbitrary axis. - pub fn project_on_axis(&self, axis: &UnitVector) -> (Real, Real) { + pub fn project_on_axis(&self, axis: Vector) -> (Real, Real) { let cuboid = Cuboid::new(self.half_extents()); - let shift = cuboid - .local_support_point_toward(axis) - .coords - .dot(axis) - .abs(); - let center = self.center().coords.dot(axis); + let shift = cuboid.local_support_point_toward(axis).dot(axis).abs(); + let center = self.center().dot(axis); (center - shift, center + shift) } @@ -812,84 +792,125 @@ impl Aabb { #[cfg(feature = "alloc")] pub fn intersects_spiral( &self, - point: &Point, - center: &Point, - axis: &UnitVector, - linvel: &Vector, + point: Vector, + spiral_center: Vector, + axis: Vector, + linvel: Vector, angvel: Real, ) -> bool { + use crate::math::{Matrix2, Vector2}; use crate::utils::WBasis; use crate::utils::{Interval, IntervalFunction}; use alloc::vec; struct SpiralPlaneDistance { - center: Point, - tangents: [Vector; 2], - linvel: Vector, + spiral_center: Vector, + tangents: [Vector; 2], + linvel: Vector, angvel: Real, - point: na::Vector2, - plane: Vector, + point: Vector2, + plane: Vector, bias: Real, } impl SpiralPlaneDistance { - fn spiral_pt_at(&self, t: Real) -> Point { + fn spiral_pt_at(&self, t: Real) -> Vector { let angle = t * self.angvel; // NOTE: we construct the rotation matrix explicitly here instead // of using `Rotation2::new()` because we will use similar // formulas on the interval methods. - let (sin, cos) = angle.sin_cos(); - let rotmat = na::Matrix2::new(cos, -sin, sin, cos); + let (sin, cos) = ::sin_cos(angle); + // Mat2 in column-major: first column is [cos, sin], second is [-sin, cos] + let rotmat = Matrix2::from_cols(Vector2::new(cos, sin), Vector2::new(-sin, cos)); let rotated_pt = rotmat * self.point; let shift = self.tangents[0] * rotated_pt.x + self.tangents[1] * rotated_pt.y; - self.center + self.linvel * t + shift + self.spiral_center + self.linvel * t + shift } } impl IntervalFunction for SpiralPlaneDistance { fn eval(&self, t: Real) -> Real { let point_pos = self.spiral_pt_at(t); - point_pos.coords.dot(&self.plane) - self.bias + point_pos.dot(self.plane) - self.bias } fn eval_interval(&self, t: Interval) -> Interval { // This is the same as `self.eval` except that `t` is an interval. let angle = t * self.angvel; let (sin, cos) = angle.sin_cos(); - let rotmat = na::Matrix2::new(cos, -sin, sin, cos); - - let rotated_pt = rotmat * self.point.map(Interval::splat); - let shift = self.tangents[0].map(Interval::splat) * rotated_pt.x - + self.tangents[1].map(Interval::splat) * rotated_pt.y; - let point_pos = - self.center.map(Interval::splat) + self.linvel.map(Interval::splat) * t + shift; - point_pos.coords.dot(&self.plane.map(Interval::splat)) - Interval::splat(self.bias) + + // Compute rotated point manually with interval arithmetic + let rotated_pt_x = + cos * Interval::splat(self.point.x) - sin * Interval::splat(self.point.y); + let rotated_pt_y = + sin * Interval::splat(self.point.x) + cos * Interval::splat(self.point.y); + + let shift_x = Interval::splat(self.tangents[0].x) * rotated_pt_x + + Interval::splat(self.tangents[1].x) * rotated_pt_y; + let shift_y = Interval::splat(self.tangents[0].y) * rotated_pt_x + + Interval::splat(self.tangents[1].y) * rotated_pt_y; + let shift_z = Interval::splat(self.tangents[0].z) * rotated_pt_x + + Interval::splat(self.tangents[1].z) * rotated_pt_y; + + let point_pos_x = Interval::splat(self.spiral_center.x) + + Interval::splat(self.linvel.x) * t + + shift_x; + let point_pos_y = Interval::splat(self.spiral_center.y) + + Interval::splat(self.linvel.y) * t + + shift_y; + let point_pos_z = Interval::splat(self.spiral_center.z) + + Interval::splat(self.linvel.z) * t + + shift_z; + + point_pos_x * Interval::splat(self.plane.x) + + point_pos_y * Interval::splat(self.plane.y) + + point_pos_z * Interval::splat(self.plane.z) + - Interval::splat(self.bias) } fn eval_interval_gradient(&self, t: Interval) -> Interval { let angle = t * self.angvel; let (sin, cos) = angle.sin_cos(); - let rotmat = na::Matrix2::new(-sin, -cos, cos, -sin) * Interval::splat(self.angvel); - - let rotated_pt = rotmat * self.point.map(Interval::splat); - let shift = self.tangents[0].map(Interval::splat) * rotated_pt.x - + self.tangents[1].map(Interval::splat) * rotated_pt.y; - let point_vel = shift + self.linvel.map(Interval::splat); - point_vel.dot(&self.plane.map(Interval::splat)) + let angvel_interval = Interval::splat(self.angvel); + + // Derivative of rotation matrix applied to point + let rotated_pt_x = (-sin * angvel_interval) * Interval::splat(self.point.x) + - (cos * angvel_interval) * Interval::splat(self.point.y); + let rotated_pt_y = (cos * angvel_interval) * Interval::splat(self.point.x) + + (-sin * angvel_interval) * Interval::splat(self.point.y); + + let shift_x = Interval::splat(self.tangents[0].x) * rotated_pt_x + + Interval::splat(self.tangents[1].x) * rotated_pt_y; + let shift_y = Interval::splat(self.tangents[0].y) * rotated_pt_x + + Interval::splat(self.tangents[1].y) * rotated_pt_y; + let shift_z = Interval::splat(self.tangents[0].z) * rotated_pt_x + + Interval::splat(self.tangents[1].z) * rotated_pt_y; + + let point_vel_x = shift_x + Interval::splat(self.linvel.x); + let point_vel_y = shift_y + Interval::splat(self.linvel.y); + let point_vel_z = shift_z + Interval::splat(self.linvel.z); + + point_vel_x * Interval::splat(self.plane.x) + + point_vel_y * Interval::splat(self.plane.y) + + point_vel_z * Interval::splat(self.plane.z) } } let tangents = axis.orthonormal_basis(); - let dpos = point - center; + let dpos = point - spiral_center; + #[cfg(feature = "f32")] + let spiral_point = glamx::Vec2::new(dpos.dot(tangents[0]), dpos.dot(tangents[1])); + #[cfg(feature = "f64")] + let spiral_point = glamx::DVec2::new(dpos.dot(tangents[0]), dpos.dot(tangents[1])); let mut distance_fn = SpiralPlaneDistance { - center: *center, + spiral_center, tangents, - linvel: *linvel, + linvel, angvel, - point: na::Vector2::new(dpos.dot(&tangents[0]), dpos.dot(&tangents[1])), - plane: Vector::x(), + point: spiral_point, + plane: Vector::X, bias: 0.0, }; @@ -898,16 +919,16 @@ impl Aabb { let mut candidates = vec![]; let planes = [ - (-self.mins[0], -Vector::x(), 0), - (self.maxs[0], Vector::x(), 0), - (-self.mins[1], -Vector::y(), 1), - (self.maxs[1], Vector::y(), 1), - (-self.mins[2], -Vector::z(), 2), - (self.maxs[2], Vector::z(), 2), + (-self.mins[0], -Vector::X, 0), + (self.maxs[0], Vector::X, 0), + (-self.mins[1], -Vector::Y, 1), + (self.maxs[1], Vector::Y, 1), + (-self.mins[2], -Vector::Z, 2), + (self.maxs[2], Vector::Z, 2), ]; let range = self.project_on_axis(axis); - let range_bias = center.coords.dot(axis); + let range_bias = spiral_center.dot(axis); let interval = Interval::sort(range.0, range.1) - range_bias; for (bias, axis, i) in &planes { @@ -943,57 +964,57 @@ impl Aabb { impl BoundingVolume for Aabb { #[inline] - fn center(&self) -> Point { + fn center(&self) -> Vector { self.center() } #[inline] fn intersects(&self, other: &Aabb) -> bool { - na::partial_le(&self.mins, &other.maxs) && na::partial_ge(&self.maxs, &other.mins) + (self.mins.cmple(other.maxs) & self.maxs.cmpge(other.mins)).all() } #[inline] fn contains(&self, other: &Aabb) -> bool { - na::partial_le(&self.mins, &other.mins) && na::partial_ge(&self.maxs, &other.maxs) + (self.mins.cmple(other.mins) & self.maxs.cmpge(other.maxs)).all() } #[inline] fn merge(&mut self, other: &Aabb) { - self.mins = self.mins.inf(&other.mins); - self.maxs = self.maxs.sup(&other.maxs); + self.mins = self.mins.min(other.mins); + self.maxs = self.maxs.max(other.maxs); } #[inline] fn merged(&self, other: &Aabb) -> Aabb { Aabb { - mins: self.mins.inf(&other.mins), - maxs: self.maxs.sup(&other.maxs), + mins: self.mins.min(other.mins), + maxs: self.maxs.max(other.maxs), } } #[inline] fn loosen(&mut self, amount: Real) { assert!(amount >= 0.0, "The loosening margin must be positive."); - self.mins += Vector::repeat(-amount); - self.maxs += Vector::repeat(amount); + self.mins += Vector::splat(-amount); + self.maxs += Vector::splat(amount); } #[inline] fn loosened(&self, amount: Real) -> Aabb { assert!(amount >= 0.0, "The loosening margin must be positive."); Aabb { - mins: self.mins + Vector::repeat(-amount), - maxs: self.maxs + Vector::repeat(amount), + mins: self.mins + Vector::splat(-amount), + maxs: self.maxs + Vector::splat(amount), } } #[inline] fn tighten(&mut self, amount: Real) { assert!(amount >= 0.0, "The tightening margin must be positive."); - self.mins += Vector::repeat(amount); - self.maxs += Vector::repeat(-amount); + self.mins += Vector::splat(amount); + self.maxs += Vector::splat(-amount); assert!( - na::partial_le(&self.mins, &self.maxs), + self.mins.cmple(self.maxs).all(), "The tightening margin is to large." ); } @@ -1003,8 +1024,8 @@ impl BoundingVolume for Aabb { assert!(amount >= 0.0, "The tightening margin must be positive."); Aabb::new( - self.mins + Vector::repeat(amount), - self.maxs + Vector::repeat(-amount), + self.mins + Vector::splat(amount), + self.maxs + Vector::splat(-amount), ) } } diff --git a/src/bounding_volume/aabb_ball.rs b/src/bounding_volume/aabb_ball.rs index 5d75021d..c06aadad 100644 --- a/src/bounding_volume/aabb_ball.rs +++ b/src/bounding_volume/aabb_ball.rs @@ -1,20 +1,20 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::Ball; /// Computes the Axis-Aligned Bounding Box of a ball transformed by `center`. #[inline] -pub fn ball_aabb(center: &Point, radius: Real) -> Aabb { +pub fn ball_aabb(center: Vector, radius: Real) -> Aabb { Aabb::new( - *center + Vector::repeat(-radius), - *center + Vector::repeat(radius), + center + Vector::splat(-radius), + center + Vector::splat(radius), ) } /// Computes the Axis-Aligned Bounding Box of a ball. #[inline] pub fn local_ball_aabb(radius: Real) -> Aabb { - let half_extents = Point::from(Vector::repeat(radius)); + let half_extents = Vector::splat(radius); Aabb::new(-half_extents, half_extents) } @@ -22,8 +22,8 @@ pub fn local_ball_aabb(radius: Real) -> Aabb { impl Ball { /// Computes the world-space [`Aabb`] of this ball transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { - ball_aabb(&Point::::from(pos.translation.vector), self.radius) + pub fn aabb(&self, pos: &Pose) -> Aabb { + ball_aabb(pos.translation, self.radius) } /// Computes the local-space [`Aabb`] of this ball. diff --git a/src/bounding_volume/aabb_capsule.rs b/src/bounding_volume/aabb_capsule.rs index b735ab05..ed68b615 100644 --- a/src/bounding_volume/aabb_capsule.rs +++ b/src/bounding_volume/aabb_capsule.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Vector}; use crate::shape::Capsule; impl Capsule { /// The axis-aligned bounding box of this capsule. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.transform_by(pos).local_aabb() } @@ -14,8 +14,8 @@ impl Capsule { pub fn local_aabb(&self) -> Aabb { let a = self.segment.a; let b = self.segment.b; - let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); - let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); - Aabb::new(mins.into(), maxs.into()) + let mins = a.min(b) - Vector::splat(self.radius); + let maxs = a.max(b) + Vector::splat(self.radius); + Aabb::new(mins, maxs) } } diff --git a/src/bounding_volume/aabb_convex_polygon.rs b/src/bounding_volume/aabb_convex_polygon.rs index a365c2f8..8021a767 100644 --- a/src/bounding_volume/aabb_convex_polygon.rs +++ b/src/bounding_volume/aabb_convex_polygon.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::ConvexPolygon; impl ConvexPolygon { /// Computes the world-space [`Aabb`] of this convex polygon, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { super::details::point_cloud_aabb_ref(pos, self.points()) } diff --git a/src/bounding_volume/aabb_convex_polyhedron.rs b/src/bounding_volume/aabb_convex_polyhedron.rs index 3dc24b93..aafa8934 100644 --- a/src/bounding_volume/aabb_convex_polyhedron.rs +++ b/src/bounding_volume/aabb_convex_polyhedron.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::ConvexPolyhedron; impl ConvexPolyhedron { /// Computes the world-space [`Aabb`] of this convex polyhedron, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { super::details::point_cloud_aabb_ref(pos, self.points()) } diff --git a/src/bounding_volume/aabb_cuboid.rs b/src/bounding_volume/aabb_cuboid.rs index 4bb2ebe2..aea10969 100644 --- a/src/bounding_volume/aabb_cuboid.rs +++ b/src/bounding_volume/aabb_cuboid.rs @@ -1,14 +1,14 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real}; +use crate::math::Pose; use crate::shape::Cuboid; -use crate::utils::IsometryOps; +use crate::utils::PoseOps; impl Cuboid { /// Computes the world-space [`Aabb`] of this cuboid, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { - let center = Point::from(pos.translation.vector); - let ws_half_extents = pos.absolute_transform_vector(&self.half_extents); + pub fn aabb(&self, pos: &Pose) -> Aabb { + let center = pos.translation; + let ws_half_extents = pos.absolute_transform_vector(self.half_extents); Aabb::from_half_extents(center, ws_half_extents) } @@ -16,8 +16,6 @@ impl Cuboid { /// Computes the local-space [`Aabb`] of this cuboid. #[inline] pub fn local_aabb(&self) -> Aabb { - let half_extents = Point::from(self.half_extents); - - Aabb::new(-half_extents, half_extents) + Aabb::new(-self.half_extents, self.half_extents) } } diff --git a/src/bounding_volume/aabb_halfspace.rs b/src/bounding_volume/aabb_halfspace.rs index 017a04a0..43434ee8 100644 --- a/src/bounding_volume/aabb_halfspace.rs +++ b/src/bounding_volume/aabb_halfspace.rs @@ -1,13 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real}; -use crate::num::Bounded; +use crate::math::{Pose, Vector}; use crate::shape::HalfSpace; -use na; impl HalfSpace { /// Computes the world-space [`Aabb`] of this half-space. #[inline] - pub fn aabb(&self, _pos: &Isometry) -> Aabb { + pub fn aabb(&self, _pos: &Pose) -> Aabb { self.local_aabb() } @@ -16,7 +14,7 @@ impl HalfSpace { pub fn local_aabb(&self) -> Aabb { // We divide by 2.0 so that we can still make some operations with it (like loosening) // without breaking the box. - let max = Point::max_value() * na::convert::(0.5f64); + let max = Vector::MAX * 0.5; Aabb::new(-max, max) } } diff --git a/src/bounding_volume/aabb_heightfield.rs b/src/bounding_volume/aabb_heightfield.rs index c97ae3ce..0baf8e62 100644 --- a/src/bounding_volume/aabb_heightfield.rs +++ b/src/bounding_volume/aabb_heightfield.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::HeightField; impl HeightField { /// Computes the world-space [`Aabb`] of this heightfield, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.root_aabb().transform_by(pos) } diff --git a/src/bounding_volume/aabb_support_map.rs b/src/bounding_volume/aabb_support_map.rs index 754d8ff3..0af1e2a6 100644 --- a/src/bounding_volume/aabb_support_map.rs +++ b/src/bounding_volume/aabb_support_map.rs @@ -1,6 +1,6 @@ use crate::bounding_volume; use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Segment; #[cfg(feature = "dim3")] use crate::shape::{Cone, Cylinder}; @@ -9,7 +9,7 @@ use crate::shape::{Cone, Cylinder}; impl Cone { /// Computes the world-space [`Aabb`] of this cone, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { bounding_volume::details::support_map_aabb(pos, self) } @@ -24,7 +24,7 @@ impl Cone { impl Cylinder { /// Computes the world-space [`Aabb`] of this cylinder, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { bounding_volume::details::support_map_aabb(pos, self) } @@ -38,7 +38,7 @@ impl Cylinder { impl Segment { /// Computes the world-space [`Aabb`] of this segment, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.transformed(pos).local_aabb() } diff --git a/src/bounding_volume/aabb_triangle.rs b/src/bounding_volume/aabb_triangle.rs index 1c8867c0..0717fd4e 100644 --- a/src/bounding_volume/aabb_triangle.rs +++ b/src/bounding_volume/aabb_triangle.rs @@ -1,29 +1,29 @@ use crate::{ bounding_volume::Aabb, - math::{Isometry, Point, Real, DIM}, + math::{Pose, Vector, DIM}, shape::Triangle, }; impl Triangle { /// Computes the world-space [`Aabb`] of this triangle, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.transformed(pos).local_aabb() } /// Computes the local-space [`Aabb`] of this triangle. #[inline] pub fn local_aabb(&self) -> Aabb { - let a = self.a.coords; - let b = self.b.coords; - let c = self.c.coords; + let a = self.a; + let b = self.b; + let c = self.c; - let mut min = Point::origin(); - let mut max = Point::origin(); + let mut min = Vector::ZERO; + let mut max = Vector::ZERO; for d in 0..DIM { - min.coords[d] = a[d].min(b[d]).min(c[d]); - max.coords[d] = a[d].max(b[d]).max(c[d]); + min[d] = a[d].min(b[d]).min(c[d]); + max[d] = a[d].max(b[d]).max(c[d]); } Aabb::new(min, max) @@ -35,22 +35,22 @@ impl Triangle { mod test { use crate::{ bounding_volume::details::support_map_aabb, - math::{Isometry, Point, Real, Translation}, + math::{Pose, Real, Rotation, Vector}, shape::Triangle, }; - use na::{RealField, UnitQuaternion}; + use core::f64::consts::FRAC_PI_2; #[test] fn triangle_aabb_matches_support_map_aabb() { let t = Triangle::new( - Point::new(0.3, -0.1, 0.2), - Point::new(-0.7, 1.0, 0.0), - Point::new(-0.7, 1.5, 0.0), + Vector::new(0.3, -0.1, 0.2), + Vector::new(-0.7, 1.0, 0.0), + Vector::new(-0.7, 1.5, 0.0), ); - let m = Isometry::from_parts( - Translation::new(-0.2, 5.0, 0.2), - UnitQuaternion::from_euler_angles(0.0, Real::frac_pi_2(), 0.0), + let m = Pose::from_parts( + Vector::new(-0.2, 5.0, 0.2), + Rotation::from_euler(glamx::EulerRot::XYZ, 0.0, FRAC_PI_2 as Real, 0.0), ); assert_eq!(t.aabb(&m), support_map_aabb(&m, &t)); diff --git a/src/bounding_volume/aabb_utils.rs b/src/bounding_volume/aabb_utils.rs index 80f54610..8d4f7551 100644 --- a/src/bounding_volume/aabb_utils.rs +++ b/src/bounding_volume/aabb_utils.rs @@ -1,33 +1,32 @@ use core::iter::IntoIterator; use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, Vector, DIM}; +use crate::math::{Pose, Vector, DIM}; use crate::shape::SupportMap; -use na; /// Computes the [`Aabb`] of an [support mapped shape](SupportMap). #[cfg(feature = "dim3")] -pub fn support_map_aabb(m: &Isometry, i: &G) -> Aabb +pub fn support_map_aabb(m: &Pose, i: &G) -> Aabb where G: SupportMap, { - let mut min = na::zero::>(); - let mut max = na::zero::>(); - let mut basis = na::zero::>(); + let mut min = Vector::ZERO; + let mut max = Vector::ZERO; + let mut basis = Vector::ZERO; for d in 0..DIM { // TODO: this could be further improved iterating on `m`'s columns, and passing // Id as the transformation matrix. basis[d] = 1.0; - max[d] = i.support_point(m, &basis)[d]; + max[d] = i.support_point(m, basis)[d]; basis[d] = -1.0; - min[d] = i.support_point(m, &basis)[d]; + min[d] = i.support_point(m, basis)[d]; basis[d] = 0.0; } - Aabb::new(Point::from(min), Point::from(max)) + Aabb::new(min, max) } /// Computes the [`Aabb`] of an [support mapped shape](SupportMap). @@ -35,51 +34,51 @@ pub fn local_support_map_aabb(i: &G) -> Aabb where G: SupportMap, { - let mut min = na::zero::>(); - let mut max = na::zero::>(); - let mut basis = na::zero::>(); + let mut min = Vector::ZERO; + let mut max = Vector::ZERO; + let mut basis = Vector::ZERO; for d in 0..DIM { // TODO: this could be further improved iterating on `m`'s columns, and passing // Id as the transformation matrix. basis[d] = 1.0; - max[d] = i.local_support_point(&basis)[d]; + max[d] = i.local_support_point(basis)[d]; basis[d] = -1.0; - min[d] = i.local_support_point(&basis)[d]; + min[d] = i.local_support_point(basis)[d]; basis[d] = 0.0; } - Aabb::new(Point::from(min), Point::from(max)) + Aabb::new(min, max) } /// Computes the [`Aabb`] of a set of point references transformed by `m`. -pub fn point_cloud_aabb_ref<'a, I>(m: &Isometry, pts: I) -> Aabb +pub fn point_cloud_aabb_ref<'a, I>(m: &Pose, pts: I) -> Aabb where - I: IntoIterator>, + I: IntoIterator, { point_cloud_aabb(m, pts.into_iter().copied()) } /// Computes the [`Aabb`] of a set of points transformed by `m`. -pub fn point_cloud_aabb(m: &Isometry, pts: I) -> Aabb +pub fn point_cloud_aabb(m: &Pose, pts: I) -> Aabb where - I: IntoIterator>, + I: IntoIterator, { let mut it = pts.into_iter(); let p0 = it.next().expect( - "Point cloud Aabb construction: the input iterator should yield at least one point.", + "Vector cloud Aabb construction: the input iterator should yield at least one point.", ); - let wp0 = m.transform_point(&p0); - let mut min: Point = wp0; - let mut max: Point = wp0; + let wp0 = m * p0; + let mut min: Vector = wp0; + let mut max: Vector = wp0; for pt in it { let wpt = m * pt; - min = min.inf(&wpt); - max = max.sup(&wpt); + min = min.min(wpt); + max = max.max(wpt); } Aabb::new(min, max) @@ -88,7 +87,7 @@ where /// Computes the [`Aabb`] of a set of points. pub fn local_point_cloud_aabb_ref<'a, I>(pts: I) -> Aabb where - I: IntoIterator>, + I: IntoIterator, { local_point_cloud_aabb(pts.into_iter().copied()) } @@ -96,19 +95,19 @@ where /// Computes the [`Aabb`] of a set of points. pub fn local_point_cloud_aabb(pts: I) -> Aabb where - I: IntoIterator>, + I: IntoIterator, { let mut it = pts.into_iter(); let p0 = it.next().expect( - "Point cloud Aabb construction: the input iterator should yield at least one point.", + "Vector cloud Aabb construction: the input iterator should yield at least one point.", ); - let mut min: Point = p0; - let mut max: Point = p0; + let mut min: Vector = p0; + let mut max: Vector = p0; for pt in it { - min = min.inf(&pt); - max = max.sup(&pt); + min = min.min(pt); + max = max.max(pt); } Aabb::new(min, max) diff --git a/src/bounding_volume/aabb_voxels.rs b/src/bounding_volume/aabb_voxels.rs index f59e26fa..881ef8a8 100644 --- a/src/bounding_volume/aabb_voxels.rs +++ b/src/bounding_volume/aabb_voxels.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Voxels; impl Voxels { /// Computes the world-space Aabb of this set of voxels, transformed by `pos`. #[inline] - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.chunk_bvh().root_aabb().transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere.rs b/src/bounding_volume/bounding_sphere.rs index 77420a8a..ac9ed1fa 100644 --- a/src/bounding_volume/bounding_sphere.rs +++ b/src/bounding_volume/bounding_sphere.rs @@ -1,12 +1,7 @@ //! Bounding sphere. use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Point, Real, Vector}; -use na; -use num::Zero; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; +use crate::math::{Pose, Real, Vector}; /// A Bounding Sphere. /// @@ -61,18 +56,18 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create a bounding sphere with center at origin and radius 2.0 -/// let sphere = BoundingSphere::new(Point3::origin(), 2.0); +/// let sphere = BoundingSphere::new(Vector::ZERO, 2.0); /// /// // Check basic properties -/// assert_eq!(*sphere.center(), Point3::origin()); +/// assert_eq!(sphere.center(), Vector::ZERO); /// assert_eq!(sphere.radius(), 2.0); /// /// // Test if a point is within the sphere -/// let point = Point3::new(1.0, 1.0, 0.0); -/// let distance = (point - sphere.center()).norm(); +/// let point = Vector::new(1.0, 1.0, 0.0); +/// let distance = (point - sphere.center()).length(); /// assert!(distance <= sphere.radius()); /// # } /// ``` @@ -80,14 +75,14 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; -/// use nalgebra::{Point3, Vector3, Translation3}; +/// use parry3d::math::Vector; /// /// // Create a sphere and translate it -/// let sphere = BoundingSphere::new(Point3::new(1.0, 2.0, 3.0), 1.5); -/// let translation = Vector3::new(5.0, 0.0, 0.0); -/// let moved = sphere.translated(&translation); +/// let sphere = BoundingSphere::new(Vector::new(1.0, 2.0, 3.0), 1.5); +/// let translation = Vector::new(5.0, 0.0, 0.0); +/// let moved = sphere.translated(translation); /// -/// assert_eq!(*moved.center(), Point3::new(6.0, 2.0, 3.0)); +/// assert_eq!(moved.center(), Vector::new(6.0, 2.0, 3.0)); /// assert_eq!(moved.radius(), 1.5); // Radius unchanged by translation /// # } /// ``` @@ -95,11 +90,11 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Merge two bounding spheres -/// let sphere1 = BoundingSphere::new(Point3::origin(), 1.0); -/// let sphere2 = BoundingSphere::new(Point3::new(4.0, 0.0, 0.0), 1.0); +/// let sphere1 = BoundingSphere::new(Vector::ZERO, 1.0); +/// let sphere2 = BoundingSphere::new(Vector::new(4.0, 0.0, 0.0), 1.0); /// /// let merged = sphere1.merged(&sphere2); /// // The merged sphere contains both original spheres @@ -111,14 +106,13 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Debug, PartialEq, Copy, Clone)] #[repr(C)] pub struct BoundingSphere { /// The center point of the bounding sphere. - pub center: Point, + pub center: Vector, /// The radius of the bounding sphere. /// @@ -140,19 +134,19 @@ impl BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a sphere centered at (1, 2, 3) with radius 5.0 /// let sphere = BoundingSphere::new( - /// Point3::new(1.0, 2.0, 3.0), + /// Vector::new(1.0, 2.0, 3.0), /// 5.0 /// ); /// - /// assert_eq!(*sphere.center(), Point3::new(1.0, 2.0, 3.0)); + /// assert_eq!(sphere.center(), Vector::new(1.0, 2.0, 3.0)); /// assert_eq!(sphere.radius(), 5.0); /// # } /// ``` - pub fn new(center: Point, radius: Real) -> BoundingSphere { + pub fn new(center: Vector, radius: Real) -> BoundingSphere { BoundingSphere { center, radius } } @@ -163,17 +157,17 @@ impl BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere = BoundingSphere::new(Point3::new(1.0, 2.0, 3.0), 5.0); + /// let sphere = BoundingSphere::new(Vector::new(1.0, 2.0, 3.0), 5.0); /// let center = sphere.center(); /// - /// assert_eq!(*center, Point3::new(1.0, 2.0, 3.0)); + /// assert_eq!(center, Vector::new(1.0, 2.0, 3.0)); /// # } /// ``` #[inline] - pub fn center(&self) -> &Point { - &self.center + pub fn center(&self) -> Vector { + self.center } /// Returns the radius of this bounding sphere. @@ -185,9 +179,9 @@ impl BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere = BoundingSphere::new(Point3::origin(), 10.0); + /// let sphere = BoundingSphere::new(Vector::ZERO, 10.0); /// /// assert_eq!(sphere.radius(), 10.0); /// # } @@ -212,25 +206,25 @@ impl BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; - /// use nalgebra::{Point3, Vector3, Isometry3, UnitQuaternion}; + /// use parry3d::math::{Vector, Pose, Rotation}; /// - /// let sphere = BoundingSphere::new(Point3::new(1.0, 0.0, 0.0), 2.0); + /// let sphere = BoundingSphere::new(Vector::new(1.0, 0.0, 0.0), 2.0); /// /// // Create a transformation: translate by (5, 0, 0) and rotate 90 degrees around Z - /// let translation = Vector3::new(5.0, 0.0, 0.0); - /// let rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, std::f32::consts::FRAC_PI_2); - /// let transform = Isometry3::from_parts(translation.into(), rotation); + /// let translation = Vector::new(5.0, 0.0, 0.0); + /// let rotation = Rotation::from_rotation_z(std::f32::consts::FRAC_PI_2); + /// let transform = Pose::from_parts(translation, rotation); /// /// let transformed = sphere.transform_by(&transform); /// /// // The center is transformed - /// assert!((*transformed.center() - Point3::new(5.0, 1.0, 0.0)).norm() < 1e-5); + /// assert!((transformed.center() - Vector::new(5.0, 1.0, 0.0)).length() < 1e-5); /// // The radius is unchanged /// assert_eq!(transformed.radius(), 2.0); /// # } /// ``` #[inline] - pub fn transform_by(&self, m: &Isometry) -> BoundingSphere { + pub fn transform_by(&self, m: &Pose) -> BoundingSphere { BoundingSphere::new(m * self.center, self.radius) } @@ -248,19 +242,19 @@ impl BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::BoundingSphere; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// - /// let sphere = BoundingSphere::new(Point3::origin(), 1.0); - /// let translation = Vector3::new(10.0, 5.0, -3.0); + /// let sphere = BoundingSphere::new(Vector::ZERO, 1.0); + /// let translation = Vector::new(10.0, 5.0, -3.0); /// - /// let moved = sphere.translated(&translation); + /// let moved = sphere.translated(translation); /// - /// assert_eq!(*moved.center(), Point3::new(10.0, 5.0, -3.0)); + /// assert_eq!(moved.center(), Vector::new(10.0, 5.0, -3.0)); /// assert_eq!(moved.radius(), 1.0); // Radius unchanged /// # } /// ``` #[inline] - pub fn translated(&self, translation: &Vector) -> BoundingSphere { + pub fn translated(&self, translation: Vector) -> BoundingSphere { BoundingSphere::new(self.center + translation, self.radius) } } @@ -273,17 +267,17 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere = BoundingSphere::new(Point3::new(1.0, 2.0, 3.0), 5.0); + /// let sphere = BoundingSphere::new(Vector::new(1.0, 2.0, 3.0), 5.0); /// - /// // BoundingVolume::center() returns a Point by value - /// assert_eq!(BoundingVolume::center(&sphere), Point3::new(1.0, 2.0, 3.0)); + /// // BoundingVolume::center() returns a Vector by value + /// assert_eq!(BoundingVolume::center(&sphere), Vector::new(1.0, 2.0, 3.0)); /// # } /// ``` #[inline] - fn center(&self) -> Point { - *self.center() + fn center(&self) -> Vector { + self.center() } /// Tests if this bounding sphere intersects another bounding sphere. @@ -300,11 +294,11 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere1 = BoundingSphere::new(Point3::origin(), 2.0); - /// let sphere2 = BoundingSphere::new(Point3::new(3.0, 0.0, 0.0), 2.0); - /// let sphere3 = BoundingSphere::new(Point3::new(10.0, 0.0, 0.0), 1.0); + /// let sphere1 = BoundingSphere::new(Vector::ZERO, 2.0); + /// let sphere2 = BoundingSphere::new(Vector::new(3.0, 0.0, 0.0), 2.0); + /// let sphere3 = BoundingSphere::new(Vector::new(10.0, 0.0, 0.0), 1.0); /// /// assert!(sphere1.intersects(&sphere2)); // Distance 3.0 <= sum of radii 4.0 /// assert!(!sphere1.intersects(&sphere3)); // Distance 10.0 > sum of radii 3.0 @@ -314,7 +308,7 @@ impl BoundingVolume for BoundingSphere { fn intersects(&self, other: &BoundingSphere) -> bool { // TODO: refactor that with the code from narrow_phase::ball_ball::collide(...) ? let delta_pos = other.center - self.center; - let distance_squared = delta_pos.norm_squared(); + let distance_squared = delta_pos.length_squared(); let sum_radius = self.radius + other.radius; distance_squared <= sum_radius * sum_radius @@ -334,11 +328,11 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let large = BoundingSphere::new(Point3::origin(), 10.0); - /// let small = BoundingSphere::new(Point3::new(2.0, 0.0, 0.0), 1.0); - /// let outside = BoundingSphere::new(Point3::new(15.0, 0.0, 0.0), 2.0); + /// let large = BoundingSphere::new(Vector::ZERO, 10.0); + /// let small = BoundingSphere::new(Vector::new(2.0, 0.0, 0.0), 1.0); + /// let outside = BoundingSphere::new(Vector::new(15.0, 0.0, 0.0), 2.0); /// /// assert!(large.contains(&small)); // Small sphere is inside large sphere /// assert!(!large.contains(&outside)); // Outside sphere extends beyond large sphere @@ -348,7 +342,7 @@ impl BoundingVolume for BoundingSphere { #[inline] fn contains(&self, other: &BoundingSphere) -> bool { let delta_pos = other.center - self.center; - let distance = delta_pos.norm(); + let distance = delta_pos.length(); distance + other.radius <= self.radius } @@ -367,30 +361,30 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let mut sphere1 = BoundingSphere::new(Point3::origin(), 1.0); - /// let sphere2 = BoundingSphere::new(Point3::new(4.0, 0.0, 0.0), 1.0); + /// let mut sphere1 = BoundingSphere::new(Vector::ZERO, 1.0); + /// let sphere2 = BoundingSphere::new(Vector::new(4.0, 0.0, 0.0), 1.0); /// /// sphere1.merge(&sphere2); /// /// // The merged sphere now contains both original spheres - /// assert!(sphere1.contains(&BoundingSphere::new(Point3::origin(), 1.0))); + /// assert!(sphere1.contains(&BoundingSphere::new(Vector::ZERO, 1.0))); /// assert!(sphere1.contains(&sphere2)); /// # } /// ``` #[inline] fn merge(&mut self, other: &BoundingSphere) { - let mut dir = *other.center() - *self.center(); - let norm = dir.normalize_mut(); + let dir = other.center() - self.center(); + let (dir, length) = dir.normalize_and_length(); - if norm.is_zero() { + if length == 0.0 { if other.radius > self.radius { self.radius = other.radius } } else { - let s_center_dir = self.center.coords.dot(&dir); - let o_center_dir = other.center.coords.dot(&dir); + let s_center_dir = self.center.dot(dir); + let o_center_dir = other.center.dot(dir); let right = if s_center_dir + self.radius > o_center_dir + other.radius { self.center + dir * self.radius @@ -404,8 +398,8 @@ impl BoundingVolume for BoundingSphere { other.center - dir * other.radius }; - self.center = na::center(&left, &right); - self.radius = na::distance(&right, &self.center); + self.center = left.midpoint(right); + self.radius = right.distance(self.center); } } @@ -423,10 +417,10 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere1 = BoundingSphere::new(Point3::origin(), 1.0); - /// let sphere2 = BoundingSphere::new(Point3::new(4.0, 0.0, 0.0), 1.0); + /// let sphere1 = BoundingSphere::new(Vector::ZERO, 1.0); + /// let sphere2 = BoundingSphere::new(Vector::new(4.0, 0.0, 0.0), 1.0); /// /// let merged = sphere1.merged(&sphere2); /// @@ -464,13 +458,13 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let mut sphere = BoundingSphere::new(Point3::origin(), 5.0); + /// let mut sphere = BoundingSphere::new(Vector::ZERO, 5.0); /// sphere.loosen(2.0); /// /// assert_eq!(sphere.radius(), 7.0); - /// assert_eq!(*sphere.center(), Point3::origin()); // Center unchanged + /// assert_eq!(sphere.center(), Vector::ZERO); // Center unchanged /// # } /// ``` #[inline] @@ -497,14 +491,14 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere = BoundingSphere::new(Point3::origin(), 5.0); + /// let sphere = BoundingSphere::new(Vector::ZERO, 5.0); /// let larger = sphere.loosened(3.0); /// /// assert_eq!(sphere.radius(), 5.0); // Original unchanged /// assert_eq!(larger.radius(), 8.0); - /// assert_eq!(*larger.center(), Point3::origin()); + /// assert_eq!(larger.center(), Vector::ZERO); /// # } /// ``` #[inline] @@ -531,13 +525,13 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let mut sphere = BoundingSphere::new(Point3::origin(), 10.0); + /// let mut sphere = BoundingSphere::new(Vector::ZERO, 10.0); /// sphere.tighten(3.0); /// /// assert_eq!(sphere.radius(), 7.0); - /// assert_eq!(*sphere.center(), Point3::origin()); // Center unchanged + /// assert_eq!(sphere.center(), Vector::ZERO); // Center unchanged /// # } /// ``` #[inline] @@ -565,14 +559,14 @@ impl BoundingVolume for BoundingSphere { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::{BoundingSphere, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let sphere = BoundingSphere::new(Point3::origin(), 10.0); + /// let sphere = BoundingSphere::new(Vector::ZERO, 10.0); /// let smaller = sphere.tightened(4.0); /// /// assert_eq!(sphere.radius(), 10.0); // Original unchanged /// assert_eq!(smaller.radius(), 6.0); - /// assert_eq!(*smaller.center(), Point3::origin()); + /// assert_eq!(smaller.center(), Vector::ZERO); /// # } /// ``` #[inline] diff --git a/src/bounding_volume/bounding_sphere_ball.rs b/src/bounding_volume/bounding_sphere_ball.rs index dde07cf2..dd6263bb 100644 --- a/src/bounding_volume/bounding_sphere_ball.rs +++ b/src/bounding_volume/bounding_sphere_ball.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Vector}; use crate::shape::Ball; impl Ball { /// Computes the world-space bounding sphere of this ball, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } @@ -13,6 +13,6 @@ impl Ball { /// Computes the local-space Aabb of this ball. #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { - BoundingSphere::new(Point::origin(), self.radius) + BoundingSphere::new(Vector::ZERO, self.radius) } } diff --git a/src/bounding_volume/bounding_sphere_capsule.rs b/src/bounding_volume/bounding_sphere_capsule.rs index 6b274859..ec2d42c8 100644 --- a/src/bounding_volume/bounding_sphere_capsule.rs +++ b/src/bounding_volume/bounding_sphere_capsule.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Capsule; impl Capsule { /// Computes the world-space bounding sphere of this capsule, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { self.local_bounding_sphere().transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_cone.rs b/src/bounding_volume/bounding_sphere_cone.rs index ae95802b..e83dda98 100644 --- a/src/bounding_volume/bounding_sphere_cone.rs +++ b/src/bounding_volume/bounding_sphere_cone.rs @@ -1,12 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Point, Real}; +use crate::math::{ComplexField, Pose, Real, Vector}; use crate::shape::Cone; -use na::ComplexField; impl Cone { /// Computes the world-space bounding sphere of this cone, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } @@ -14,9 +13,10 @@ impl Cone { /// Computes the local-space bounding sphere of this cone. #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { - let radius = - ComplexField::sqrt(self.radius * self.radius + self.half_height * self.half_height); + let radius = ::sqrt( + self.radius * self.radius + self.half_height * self.half_height, + ); - BoundingSphere::new(Point::origin(), radius) + BoundingSphere::new(Vector::ZERO, radius) } } diff --git a/src/bounding_volume/bounding_sphere_convex.rs b/src/bounding_volume/bounding_sphere_convex.rs index 793bd3ec..a02ce556 100644 --- a/src/bounding_volume/bounding_sphere_convex.rs +++ b/src/bounding_volume/bounding_sphere_convex.rs @@ -1,12 +1,12 @@ use crate::bounding_volume; use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::ConvexPolyhedron; impl ConvexPolyhedron { /// Computes the world-space bounding sphere of this convex polyhedron, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_convex_polygon.rs b/src/bounding_volume/bounding_sphere_convex_polygon.rs index 1f1479a8..0fbedd75 100644 --- a/src/bounding_volume/bounding_sphere_convex_polygon.rs +++ b/src/bounding_volume/bounding_sphere_convex_polygon.rs @@ -1,12 +1,12 @@ use crate::bounding_volume; use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::ConvexPolygon; impl ConvexPolygon { /// Computes the world-space bounding sphere of this convex polygon, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_cuboid.rs b/src/bounding_volume/bounding_sphere_cuboid.rs index 00cf78ba..02f9db93 100644 --- a/src/bounding_volume/bounding_sphere_cuboid.rs +++ b/src/bounding_volume/bounding_sphere_cuboid.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Vector}; use crate::shape::Cuboid; impl Cuboid { /// Computes the world-space bounding sphere of this cuboid, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } @@ -13,7 +13,7 @@ impl Cuboid { /// Computes the local-space bounding sphere of this cuboid. #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { - let radius = self.half_extents.norm(); - BoundingSphere::new(Point::origin(), radius) + let radius = self.half_extents.length(); + BoundingSphere::new(Vector::ZERO, radius) } } diff --git a/src/bounding_volume/bounding_sphere_cylinder.rs b/src/bounding_volume/bounding_sphere_cylinder.rs index a2edafb1..9fd1d057 100644 --- a/src/bounding_volume/bounding_sphere_cylinder.rs +++ b/src/bounding_volume/bounding_sphere_cylinder.rs @@ -1,12 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Point, Real}; +use crate::math::{ComplexField, Pose, Real, Vector}; use crate::shape::Cylinder; -use na::ComplexField; impl Cylinder { /// Computes the world-space bounding sphere of this cylinder, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } @@ -14,9 +13,10 @@ impl Cylinder { /// Computes the local-space bounding sphere of this cylinder. #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { - let radius = - ComplexField::sqrt(self.radius * self.radius + self.half_height * self.half_height); + let radius = ::sqrt( + self.radius * self.radius + self.half_height * self.half_height, + ); - BoundingSphere::new(Point::origin(), radius) + BoundingSphere::new(Vector::ZERO, radius) } } diff --git a/src/bounding_volume/bounding_sphere_halfspace.rs b/src/bounding_volume/bounding_sphere_halfspace.rs index f383a1ae..626772ee 100644 --- a/src/bounding_volume/bounding_sphere_halfspace.rs +++ b/src/bounding_volume/bounding_sphere_halfspace.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Real, Vector}; use crate::shape::HalfSpace; use num::Bounded; @@ -7,7 +7,7 @@ use num::Bounded; impl HalfSpace { /// Computes the world-space bounding sphere of this half-space, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } @@ -17,6 +17,6 @@ impl HalfSpace { pub fn local_bounding_sphere(&self) -> BoundingSphere { let radius = Real::max_value(); - BoundingSphere::new(Point::origin(), radius) + BoundingSphere::new(Vector::ZERO, radius) } } diff --git a/src/bounding_volume/bounding_sphere_heightfield.rs b/src/bounding_volume/bounding_sphere_heightfield.rs index 714e369f..9dd32946 100644 --- a/src/bounding_volume/bounding_sphere_heightfield.rs +++ b/src/bounding_volume/bounding_sphere_heightfield.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::HeightField; impl HeightField { /// Computes the world-space bounding sphere of this height-field, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { self.local_aabb().bounding_sphere().transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_polyline.rs b/src/bounding_volume/bounding_sphere_polyline.rs index a9a62faf..1c24672b 100644 --- a/src/bounding_volume/bounding_sphere_polyline.rs +++ b/src/bounding_volume/bounding_sphere_polyline.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Polyline; impl Polyline { /// Computes the world-space bounding sphere of this polyline, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { self.local_aabb().bounding_sphere().transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_segment.rs b/src/bounding_volume/bounding_sphere_segment.rs index 4069280a..d9de1f7a 100644 --- a/src/bounding_volume/bounding_sphere_segment.rs +++ b/src/bounding_volume/bounding_sphere_segment.rs @@ -1,12 +1,12 @@ use crate::bounding_volume; use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Segment; impl Segment { /// Computes the world-space bounding sphere of this segment, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_triangle.rs b/src/bounding_volume/bounding_sphere_triangle.rs index 936a3435..2cbc1dc7 100644 --- a/src/bounding_volume/bounding_sphere_triangle.rs +++ b/src/bounding_volume/bounding_sphere_triangle.rs @@ -1,12 +1,12 @@ use crate::bounding_volume; use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Triangle; impl Triangle { /// Computes the world-space bounding sphere of this triangle, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { let bv: BoundingSphere = self.local_bounding_sphere(); bv.transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_trimesh.rs b/src/bounding_volume/bounding_sphere_trimesh.rs index 6b1a4a7a..211e5548 100644 --- a/src/bounding_volume/bounding_sphere_trimesh.rs +++ b/src/bounding_volume/bounding_sphere_trimesh.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::TriMesh; impl TriMesh { /// Computes the world-space bounding sphere of this triangle mesh, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { self.local_aabb().bounding_sphere().transform_by(pos) } diff --git a/src/bounding_volume/bounding_sphere_utils.rs b/src/bounding_volume/bounding_sphere_utils.rs index 0a257bea..bd1b0498 100644 --- a/src/bounding_volume/bounding_sphere_utils.rs +++ b/src/bounding_volume/bounding_sphere_utils.rs @@ -1,29 +1,25 @@ -use crate::math::{Point, Real}; +use crate::math::{ComplexField, Real, Vector}; use crate::utils; -use na::{self, ComplexField}; use super::BoundingSphere; /// Computes the bounding sphere of a set of point, given its center. #[inline] -pub fn point_cloud_bounding_sphere_with_center( - pts: &[Point], - center: Point, -) -> BoundingSphere { +pub fn point_cloud_bounding_sphere_with_center(pts: &[Vector], center: Vector) -> BoundingSphere { let mut sqradius = 0.0; for pt in pts.iter() { - let distance_squared = na::distance_squared(pt, ¢er); + let dist_sq = pt.distance_squared(center); - if distance_squared > sqradius { - sqradius = distance_squared + if dist_sq > sqradius { + sqradius = dist_sq } } - BoundingSphere::new(center, ComplexField::sqrt(sqradius)) + BoundingSphere::new(center, ::sqrt(sqradius)) } /// Computes a bounding sphere of the specified set of point. #[inline] -pub fn point_cloud_bounding_sphere(pts: &[Point]) -> BoundingSphere { +pub fn point_cloud_bounding_sphere(pts: &[Vector]) -> BoundingSphere { point_cloud_bounding_sphere_with_center(pts, utils::center(pts)) } diff --git a/src/bounding_volume/bounding_sphere_voxels.rs b/src/bounding_volume/bounding_sphere_voxels.rs index 54e54b9a..bef3c8bb 100644 --- a/src/bounding_volume/bounding_sphere_voxels.rs +++ b/src/bounding_volume/bounding_sphere_voxels.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::Voxels; impl Voxels { /// Computes the world-space bounding sphere of this set of voxels, transformed by `pos`. #[inline] - pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { + pub fn bounding_sphere(&self, pos: &Pose) -> BoundingSphere { self.local_aabb().bounding_sphere().transform_by(pos) } diff --git a/src/bounding_volume/bounding_volume.rs b/src/bounding_volume/bounding_volume.rs index a9cd8b4b..69e36268 100644 --- a/src/bounding_volume/bounding_volume.rs +++ b/src/bounding_volume/bounding_volume.rs @@ -1,4 +1,4 @@ -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; /// Trait of bounding volumes. /// @@ -9,7 +9,7 @@ pub trait BoundingVolume { // TODO: keep that ? What about non-spacial bounding volumes (e.g. bounding cones, curvature // bounds, etc.) ? /// Returns a point inside of this bounding volume. This is ideally its center. - fn center(&self) -> Point; + fn center(&self) -> Vector; /// Checks if this bounding volume intersect with another one. fn intersects(&self, _: &Self) -> bool; diff --git a/src/bounding_volume/mod.rs b/src/bounding_volume/mod.rs index 0362ea5f..142ebcc4 100644 --- a/src/bounding_volume/mod.rs +++ b/src/bounding_volume/mod.rs @@ -3,8 +3,8 @@ #[doc(inline)] pub use crate::bounding_volume::aabb::Aabb; -#[cfg(feature = "simd-is-enabled")] -pub use crate::bounding_volume::simd_aabb::SimdAabb; +// #[cfg(feature = "simd-is-enabled")] +// pub use crate::bounding_volume::simd_aabb::SimdAabb; #[doc(inline)] pub use crate::bounding_volume::bounding_sphere::BoundingSphere; @@ -62,8 +62,8 @@ mod bounding_sphere_utils; #[cfg(feature = "alloc")] mod bounding_sphere_voxels; -#[cfg(feature = "simd-is-enabled")] -mod simd_aabb; +// #[cfg(feature = "simd-is-enabled")] +// mod simd_aabb; /// Free functions for some special cases of bounding-volume computation. pub mod details { diff --git a/src/bounding_volume/simd_aabb.rs b/src/bounding_volume/simd_aabb.rs index 640bb4ce..62179570 100644 --- a/src/bounding_volume/simd_aabb.rs +++ b/src/bounding_volume/simd_aabb.rs @@ -1,7 +1,7 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, SimdBool, SimdReal, Vector, DIM, SIMD_WIDTH}; +use crate::math::{Pose, Real, SimdBool, SimdReal, Vector, Vector, DIM, SIMD_WIDTH}; use crate::query::SimdRay; -use crate::utils::{self, IsometryOps}; +use crate::utils::{self, PoseOps}; use num::{One, Zero}; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -10,13 +10,12 @@ use simba::simd::{SimdPartialOrd, SimdValue}; #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) )] pub struct SimdAabb { /// The min coordinates of the Aabbs. - pub mins: Point, + pub mins: Vector, /// The max coordinates the Aabbs. - pub maxs: Point, + pub maxs: Vector, } #[cfg(feature = "serde-serialize")] @@ -27,16 +26,10 @@ impl serde::Serialize for SimdAabb { { use serde::ser::SerializeStruct; - let mins: Point<[Real; SIMD_WIDTH]> = Point::from( - self.mins - .coords - .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), - ); - let maxs: Point<[Real; SIMD_WIDTH]> = Point::from( - self.maxs - .coords - .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), - ); + let mins: Vector<[Real; SIMD_WIDTH]> = + Vector::from(self.mins.map(|e| array![|ii| e.extract(ii); SIMD_WIDTH])); + let maxs: Vector<[Real; SIMD_WIDTH]> = + Vector::from(self.maxs.map(|e| array![|ii| e.extract(ii); SIMD_WIDTH])); let mut simd_aabb = serializer.serialize_struct("SimdAabb", 2)?; simd_aabb.serialize_field("mins", &mins)?; @@ -74,8 +67,8 @@ impl<'de> serde::Deserialize<'de> for SimdAabb { where A: serde::de::MapAccess<'de>, { - let mut mins: Option> = None; - let mut maxs: Option> = None; + let mut mins: Option> = None; + let mut maxs: Option> = None; while let Some(key) = map.next_key()? { match key { @@ -105,10 +98,10 @@ impl<'de> serde::Deserialize<'de> for SimdAabb { where A: serde::de::SeqAccess<'de>, { - let mins: Point<[Real; SIMD_WIDTH]> = seq + let mins: Vector<[Real; SIMD_WIDTH]> = seq .next_element()? .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - let maxs: Point<[Real; SIMD_WIDTH]> = seq + let maxs: Vector<[Real; SIMD_WIDTH]> = seq .next_element()? .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; let mins = mins.map(SimdReal::from); @@ -130,14 +123,14 @@ impl SimdAabb { /// Builds an SIMD aabb composed of four identical aabbs. pub fn splat(aabb: Aabb) -> Self { Self { - mins: Point::splat(aabb.mins), - maxs: Point::splat(aabb.maxs), + mins: Vector::splat(aabb.mins), + maxs: Vector::splat(aabb.maxs), } } /// The center of all the Aabbs represented by `self`. - pub fn center(&self) -> Point { - na::center(&self.mins, &self.maxs) + pub fn center(&self) -> Vector { + Vector::from((self.mins + self.maxs) * SimdReal::splat(0.5)) } /// The half-extents of all the Aabbs represented by `self`. @@ -147,11 +140,11 @@ impl SimdAabb { /// The radius of all the Aabbs represented by `self`. pub fn radius(&self) -> SimdReal { - (self.maxs - self.mins).norm() + (self.maxs - self.mins).length() } /// Return the Aabb of the `self` transformed by the given isometry. - pub fn transform_by(&self, transform: &Isometry) -> Self { + pub fn transform_by(&self, transform: &Pose) -> Self { let ls_center = self.center(); let center = transform * ls_center; let ws_half_extents = transform.absolute_transform_vector(&self.half_extents()); @@ -163,19 +156,19 @@ impl SimdAabb { /// Returns a scaled version of this Aabb. #[inline] - pub fn scaled(self, scale: &Vector) -> Self { - let a = self.mins.coords.component_mul(scale); - let b = self.maxs.coords.component_mul(scale); + pub fn scaled(self, scale: Vector) -> Self { + let a = self.mins * (scale); + let b = self.maxs * (scale); Self { - mins: a.inf(&b).into(), - maxs: a.sup(&b).into(), + mins: a.min(b).into(), + maxs: a.max(b).into(), } } /// Enlarges this bounding volume by the given margin. pub fn loosen(&mut self, margin: SimdReal) { - self.mins -= Vector::repeat(margin); - self.maxs += Vector::repeat(margin); + self.mins -= Vector::splat(margin); + self.maxs += Vector::splat(margin); } /// Dilate all the Aabbs represented by `self` by their extents multiplied @@ -243,24 +236,20 @@ impl SimdAabb { } /// Computes the distances between a point and all the Aabbs represented by `self`. - pub fn distance_to_local_point(&self, point: &Point) -> SimdReal { + pub fn distance_to_local_point(&self, point: Vector) -> SimdReal { let mins_point = self.mins - point; let point_maxs = point - self.maxs; - let shift = mins_point.sup(&point_maxs).sup(&na::zero()); - shift.norm() + let shift = mins_point.max(point_maxs).max(Vector::ZERO); + shift.length() } /// Computes the distances between the origin and all the Aabbs represented by `self`. pub fn distance_to_origin(&self) -> SimdReal { - self.mins - .coords - .sup(&-self.maxs.coords) - .sup(&Vector::zeros()) - .norm() + self.mins.max(-self.maxs).max(Vector::ZERO).length() } /// Check which Aabb represented by `self` contains the given `point`. - pub fn contains_local_point(&self, point: &Point) -> SimdBool { + pub fn contains_local_point(&self, point: Vector) -> SimdBool { #[cfg(feature = "dim2")] return self.mins.x.simd_le(point.x) & self.mins.y.simd_le(point.y) @@ -338,8 +327,8 @@ impl SimdAabb { /// Merge all the Aabb represented by `self` into a single one. pub fn to_merged_aabb(&self) -> Aabb { Aabb::new( - self.mins.coords.map(|e| e.simd_horizontal_min()).into(), - self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), + self.mins.map(|e| e.simd_horizontal_min()).into(), + self.maxs.map(|e| e.simd_horizontal_max()).into(), ) } @@ -355,8 +344,8 @@ impl From<[Aabb; SIMD_WIDTH]> for SimdAabb { let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; SimdAabb { - mins: Point::from(mins), - maxs: Point::from(maxs), + mins: Vector::from(mins), + maxs: Vector::from(maxs), } } } diff --git a/src/lib.rs b/src/lib.rs index 6a4755af..3d11e132 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -19,7 +19,8 @@ the rust programming language. #![allow(clippy::module_inception)] #![allow(clippy::manual_range_contains)] // This usually makes it way more verbose that it could be. #![allow(clippy::type_complexity)] // Complains about closures that are fairly simple. -#![doc(html_root_url = "http://docs.rs/parry/0.1.1")] +#![cfg_attr(feature = "dim2", doc(html_root_url = "https://docs.rs/parry2d"))] +#![cfg_attr(feature = "dim3", doc(html_root_url = "https://docs.rs/parry3d"))] #![no_std] #[cfg(all( @@ -34,6 +35,7 @@ std::compile_error!( ); #[cfg(feature = "simd-is-enabled")] +#[allow(unused_macros)] macro_rules! array( ($callback: expr; SIMD_WIDTH) => { { @@ -66,11 +68,12 @@ extern crate approx; extern crate num_traits as num; pub extern crate either; -pub extern crate nalgebra as na; +pub extern crate glamx; pub extern crate simba; pub mod bounding_volume; pub mod mass_properties; +pub mod math; pub mod partitioning; pub mod query; pub mod shape; @@ -78,158 +81,6 @@ pub mod shape; pub mod transformation; pub mod utils; -mod real { - /// The scalar type used throughout this crate. - #[cfg(feature = "f64")] - pub use f64 as Real; - - /// The scalar type used throughout this crate. - #[cfg(feature = "f32")] - pub use f32 as Real; -} - -/// Compilation flags dependent aliases for mathematical types. -#[cfg(feature = "dim3")] -pub mod math { - pub use super::real::*; - pub use super::simd::*; - use na::{ - Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, UnitVector3, Vector3, Vector6, - U3, U6, - }; - - /// The default tolerance used for geometric operations. - pub const DEFAULT_EPSILON: Real = Real::EPSILON; - - /// The dimension of the space. - pub const DIM: usize = 3; - - /// The dimension of the space multiplied by two. - pub const TWO_DIM: usize = DIM * 2; - - /// The dimension of the ambient space. - pub type Dim = U3; - - /// The dimension of a spatial vector. - pub type SpatialDim = U6; - - /// The dimension of the rotations. - pub type AngDim = U3; - - /// The point type. - pub type Point = Point3; - - /// The angular vector type. - pub type AngVector = Vector3; - - /// The vector type. - pub type Vector = Vector3; - - /// The unit vector type. - pub type UnitVector = UnitVector3; - - /// The matrix type. - pub type Matrix = Matrix3; - - /// The vector type with dimension `SpatialDim × 1`. - pub type SpatialVector = Vector6; - - /// The orientation type. - pub type Orientation = Vector3; - - /// The transformation matrix type. - pub type Isometry = Isometry3; - - /// The rotation matrix type. - pub type Rotation = UnitQuaternion; - - /// The translation type. - pub type Translation = Translation3; - - /// The angular inertia of a rigid body. - pub type AngularInertia = crate::utils::SdpMatrix3; - - /// The principal angular inertia of a rigid body. - pub type PrincipalAngularInertia = Vector3; - - /// A matrix that represent the cross product with a given vector. - pub type CrossMatrix = Matrix3; - - /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. - pub type SpacialVector = Vector6; - - /// A 3D symmetric-definite-positive matrix. - pub type SdpMatrix = crate::utils::SdpMatrix3; -} - -/// Compilation flags dependent aliases for mathematical types. -#[cfg(feature = "dim2")] -pub mod math { - pub use super::real::*; - pub use super::simd::*; - use na::{ - Isometry2, Matrix2, Point2, Translation2, UnitComplex, UnitVector2, Vector1, Vector2, - Vector3, U1, U2, - }; - - /// The default tolerance used for geometric operations. - pub const DEFAULT_EPSILON: Real = Real::EPSILON; - - /// The dimension of the space. - pub const DIM: usize = 2; - - /// The dimension of the space multiplied by two. - pub const TWO_DIM: usize = DIM * 2; - - /// The dimension of the ambient space. - pub type Dim = U2; - - /// The dimension of the rotations. - pub type AngDim = U1; - - /// The point type. - pub type Point = Point2; - - /// The angular vector type. - pub type AngVector = N; - - /// The vector type. - pub type Vector = Vector2; - - /// The unit vector type. - pub type UnitVector = UnitVector2; - - /// The matrix type. - pub type Matrix = Matrix2; - - /// The orientation type. - pub type Orientation = Vector1; - - /// The transformation matrix type. - pub type Isometry = Isometry2; - - /// The rotation matrix type. - pub type Rotation = UnitComplex; - - /// The translation type. - pub type Translation = Translation2; - - /// The angular inertia of a rigid body. - pub type AngularInertia = N; - - /// The principal angular inertia of a rigid body. - pub type PrincipalAngularInertia = N; - - /// A matrix that represent the cross product with a given vector. - pub type CrossMatrix = Vector2; - - /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. - pub type SpacialVector = Vector3; - - /// A 2D symmetric-definite-positive matrix. - pub type SdpMatrix = crate::utils::SdpMatrix2; -} - #[cfg(not(feature = "simd-is-enabled"))] mod simd { /// The number of lanes of a SIMD number. diff --git a/src/mass_properties/mass_properties.rs b/src/mass_properties/mass_properties.rs index b37a0306..8c08146f 100644 --- a/src/mass_properties/mass_properties.rs +++ b/src/mass_properties/mass_properties.rs @@ -1,12 +1,9 @@ -use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Rotation, Vector}; +use crate::math::{AngVector, AngularInertia, Pose, Real, Rotation, Vector}; +#[cfg(feature = "dim3")] +use crate::math::{MatExt, Matrix, VectorExt}; use crate::utils; use core::ops::{Add, AddAssign, Sub, SubAssign}; use num::Zero; -#[cfg(feature = "dim3")] -use {core::ops::MulAssign, na::Matrix3}; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "f32", expect(clippy::unnecessary_cast))] const EPSILON: Real = f32::EPSILON as Real; @@ -15,8 +12,7 @@ const EPSILON: Real = f32::EPSILON as Real; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] /// The mass properties of a rigid body. /// @@ -59,7 +55,7 @@ const EPSILON: Real = f32::EPSILON as Real; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::{Ball, Shape}; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Compute mass properties for a unit ball with density 1.0 /// let ball = Ball::new(1.0); @@ -70,7 +66,7 @@ const EPSILON: Real = f32::EPSILON as Real; /// println!("Mass: {}", mass); /// /// // Center of mass (at origin for a ball) -/// assert_eq!(props.local_com, Point3::origin()); +/// assert_eq!(props.local_com, Vector::ZERO); /// /// // For simulation, use inverse values /// if props.inv_mass > 0.0 { @@ -91,10 +87,10 @@ const EPSILON: Real = f32::EPSILON as Real; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::{Ball, Cuboid, Shape}; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// /// let ball = Ball::new(1.0); -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// let ball_props = ball.mass_properties(1.0); /// let cuboid_props = cuboid.mass_properties(1.0); @@ -112,7 +108,7 @@ pub struct MassProperties { /// /// This is the balance point of the object. For symmetric shapes, it's typically /// at the geometric center. All angular inertia calculations are relative to this point. - pub local_com: Point, + pub local_com: Vector, /// The inverse of the mass (1 / mass). /// @@ -130,14 +126,14 @@ pub struct MassProperties { /// /// Angular inertia relative to the center of mass (`local_com`). /// Zero components indicate infinite inertia (no rotation) along that axis. - pub inv_principal_inertia: AngVector, + pub inv_principal_inertia: AngVector, #[cfg(feature = "dim3")] /// The rotation from local coordinates to principal inertia axes (3D only). /// /// This rotation aligns the object's coordinate system with its principal /// axes of inertia, where the inertia tensor is diagonal. - pub principal_inertia_local_frame: Rotation, + pub principal_inertia_local_frame: Rotation, } impl MassProperties { @@ -154,11 +150,11 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create mass properties for a 10kg object /// let props = MassProperties::new( - /// Point2::origin(), // Centered at origin + /// Vector::ZERO, // Centered at origin /// 10.0, // 10kg mass /// 5.0 // Angular inertia /// ); @@ -168,7 +164,7 @@ impl MassProperties { /// # } /// ``` #[cfg(feature = "dim2")] - pub fn new(local_com: Point, mass: Real, principal_inertia: Real) -> Self { + pub fn new(local_com: Vector, mass: Real, principal_inertia: Real) -> Self { let inv_mass = utils::inv(mass); let inv_principal_inertia = utils::inv(principal_inertia); Self { @@ -184,8 +180,8 @@ impl MassProperties { /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space /// of the rigid-body. #[cfg(feature = "dim3")] - pub fn new(local_com: Point, mass: Real, principal_inertia: AngVector) -> Self { - Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) + pub fn new(local_com: Vector, mass: Real, principal_inertia: AngVector) -> Self { + Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::IDENTITY) } /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. @@ -195,10 +191,10 @@ impl MassProperties { /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body. #[cfg(feature = "dim3")] pub fn with_principal_inertia_frame( - local_com: Point, + local_com: Vector, mass: Real, - principal_inertia: AngVector, - principal_inertia_local_frame: Rotation, + principal_inertia: AngVector, + principal_inertia_local_frame: Rotation, ) -> Self { let inv_mass = utils::inv(mass); let inv_principal_inertia = principal_inertia.map(utils::inv); @@ -215,17 +211,14 @@ impl MassProperties { /// The angular inertia matrix will be diagonalized in order to extract the principal inertia /// values and principal inertia frame. #[cfg(feature = "dim3")] - pub fn with_inertia_matrix(local_com: Point, mass: Real, inertia: Matrix3) -> Self { + pub fn with_inertia_matrix(local_com: Vector, mass: Real, inertia: Matrix) -> Self { let mut eigen = inertia.symmetric_eigen(); - if eigen.eigenvectors.determinant() < 0.0 { - eigen.eigenvectors.swap_columns(1, 2); - eigen.eigenvalues.swap_rows(1, 2); + eigen.eigenvectors.swap_cols(1, 2); + eigen.eigenvalues.as_mut().swap(1, 2); } - let mut principal_inertia_local_frame = Rotation::from_rotation_matrix( - &na::Rotation3::from_matrix_unchecked(eigen.eigenvectors), - ); - let _ = principal_inertia_local_frame.renormalize(); + let eigenvectors = eigen.eigenvectors; + let principal_inertia_local_frame = Rotation::from_mat3(&eigenvectors).normalize(); // Drop negative eigenvalues. let principal_inertia = eigen.eigenvalues.map(|e| e.max(0.0)); @@ -244,7 +237,7 @@ impl MassProperties { } /// The angular inertia along the principal inertia axes and center of mass of the rigid-body. - pub fn principal_inertia(&self) -> AngVector { + pub fn principal_inertia(&self) -> AngVector { #[cfg(feature = "dim2")] return utils::inv(self.inv_principal_inertia); #[cfg(feature = "dim3")] @@ -252,27 +245,25 @@ impl MassProperties { } /// The world-space center of mass of the rigid-body. - pub fn world_com(&self, pos: &Isometry) -> Point { + pub fn world_com(&self, pos: &Pose) -> Vector { pos * self.local_com } #[cfg(feature = "dim2")] /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia(&self, _rot: &Rotation) -> AngularInertia { + pub fn world_inv_inertia(&self, _rot: &Rotation) -> AngularInertia { self.inv_principal_inertia } #[cfg(feature = "dim3")] /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia(&self, rot: &Rotation) -> AngularInertia { - if !self.inv_principal_inertia.is_zero() { - let mut lhs = (rot * self.principal_inertia_local_frame) - .to_rotation_matrix() - .into_inner(); + pub fn world_inv_inertia(&self, rot: &Rotation) -> AngularInertia { + if self.inv_principal_inertia != Vector::ZERO { + let mut lhs = Matrix::from_quat(rot * self.principal_inertia_local_frame); let rhs = lhs.transpose(); - lhs.column_mut(0).mul_assign(self.inv_principal_inertia.x); - lhs.column_mut(1).mul_assign(self.inv_principal_inertia.y); - lhs.column_mut(2).mul_assign(self.inv_principal_inertia.z); + lhs.x_axis *= self.inv_principal_inertia.x; + lhs.y_axis *= self.inv_principal_inertia.y; + lhs.z_axis *= self.inv_principal_inertia.z; let inertia = lhs * rhs; AngularInertia::from_sdp_matrix(inertia) } else { @@ -282,56 +273,51 @@ impl MassProperties { #[cfg(feature = "dim3")] /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { + pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix { let inv_principal_inertia = self.inv_principal_inertia; - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&inv_principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() + let rot_mat = Matrix::from_quat(self.principal_inertia_local_frame); + let inv_rot_mat = Matrix::from_quat(self.principal_inertia_local_frame.inverse()); + + rot_mat * Matrix::from_diagonal(inv_principal_inertia) * inv_rot_mat } #[cfg(feature = "dim3")] /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { + pub fn reconstruct_inertia_matrix(&self) -> Matrix { let principal_inertia = self.inv_principal_inertia.map(utils::inv); - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() + let rot_mat = Matrix::from_quat(self.principal_inertia_local_frame); + let inv_rot_mat = Matrix::from_quat(self.principal_inertia_local_frame.inverse()); + rot_mat * Matrix::from_diagonal(principal_inertia) * inv_rot_mat } #[cfg(feature = "dim2")] - pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Real { + pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Real { let i = utils::inv(self.inv_principal_inertia); if self.inv_mass != 0.0 { let mass = 1.0 / self.inv_mass; - i + shift.norm_squared() * mass + i + shift.length_squared() * mass } else { i } } #[cfg(feature = "dim3")] - pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix3 { + pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix { let matrix = self.reconstruct_inertia_matrix(); if self.inv_mass != 0.0 { let mass = 1.0 / self.inv_mass; - let diag = shift.norm_squared(); - let diagm = Matrix3::from_diagonal_element(diag); - matrix + (diagm - shift * shift.transpose()) * mass + let diag = shift.length_squared(); + let diagm = Matrix::from_diagonal(Vector::splat(diag)); + matrix + (diagm - shift.kronecker(shift)) * mass } else { matrix } } /// Transform each element of the mass properties. - pub fn transform_by(&self, m: &Isometry) -> Self { + pub fn transform_by(&self, m: &Pose) -> Self { // NOTE: we don't apply the parallel axis theorem here // because the center of mass is also transformed. Self { @@ -367,10 +353,13 @@ impl Zero for MassProperties { fn zero() -> Self { Self { inv_mass: 0.0, - inv_principal_inertia: na::zero(), + #[cfg(feature = "dim2")] + inv_principal_inertia: 0.0, + #[cfg(feature = "dim3")] + inv_principal_inertia: Vector::ZERO, #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - local_com: Point::origin(), + principal_inertia_local_frame: Rotation::IDENTITY, + local_com: Vector::ZERO, } } @@ -400,7 +389,7 @@ impl Sub for MassProperties { let inv_mass = utils::inv(new_mass); - let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let local_com = (self.local_com * m1 - other.local_com * m2) * inv_mass; let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let mut inertia = i1 - i2; @@ -435,7 +424,7 @@ impl Sub for MassProperties { } let inv_mass = utils::inv(new_mass); - let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; + let local_com = (self.local_com * m1 - other.local_com * m2) * inv_mass; let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let inertia = i1 - i2; @@ -463,7 +452,7 @@ impl Add for MassProperties { let m1 = utils::inv(self.inv_mass); let m2 = utils::inv(other.inv_mass); let inv_mass = utils::inv(m1 + m2); - let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; + let local_com = (self.local_com * m1 + other.local_com * m2) * inv_mass; let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let inertia = i1 + i2; @@ -487,7 +476,7 @@ impl Add for MassProperties { let m1 = utils::inv(self.inv_mass); let m2 = utils::inv(other.inv_mass); let inv_mass = utils::inv(m1 + m2); - let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; + let local_com = (self.local_com * m1 + other.local_com * m2) * inv_mass; let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); let inertia = i1 + i2; @@ -512,7 +501,7 @@ impl core::iter::Sum for MassProperties { use alloc::vec::Vec; let mut total_mass = 0.0; - let mut total_com = Point::origin(); + let mut total_com = Vector::ZERO; let mut total_inertia = 0.0; // TODO: avoid this allocation. // This is needed because we iterate twice. @@ -521,7 +510,7 @@ impl core::iter::Sum for MassProperties { for props in iter { let mass = utils::inv(props.inv_mass); total_mass += mass; - total_com += props.local_com.coords * mass; + total_com += props.local_com * mass; all_props.push(props); } @@ -548,8 +537,8 @@ impl core::iter::Sum for MassProperties { use alloc::vec::Vec; let mut total_mass = 0.0; - let mut total_com = Point::origin(); - let mut total_inertia = Matrix3::zeros(); + let mut total_com = Vector::ZERO; + let mut total_inertia = Matrix::ZERO; // TODO: avoid this allocation. // This is needed because we iterate twice. let mut all_props = Vec::new(); @@ -557,7 +546,7 @@ impl core::iter::Sum for MassProperties { for props in iter { let mass = utils::inv(props.inv_mass); total_mass += mass; - total_com += props.local_com.coords * mass; + total_com += props.local_com * mass; all_props.push(props); } @@ -565,6 +554,7 @@ impl core::iter::Sum for MassProperties { total_com /= total_mass; } + let total_com = total_com; for props in all_props { total_inertia += props.construct_shifted_inertia_matrix(total_com - props.local_com); } @@ -588,14 +578,11 @@ impl approx::AbsDiffEq for MassProperties { #[cfg(feature = "dim3")] let inertia_is_ok = self .reconstruct_inverse_inertia_matrix() - .abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon); + .abs_diff_eq(other.reconstruct_inverse_inertia_matrix(), epsilon); inertia_is_ok - && self.local_com.abs_diff_eq(&other.local_com, epsilon) + && self.local_com.abs_diff_eq(other.local_com, epsilon) && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) - && self - .inv_principal_inertia - .abs_diff_eq(&other.inv_principal_inertia, epsilon) } } @@ -617,11 +604,16 @@ impl approx::RelativeEq for MassProperties { max_relative, ); + // Compare either the inertia matrix or its inverse, whichever is most precise. #[cfg(feature = "dim3")] let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( &other.reconstruct_inverse_inertia_matrix(), epsilon, max_relative, + ) || self.reconstruct_inertia_matrix().relative_eq( + &other.reconstruct_inertia_matrix(), + epsilon, + max_relative, ); inertia_is_ok @@ -637,9 +629,9 @@ impl approx::RelativeEq for MassProperties { #[cfg(test)] mod test { use super::MassProperties; - use crate::math::{AngVector, Point}; #[cfg(feature = "dim3")] - use crate::math::{Rotation, Vector}; + use crate::math::Rotation; + use crate::math::{AngVector, Vector}; use crate::shape::{Ball, Capsule, Shape}; use approx::assert_relative_eq; use num::Zero; @@ -647,31 +639,37 @@ mod test { #[test] fn mass_properties_add_partial_zero() { let m1 = MassProperties { - local_com: Point::origin(), + local_com: Vector::ZERO, inv_mass: 2.0, - inv_principal_inertia: na::zero(), + #[cfg(feature = "dim2")] + inv_principal_inertia: 0.0, + #[cfg(feature = "dim3")] + inv_principal_inertia: Vector::ZERO, #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), + principal_inertia_local_frame: Rotation::IDENTITY, }; let m2 = MassProperties { - local_com: Point::origin(), + local_com: Vector::ZERO, inv_mass: 0.0, #[cfg(feature = "dim2")] inv_principal_inertia: 1.0, #[cfg(feature = "dim3")] - inv_principal_inertia: Vector::new(1.0, 2.0, 3.0), + inv_principal_inertia: Vector::new(3.0, 2.0, 1.0), #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), + principal_inertia_local_frame: Rotation::IDENTITY, }; let result = MassProperties { - local_com: Point::origin(), + local_com: Vector::ZERO, inv_mass: 2.0, #[cfg(feature = "dim2")] inv_principal_inertia: 1.0, #[cfg(feature = "dim3")] - inv_principal_inertia: Vector::new(1.0, 2.0, 3.0), + inv_principal_inertia: Vector::new(3.0, 2.0, 1.0), #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), + // TODO: ideally this should be IDENTITY, but glam’s conversion from matrix + // to quaternion returns this instead. This is OK for the cube test + // due to its symmetry though this needs a closer look. + principal_inertia_local_frame: Rotation::from_xyzw(1.0, 0.0, 0.0, 0.0), }; assert_eq!(m1 + m2, result); @@ -694,20 +692,21 @@ mod test { assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6); assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6); assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-5); assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6); assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-5); assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6); assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); + assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); - // NOTE: converting the inverse inertia matrices don’t work well here because + // NOTE: converting the inverse inertia matrices don't work well here because // tiny inertia value originating from the subtraction can result in a non-zero // (but large) inverse. assert_relative_eq!( (((m1m2m3 - m1) - m2) - m3).principal_inertia(), - AngVector::zero(), - epsilon = 1.0e-3 + AngVector::default(), + epsilon = 1.0e-2 ); assert_relative_eq!((((m1m2m3 - m1) - m2) - m3).mass(), 0.0, epsilon = 1.0e-6); } @@ -715,18 +714,16 @@ mod test { #[test] #[cfg(feature = "alloc")] fn mass_properties_compound() { - use na::Isometry; - use crate::{ - math::Vector, + math::{Pose, Vector}, shape::{Compound, Cuboid, SharedShape}, }; // Compute the mass properties of a compound shape made of three 1x1x1 cuboids. - let shape = Cuboid::new(Vector::repeat(0.5)); + let shape = Cuboid::new(Vector::splat(0.5)); let mp = shape.mass_properties(1.0); - let iso2 = Isometry::from_parts(Vector::y().into(), Default::default()); - let iso3 = Isometry::from_parts((-Vector::y()).into(), Default::default()); + let iso2 = Pose::from_parts(Vector::Y.into(), Default::default()); + let iso3 = Pose::from_parts((-Vector::Y).into(), Default::default()); // Test sum shifted result through `MassProperties::add` let sum = [mp, mp.transform_by(&iso2), mp.transform_by(&iso3)] @@ -736,7 +733,7 @@ mod test { // Test compound through `MassProperties::from_compound` let compound_shape = Compound::new(vec![ ( - Isometry::from_parts(Vector::default().into(), Default::default()), + Pose::from_parts(Vector::default().into(), Default::default()), SharedShape::new(shape), ), (iso2, SharedShape::new(shape)), @@ -747,13 +744,18 @@ mod test { // Check that the mass properties of the compound shape match the mass properties // of a single 1x3x1 cuboid. #[cfg(feature = "dim2")] - let expected = Cuboid::new(Vector::new(0.5, 1.5)).mass_properties(1.0); + let expected = Cuboid::new(Vector::new(1.5, 0.5)).mass_properties(1.0); #[cfg(feature = "dim3")] - let expected = Cuboid::new(Vector::new(0.5, 1.5, 0.5)).mass_properties(1.0); + let expected = Cuboid::new(Vector::new(1.5, 0.5, 0.5)).mass_properties(1.0); // Sum shifted assert_relative_eq!(sum.local_com, expected.local_com, epsilon = 1.0e-6); assert_relative_eq!(sum.inv_mass, expected.inv_mass, epsilon = 1.0e-6); + #[cfg(feature = "dim3")] + assert!(sum + .inv_principal_inertia + .abs_diff_eq(expected.inv_principal_inertia, 1.0e-6)); + #[cfg(feature = "dim2")] assert_relative_eq!( sum.inv_principal_inertia, expected.inv_principal_inertia, @@ -763,6 +765,11 @@ mod test { // Compound assert_relative_eq!(mp_compound.local_com, expected.local_com, epsilon = 1.0e-6); assert_relative_eq!(mp_compound.inv_mass, expected.inv_mass, epsilon = 1.0e-6); + #[cfg(feature = "dim3")] + assert!(mp_compound + .inv_principal_inertia + .abs_diff_eq(expected.inv_principal_inertia, 1.0e-6)); + #[cfg(feature = "dim2")] assert_relative_eq!( mp_compound.inv_principal_inertia, expected.inv_principal_inertia, diff --git a/src/mass_properties/mass_properties_ball.rs b/src/mass_properties/mass_properties_ball.rs index 1cd0193c..8a07185c 100644 --- a/src/mass_properties/mass_properties_ball.rs +++ b/src/mass_properties/mass_properties_ball.rs @@ -1,13 +1,10 @@ use crate::mass_properties::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::Vector; -use crate::math::{Point, PrincipalAngularInertia, Real}; -use na::RealField; +use crate::math::{PrincipalAngularInertia, Real, RealField, Vector}; impl MassProperties { pub(crate) fn ball_volume_unit_angular_inertia( radius: Real, - ) -> (Real, PrincipalAngularInertia) { + ) -> (Real, PrincipalAngularInertia) { #[cfg(feature = "dim2")] { let volume = Real::pi() * radius * radius; @@ -19,7 +16,7 @@ impl MassProperties { let volume = Real::pi() * radius * radius * radius * 4.0 / 3.0; let i = radius * radius * 2.0 / 5.0; - (volume, Vector::repeat(i)) + (volume, Vector::splat(i)) } } @@ -56,7 +53,7 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create mass properties for a 0.5m radius ball with density 1000 kg/m³ (water density) /// let radius = 0.5; @@ -69,7 +66,7 @@ impl MassProperties { /// assert!((mass - 523.6).abs() < 1.0); // Approximately 524 kg /// /// // Center of mass is at the origin for symmetric shapes - /// assert_eq!(ball_props.local_com, Point3::origin()); + /// assert_eq!(ball_props.local_com, Vector::ZERO); /// /// // Check if object can be moved (finite mass) /// assert!(ball_props.inv_mass > 0.0, "Ball has finite mass and can move"); @@ -81,7 +78,7 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a circular disc with 1.0m radius and density 100 kg/m² /// let radius = 1.0; @@ -102,7 +99,7 @@ impl MassProperties { /// /// - **Sports balls**: Soccer balls, basketballs, bowling balls /// - **Planets and celestial bodies**: Spherical approximations - /// - **Particles**: Point-like objects with rotational inertia + /// - **Particles**: Vector-like objects with rotational inertia /// - **Wheels and gears**: Cylindrical objects in 2D simulations /// /// # Performance Note @@ -113,6 +110,6 @@ impl MassProperties { pub fn from_ball(density: Real, radius: Real) -> Self { let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius); let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) + Self::new(Vector::ZERO, mass, unit_i * mass) } } diff --git a/src/mass_properties/mass_properties_capsule.rs b/src/mass_properties/mass_properties_capsule.rs index ce44a04d..4bf2fb03 100644 --- a/src/mass_properties/mass_properties_capsule.rs +++ b/src/mass_properties/mass_properties_capsule.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; #[cfg(feature = "dim3")] use crate::shape::Capsule; @@ -41,20 +41,20 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a capsule for a standing character (height ~2m, radius 0.3m) /// // Endpoints at (0, 0, 0) and (0, 2, 0) form vertical capsule - /// let a = Point3::origin(); - /// let b = Point3::new(0.0, 2.0, 0.0); + /// let a = Vector::ZERO; + /// let b = Vector::new(0.0, 2.0, 0.0); /// let radius = 0.3; /// let density = 985.0; // Similar to human body density /// /// let character_props = MassProperties::from_capsule(density, a, b, radius); /// /// // Center of mass is at the midpoint - /// let expected_com = Point3::new(0.0, 1.0, 0.0); - /// assert!((character_props.local_com - expected_com).norm() < 0.01); + /// let expected_com = Vector::new(0.0, 1.0, 0.0); + /// assert!((character_props.local_com - expected_com).length() < 0.01); /// /// let mass = character_props.mass(); /// println!("Character mass: {:.2} kg", mass); // Approximately 70-80 kg @@ -70,18 +70,18 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a horizontal capsule along the X-axis - /// let a = Point3::new(-1.0, 0.0, 0.0); - /// let b = Point3::new(1.0, 0.0, 0.0); + /// let a = Vector::new(-1.0, 0.0, 0.0); + /// let b = Vector::new(1.0, 0.0, 0.0); /// let radius = 0.5; /// let density = 1000.0; /// /// let capsule_props = MassProperties::from_capsule(density, a, b, radius); /// /// // Center of mass at midpoint (origin) - /// assert_eq!(capsule_props.local_com, Point3::origin()); + /// assert_eq!(capsule_props.local_com, Vector::ZERO); /// /// // Total length = distance + 2*radius = 2.0 + 1.0 = 3.0 meters /// println!("Mass: {:.2} kg", capsule_props.mass()); @@ -93,11 +93,11 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a horizontal 2D capsule (stadium/discorectangle shape) - /// let a = Point2::new(-2.0, 0.0); - /// let b = Point2::new(2.0, 0.0); + /// let a = Vector::new(-2.0, 0.0); + /// let b = Vector::new(2.0, 0.0); /// let radius = 1.0; /// let density = 100.0; // kg/m² /// @@ -120,7 +120,7 @@ impl MassProperties { /// /// - **Total length confusion**: The visual length is `distance(a, b) + 2 * radius`, /// not just `distance(a, b)`. The hemispheres add extra length. - /// - **Endpoint placement**: Points `a` and `b` are centers of the hemispherical caps, + /// - **Endpoint placement**: Vectors `a` and `b` are centers of the hemispherical caps, /// not the extreme ends of the capsule. /// /// # Performance Note @@ -128,14 +128,14 @@ impl MassProperties { /// Capsules are very efficient for collision detection (almost as fast as spheres) /// and provide smooth rolling behavior. They're preferred over cylinders for /// dynamic objects that need to move smoothly. - pub fn from_capsule(density: Real, a: Point, b: Point, radius: Real) -> Self { - let half_height = (b - a).norm() / 2.0; + pub fn from_capsule(density: Real, a: Vector, b: Vector, radius: Real) -> Self { + let half_height = (b - a).length() / 2.0; let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); let cap_vol = cyl_vol + ball_vol; let cap_mass = cap_vol * density; let mut cap_i = (cyl_unit_i * cyl_vol + ball_unit_i * ball_vol) * density; - let local_com = na::center(&a, &b); + let local_com = a.midpoint(b); #[cfg(feature = "dim2")] { diff --git a/src/mass_properties/mass_properties_compound.rs b/src/mass_properties/mass_properties_compound.rs index b1b2c1bb..0f0eeb0d 100644 --- a/src/mass_properties/mass_properties_compound.rs +++ b/src/mass_properties/mass_properties_compound.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::shape::SharedShape; impl MassProperties { @@ -16,7 +16,7 @@ impl MassProperties { /// - In 3D: kg/m³ (mass per unit volume) /// - In 2D: kg/m² (mass per unit area) /// * `shapes` - Array of (position, shape) pairs - /// - Each shape has an `Isometry` (position + rotation) + /// - Each shape has an `Pose` (position + rotation) /// - Shapes can be any type implementing the `Shape` trait /// /// # Returns @@ -40,16 +40,16 @@ impl MassProperties { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::{Ball, SharedShape}; - /// use nalgebra::{Isometry3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// // Create a dumbbell: two balls connected by a bar /// let ball = SharedShape::new(Ball::new(0.5)); - /// let bar = SharedShape::new(parry3d::shape::Cuboid::new(Vector3::new(0.1, 1.0, 0.1))); + /// let bar = SharedShape::new(parry3d::shape::Cuboid::new(Vector::new(0.1, 1.0, 0.1))); /// /// let shapes = vec![ - /// (Isometry3::translation(0.0, -1.0, 0.0), ball.clone()), // Left ball - /// (Isometry3::identity(), bar), // Center bar - /// (Isometry3::translation(0.0, 1.0, 0.0), ball), // Right ball + /// (Pose::translation(0.0, -1.0, 0.0), ball.clone()), // Left ball + /// (Pose::identity(), bar), // Center bar + /// (Pose::translation(0.0, 1.0, 0.0), ball), // Right ball /// ]; /// /// let density = 1000.0; @@ -71,16 +71,16 @@ impl MassProperties { /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; /// use parry2d::shape::{Cuboid, SharedShape}; - /// use nalgebra::{Isometry2, Vector2}; + /// use parry2d::math::{Pose, Vector}; /// /// // Create a simple table: top surface + legs - /// let top = SharedShape::new(Cuboid::new(Vector2::new(2.0, 0.1))); // Wide, thin top - /// let leg = SharedShape::new(Cuboid::new(Vector2::new(0.1, 0.5))); // Narrow, tall leg + /// let top = SharedShape::new(Cuboid::new(Vector::new(2.0, 0.1))); // Wide, thin top + /// let leg = SharedShape::new(Cuboid::new(Vector::new(0.1, 0.5))); // Narrow, tall leg /// /// let shapes = vec![ - /// (Isometry2::translation(0.0, 0.6), top), // Table top - /// (Isometry2::translation(-1.5, 0.0), leg.clone()), // Left leg - /// (Isometry2::translation(1.5, 0.0), leg), // Right leg + /// (Pose::translation(0.0, 0.6), top), // Table top + /// (Pose::translation(-1.5, 0.0), leg.clone()), // Left leg + /// (Pose::translation(1.5, 0.0), leg), // Right leg /// ]; /// /// let density = 500.0; // Wood @@ -96,25 +96,25 @@ impl MassProperties { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::{Capsule, Cuboid, SharedShape}; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// // Simple robot arm with multiple segments - /// let base = SharedShape::new(Cuboid::new(Vector3::new(0.3, 0.2, 0.3))); + /// let base = SharedShape::new(Cuboid::new(Vector::new(0.3, 0.2, 0.3))); /// let upper_arm = SharedShape::new(Capsule::new( - /// Point3::origin(), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(0.0, 1.0, 0.0), /// 0.1 /// )); /// let forearm = SharedShape::new(Capsule::new( - /// Point3::origin(), - /// Point3::new(0.0, 0.8, 0.0), + /// Vector::ZERO, + /// Vector::new(0.0, 0.8, 0.0), /// 0.08 /// )); /// /// let shapes = vec![ - /// (Isometry3::identity(), base), - /// (Isometry3::translation(0.0, 0.2, 0.0), upper_arm), - /// (Isometry3::translation(0.0, 1.2, 0.0), forearm), + /// (Pose::identity(), base), + /// (Pose::translation(0.0, 0.2, 0.0), upper_arm), + /// (Pose::translation(0.0, 1.2, 0.0), forearm), /// ]; /// /// let density = 2700.0; // Aluminum @@ -157,7 +157,7 @@ impl MassProperties { /// - `MassProperties::transform_by()`: Transform mass properties to a new frame /// - `Add` trait: Combine mass properties with `+` operator /// - `Sum` trait: Sum an iterator of mass properties - pub fn from_compound(density: Real, shapes: &[(Isometry, SharedShape)]) -> Self { + pub fn from_compound(density: Real, shapes: &[(Pose, SharedShape)]) -> Self { shapes .iter() .map(|s| s.1.mass_properties(density).transform_by(&s.0)) diff --git a/src/mass_properties/mass_properties_cone.rs b/src/mass_properties/mass_properties_cone.rs index 53d8be47..4a94819b 100644 --- a/src/mass_properties/mass_properties_cone.rs +++ b/src/mass_properties/mass_properties_cone.rs @@ -1,12 +1,11 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Real, Rotation, Vector}; -use na::RealField; +use crate::math::{PrincipalAngularInertia, Real, RealField, Rotation, Vector}; impl MassProperties { pub(crate) fn cone_y_volume_unit_inertia( half_height: Real, radius: Real, - ) -> (Real, PrincipalAngularInertia) { + ) -> (Real, PrincipalAngularInertia) { let volume = radius * radius * Real::pi() * half_height * 2.0 / 3.0; let sq_radius = radius * radius; let sq_height = half_height * half_height * 4.0; @@ -51,7 +50,7 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Standard traffic cone: 70cm tall, 30cm base diameter /// // Made of flexible plastic, density ~950 kg/m³ @@ -173,10 +172,10 @@ impl MassProperties { let cyl_mass = cyl_vol * density; Self::with_principal_inertia_frame( - Point::new(0.0, -half_height / 2.0, 0.0), + Vector::new(0.0, -half_height / 2.0, 0.0), cyl_mass, cyl_unit_i * cyl_mass, - Rotation::identity(), + Rotation::IDENTITY, ) } } diff --git a/src/mass_properties/mass_properties_convex_polygon.rs b/src/mass_properties/mass_properties_convex_polygon.rs index cee0054e..5f510e13 100644 --- a/src/mass_properties/mass_properties_convex_polygon.rs +++ b/src/mass_properties/mass_properties_convex_polygon.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::shape::Triangle; impl MassProperties { @@ -37,14 +37,14 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// use std::f32::consts::PI; /// /// // Create a regular pentagon with radius 1.0 /// let mut vertices = Vec::new(); /// for i in 0..5 { /// let angle = (i as f32) * 2.0 * PI / 5.0; - /// vertices.push(Point2::new(angle.cos(), angle.sin())); + /// vertices.push(Vector::new(angle.cos(), angle.sin())); /// } /// /// let density = 100.0; @@ -54,7 +54,7 @@ impl MassProperties { /// println!("Center of mass: {:?}", pentagon_props.local_com); /// /// // For a regular polygon centered at origin, COM should be near origin - /// assert!(pentagon_props.local_com.coords.norm() < 0.01); + /// assert!(pentagon_props.local_com.length() < 0.01); /// # } /// ``` /// @@ -63,14 +63,14 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a trapezoid (4 vertices) /// let vertices = vec![ - /// Point2::origin(), // Bottom left - /// Point2::new(4.0, 0.0), // Bottom right - /// Point2::new(3.0, 2.0), // Top right - /// Point2::new(1.0, 2.0), // Top left + /// Vector::ZERO, // Bottom left + /// Vector::new(4.0, 0.0), // Bottom right + /// Vector::new(3.0, 2.0), // Top right + /// Vector::new(1.0, 2.0), // Top left /// ]; /// /// let density = 50.0; @@ -88,15 +88,15 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Arbitrary convex polygon /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(2.0, 0.0), - /// Point2::new(3.0, 1.0), - /// Point2::new(2.0, 2.5), - /// Point2::new(0.0, 2.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0), + /// Vector::new(3.0, 1.0), + /// Vector::new(2.0, 2.5), + /// Vector::new(0.0, 2.0), /// ]; /// /// let density = 200.0; @@ -135,7 +135,7 @@ impl MassProperties { /// /// Computation time is O(n) where n is the number of vertices. The polygon is /// decomposed into n triangles, each processed independently. - pub fn from_convex_polygon(density: Real, vertices: &[Point]) -> MassProperties { + pub fn from_convex_polygon(density: Real, vertices: &[Vector]) -> MassProperties { let (area, com) = convex_polygon_area_and_center_of_mass(vertices); if area == 0.0 { @@ -158,14 +158,10 @@ impl MassProperties { } /// Computes the area and center-of-mass of a convex polygon. -pub fn convex_polygon_area_and_center_of_mass( - convex_polygon: &[Point], -) -> (Real, Point) { - let geometric_center = convex_polygon - .iter() - .fold(Point::origin(), |e1, e2| e1 + e2.coords) - / convex_polygon.len() as Real; - let mut res = Point::origin(); +pub fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Vector]) -> (Real, Vector) { + let mut geometric_center = convex_polygon.iter().fold(Vector::ZERO, |e1, e2| e1 + e2); + geometric_center /= convex_polygon.len() as Real; + let mut res = Vector::ZERO; let mut areasum = 0.0; let mut iterpeek = convex_polygon.iter().peekable(); @@ -177,7 +173,7 @@ pub fn convex_polygon_area_and_center_of_mass( &geometric_center, ); let area = Triangle::new(*a, **b, *c).area(); - let center = (a.coords + b.coords + c.coords) / 3.0; + let center = (*a + **b + *c) / 3.0; res += center * area; areasum += area; @@ -186,6 +182,7 @@ pub fn convex_polygon_area_and_center_of_mass( if areasum == 0.0 { (areasum, geometric_center) } else { - (areasum, res / areasum) + res /= areasum; + (areasum, res) } } diff --git a/src/mass_properties/mass_properties_convex_polyhedron.rs b/src/mass_properties/mass_properties_convex_polyhedron.rs index 603aec55..5af7aac2 100644 --- a/src/mass_properties/mass_properties_convex_polyhedron.rs +++ b/src/mass_properties/mass_properties_convex_polyhedron.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, Real, DIM}; +use crate::math::{Real, Vector, DIM}; impl MassProperties { /// Computes the mass properties of a convex polyhedron (3D) or polygon (2D). @@ -30,14 +30,14 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a regular tetrahedron (4 vertices, 4 triangular faces) /// let vertices = vec![ - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(0.0, 0.0, 1.0), - /// Point3::origin(), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), + /// Vector::ZERO, /// ]; /// /// let indices = vec![ @@ -60,16 +60,16 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Regular octahedron (6 vertices, 8 triangular faces) /// let vertices = vec![ - /// Point3::new(1.0, 0.0, 0.0), // +X - /// Point3::new(-1.0, 0.0, 0.0), // -X - /// Point3::new(0.0, 1.0, 0.0), // +Y - /// Point3::new(0.0, -1.0, 0.0), // -Y - /// Point3::new(0.0, 0.0, 1.0), // +Z - /// Point3::new(0.0, 0.0, -1.0), // -Z + /// Vector::new(1.0, 0.0, 0.0), // +X + /// Vector::new(-1.0, 0.0, 0.0), // -X + /// Vector::new(0.0, 1.0, 0.0), // +Y + /// Vector::new(0.0, -1.0, 0.0), // -Y + /// Vector::new(0.0, 0.0, 1.0), // +Z + /// Vector::new(0.0, 0.0, -1.0), // -Z /// ]; /// /// let indices = vec![ @@ -120,7 +120,7 @@ impl MassProperties { /// - `from_compound()`: For combining multiple convex shapes pub fn from_convex_polyhedron( density: Real, - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], ) -> MassProperties { Self::from_trimesh(density, vertices, indices) diff --git a/src/mass_properties/mass_properties_cuboid.rs b/src/mass_properties/mass_properties_cuboid.rs index 460d68f5..3f170221 100644 --- a/src/mass_properties/mass_properties_cuboid.rs +++ b/src/mass_properties/mass_properties_cuboid.rs @@ -1,10 +1,10 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Real, Vector}; +use crate::math::{PrincipalAngularInertia, Real, Vector}; impl MassProperties { pub(crate) fn cuboid_volume_unit_inertia( - half_extents: Vector, - ) -> (Real, PrincipalAngularInertia) { + half_extents: Vector, + ) -> (Real, PrincipalAngularInertia) { #[cfg(feature = "dim2")] { let volume = half_extents.x * half_extents.y * 4.0; @@ -37,8 +37,8 @@ impl MassProperties { /// - In 3D: units are typically kg/m³ (e.g., wood = 500-900, concrete = 2400) /// - In 2D: units are typically kg/m² (mass per unit area) /// * `half_extents` - Half the size along each axis (center to face distance). - /// - In 3D: `Vector3::new(hx, hy, hz)` creates a box with dimensions 2hx × 2hy × 2hz - /// - In 2D: `Vector2::new(hx, hy)` creates a rectangle with dimensions 2hx × 2hy + /// - In 3D: `Vector::new(hx, hy, hz)` creates a box with dimensions 2hx × 2hy × 2hz + /// - In 2D: `Vector::new(hx, hy)` creates a rectangle with dimensions 2hx × 2hy /// /// # Returns /// @@ -60,11 +60,11 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// // Create a wooden crate: 2m × 1m × 1m (half_extents = 1.0, 0.5, 0.5) /// // Wood density: approximately 600 kg/m³ - /// let half_extents = Vector3::new(1.0, 0.5, 0.5); + /// let half_extents = Vector::new(1.0, 0.5, 0.5); /// let density = 600.0; /// let crate_props = MassProperties::from_cuboid(density, half_extents); /// @@ -80,7 +80,7 @@ impl MassProperties { /// println!("Inertia around z-axis: {:.2}", inertia.z); // Higher /// /// // Center of mass is at the origin - /// assert_eq!(crate_props.local_com, Point3::origin()); + /// assert_eq!(crate_props.local_com, Vector::ZERO); /// # } /// ``` /// @@ -89,10 +89,10 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// /// // Create a 1m × 1m × 1m cube (half_extents = 0.5 on all axes) - /// let half_extents = Vector3::new(0.5, 0.5, 0.5); + /// let half_extents = Vector::new(0.5, 0.5, 0.5); /// let density = 1000.0; // Water density /// let cube_props = MassProperties::from_cuboid(density, half_extents); /// @@ -111,10 +111,10 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Vector2; + /// use parry2d::math::Vector; /// /// // Create a 4m × 2m rectangular platform (half_extents = 2.0, 1.0) - /// let half_extents = Vector2::new(2.0, 1.0); + /// let half_extents = Vector::new(2.0, 1.0); /// let density = 500.0; // kg/m² /// let platform_props = MassProperties::from_cuboid(density, half_extents); /// @@ -139,7 +139,7 @@ impl MassProperties { /// # Common Mistakes /// /// - **Wrong dimensions**: Remember that `half_extents` are HALF the total size. - /// For a 2m × 2m × 2m box, use `Vector3::new(1.0, 1.0, 1.0)`, not `(2.0, 2.0, 2.0)` + /// For a 2m × 2m × 2m box, use `Vector::new(1.0, 1.0, 1.0)`, not `(2.0, 2.0, 2.0)` /// - **Unit confusion**: Ensure density units match your distance units /// (kg/m³ with meters, kg/cm³ with centimeters, etc.) /// @@ -147,9 +147,9 @@ impl MassProperties { /// /// This is a very fast computation (constant time). Cuboids are the second simplest /// shape after balls and are highly efficient for collision detection. - pub fn from_cuboid(density: Real, half_extents: Vector) -> Self { + pub fn from_cuboid(density: Real, half_extents: Vector) -> Self { let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents); let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) + Self::new(Vector::ZERO, mass, unit_i * mass) } } diff --git a/src/mass_properties/mass_properties_cylinder.rs b/src/mass_properties/mass_properties_cylinder.rs index c099ceb2..53e78028 100644 --- a/src/mass_properties/mass_properties_cylinder.rs +++ b/src/mass_properties/mass_properties_cylinder.rs @@ -1,16 +1,13 @@ use crate::mass_properties::MassProperties; use crate::math::{PrincipalAngularInertia, Real, Vector}; #[cfg(feature = "dim3")] -use { - crate::math::{Point, Rotation}, - na::RealField, -}; +use crate::math::{RealField, Rotation}; impl MassProperties { pub(crate) fn cylinder_y_volume_unit_inertia( half_height: Real, radius: Real, - ) -> (Real, PrincipalAngularInertia) { + ) -> (Real, PrincipalAngularInertia) { #[cfg(feature = "dim2")] { Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) @@ -65,7 +62,7 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Standard soda can: 12.3cm tall, 6.6cm diameter /// // Aluminum density: ~2700 kg/m³ @@ -79,7 +76,7 @@ impl MassProperties { /// println!("Can mass: {:.2} kg", mass); // Approximately 0.15 kg (150 grams) /// /// // Center of mass at origin - /// assert_eq!(can_props.local_com, Point3::origin()); + /// assert_eq!(can_props.local_com, Vector::ZERO); /// /// // Check inertia differences /// let inertia = can_props.principal_inertia(); @@ -117,7 +114,7 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let half_height = 1.0; /// let radius = 0.5; @@ -127,8 +124,8 @@ impl MassProperties { /// let cylinder = MassProperties::from_cylinder(density, half_height, radius); /// /// // Capsule has rounded ends (smooth) - /// let a = Point3::new(0.0, -half_height, 0.0); - /// let b = Point3::new(0.0, half_height, 0.0); + /// let a = Vector::new(0.0, -half_height, 0.0); + /// let b = Vector::new(0.0, half_height, 0.0); /// let capsule = MassProperties::from_capsule(density, a, b, radius); /// /// // Capsule has more mass due to hemispherical caps @@ -174,10 +171,10 @@ impl MassProperties { let cyl_mass = cyl_vol * density; Self::with_principal_inertia_frame( - Point::origin(), + Vector::ZERO, cyl_mass, cyl_unit_i * cyl_mass, - Rotation::identity(), + Rotation::IDENTITY, ) } } diff --git a/src/mass_properties/mass_properties_triangle.rs b/src/mass_properties/mass_properties_triangle.rs index 5800e972..67ec48bb 100644 --- a/src/mass_properties/mass_properties_triangle.rs +++ b/src/mass_properties/mass_properties_triangle.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::shape::Triangle; impl MassProperties { @@ -37,15 +37,15 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a right triangle with legs of 3m and 4m - /// let a = Point2::origin(); - /// let b = Point2::new(3.0, 0.0); - /// let c = Point2::new(0.0, 4.0); + /// let a = Vector::ZERO; + /// let b = Vector::new(3.0, 0.0); + /// let c = Vector::new(0.0, 4.0); /// let density = 100.0; // kg/m² /// - /// let triangle_props = MassProperties::from_triangle(density, &a, &b, &c); + /// let triangle_props = MassProperties::from_triangle(density, a, b, c); /// /// // Area = (1/2) × base × height = (1/2) × 3 × 4 = 6 m² /// // Mass = area × density = 600 kg @@ -67,18 +67,18 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Equilateral triangle with side length 2m /// let side = 2.0; /// let height = side * (3.0_f32.sqrt() / 2.0); /// - /// let a = Point2::origin(); - /// let b = Point2::new(side, 0.0); - /// let c = Point2::new(side / 2.0, height); + /// let a = Vector::ZERO; + /// let b = Vector::new(side, 0.0); + /// let c = Vector::new(side / 2.0, height); /// let density = 50.0; /// - /// let tri_props = MassProperties::from_triangle(density, &a, &b, &c); + /// let tri_props = MassProperties::from_triangle(density, a, b, c); /// /// // For equilateral triangle: Area = (side² × √3) / 4 /// let expected_area = side * side * 3.0_f32.sqrt() / 4.0; @@ -94,15 +94,15 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Triangle in 3D space (e.g., a metal plate or sail) - /// let a = Point3::origin(); - /// let b = Point3::new(2.0, 0.0, 0.0); - /// let c = Point3::new(1.0, 2.0, 0.0); + /// let a = Vector::ZERO; + /// let b = Vector::new(2.0, 0.0, 0.0); + /// let c = Vector::new(1.0, 2.0, 0.0); /// let density = 200.0; // kg/m² (sheet metal) /// - /// let plate_props = MassProperties::from_triangle(density, &a, &b, &c); + /// let plate_props = MassProperties::from_triangle(density, a, b, c); /// /// // Area = 2 m² (base=2, height=2, area=(1/2)×2×2=2) /// // Mass = 400 kg @@ -113,20 +113,20 @@ impl MassProperties { /// # } /// ``` /// - /// # Example - Degenerate Triangle (Collinear Points) + /// # Example - Degenerate Triangle (Collinear Vectors) /// /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Three points on a line (no area) - /// let a = Point2::origin(); - /// let b = Point2::new(1.0, 1.0); - /// let c = Point2::new(2.0, 2.0); + /// let a = Vector::ZERO; + /// let b = Vector::new(1.0, 1.0); + /// let c = Vector::new(2.0, 2.0); /// let density = 100.0; /// - /// let degenerate = MassProperties::from_triangle(density, &a, &b, &c); + /// let degenerate = MassProperties::from_triangle(density, a, b, c); /// /// // Zero area means zero mass /// assert_eq!(degenerate.mass(), 0.0); @@ -167,13 +167,8 @@ impl MassProperties { /// /// Computing triangle mass properties is very fast (constant time) and involves /// only basic geometric calculations (area, centroid, and moment of inertia). - pub fn from_triangle( - density: Real, - a: &Point, - b: &Point, - c: &Point, - ) -> MassProperties { - let triangle = Triangle::new(*a, *b, *c); + pub fn from_triangle(density: Real, a: Vector, b: Vector, c: Vector) -> MassProperties { + let triangle = Triangle::new(a, b, c); let area = triangle.area(); let com = triangle.center(); diff --git a/src/mass_properties/mass_properties_trimesh2d.rs b/src/mass_properties/mass_properties_trimesh2d.rs index 1e005e26..f3a4ec12 100644 --- a/src/mass_properties/mass_properties_trimesh2d.rs +++ b/src/mass_properties/mass_properties_trimesh2d.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::shape::Triangle; impl MassProperties { @@ -41,16 +41,16 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::mass_properties::MassProperties; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create an L-shaped mesh from two rectangles (4 triangles) /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(2.0, 0.0), - /// Point2::new(2.0, 1.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(1.0, 3.0), - /// Point2::new(0.0, 3.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0), + /// Vector::new(2.0, 1.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(1.0, 3.0), + /// Vector::new(0.0, 3.0), /// ]; /// /// let indices = vec![ @@ -73,15 +73,15 @@ impl MassProperties { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Square pyramid: 4 vertices at base + 1 apex /// let vertices = vec![ - /// Point3::new(-1.0, 0.0, -1.0), // Base corner 1 - /// Point3::new(1.0, 0.0, -1.0), // Base corner 2 - /// Point3::new(1.0, 0.0, 1.0), // Base corner 3 - /// Point3::new(-1.0, 0.0, 1.0), // Base corner 4 - /// Point3::new(0.0, 2.0, 0.0), // Apex + /// Vector::new(-1.0, 0.0, -1.0), // Base corner 1 + /// Vector::new(1.0, 0.0, -1.0), // Base corner 2 + /// Vector::new(1.0, 0.0, 1.0), // Base corner 3 + /// Vector::new(-1.0, 0.0, 1.0), // Base corner 4 + /// Vector::new(0.0, 2.0, 0.0), // Apex /// ]; /// /// let indices = vec![ @@ -161,7 +161,7 @@ impl MassProperties { /// - `from_compound()`: Combine multiple simpler shapes pub fn from_trimesh( density: Real, - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; 3]], ) -> MassProperties { let (area, com) = trimesh_area_and_center_of_mass(vertices, indices); @@ -191,10 +191,10 @@ impl MassProperties { /// Computes the area and center-of-mass of a triangle-mesh. pub fn trimesh_area_and_center_of_mass( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; 3]], -) -> (Real, Point) { - let mut res = Point::origin(); +) -> (Real, Vector) { + let mut res = Vector::ZERO; let mut areasum = 0.0; for idx in indices { @@ -206,13 +206,14 @@ pub fn trimesh_area_and_center_of_mass( let area = triangle.area(); let center = triangle.center(); - res += center.coords * area; + res += center * area; areasum += area; } if areasum == 0.0 { (areasum, res) } else { - (areasum, res / areasum) + res /= areasum; + (areasum, res) } } diff --git a/src/mass_properties/mass_properties_trimesh3d.rs b/src/mass_properties/mass_properties_trimesh3d.rs index 7c1ec560..fd9c2281 100644 --- a/src/mass_properties/mass_properties_trimesh3d.rs +++ b/src/mass_properties/mass_properties_trimesh3d.rs @@ -1,5 +1,5 @@ use crate::mass_properties::MassProperties; -use crate::math::{Matrix, Point, Real, DIM}; +use crate::math::{Matrix, Real, Vector, DIM}; use crate::shape::Tetrahedron; use num::Zero; @@ -7,7 +7,7 @@ impl MassProperties { /// Computes the mass properties of a triangle mesh. pub fn from_trimesh( density: Real, - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], ) -> MassProperties { let (volume, com) = trimesh_signed_volume_and_center_of_mass(vertices, indices); @@ -16,15 +16,15 @@ impl MassProperties { return MassProperties::zero(); } - let mut itot = Matrix::zeros(); + let mut itot = Matrix::ZERO; for t in indices { - let p2 = &vertices[t[0] as usize]; - let p3 = &vertices[t[1] as usize]; - let p4 = &vertices[t[2] as usize]; + let p2 = vertices[t[0] as usize]; + let p3 = vertices[t[1] as usize]; + let p4 = vertices[t[2] as usize]; - let vol = Tetrahedron::new(com, *p2, *p3, *p4).signed_volume(); - let ipart = tetrahedron_unit_inertia_tensor_wrt_point(&com, &com, p2, p3, p4); + let vol = Tetrahedron::new(com, p2, p3, p4).signed_volume(); + let ipart = tetrahedron_unit_inertia_tensor_wrt_point(com, com, p2, p3, p4); itot += ipart * vol; } @@ -36,12 +36,12 @@ impl MassProperties { /// Computes the unit inertia tensor of a tetrahedron, with regard to the given `point`. pub fn tetrahedron_unit_inertia_tensor_wrt_point( - point: &Point, - p1: &Point, - p2: &Point, - p3: &Point, - p4: &Point, -) -> Matrix { + point: Vector, + p1: Vector, + p2: Vector, + p3: Vector, + p4: Vector, +) -> Matrix { let p1 = p1 - point; let p2 = p2 - point; let p3 = p3 - point; @@ -148,17 +148,17 @@ pub fn tetrahedron_unit_inertia_tensor_wrt_point( + x4 * y4 * 2.0) * 0.05; - Matrix::new(a0, -c1, -b1, -c1, b0, -a1, -b1, -a1, c0) + Matrix::from_cols_array(&[a0, -c1, -b1, -c1, b0, -a1, -b1, -a1, c0]) } /// Computes the volume and center-of-mass of a mesh. pub fn trimesh_signed_volume_and_center_of_mass( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], -) -> (Real, Point) { +) -> (Real, Vector) { let geometric_center = crate::utils::center(vertices); - let mut res = Point::origin(); + let mut res = Vector::ZERO; let mut vol = 0.0; for t in indices { @@ -169,7 +169,7 @@ pub fn trimesh_signed_volume_and_center_of_mass( let volume = Tetrahedron::new(geometric_center, p2, p3, p4).signed_volume(); let center = Tetrahedron::new(geometric_center, p2, p3, p4).center(); - res += center.coords * volume; + res += center * volume; vol += volume; } @@ -182,25 +182,22 @@ pub fn trimesh_signed_volume_and_center_of_mass( #[cfg(test)] mod test { - use crate::math::{Isometry, Vector}; + use crate::math::{Pose, Rotation, Vector, VectorExt}; use crate::{ mass_properties::MassProperties, shape::{Ball, Capsule, Cone, Cuboid, Cylinder, Shape}, }; - use na::UnitQuaternion; use std::dbg; fn assert_same_principal_inertias(mprops1: &MassProperties, mprops2: &MassProperties) { for k in 0..3 { let i1 = mprops1.principal_inertia_local_frame - * mprops1.principal_inertia().component_mul( - &(mprops1.principal_inertia_local_frame.inverse() * Vector::ith(k, 1.0)), - ); + * (mprops1.principal_inertia() + * (mprops1.principal_inertia_local_frame.inverse() * Vector::ith(k, 1.0))); let i2 = mprops2.principal_inertia_local_frame - * mprops2.principal_inertia().component_mul( - &(mprops2.principal_inertia_local_frame.inverse() * Vector::ith(k, 1.0)), - ); - assert_relative_eq!(i1, i2, epsilon = 0.5) + * (mprops2.principal_inertia() + * (mprops2.principal_inertia_local_frame.inverse() * Vector::ith(k, 1.0))); + assert!(i1.abs_diff_eq(i2, 0.5)) } } @@ -215,10 +212,10 @@ mod test { let mut trimesh = cuboid.to_trimesh(); let mprops = MassProperties::from_trimesh(1.0, &trimesh.0, &trimesh.1); assert_relative_eq!(mprops.mass(), 48.0, epsilon = 1.0e-4); - assert_relative_eq!( - (mprops.principal_inertia_local_frame * mprops.principal_inertia()).abs(), - Vector::new(208.0, 160.0, 80.0), - epsilon = 1.0e-4 + assert!( + (mprops.principal_inertia_local_frame * mprops.principal_inertia()) + .abs() + .abs_diff_eq(Vector::new(208.0, 160.0, 80.0), 1.0e-4) ); // Check after shifting the trimesh off the origin. @@ -228,10 +225,10 @@ mod test { .for_each(|pt| *pt += Vector::new(30.0, 20.0, 10.0)); let mprops = MassProperties::from_trimesh(1.0, &trimesh.0, &trimesh.1); assert_relative_eq!(mprops.mass(), 48.0, epsilon = 1.0e-4); - assert_relative_eq!( - (mprops.principal_inertia_local_frame * mprops.principal_inertia()).abs(), - Vector::new(208.0, 160.0, 80.0), - epsilon = 1.0e-4 + assert!( + (mprops.principal_inertia_local_frame * mprops.principal_inertia()) + .abs() + .abs_diff_eq(Vector::new(208.0, 160.0, 80.0), 1.0e-4) ); } @@ -293,11 +290,11 @@ mod test { // vertices. #[test] fn rotated_inertia_tensor() { - let cuboid = Cuboid::new(Vector::new(1.0, 2.0, 3.0)); + let cuboid = Cuboid::new(Vector::new(3.0, 2.0, 1.0)); let density = 1.0; // Compute mass properties with a translated and rotated cuboid. - let pose = Isometry::new(Vector::new(5.0, 2.0, 3.0), Vector::new(0.6, 0.7, 0.8)); + let pose = Pose::new(Vector::new(5.0, 2.0, 3.0), Vector::new(0.6, 0.7, 0.8)); let mprops = cuboid.mass_properties(density); // Compute mass properties with a manually transformed cuboid. @@ -330,13 +327,11 @@ mod test { trimesh_transformed_mprops.local_com, epsilon = 1.0e-4 ); - assert_relative_eq!( - trimesh_origin_mprops.principal_inertia(), - mprops.principal_inertia(), - epsilon = 1.0e-4 - ); + assert!(trimesh_origin_mprops + .principal_inertia() + .abs_diff_eq(mprops.principal_inertia(), 1.0e-4)); let w1 = trimesh_origin_mprops.world_inv_inertia(&pose.rotation); - let w2 = trimesh_transformed_mprops.world_inv_inertia(&UnitQuaternion::identity()); + let w2 = trimesh_transformed_mprops.world_inv_inertia(&Rotation::IDENTITY); assert_relative_eq!(w1.m11, w2.m11, epsilon = 1.0e-7); assert_relative_eq!(w1.m12, w2.m12, epsilon = 1.0e-7); assert_relative_eq!(w1.m13, w2.m13, epsilon = 1.0e-7); diff --git a/src/mass_properties/mass_properties_voxels.rs b/src/mass_properties/mass_properties_voxels.rs index b321a739..e08b907f 100644 --- a/src/mass_properties/mass_properties_voxels.rs +++ b/src/mass_properties/mass_properties_voxels.rs @@ -1,5 +1,7 @@ use crate::mass_properties::MassProperties; -use crate::math::{Point, Real}; +#[cfg(feature = "dim3")] +use crate::math::Matrix; +use crate::math::{Real, Vector}; use crate::shape::Voxels; impl MassProperties { @@ -41,18 +43,18 @@ impl MassProperties { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // Create a 3×3×3 voxel grid with 1m voxels - /// let voxel_size = Vector3::new(1.0, 1.0, 1.0); + /// let voxel_size = Vector::new(1.0, 1.0, 1.0); /// /// // Fill some voxels to create an L-shape /// let voxels = &[ - /// Point3::new(0, 0, 0), // Bottom bar - /// Point3::new(1, 0, 0), - /// Point3::new(2, 0, 0), - /// Point3::new(0, 1, 0), // Vertical part - /// Point3::new(0, 2, 0), + /// IVector::new(0, 0, 0), // Bottom bar + /// IVector::new(1, 0, 0), + /// IVector::new(2, 0, 0), + /// IVector::new(0, 1, 0), // Vertical part + /// IVector::new(0, 2, 0), /// ]; /// let voxels = Voxels::new(voxel_size, voxels); /// @@ -71,17 +73,17 @@ impl MassProperties { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // Create a chunk of destructible terrain - /// let voxel_size = Vector3::new(0.5, 0.5, 0.5); // 50cm voxels + /// let voxel_size = Vector::new(0.5, 0.5, 0.5); // 50cm voxels /// let mut voxels = vec![]; /// /// // Fill a 4×4×4 solid block /// for x in 0..4 { /// for y in 0..4 { /// for z in 0..4 { - /// voxels.push(Point3::new(x, y, z)); + /// voxels.push(IVector::new(x, y, z)); /// } /// } /// } @@ -101,16 +103,16 @@ impl MassProperties { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::mass_properties::MassProperties; /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // Large sparse grid (only stores filled voxels since v0.25.0) - /// let voxel_size = Vector3::new(0.1, 0.1, 0.1); + /// let voxel_size = Vector::new(0.1, 0.1, 0.1); /// /// // Scatter some voxels in a large space (efficient with sparse storage) /// let voxels = &[ - /// Point3::new(0, 0, 0), - /// Point3::new(100, 50, 75), - /// Point3::new(-50, 200, -30), + /// IVector::new(0, 0, 0), + /// IVector::new(100, 50, 75), + /// IVector::new(-50, 200, -30), /// ]; /// let voxels = Voxels::new(voxel_size, voxels); /// let density = 1000.0; @@ -170,19 +172,22 @@ impl MassProperties { /// - `from_trimesh()`: Alternative for precise shapes /// - `from_compound()`: Combine multiple shapes efficiently pub fn from_voxels(density: Real, voxels: &Voxels) -> Self { - let mut com = Point::origin(); + let mut com = Vector::ZERO; let mut num_not_empty = 0; - let mut angular_inertia = na::zero(); + #[cfg(feature = "dim2")] + let mut angular_inertia = 0.0; + #[cfg(feature = "dim3")] + let mut angular_inertia = Matrix::ZERO; let block_ref_mprops = MassProperties::from_cuboid(density, voxels.voxel_size() / 2.0); for vox in voxels.voxels() { if !vox.state.is_empty() { - com += vox.center.coords; + com += vox.center; num_not_empty += 1; } } - com.coords /= num_not_empty as Real; + com /= num_not_empty as Real; for vox in voxels.voxels() { if !vox.state.is_empty() { diff --git a/src/math/mod.rs b/src/math/mod.rs new file mode 100644 index 00000000..fcb3819a --- /dev/null +++ b/src/math/mod.rs @@ -0,0 +1,314 @@ +//! Math types and utilities for parry, wrapping glam types. + +mod vector_ext; + +/// The scalar type used throughout this crate. +#[cfg(feature = "f64")] +pub use f64 as Real; +#[cfg(feature = "f64")] +pub use i64 as Int; + +/// The scalar type used throughout this crate. +#[cfg(feature = "f32")] +pub use f32 as Real; +#[cfg(feature = "f32")] +pub use i32 as Int; + +// Re-export ComplexField and RealField from simba for math operations +pub use simba::scalar::{ComplexField, RealField}; + +pub use vector_ext::*; + +// Re-export SIMD types from parent module +pub use crate::simd::*; + +// Re-export types from glamx based on precision +#[cfg(feature = "f64")] +pub use glamx::{ + DPose2 as Pose2, DPose3 as Pose3, DRot2 as Rot2, DRot3 as Rot3, + DSymmetricEigen2 as SymmetricEigen2, DSymmetricEigen3 as SymmetricEigen3, MatExt, +}; +#[cfg(feature = "f32")] +pub use glamx::{MatExt, Pose2, Pose3, Rot2, Rot3, SymmetricEigen2, SymmetricEigen3}; + +// Re-export glam types used directly +#[cfg(feature = "f64")] +pub use glamx::{ + DMat2 as Mat2, DMat3 as Mat3, DVec2 as Vec2, DVec3 as Vec3, DVec3 as Vec3A, DVec4 as Vec4, +}; +#[cfg(feature = "f32")] +pub use glamx::{Mat2, Mat3, Vec2, Vec3, Vec3A, Vec4}; + +/// The default tolerance used for geometric operations. +pub const DEFAULT_EPSILON: Real = Real::EPSILON; + +/// The dimension of the space. +#[cfg(feature = "dim2")] +pub const DIM: usize = 2; +/// The dimension of the space. +#[cfg(feature = "dim3")] +pub const DIM: usize = 3; + +/// The dimension of the space multiplied by two. +pub const TWO_DIM: usize = DIM * 2; + +// ==================== DIMENSION-DEPENDENT TYPE ALIASES ==================== + +// --- 3D Type Aliases --- + +#[cfg(feature = "dim3")] +mod dim3_types { + use super::*; + #[cfg(feature = "f64")] + use glamx::{DMat3, DVec3}; + + /// The vector type. + #[cfg(feature = "f32")] + pub type Vector = Vec3; + /// The vector type. + #[cfg(feature = "f64")] + pub type Vector = DVec3; + + /// The integer vector type. + #[cfg(feature = "f32")] + pub type IVector = glamx::IVec3; + /// The integer vector type. + #[cfg(feature = "f64")] + pub type IVector = glamx::I64Vec3; + + /// The angular vector type. + #[cfg(feature = "f32")] + pub type AngVector = Vec3; + /// The angular vector type. + #[cfg(feature = "f64")] + pub type AngVector = DVec3; + + /// The matrix type. + #[cfg(feature = "f32")] + pub type Matrix = Mat3; + /// The matrix type. + #[cfg(feature = "f64")] + pub type Matrix = DMat3; + + /// The transformation matrix type (pose = rotation + translation). + pub type Pose = Pose3; + + /// The rotation type. + pub type Rotation = Rot3; + + /// The orientation type. + #[cfg(feature = "f32")] + pub type Orientation = Vec3; + /// The orientation type. + #[cfg(feature = "f64")] + pub type Orientation = DVec3; + + /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. + #[cfg(feature = "f32")] + pub type SpatialVector = [f32; 6]; + /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. + #[cfg(feature = "f64")] + pub type SpatialVector = [f64; 6]; + + /// The angular inertia of a rigid body. + pub type AngularInertia = crate::utils::SdpMatrix3; + + /// The principal angular inertia of a rigid body. + #[cfg(feature = "f32")] + pub type PrincipalAngularInertia = Vec3; + /// The principal angular inertia of a rigid body. + #[cfg(feature = "f64")] + pub type PrincipalAngularInertia = DVec3; + + /// A matrix that represent the cross product with a given vector. + #[cfg(feature = "f32")] + pub type CrossMatrix = Mat3; + /// A matrix that represent the cross product with a given vector. + #[cfg(feature = "f64")] + pub type CrossMatrix = DMat3; + + /// A 3D symmetric-definite-positive matrix. + pub type SdpMatrix = crate::utils::SdpMatrix3; + + /// The result of eigendecomposition of a symmetric matrix. + #[cfg(feature = "f32")] + pub type SymmetricEigen = SymmetricEigen3; + /// The result of eigendecomposition of a symmetric matrix. + #[cfg(feature = "f64")] + pub type SymmetricEigen = glamx::DSymmetricEigen3; +} + +#[cfg(feature = "dim3")] +pub use dim3_types::*; + +// --- 2D Type Aliases --- + +#[cfg(feature = "dim2")] +mod dim2_types { + use super::*; + #[cfg(feature = "f64")] + use glamx::{DMat2, DVec2, DVec3}; + + /// The vector type. + #[cfg(feature = "f32")] + pub type Vector = Vec2; + /// The vector type. + #[cfg(feature = "f64")] + pub type Vector = DVec2; + + /// The integer vector type. + #[cfg(feature = "f32")] + pub type IVector = glamx::IVec2; + /// The integer vector type. + #[cfg(feature = "f64")] + pub type IVector = glamx::I64Vec2; + + /// The angular vector type (scalar for 2D). + pub type AngVector = Real; + + /// The matrix type. + #[cfg(feature = "f32")] + pub type Matrix = Mat2; + /// The matrix type. + #[cfg(feature = "f64")] + pub type Matrix = DMat2; + + /// The transformation matrix type (pose = rotation + translation). + pub type Pose = Pose2; + + /// The rotation type. + pub type Rotation = Rot2; + + /// The orientation type (scalar angle for 2D). + pub type Orientation = Real; + + /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. + #[cfg(feature = "f32")] + pub type SpatialVector = Vec3; + /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. + #[cfg(feature = "f64")] + pub type SpatialVector = DVec3; + + /// The angular inertia of a rigid body (scalar for 2D). + pub type AngularInertia = Real; + + /// The principal angular inertia of a rigid body (scalar for 2D). + pub type PrincipalAngularInertia = Real; + + /// A matrix that represent the cross product with a given vector. + #[cfg(feature = "f32")] + pub type CrossMatrix = Vec2; + /// A matrix that represent the cross product with a given vector. + #[cfg(feature = "f64")] + pub type CrossMatrix = DVec2; + + /// A 2D symmetric-definite-positive matrix. + pub type SdpMatrix = crate::utils::SdpMatrix2; + + /// The result of eigendecomposition of a symmetric matrix. + #[cfg(feature = "f32")] + pub type SymmetricEigen = SymmetricEigen2; + /// The result of eigendecomposition of a symmetric matrix. + #[cfg(feature = "f64")] + pub type SymmetricEigen = glamx::DSymmetricEigen2; +} + +#[cfg(feature = "dim2")] +pub use dim2_types::*; + +// ==================== ORTHONORMAL BASIS COMPUTATION ==================== + +/// Computes an orthonormal basis for the subspace orthogonal to the given vectors. +/// Calls the callback `f` with each basis vector. +/// +/// For 3D: given 1 vector, produces 2 orthonormal vectors perpendicular to it. +#[cfg(feature = "dim3")] +pub fn orthonormal_subspace_basis(vs: &[Vector], mut f: F) +where + F: FnMut(Vector) -> bool, +{ + if vs.is_empty() { + return; + } + + // Normalize the input vector + let v = vs[0].normalize_or_zero(); + + if v == Vector::ZERO { + return; + } + + // Find a vector that's not parallel to v + let orth = if v.x.abs() > v.z.abs() { + Vector::new(-v.y, v.x, 0.0) + } else { + Vector::new(0.0, -v.z, v.y) + }; + + // First orthonormal vector + let orth1 = orth.normalize(); + if !f(orth1) { + return; + } + + // Second orthonormal vector (cross product) + let orth2 = v.cross(orth1); + let _ = f(orth2); +} + +// ==================== 2D TYPES FOR 3D CONTEXTS ==================== +// These are needed for algorithms like spiral intersection that use 2D math in 3D contexts. + +/// A 2D vector type for use in any dimension context. +pub type Vector2 = Vec2; + +/// A 2x2 matrix type for use in any dimension context. +pub type Matrix2 = Mat2; + +/// A 3D vector type for use in any dimension context. +pub type Vector3 = Vec3; + +/// A 3x3 matrix type for use in any dimension context. +pub type Matrix3 = Mat3; + +/// Converts an integer vector to a floating-point vector. +#[cfg(all(feature = "dim2", feature = "f32"))] +pub fn ivect_to_vect(p: IVector) -> Vector { + p.as_vec2() +} +/// Converts an integer vector to a floating-point vector. +#[cfg(all(feature = "dim2", feature = "f64"))] +pub fn ivect_to_vect(p: IVector) -> Vector { + p.as_dvec2() +} +/// Converts an integer vector to a floating-point vector. +#[cfg(all(feature = "dim3", feature = "f32"))] +pub fn ivect_to_vect(p: IVector) -> Vector { + p.as_vec3() +} +/// Converts an integer vector to a floating-point vector. +#[cfg(all(feature = "dim3", feature = "f64"))] +pub fn ivect_to_vect(p: IVector) -> Vector { + p.as_dvec3() +} + +/// Converts a floating-point vector to an integer vector. +#[cfg(all(feature = "dim2", feature = "f32"))] +pub fn vect_to_ivect(p: Vector) -> IVector { + p.as_ivec2() +} +/// Converts a floating-point vector to an integer vector. +#[cfg(all(feature = "dim2", feature = "f64"))] +pub fn vect_to_ivect(p: Vector) -> IVector { + p.as_i64vec2() +} +/// Converts a floating-point vector to an integer vector. +#[cfg(all(feature = "dim3", feature = "f32"))] +pub fn vect_to_ivect(p: Vector) -> IVector { + p.as_ivec3() +} +/// Converts a floating-point vector to an integer vector. +#[cfg(all(feature = "dim3", feature = "f64"))] +pub fn vect_to_ivect(p: Vector) -> IVector { + p.as_i64vec3() +} diff --git a/src/math/vector_ext.rs b/src/math/vector_ext.rs new file mode 100644 index 00000000..2c6b827c --- /dev/null +++ b/src/math/vector_ext.rs @@ -0,0 +1,64 @@ +//! Vector extension trait for glam types. + +use crate::math::{Matrix, Real, Vector}; + +#[cfg(not(feature = "std"))] +use simba::scalar::ComplexField; + +/// Extension trait for glam vector types to provide additional functionality. +pub trait VectorExt: Sized + Copy { + /// Creates a vector with the i-th component set to `val` and all others to zero. + fn ith(i: usize, val: Real) -> Self; + + /// Computes the angle between two vectors in radians. + fn angle(self, other: Self) -> Real; + + /// Computes the kronecker product between two vectors. + fn kronecker(self, other: Self) -> Matrix; +} + +impl VectorExt for Vector { + #[inline] + #[cfg(feature = "dim2")] + fn ith(i: usize, val: Real) -> Self { + match i { + 0 => Self::new(val, 0.0), + 1 => Self::new(0.0, val), + _ => Self::ZERO, + } + } + #[inline] + #[cfg(feature = "dim3")] + fn ith(i: usize, val: Real) -> Self { + match i { + 0 => Self::new(val, 0.0, 0.0), + 1 => Self::new(0.0, val, 0.0), + 2 => Self::new(0.0, 0.0, val), + _ => Self::ZERO, + } + } + + #[inline] + fn angle(self, other: Self) -> Real { + let dot = self.dot(other); + let len_self = self.length(); + let len_other = other.length(); + if len_self > 0.0 && len_other > 0.0 { + (dot / (len_self * len_other)).clamp(-1.0, 1.0).acos() + } else { + 0.0 + } + } + + #[inline] + #[cfg(feature = "dim2")] + fn kronecker(self, other: Self) -> Matrix { + Matrix::from_cols(self * other.x, self * other.y) + } + + #[inline] + #[cfg(feature = "dim3")] + fn kronecker(self, other: Self) -> Matrix { + Matrix::from_cols(self * other.x, self * other.y, self * other.z) + } +} diff --git a/src/partitioning/bvh/bvh_binned_build.rs b/src/partitioning/bvh/bvh_binned_build.rs index 27c8b1b6..0f3888a4 100644 --- a/src/partitioning/bvh/bvh_binned_build.rs +++ b/src/partitioning/bvh/bvh_binned_build.rs @@ -47,7 +47,7 @@ impl Bvh { assert!(leaves.len() > 1); let centroid_aabb = Aabb::from_points(leaves.iter().map(|node| node.center())); - let bins_axis = centroid_aabb.extents().imax(); + let bins_axis = centroid_aabb.extents().max_position(); let bins_range = [centroid_aabb.mins[bins_axis], centroid_aabb.maxs[bins_axis]]; // Compute bins characteristics. diff --git a/src/partitioning/bvh/bvh_insert.rs b/src/partitioning/bvh/bvh_insert.rs index 984292c0..97188643 100644 --- a/src/partitioning/bvh/bvh_insert.rs +++ b/src/partitioning/bvh/bvh_insert.rs @@ -36,14 +36,14 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::Bvh; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// /// // Insert objects with custom IDs - /// bvh.insert(Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), 100); - /// bvh.insert(Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), 200); - /// bvh.insert(Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), 300); + /// bvh.insert(Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), 100); + /// bvh.insert(Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), 200); + /// bvh.insert(Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::new(11.0, 1.0, 1.0)), 300); /// /// assert_eq!(bvh.leaf_count(), 3); /// # } @@ -55,15 +55,15 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::Bvh; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// /// // Insert an object - /// bvh.insert(Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), 42); + /// bvh.insert(Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), 42); /// /// // Simulate the object moving - just insert with the same ID - /// bvh.insert(Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), 42); + /// bvh.insert(Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), 42); /// /// // The BVH still has only 1 leaf, but at the new position /// assert_eq!(bvh.leaf_count(), 1); @@ -76,7 +76,7 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -84,8 +84,8 @@ impl Bvh { /// // Add initial objects /// for i in 0..100 { /// let aabb = Aabb::new( - /// Point3::new(i as f32, 0.0, 0.0), - /// Point3::new(i as f32 + 1.0, 1.0, 1.0) + /// Vector::new(i as f32, 0.0, 0.0), + /// Vector::new(i as f32 + 1.0, 1.0, 1.0) /// ); /// bvh.insert(aabb, i); /// } @@ -94,8 +94,8 @@ impl Bvh { /// // then refit once at the end /// for i in 0..100 { /// let aabb = Aabb::new( - /// Point3::new(i as f32 + 0.1, 0.0, 0.0), - /// Point3::new(i as f32 + 1.1, 1.0, 1.0) + /// Vector::new(i as f32 + 0.1, 0.0, 0.0), + /// Vector::new(i as f32 + 1.1, 1.0, 1.0) /// ); /// bvh.insert_or_update_partially(aabb, i, 0.0); /// } @@ -143,8 +143,8 @@ impl Bvh { if change_detection_margin > 0.0 { if !node.contains_aabb(&aabb) { - node.mins = aabb.mins - Vector::repeat(change_detection_margin); - node.maxs = aabb.maxs + Vector::repeat(change_detection_margin); + node.mins = aabb.mins - Vector::splat(change_detection_margin); + node.maxs = aabb.maxs + Vector::splat(change_detection_margin); node.data.set_change_pending(); } else { // No change detected, no propagation needed. @@ -181,8 +181,8 @@ impl Bvh { break; } - node.mins = node.mins.inf(&aabb.mins); - node.maxs = node.maxs.sup(&aabb.maxs); + node.mins = node.mins.min(aabb.mins); + node.maxs = node.maxs.max(aabb.maxs); let wide_node_id = parent.decompose().0; if wide_node_id == 0 { @@ -217,8 +217,8 @@ impl Bvh { if change_detection_margin > 0.0 { if !node.contains_aabb(&aabb) { - node.mins = aabb.mins - Vector::repeat(change_detection_margin); - node.maxs = aabb.maxs + Vector::repeat(change_detection_margin); + node.mins = aabb.mins - Vector::splat(change_detection_margin); + node.maxs = aabb.maxs + Vector::splat(change_detection_margin); node.data.set_change_pending(); } } else { @@ -318,15 +318,15 @@ impl Bvh { left.children = new_leaf_id as u32; left.data.add_leaf_count(1); - left.mins = left.mins.inf(&aabb.mins); - left.maxs = left.maxs.sup(&aabb.maxs); + left.mins = left.mins.min(aabb.mins); + left.maxs = left.maxs.max(aabb.maxs); break; } else { let left = &mut self.nodes[curr_id as usize].left; curr_id = left.children; left.data.add_leaf_count(1); - left.mins = left.mins.inf(&aabb.mins); - left.maxs = left.maxs.sup(&aabb.maxs); + left.mins = left.mins.min(aabb.mins); + left.maxs = left.maxs.max(aabb.maxs); } } else { // Insert right. The `right` node will become an internal node. @@ -349,15 +349,15 @@ impl Bvh { right.children = new_leaf_id as u32; right.data.add_leaf_count(1); - right.mins = right.mins.inf(&aabb.mins); - right.maxs = right.maxs.sup(&aabb.maxs); + right.mins = right.mins.min(aabb.mins); + right.maxs = right.maxs.max(aabb.maxs); break; } else { let right = &mut self.nodes[curr_id as usize].right; curr_id = right.children; right.data.add_leaf_count(1); - right.mins = right.mins.inf(&aabb.mins); - right.maxs = right.maxs.sup(&aabb.maxs); + right.mins = right.mins.min(aabb.mins); + right.maxs = right.maxs.max(aabb.maxs); } } } diff --git a/src/partitioning/bvh/bvh_optimize.rs b/src/partitioning/bvh/bvh_optimize.rs index b55b2d20..974753ab 100644 --- a/src/partitioning/bvh/bvh_optimize.rs +++ b/src/partitioning/bvh/bvh_optimize.rs @@ -4,7 +4,7 @@ use core::cmp::Ordering; use ordered_float::OrderedFloat; #[cfg(not(feature = "std"))] -use na::ComplexField; // For `round` and `sqrt` in no-std+alloc builds. +use crate::math::ComplexField; // For `round` and `sqrt` in no-std+alloc builds. impl Bvh { fn optimization_config(&self, frame_index: u32) -> OptimizationConfig { @@ -92,7 +92,7 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -100,8 +100,8 @@ impl Bvh { /// // Add initial objects /// for i in 0..1000 { /// let aabb = Aabb::new( - /// Point3::new(i as f32, 0.0, 0.0), - /// Point3::new(i as f32 + 1.0, 1.0, 1.0) + /// Vector::new(i as f32, 0.0, 0.0), + /// Vector::new(i as f32 + 1.0, 1.0, 1.0) /// ); /// bvh.insert(aabb, i); /// } @@ -113,8 +113,8 @@ impl Bvh { /// let time = frame as f32 * 0.016; /// let offset = (time * (i as f32 + 1.0)).sin() * 5.0; /// let aabb = Aabb::new( - /// Point3::new(i as f32 + offset, 0.0, 0.0), - /// Point3::new(i as f32 + offset + 1.0, 1.0, 1.0) + /// Vector::new(i as f32 + offset, 0.0, 0.0), + /// Vector::new(i as f32 + offset + 1.0, 1.0, 1.0) /// ); /// bvh.insert_or_update_partially(aabb, i, 0.0); /// } @@ -138,7 +138,7 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -164,7 +164,7 @@ impl Bvh { /// } /// /// # fn get_body_aabb(id: u32) -> Aabb { - /// # Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)) + /// # Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)) /// # } /// # } /// ``` @@ -175,7 +175,7 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -183,8 +183,8 @@ impl Bvh { /// // Dynamically add many objects over time /// for i in 0..1000 { /// let aabb = Aabb::new( - /// Point3::new(i as f32, 0.0, 0.0), - /// Point3::new(i as f32 + 1.0, 1.0, 1.0) + /// Vector::new(i as f32, 0.0, 0.0), + /// Vector::new(i as f32 + 1.0, 1.0, 1.0) /// ); /// bvh.insert(aabb, i); /// diff --git a/src/partitioning/bvh/bvh_ploc_build.rs b/src/partitioning/bvh/bvh_ploc_build.rs index 61baf016..228cfe67 100644 --- a/src/partitioning/bvh/bvh_ploc_build.rs +++ b/src/partitioning/bvh/bvh_ploc_build.rs @@ -14,8 +14,8 @@ impl Bvh { // Sort the leaves. leaves.sort_by_cached_key(|node| { - let center = (node.center() - aabb.mins).component_mul(&inv_extents); - morton::morton_encode_u64_unorm(center.cast::()) + let center = (node.center() - aabb.mins) * (inv_extents); + morton::morton_encode_u64_unorm(center) }); // Build all the levels. diff --git a/src/partitioning/bvh/bvh_queries.rs b/src/partitioning/bvh/bvh_queries.rs index 3eded82d..5b915a92 100644 --- a/src/partitioning/bvh/bvh_queries.rs +++ b/src/partitioning/bvh/bvh_queries.rs @@ -1,7 +1,7 @@ use super::{Bvh, BvhNode}; use crate::bounding_volume::{Aabb, BoundingVolume}; -use crate::math::Point; use crate::math::Real; +use crate::math::Vector; use crate::query::PointProjection; use crate::query::{PointQuery, Ray}; use crate::shape::FeatureId; @@ -11,8 +11,8 @@ pub(super) struct SimdInvRay { // TODO: we need to use `glam` here instead of `wide` because `wide` is lacking // operations for getting the min/max vector element. // We can switch back to `wide` once it's supported. - pub origin: glam::Vec3A, - pub inv_dir: glam::Vec3A, + pub origin: glamx::Vec3A, + pub inv_dir: glamx::Vec3A, } #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] @@ -26,8 +26,8 @@ impl From for SimdInvRay { } }); Self { - origin: glam::Vec3A::from([ray.origin.x, ray.origin.y, ray.origin.z]), - inv_dir: glam::Vec3A::from([inv_dir.x, inv_dir.y, inv_dir.z]), + origin: glamx::Vec3A::from([ray.origin.x, ray.origin.y, ray.origin.z]), + inv_dir: glamx::Vec3A::from([inv_dir.x, inv_dir.y, inv_dir.z]), } } } @@ -68,21 +68,21 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a scene with objects /// let objects = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::new(11.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects); /// /// // Query: which objects are near the origin? /// let query_region = Aabb::new( - /// Point3::new(-1.0, -1.0, -1.0), - /// Point3::new(2.0, 2.0, 2.0) + /// Vector::new(-1.0, -1.0, -1.0), + /// Vector::new(2.0, 2.0, 2.0) /// ); /// /// for leaf_id in bvh.intersect_aabb(&query_region) { @@ -98,12 +98,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let objects = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(0.5, 0.0, 0.0), Point3::new(1.5, 1.0, 1.0)), // Overlaps first - /// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), // Far away + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(0.5, 0.0, 0.0), Vector::new(1.5, 1.0, 1.0)), // Overlaps first + /// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::new(11.0, 1.0, 1.0)), // Far away /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects); @@ -130,20 +130,20 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let scene_objects = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(100.0, 0.0, 0.0), Point3::new(101.0, 1.0, 1.0)), // Far away + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(100.0, 0.0, 0.0), Vector::new(101.0, 1.0, 1.0)), // Far away /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &scene_objects); /// /// // Camera frustum as an AABB (simplified - real frustums are more complex) /// let camera_frustum = Aabb::new( - /// Point3::new(-10.0, -10.0, -10.0), - /// Point3::new(10.0, 10.0, 10.0) + /// Vector::new(-10.0, -10.0, -10.0), + /// Vector::new(10.0, 10.0, 10.0) /// ); /// /// let mut visible_objects = Vec::new(); @@ -162,12 +162,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::{Aabb, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let objects = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(1.5, 0.0, 0.0), Point3::new(2.5, 1.0, 1.0)), - /// Aabb::new(Point3::new(100.0, 0.0, 0.0), Point3::new(101.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(1.5, 0.0, 0.0), Vector::new(2.5, 1.0, 1.0)), + /// Aabb::new(Vector::new(100.0, 0.0, 0.0), Vector::new(101.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects); @@ -212,7 +212,7 @@ impl Bvh { /// `max_distance` if no projection was found so far). pub fn project_point( &self, - point: &Point, + point: Vector, max_distance: Real, primitive_check: impl Fn(u32, Real) -> Option, ) -> Option<(u32, (Real, PointProjection))> { @@ -221,7 +221,7 @@ impl Bvh { |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true), |primitive, _| { let proj = primitive_check(primitive, max_distance)?; - Some((na::distance(&proj.point, point), proj)) + Some((proj.point.distance(point), proj)) }, ) } @@ -236,7 +236,7 @@ impl Bvh { /// `max_distance` if no projection was found so far). pub fn project_point_and_get_feature( &self, - point: &Point, + point: Vector, max_distance: Real, primitive_check: impl Fn(u32, Real) -> Option<(PointProjection, FeatureId)>, ) -> Option<(u32, (Real, (PointProjection, FeatureId)))> { @@ -245,7 +245,7 @@ impl Bvh { |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true), |primitive, _| { let proj = primitive_check(primitive, max_distance)?; - Some((na::distance(&proj.0.point, point), proj)) + Some((proj.0.point.distance(point), proj)) }, ) } diff --git a/src/partitioning/bvh/bvh_refit.rs b/src/partitioning/bvh/bvh_refit.rs index e6c38a97..f1b302d5 100644 --- a/src/partitioning/bvh/bvh_refit.rs +++ b/src/partitioning/bvh/bvh_refit.rs @@ -46,7 +46,7 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -54,8 +54,8 @@ impl Bvh { /// // Insert initial objects /// for i in 0..100 { /// let aabb = Aabb::new( - /// Point3::new(i as f32, 0.0, 0.0), - /// Point3::new(i as f32 + 1.0, 1.0, 1.0) + /// Vector::new(i as f32, 0.0, 0.0), + /// Vector::new(i as f32 + 1.0, 1.0, 1.0) /// ); /// bvh.insert(aabb, i); /// } @@ -64,8 +64,8 @@ impl Bvh { /// for i in 0..100 { /// let offset = 0.1; /// let aabb = Aabb::new( - /// Point3::new(i as f32 + offset, 0.0, 0.0), - /// Point3::new(i as f32 + 1.0 + offset, 1.0, 1.0) + /// Vector::new(i as f32 + offset, 0.0, 0.0), + /// Vector::new(i as f32 + 1.0 + offset, 1.0, 1.0) /// ); /// bvh.insert_or_update_partially(aabb, i, 0.0); /// } @@ -81,7 +81,7 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -89,8 +89,8 @@ impl Bvh { /// // Game initialization - add objects /// for i in 0..1000 { /// let aabb = Aabb::new( - /// Point3::new(i as f32, 0.0, 0.0), - /// Point3::new(i as f32 + 1.0, 1.0, 1.0) + /// Vector::new(i as f32, 0.0, 0.0), + /// Vector::new(i as f32 + 1.0, 1.0, 1.0) /// ); /// bvh.insert(aabb, i); /// } @@ -102,8 +102,8 @@ impl Bvh { /// let time = frame as f32 * 0.016; // ~60 FPS /// let pos = time.sin() * 10.0; /// let aabb = Aabb::new( - /// Point3::new(i as f32 + pos, 0.0, 0.0), - /// Point3::new(i as f32 + pos + 1.0, 1.0, 1.0) + /// Vector::new(i as f32 + pos, 0.0, 0.0), + /// Vector::new(i as f32 + pos + 1.0, 1.0, 1.0) /// ); /// bvh.insert_or_update_partially(aabb, i, 0.0); /// } @@ -122,18 +122,18 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); /// /// // Add an object - /// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// bvh.insert(aabb, 0); /// /// // Update with a margin - tree won't update if movement is small /// let margin = 0.5; - /// let new_aabb = Aabb::new(Point3::new(0.1, 0.0, 0.0), Point3::new(1.1, 1.0, 1.0)); + /// let new_aabb = Aabb::new(Vector::new(0.1, 0.0, 0.0), Vector::new(1.1, 1.0, 1.0)); /// bvh.insert_or_update_partially(new_aabb, 0, margin); /// /// // Refit propagates the change detection flags diff --git a/src/partitioning/bvh/bvh_tests.rs b/src/partitioning/bvh/bvh_tests.rs index fd27f02a..4b2898b2 100644 --- a/src/partitioning/bvh/bvh_tests.rs +++ b/src/partitioning/bvh/bvh_tests.rs @@ -3,7 +3,7 @@ use crate::math::{Real, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy}; fn make_test_aabb(i: usize) -> Aabb { - Aabb::from_half_extents(Vector::repeat(i as Real).into(), Vector::repeat(1.0)) + Aabb::from_half_extents(Vector::splat(i as Real).into(), Vector::splat(1.0)) } #[test] diff --git a/src/partitioning/bvh/bvh_traverse.rs b/src/partitioning/bvh/bvh_traverse.rs index b2ae2891..9252bbf7 100644 --- a/src/partitioning/bvh/bvh_traverse.rs +++ b/src/partitioning/bvh/bvh_traverse.rs @@ -173,19 +173,19 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy, TraversalAction}; /// use parry3d::bounding_volume::{Aabb, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::new(11.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); /// /// let query_region = Aabb::new( - /// Point3::new(-1.0, -1.0, -1.0), - /// Point3::new(7.0, 2.0, 2.0) + /// Vector::new(-1.0, -1.0, -1.0), + /// Vector::new(7.0, 2.0, 2.0) /// ); /// /// let mut count = 0; @@ -212,21 +212,21 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy, TraversalAction}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::new(11.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); /// - /// let target_point = Point3::new(5.5, 0.5, 0.5); + /// let target_point = Vector::new(5.5, 0.5, 0.5); /// let mut found_leaf = None; /// /// bvh.traverse(|node| { - /// if !node.aabb().contains_local_point(&target_point) { + /// if !node.aabb().contains_local_point(target_point) { /// return TraversalAction::Prune; /// } /// @@ -248,12 +248,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy, TraversalAction}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(4.0, 0.0, 0.0), Point3::new(5.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(4.0, 0.0, 0.0), Vector::new(5.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index edc5c6c4..35baba88 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -1,6 +1,6 @@ use super::BvhOptimizationHeapEntry; use crate::bounding_volume::{Aabb, BoundingVolume}; -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::query::{Ray, RayCast}; use crate::utils::VecMap; use alloc::collections::{BinaryHeap, VecDeque}; @@ -31,13 +31,13 @@ use core::ops::{Deref, DerefMut, Index, IndexMut}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create some AABBs for objects in the scene /// let aabbs = vec![ -/// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(0.0, 2.0, 0.0), Point3::new(1.0, 3.0, 1.0)), +/// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(0.0, 2.0, 0.0), Vector::new(1.0, 3.0, 1.0)), /// ]; /// /// // Use binned strategy for general purpose (default) @@ -108,11 +108,11 @@ pub enum BvhBuildStrategy { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let aabbs = vec![ -/// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), +/// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), /// ]; /// /// let mut bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); @@ -149,8 +149,7 @@ pub struct BvhWorkspace { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(transparent)] pub struct BvhNodeData(u32); @@ -229,12 +228,12 @@ impl BvhNodeData { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let aabbs = vec![ -/// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(4.0, 0.0, 0.0), Point3::new(5.0, 1.0, 1.0)), +/// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(4.0, 0.0, 0.0), Vector::new(5.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); @@ -253,8 +252,7 @@ impl BvhNodeData { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] // PERF: the size of this struct is 64 bytes but has a default alignment of 16 (in f32 + 3d + simd mode). @@ -306,11 +304,11 @@ impl BvhNodeWide { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::{Aabb, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); @@ -338,15 +336,15 @@ impl BvhNodeWide { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNodeWide; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// /// let mut node_wide = BvhNodeWide::zeros(); /// let nodes = node_wide.as_array_mut(); /// /// // Scale both nodes by 2.0 - /// let scale = Vector3::new(2.0, 2.0, 2.0); - /// nodes[0].scale(&scale); - /// nodes[1].scale(&scale); + /// let scale = Vector::splat(2.0); + /// nodes[0].scale(scale); + /// nodes[1].scale(scale); /// # } /// ``` /// @@ -391,8 +389,8 @@ impl BvhNodeWide { #[repr(C)] // SAFETY: needed to ensure SIMD aabb checks rely on the layout. #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] pub(super) struct BvhNodeSimd { - mins: glam::Vec3A, - maxs: glam::Vec3A, + mins: glamx::Vec3A, + maxs: glamx::Vec3A, } // SAFETY: compile-time assertions to ensure we can transmute between `BvhNode` and `BvhNodeSimd`. @@ -428,10 +426,10 @@ static_assertions::assert_eq_size!(BvhNode, BvhNodeSimd); /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create a leaf node -/// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); +/// let aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// let leaf = BvhNode::leaf(aabb, 42); /// /// assert!(leaf.is_leaf()); @@ -450,18 +448,17 @@ static_assertions::assert_eq_size!(BvhNode, BvhNodeSimd); #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct BvhNode { /// Mins coordinates of the node’s bounding volume. - pub(super) mins: Point, + pub(super) mins: Vector, /// Children of this node. A node has either 0 (i.e. it’s a leaf) or 2 children. /// /// If [`Self::leaf_count`] is 1, then the node has 0 children and is a leaf. pub(super) children: u32, /// Maxs coordinates of this node’s bounding volume. - pub(super) maxs: Point, + pub(super) maxs: Vector, /// Packed data associated to this node (leaf count and flags). pub(super) data: BvhNodeData, } @@ -470,9 +467,9 @@ impl BvhNode { #[inline(always)] pub(super) fn zeros() -> Self { Self { - mins: Point::origin(), + mins: Vector::ZERO, children: 0, - maxs: Point::origin(), + maxs: Vector::ZERO, data: BvhNodeData(0), } } @@ -498,10 +495,10 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create an AABB for a unit cube - /// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// /// // Create a leaf node with index 0 /// let leaf = BvhNode::leaf(aabb, 0); @@ -543,9 +540,9 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// let leaf = BvhNode::leaf(aabb, 42); /// /// assert_eq!(leaf.leaf_data(), Some(42)); @@ -576,9 +573,9 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// let leaf = BvhNode::leaf(aabb, 0); /// /// assert!(leaf.is_leaf()); @@ -610,9 +607,9 @@ impl BvhNode { pub(super) fn merged(&self, other: &Self, children: u32) -> Self { // TODO PERF: simd optimizations? Self { - mins: self.mins.inf(&other.mins), + mins: self.mins.min(other.mins), children, - maxs: self.maxs.sup(&other.maxs), + maxs: self.maxs.max(other.maxs), data: self.data.merged(other.data), } } @@ -632,12 +629,12 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::new(1.0, 2.0, 3.0), Point3::new(4.0, 5.0, 6.0)); + /// let aabb = Aabb::new(Vector::new(1.0, 2.0, 3.0), Vector::new(4.0, 5.0, 6.0)); /// let node = BvhNode::leaf(aabb, 0); /// - /// assert_eq!(node.mins(), Point3::new(1.0, 2.0, 3.0)); + /// assert_eq!(node.mins(), Vector::new(1.0, 2.0, 3.0)); /// # } /// ``` /// @@ -646,7 +643,7 @@ impl BvhNode { /// - [`maxs`](Self::maxs) - Get the maximum corner /// - [`aabb`](Self::aabb) - Get the full AABB #[inline] - pub fn mins(&self) -> Point { + pub fn mins(&self) -> Vector { self.mins } @@ -665,12 +662,12 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::new(1.0, 2.0, 3.0), Point3::new(4.0, 5.0, 6.0)); + /// let aabb = Aabb::new(Vector::new(1.0, 2.0, 3.0), Vector::new(4.0, 5.0, 6.0)); /// let node = BvhNode::leaf(aabb, 0); /// - /// assert_eq!(node.maxs(), Point3::new(4.0, 5.0, 6.0)); + /// assert_eq!(node.maxs(), Vector::new(4.0, 5.0, 6.0)); /// # } /// ``` /// @@ -679,7 +676,7 @@ impl BvhNode { /// - [`mins`](Self::mins) - Get the minimum corner /// - [`aabb`](Self::aabb) - Get the full AABB #[inline] - pub fn maxs(&self) -> Point { + pub fn maxs(&self) -> Vector { self.maxs } @@ -698,9 +695,9 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let original_aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); + /// let original_aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// let node = BvhNode::leaf(original_aabb, 0); /// /// assert_eq!(node.aabb(), original_aabb); @@ -734,17 +731,17 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::origin(), Point3::new(2.0, 4.0, 6.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(2.0, 4.0, 6.0)); /// let node = BvhNode::leaf(aabb, 0); /// - /// assert_eq!(node.center(), Point3::new(1.0, 2.0, 3.0)); + /// assert_eq!(node.center(), Vector::new(1.0, 2.0, 3.0)); /// # } /// ``` #[inline] - pub fn center(&self) -> Point { - na::center(&self.mins, &self.maxs) + pub fn center(&self) -> Vector { + self.mins.midpoint(self.maxs) } /// Returns `true` if this node has been marked as changed. @@ -763,9 +760,9 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); /// let node = BvhNode::leaf(aabb, 0); /// /// // New leaf nodes are marked as changed (pending change) @@ -797,15 +794,15 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::new(1.0, 1.0, 1.0), Point3::new(2.0, 2.0, 2.0)); + /// let aabb = Aabb::new(Vector::new(1.0, 1.0, 1.0), Vector::new(2.0, 2.0, 2.0)); /// let mut node = BvhNode::leaf(aabb, 0); /// - /// node.scale(&Vector3::new(2.0, 2.0, 2.0)); + /// node.scale(Vector::new(2.0, 2.0, 2.0)); /// - /// assert_eq!(node.mins(), Point3::new(2.0, 2.0, 2.0)); - /// assert_eq!(node.maxs(), Point3::new(4.0, 4.0, 4.0)); + /// assert_eq!(node.mins(), Vector::new(2.0, 2.0, 2.0)); + /// assert_eq!(node.maxs(), Vector::new(4.0, 4.0, 4.0)); /// # } /// ``` /// @@ -813,9 +810,9 @@ impl BvhNode { /// /// - [`Bvh::scale`] - Scale an entire BVH tree #[inline] - pub fn scale(&mut self, scale: &Vector) { - self.mins.coords.component_mul_assign(scale); - self.maxs.coords.component_mul_assign(scale); + pub fn scale(&mut self, scale: Vector) { + self.mins *= scale; + self.maxs *= scale; } /// Calculates the volume of this node's AABB. @@ -834,10 +831,10 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a 2×3×4 box - /// let aabb = Aabb::new(Point3::origin(), Point3::new(2.0, 3.0, 4.0)); + /// let aabb = Aabb::new(Vector::ZERO, Vector::new(2.0, 3.0, 4.0)); /// let node = BvhNode::leaf(aabb, 0); /// /// assert_eq!(node.volume(), 24.0); // 2 * 3 * 4 = 24 @@ -881,10 +878,10 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb1 = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)); - /// let aabb2 = Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)); + /// let aabb1 = Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)); + /// let aabb2 = Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)); /// /// let node1 = BvhNode::leaf(aabb1, 0); /// let node2 = BvhNode::leaf(aabb2, 1); @@ -899,8 +896,8 @@ impl BvhNode { /// - [`volume`](Self::volume) - Volume of a single node pub fn merged_volume(&self, other: &Self) -> Real { // TODO PERF: simd optimizations? - let mins = self.mins.inf(&other.mins); - let maxs = self.maxs.sup(&other.maxs); + let mins = self.mins.min(other.mins); + let maxs = self.maxs.max(other.maxs); let extents = maxs - mins; #[cfg(feature = "dim2")] @@ -933,11 +930,11 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb1 = Aabb::new(Point3::origin(), Point3::new(2.0, 2.0, 2.0)); - /// let aabb2 = Aabb::new(Point3::new(1.0, 1.0, 1.0), Point3::new(3.0, 3.0, 3.0)); - /// let aabb3 = Aabb::new(Point3::new(5.0, 5.0, 5.0), Point3::new(6.0, 6.0, 6.0)); + /// let aabb1 = Aabb::new(Vector::ZERO, Vector::new(2.0, 2.0, 2.0)); + /// let aabb2 = Aabb::new(Vector::new(1.0, 1.0, 1.0), Vector::new(3.0, 3.0, 3.0)); + /// let aabb3 = Aabb::new(Vector::new(5.0, 5.0, 5.0), Vector::new(6.0, 6.0, 6.0)); /// /// let node1 = BvhNode::leaf(aabb1, 0); /// let node2 = BvhNode::leaf(aabb2, 1); @@ -953,7 +950,7 @@ impl BvhNode { /// - [`contains`](Self::contains) - Check full containment #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] pub fn intersects(&self, other: &Self) -> bool { - na::partial_le(&self.mins, &other.maxs) && na::partial_ge(&self.maxs, &other.mins) + self.mins.cmple(other.maxs).all() && self.maxs.cmpge(other.mins).all() } /// Tests if this node's AABB intersects another node's AABB. @@ -979,11 +976,11 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::bvh::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let aabb1 = Aabb::new(Point3::origin(), Point3::new(2.0, 2.0, 2.0)); - /// let aabb2 = Aabb::new(Point3::new(1.0, 1.0, 1.0), Point3::new(3.0, 3.0, 3.0)); - /// let aabb3 = Aabb::new(Point3::new(5.0, 5.0, 5.0), Point3::new(6.0, 6.0, 6.0)); + /// let aabb1 = Aabb::new(Vector::ZERO, Vector::new(2.0, 2.0, 2.0)); + /// let aabb2 = Aabb::new(Vector::new(1.0, 1.0, 1.0), Vector::new(3.0, 3.0, 3.0)); + /// let aabb3 = Aabb::new(Vector::new(5.0, 5.0, 5.0), Vector::new(6.0, 6.0, 6.0)); /// /// let node1 = BvhNode::leaf(aabb1, 0); /// let node2 = BvhNode::leaf(aabb2, 1); @@ -1028,10 +1025,10 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let large = Aabb::new(Point3::origin(), Point3::new(10.0, 10.0, 10.0)); - /// let small = Aabb::new(Point3::new(2.0, 2.0, 2.0), Point3::new(5.0, 5.0, 5.0)); + /// let large = Aabb::new(Vector::ZERO, Vector::new(10.0, 10.0, 10.0)); + /// let small = Aabb::new(Vector::new(2.0, 2.0, 2.0), Vector::new(5.0, 5.0, 5.0)); /// /// let node_large = BvhNode::leaf(large, 0); /// let node_small = BvhNode::leaf(small, 1); @@ -1047,7 +1044,7 @@ impl BvhNode { /// - [`contains_aabb`](Self::contains_aabb) - Contains an `Aabb` directly #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] pub fn contains(&self, other: &Self) -> bool { - na::partial_le(&self.mins, &other.mins) && na::partial_ge(&self.maxs, &other.maxs) + self.mins.cmple(other.mins).all() && self.maxs.cmpge(other.maxs).all() } /// Tests if this node's AABB fully contains another node's AABB. @@ -1073,10 +1070,10 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::bvh::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let large = Aabb::new(Point3::origin(), Point3::new(10.0, 10.0, 10.0)); - /// let small = Aabb::new(Point3::new(2.0, 2.0, 2.0), Point3::new(5.0, 5.0, 5.0)); + /// let large = Aabb::new(Vector::ZERO, Vector::new(10.0, 10.0, 10.0)); + /// let small = Aabb::new(Vector::new(2.0, 2.0, 2.0), Vector::new(5.0, 5.0, 5.0)); /// /// let node_large = BvhNode::leaf(large, 0); /// let node_small = BvhNode::leaf(small, 1); @@ -1116,10 +1113,10 @@ impl BvhNode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let large = Aabb::new(Point3::origin(), Point3::new(10.0, 10.0, 10.0)); - /// let small = Aabb::new(Point3::new(2.0, 2.0, 2.0), Point3::new(5.0, 5.0, 5.0)); + /// let large = Aabb::new(Vector::ZERO, Vector::new(10.0, 10.0, 10.0)); + /// let small = Aabb::new(Vector::new(2.0, 2.0, 2.0), Vector::new(5.0, 5.0, 5.0)); /// /// let node = BvhNode::leaf(large, 0); /// @@ -1132,7 +1129,7 @@ impl BvhNode { /// - [`contains`](Self::contains) - Contains another `BvhNode` pub fn contains_aabb(&self, other: &Aabb) -> bool { // TODO PERF: simd optimizations? - na::partial_le(&self.mins, &other.mins) && na::partial_ge(&self.maxs, &other.maxs) + self.mins.cmple(other.mins).all() && self.maxs.cmpge(other.maxs).all() } /// Casts a ray against this node's AABB. @@ -1157,13 +1154,13 @@ impl BvhNode { /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; /// use parry3d::query::Ray; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// - /// let aabb = Aabb::new(Point3::new(5.0, -1.0, -1.0), Point3::new(6.0, 1.0, 1.0)); + /// let aabb = Aabb::new(Vector::new(5.0, -1.0, -1.0), Vector::new(6.0, 1.0, 1.0)); /// let node = BvhNode::leaf(aabb, 0); /// /// // Ray from origin along X axis - /// let ray = Ray::new(Point3::origin(), Vector3::new(1.0, 0.0, 0.0)); + /// let ray = Ray::new(Vector::ZERO, Vector::new(1.0, 0.0, 0.0)); /// /// let toi = node.cast_ray(&ray, f32::MAX); /// assert_eq!(toi, 5.0); // Ray hits at x=5.0 @@ -1244,8 +1241,7 @@ impl BvhNode { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct BvhNodeIndex(pub usize); @@ -1418,8 +1414,7 @@ impl BvhNodeIndex { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub(crate) struct BvhNodeVec(pub(crate) Vec); @@ -1510,13 +1505,13 @@ impl IndexMut for BvhNodeVec { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create AABBs for your objects /// let objects = vec![ -/// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), +/// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::new(11.0, 1.0, 1.0)), /// ]; /// /// // Build the BVH - the index of each AABB becomes its leaf ID @@ -1524,8 +1519,8 @@ impl IndexMut for BvhNodeVec { /// /// // Query which objects intersect a region /// let query_region = Aabb::new( -/// Point3::new(-1.0, -1.0, -1.0), -/// Point3::new(2.0, 2.0, 2.0) +/// Vector::new(-1.0, -1.0, -1.0), +/// Vector::new(2.0, 2.0, 2.0) /// ); /// /// for leaf_id in bvh.intersect_aabb(&query_region) { @@ -1543,17 +1538,17 @@ impl IndexMut for BvhNodeVec { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); /// /// // Add objects dynamically with custom IDs -/// bvh.insert(Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), 100); -/// bvh.insert(Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), 200); +/// bvh.insert(Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), 100); +/// bvh.insert(Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), 200); /// /// // Update an object's position (by re-inserting with same ID) -/// bvh.insert(Aabb::new(Point3::new(0.5, 0.5, 0.0), Point3::new(1.5, 1.5, 1.0)), 100); +/// bvh.insert(Aabb::new(Vector::new(0.5, 0.5, 0.0), Vector::new(1.5, 1.5, 1.0)), 100); /// /// // Refit the tree after updates for optimal query performance /// bvh.refit(&mut workspace); @@ -1572,17 +1567,17 @@ impl IndexMut for BvhNodeVec { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; /// use parry3d::query::{Ray, RayCast}; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::Vector; /// /// let objects = vec![ -/// Aabb::new(Point3::new(0.0, 0.0, 5.0), Point3::new(1.0, 1.0, 6.0)), -/// Aabb::new(Point3::new(0.0, 0.0, 10.0), Point3::new(1.0, 1.0, 11.0)), +/// Aabb::new(Vector::new(0.0, 0.0, 5.0), Vector::new(1.0, 1.0, 6.0)), +/// Aabb::new(Vector::new(0.0, 0.0, 10.0), Vector::new(1.0, 1.0, 11.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects); /// /// // Cast a ray forward along the Z axis -/// let ray = Ray::new(Point3::new(0.5, 0.5, 0.0), Vector3::new(0.0, 0.0, 1.0)); +/// let ray = Ray::new(Vector::new(0.5, 0.5, 0.0), Vector::new(0.0, 0.0, 1.0)); /// let max_distance = 100.0; /// /// // The BVH finds potentially intersecting leaves, then you test actual geometry @@ -1607,11 +1602,11 @@ impl IndexMut for BvhNodeVec { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let aabbs = vec![ -/// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), -/// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), +/// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), +/// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), /// ]; /// /// // Binned strategy: Fast construction, good quality (recommended default) @@ -1635,21 +1630,21 @@ impl IndexMut for BvhNodeVec { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); /// /// // Insert initial objects -/// bvh.insert(Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), 0); -/// bvh.insert(Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), 1); +/// bvh.insert(Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), 0); +/// bvh.insert(Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), 1); /// /// // Simulate object movement every frame /// for frame in 0..100 { /// let offset = frame as f32 * 0.1; /// bvh.insert(Aabb::new( -/// Point3::new(offset, 0.0, 0.0), -/// Point3::new(1.0 + offset, 1.0, 1.0) +/// Vector::new(offset, 0.0, 0.0), +/// Vector::new(1.0 + offset, 1.0, 1.0) /// ), 0); /// /// // Refit updates internal AABBs - very fast operation @@ -1668,7 +1663,7 @@ impl IndexMut for BvhNodeVec { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhWorkspace}; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let mut bvh = Bvh::new(); /// let mut workspace = BvhWorkspace::default(); @@ -1676,8 +1671,8 @@ impl IndexMut for BvhNodeVec { /// // Build initial tree /// for i in 0..1000 { /// let aabb = Aabb::new( -/// Point3::new(i as f32, 0.0, 0.0), -/// Point3::new(i as f32 + 1.0, 1.0, 1.0) +/// Vector::new(i as f32, 0.0, 0.0), +/// Vector::new(i as f32 + 1.0, 1.0, 1.0) /// ); /// bvh.insert(aabb, i); /// } @@ -1749,8 +1744,7 @@ impl IndexMut for BvhNodeVec { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct Bvh { pub(super) nodes: BvhNodeVec, @@ -1813,12 +1807,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(4.0, 0.0, 0.0), Point3::new(5.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(4.0, 0.0, 0.0), Vector::new(5.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::Binned, &aabbs); @@ -1868,13 +1862,13 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a BVH with custom indices /// let leaves = vec![ - /// (10, Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0))), - /// (20, Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0))), - /// (30, Aabb::new(Point3::new(4.0, 0.0, 0.0), Point3::new(5.0, 1.0, 1.0))), + /// (10, Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0))), + /// (20, Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0))), + /// (30, Aabb::new(Vector::new(4.0, 0.0, 0.0), Vector::new(5.0, 1.0, 1.0))), /// ]; /// /// let bvh = Bvh::from_iter(BvhBuildStrategy::Binned, leaves.into_iter()); @@ -1969,11 +1963,11 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::{Aabb, BoundingVolume}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); @@ -2020,26 +2014,26 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), /// ]; /// /// let mut bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); /// /// // Scale by 2x on all axes - /// bvh.scale(&Vector3::new(2.0, 2.0, 2.0)); + /// bvh.scale(Vector::new(2.0, 2.0, 2.0)); /// /// let root = bvh.root_aabb(); - /// assert_eq!(root.maxs, Point3::new(2.0, 2.0, 2.0)); + /// assert_eq!(root.maxs, Vector::new(2.0, 2.0, 2.0)); /// # } /// ``` /// /// # See Also /// /// - [`BvhNode::scale`] - Scale a single node - pub fn scale(&mut self, scale: &Vector) { + pub fn scale(&mut self, scale: Vector) { for node in self.nodes.0.iter_mut() { node.left.scale(scale); node.right.scale(scale); @@ -2093,10 +2087,10 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); @@ -2132,12 +2126,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs: Vec<_> = (0..100) /// .map(|i| { /// let f = i as f32; - /// Aabb::new(Point3::new(f, 0.0, 0.0), Point3::new(f + 1.0, 1.0, 1.0)) + /// Aabb::new(Vector::new(f, 0.0, 0.0), Vector::new(f + 1.0, 1.0, 1.0)) /// }) /// .collect(); /// @@ -2169,12 +2163,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs: Vec<_> = (0..100) /// .map(|i| { /// let f = i as f32; - /// Aabb::new(Point3::new(f, 0.0, 0.0), Point3::new(f + 1.0, 1.0, 1.0)) + /// Aabb::new(Vector::new(f, 0.0, 0.0), Vector::new(f + 1.0, 1.0, 1.0)) /// }) /// .collect(); /// @@ -2217,12 +2211,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs: Vec<_> = (0..4) /// .map(|i| { /// let f = i as f32; - /// Aabb::new(Point3::new(f, 0.0, 0.0), Point3::new(f + 1.0, 1.0, 1.0)) + /// Aabb::new(Vector::new(f, 0.0, 0.0), Vector::new(f + 1.0, 1.0, 1.0)) /// }) /// .collect(); /// @@ -2276,12 +2270,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(4.0, 0.0, 0.0), Point3::new(5.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(4.0, 0.0, 0.0), Vector::new(5.0, 1.0, 1.0)), /// ]; /// /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); @@ -2328,12 +2322,12 @@ impl Bvh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::partitioning::{Bvh, BvhBuildStrategy}; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let aabbs = vec![ - /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)), - /// Aabb::new(Point3::new(4.0, 0.0, 0.0), Point3::new(5.0, 1.0, 1.0)), + /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)), + /// Aabb::new(Vector::new(4.0, 0.0, 0.0), Vector::new(5.0, 1.0, 1.0)), /// ]; /// /// let mut bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &aabbs); diff --git a/src/query/clip/clip_aabb_line.rs b/src/query/clip/clip_aabb_line.rs index 966600b9..bbf47de0 100644 --- a/src/query/clip/clip_aabb_line.rs +++ b/src/query/clip/clip_aabb_line.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{Real, Vector, DIM}; use crate::query::Ray; use crate::shape::Segment; use num::{Bounded, Zero}; @@ -9,10 +9,14 @@ impl Aabb { /// /// Returns `None` if there is no intersection or if `pa` is invalid (contains `NaN`). #[inline] - pub fn clip_segment(&self, pa: &Point, pb: &Point) -> Option { + pub fn clip_segment(&self, pa: Vector, pb: Vector) -> Option { let ab = pb - pa; - clip_aabb_line(self, pa, &ab) - .map(|clip| Segment::new(pa + ab * (clip.0).0.max(0.0), pa + ab * (clip.1).0.min(1.0))) + clip_aabb_line(self, pa, ab).map(|clip| { + Segment::new( + pa + (ab * (clip.0).0.max(0.0)), + pa + (ab * (clip.1).0.min(1.0)), + ) + }) } /// Computes the parameters of the two intersection points between a line and this Aabb. @@ -20,11 +24,7 @@ impl Aabb { /// The parameters are such that the point are given by `orig + dir * parameter`. /// Returns `None` if there is no intersection or if `orig` is invalid (contains `NaN`). #[inline] - pub fn clip_line_parameters( - &self, - orig: &Point, - dir: &Vector, - ) -> Option<(Real, Real)> { + pub fn clip_line_parameters(&self, orig: Vector, dir: Vector) -> Option<(Real, Real)> { clip_aabb_line(self, orig, dir).map(|clip| ((clip.0).0, (clip.1).0)) } @@ -32,9 +32,9 @@ impl Aabb { /// /// Returns `None` if there is no intersection or if `orig` is invalid (contains `NaN`). #[inline] - pub fn clip_line(&self, orig: &Point, dir: &Vector) -> Option { + pub fn clip_line(&self, orig: Vector, dir: Vector) -> Option { clip_aabb_line(self, orig, dir) - .map(|clip| Segment::new(orig + dir * (clip.0).0, orig + dir * (clip.1).0)) + .map(|clip| Segment::new(orig + (dir * (clip.0).0), orig + (dir * (clip.1).0))) } /// Computes the parameters of the two intersection points between a ray and this Aabb. @@ -43,7 +43,7 @@ impl Aabb { /// Returns `None` if there is no intersection or if `ray.orig` is invalid (contains `NaN`). #[inline] pub fn clip_ray_parameters(&self, ray: &Ray) -> Option<(Real, Real)> { - self.clip_line_parameters(&ray.origin, &ray.dir) + self.clip_line_parameters(ray.origin, ray.dir) .and_then(|clip| { let t0 = clip.0; let t1 = clip.1; @@ -78,9 +78,9 @@ impl Aabb { /// and the line origin is inside the AABB. pub fn clip_aabb_line( aabb: &Aabb, - origin: &Point, - dir: &Vector, -) -> Option<((Real, Vector, isize), (Real, Vector, isize))> { + origin: Vector, + dir: Vector, +) -> Option<((Real, Vector, isize), (Real, Vector, isize))> { let mut tmax: Real = Bounded::max_value(); let mut tmin: Real = -tmax; let mut near_side = 0; @@ -146,11 +146,11 @@ pub fn clip_aabb_line( // zero or NaN. Return `Some` only if the ray starts inside the // aabb. if near_side == 0 { - let zero = (0.0, Vector::zeros(), 0); + let zero = (0.0, Vector::ZERO, 0); return aabb.contains_local_point(origin).then_some((zero, zero)); } - let mut normal = Vector::zeros(); + let mut normal = Vector::ZERO; if near_side < 0 { normal[(-near_side - 1) as usize] = 1.0; @@ -168,11 +168,11 @@ pub fn clip_aabb_line( // zero or NaN. Return `Some` only if the ray starts inside the // aabb. if far_side == 0 { - let zero = (0.0, Vector::zeros(), 0); + let zero = (0.0, Vector::ZERO, 0); return aabb.contains_local_point(origin).then_some((zero, zero)); } - let mut normal = Vector::zeros(); + let mut normal = Vector::ZERO; if far_side < 0 { normal[(-far_side - 1) as usize] = -1.0; @@ -192,28 +192,28 @@ mod test { #[test] pub fn clip_empty_aabb_line() { assert!(clip_aabb_line( - &Aabb::new(Point::origin(), Point::origin()), - &Point::origin(), - &Vector::zeros(), + &Aabb::new(Vector::ZERO, Vector::ZERO), + Vector::ZERO, + Vector::ZERO, ) .is_some()); assert!(clip_aabb_line( - &Aabb::new(Vector::repeat(1.0).into(), Vector::repeat(2.0).into()), - &Point::origin(), - &Vector::zeros(), + &Aabb::new(Vector::splat(1.0).into(), Vector::splat(2.0).into()), + Vector::ZERO, + Vector::ZERO, ) .is_none()); } #[test] pub fn clip_empty_aabb_segment() { - let aabb_origin = Aabb::new(Point::origin(), Point::origin()); - let aabb_shifted = Aabb::new(Vector::repeat(1.0).into(), Vector::repeat(2.0).into()); + let aabb_origin = Aabb::new(Vector::ZERO, Vector::ZERO); + let aabb_shifted = Aabb::new(Vector::splat(1.0), Vector::splat(2.0)); assert!(aabb_origin - .clip_segment(&Point::origin(), &Point::from(Vector::repeat(Real::NAN))) + .clip_segment(Vector::ZERO, Vector::splat(Real::NAN)) .is_some()); assert!(aabb_shifted - .clip_segment(&Point::origin(), &Point::from(Vector::repeat(Real::NAN))) + .clip_segment(Vector::ZERO, Vector::splat(Real::NAN)) .is_none()); } } diff --git a/src/query/clip/clip_aabb_polygon.rs b/src/query/clip/clip_aabb_polygon.rs index 2350c757..271483e4 100644 --- a/src/query/clip/clip_aabb_polygon.rs +++ b/src/query/clip/clip_aabb_polygon.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector}; +use crate::math::Vector; use alloc::vec::Vec; impl Aabb { @@ -10,7 +10,7 @@ impl Aabb { /// In order to avoid internal allocations, uses `self.clip_polygon_with_workspace` /// instead. #[inline] - pub fn clip_polygon(&self, points: &mut Vec>) { + pub fn clip_polygon(&self, points: &mut Vec) { let mut workspace = Vec::new(); self.clip_polygon_with_workspace(points, &mut workspace) } @@ -22,19 +22,19 @@ impl Aabb { #[inline] pub fn clip_polygon_with_workspace( &self, - points: &mut Vec>, - workspace: &mut Vec>, + points: &mut Vec, + workspace: &mut Vec, ) { - super::clip_halfspace_polygon(&self.mins, &-Vector::x(), points, workspace); - super::clip_halfspace_polygon(&self.maxs, &Vector::x(), workspace, points); + super::clip_halfspace_polygon(self.mins, -Vector::X, points, workspace); + super::clip_halfspace_polygon(self.maxs, Vector::X, workspace, points); - super::clip_halfspace_polygon(&self.mins, &-Vector::y(), points, workspace); - super::clip_halfspace_polygon(&self.maxs, &Vector::y(), workspace, points); + super::clip_halfspace_polygon(self.mins, -Vector::Y, points, workspace); + super::clip_halfspace_polygon(self.maxs, Vector::Y, workspace, points); #[cfg(feature = "dim3")] { - super::clip_halfspace_polygon(&self.mins, &-Vector::z(), points, workspace); - super::clip_halfspace_polygon(&self.maxs, &Vector::z(), workspace, points); + super::clip_halfspace_polygon(self.mins, -Vector::Z, points, workspace); + super::clip_halfspace_polygon(self.maxs, Vector::Z, workspace, points); } } } diff --git a/src/query/clip/clip_halfspace_polygon.rs b/src/query/clip/clip_halfspace_polygon.rs index 6bc2d376..6f823bd8 100644 --- a/src/query/clip/clip_halfspace_polygon.rs +++ b/src/query/clip/clip_halfspace_polygon.rs @@ -1,4 +1,4 @@ -use crate::math::{Point, Real, Vector}; +use crate::math::Vector; use crate::query::{self, Ray}; use alloc::vec::Vec; @@ -7,12 +7,12 @@ use alloc::vec::Vec; /// Given the half-space `center` and outward `normal`, /// this computes the intersecting between the half-space and /// the polygon. (Note that a point `pt` is considered as inside of -/// the half-space if `normal.dot(&(pt - center)) <= 0.0`. +/// the half-space if `normal.dot((pt - center)) <= 0.0`. pub fn clip_halfspace_polygon( - center: &Point, - normal: &Vector, - polygon: &[Point], - result: &mut Vec>, + center: Vector, + normal: Vector, + polygon: &[Vector], + result: &mut Vec, ) { result.clear(); @@ -20,24 +20,24 @@ pub fn clip_halfspace_polygon( return; } - let keep_point = |pt: &Point| (pt - center).dot(normal) <= 0.0; + let keep_point = |pt: Vector| (pt - center).dot(normal) <= 0.0; let last_pt = polygon.last().unwrap(); - let mut last_keep = keep_point(last_pt); + let mut last_keep = keep_point(*last_pt); if last_keep { result.push(*last_pt); } for i in 0..polygon.len() { - let pt = &polygon[i]; + let pt = polygon[i]; let keep = keep_point(pt); if keep != last_keep { // We crossed the plane, so we need // to cut the edge. let prev_i = if i == 0 { polygon.len() - 1 } else { i - 1 }; - let prev_pt = &polygon[prev_i]; - let ray = Ray::new(*prev_pt, pt - prev_pt); + let prev_pt = polygon[prev_i]; + let ray = Ray::new(prev_pt, pt - prev_pt); if let Some(time_of_impact) = query::details::ray_toi_with_halfspace(center, normal, &ray) @@ -51,7 +51,7 @@ pub fn clip_halfspace_polygon( } if keep && i != polygon.len() - 1 { - result.push(*pt); + result.push(pt); } } } diff --git a/src/query/clip/clip_segment_segment.rs b/src/query/clip/clip_segment_segment.rs index d5e44701..5e9b02d6 100644 --- a/src/query/clip/clip_segment_segment.rs +++ b/src/query/clip/clip_segment_segment.rs @@ -1,26 +1,26 @@ -use crate::math::{Point, Real}; +use crate::math::Vector; #[cfg(feature = "dim2")] -use crate::{math::Vector, utils}; +use crate::utils; // Features in clipping points are: // 0 = First vertex. // 1 = On the face. // 2 = Second vertex. -pub type ClippingPoints = (Point, Point, usize, usize); +pub type ClippingPoints = (Vector, Vector, usize, usize); /// Projects two segments on one another towards the direction `normal`, /// and compute their intersection. #[cfg(feature = "dim2")] pub fn clip_segment_segment_with_normal( - mut seg1: (Point, Point), - mut seg2: (Point, Point), - normal: Vector, + mut seg1: (Vector, Vector), + mut seg2: (Vector, Vector), + normal: Vector, ) -> Option<(ClippingPoints, ClippingPoints)> { use crate::utils::WBasis; let tangent = normal.orthonormal_basis()[0]; - let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)]; - let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)]; + let mut range1 = [seg1.0.dot(tangent), seg1.1.dot(tangent)]; + let mut range2 = [seg2.0.dot(tangent), seg2.1.dot(tangent)]; let mut features1 = [0, 2]; let mut features2 = [0, 2]; @@ -74,17 +74,17 @@ pub fn clip_segment_segment_with_normal( /// Projects two segments on one another and compute their intersection. pub fn clip_segment_segment( - mut seg1: (Point, Point), - mut seg2: (Point, Point), + mut seg1: (Vector, Vector), + mut seg2: (Vector, Vector), ) -> Option<(ClippingPoints, ClippingPoints)> { // NOTE: no need to normalize the tangent. let tangent1 = seg1.1 - seg1.0; - let sqnorm_tangent1 = tangent1.norm_squared(); + let sqnorm_tangent1 = tangent1.length_squared(); let mut range1 = [0.0, sqnorm_tangent1]; let mut range2 = [ - (seg2.0 - seg1.0).dot(&tangent1), - (seg2.1 - seg1.0).dot(&tangent1), + (seg2.0 - seg1.0).dot(tangent1), + (seg2.1 - seg1.0).dot(tangent1), ]; let mut features1 = [0, 2]; let mut features2 = [0, 2]; diff --git a/src/query/closest_points/closest_points.rs b/src/query/closest_points/closest_points.rs index 144464c5..92d60425 100644 --- a/src/query/closest_points/closest_points.rs +++ b/src/query/closest_points/closest_points.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Vector}; use core::mem; @@ -30,13 +30,13 @@ use core::mem; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// // Search for closest points within 10.0 units /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, 10.0).unwrap(); @@ -48,7 +48,7 @@ use core::mem; /// ClosestPoints::WithinMargin(pt1, pt2) => { /// println!("Closest point on shape1: {:?}", pt1); /// println!("Closest point on shape2: {:?}", pt2); -/// let distance = (pt2 - pt1).norm(); +/// let distance = (pt2 - pt1).length(); /// println!("Distance between shapes: {}", distance); /// } /// ClosestPoints::Disjoint => { @@ -67,8 +67,7 @@ use core::mem; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub enum ClosestPoints { /// The two shapes are intersecting (overlapping or touching). @@ -83,14 +82,14 @@ pub enum ClosestPoints { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; - /// use nalgebra::Isometry3; + /// use parry3d::math::Pose; /// /// let ball1 = Ball::new(2.0); /// let ball2 = Ball::new(2.0); /// /// // Overlapping balls (centers 3.0 apart, combined radii 4.0) - /// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); - /// let pos2 = Isometry3::translation(3.0, 0.0, 0.0); + /// let pos1 = Pose::translation(0.0, 0.0, 0.0); + /// let pos2 = Pose::translation(3.0, 0.0, 0.0); /// /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, 10.0).unwrap(); /// assert_eq!(result, ClosestPoints::Intersecting); @@ -112,31 +111,31 @@ pub enum ClosestPoints { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; - /// use nalgebra::Isometry3; + /// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Balls separated by 3.0 units (centers 5.0 apart, combined radii 2.0) - /// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); - /// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); + /// let pos1 = Pose::translation(0.0, 0.0, 0.0); + /// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, 10.0).unwrap(); /// /// if let ClosestPoints::WithinMargin(pt1, pt2) = result { - /// // Points are on the surface, facing each other + /// // Vectors are on the surface, facing each other /// assert!((pt1.x - 1.0).abs() < 1e-5); // ball1 surface at x=1.0 /// assert!((pt2.x - 4.0).abs() < 1e-5); // ball2 surface at x=4.0 /// /// // Distance between points - /// let dist = (pt2 - pt1).norm(); + /// let dist = (pt2 - pt1).length(); /// assert!((dist - 3.0).abs() < 1e-5); /// } else { /// panic!("Expected WithinMargin"); /// } /// # } /// ``` - WithinMargin(Point, Point), + WithinMargin(Vector, Vector), /// The shapes are separated by more than the specified maximum distance. /// @@ -150,14 +149,14 @@ pub enum ClosestPoints { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; - /// use nalgebra::Isometry3; + /// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Balls separated by 8.0 units - /// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); - /// let pos2 = Isometry3::translation(10.0, 0.0, 0.0); + /// let pos1 = Pose::translation(0.0, 0.0, 0.0); + /// let pos2 = Pose::translation(10.0, 0.0, 0.0); /// /// // Only search within 5.0 units /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, 5.0).unwrap(); @@ -184,13 +183,13 @@ impl ClosestPoints { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; - /// use nalgebra::Isometry3; + /// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(2.0); /// - /// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); - /// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); + /// let pos1 = Pose::translation(0.0, 0.0, 0.0); + /// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// let mut result = closest_points(&pos1, &ball1, &pos2, &ball2, 10.0).unwrap(); /// @@ -221,13 +220,13 @@ impl ClosestPoints { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; - /// use nalgebra::Isometry3; + /// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(2.0); /// - /// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); - /// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); + /// let pos1 = Pose::translation(0.0, 0.0, 0.0); + /// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, 10.0).unwrap(); /// let flipped = result.flipped(); @@ -256,8 +255,8 @@ impl ClosestPoints { /// respective local coordinate frames to world-space coordinates. This is used /// internally by the query system. /// - /// - Point 1 is transformed by `pos1` - /// - Point 2 is transformed by `pos2` + /// - Vector 1 is transformed by `pos1` + /// - Vector 2 is transformed by `pos2` /// - `Intersecting` and `Disjoint` variants are returned unchanged /// /// # Arguments @@ -270,17 +269,17 @@ impl ClosestPoints { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::ClosestPoints; - /// use nalgebra::{Isometry3, Point3}; + /// use parry3d::math::{Pose, Vector}; /// - /// // Points in local space + /// // Vectors in local space /// let local_result = ClosestPoints::WithinMargin( - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(-1.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(-1.0, 0.0, 0.0), /// ); /// /// // Transform to world space - /// let pos1 = Isometry3::translation(10.0, 0.0, 0.0); - /// let pos2 = Isometry3::translation(20.0, 0.0, 0.0); + /// let pos1 = Pose::translation(10.0, 0.0, 0.0); + /// let pos2 = Pose::translation(20.0, 0.0, 0.0); /// /// let world_result = local_result.transform_by(&pos1, &pos2); /// @@ -291,7 +290,7 @@ impl ClosestPoints { /// # } /// ``` #[must_use] - pub fn transform_by(self, pos1: &Isometry, pos2: &Isometry) -> Self { + pub fn transform_by(self, pos1: &Pose, pos2: &Pose) -> Self { if let ClosestPoints::WithinMargin(p1, p2) = self { ClosestPoints::WithinMargin(pos1 * p1, pos2 * p2) } else { diff --git a/src/query/closest_points/closest_points_ball_ball.rs b/src/query/closest_points/closest_points_ball_ball.rs index 5a6c9420..122d88ae 100644 --- a/src/query/closest_points/closest_points_ball_ball.rs +++ b/src/query/closest_points/closest_points_ball_ball.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Real}; use crate::query::ClosestPoints; use crate::shape::Ball; @@ -6,12 +6,7 @@ use crate::shape::Ball; /// /// Each returned point is expressed on the local-space of the corresponding shape. #[inline] -pub fn closest_points_ball_ball( - pos12: &Isometry, - b1: &Ball, - b2: &Ball, - margin: Real, -) -> ClosestPoints { +pub fn closest_points_ball_ball(pos12: &Pose, b1: &Ball, b2: &Ball, margin: Real) -> ClosestPoints { assert!( margin >= 0.0, "The proximity margin must be positive or null." @@ -19,8 +14,8 @@ pub fn closest_points_ball_ball( let r1 = b1.radius; let r2 = b2.radius; - let delta_pos = pos12.translation.vector; - let distance = delta_pos.norm(); + let delta_pos = pos12.translation; + let distance = delta_pos.length(); let sum_radius = r1 + r2; if distance - margin <= sum_radius { @@ -28,8 +23,8 @@ pub fn closest_points_ball_ball( ClosestPoints::Intersecting } else { let normal = delta_pos.normalize(); - let p1 = Point::from(normal * r1); - let p2 = Point::from(pos12.inverse_transform_vector(&normal) * -r2); + let p1 = normal * r1; + let p2 = pos12.rotation.inverse() * normal * -r2; ClosestPoints::WithinMargin(p1, p2) } } else { diff --git a/src/query/closest_points/closest_points_ball_convex_polyhedron.rs b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs index 49f59280..9f9d21fe 100644 --- a/src/query/closest_points/closest_points_ball_convex_polyhedron.rs +++ b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::ClosestPoints; use crate::shape::{Ball, Shape}; @@ -8,7 +8,7 @@ use crate::shape::{Ball, Shape}; /// both the ConvexPolyhedron and PointQuery traits. #[inline] pub fn closest_points_ball_convex_polyhedron( - pos12: &Isometry, + pos12: &Pose, ball1: &Ball, shape2: &(impl Shape + ?Sized), prediction: Real, @@ -31,7 +31,7 @@ pub fn closest_points_ball_convex_polyhedron( /// both the ConvexPolyhedron and PointQuery traits. #[inline] pub fn closest_points_convex_polyhedron_ball( - pos12: &Isometry, + pos12: &Pose, shape1: &(impl Shape + ?Sized), ball2: &Ball, prediction: Real, diff --git a/src/query/closest_points/closest_points_composite_shape_shape.rs b/src/query/closest_points/closest_points_composite_shape_shape.rs index 7953dd0f..90cd0fea 100644 --- a/src/query/closest_points/closest_points_composite_shape_shape.rs +++ b/src/query/closest_points/closest_points_composite_shape_shape.rs @@ -1,10 +1,9 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::partitioning::BvhNode; use crate::query::{ClosestPoints, QueryDispatcher}; use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; -use crate::utils::IsometryOpt; -use na; +use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { /// Returns the closest points between `self` and the given `shape2` positioned at @@ -21,12 +20,12 @@ impl CompositeShapeRef<'_, S> { pub fn closest_points_to_shape( &self, dispatcher: &D, - pose12: &Isometry, + pose12: &Pose, shape2: &dyn Shape, margin: Real, ) -> Option<(u32, ClosestPoints)> { let ls_aabb2 = shape2.compute_aabb(pose12); - let msum_shift = -ls_aabb2.center().coords; + let msum_shift = -ls_aabb2.center(); let msum_margin = ls_aabb2.half_extents(); self.0 @@ -52,9 +51,9 @@ impl CompositeShapeRef<'_, S> { ) { let cost = match &mut pts { ClosestPoints::WithinMargin(p1, p2) => { - *p1 = part_pos1.transform_point(&*p1); + *p1 = part_pos1.transform_point(*p1); let p2_1 = pose12 * *p2; - na::distance(&*p1, &p2_1) + (*p1 - p2_1).length() } ClosestPoints::Intersecting => -Real::MAX, ClosestPoints::Disjoint => Real::MAX, @@ -73,7 +72,7 @@ impl CompositeShapeRef<'_, S> { /// Closest points between a composite shape and any other shape. pub fn closest_points_composite_shape_shape( dispatcher: &D, - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &dyn Shape, margin: Real, @@ -91,7 +90,7 @@ where /// Closest points between a shape and a composite shape. pub fn closest_points_shape_composite_shape( dispatcher: &D, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &G2, margin: Real, diff --git a/src/query/closest_points/closest_points_cuboid_cuboid.rs b/src/query/closest_points/closest_points_cuboid_cuboid.rs index 81173b3d..19adb60d 100644 --- a/src/query/closest_points/closest_points_cuboid_cuboid.rs +++ b/src/query/closest_points/closest_points_cuboid_cuboid.rs @@ -1,11 +1,11 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{sat, ClosestPoints, PointQuery}; use crate::shape::{Cuboid, SupportMap}; /// Closest points between two cuboids. #[inline] pub fn closest_points_cuboid_cuboid( - pos12: &Isometry, + pos12: &Pose, cuboid1: &Cuboid, cuboid2: &Cuboid, margin: Real, @@ -23,7 +23,7 @@ pub fn closest_points_cuboid_cuboid( } #[cfg(feature = "dim2")] - let sep3 = (-Real::MAX, crate::math::Vector::::y()); // This case does not exist in 2D. + let sep3 = (-Real::MAX, crate::math::Vector::Y); // This case does not exist in 2D. #[cfg(feature = "dim3")] let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pos12); if sep3.0 > margin { @@ -41,9 +41,9 @@ pub fn closest_points_cuboid_cuboid( // To compute the closest points, we need to project the support point // from cuboid2 on the support-face of cuboid1. For simplicity, we just // project the support point from cuboid2 on cuboid1 itself (not just the face). - let pt2_1 = cuboid2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, true); - if na::distance_squared(&proj1.point, &pt2_1) > margin * margin { + let pt2_1 = cuboid2.support_point(pos12, -sep1.1); + let proj1 = cuboid1.project_local_point(pt2_1, true); + if (proj1.point - pt2_1).length_squared() > margin * margin { return ClosestPoints::Disjoint; } else { return ClosestPoints::WithinMargin(proj1.point, pos21 * pt2_1); @@ -57,10 +57,10 @@ pub fn closest_points_cuboid_cuboid( // To compute the actual closest points, we need to project the support point // from cuboid1 on the support-face of cuboid2. For simplicity, we just // project the support point from cuboid1 on cuboid2 itself (not just the face). - let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = cuboid2.project_local_point(&pt1_2, true); + let pt1_2 = cuboid1.support_point(&pos21, -sep2.1); + let proj2 = cuboid2.project_local_point(pt1_2, true); - if na::distance_squared(&proj2.point, &pt1_2) > margin * margin { + if (proj2.point - pt1_2).length_squared() > margin * margin { return ClosestPoints::Disjoint; } else { return ClosestPoints::WithinMargin(pos12 * pt1_2, proj2.point); @@ -75,7 +75,7 @@ pub fn closest_points_cuboid_cuboid( // To compute the actual distance, we need to compute the closest // points between the two edges that generated the separating axis. let edge1 = cuboid1.local_support_edge_segment(sep3.1); - let edge2 = cuboid2.local_support_edge_segment(pos21 * -sep3.1); + let edge2 = cuboid2.local_support_edge_segment(pos21.rotation * -sep3.1); return super::closest_points_segment_segment(pos12, &edge1, &edge2, margin); } diff --git a/src/query/closest_points/closest_points_cuboid_triangle.rs b/src/query/closest_points/closest_points_cuboid_triangle.rs index 0ac6449d..95ef7c80 100644 --- a/src/query/closest_points/closest_points_cuboid_triangle.rs +++ b/src/query/closest_points/closest_points_cuboid_triangle.rs @@ -1,11 +1,11 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{sat, ClosestPoints, PointQuery}; use crate::shape::{Cuboid, SupportMap, Triangle}; /// Closest points between a cuboid and a triangle. #[inline] pub fn closest_points_cuboid_triangle( - pos12: &Isometry, + pos12: &Pose, cuboid1: &Cuboid, triangle2: &Triangle, margin: Real, @@ -24,7 +24,7 @@ pub fn closest_points_cuboid_triangle( } #[cfg(feature = "dim2")] - let sep3 = (-Real::MAX, crate::math::Vector::::y()); // This case does not exist in 2D. + let sep3 = (-Real::MAX, crate::math::Vector::Y); // This case does not exist in 2D. #[cfg(feature = "dim3")] let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cuboid1, triangle2, pos12); if sep3.0 > margin { @@ -40,9 +40,9 @@ pub fn closest_points_cuboid_triangle( // To compute the closest points, we need to project the support point // from triangle2 on the support-face of triangle1. For simplicity, we just // project the support point from triangle2 on cuboid1 itself (not just the face). - let pt2_1 = triangle2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, true); - if na::distance_squared(&proj1.point, &pt2_1) > margin * margin { + let pt2_1 = triangle2.support_point(pos12, -sep1.1); + let proj1 = cuboid1.project_local_point(pt2_1, true); + if (proj1.point - pt2_1).length_squared() > margin * margin { return ClosestPoints::Disjoint; } else { return ClosestPoints::WithinMargin(proj1.point, pos21 * pt2_1); @@ -54,10 +54,10 @@ pub fn closest_points_cuboid_triangle( // To compute the actual closest points, we need to project the support point // from cuboid1 on the support-face of triangle2. For simplicity, we just // project the support point from cuboid1 on triangle2 itself (not just the face). - let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = triangle2.project_local_point(&pt1_2, true); + let pt1_2 = cuboid1.support_point(&pos21, -sep2.1); + let proj2 = triangle2.project_local_point(pt1_2, true); - if na::distance_squared(&proj2.point, &pt1_2) > margin * margin { + if (proj2.point - pt1_2).length_squared() > margin * margin { return ClosestPoints::Disjoint; } else { return ClosestPoints::WithinMargin(pos12 * pt1_2, proj2.point); @@ -70,7 +70,7 @@ pub fn closest_points_cuboid_triangle( // To compute the actual distance, we need to compute the closest // points between the two edges that generated the separating axis. let edge1 = cuboid1.local_support_edge_segment(sep3.1); - let edge2 = triangle2.local_support_edge_segment(pos21 * -sep3.1); + let edge2 = triangle2.local_support_edge_segment(pos21.rotation * -sep3.1); return super::closest_points_segment_segment(pos12, &edge1, &edge2, margin); } @@ -80,7 +80,7 @@ pub fn closest_points_cuboid_triangle( /// Closest points between a triangle and a cuboid. #[inline] pub fn closest_points_triangle_cuboid( - pos12: &Isometry, + pos12: &Pose, triangle1: &Triangle, cuboid2: &Cuboid, margin: Real, diff --git a/src/query/closest_points/closest_points_halfspace_support_map.rs b/src/query/closest_points/closest_points_halfspace_support_map.rs index 75243bcd..563a20e1 100644 --- a/src/query/closest_points/closest_points_halfspace_support_map.rs +++ b/src/query/closest_points/closest_points_halfspace_support_map.rs @@ -1,11 +1,11 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::ClosestPoints; use crate::shape::HalfSpace; use crate::shape::SupportMap; /// Closest points between a halfspace and a support-mapped shape (Cuboid, ConvexHull, etc.) pub fn closest_points_halfspace_support_map( - pos12: &Isometry, + pos12: &Pose, halfspace: &HalfSpace, other: &G, margin: Real, @@ -15,15 +15,15 @@ pub fn closest_points_halfspace_support_map( "The proximity margin must be positive or null." ); - let deepest = other.support_point(pos12, &-halfspace.normal); - let distance = halfspace.normal.dot(&(-deepest.coords)); + let deepest = other.support_point(pos12, -halfspace.normal); + let distance = halfspace.normal.dot(-deepest); if distance >= -margin { if distance >= 0.0 { ClosestPoints::Intersecting } else { - let p1 = deepest + *halfspace.normal * distance; - let p2 = pos12.inverse_transform_point(&deepest); + let p1 = deepest + halfspace.normal * distance; + let p2 = pos12.inverse_transform_point(deepest); ClosestPoints::WithinMargin(p1, p2) } } else { @@ -33,7 +33,7 @@ pub fn closest_points_halfspace_support_map( /// Closest points between a support-mapped shape (Cuboid, ConvexHull, etc.) and a halfspace. pub fn closest_points_support_map_halfspace( - pos12: &Isometry, + pos12: &Pose, other: &G, halfspace: &HalfSpace, margin: Real, diff --git a/src/query/closest_points/closest_points_line_line.rs b/src/query/closest_points/closest_points_line_line.rs index 9b6ef50a..5b3e48d1 100644 --- a/src/query/closest_points/closest_points_line_line.rs +++ b/src/query/closest_points/closest_points_line_line.rs @@ -1,5 +1,4 @@ -use crate::math::{Point, Real, Vector}; -use crate::na::{Point as SPoint, SVector}; +use crate::math::{Real, Vector}; /// Closest points between two lines. /// @@ -7,10 +6,10 @@ use crate::na::{Point as SPoint, SVector}; /// `orig1 + dir1 * res.0` and `orig2 + dir2 * res.1`. #[inline] pub fn closest_points_line_line_parameters( - orig1: &Point, - dir1: &Vector, - orig2: &Point, - dir2: &Vector, + orig1: Vector, + dir1: Vector, + orig2: Vector, + dir2: Vector, ) -> (Real, Real) { let res = closest_points_line_line_parameters_eps( orig1, @@ -29,26 +28,26 @@ pub fn closest_points_line_line_parameters( /// then `res.2` is set to `true` and the returned closest points are `orig1` and /// its projection on the second line. #[inline] -pub fn closest_points_line_line_parameters_eps( - orig1: &SPoint, - dir1: &SVector, - orig2: &SPoint, - dir2: &SVector, +pub fn closest_points_line_line_parameters_eps( + orig1: Vector, + dir1: Vector, + orig2: Vector, + dir2: Vector, eps: Real, ) -> (Real, Real, bool) { // Inspired by RealField-time collision detection by Christer Ericson. let r = orig1 - orig2; - let a = dir1.norm_squared(); - let e = dir2.norm_squared(); - let f = dir2.dot(&r); + let a = dir1.length_squared(); + let e = dir2.length_squared(); + let f = dir2.dot(r); if a <= eps && e <= eps { (0.0, 0.0, false) } else if a <= eps { (0.0, f / e, false) } else { - let c = dir1.dot(&r); + let c = dir1.dot(r); if e <= eps { (-c / a, 0.0, false) } else { @@ -75,11 +74,11 @@ pub fn closest_points_line_line_parameters_eps( /// Closest points between two segments. #[inline] pub fn closest_points_line_line( - orig1: &Point, - dir1: &Vector, - orig2: &Point, - dir2: &Vector, -) -> (Point, Point) { + orig1: Vector, + dir1: Vector, + orig2: Vector, + dir2: Vector, +) -> (Vector, Vector) { let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2); - (*orig1 + *dir1 * s, *orig2 + *dir2 * t) + (orig1 + dir1 * s, orig2 + dir2 * t) } diff --git a/src/query/closest_points/closest_points_segment_segment.rs b/src/query/closest_points/closest_points_segment_segment.rs index 18168f75..d3823451 100644 --- a/src/query/closest_points/closest_points_segment_segment.rs +++ b/src/query/closest_points/closest_points_segment_segment.rs @@ -1,13 +1,13 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real, Vector2}; use crate::query::ClosestPoints; use crate::shape::{Segment, SegmentPointLocation}; -use na::{self, Point}; +use crate::math::Vector; /// Closest points between segments. #[inline] pub fn closest_points_segment_segment( - pos12: &Isometry, + pos12: &Pose, seg1: &Segment, seg2: &Segment, margin: Real, @@ -16,7 +16,7 @@ pub fn closest_points_segment_segment( let p1 = seg1.point_at(&loc1); let p2 = seg2.point_at(&loc2); - if na::distance_squared(&p1, &(pos12 * p2)) <= margin * margin { + if (p1 - (pos12 * p2)).length_squared() <= margin * margin { ClosestPoints::WithinMargin(p1, p2) } else { ClosestPoints::Disjoint @@ -27,7 +27,7 @@ pub fn closest_points_segment_segment( /// Closest points between two segments. #[inline] pub fn closest_points_segment_segment_with_locations( - pos12: &Isometry, + pos12: &Pose, seg1: &Segment, seg2: &Segment, ) -> (SegmentPointLocation, SegmentPointLocation) { @@ -35,77 +35,84 @@ pub fn closest_points_segment_segment_with_locations( closest_points_segment_segment_with_locations_nD((&seg1.a, &seg1.b), (&seg2_1.a, &seg2_1.b)) } -/// Segment-segment closest points computation in an arbitrary dimension. -#[allow(non_snake_case)] -#[inline] -pub fn closest_points_segment_segment_with_locations_nD( - seg1: (&Point, &Point), - seg2: (&Point, &Point), -) -> (SegmentPointLocation, SegmentPointLocation) { - // Inspired by RealField-time collision detection by Christer Ericson. - let d1 = seg1.1 - seg1.0; - let d2 = seg2.1 - seg2.0; - let r = seg1.0 - seg2.0; - - let a = d1.norm_squared(); - let e = d2.norm_squared(); - let f = d2.dot(&r); +macro_rules! impl_closest_points( + ($Vect: ty, $fname: ident) => { + /// Segment-segment closest points computation in an arbitrary dimension. + #[allow(non_snake_case)] + #[inline] + pub fn $fname( + seg1: (&$Vect, &$Vect), + seg2: (&$Vect, &$Vect), + ) -> (SegmentPointLocation, SegmentPointLocation) { + // Inspired by RealField-time collision detection by Christer Ericson. + let d1 = seg1.1 - seg1.0; + let d2 = seg2.1 - seg2.0; + let r = seg1.0 - seg2.0; - let mut s; - let mut t; + let a = d1.length_squared(); + let e = d2.length_squared(); + let f = d2.dot(r); - let _eps = crate::math::DEFAULT_EPSILON; - if a <= _eps && e <= _eps { - s = 0.0; - t = 0.0; - } else if a <= _eps { - s = 0.0; - t = na::clamp(f / e, 0.0, 1.0); - } else { - let c = d1.dot(&r); - if e <= _eps { - t = 0.0; - s = na::clamp(-c / a, 0.0, 1.0); - } else { - let b = d1.dot(&d2); - let ae = a * e; - let bb = b * b; - let denom = ae - bb; + let mut s; + let mut t; - // Use absolute and ulps error to test collinearity. - if denom > _eps && !ulps_eq!(ae, bb) { - s = na::clamp((b * f - c * e) / denom, 0.0, 1.0); - } else { + let _eps = crate::math::DEFAULT_EPSILON; + if a <= _eps && e <= _eps { s = 0.0; - } + t = 0.0; + } else if a <= _eps { + s = 0.0; + t = (f / e).clamp(0.0, 1.0); + } else { + let c = d1.dot(r); + if e <= _eps { + t = 0.0; + s = (-c / a).clamp(0.0, 1.0); + } else { + let b = d1.dot(d2); + let ae = a * e; + let bb = b * b; + let denom = ae - bb; - t = (b * s + f) / e; + // Use absolute and ulps error to test collinearity. + if denom > _eps && !ulps_eq!(ae, bb) { + s = ((b * f - c * e) / denom).clamp(0.0, 1.0); + } else { + s = 0.0; + } - if t < 0.0 { - t = 0.0; - s = na::clamp(-c / a, 0.0, 1.0); - } else if t > 1.0 { - t = 1.0; - s = na::clamp((b - c) / a, 0.0, 1.0); + t = (b * s + f) / e; + + if t < 0.0 { + t = 0.0; + s = (-c / a).clamp(0.0, 1.0); + } else if t > 1.0 { + t = 1.0; + s = ((b - c) / a).clamp(0.0, 1.0); + } + } } - } - } - let loc1 = if s == 0.0 { - SegmentPointLocation::OnVertex(0) - } else if s == 1.0 { - SegmentPointLocation::OnVertex(1) - } else { - SegmentPointLocation::OnEdge([1.0 - s, s]) - }; + let loc1 = if s == 0.0 { + SegmentPointLocation::OnVertex(0) + } else if s == 1.0 { + SegmentPointLocation::OnVertex(1) + } else { + SegmentPointLocation::OnEdge([1.0 - s, s]) + }; - let loc2 = if t == 0.0 { - SegmentPointLocation::OnVertex(0) - } else if t == 1.0 { - SegmentPointLocation::OnVertex(1) - } else { - SegmentPointLocation::OnEdge([1.0 - t, t]) - }; + let loc2 = if t == 0.0 { + SegmentPointLocation::OnVertex(0) + } else if t == 1.0 { + SegmentPointLocation::OnVertex(1) + } else { + SegmentPointLocation::OnEdge([1.0 - t, t]) + }; - (loc1, loc2) -} + (loc1, loc2) + } + } +); + +impl_closest_points!(Vector2, closest_points_segment_segment_2d); +impl_closest_points!(Vector, closest_points_segment_segment_with_locations_nD); diff --git a/src/query/closest_points/closest_points_shape_shape.rs b/src/query/closest_points/closest_points_shape_shape.rs index c1c731a9..b886bb43 100644 --- a/src/query/closest_points/closest_points_shape_shape.rs +++ b/src/query/closest_points/closest_points_shape_shape.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{ClosestPoints, DefaultQueryDispatcher, QueryDispatcher, Unsupported}; use crate::shape::Shape; @@ -61,14 +61,14 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Position balls 5 units apart -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// // Find closest points (unlimited distance) /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, f32::MAX).unwrap(); @@ -76,7 +76,7 @@ use crate::shape::Shape; /// if let ClosestPoints::WithinMargin(pt1, pt2) = result { /// // pt1 is at (1.0, 0.0, 0.0) - surface of ball1 /// // pt2 is at (4.0, 0.0, 0.0) - surface of ball2 -/// let distance = (pt2 - pt1).norm(); +/// let distance = (pt2 - pt1).length(); /// assert!((distance - 3.0).abs() < 1e-5); // 5.0 - 1.0 - 1.0 = 3.0 /// } /// # } @@ -88,13 +88,13 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(10.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(10.0, 0.0, 0.0); /// /// // Only search within 5.0 units /// let result = closest_points(&pos1, &ball1, &pos2, &ball2, 5.0).unwrap(); @@ -114,14 +114,14 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Cuboid; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// -/// let box1 = Cuboid::new(Vector3::new(2.0, 2.0, 2.0)); -/// let box2 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let box1 = Cuboid::new(Vector::new(2.0, 2.0, 2.0)); +/// let box2 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// // Position boxes so they overlap -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(2.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(2.0, 0.0, 0.0); /// /// let result = closest_points(&pos1, &box1, &pos2, &box2, 10.0).unwrap(); /// @@ -136,7 +136,7 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// // Enemy detection radius /// let detection_radius = 15.0; @@ -144,8 +144,8 @@ use crate::shape::Shape; /// let player = Ball::new(0.5); /// let enemy = Ball::new(0.5); /// -/// let player_pos = Isometry3::translation(0.0, 0.0, 0.0); -/// let enemy_pos = Isometry3::translation(12.0, 0.0, 0.0); +/// let player_pos = Pose::translation(0.0, 0.0, 0.0); +/// let enemy_pos = Pose::translation(12.0, 0.0, 0.0); /// /// let result = closest_points( /// &player_pos, @@ -177,13 +177,13 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{closest_points, ClosestPoints}; /// use parry3d::shape::{Ball, Cuboid}; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball = Ball::new(2.0); -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// -/// let pos_ball = Isometry3::translation(5.0, 0.0, 0.0); -/// let pos_cuboid = Isometry3::translation(0.0, 0.0, 0.0); +/// let pos_ball = Pose::translation(5.0, 0.0, 0.0); +/// let pos_cuboid = Pose::translation(0.0, 0.0, 0.0); /// /// let result = closest_points(&pos_ball, &ball, &pos_cuboid, &cuboid, 10.0).unwrap(); /// @@ -194,7 +194,7 @@ use crate::shape::Shape; /// println!("Closest point on cuboid: {:?}", pt_cuboid); /// /// // Verify distance -/// let separation = (pt_ball - pt_cuboid).norm(); +/// let separation = (pt_ball - pt_cuboid).length(); /// println!("Separation distance: {}", separation); /// } /// # } @@ -206,7 +206,7 @@ use crate::shape::Shape; /// /// | Query | Returns | Use When | /// |-------|---------|----------| -/// | `closest_points` | Point locations | You need exact surface points | +/// | `closest_points` | Vector locations | You need exact surface points | /// | [`distance`](crate::query::distance::distance()) | Distance value | You only need the distance | /// | [`contact`](crate::query::contact::contact()) | Contact info | Shapes are touching/penetrating | /// | [`intersection_test`](crate::query::intersection_test::intersection_test()) | Boolean | You only need yes/no overlap | @@ -218,9 +218,9 @@ use crate::shape::Shape; /// - [`contact`](crate::query::contact::contact()) - For penetration depth and contact normals /// - [`intersection_test`](crate::query::intersection_test::intersection_test()) - For boolean overlap test pub fn closest_points( - pos1: &Isometry, + pos1: &Pose, g1: &dyn Shape, - pos2: &Isometry, + pos2: &Pose, g2: &dyn Shape, max_dist: Real, ) -> Result { diff --git a/src/query/closest_points/closest_points_support_map_support_map.rs b/src/query/closest_points/closest_points_support_map_support_map.rs index 5930b03a..0d151338 100644 --- a/src/query/closest_points/closest_points_support_map_support_map.rs +++ b/src/query/closest_points/closest_points_support_map_support_map.rs @@ -1,13 +1,11 @@ -use crate::math::{Isometry, Real, Vector}; -use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex}; +use crate::math::{Pose, Real, Vector}; +use crate::query::gjk::{self, CsoPoint, GJKResult, VoronoiSimplex}; use crate::query::ClosestPoints; use crate::shape::SupportMap; -use na::Unit; - /// Closest points between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.) pub fn closest_points_support_map_support_map( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, prediction: Real, @@ -25,7 +23,7 @@ where None, ) { GJKResult::ClosestPoints(pt1, pt2, _) => { - ClosestPoints::WithinMargin(pt1, pos12.inverse_transform_point(&pt2)) + ClosestPoints::WithinMargin(pt1, pos12.inverse_transform_point(pt2)) } GJKResult::NoIntersection(_) => ClosestPoints::Disjoint, GJKResult::Intersection => ClosestPoints::Intersecting, @@ -37,32 +35,27 @@ where /// /// This allows a more fine grained control other the underlying GJK algorigtm. pub fn closest_points_support_map_support_map_with_params( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, prediction: Real, simplex: &mut VoronoiSimplex, - init_dir: Option>, + init_dir: Option, ) -> GJKResult where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let dir = match init_dir { - // TODO: or pos12.translation.vector (without the minus sign) ? - None => -pos12.translation.vector, + // TODO: or pos12.translation (without the minus sign) ? + None => -pos12.translation, Some(dir) => dir, }; - if let Some(dir) = Unit::try_new(dir, crate::math::DEFAULT_EPSILON) { - simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir)); + if let Some(dir) = (dir).try_normalize() { + simplex.reset(CsoPoint::from_shapes(pos12, g1, g2, dir)); } else { - simplex.reset(CSOPoint::from_shapes( - pos12, - g1, - g2, - &Vector::::x_axis(), - )); + simplex.reset(CsoPoint::from_shapes(pos12, g1, g2, Vector::X)); } gjk::closest_points(pos12, g1, g2, prediction, true, simplex) diff --git a/src/query/closest_points/mod.rs b/src/query/closest_points/mod.rs index 070fe827..fa962b33 100644 --- a/src/query/closest_points/mod.rs +++ b/src/query/closest_points/mod.rs @@ -21,7 +21,8 @@ pub use self::closest_points_line_line::{ closest_points_line_line_parameters_eps, }; pub use self::closest_points_segment_segment::{ - closest_points_segment_segment, closest_points_segment_segment_with_locations, + closest_points_segment_segment, closest_points_segment_segment_2d, + closest_points_segment_segment_with_locations, closest_points_segment_segment_with_locations_nD, }; pub use self::closest_points_shape_shape::closest_points; diff --git a/src/query/contact/contact.rs b/src/query/contact/contact.rs index 3fb19fe5..268f6bfa 100644 --- a/src/query/contact/contact.rs +++ b/src/query/contact/contact.rs @@ -1,9 +1,5 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use core::mem; -use na::{self, Unit}; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// Geometric description of a contact between two shapes. /// @@ -38,14 +34,14 @@ use rkyv::{bytecheck, CheckBytes}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::contact; /// use parry3d::shape::Ball; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Overlapping balls (centers 1.5 units apart, combined radii = 2.0) -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(1.5, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(1.5, 0.0, 0.0); /// /// if let Ok(Some(contact)) = contact(&pos1, &ball1, &pos2, &ball2, 0.0) { /// // Penetration depth (negative distance) @@ -56,8 +52,8 @@ use rkyv::{bytecheck, CheckBytes}; /// println!("Normal: {:?}", contact.normal1); /// /// // Contact points are on each shape's surface -/// println!("Point on ball1: {:?}", contact.point1); -/// println!("Point on ball2: {:?}", contact.point2); +/// println!("Vector on ball1: {:?}", contact.point1); +/// println!("Vector on ball2: {:?}", contact.point2); /// } /// # } /// ``` @@ -65,34 +61,33 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct Contact { /// Position of the contact point on the first shape's surface. /// /// This is the point on shape 1 that is closest to (or penetrating into) shape 2. /// Expressed in the same coordinate system as the contact (world or local space). - pub point1: Point, + pub point1: Vector, /// Position of the contact point on the second shape's surface. /// /// This is the point on shape 2 that is closest to (or penetrating into) shape 1. /// When shapes are penetrating, this point may be inside shape 1. - pub point2: Point, + pub point2: Vector, /// Contact normal pointing outward from the first shape. /// /// This unit vector points from shape 1 toward shape 2, perpendicular to the /// contact surface. Used to compute separation direction and collision response /// forces for shape 1. - pub normal1: Unit>, + pub normal1: Vector, /// Contact normal pointing outward from the second shape. /// /// In world space, this is always equal to `-normal1`. In local space coordinates, /// it may differ due to different shape orientations. - pub normal2: Unit>, + pub normal2: Vector, /// Signed distance between the two contact points. /// @@ -120,13 +115,13 @@ impl Contact { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::Contact; - /// use nalgebra::{Point3, Unit, Vector3}; + /// use parry3d::math::Vector; /// /// // Create a contact representing two spheres touching - /// let point1 = Point3::new(1.0, 0.0, 0.0); - /// let point2 = Point3::new(2.0, 0.0, 0.0); - /// let normal1 = Unit::new_normalize(Vector3::new(1.0, 0.0, 0.0)); - /// let normal2 = Unit::new_normalize(Vector3::new(-1.0, 0.0, 0.0)); + /// let point1 = Vector::new(1.0, 0.0, 0.0); + /// let point2 = Vector::new(2.0, 0.0, 0.0); + /// let normal1 = Vector::new(1.0, 0.0, 0.0).normalize(); + /// let normal2 = Vector::new(-1.0, 0.0, 0.0).normalize(); /// /// let contact = Contact::new(point1, point2, normal1, normal2, 0.0); /// assert_eq!(contact.dist, 0.0); // Touching, not penetrating @@ -134,10 +129,10 @@ impl Contact { /// ``` #[inline] pub fn new( - point1: Point, - point2: Point, - normal1: Unit>, - normal2: Unit>, + point1: Vector, + point2: Vector, + normal1: Vector, + normal2: Vector, dist: Real, ) -> Self { Contact { @@ -168,16 +163,16 @@ impl Contact { /// Transform the points and normals from this contact by /// the given transformations. #[inline] - pub fn transform_by_mut(&mut self, pos1: &Isometry, pos2: &Isometry) { + pub fn transform_by_mut(&mut self, pos1: &Pose, pos2: &Pose) { self.point1 = pos1 * self.point1; self.point2 = pos2 * self.point2; - self.normal1 = pos1 * self.normal1; - self.normal2 = pos2 * self.normal2; + self.normal1 = pos1.rotation * self.normal1; + self.normal2 = pos2.rotation * self.normal2; } /// Transform `self.point1` and `self.normal1` by the `pos`. - pub fn transform1_by_mut(&mut self, pos: &Isometry) { + pub fn transform1_by_mut(&mut self, pos: &Pose) { self.point1 = pos * self.point1; - self.normal1 = pos * self.normal1; + self.normal1 = pos.rotation * self.normal1; } } diff --git a/src/query/contact/contact_ball_ball.rs b/src/query/contact/contact_ball_ball.rs index c4373592..e365334f 100644 --- a/src/query/contact/contact_ball_ball.rs +++ b/src/query/contact/contact_ball_ball.rs @@ -1,40 +1,34 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{ComplexField, Pose, Real, Vector}; use crate::query::Contact; use crate::shape::Ball; -use na::{self, ComplexField, Unit}; use num::Zero; /// Contact between balls. #[inline] -pub fn contact_ball_ball( - pos12: &Isometry, - b1: &Ball, - b2: &Ball, - prediction: Real, -) -> Option { +pub fn contact_ball_ball(pos12: &Pose, b1: &Ball, b2: &Ball, prediction: Real) -> Option { let r1 = b1.radius; let r2 = b2.radius; - let center2_1 = pos12.translation.vector; - let distance_squared = center2_1.norm_squared(); + let center2_1 = pos12.translation; + let distance_squared = center2_1.length_squared(); let sum_radius = r1 + r2; let sum_radius_with_error = sum_radius + prediction; if distance_squared < sum_radius_with_error * sum_radius_with_error { let normal1 = if !distance_squared.is_zero() { - Unit::new_normalize(center2_1) + (center2_1).normalize() } else { - Vector::x_axis() + Vector::X }; - let normal2 = -pos12.inverse_transform_unit_vector(&normal1); - let point1 = Point::from(*normal1 * r1); - let point2 = Point::from(*normal2 * r2); + let normal2 = -(pos12.rotation.inverse() * normal1); + let point1 = normal1 * r1; + let point2 = normal2 * r2; Some(Contact::new( point1, point2, normal1, normal2, - ComplexField::sqrt(distance_squared) - sum_radius, + ::sqrt(distance_squared) - sum_radius, )) } else { None diff --git a/src/query/contact/contact_ball_convex_polyhedron.rs b/src/query/contact/contact_ball_convex_polyhedron.rs index 15786941..da3fde53 100644 --- a/src/query/contact/contact_ball_convex_polyhedron.rs +++ b/src/query/contact/contact_ball_convex_polyhedron.rs @@ -1,16 +1,14 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::Contact; use crate::shape::{Ball, Shape}; -use na::{self, Unit}; - /// Contact between a ball and a convex polyhedron. /// /// This function panics if the input shape does not implement /// both the ConvexPolyhedron and PointQuery traits. #[inline] pub fn contact_ball_convex_polyhedron( - pos12: &Isometry, + pos12: &Pose, ball1: &Ball, shape2: &(impl Shape + ?Sized), prediction: Real, @@ -24,19 +22,18 @@ pub fn contact_ball_convex_polyhedron( /// both the ConvexPolyhedron and PointQuery traits. #[inline] pub fn contact_convex_polyhedron_ball( - pos12: &Isometry, + pos12: &Pose, shape1: &(impl Shape + ?Sized), ball2: &Ball, prediction: Real, ) -> Option { - let center2_1 = Point::from(pos12.translation.vector); - let (proj, f1) = shape1.project_local_point_and_get_feature(¢er2_1); + let center2_1 = pos12.translation; + let (proj, f1) = shape1.project_local_point_and_get_feature(center2_1); let dist; let normal1; - if let Some((dir1, len)) = - Unit::try_new_and_get(proj.point - center2_1, crate::math::DEFAULT_EPSILON) - { + let (dir1, len) = (proj.point - center2_1).normalize_and_length(); + if len >= crate::math::DEFAULT_EPSILON { if proj.is_inside { dist = -len - ball2.radius; normal1 = dir1; @@ -47,14 +44,14 @@ pub fn contact_convex_polyhedron_ball( } else { dist = -ball2.radius; normal1 = shape1 - .feature_normal_at_point(f1, &proj.point) - .or_else(|| Unit::try_new(proj.point.coords, crate::math::DEFAULT_EPSILON)) - .unwrap_or_else(Vector::y_axis); + .feature_normal_at_point(f1, proj.point) + .or_else(|| (proj.point).try_normalize()) + .unwrap_or(Vector::Y); } if dist <= prediction { - let normal2 = pos12.inverse_transform_unit_vector(&-normal1); - let point2 = Point::from(*normal2 * ball2.radius); + let normal2 = pos12.rotation.inverse() * -normal1; + let point2 = normal2 * ball2.radius; let point1 = proj.point; return Some(Contact::new(point1, point2, normal1, normal2, dist)); } diff --git a/src/query/contact/contact_composite_shape_shape.rs b/src/query/contact/contact_composite_shape_shape.rs index 2f84b4a9..6012e76e 100644 --- a/src/query/contact/contact_composite_shape_shape.rs +++ b/src/query/contact/contact_composite_shape_shape.rs @@ -1,8 +1,8 @@ use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{Contact, QueryDispatcher}; use crate::shape::{CompositeShape, CompositeShapeRef, Shape}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { /// Returns the closest/deepest contact between `self` and the given `shape2` positioned at @@ -14,7 +14,7 @@ impl CompositeShapeRef<'_, S> { pub fn contact_with_shape( &self, dispatcher: &D, - pose12: &Isometry, + pose12: &Pose, shape2: &dyn Shape, prediction: Real, ) -> Option<(u32, Contact)> { @@ -45,7 +45,7 @@ impl CompositeShapeRef<'_, S> { /// Best contact between a composite shape (`Mesh`, `Compound`) and any other shape. pub fn contact_composite_shape_shape( dispatcher: &D, - pose12: &Isometry, + pose12: &Pose, g1: &G1, g2: &dyn Shape, prediction: Real, @@ -62,7 +62,7 @@ where /// Best contact between a shape and a composite (`Mesh`, `Compound`) shape. pub fn contact_shape_composite_shape( dispatcher: &D, - pose12: &Isometry, + pose12: &Pose, g1: &dyn Shape, g2: &G2, prediction: Real, diff --git a/src/query/contact/contact_cuboid_cuboid.rs b/src/query/contact/contact_cuboid_cuboid.rs index d6a19557..e75fbf8c 100644 --- a/src/query/contact/contact_cuboid_cuboid.rs +++ b/src/query/contact/contact_cuboid_cuboid.rs @@ -1,13 +1,12 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{sat, Contact, PointQuery}; use crate::shape::{Cuboid, SupportMap}; use approx::AbsDiffEq; -use na::Unit; /// Contact between two cuboids. #[inline] pub fn contact_cuboid_cuboid( - pos12: &Isometry, + pos12: &Pose, cuboid1: &Cuboid, cuboid2: &Cuboid, prediction: Real, @@ -25,7 +24,7 @@ pub fn contact_cuboid_cuboid( } #[cfg(feature = "dim2")] - let sep3 = (-Real::MAX, crate::math::Vector::::y()); // This case does not exist in 2D. + let sep3 = (-Real::MAX, crate::math::Vector::Y); // This case does not exist in 2D. #[cfg(feature = "dim3")] let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pos12); if sep3.0 > prediction { @@ -37,27 +36,18 @@ pub fn contact_cuboid_cuboid( // To compute the closest points, we need to project the support point // from cuboid2 on the support-face of cuboid1. For simplicity, we just // project the support point from cuboid2 on cuboid1 itself (not just the face). - let pt2_1 = cuboid2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, false); + let pt2_1 = cuboid2.support_point(pos12, -sep1.1); + let proj1 = cuboid1.project_local_point(pt2_1, false); - let separation = (pt2_1 - proj1.point).dot(&sep1.1); - let normalized_dir = Unit::try_new_and_get(pt2_1 - proj1.point, Real::default_epsilon()); - let normal1; - let dist; + let separation = (pt2_1 - proj1.point).dot(sep1.1); + let (mut normal1, mut dist) = (pt2_1 - proj1.point).normalize_and_length(); // NOTE: we had to recompute the normal because we can't use // the separation vector for the case where we have a vertex-vertex contact. - if separation < 0.0 || normalized_dir.is_none() { + if separation < 0.0 || dist <= Real::default_epsilon() { // Penetration or contact lying on the boundary exactly. - normal1 = Unit::new_unchecked(sep1.1); + normal1 = sep1.1; dist = separation; - } else { - let Some((dir, norm)) = normalized_dir else { - unreachable!() - }; - // No penetration. - normal1 = dir; - dist = norm; } if dist > prediction { @@ -66,9 +56,9 @@ pub fn contact_cuboid_cuboid( return Some(Contact::new( proj1.point, - pos12.inverse_transform_point(&pt2_1), + pos12.inverse_transform_point(pt2_1), normal1, - pos12.inverse_transform_unit_vector(&-normal1), + pos12.rotation.inverse() * -normal1, dist, )); } @@ -78,27 +68,18 @@ pub fn contact_cuboid_cuboid( // To compute the actual closest points, we need to project the support point // from cuboid1 on the support-face of cuboid2. For simplicity, we just // project the support point from cuboid1 on cuboid2 itself (not just the face). - let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = cuboid2.project_local_point(&pt1_2, false); + let pt1_2 = cuboid1.support_point(&pos21, -sep2.1); + let proj2 = cuboid2.project_local_point(pt1_2, false); - let separation = (pt1_2 - proj2.point).dot(&sep2.1); - let normalized_dir = Unit::try_new_and_get(pt1_2 - proj2.point, Real::default_epsilon()); - let normal2; - let dist; + let separation = (pt1_2 - proj2.point).dot(sep2.1); + let (mut normal2, mut dist) = (pt1_2 - proj2.point).normalize_and_length(); // NOTE: we had to recompute the normal because we can't use // the separation vector for the case where we have a vertex-vertex contact. - if separation < 0.0 || normalized_dir.is_none() { + if separation < 0.0 || dist <= Real::default_epsilon() { // Penetration or contact lying on the boundary exactly. - normal2 = Unit::new_unchecked(sep2.1); + normal2 = sep2.1; dist = separation; - } else { - // No penetration. - let Some((dir, norm)) = normalized_dir else { - unreachable!() - }; - normal2 = dir; - dist = norm; } if dist > prediction { @@ -106,9 +87,9 @@ pub fn contact_cuboid_cuboid( } return Some(Contact::new( - pos12.transform_point(&pt1_2), + pos12 * pt1_2, proj2.point, - pos12 * -normal2, + pos12.rotation * -normal2, normal2, dist, )); @@ -121,13 +102,13 @@ pub fn contact_cuboid_cuboid( // To compute the actual distance, we need to compute the closest // points between the two edges that generated the separating axis. let edge1 = cuboid1.local_support_edge_segment(sep3.1); - let edge2 = cuboid2.local_support_edge_segment(pos21 * -sep3.1); + let edge2 = cuboid2.local_support_edge_segment(pos21.rotation * -sep3.1); match details::closest_points_segment_segment(pos12, &edge1, &edge2, prediction) { ClosestPoints::Disjoint => return None, ClosestPoints::WithinMargin(a, b) => { - let normal1 = Unit::new_unchecked(sep3.1); - let normal2 = pos12.inverse_transform_unit_vector(&-normal1); + let normal1 = sep3.1; + let normal2 = pos12.rotation.inverse() * -normal1; return Some(Contact::new(a, b, normal1, normal2, sep3.0)); } ClosestPoints::Intersecting => unreachable!(), diff --git a/src/query/contact/contact_halfspace_support_map.rs b/src/query/contact/contact_halfspace_support_map.rs index 9d8f40fa..7e82036b 100644 --- a/src/query/contact/contact_halfspace_support_map.rs +++ b/src/query/contact/contact_halfspace_support_map.rs @@ -1,21 +1,21 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::Contact; use crate::shape::{HalfSpace, SupportMap}; /// Contact between a halfspace and a support-mapped shape (Cuboid, ConvexHull, etc.) pub fn contact_halfspace_support_map( - pos12: &Isometry, + pos12: &Pose, halfspace: &HalfSpace, other: &G, prediction: Real, ) -> Option { - let deepest = other.support_point_toward(pos12, &-halfspace.normal); - let distance = halfspace.normal.dot(&deepest.coords); + let deepest = other.support_point_toward(pos12, -halfspace.normal); + let distance = halfspace.normal.dot(deepest); if distance <= prediction { - let point1 = deepest - halfspace.normal.into_inner() * distance; - let point2 = pos12.inverse_transform_point(&deepest); - let normal2 = pos12.inverse_transform_unit_vector(&-halfspace.normal); + let point1 = deepest - halfspace.normal * distance; + let point2 = pos12.inverse_transform_point(deepest); + let normal2 = pos12.rotation.inverse() * -halfspace.normal; Some(Contact::new( point1, @@ -31,7 +31,7 @@ pub fn contact_halfspace_support_map( /// Contact between a support-mapped shape (Cuboid, ConvexHull, etc.) and a halfspace. pub fn contact_support_map_halfspace( - pos12: &Isometry, + pos12: &Pose, other: &G, halfspace: &HalfSpace, prediction: Real, diff --git a/src/query/contact/contact_shape_shape.rs b/src/query/contact/contact_shape_shape.rs index 00599d6c..aa2201b1 100644 --- a/src/query/contact/contact_shape_shape.rs +++ b/src/query/contact/contact_shape_shape.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{Contact, DefaultQueryDispatcher, QueryDispatcher, Unsupported}; use crate::shape::Shape; @@ -61,14 +61,14 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::contact; /// use parry3d::shape::{Ball, Cuboid}; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball = Ball::new(1.0); -/// let cuboid = Cuboid::new(Vector3::new(2.0, 2.0, 2.0)); +/// let cuboid = Cuboid::new(Vector::new(2.0, 2.0, 2.0)); /// /// // Position shapes so they're penetrating -/// let pos_ball = Isometry3::translation(2.5, 0.0, 0.0); -/// let pos_cuboid = Isometry3::identity(); +/// let pos_ball = Pose::translation(2.5, 0.0, 0.0); +/// let pos_cuboid = Pose::identity(); /// /// // Compute contact (no prediction distance) /// if let Ok(Some(contact)) = contact(&pos_ball, &ball, &pos_cuboid, &cuboid, 0.0) { @@ -92,14 +92,14 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::contact; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Balls separated by 2.2 units (just outside contact range) -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(4.2, 0.0, 0.0); // radii sum = 2.0, gap = 2.2 +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(4.2, 0.0, 0.0); // radii sum = 2.0, gap = 2.2 /// /// // Without prediction: no contact /// assert!(contact(&pos1, &ball1, &pos2, &ball2, 0.0).unwrap().is_none()); @@ -121,9 +121,9 @@ use crate::shape::Shape; /// - [`closest_points`](crate::query::closest_points::closest_points()) - For closest point locations /// - [`intersection_test`](crate::query::intersection_test::intersection_test()) - For boolean overlap test pub fn contact( - pos1: &Isometry, + pos1: &Pose, g1: &dyn Shape, - pos2: &Isometry, + pos2: &Pose, g2: &dyn Shape, prediction: Real, ) -> Result, Unsupported> { diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 831ba246..3946010a 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/src/query/contact/contact_support_map_support_map.rs @@ -1,14 +1,12 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::epa::EPA; -use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex}; +use crate::query::gjk::{self, CsoPoint, GJKResult, VoronoiSimplex}; use crate::query::Contact; use crate::shape::SupportMap; -use na::Unit; - /// Contact between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.) pub fn contact_support_map_support_map( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, prediction: Real, @@ -20,9 +18,9 @@ where let simplex = &mut VoronoiSimplex::new(); match contact_support_map_support_map_with_params(pos12, g1, g2, prediction, simplex, None) { GJKResult::ClosestPoints(point1, point2_1, normal1) => { - let dist = (point2_1 - point1).dot(&normal1); - let point2 = pos12.inverse_transform_point(&point2_1); - let normal2 = pos12.inverse_transform_unit_vector(&-normal1); + let dist = (point2_1 - point1).dot(normal1); + let point2 = pos12.inverse_transform_point(point2_1); + let normal2 = pos12.rotation.inverse() * -normal1; Some(Contact::new(point1, point2, normal1, normal2, dist)) } GJKResult::NoIntersection(_) => None, @@ -38,12 +36,12 @@ where /// subsequent executions of the algorithm. It is also the contact /// normal (that points toward the outside of the first solid). pub fn contact_support_map_support_map_with_params( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, prediction: Real, simplex: &mut VoronoiSimplex, - init_dir: Option>>, + init_dir: Option, ) -> GJKResult where G1: ?Sized + SupportMap, @@ -51,15 +49,13 @@ where { let dir = if let Some(init_dir) = init_dir { init_dir - } else if let Some(init_dir) = - Unit::try_new(pos12.translation.vector, crate::math::DEFAULT_EPSILON) - { + } else if let Some(init_dir) = (pos12.translation).try_normalize() { init_dir } else { - Vector::x_axis() + Vector::X }; - simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir)); + simplex.reset(CsoPoint::from_shapes(pos12, g1, g2, dir)); let cpts = gjk::closest_points(pos12, g1, g2, prediction, true, simplex); if cpts != GJKResult::Intersection { @@ -73,5 +69,5 @@ where } // Everything failed - GJKResult::NoIntersection(Vector::x_axis()) + GJKResult::NoIntersection(Vector::X) } diff --git a/src/query/contact_manifolds/contact_manifold.rs b/src/query/contact_manifolds/contact_manifold.rs index c106fcdc..ecbf0d5c 100644 --- a/src/query/contact_manifolds/contact_manifold.rs +++ b/src/query/contact_manifolds/contact_manifold.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::PackedFeatureId; #[cfg(feature = "dim3")] use alloc::vec::Vec; @@ -7,8 +7,7 @@ use alloc::vec::Vec; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] /// A single contact point between two shapes. /// @@ -46,12 +45,12 @@ use alloc::vec::Vec; /// use parry3d::query::{ContactManifold, TrackedContact}; /// use parry3d::query::details::contact_manifold_ball_ball; /// use parry3d::shape::Ball; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// // Two balls, one slightly overlapping the other /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos12 = Isometry::translation(1.5, 0.0, 0.0); // Overlapping by 0.5 +/// let pos12 = Pose::translation(1.5, 0.0, 0.0); // Overlapping by 0.5 /// /// let mut manifold = ContactManifold::<(), ()>::new(); /// contact_manifold_ball_ball(&pos12, &ball1, &ball2, 0.0, &mut manifold); @@ -71,14 +70,14 @@ use alloc::vec::Vec; /// use parry3d::query::{ContactManifold, TrackedContact}; /// use parry3d::query::details::contact_manifold_ball_ball; /// use parry3d::shape::Ball; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Position shapes in world space -/// let pos1 = Isometry::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry::translation(1.5, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(1.5, 0.0, 0.0); /// let pos12 = pos1.inverse() * pos2; // Relative position /// /// let mut manifold = ContactManifold::<(), ()>::new(); @@ -110,13 +109,13 @@ pub struct TrackedContact { /// /// This is the point on the first shape's surface (or interior if penetrating) /// that is closest to or in contact with the second shape. - pub local_p1: Point, + pub local_p1: Vector, /// The contact point in the local-space of the second shape. /// /// This is the point on the second shape's surface (or interior if penetrating) /// that is closest to or in contact with the first shape. - pub local_p2: Point, + pub local_p2: Vector, /// The signed distance between the two contact points. /// @@ -169,11 +168,11 @@ impl TrackedContact { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::TrackedContact; /// use parry3d::shape::PackedFeatureId; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let contact = TrackedContact::<()>::new( - /// Point::new(1.0, 0.0, 0.0), // Point on shape 1 - /// Point::new(-1.0, 0.0, 0.0), // Point on shape 2 + /// Vector::new(1.0, 0.0, 0.0), // Point on shape 1 + /// Vector::new(-1.0, 0.0, 0.0), // Point on shape 2 /// PackedFeatureId::face(0), // Face 0 of shape 1 /// PackedFeatureId::face(0), // Face 0 of shape 2 /// -0.1, // Penetration depth of 0.1 @@ -183,8 +182,8 @@ impl TrackedContact { /// # } /// ``` pub fn new( - local_p1: Point, - local_p2: Point, + local_p1: Vector, + local_p2: Vector, fid1: PackedFeatureId, fid2: PackedFeatureId, dist: Real, @@ -201,8 +200,8 @@ impl TrackedContact { /// Creates a new tracked contact where its input may need to be flipped. pub fn flipped( - local_p1: Point, - local_p2: Point, + local_p1: Vector, + local_p2: Vector, fid1: PackedFeatureId, fid2: PackedFeatureId, dist: Real, @@ -270,14 +269,14 @@ impl TrackedContact { /// use parry3d::query::{ContactManifold, TrackedContact}; /// use parry3d::query::details::contact_manifold_ball_ball; /// use parry3d::shape::Ball; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// // Create two balls /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Position them so they overlap -/// let pos12 = Isometry::translation(1.5, 0.0, 0.0); // Overlapping by 0.5 +/// let pos12 = Pose::translation(1.5, 0.0, 0.0); // Overlapping by 0.5 /// /// // Create an empty manifold /// let mut manifold = ContactManifold::<(), ()>::new(); @@ -306,13 +305,13 @@ impl TrackedContact { /// use parry3d::query::ContactManifold; /// use parry3d::query::details::contact_manifold_ball_ball; /// use parry3d::shape::Ball; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Balls are separated by 0.1 -/// let pos12 = Isometry::translation(2.1, 0.0, 0.0); +/// let pos12 = Pose::translation(2.1, 0.0, 0.0); /// /// let mut manifold = ContactManifold::<(), ()>::new(); /// @@ -337,19 +336,19 @@ impl TrackedContact { /// use parry3d::query::ContactManifold; /// use parry3d::query::details::contact_manifold_ball_ball; /// use parry3d::shape::Ball; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// let mut manifold = ContactManifold::<(), ()>::new(); /// /// // Frame 1: Initial contact -/// let pos12_frame1 = Isometry::translation(1.9, 0.0, 0.0); +/// let pos12_frame1 = Pose::translation(1.9, 0.0, 0.0); /// contact_manifold_ball_ball(&pos12_frame1, &ball1, &ball2, 0.1, &mut manifold); /// println!("Frame 1: {} contacts", manifold.points.len()); /// /// // Frame 2: Small movement - try to update efficiently -/// let pos12_frame2 = Isometry::translation(1.85, 0.0, 0.0); +/// let pos12_frame2 = Pose::translation(1.85, 0.0, 0.0); /// /// if manifold.try_update_contacts(&pos12_frame2) { /// println!("Successfully updated contacts using spatial coherence"); @@ -369,14 +368,14 @@ impl TrackedContact { /// use parry3d::query::ContactManifold; /// use parry3d::query::details::contact_manifold_cuboid_cuboid; /// use parry3d::shape::Cuboid; -/// use parry3d::math::{Isometry, Vector}; +/// use parry3d::math::{Pose, Vector}; /// /// // Two boxes /// let cuboid1 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let cuboid2 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// // One box sitting on top of another -/// let pos12 = Isometry::translation(0.0, 1.9, 0.0); // Slight overlap +/// let pos12 = Pose::translation(0.0, 1.9, 0.0); // Slight overlap /// /// let mut manifold = ContactManifold::<(), ()>::new(); /// contact_manifold_cuboid_cuboid(&pos12, &cuboid1, &cuboid2, 0.0, &mut manifold); @@ -405,7 +404,7 @@ impl TrackedContact { /// use parry3d::query::ContactManifold; /// use parry3d::query::details::contact_manifold_ball_ball; /// use parry3d::shape::Ball; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// // Custom data structures /// #[derive(Clone, Default, Copy)] @@ -422,7 +421,7 @@ impl TrackedContact { /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos12 = Isometry::translation(1.8, 0.0, 0.0); +/// let pos12 = Pose::translation(1.8, 0.0, 0.0); /// /// // Create manifold with custom data /// let manifold_data = MyManifoldData { @@ -482,9 +481,9 @@ pub struct ContactManifold { #[cfg(feature = "dim3")] pub points: Vec>, /// The contact normal of all the contacts of this manifold, expressed in the local space of the first shape. - pub local_n1: Vector, + pub local_n1: Vector, /// The contact normal of all the contacts of this manifold, expressed in the local space of the second shape. - pub local_n2: Vector, + pub local_n2: Vector, /// The first subshape involved in this contact manifold. /// /// This is zero if the first shape is not a composite shape. @@ -495,10 +494,10 @@ pub struct ContactManifold { pub subshape2: u32, /// If the first shape involved is a composite shape, this contains the position of its subshape /// involved in this contact. - pub subshape_pos1: Option>, + pub subshape_pos1: Option, /// If the second shape involved is a composite shape, this contains the position of its subshape /// involved in this contact. - pub subshape_pos2: Option>, + pub subshape_pos2: Option, /// Additional tracked data associated to this contact manifold. pub data: ManifoldData, } @@ -519,8 +518,8 @@ impl ContactManifold ContactManifold::new(); /// contact_manifold_ball_ball(&pos12, &ball1, &ball2, 0.0, &mut manifold); @@ -627,18 +626,18 @@ impl ContactManifold::new(); /// /// // Initial computation - /// let pos12_old = Isometry::translation(1.9, 0.0, 0.0); + /// let pos12_old = Pose::translation(1.9, 0.0, 0.0); /// contact_manifold_ball_ball(&pos12_old, &ball1, &ball2, 0.1, &mut manifold); /// /// // Next frame: shapes moved slightly - /// let pos12_new = Isometry::translation(1.85, 0.05, 0.0); + /// let pos12_new = Pose::translation(1.85, 0.05, 0.0); /// /// if manifold.try_update_contacts(&pos12_new) { /// println!("Updated contacts efficiently!"); @@ -649,7 +648,7 @@ impl ContactManifold) -> bool { + pub fn try_update_contacts(&mut self, pos12: &Pose) -> bool { // const DOT_THRESHOLD: Real = 0.crate::COS_10_DEGREES; // const DOT_THRESHOLD: Real = crate::utils::COS_5_DEGREES; const DOT_THRESHOLD: Real = crate::utils::COS_1_DEGREES; @@ -661,7 +660,7 @@ impl ContactManifold, + pos12: &Pose, angle_dot_threshold: Real, dist_sq_threshold: Real, ) -> bool { @@ -669,16 +668,16 @@ impl ContactManifold ContactManifold dist_sq_threshold { + if pt.local_p1.distance_squared(new_local_p1) > dist_sq_threshold { return false; } @@ -722,7 +721,7 @@ impl ContactManifold ContactManifold::new(); /// /// // Frame 1: Compute contacts - /// let pos12_frame1 = Isometry::translation(1.9, 0.0, 0.0); + /// let pos12_frame1 = Pose::translation(1.9, 0.0, 0.0); /// contact_manifold_ball_ball(&pos12_frame1, &ball1, &ball2, 0.0, &mut manifold); /// /// // Simulate physics, accumulate impulse @@ -746,7 +745,7 @@ impl ContactManifold ContactManifold ContactManifold::new(); /// contact_manifold_cuboid_cuboid(&pos12, &cuboid1, &cuboid2, 0.0, &mut manifold); diff --git a/src/query/contact_manifolds/contact_manifolds_ball_ball.rs b/src/query/contact_manifolds/contact_manifolds_ball_ball.rs index 2cbcd761..7733a8d5 100644 --- a/src/query/contact_manifolds/contact_manifolds_ball_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_ball_ball.rs @@ -1,10 +1,10 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::{ContactManifold, TrackedContact}; use crate::shape::{Ball, PackedFeatureId, Shape}; /// Computes the contact manifold between two balls given as `Shape` trait-objects. pub fn contact_manifold_ball_ball_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -17,7 +17,7 @@ pub fn contact_manifold_ball_ball_shapes( - pos12: &Isometry, + pos12: &Pose, ball1: &Ball, ball2: &Ball, prediction: Real, @@ -26,22 +26,22 @@ pub fn contact_manifold_ball_ball( let radius_a = ball1.radius; let radius_b = ball2.radius; - let dcenter = pos12.translation.vector; - let center_dist = dcenter.magnitude(); + let dcenter = pos12.translation; + let center_dist = dcenter.length(); let dist = center_dist - radius_a - radius_b; if dist < prediction { let local_n1 = if center_dist != 0.0 { dcenter / center_dist } else { - Vector::y() + Vector::Y }; - let local_n2 = pos12.inverse_transform_vector(&-local_n1); + let local_n2 = pos12.rotation.inverse() * -local_n1; let local_p1 = local_n1 * radius_a; let local_p2 = local_n2 * radius_b; let fid = PackedFeatureId::face(0); - let contact = TrackedContact::new(local_p1.into(), local_p2.into(), fid, fid, dist); + let contact = TrackedContact::new(local_p1, local_p2, fid, fid, dist); if !manifold.points.is_empty() { manifold.points[0].copy_geometry_from(contact); diff --git a/src/query/contact_manifolds/contact_manifolds_capsule_capsule.rs b/src/query/contact_manifolds/contact_manifolds_capsule_capsule.rs index 8c945383..90dea435 100644 --- a/src/query/contact_manifolds/contact_manifolds_capsule_capsule.rs +++ b/src/query/contact_manifolds/contact_manifolds_capsule_capsule.rs @@ -1,14 +1,14 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::{ContactManifold, TrackedContact}; #[cfg(feature = "dim2")] use crate::shape::SegmentPointLocation; use crate::shape::{Capsule, PackedFeatureId, Shape}; +#[cfg(feature = "dim2")] use approx::AbsDiffEq; -use na::Unit; /// Computes the contact manifold between two capsules given as `Shape` trait-objects. pub fn contact_manifold_capsule_capsule_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -24,7 +24,7 @@ pub fn contact_manifold_capsule_capsule_shapes( /// Computes the contact manifold between two capsules. #[cfg(feature = "dim2")] pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( - pos12: &Isometry, + pos12: &Pose, capsule1: &'a Capsule, capsule2: &'a Capsule, prediction: Real, @@ -57,16 +57,15 @@ pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; - let local_p2_1 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; + let local_p1 = seg1.a * bcoords1[0] + seg1.b * bcoords1[1]; + let local_p2_1 = seg2_1.a * bcoords2[0] + seg2_1.b * bcoords2[1]; - let local_n1 = - Unit::try_new(local_p2_1 - local_p1, Real::default_epsilon()).unwrap_or(Vector::y_axis()); - let dist = (local_p2_1 - local_p1).dot(&local_n1); + let local_n1 = (local_p2_1 - local_p1).try_normalize().unwrap_or(Vector::Y); + let dist = (local_p2_1 - local_p1).dot(local_n1); if dist <= prediction + capsule1.radius + capsule2.radius { - let local_n2 = pos12.inverse_transform_unit_vector(&-local_n1); - let local_p2 = pos12.inverse_transform_point(&local_p2_1); + let local_n2 = pos12.rotation.inverse() * -local_n1; + let local_p2 = pos12.inverse_transform_point(local_p2_1); let contact = TrackedContact::new( local_p1, local_p2, @@ -76,42 +75,42 @@ pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( ); manifold.points.push(contact); - manifold.local_n1 = *local_n1; - manifold.local_n2 = *local_n2; + manifold.local_n1 = local_n1; + manifold.local_n2 = local_n2; } else { // No contact within tolerance. return; } if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) { - if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 - && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 + if dir1.dot(dir2).abs() >= crate::utils::COS_FRAC_PI_8 + && dir1.dot(local_n1).abs() < crate::utils::SIN_FRAC_PI_8 { // Capsules axes are almost parallel and are almost perpendicular to the normal. // Find a second contact point. if let Some((clip_a, clip_b)) = crate::query::details::clip_segment_segment_with_normal( (seg1.a, seg1.b), (seg2_1.a, seg2_1.b), - *local_n1, + local_n1, ) { let contact = - if (clip_a.0 - local_p1).norm_squared() > Real::default_epsilon() * 100.0 { + if (clip_a.0 - local_p1).length_squared() > Real::default_epsilon() * 100.0 { // Use clip_a as the second contact. TrackedContact::new( clip_a.0, - pos12.inverse_transform_point(&clip_a.1), + pos12.inverse_transform_point(clip_a.1), PackedFeatureId::face(clip_a.2 as u32), PackedFeatureId::face(clip_a.3 as u32), - (clip_a.1 - clip_a.0).dot(&local_n1), + (clip_a.1 - clip_a.0).dot(local_n1), ) } else { // Use clip_b as the second contact. TrackedContact::new( clip_b.0, - pos12.inverse_transform_point(&clip_b.1), + pos12.inverse_transform_point(clip_b.1), PackedFeatureId::face(clip_b.2 as u32), PackedFeatureId::face(clip_b.3 as u32), - (clip_b.1 - clip_b.0).dot(&local_n1), + (clip_b.1 - clip_b.0).dot(local_n1), ) }; @@ -132,7 +131,7 @@ pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( /// Computes the contact manifold between two capsules. #[cfg(feature = "dim3")] pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( - pos12: &Isometry, + pos12: &Pose, capsule1: &'a Capsule, capsule2: &'a Capsule, prediction: Real, @@ -150,19 +149,18 @@ pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; - let local_p2_1 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; + let local_p1 = seg1.a * bcoords1[0] + seg1.b * bcoords1[1]; + let local_p2_1 = seg2_1.a * bcoords2[0] + seg2_1.b * bcoords2[1]; - let local_n1 = - Unit::try_new(local_p2_1 - local_p1, Real::default_epsilon()).unwrap_or(Vector::y_axis()); - let dist = (local_p2_1 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius; + let local_n1 = (local_p2_1 - local_p1).try_normalize().unwrap_or(Vector::Y); + let dist = (local_p2_1 - local_p1).dot(local_n1) - capsule1.radius - capsule2.radius; if dist <= prediction { - let local_n2 = pos12.inverse_transform_unit_vector(&-local_n1); + let local_n2 = pos12.rotation.inverse() * -local_n1; let fid = PackedFeatureId::face(0); let contact = TrackedContact::new( - local_p1 + *local_n1 * capsule1.radius, - pos12.inverse_transform_point(&local_p2_1) + *local_n2 * capsule2.radius, + local_p1 + local_n1 * capsule1.radius, + pos12.inverse_transform_point(local_p2_1) + local_n2 * capsule2.radius, fid, fid, dist, @@ -174,8 +172,8 @@ pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>( manifold.points.push(contact); } - manifold.local_n1 = *local_n1; - manifold.local_n2 = *local_n2; + manifold.local_n1 = local_n1; + manifold.local_n2 = local_n2; } else { manifold.clear(); } diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs index cef43c74..f6781130 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs @@ -2,7 +2,7 @@ use alloc::boxed::Box; use alloc::vec::Vec; use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; @@ -11,13 +11,12 @@ use crate::query::query_dispatcher::PersistentQueryDispatcher; use crate::query::ContactManifold; use crate::shape::CompositeShape; use crate::utils::hashmap::{Entry, HashMap}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone)] struct SubDetector { @@ -57,7 +56,7 @@ fn ensure_workspace_exists(workspace: &mut Option) { /// Computes the contact manifolds between two composite shapes. pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, ContactData>( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, mut composite1: &'a dyn CompositeShape, mut composite2: &'a dyn CompositeShape, prediction: Real, @@ -85,7 +84,8 @@ pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, Conta let mut ls_aabb1 = bvh1.root_aabb(); let mut ls_aabb2 = bvh2.root_aabb(); - let flipped = ls_aabb1.half_extents().norm_squared() < ls_aabb2.half_extents().norm_squared(); + let flipped = + ls_aabb1.half_extents().length_squared() < ls_aabb2.half_extents().length_squared(); if flipped { core::mem::swap(&mut composite1, &mut composite2); diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs index 2585d82f..711fbd62 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs @@ -1,7 +1,7 @@ use alloc::{boxed::Box, vec::Vec}; use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; @@ -10,13 +10,12 @@ use crate::query::query_dispatcher::PersistentQueryDispatcher; use crate::query::ContactManifold; use crate::shape::{CompositeShape, Shape}; use crate::utils::hashmap::{Entry, HashMap}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone)] struct SubDetector { @@ -56,7 +55,7 @@ fn ensure_workspace_exists(workspace: &mut Option) { /// Computes the contact manifolds between a composite shape and an abstract shape. pub fn contact_manifolds_composite_shape_shape( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, composite1: &dyn CompositeShape, shape2: &dyn Shape, prediction: Real, diff --git a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs index 3fcbb772..f58e8d3d 100644 --- a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs @@ -1,12 +1,11 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; use crate::query::{ContactManifold, Ray, TrackedContact}; use crate::shape::{Ball, PackedFeatureId, Shape}; -use na::Unit; /// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object. pub fn contact_manifold_convex_ball_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, normal_constraints1: Option<&dyn NormalConstraints>, @@ -43,7 +42,7 @@ pub fn contact_manifold_convex_ball_shapes( /// Computes the contact manifold between a convex shape and a ball. pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( - pos12: &Isometry, + pos12: &Pose, shape1: &'a S1, ball2: &'a Ball, normal_constraints1: Option<&dyn NormalConstraints>, @@ -55,19 +54,24 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( S1: ?Sized + Shape, ContactData: Default + Copy, { - let local_p2_1 = Point::from(pos12.translation.vector); - let (proj, mut fid1) = shape1.project_local_point_and_get_feature(&local_p2_1); + let local_p2_1 = pos12.translation; + let (proj, mut fid1) = shape1.project_local_point_and_get_feature(local_p2_1); let mut local_p1 = proj.point; let dpos = local_p2_1 - local_p1; // local_n1 points from the surface towards our origin if defined, otherwise from the other // shape's origin towards our origin if defined, otherwise towards +x - let (mut local_n1, mut dist) = Unit::try_new_and_get(dpos, 0.0).unwrap_or_else(|| { - ( - Unit::try_new(pos12.translation.vector, 0.0).unwrap_or_else(Vector::x_axis), - 0.0, - ) - }); + // NOTE: we used `dpos.normalize_and_length()` here before. But since `normalize_and_length()` + // multiplying by the reciprocal (1.0 / length) instead of directly dividing by `length`, + // it can introduce tiny numerical errors that result in drift breaking perfectly verticality + // of contact normals for, e.g., the 3D spring joint demo in rapier. So we divide ourselves + // instead. + let mut dist = dpos.length(); + let mut local_n1 = if dist == 0.0 { + pos12.translation.normalize_or(Vector::Y) + } else { + dpos / dist + }; if proj.is_inside { local_n1 = -local_n1; @@ -75,12 +79,12 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( } if dist <= ball2.radius + prediction { - let mut local_n2 = pos12.inverse_transform_vector(&-*local_n1); + let mut local_n2 = pos12.rotation.inverse() * -local_n1; let uncorrected_local_n2 = local_n2; if !(normal_constraints1, normal_constraints2).project_local_normals( pos12, - local_n1.as_mut_unchecked(), + &mut local_n1, &mut local_n2, ) { // The contact got completely discarded by the normal correction. @@ -88,17 +92,13 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( return; } - let local_p2 = (local_n2 * ball2.radius).into(); + let local_p2 = local_n2 * ball2.radius; // If a correction happened, adjust the contact point on the first body. if uncorrected_local_n2 != local_n2 { let ray1 = Ray::new( - pos12.translation.vector.into(), - if proj.is_inside { - *local_n1 - } else { - -*local_n1 - }, + pos12.translation, + if proj.is_inside { local_n1 } else { -local_n1 }, ); if let Some(hit) = shape1.cast_local_ray_and_get_normal(&ray1, Real::MAX, false) { @@ -134,9 +134,9 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( if flipped { manifold.local_n1 = local_n2; - manifold.local_n2 = *local_n1; + manifold.local_n2 = local_n1; } else { - manifold.local_n1 = *local_n1; + manifold.local_n1 = local_n1; manifold.local_n2 = local_n2; } } else { diff --git a/src/query/contact_manifolds/contact_manifolds_cuboid_capsule.rs b/src/query/contact_manifolds/contact_manifolds_cuboid_capsule.rs index 0e814a79..8fd27a87 100644 --- a/src/query/contact_manifolds/contact_manifolds_cuboid_capsule.rs +++ b/src/query/contact_manifolds/contact_manifolds_cuboid_capsule.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::{sat, ContactManifold}; #[cfg(feature = "dim3")] use crate::shape::PolygonalFeature; @@ -8,7 +8,7 @@ use crate::shape::{CuboidFeature, CuboidFeatureFace}; /// Computes the contact manifold between a cuboid and a capsule, both represented as `Shape` trait-objects. pub fn contact_manifold_cuboid_capsule_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -41,8 +41,8 @@ pub fn contact_manifold_cuboid_capsule_shapes( /// Computes the contact manifold between a cuboid and a capsule. pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>( - pos12: &Isometry, - pos21: &Isometry, + pos12: &Pose, + pos21: &Pose, cube1: &'a Cuboid, capsule2: &'a Capsule, prediction: Real, @@ -61,7 +61,7 @@ pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>( /* * - * Point-Face cases. + * Vector-Face cases. * */ let sep1 = @@ -72,7 +72,7 @@ pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>( } #[cfg(feature = "dim3")] - let sep2 = (-Real::MAX, Vector::x()); + let sep2 = (-Real::MAX, Vector::X); #[cfg(feature = "dim2")] let sep2 = sat::segment_cuboid_find_local_separating_normal_oneway(&segment2, cube1, &pos21); if sep2.0 > prediction + capsule2.radius { @@ -86,7 +86,7 @@ pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>( * */ #[cfg(feature = "dim2")] - let sep3 = (-Real::MAX, Vector::x()); // This case does not exist in 2D. + let sep3 = (-Real::MAX, Vector::X); // This case does not exist in 2D. #[cfg(feature = "dim3")] let sep3 = sat::cuboid_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12); if sep3.0 > prediction + capsule2.radius { @@ -103,7 +103,7 @@ pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>( let mut best_sep = sep1; if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - best_sep = (sep2.0, pos12 * -sep2.1); + best_sep = (sep2.0, pos12.rotation * -sep2.1); } else if sep3.0 > sep1.0 { best_sep = sep3; } @@ -150,7 +150,7 @@ pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>( ); // Adjust points to take the radius into account. - let normal2 = pos21 * -best_sep.1; + let normal2 = pos21.rotation * -best_sep.1; if flipped { manifold.local_n1 = normal2; diff --git a/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs b/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs index bd8301ff..5b36bed6 100644 --- a/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs +++ b/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs @@ -1,12 +1,12 @@ #[cfg(feature = "dim2")] use crate::math::Vector; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{sat, ContactManifold}; use crate::shape::{Cuboid, PolygonalFeature, Shape}; /// Computes the contact manifold between two cuboids represented as `Shape` trait-objects. pub fn contact_manifold_cuboid_cuboid_shapes( - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, prediction: Real, @@ -19,7 +19,7 @@ pub fn contact_manifold_cuboid_cuboid_shapes( - pos12: &Isometry, + pos12: &Pose, cuboid1: &'a Cuboid, cuboid2: &'a Cuboid, prediction: Real, @@ -33,7 +33,7 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C /* * - * Point-Face + * Vector-Face * */ let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, pos12); @@ -54,7 +54,7 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C * */ #[cfg(feature = "dim2")] - let sep3 = (-Real::MAX, Vector::x()); // This case does not exist in 2D. + let sep3 = (-Real::MAX, Vector::X); // This case does not exist in 2D. #[cfg(feature = "dim3")] let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pos12); if sep3.0 > prediction { @@ -71,7 +71,7 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C let mut best_sep = sep1; if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - best_sep = (sep2.0, pos12 * -sep2.1); + best_sep = (sep2.0, pos12.rotation * -sep2.1); } else if sep3.0 > sep1.0 { best_sep = sep3; } @@ -81,7 +81,7 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C let old_manifold_points = manifold.points.clone(); manifold.clear(); - let local_n2 = pos21 * -best_sep.1; + let local_n2 = pos21.rotation * -best_sep.1; // Now the reference feature is from `cuboid1` and the best separation is `best_sep`. // Everything must be expressed in the local-space of `cuboid1` for contact clipping. @@ -89,14 +89,7 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C let feature2 = cuboid2.support_feature(local_n2); PolygonalFeature::contacts( - pos12, - pos21, - &best_sep.1, - &local_n2, - &feature1, - &feature2, - manifold, - false, + pos12, pos21, best_sep.1, local_n2, &feature1, &feature2, manifold, false, ); manifold.local_n1 = best_sep.1; diff --git a/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs b/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs index eb2baa30..cb1952f3 100644 --- a/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs +++ b/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs @@ -1,6 +1,6 @@ #[cfg(feature = "dim2")] use crate::math::Vector; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; use crate::query::{sat, ContactManifold}; use crate::shape::PolygonalFeature; @@ -8,7 +8,7 @@ use crate::shape::{Cuboid, Shape, Triangle}; /// Computes the contact manifold between a cuboid and a triangle represented as `Shape` trait-objects. pub fn contact_manifold_cuboid_triangle_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, normal_constraints1: Option<&dyn NormalConstraints>, @@ -47,8 +47,8 @@ pub fn contact_manifold_cuboid_triangle_shapes( /// Computes the contact manifold between a cuboid and a triangle. pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( - pos12: &Isometry, - pos21: &Isometry, + pos12: &Pose, + pos21: &Pose, cuboid1: &'a Cuboid, triangle2: &'a Triangle, normal_constraints1: Option<&dyn NormalConstraints>, @@ -67,7 +67,7 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( /* * - * Point-Face cases. + * Vector-Face cases. * */ let sep1 = @@ -89,7 +89,7 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( * */ #[cfg(feature = "dim2")] - let sep3 = (-Real::MAX, Vector::x()); // This case does not exist in 2D. + let sep3 = (-Real::MAX, Vector::X); // This case does not exist in 2D. #[cfg(feature = "dim3")] let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cuboid1, triangle2, pos12); if sep3.0 > prediction { @@ -107,7 +107,7 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( let mut dist = sep1.0; if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - normal1 = pos12 * -sep2.1; + normal1 = pos12.rotation * -sep2.1; dist = sep2.0; } else if sep3.0 > sep1.0 { normal1 = sep3.1; @@ -115,7 +115,7 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( } // Apply any normal constraint to the separating axis. - let mut normal2 = pos21 * -normal1; + let mut normal2 = pos21.rotation * -normal1; if !(normal_constraints1, normal_constraints2).project_local_normals( pos12, @@ -146,7 +146,7 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( manifold.clear(); PolygonalFeature::contacts( - pos12, pos21, &normal1, &normal2, &feature1, &feature2, manifold, flipped, + pos12, pos21, normal1, normal2, &feature1, &feature2, manifold, flipped, ); if normal_constraints1.is_some() || normal_constraints2.is_some() { diff --git a/src/query/contact_manifolds/contact_manifolds_halfspace_pfm.rs b/src/query/contact_manifolds/contact_manifolds_halfspace_pfm.rs index 7bcafe92..4c24a533 100644 --- a/src/query/contact_manifolds/contact_manifolds_halfspace_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_halfspace_pfm.rs @@ -1,10 +1,10 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{ContactManifold, TrackedContact}; use crate::shape::{HalfSpace, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape}; /// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object. pub fn contact_manifold_halfspace_pfm_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -41,7 +41,7 @@ pub fn contact_manifold_halfspace_pfm_shapes( /// Computes the contact manifold between a convex shape and a ball. pub fn contact_manifold_halfspace_pfm<'a, ManifoldData, ContactData, S2>( - pos12: &Isometry, + pos12: &Pose, halfspace1: &'a HalfSpace, pfm2: &'a S2, border_radius2: Real, @@ -52,9 +52,9 @@ pub fn contact_manifold_halfspace_pfm<'a, ManifoldData, ContactData, S2>( S2: ?Sized + PolygonalFeatureMap, ContactData: Default + Copy, { - let normal1_2 = pos12.inverse_transform_unit_vector(&halfspace1.normal); + let normal1_2 = pos12.rotation.inverse() * halfspace1.normal; let mut feature2 = PolygonalFeature::default(); - pfm2.local_support_feature(&-normal1_2, &mut feature2); + pfm2.local_support_feature(-normal1_2, &mut feature2); // We do this clone to perform contact tracking and transfer impulses. // TODO: find a more efficient way of doing this. @@ -63,13 +63,13 @@ pub fn contact_manifold_halfspace_pfm<'a, ManifoldData, ContactData, S2>( for i in 0..feature2.num_vertices { let vtx2 = feature2.vertices[i]; let vtx2_1 = pos12 * vtx2; - let dist_to_plane = vtx2_1.coords.dot(&halfspace1.normal); + let dist_to_plane = vtx2_1.dot(halfspace1.normal); if dist_to_plane - border_radius2 <= prediction { // Keep this contact point. manifold.points.push(TrackedContact::flipped( - vtx2_1 - *halfspace1.normal * dist_to_plane, - vtx2 - *normal1_2 * border_radius2, + vtx2_1 - halfspace1.normal * dist_to_plane, + vtx2 - normal1_2 * border_radius2, PackedFeatureId::face(0), feature2.vids[i], dist_to_plane - border_radius2, @@ -79,11 +79,11 @@ pub fn contact_manifold_halfspace_pfm<'a, ManifoldData, ContactData, S2>( } if flipped { - manifold.local_n1 = -*normal1_2; - manifold.local_n2 = *halfspace1.normal; + manifold.local_n1 = -normal1_2; + manifold.local_n2 = halfspace1.normal; } else { - manifold.local_n1 = *halfspace1.normal; - manifold.local_n2 = -*normal1_2; + manifold.local_n1 = halfspace1.normal; + manifold.local_n2 = -normal1_2; } // println!("Found contacts: {}", manifold.points.len()); diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs index 09607211..fd30c6c8 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs @@ -1,7 +1,7 @@ use alloc::{boxed::Box, vec::Vec}; use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; @@ -12,13 +12,12 @@ use crate::query::ContactManifold; use crate::shape::Capsule; use crate::shape::{CompositeShape, HeightField, Shape}; use crate::utils::hashmap::{Entry, HashMap}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone)] struct SubDetector { @@ -58,8 +57,8 @@ fn ensure_workspace_exists(workspace: &mut Option) { /// Computes the contact manifold between an heightfield and a composite shape. pub fn contact_manifolds_heightfield_composite_shape( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, - pos21: &Isometry, + pos12: &Pose, + pos21: &Pose, heightfield1: &HeightField, composite2: &dyn CompositeShape, prediction: Real, diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs index 4f3898c6..eebbe22a 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs @@ -1,7 +1,7 @@ use alloc::{boxed::Box, vec::Vec}; use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; @@ -16,8 +16,7 @@ use crate::utils::hashmap::{Entry, HashMap}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone)] struct SubDetector { @@ -41,7 +40,7 @@ impl HeightFieldShapeContactManifoldsWorkspace { /// Computes the contact manifold between an heightfield and a shape, both represented as `Shape` trait-objects. pub fn contact_manifolds_heightfield_shape_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -95,7 +94,7 @@ fn ensure_workspace_exists(workspace: &mut Option) { /// Computes the contact manifold between an heightfield and an abstract shape. pub fn contact_manifolds_heightfield_shape( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, heightfield1: &HeightField, shape2: &dyn Shape, prediction: Real, diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index bb13a838..9e97cca7 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real, Vector}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; use crate::query::{ self, @@ -6,12 +6,11 @@ use crate::query::{ ContactManifold, TrackedContact, }; use crate::shape::{PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, Shape}; -use na::Unit; /// Computes the contact manifold between two convex shapes implementing the `PolygonalSupportMap` /// trait, both represented as `Shape` trait-objects. pub fn contact_manifold_pfm_pfm_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, normal_constraints1: Option<&dyn NormalConstraints>, @@ -42,7 +41,7 @@ pub fn contact_manifold_pfm_pfm_shapes( /// Computes the contact manifold between two convex shapes implementing the `PolygonalSupportMap` trait. pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( - pos12: &Isometry, + pos12: &Pose, pfm1: &'a S1, border_radius1: Real, normal_constraints1: Option<&dyn NormalConstraints>, @@ -64,7 +63,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( return; } - let init_dir = Unit::try_new(manifold.local_n1, crate::math::DEFAULT_EPSILON); + let init_dir = (manifold.local_n1).try_normalize(); let total_prediction = prediction + border_radius1 + border_radius2; let contact = query::details::contact_support_map_support_map_with_params( pos12, @@ -81,13 +80,13 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( match contact { GJKResult::ClosestPoints(p1, p2_1, dir) => { let mut local_n1 = dir; - let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir); - let dist = (p2_1 - p1).dot(&local_n1); + let mut local_n2 = pos12.rotation.inverse() * -dir; + let dist = (p2_1 - p1).dot(local_n1); if !(normal_constraints1, normal_constraints2).project_local_normals( pos12, - local_n1.as_mut_unchecked(), - local_n2.as_mut_unchecked(), + &mut local_n1, + &mut local_n2, ) { // The contact got completely discarded by the normal correction. return; @@ -95,14 +94,14 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( let mut feature1 = PolygonalFeature::default(); let mut feature2 = PolygonalFeature::default(); - pfm1.local_support_feature(&local_n1, &mut feature1); - pfm2.local_support_feature(&local_n2, &mut feature2); + pfm1.local_support_feature(local_n1, &mut feature1); + pfm2.local_support_feature(local_n2, &mut feature2); PolygonalFeature::contacts( pos12, &pos12.inverse(), - &local_n1, - &local_n2, + local_n1, + local_n2, &feature1, &feature2, manifold, @@ -115,10 +114,10 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( { let contact = TrackedContact::new( p1, - pos12.inverse_transform_point(&p2_1), + pos12.inverse_transform_point(p2_1), PackedFeatureId::UNKNOWN, // TODO: We don't know what features are involved. PackedFeatureId::UNKNOWN, - (p2_1 - p1).dot(&local_n1), + (p2_1 - p1).dot(local_n1), ); manifold.points.push(contact); } @@ -138,22 +137,22 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( // Adjust points to take the radius into account. if border_radius1 != 0.0 || border_radius2 != 0.0 { for contact in &mut manifold.points { - contact.local_p1 += *local_n1 * border_radius1; - contact.local_p2 += *local_n2 * border_radius2; + contact.local_p1 += local_n1 * border_radius1; + contact.local_p2 += local_n2 * border_radius2; contact.dist -= border_radius1 + border_radius2; } } - manifold.local_n1 = *local_n1; - manifold.local_n2 = *local_n2; + manifold.local_n1 = local_n1; + manifold.local_n2 = local_n2; } GJKResult::NoIntersection(dir) => { // Use the manifold normal as a cache. - manifold.local_n1 = *dir; + manifold.local_n1 = dir; } _ => { // Reset the cached direction. - manifold.local_n1.fill(0.0); + manifold.local_n1 = Vector::ZERO; } } diff --git a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs index b01b52b6..1217fbae 100644 --- a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs @@ -1,7 +1,7 @@ use alloc::{boxed::Box, vec::Vec}; use crate::bounding_volume::{Aabb, BoundingVolume}; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; @@ -14,8 +14,7 @@ use crate::shape::{Shape, TriMesh}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone)] pub struct TriMeshShapeContactManifoldsWorkspace { @@ -43,7 +42,7 @@ impl TriMeshShapeContactManifoldsWorkspace { /// Computes the contact manifold between a triangle-mesh an a shape, both represented as `Shape` trait-objects. pub fn contact_manifolds_trimesh_shape_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -88,7 +87,7 @@ fn ensure_workspace_exists(workspace: &mut Option) { /// Computes the contact manifold between a triangle-mesh and a shape. pub fn contact_manifolds_trimesh_shape( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, trimesh1: &TriMesh, shape2: &dyn Shape, prediction: Real, diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs b/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs index e9b1f91a..e4c2db46 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector, VectorExt}; use crate::query::{ContactManifold, PointQuery, TrackedContact}; use crate::shape::{ Ball, Cuboid, OctantPattern, PackedFeatureId, Shape, VoxelState, VoxelType, Voxels, @@ -8,7 +8,7 @@ use alloc::vec::Vec; /// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object. pub fn contact_manifolds_voxels_ball_shapes( - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -33,7 +33,7 @@ pub fn contact_manifolds_voxels_ball_shapes( /// Computes the contact manifold between a convex shape and a ball. pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>( - pos12: &Isometry, + pos12: &Pose, voxels1: &'a Voxels, ball2: &'a Ball, prediction: Real, @@ -47,7 +47,7 @@ pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>( manifolds.clear(); let radius2 = ball2.radius; - let center2 = Point::origin(); // The ball’s center. + let center2 = Vector::ZERO; // The ball’s center. let radius1 = voxels1.voxel_size() / 2.0; // FIXME: optimize this. @@ -79,11 +79,11 @@ pub fn contact_manifolds_voxels_ball<'a, ManifoldData, ContactData>( } pub(crate) fn detect_hit_voxel_ball( - pos12: Isometry, - center1: Point, - radius1: Vector, + pos12: Pose, + center1: Vector, + radius1: Vector, data1: VoxelState, - center2: Point, + center2: Vector, radius2: Real, prediction: Real, flipped: bool, @@ -99,7 +99,7 @@ pub(crate) fn detect_hit_voxel_ball( let dist = dist_to_voxel - radius2; if dist <= prediction { - let local_n2 = pos12.inverse_transform_vector(&-local_n1); + let local_n2 = pos12.rotation.inverse() * -local_n1; let local_p1 = center2_1 - local_n1 * dist_to_voxel; let local_p2 = center2 + local_n2 * radius2; let contact_point = TrackedContact::::flipped( @@ -128,11 +128,11 @@ pub(crate) fn detect_hit_voxel_ball( } pub fn project_point_on_pseudo_cube( - voxel_center: Point, - voxel_radius: Vector, + voxel_center: Vector, + voxel_radius: Vector, voxel_mask: u32, - point: Point, -) -> Option<(Vector, Real)> { + point: Vector, +) -> Option<(Vector, Real)> { let dpos = point - voxel_center; // This maps our easy-to map octant_key, to the vertex key as @@ -150,7 +150,7 @@ pub fn project_point_on_pseudo_cube( | (((dpos.z >= 0.0) as usize) << 2); let aabb_octant_key = AABB_OCTANT_KEYS[octant_key]; let dpos_signs = dpos.map(|x| x.signum()); - let unit_dpos = dpos.abs().component_div(&voxel_radius); // Project the point in "local unit octant space". + let unit_dpos = dpos.abs() / voxel_radius; // Project the point in "local unit octant space". // Extract the feature pattern specific to the selected octant. let pattern = (voxel_mask >> (aabb_octant_key * 3)) & 0b0111; @@ -161,21 +161,21 @@ pub fn project_point_on_pseudo_cube( // PERF: inline the projection on cuboid to improve performances further. // In particular we already know on what octant we are to compute the // collision. - let cuboid = Cuboid::new(Vector::repeat(1.0)); - let unit_dpos_pt = Point::from(unit_dpos); - let proj = cuboid.project_local_point(&unit_dpos_pt, false); - let mut normal = unit_dpos_pt - proj.point; - let dist = normal.try_normalize_mut(1.0e-8)?; - Some((normal, dist)) + let cuboid = Cuboid::new(Vector::splat(1.0)); + let unit_dpos_pt = unit_dpos; + let proj = cuboid.project_local_point(unit_dpos_pt, false); + let normal = unit_dpos_pt - proj.point; + let (normal, dist) = normal.normalize_and_length(); + (dist != 0.0).then_some((normal, dist)) } #[cfg(feature = "dim3")] OctantPattern::EDGE_X | OctantPattern::EDGE_Y | OctantPattern::EDGE_Z => { - let cuboid = Cuboid::new(Vector::repeat(1.0)); - let unit_dpos_pt = Point::from(unit_dpos); - let proj = cuboid.project_local_point(&unit_dpos_pt, false); - let mut normal = unit_dpos_pt - proj.point; - let dist = normal.try_normalize_mut(1.0e-8)?; - Some((normal, dist)) + let cuboid = Cuboid::new(Vector::splat(1.0)); + let unit_dpos_pt = unit_dpos; + let proj = cuboid.project_local_point(unit_dpos_pt, false); + let normal = unit_dpos_pt - proj.point; + let (normal, dist) = normal.normalize_and_length(); + (dist != 0.0).then_some((normal, dist)) } #[cfg(feature = "dim2")] OctantPattern::FACE_X | OctantPattern::FACE_Y => { @@ -210,8 +210,8 @@ pub fn project_point_on_pseudo_cube( }; unit_result.map(|(n, d)| { - let mut scaled_n = n.component_mul(&dpos_signs).component_mul(&voxel_radius); - let scaled_d = scaled_n.normalize_mut(); - (scaled_n, d * scaled_d) + let scaled_n = n * (dpos_signs) * (voxel_radius); + let (n, scaled_d) = scaled_n.normalize_and_length(); + (n, d * scaled_d) }) } diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs index ed6118b2..ba815adc 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real, Translation, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::details::{ CanonicalVoxelShape, VoxelsShapeContactManifoldsWorkspace, VoxelsShapeSubDetector, }; @@ -9,9 +9,8 @@ use crate::query::{ }; use crate::shape::{CompositeShape, Cuboid, Shape, SupportMap, VoxelType, Voxels}; use crate::utils::hashmap::Entry; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; use alloc::{boxed::Box, vec::Vec}; -use na::Vector3; impl WorkspaceData for VoxelsShapeContactManifoldsWorkspace<3> { fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> { @@ -26,7 +25,7 @@ impl WorkspaceData for VoxelsShapeContactManifoldsWorkspace<3> { /// Computes the contact manifold between voxels and a composite shape, both represented as a `Shape` trait-object. pub fn contact_manifolds_voxels_composite_shape_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -58,7 +57,7 @@ pub fn contact_manifolds_voxels_composite_shape_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, voxels1: &Voxels, shape2: &dyn CompositeShape, prediction: Real, @@ -103,53 +102,54 @@ pub fn contact_manifolds_voxels_composite_shape( // one way of figuring this out is to re-traverse the bvh). let canon1 = CanonicalVoxelShape::from_voxel(voxels1, &vox1); let (canonical_center1, canonical_shape1) = canon1.cuboid(voxels1, &vox1, domain2_1); - let canonical_pose12 = Translation::from(-canonical_center1) * pos12; + let canonical_pose12 = Pose::from_translation(-canonical_center1) * pos12; let mut detect_hit = |leaf2: u32| { - let key = Vector3::new(canon1.workspace_key.x, canon1.workspace_key.y, leaf2); + let key = [canon1.workspace_key[0], canon1.workspace_key[1], leaf2]; // TODO: could we refactor the workspace system between Voxels, HeightField, and CompoundShape? - // (and maybe TriMesh too but it’s using a different approach). - let (sub_detector, manifold_updated) = match workspace.sub_detectors.entry(key) { - Entry::Occupied(entry) => { - let sub_detector = entry.into_mut(); + // (and maybe TriMesh too but it's using a different approach). + let (sub_detector, manifold_updated) = + match workspace.sub_detectors.entry(key.into()) { + Entry::Occupied(entry) => { + let sub_detector = entry.into_mut(); - if sub_detector.timestamp != new_timestamp { - let manifold = old_manifolds[sub_detector.manifold_id].take(); - *sub_detector = VoxelsShapeSubDetector { + if sub_detector.timestamp != new_timestamp { + let manifold = old_manifolds[sub_detector.manifold_id].take(); + *sub_detector = VoxelsShapeSubDetector { + manifold_id: manifolds.len(), + timestamp: new_timestamp, + selected_contacts: 0, + }; + + manifolds.push(manifold); + (sub_detector, false) + } else { + (sub_detector, true) + } + } + Entry::Vacant(entry) => { + let sub_detector = VoxelsShapeSubDetector { manifold_id: manifolds.len(), - timestamp: new_timestamp, selected_contacts: 0, + timestamp: new_timestamp, }; - manifolds.push(manifold); - (sub_detector, false) - } else { - (sub_detector, true) - } - } - Entry::Vacant(entry) => { - let sub_detector = VoxelsShapeSubDetector { - manifold_id: manifolds.len(), - selected_contacts: 0, - timestamp: new_timestamp, - }; - - let vox_id = vox1.linear_id.flat_id() as u32; - let (id1, id2) = if flipped { - (leaf2, vox_id) - } else { - (vox_id, leaf2) - }; - manifolds.push(ContactManifold::with_data( - id1, - id2, - ManifoldData::default(), - )); + let vox_id = vox1.linear_id.flat_id() as u32; + let (id1, id2) = if flipped { + (leaf2, vox_id) + } else { + (vox_id, leaf2) + }; + manifolds.push(ContactManifold::with_data( + id1, + id2, + ManifoldData::default(), + )); - (entry.insert(sub_detector), false) - } - }; + (entry.insert(sub_detector), false) + } + }; /* * Update the contact manifold if needed. @@ -170,16 +170,16 @@ pub fn contact_manifolds_voxels_composite_shape( manifold .subshape_pos2 .as_ref() - .map(|p| p.translation.vector) + .map(|p| p.translation) .unwrap_or_default() } else { manifold .subshape_pos1 .as_ref() - .map(|p| p.translation.vector) + .map(|p| p.translation) .unwrap_or_default() }; - let delta_center = canonical_center1.coords - prev_center; + let delta_center = canonical_center1 - prev_center; if flipped { for pt in &mut manifold.points { @@ -194,7 +194,8 @@ pub fn contact_manifolds_voxels_composite_shape( // Update contacts. if flipped { manifold.subshape_pos1 = part_pos2.copied(); - manifold.subshape_pos2 = Some(Isometry::from(canonical_center1)); + manifold.subshape_pos2 = + Some(Pose::from_translation(canonical_center1)); let _ = dispatcher.contact_manifold_convex_convex( &relative_pos12.inverse(), part_shape2, @@ -205,7 +206,8 @@ pub fn contact_manifolds_voxels_composite_shape( manifold, ); } else { - manifold.subshape_pos1 = Some(Isometry::from(canonical_center1)); + manifold.subshape_pos1 = + Some(Pose::from_translation(canonical_center1)); manifold.subshape_pos2 = part_pos2.copied(); let _ = dispatcher.contact_manifold_convex_convex( &relative_pos12, @@ -222,7 +224,7 @@ pub fn contact_manifolds_voxels_composite_shape( /* * Filter-out points that don’t belong to this block. */ - let test_voxel = Cuboid::new(radius1 + Vector::repeat(1.0e-2)); + let test_voxel = Cuboid::new(radius1 + Vector::splat(1.0e-2)); let penetration_dir1 = if flipped { manifold.local_n2 } else { @@ -235,14 +237,13 @@ pub fn contact_manifolds_voxels_composite_shape( // interior of the infinitely expanded canonical shape by checking if // the opposite normal had led to a better vector. let cuboid1 = Cuboid::new(radius1); - let sp1 = cuboid1.local_support_point(&-penetration_dir1) - + vox1.center.coords; + let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center; let sm2 = part_shape2 .as_support_map() .expect("Unsupported collision pair."); - let sp2 = sm2.support_point(&relative_pos12, &penetration_dir1) - + canonical_center1.coords; - let test_dist = (sp2 - sp1).dot(&-penetration_dir1); + let sp2 = sm2.support_point(&relative_pos12, penetration_dir1) + + canonical_center1; + let test_dist = (sp2 - sp1).dot(-penetration_dir1); let keep = test_dist < pt.dist; if !keep { @@ -253,14 +254,12 @@ pub fn contact_manifolds_voxels_composite_shape( } let pt_in_voxel_space = if flipped { - manifold.subshape_pos2.transform_point(&pt.local_p2) - - vox1.center.coords + manifold.subshape_pos2.transform_point(pt.local_p2) - vox1.center } else { - manifold.subshape_pos1.transform_point(&pt.local_p1) - - vox1.center.coords + manifold.subshape_pos1.transform_point(pt.local_p1) - vox1.center }; sub_detector.selected_contacts |= - (test_voxel.contains_local_point(&pt_in_voxel_space) as u32) << i; + (test_voxel.contains_local_point(pt_in_voxel_space) as u32) << i; } }); }; diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs index 6aaa57e1..c92eaa0e 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs @@ -1,20 +1,19 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, Translation, Vector, DIM}; +use crate::math::{IVector, Int, Pose, Real, Vector, DIM}; use crate::query::{ ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery, TypedWorkspaceData, WorkspaceData, }; use crate::shape::{AxisMask, Cuboid, Shape, SupportMap, VoxelData, VoxelType, Voxels}; use crate::utils::hashmap::{Entry, HashMap}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; use alloc::{boxed::Box, vec::Vec}; -use na::{SVector, Vector2}; +use num::AsPrimitive; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone)] pub(crate) struct VoxelsShapeSubDetector { @@ -23,6 +22,19 @@ pub(crate) struct VoxelsShapeSubDetector { pub timestamp: bool, } +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Eq, Hash, PartialEq)] +pub(crate) struct VoxelsWorkspaceKey { + #[cfg_attr(feature = "serde-serialize", serde(with = "serde_arrays"))] + idx: [u32; N], +} + +impl From<[u32; N]> for VoxelsWorkspaceKey { + fn from(idx: [u32; N]) -> Self { + Self { idx } + } +} + // NOTE: this is using a similar kind of cache as compound shape and height-field. // It is different from the trimesh cash though. Which one is better? /// A workspace for collision-detection against voxels shape. @@ -30,7 +42,7 @@ pub(crate) struct VoxelsShapeSubDetector { #[derive(Clone, Default)] pub struct VoxelsShapeContactManifoldsWorkspace { pub(crate) timestamp: bool, - pub(crate) sub_detectors: HashMap, VoxelsShapeSubDetector>, + pub(crate) sub_detectors: HashMap, VoxelsShapeSubDetector>, } impl VoxelsShapeContactManifoldsWorkspace { @@ -72,7 +84,7 @@ impl WorkspaceData for VoxelsShapeContactManifoldsWorkspace<2> { /// Computes the contact manifold between a convex shape and a voxels shape, both represented as a `Shape` trait-object. pub fn contact_manifolds_voxels_shape_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -103,7 +115,7 @@ pub fn contact_manifolds_voxels_shape_shapes( /// Computes the contact manifold between a convex shape and a voxels shape. pub fn contact_manifolds_voxels_shape( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, voxels1: &Voxels, shape2: &dyn Shape, prediction: Real, @@ -146,7 +158,7 @@ pub fn contact_manifolds_voxels_shape( // TODO: could we refactor the workspace system between Voxels, HeightField, and CompoundShape? // (and maybe TriMesh too but it’s using a different approach). let (sub_detector, manifold_updated) = - match workspace.sub_detectors.entry(canon1.workspace_key) { + match workspace.sub_detectors.entry(canon1.workspace_key.into()) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); @@ -193,7 +205,7 @@ pub fn contact_manifolds_voxels_shape( canon1.cuboid(voxels1, &vox1, domain2_1); let canonical_shape1 = &canonical_pseudo_cube1 as &dyn Shape; - let canonical_pos12 = Translation::from(-canonical_center1) * pos12; + let canonical_pos12 = Pose::from_translation(-canonical_center1) * pos12; // If we already computed contacts in the previous simulation step, their // local points are relative to the previously calculated canonical shape @@ -205,16 +217,16 @@ pub fn contact_manifolds_voxels_shape( manifold .subshape_pos2 .as_ref() - .map(|p| p.translation.vector) + .map(|p| p.translation) .unwrap_or_default() } else { manifold .subshape_pos1 .as_ref() - .map(|p| p.translation.vector) + .map(|p| p.translation) .unwrap_or_default() }; - let delta_center = canonical_center1.coords - prev_center; + let delta_center = canonical_center1 - prev_center; if flipped { for pt in &mut manifold.points { @@ -228,7 +240,7 @@ pub fn contact_manifolds_voxels_shape( // Update contacts. if flipped { - manifold.subshape_pos2 = Some(Isometry::from(canonical_center1)); + manifold.subshape_pos2 = Some(Pose::from_translation(canonical_center1)); let _ = dispatcher.contact_manifold_convex_convex( &canonical_pos12.inverse(), shape2, @@ -239,7 +251,7 @@ pub fn contact_manifolds_voxels_shape( manifold, ); } else { - manifold.subshape_pos1 = Some(Isometry::from(canonical_center1)); + manifold.subshape_pos1 = Some(Pose::from_translation(canonical_center1)); let _ = dispatcher.contact_manifold_convex_convex( &canonical_pos12, canonical_shape1, @@ -255,7 +267,7 @@ pub fn contact_manifolds_voxels_shape( /* * Filter-out points that don’t belong to this block. */ - let test_voxel = Cuboid::new(radius1 + Vector::repeat(1.0e-2)); + let test_voxel = Cuboid::new(radius1 + Vector::splat(1.0e-2)); let penetration_dir1 = if flipped { manifold.local_n2 } else { @@ -268,12 +280,12 @@ pub fn contact_manifolds_voxels_shape( // interior of the infinitely expanded canonical shape by checking if // the opposite normal had led to a better vector. let cuboid1 = Cuboid::new(radius1); - let sp1 = cuboid1.local_support_point(&-penetration_dir1) + vox1.center.coords; + let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center; let sm2 = shape2 .as_support_map() .expect("Unsupported collision pair."); - let sp2 = sm2.support_point(pos12, &penetration_dir1); - let test_dist = (sp2 - sp1).dot(&-penetration_dir1); + let sp2 = sm2.support_point(pos12, penetration_dir1); + let test_dist = (sp2 - sp1).dot(-penetration_dir1); let keep = test_dist < pt.dist; if !keep { @@ -284,12 +296,12 @@ pub fn contact_manifolds_voxels_shape( } let pt_in_voxel_space = if flipped { - manifold.subshape_pos2.transform_point(&pt.local_p2) - vox1.center.coords + manifold.subshape_pos2.transform_point(pt.local_p2) - vox1.center } else { - manifold.subshape_pos1.transform_point(&pt.local_p1) - vox1.center.coords + manifold.subshape_pos1.transform_point(pt.local_p1) - vox1.center }; sub_detector.selected_contacts |= - (test_voxel.contains_local_point(&pt_in_voxel_space) as u32) << i; + (test_voxel.contains_local_point(pt_in_voxel_space) as u32) << i; } } } @@ -315,8 +327,8 @@ pub fn contact_manifolds_voxels_shape( #[derive(Copy, Clone, Debug)] pub(crate) struct CanonicalVoxelShape { - pub range: [Point; 2], - pub workspace_key: Vector2, + pub range: [IVector; 2], + pub workspace_key: [u32; 2], } impl CanonicalVoxelShape { @@ -327,14 +339,14 @@ impl CanonicalVoxelShape { // NOTE: the mins/maxs here are offset by 1 so we can expand past the last voxel if it // happens to also be infinite along the same axis (due to cross-voxels internal edges // detection). - let mins = voxels.domain()[0] - Vector::repeat(1); + let mins = voxels.domain()[0] - IVector::splat(1); let maxs = voxels.domain()[1]; let counts = maxs - mins; let mask1 = vox.state.free_faces(); - let adjust_canon = |axis: AxisMask, i: usize, key: &mut Point, val: i32| { + let adjust_canon = |axis: AxisMask, i: usize, key: &mut IVector, val: Int| { if !mask1.contains(axis) { - key[i] = val; + key[i] = val.as_(); } }; @@ -350,29 +362,24 @@ impl CanonicalVoxelShape { } #[cfg(feature = "dim2")] - let workspace_id = |vox: Point| { + let workspace_id = |vox: IVector| { let local = vox - mins; (local.x + local.y * counts.x) as u32 }; #[cfg(feature = "dim3")] - let workspace_id = |vox: Point| { + let workspace_id = |vox: IVector| { let local = vox - mins; (local.x + local.y * counts.x + local.z * counts.x * counts.y) as u32 }; Self { range: [key_low, key_high], - workspace_key: Vector2::new(workspace_id(key_low), workspace_id(key_high)), + workspace_key: [workspace_id(key_low), workspace_id(key_high)], } } - pub fn cuboid( - &self, - voxels: &Voxels, - vox: &VoxelData, - domain2_1: Aabb, - ) -> (Point, Cuboid) { + pub fn cuboid(&self, voxels: &Voxels, vox: &VoxelData, domain2_1: Aabb) -> (Vector, Cuboid) { let radius = voxels.voxel_size() / 2.0; let mut canonical_mins = voxels.voxel_center(self.range[0]); let mut canonical_maxs = voxels.voxel_center(self.range[1]); @@ -388,7 +395,7 @@ impl CanonicalVoxelShape { } let canonical_half_extents = (canonical_maxs - canonical_mins) / 2.0 + radius; - let canonical_center = na::center(&canonical_mins, &canonical_maxs); + let canonical_center = canonical_mins.midpoint(canonical_maxs); let canonical_cube = Cuboid::new(canonical_half_extents); (canonical_center, canonical_cube) diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs b/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs index ae12eeb3..21f9c8db 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real, Translation, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::contact_manifolds::{CanonicalVoxelShape, VoxelsShapeContactManifoldsWorkspace}; use crate::query::details::VoxelsShapeSubDetector; use crate::query::{ @@ -8,9 +8,8 @@ use crate::query::{ }; use crate::shape::{Cuboid, Shape, SupportMap, VoxelData, VoxelType, Voxels}; use crate::utils::hashmap::Entry; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; use alloc::{boxed::Box, vec::Vec}; -use na::Vector4; impl WorkspaceData for VoxelsShapeContactManifoldsWorkspace<4> { fn as_typed_workspace_data(&self) -> TypedWorkspaceData<'_> { @@ -25,7 +24,7 @@ impl WorkspaceData for VoxelsShapeContactManifoldsWorkspace<4> { /// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object. pub fn contact_manifolds_voxels_voxels_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -45,7 +44,7 @@ pub fn contact_manifolds_voxels_voxels_shapes( /// Computes the contact manifold between a convex shape and a ball. pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, voxels1: &'a Voxels, voxels2: &'a Voxels, prediction: Real, @@ -78,15 +77,13 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( aabb1.aligned_intersections(pos12, &aabb2) { let domain_margin = (radius1 + radius2) * 10.0; - let full_domain2_1 = voxels2.compute_aabb(pos12).add_half_extents(&domain_margin); + let full_domain2_1 = voxels2.compute_aabb(pos12).add_half_extents(domain_margin); let domain2_1 = full_domain2_1 - .intersection(&aabb1.add_half_extents(&domain_margin)) + .intersection(&aabb1.add_half_extents(domain_margin)) .unwrap_or(full_domain2_1); - let full_domain1_2 = voxels1 - .compute_aabb(&pos21) - .add_half_extents(&domain_margin); + let full_domain1_2 = voxels1.compute_aabb(&pos21).add_half_extents(domain_margin); let domain1_2 = full_domain1_2 - .intersection(&aabb2.add_half_extents(&domain_margin)) + .intersection(&aabb2.add_half_extents(domain_margin)) .unwrap_or(full_domain1_2); let mut detect_hit = |canon1: CanonicalVoxelShape, @@ -94,19 +91,19 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( vox1: &VoxelData, vox2: &VoxelData| { // Compute canonical shapes and dispatch. - let workspace_key = Vector4::new( + let workspace_key = [ canon1.workspace_key[0], canon1.workspace_key[1], canon2.workspace_key[0], canon2.workspace_key[1], - ); + ]; checked_hits += 1; // TODO: could we refactor the workspace system with the Voxels-Shape // collision detection system? let (sub_detector, manifold_updated) = - match workspace.sub_detectors.entry(workspace_key) { + match workspace.sub_detectors.entry(workspace_key.into()) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); @@ -151,9 +148,9 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( canon2.cuboid(voxels2, vox2, domain1_2); if !manifold_updated { - let canonical_pos12 = Translation::from(-canonical_center1) + let canonical_pos12 = Pose::from_translation(-canonical_center1) * pos12 - * Translation::from(canonical_center2); + * Pose::from_translation(canonical_center2); // If we already computed contacts in the previous simulation step, their // local points are relative to the previously calculated canonical shape @@ -164,15 +161,15 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( let prev_center1 = manifold .subshape_pos1 .as_ref() - .map(|p| p.translation.vector) + .map(|p| p.translation) .unwrap_or_default(); - let delta_center1 = canonical_center1.coords - prev_center1; + let delta_center1 = canonical_center1 - prev_center1; let prev_center2 = manifold .subshape_pos2 .as_ref() - .map(|p| p.translation.vector) + .map(|p| p.translation) .unwrap_or_default(); - let delta_center2 = canonical_center2.coords - prev_center2; + let delta_center2 = canonical_center2 - prev_center2; for pt in &mut manifold.points { pt.local_p1 -= delta_center1; @@ -180,8 +177,8 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( } // Update contacts. - manifold.subshape_pos1 = Some(Isometry::from(canonical_center1)); - manifold.subshape_pos2 = Some(Isometry::from(canonical_center2)); + manifold.subshape_pos1 = Some(Pose::from_translation(canonical_center1)); + manifold.subshape_pos2 = Some(Pose::from_translation(canonical_center2)); let _ = dispatcher.contact_manifold_convex_convex( &canonical_pos12, &canonical_pseudo_cube1, @@ -196,8 +193,8 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( /* * Filter-out points that don’t belong to this block. */ - let test_voxel1 = Cuboid::new(radius1 + Vector::repeat(1.0e-2)); - let test_voxel2 = Cuboid::new(radius2 + Vector::repeat(1.0e-2)); + let test_voxel1 = Cuboid::new(radius1 + Vector::splat(1.0e-2)); + let test_voxel2 = Cuboid::new(radius2 + Vector::splat(1.0e-2)); let penetration_dir1 = manifold.local_n1; for (i, pt) in manifold.points.iter().enumerate() { @@ -207,12 +204,12 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( // the opposite normal had led to a better vector. let cuboid1 = Cuboid::new(radius1); let cuboid2 = Cuboid::new(radius2); - let sp1 = cuboid1.local_support_point(&-penetration_dir1) + vox1.center.coords; + let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center; let sp2 = cuboid2.support_point( - &(pos12 * Translation::from(vox2.center.coords)), - &penetration_dir1, + &(pos12 * Pose::from_translation(vox2.center)), + penetration_dir1, ); - let test_dist = (sp2 - sp1).dot(&-penetration_dir1); + let test_dist = (sp2 - sp1).dot(-penetration_dir1); let keep = test_dist < pt.dist; if !keep { @@ -223,12 +220,12 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( } let pt_in_voxel_space1 = - manifold.subshape_pos1.transform_point(&pt.local_p1) - vox1.center.coords; + manifold.subshape_pos1.transform_point(pt.local_p1) - vox1.center; let pt_in_voxel_space2 = - manifold.subshape_pos2.transform_point(&pt.local_p2) - vox2.center.coords; + manifold.subshape_pos2.transform_point(pt.local_p2) - vox2.center; sub_detector.selected_contacts |= - ((test_voxel1.contains_local_point(&pt_in_voxel_space1) as u32) << i) - & ((test_voxel2.contains_local_point(&pt_in_voxel_space2) as u32) << i); + ((test_voxel1.contains_local_point(pt_in_voxel_space1) as u32) << i) + & ((test_voxel2.contains_local_point(pt_in_voxel_space2) as u32) << i); } }; @@ -243,8 +240,8 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( } let canon1 = CanonicalVoxelShape::from_voxel(voxels1, &vox1); - let centered_aabb1_2 = Cuboid::new(radius1 + Vector::repeat(prediction)) - .compute_aabb(&(pos21 * Translation::from(vox1.center))); + let centered_aabb1_2 = Cuboid::new(radius1 + Vector::splat(prediction)) + .compute_aabb(&(pos21 * Pose::from_translation(vox1.center))); for vox2 in voxels2.voxels_intersecting_local_aabb(¢ered_aabb1_2) { let type2 = vox2.state.voxel_type(); @@ -284,8 +281,8 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( } let canon2 = CanonicalVoxelShape::from_voxel(voxels2, &vox2); - let centered_aabb2_1 = Cuboid::new(radius2 + Vector::repeat(prediction)) - .compute_aabb(&(pos12 * Translation::from(vox2.center))); + let centered_aabb2_1 = Cuboid::new(radius2 + Vector::splat(prediction)) + .compute_aabb(&(pos12 * Pose::from_translation(vox2.center))); for vox1 in voxels1.voxels_intersecting_local_aabb(¢ered_aabb2_1) { let type1 = vox1.state.voxel_type(); diff --git a/src/query/contact_manifolds/mod.rs b/src/query/contact_manifolds/mod.rs index a05de79f..7fe44fb2 100644 --- a/src/query/contact_manifolds/mod.rs +++ b/src/query/contact_manifolds/mod.rs @@ -36,14 +36,14 @@ //! use parry3d::query::{ContactManifold, TrackedContact}; //! use parry3d::query::details::contact_manifold_ball_ball; //! use parry3d::shape::Ball; -//! use parry3d::math::{Isometry, Point, Vector}; +//! use parry3d::math::{Pose, Vector}; //! //! // Create two balls //! let ball1 = Ball::new(1.0); //! let ball2 = Ball::new(1.0); //! //! // Position them so they're touching -//! let pos12 = Isometry::translation(1.9, 0.0, 0.0); +//! let pos12 = Pose::translation(1.9, 0.0, 0.0); //! //! // Create an empty contact manifold //! // Note: ManifoldData and ContactData are user-defined types for tracking data @@ -72,18 +72,18 @@ //! use parry3d::query::{ContactManifold, TrackedContact}; //! use parry3d::query::details::contact_manifold_ball_ball; //! use parry3d::shape::Ball; -//! use parry3d::math::Isometry; +//! use parry3d::math::Pose; //! //! let ball1 = Ball::new(1.0); //! let ball2 = Ball::new(1.0); //! let mut manifold = ContactManifold::<(), ()>::new(); //! //! // Initial contact computation -//! let pos12_frame1 = Isometry::translation(1.9, 0.0, 0.0); +//! let pos12_frame1 = Pose::translation(1.9, 0.0, 0.0); //! contact_manifold_ball_ball(&pos12_frame1, &ball1, &ball2, 0.1, &mut manifold); //! //! // On the next frame, try to update using spatial coherence -//! let pos12_frame2 = Isometry::translation(1.85, 0.0, 0.0); +//! let pos12_frame2 = Pose::translation(1.85, 0.0, 0.0); //! //! // Try to update contacts efficiently //! if !manifold.try_update_contacts(&pos12_frame2) { diff --git a/src/query/contact_manifolds/normals_constraint.rs b/src/query/contact_manifolds/normals_constraint.rs index 2d48d9e2..b5d05179 100644 --- a/src/query/contact_manifolds/normals_constraint.rs +++ b/src/query/contact_manifolds/normals_constraint.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Vector}; // NOTE: the 'static requirement is only needed for the following impl to work: // impl<'a> TypedCompositeShape for dyn CompositeShape @@ -19,18 +19,17 @@ pub trait NormalConstraints: 'static { /// /// If this method returns `false` then the contacts associated to that normal should be /// considered invalid and be ignored by the collision-detection pipeline. - fn project_local_normal_mut(&self, normal: &mut Vector) -> bool; + fn project_local_normal_mut(&self, normal: &mut Vector) -> bool; /// Corrects or discards the specified normal (assumed to be unit-sized) based on the constraints /// encoded by `Self`. /// /// If this method returns `None` then the contacts associated to that normal should be /// considered invalid and be ignored by the collision-detection pipeline. - fn project_local_normal(&self, mut normal: Vector) -> Option> { + fn project_local_normal(&self, mut normal: Vector) -> Option { self.project_local_normal_mut(&mut normal).then_some(normal) } - // NOTE: despite this not taking an UnitVector, the normal is - // assumed to be unit-sized. + // NOTE: the normal is assumed to be unit-sized. /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the /// assumption that `normal1` is in the same coordinates space as `Self`. /// @@ -41,15 +40,15 @@ pub trait NormalConstraints: 'static { /// considered invalid and be ignored by the collision-detection pipeline. fn project_local_normal1( &self, - pos12: &Isometry, - normal1: &mut Vector, - normal2: &mut Vector, + pos12: &Pose, + normal1: &mut Vector, + normal2: &mut Vector, ) -> bool { if !self.project_local_normal_mut(normal1) { return false; } - *normal2 = pos12.inverse_transform_vector(&-*normal1); + *normal2 = pos12.rotation.inverse() * -*normal1; true } @@ -64,22 +63,22 @@ pub trait NormalConstraints: 'static { /// considered invalid and be ignored by the collision-detection pipeline. fn project_local_normal2( &self, - pos12: &Isometry, - normal1: &mut Vector, - normal2: &mut Vector, + pos12: &Pose, + normal1: &mut Vector, + normal2: &mut Vector, ) -> bool { if !self.project_local_normal_mut(normal2) { return false; } - *normal1 = pos12 * (-*normal2); + *normal1 = pos12.rotation * (-*normal2); true } } impl NormalConstraints for () { - fn project_local_normal_mut(&self, _: &mut Vector) -> bool { + fn project_local_normal_mut(&self, _: &mut Vector) -> bool { true } } @@ -91,9 +90,9 @@ pub trait NormalConstraintsPair { /// This trait is mostly used internally to combine two [`NormalConstraints`] conveniently. fn project_local_normals( &self, - pos12: &Isometry, - normal1: &mut Vector, - normal2: &mut Vector, + pos12: &Pose, + normal1: &mut Vector, + normal2: &mut Vector, ) -> bool; } @@ -107,9 +106,9 @@ impl NormalConstraintsPair { fn project_local_normals( &self, - pos12: &Isometry, - normal1: &mut Vector, - normal2: &mut Vector, + pos12: &Pose, + normal1: &mut Vector, + normal2: &mut Vector, ) -> bool { if let Some(proj) = self.0 { if !proj.project_local_normal1(pos12, normal1, normal2) { diff --git a/src/query/contact_manifolds/polygon_polygon_contact_generator.rs b/src/query/contact_manifolds/polygon_polygon_contact_generator.rs index 2d2791c4..1de50397 100644 --- a/src/query/contact_manifolds/polygon_polygon_contact_generator.rs +++ b/src/query/contact_manifolds/polygon_polygon_contact_generator.rs @@ -2,7 +2,7 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; use crate::geometry::{sat, Contact, ContactData, ContactManifold, ContactManifoldData, Polygon}; -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Vector, Real}; #[cfg(feature = "dim2")] use crate::{math::Vector, utils}; use parry::query; @@ -26,9 +26,9 @@ pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationC fn generate_contacts<'a>( mut p1: &'a Polygon, - mut m1: &'a Isometry, + mut m1: &'a Pose, mut p2: &'a Polygon, - mut m2: &'a Isometry, + mut m2: &'a Pose, manifold: &'a mut ContactManifold, ) { let mut m12 = m1.inv_mul(&m2); @@ -62,7 +62,7 @@ fn generate_contacts<'a>( let support_face1 = sep1.1; let local_n1 = p1.normals[support_face1]; - let local_n2 = m21 * -local_n1; + let local_n2 = m21.rotation * -local_n1; let support_face2 = p2.support_face(&local_n2); let len1 = p1.vertices.len(); let len2 = p2.vertices.len(); @@ -76,8 +76,8 @@ fn generate_contacts<'a>( m12 * p2.vertices[(support_face2 + 1) % len2], ); if let Some((clip_a, clip_b)) = query::details::clip_segment_segment(seg1, seg2) { - let dist_a = (clip_a.1 - clip_a.0).dot(&local_n1); - let dist_b = (clip_b.1 - clip_b.0).dot(&local_n1); + let dist_a = (clip_a.1 - clip_a.0).dot(local_n1); + let dist_b = (clip_b.1 - clip_b.0).dot(local_n1); let mut data_a = ContactData::default(); let mut data_b = ContactData::default(); diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 1f8d731d..ad815d7c 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{ self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, @@ -10,10 +10,13 @@ use crate::query::{ query_dispatcher::PersistentQueryDispatcher, ContactManifold, }; -use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; +use crate::shape::{HalfSpace, Segment, Shape}; #[cfg(feature = "alloc")] use alloc::vec::Vec; +#[cfg(any(feature = "std", feature = "alloc"))] +use crate::shape::ShapeType; + /// The default query dispatcher implementation provided by Parry. /// /// This dispatcher handles all the built-in shape types and automatically selects the most @@ -64,12 +67,12 @@ use alloc::vec::Vec; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query; /// use parry3d::shape::Ball; -/// use parry3d::na::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos1 = Isometry3::identity(); -/// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +/// let pos1 = Pose::identity(); +/// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// // This uses DefaultQueryDispatcher internally /// let distance = query::distance(&pos1, &ball1, &pos2, &ball2); @@ -88,15 +91,15 @@ use alloc::vec::Vec; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{QueryDispatcher, DefaultQueryDispatcher}; /// use parry3d::shape::{Ball, Cuboid}; -/// use parry3d::na::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let dispatcher = DefaultQueryDispatcher; /// /// let ball = Ball::new(1.0); -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::splat(1.0)); /// -/// let pos1 = Isometry3::identity(); -/// let pos2 = Isometry3::translation(3.0, 0.0, 0.0); +/// let pos1 = Pose::identity(); +/// let pos2 = Pose::translation(3.0, 0.0, 0.0); /// let pos12 = pos1.inv_mul(&pos2); /// /// // Query intersection @@ -125,7 +128,7 @@ use alloc::vec::Vec; /// /// // Now queries will use your custom logic when applicable, /// // and Parry's default logic otherwise -/// let dist = dispatcher.distance(&pos12, shape1, shape2)?; +/// let dist = dispatcher.distance(pos12, shape1, shape2)?; /// # } /// ``` /// @@ -173,13 +176,13 @@ pub struct DefaultQueryDispatcher; impl QueryDispatcher for DefaultQueryDispatcher { fn intersection_test( &self, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, ) -> Result { if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) { - let p12 = Point::from(pos12.translation.vector); - Ok(query::details::intersection_test_ball_ball(&p12, b1, b2)) + let p12 = pos12.translation; + Ok(query::details::intersection_test_ball_ball(p12, b1, b2)) } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { Ok(query::details::intersection_test_cuboid_cuboid( pos12, c1, c2, @@ -245,7 +248,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { /// Returns `0.0` if the objects are touching or penetrating. fn distance( &self, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, ) -> Result { @@ -253,8 +256,8 @@ impl QueryDispatcher for DefaultQueryDispatcher { let ball2 = shape2.as_ball(); if let (Some(b1), Some(b2)) = (ball1, ball2) { - let p2 = Point::from(pos12.translation.vector); - Ok(query::details::distance_ball_ball(b1, &p2, b2)) + let p2 = pos12.translation; + Ok(query::details::distance_ball_ball(b1, p2, b2)) } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { Ok(query::details::distance_ball_convex_polyhedron( pos12, b1, shape2, @@ -301,7 +304,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn contact( &self, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -357,7 +360,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn closest_points( &self, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, max_dist: Real, @@ -433,8 +436,8 @@ impl QueryDispatcher for DefaultQueryDispatcher { fn cast_shapes( &self, - pos12: &Isometry, - local_vel12: &Vector, + pos12: &Pose, + local_vel12: Vector, shape1: &dyn Shape, shape2: &dyn Shape, options: ShapeCastOptions, @@ -628,7 +631,7 @@ where { fn contact_manifolds( &self, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, prediction: Real, @@ -748,7 +751,7 @@ where fn contact_manifold_convex_convex( &self, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, normal_constraints1: Option<&dyn NormalConstraints>, diff --git a/src/query/distance/distance.rs b/src/query/distance/distance.rs index 0b862059..49094c6a 100644 --- a/src/query/distance/distance.rs +++ b/src/query/distance/distance.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; use crate::shape::Shape; @@ -42,15 +42,15 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::distance; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// // Create two balls /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(2.0); /// /// // Position them 10 units apart along the x-axis -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(10.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(10.0, 0.0, 0.0); /// /// // Compute distance /// let dist = distance(&pos1, &ball1, &pos2, &ball2).unwrap(); @@ -64,15 +64,15 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::distance; /// use parry3d::shape::Cuboid; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// // Create two boxes -/// let box1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// let box2 = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); +/// let box1 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); +/// let box2 = Cuboid::new(Vector::new(0.5, 0.5, 0.5)); /// /// // Position them so they're touching -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(1.5, 0.0, 0.0); // Edge to edge +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(1.5, 0.0, 0.0); // Edge to edge /// /// let dist = distance(&pos1, &box1, &pos2, &box2).unwrap(); /// @@ -87,9 +87,9 @@ use crate::shape::Shape; /// - [`contact`](crate::query::contact()) - For penetration depth when overlapping /// - [`intersection_test`](crate::query::intersection_test()) - For boolean overlap test pub fn distance( - pos1: &Isometry, + pos1: &Pose, g1: &dyn Shape, - pos2: &Isometry, + pos2: &Pose, g2: &dyn Shape, ) -> Result { let pos12 = pos1.inv_mul(pos2); diff --git a/src/query/distance/distance_ball_ball.rs b/src/query/distance/distance_ball_ball.rs index ca2fe05d..0b44f403 100644 --- a/src/query/distance/distance_ball_ball.rs +++ b/src/query/distance/distance_ball_ball.rs @@ -1,18 +1,18 @@ -use crate::math::{Point, Real}; +use crate::math::ComplexField; +use crate::math::{Real, Vector}; use crate::shape::Ball; -use na::{self, ComplexField}; /// Distance between balls. #[inline] -pub fn distance_ball_ball(b1: &Ball, center2: &Point, b2: &Ball) -> Real { +pub fn distance_ball_ball(b1: &Ball, center2: Vector, b2: &Ball) -> Real { let r1 = b1.radius; let r2 = b2.radius; - let distance_squared = center2.coords.norm_squared(); + let distance_squared = center2.length_squared(); let sum_radius = r1 + r2; if distance_squared <= sum_radius * sum_radius { 0.0 } else { - ComplexField::sqrt(distance_squared) - sum_radius + ::sqrt(distance_squared) - sum_radius } } diff --git a/src/query/distance/distance_ball_convex_polyhedron.rs b/src/query/distance/distance_ball_convex_polyhedron.rs index 3b1f0f31..f3057c24 100644 --- a/src/query/distance/distance_ball_convex_polyhedron.rs +++ b/src/query/distance/distance_ball_convex_polyhedron.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Real}; use crate::shape::{Ball, Shape}; /// Distance between a ball and a convex polyhedron. @@ -7,7 +7,7 @@ use crate::shape::{Ball, Shape}; /// both the ConvexPolyhedron and PointQuery traits. #[inline] pub fn distance_ball_convex_polyhedron( - pos12: &Isometry, + pos12: &Pose, ball1: &Ball, shape2: &(impl Shape + ?Sized), ) -> Real { @@ -20,11 +20,11 @@ pub fn distance_ball_convex_polyhedron( /// both the ConvexPolyhedron and PointQuery traits. #[inline] pub fn distance_convex_polyhedron_ball( - pos12: &Isometry, + pos12: &Pose, shape1: &(impl Shape + ?Sized), ball2: &Ball, ) -> Real { - let center2_1 = Point::from(pos12.translation.vector); - let proj = shape1.project_local_point(¢er2_1, true); - (na::distance(&proj.point, ¢er2_1) - ball2.radius).max(0.0) + let center2_1 = pos12.translation; + let proj = shape1.project_local_point(center2_1, true); + ((proj.point - center2_1).length() - ball2.radius).max(0.0) } diff --git a/src/query/distance/distance_composite_shape_shape.rs b/src/query/distance/distance_composite_shape_shape.rs index c77e212c..8ed7184f 100644 --- a/src/query/distance/distance_composite_shape_shape.rs +++ b/src/query/distance/distance_composite_shape_shape.rs @@ -1,9 +1,9 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::partitioning::BvhNode; use crate::query::QueryDispatcher; use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { /// Calculates the closest distance between `self` and the given `shape2` positioned at @@ -13,11 +13,11 @@ impl CompositeShapeRef<'_, S> { pub fn distance_to_shape( &self, dispatcher: &D, - pose12: &Isometry, + pose12: &Pose, shape2: &dyn Shape, ) -> Option<(u32, Real)> { let ls_aabb2 = shape2.compute_aabb(pose12); - let msum_shift = -ls_aabb2.center().coords; + let msum_shift = -ls_aabb2.center(); let msum_margin = ls_aabb2.half_extents(); self.0.bvh().find_best( @@ -44,7 +44,7 @@ impl CompositeShapeRef<'_, S> { /// Smallest distance between a composite shape and any other shape. pub fn distance_composite_shape_shape( dispatcher: &D, - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &dyn Shape, ) -> Real @@ -61,7 +61,7 @@ where /// Smallest distance between a shape and a composite shape. pub fn distance_shape_composite_shape( dispatcher: &D, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &G2, ) -> Real diff --git a/src/query/distance/distance_cuboid_cuboid.rs b/src/query/distance/distance_cuboid_cuboid.rs index bfe4416c..9dcf8f9c 100644 --- a/src/query/distance/distance_cuboid_cuboid.rs +++ b/src/query/distance/distance_cuboid_cuboid.rs @@ -1,12 +1,12 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::ClosestPoints; use crate::shape::Cuboid; /// Distance between two cuboids. #[inline] -pub fn distance_cuboid_cuboid(pos12: &Isometry, cuboid1: &Cuboid, cuboid2: &Cuboid) -> Real { +pub fn distance_cuboid_cuboid(pos12: &Pose, cuboid1: &Cuboid, cuboid2: &Cuboid) -> Real { match crate::query::details::closest_points_cuboid_cuboid(pos12, cuboid1, cuboid2, Real::MAX) { - ClosestPoints::WithinMargin(p1, p2) => na::distance(&p1, &(pos12 * p2)), + ClosestPoints::WithinMargin(p1, p2) => (p1 - (pos12 * p2)).length(), _ => 0.0, } } diff --git a/src/query/distance/distance_halfspace_support_map.rs b/src/query/distance/distance_halfspace_support_map.rs index d77b6770..5bbd5126 100644 --- a/src/query/distance/distance_halfspace_support_map.rs +++ b/src/query/distance/distance_halfspace_support_map.rs @@ -1,21 +1,20 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::shape::HalfSpace; use crate::shape::SupportMap; -use na; /// Distance between a halfspace and a support-mapped shape. pub fn distance_halfspace_support_map( - pos12: &Isometry, + pos12: &Pose, halfspace: &HalfSpace, other: &G, ) -> Real { - let deepest = other.support_point_toward(pos12, &-halfspace.normal); - halfspace.normal.dot(&deepest.coords).max(na::zero()) + let deepest = other.support_point_toward(pos12, -halfspace.normal); + halfspace.normal.dot(deepest).max(0.0) } /// Distance between a support-mapped shape and a halfspace. pub fn distance_support_map_halfspace( - pos12: &Isometry, + pos12: &Pose, other: &G, halfspace: &HalfSpace, ) -> Real { diff --git a/src/query/distance/distance_segment_segment.rs b/src/query/distance/distance_segment_segment.rs index fb3966c8..dae39939 100644 --- a/src/query/distance/distance_segment_segment.rs +++ b/src/query/distance/distance_segment_segment.rs @@ -1,21 +1,17 @@ -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; use crate::query::ClosestPoints; use crate::shape::Segment; /// Distance between two segments. #[inline] -pub fn distance_segment_segment( - pos12: &Isometry, - segment1: &Segment, - segment2: &Segment, -) -> Real { +pub fn distance_segment_segment(pos12: &Pose, segment1: &Segment, segment2: &Segment) -> Real { match crate::query::details::closest_points_segment_segment( pos12, segment1, segment2, Real::MAX, ) { - ClosestPoints::WithinMargin(p1, p2) => na::distance(&p1, &(pos12 * p2)), + ClosestPoints::WithinMargin(p1, p2) => (p1 - (pos12 * p2)).length(), _ => 0.0, } } diff --git a/src/query/distance/distance_support_map_support_map.rs b/src/query/distance/distance_support_map_support_map.rs index c0096ec3..0123be34 100644 --- a/src/query/distance/distance_support_map_support_map.rs +++ b/src/query/distance/distance_support_map_support_map.rs @@ -1,12 +1,10 @@ -use crate::math::{Isometry, Real, Vector}; -use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex}; +use crate::math::{Pose, Real, Vector}; +use crate::query::gjk::{self, CsoPoint, GJKResult, VoronoiSimplex}; use crate::shape::SupportMap; - -use na::{self, Unit}; use num::Bounded; /// Distance between support-mapped shapes. -pub fn distance_support_map_support_map(pos12: &Isometry, g1: &G1, g2: &G2) -> Real +pub fn distance_support_map_support_map(pos12: &Pose, g1: &G1, g2: &G2) -> Real where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -18,33 +16,31 @@ where /// /// This allows a more fine grained control other the underlying GJK algorigtm. pub fn distance_support_map_support_map_with_params( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, simplex: &mut VoronoiSimplex, - init_dir: Option>, + init_dir: Option, ) -> Real where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { // TODO: or m2.translation - m1.translation ? - let dir = init_dir.unwrap_or_else(|| -pos12.translation.vector); + let dir = init_dir.unwrap_or_else(|| -pos12.translation); - if let Some(dir) = Unit::try_new(dir, crate::math::DEFAULT_EPSILON) { - simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir)); - } else { - simplex.reset(CSOPoint::from_shapes( - pos12, - g1, - g2, - &Vector::::x_axis(), - )); - } + let normalized_dir = + if dir.length_squared() > crate::math::DEFAULT_EPSILON * crate::math::DEFAULT_EPSILON { + dir.normalize() + } else { + Vector::X + }; + + simplex.reset(CsoPoint::from_shapes(pos12, g1, g2, normalized_dir)); match gjk::closest_points(pos12, g1, g2, Real::max_value(), true, simplex) { GJKResult::Intersection => 0.0, - GJKResult::ClosestPoints(p1, p2, _) => na::distance(&p1, &p2), + GJKResult::ClosestPoints(p1, p2, _) => (p1 - p2).length(), GJKResult::Proximity(_) => unreachable!(), GJKResult::NoIntersection(_) => 0.0, // TODO: GJK did not converge. } diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index d8588420..9be6f4f0 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -5,12 +5,10 @@ use alloc::{collections::BinaryHeap, vec::Vec}; use core::cmp::Ordering; - -use na::{self, Unit}; use num::Bounded; -use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::math::{Pose, Real, Vector}; +use crate::query::gjk::{self, ConstantOrigin, CsoPoint, VoronoiSimplex}; use crate::shape::SupportMap; use crate::utils; @@ -55,41 +53,40 @@ impl Ord for FaceId { #[derive(Clone, Debug)] struct Face { pts: [usize; 2], - normal: Unit>, - proj: Point, + normal: Vector, + proj: Vector, bcoords: [Real; 2], deleted: bool, } impl Face { - pub fn new(vertices: &[CSOPoint], pts: [usize; 2]) -> (Self, bool) { + pub fn new(vertices: &[CsoPoint], pts: [usize; 2]) -> (Self, bool) { if let Some((proj, bcoords)) = - project_origin(&vertices[pts[0]].point, &vertices[pts[1]].point) + project_origin(vertices[pts[0]].point, vertices[pts[1]].point) { (Self::new_with_proj(vertices, proj, bcoords, pts), true) } else { ( - Self::new_with_proj(vertices, Point::origin(), [0.0; 2], pts), + Self::new_with_proj(vertices, Vector::ZERO, [0.0; 2], pts), false, ) } } pub fn new_with_proj( - vertices: &[CSOPoint], - proj: Point, + vertices: &[CsoPoint], + proj: Vector, bcoords: [Real; 2], pts: [usize; 2], ) -> Self { let normal; let deleted; - if let Some(n) = utils::ccw_face_normal([&vertices[pts[0]].point, &vertices[pts[1]].point]) - { + if let Some(n) = utils::ccw_face_normal([vertices[pts[0]].point, vertices[pts[1]].point]) { normal = n; deleted = false; } else { - normal = Unit::new_unchecked(na::zero()); + normal = Vector::ZERO; deleted = true; } @@ -102,12 +99,12 @@ impl Face { } } - pub fn closest_points(&self, vertices: &[CSOPoint]) -> (Point, Point) { + pub fn closest_points(&self, vertices: &[CsoPoint]) -> (Vector, Vector) { ( vertices[self.pts[0]].orig1 * self.bcoords[0] - + vertices[self.pts[1]].orig1.coords * self.bcoords[1], + + vertices[self.pts[1]].orig1 * self.bcoords[1], vertices[self.pts[0]].orig2 * self.bcoords[0] - + vertices[self.pts[1]].orig2.coords * self.bcoords[1], + + vertices[self.pts[1]].orig2 * self.bcoords[1], ) } } @@ -143,11 +140,11 @@ impl Face { /// use parry2d::query::epa::EPA; /// use parry2d::query::gjk::VoronoiSimplex; /// use parry2d::shape::Ball; -/// use parry2d::na::Isometry2; +/// use parry2d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos12 = Isometry2::translation(1.5, 0.0); // Overlapping circles +/// let pos12 = Pose::translation(1.5, 0.0); // Overlapping circles /// /// // After GJK determines penetration and fills a simplex: /// let mut epa = EPA::new(); @@ -155,7 +152,7 @@ impl Face { /// /// // EPA computes the contact details /// // if let Some((pt1, pt2, normal)) = epa.closest_points(&pos12, &ball1, &ball2, &simplex) { -/// // println!("Penetration depth: {}", (pt2 - pt1).norm()); +/// // println!("Penetration depth: {}", (pt2 - pt1).length()); /// // println!("Contact normal: {}", normal); /// // } /// # } @@ -178,7 +175,7 @@ impl Face { /// may be numerical precision issues with the input geometry. #[derive(Default)] pub struct EPA { - vertices: Vec, + vertices: Vec, faces: Vec, heap: BinaryHeap, } @@ -241,10 +238,10 @@ impl EPA { /// use parry2d::query::epa::EPA; /// use parry2d::query::gjk::VoronoiSimplex; /// use parry2d::shape::Ball; - /// use parry2d::na::Isometry2; + /// use parry2d::math::Pose; /// /// let ball = Ball::new(2.0); - /// let pos: Isometry2 = Isometry2::identity(); + /// let pos = Pose::identity(); /// /// // Assume GJK determined the origin is inside and filled simplex /// let simplex = VoronoiSimplex::new(); @@ -258,10 +255,10 @@ impl EPA { /// ``` pub fn project_origin( &mut self, - m: &Isometry, + m: &Pose, g: &G, simplex: &VoronoiSimplex, - ) -> Option> { + ) -> Option { self.closest_points(&m.inverse(), g, &ConstantOrigin, simplex) .map(|(p, _, _)| p) } @@ -286,7 +283,7 @@ impl EPA { /// - `point2`: Contact point on shape `g2` in `g2`'s local frame /// - `normal`: Contact normal pointing from `g2` toward `g1`, normalized /// - /// The **penetration depth** can be computed as `(point1 - point2).norm()` after transforming + /// The **penetration depth** can be computed as `(point1 - point2).length()` after transforming /// both points to the same coordinate frame. /// /// Returns `None` if: @@ -309,13 +306,13 @@ impl EPA { /// use parry2d::query::epa::EPA; /// use parry2d::query::gjk::{GJKResult, VoronoiSimplex}; /// use parry2d::shape::Ball; - /// use parry2d::na::Isometry2; + /// use parry2d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// - /// let pos1 = Isometry2::identity(); - /// let pos2 = Isometry2::translation(1.5, 0.0); // Overlapping + /// let pos1 = Pose::identity(); + /// let pos2 = Pose::translation(1.5, 0.0); // Overlapping /// let pos12 = pos1.inverse() * pos2; /// /// // After GJK detects penetration: @@ -343,11 +340,11 @@ impl EPA { /// depth and contact normal. pub fn closest_points( &mut self, - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<(Vector, Vector, Vector)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -370,18 +367,18 @@ impl EPA { // The contact is vertex-vertex. // We need to determine a valid normal that lies // on both vertices' normal cone. - let mut n = Vector::y_axis(); + let mut n = Vector::Y; // First, find a vector on the first vertex tangent cone. let orig1 = self.vertices[0].orig1; for _ in 0..MAX_ITERS { - let supp1 = g1.local_support_point(&n); - if let Some(tangent) = Unit::try_new(supp1 - orig1, _eps_tol) { - if n.dot(&tangent) < _eps_tol { + let supp1 = g1.local_support_point(n); + if let Some(tangent) = (supp1 - orig1).try_normalize() { + if n.dot(tangent) < _eps_tol { break; } - n = Unit::new_unchecked(Vector::new(-tangent.y, tangent.x)); + n = Vector::new(-tangent.y, tangent.x); } else { break; } @@ -390,24 +387,24 @@ impl EPA { // Second, ensure the direction lies on the second vertex's tangent cone. let orig2 = self.vertices[0].orig2; for _ in 0..MAX_ITERS { - let supp2 = g2.support_point(pos12, &-n); - if let Some(tangent) = Unit::try_new(supp2 - orig2, _eps_tol) { - if (-n).dot(&tangent) < _eps_tol { + let supp2 = g2.support_point(pos12, -n); + if let Some(tangent) = (supp2 - orig2).try_normalize() { + if (-n).dot(tangent) < _eps_tol { break; } - n = Unit::new_unchecked(Vector::new(-tangent.y, tangent.x)); + n = Vector::new(-tangent.y, tangent.x); } else { break; } } - return Some((Point::origin(), Point::origin(), n)); + return Some((Vector::ZERO, Vector::ZERO, n)); } else if simplex.dimension() == 2 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; - if dp1.perp(&dp2) < 0.0 { + if dp1.perp_dot(dp2) < 0.0 { self.vertices.swap(1, 2) } @@ -424,17 +421,17 @@ impl EPA { self.faces.push(face3); if proj_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); + let dist1 = self.faces[0].normal.dot(self.vertices[0].point); self.heap.push(FaceId::new(0, -dist1)?); } if proj_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); + let dist2 = self.faces[1].normal.dot(self.vertices[1].point); self.heap.push(FaceId::new(1, -dist2)?); } if proj_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); + let dist3 = self.faces[2].normal.dot(self.vertices[2].point); self.heap.push(FaceId::new(2, -dist3)?); } @@ -451,19 +448,19 @@ impl EPA { self.faces.push(Face::new_with_proj( &self.vertices, - Point::origin(), + Vector::ZERO, [1.0, 0.0], pts1, )); self.faces.push(Face::new_with_proj( &self.vertices, - Point::origin(), + Vector::ZERO, [1.0, 0.0], pts2, )); - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); + let dist1 = self.faces[0].normal.dot(self.vertices[0].point); + let dist2 = self.faces[1].normal.dot(self.vertices[1].point); self.heap.push(FaceId::new(0, dist1)?); self.heap.push(FaceId::new(1, dist2)?); @@ -485,11 +482,11 @@ impl EPA { continue; } - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let cso_point = CsoPoint::from_shapes(pos12, g1, g2, face.normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); + let candidate_max_dist = cso_point.point.dot(face.normal); if candidate_max_dist < max_dist { best_face_id = face_id; @@ -520,7 +517,7 @@ impl EPA { for f in new_faces.iter() { if f.1 { - let dist = f.0.normal.dot(&f.0.proj.coords); + let dist = f.0.normal.dot(f.0.proj); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. @@ -550,11 +547,11 @@ impl EPA { } } -fn project_origin(a: &Point, b: &Point) -> Option<(Point, [Real; 2])> { - let ab = *b - *a; - let ap = -a.coords; - let ab_ap = ab.dot(&ap); - let sqnab = ab.norm_squared(); +fn project_origin(a: Vector, b: Vector) -> Option<(Vector, [Real; 2])> { + let ab = b - a; + let ap = -a; + let ab_ap = ab.dot(ap); + let sqnab = ab.length_squared(); if sqnab == 0.0 { return None; @@ -571,7 +568,7 @@ fn project_origin(a: &Point, b: &Point) -> Option<(Point, [Rea // Voronoï region of the segment interior. position_on_segment = ab_ap / sqnab; - let res = *a + ab * position_on_segment; + let res = a + ab * position_on_segment; Some((res, [1.0 - position_on_segment, position_on_segment])) } diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 7d360be6..14ca0fd0 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -3,15 +3,14 @@ //! This module provides the 3D-specific implementation of EPA, which works with //! polyhedra (triangular faces) rather than polygons (edges) as in the 2D version. -use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::math::{Pose, Real, Vector}; +use crate::query::gjk::{self, ConstantOrigin, CsoPoint, VoronoiSimplex}; use crate::query::PointQueryWithLocation; use crate::shape::{SupportMap, Triangle, TrianglePointLocation}; use crate::utils; use alloc::collections::BinaryHeap; use alloc::vec::Vec; use core::cmp::Ordering; -use na::{self, Unit}; use num::Bounded; #[derive(Copy, Clone, PartialEq)] @@ -56,14 +55,14 @@ impl Ord for FaceId { struct Face { pts: [usize; 3], adj: [usize; 3], - normal: Unit>, + normal: Vector, bcoords: [Real; 3], deleted: bool, } impl Face { pub fn new_with_proj( - vertices: &[CSOPoint], + vertices: &[CsoPoint], bcoords: [Real; 3], pts: [usize; 3], adj: [usize; 3], @@ -71,9 +70,9 @@ impl Face { let normal; if let Some(n) = utils::ccw_face_normal([ - &vertices[pts[0]].point, - &vertices[pts[1]].point, - &vertices[pts[2]].point, + vertices[pts[0]].point, + vertices[pts[1]].point, + vertices[pts[2]].point, ]) { normal = n; } else { @@ -81,7 +80,7 @@ impl Face { // TODO: It will work OK with our current code, though // we should do this in another way to avoid any risk // of misusing the face normal in the future. - normal = Unit::new_unchecked(na::zero()); + normal = Vector::ZERO; } Face { @@ -93,13 +92,13 @@ impl Face { } } - pub fn new(vertices: &[CSOPoint], pts: [usize; 3], adj: [usize; 3]) -> (Self, bool) { + pub fn new(vertices: &[CsoPoint], pts: [usize; 3], adj: [usize; 3]) -> (Self, bool) { let tri = Triangle::new( vertices[pts[0]].point, vertices[pts[1]].point, vertices[pts[2]].point, ); - let (proj, loc) = tri.project_local_point_and_get_location(&Point::::origin(), true); + let (proj, loc) = tri.project_local_point_and_get_location(Vector::ZERO, true); match loc { TrianglePointLocation::OnVertex(_) | TrianglePointLocation::OnEdge(_, _) => { @@ -107,7 +106,7 @@ impl Face { ( // barycentric_coordinates is guaranteed to work in OnVertex and OnEdge locations Self::new_with_proj(vertices, loc.barycentric_coordinates().unwrap(), pts, adj), - proj.is_inside_eps(&Point::::origin(), eps_tol), + proj.is_inside_eps(Vector::ZERO, eps_tol), ) } TrianglePointLocation::OnFace(_, bcoords) => { @@ -117,14 +116,14 @@ impl Face { } } - pub fn closest_points(&self, vertices: &[CSOPoint]) -> (Point, Point) { + pub fn closest_points(&self, vertices: &[CsoPoint]) -> (Vector, Vector) { ( vertices[self.pts[0]].orig1 * self.bcoords[0] - + vertices[self.pts[1]].orig1.coords * self.bcoords[1] - + vertices[self.pts[2]].orig1.coords * self.bcoords[2], + + vertices[self.pts[1]].orig1 * self.bcoords[1] + + vertices[self.pts[2]].orig1 * self.bcoords[2], vertices[self.pts[0]].orig2 * self.bcoords[0] - + vertices[self.pts[1]].orig2.coords * self.bcoords[1] - + vertices[self.pts[2]].orig2.coords * self.bcoords[2], + + vertices[self.pts[1]].orig2 * self.bcoords[1] + + vertices[self.pts[2]].orig2 * self.bcoords[2], ) } @@ -150,7 +149,7 @@ impl Face { } } - pub fn can_be_seen_by(&self, vertices: &[CSOPoint], point: usize, opp_pt_id: usize) -> bool { + pub fn can_be_seen_by(&self, vertices: &[CsoPoint], point: usize, opp_pt_id: usize) -> bool { let p0 = &vertices[self.pts[opp_pt_id]].point; let p1 = &vertices[self.pts[(opp_pt_id + 1) % 3]].point; let p2 = &vertices[self.pts[(opp_pt_id + 2) % 3]].point; @@ -161,7 +160,7 @@ impl Face { // have a zero normal, causing the dot product to be zero. // So return true for these case will let us skip the triangle // during silhouette computation. - (*pt - *p0).dot(&self.normal) >= -gjk::eps_tol() + (*pt - *p0).dot(self.normal) >= -gjk::eps_tol() || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() } } @@ -209,11 +208,11 @@ impl SilhouetteEdge { /// use parry3d::query::epa::EPA; /// use parry3d::query::gjk::VoronoiSimplex; /// use parry3d::shape::Ball; -/// use parry3d::na::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos12 = Isometry3::translation(1.5, 0.0, 0.0); // Overlapping spheres +/// let pos12 = Pose::translation(1.5, 0.0, 0.0); // Overlapping spheres /// /// // After GJK determines penetration and fills a simplex: /// let mut epa = EPA::new(); @@ -221,7 +220,7 @@ impl SilhouetteEdge { /// /// // EPA computes the contact details /// // if let Some((pt1, pt2, normal)) = epa.closest_points(&pos12, &ball1, &ball2, &simplex) { -/// // println!("Penetration depth: {}", (pt2 - pt1).norm()); +/// // println!("Penetration depth: {}", (pt2 - pt1).length()); /// // println!("Contact normal: {}", normal); /// // } /// # } @@ -252,7 +251,7 @@ impl SilhouetteEdge { /// - It handles more degenerate cases (coplanar faces, edge cases) #[derive(Default)] pub struct EPA { - vertices: Vec, + vertices: Vec, faces: Vec, silhouette: Vec, heap: BinaryHeap, @@ -317,10 +316,10 @@ impl EPA { /// use parry3d::query::epa::EPA; /// use parry3d::query::gjk::VoronoiSimplex; /// use parry3d::shape::Ball; - /// use parry3d::na::Isometry3; + /// use parry3d::math::Pose; /// /// let ball = Ball::new(2.0); - /// let pos = Isometry3::::identity(); + /// let pos = Pose::identity(); /// /// // Assume GJK determined the origin is inside and filled simplex /// let simplex = VoronoiSimplex::new(); @@ -334,10 +333,10 @@ impl EPA { /// ``` pub fn project_origin( &mut self, - m: &Isometry, + m: &Pose, g: &G, simplex: &VoronoiSimplex, - ) -> Option> { + ) -> Option { self.closest_points(&m.inverse(), g, &ConstantOrigin, simplex) .map(|(p, _, _)| p) } @@ -362,7 +361,7 @@ impl EPA { /// - `point2`: Contact point on shape `g2` in `g2`'s local frame /// - `normal`: Contact normal pointing from `g2` toward `g1`, normalized /// - /// The **penetration depth** can be computed as `(point1 - point2).norm()` after transforming + /// The **penetration depth** can be computed as `(point1 - point2).length()` after transforming /// both points to the same coordinate frame. /// /// Returns `None` if: @@ -386,13 +385,13 @@ impl EPA { /// use parry3d::query::epa::EPA; /// use parry3d::query::gjk::{GJKResult, VoronoiSimplex}; /// use parry3d::shape::Ball; - /// use parry3d::na::Isometry3; + /// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// - /// let pos1 = Isometry3::identity(); - /// let pos2 = Isometry3::translation(1.5, 0.0, 0.0); // Overlapping + /// let pos1 = Pose::identity(); + /// let pos2 = Pose::translation(1.5, 0.0, 0.0); // Overlapping /// let pos12 = pos1.inverse() * pos2; /// /// // After GJK detects penetration: @@ -427,11 +426,11 @@ impl EPA { /// depth and contact normal. pub fn closest_points( &mut self, - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<(Vector, Vector, Vector)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -449,15 +448,15 @@ impl EPA { } if simplex.dimension() == 0 { - let mut n: Vector = na::zero(); + let mut n: Vector = Vector::ZERO; n[1] = 1.0; - return Some((Point::origin(), Point::origin(), Unit::new_unchecked(n))); + return Some((Vector::ZERO, Vector::ZERO, n)); } else if simplex.dimension() == 3 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; let dp3 = self.vertices[3] - self.vertices[0]; - if dp1.cross(&dp2).dot(&dp3) > 0.0 { + if dp1.cross(dp2).dot(dp3) > 0.0 { self.vertices.swap(1, 2) } @@ -482,22 +481,22 @@ impl EPA { self.faces.push(face4); if proj_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); + let dist1 = self.faces[0].normal.dot(self.vertices[0].point); self.heap.push(FaceId::new(0, -dist1)?); } if proj_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); + let dist2 = self.faces[1].normal.dot(self.vertices[1].point); self.heap.push(FaceId::new(1, -dist2)?); } if proj_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); + let dist3 = self.faces[2].normal.dot(self.vertices[2].point); self.heap.push(FaceId::new(2, -dist3)?); } if proj_inside4 { - let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords); + let dist4 = self.faces[3].normal.dot(self.vertices[3].point); self.heap.push(FaceId::new(3, -dist4)?); } @@ -512,11 +511,10 @@ impl EPA { if simplex.dimension() == 1 { let dpt = self.vertices[1] - self.vertices[0]; - Vector::orthonormal_subspace_basis(&[dpt], |dir| { - // XXX: dir should already be unit on nalgebra! - let dir = Unit::new_unchecked(*dir); + crate::math::orthonormal_subspace_basis(&[dpt], |dir| { + // dir is already normalized by orthonormal_subspace_basis self.vertices - .push(CSOPoint::from_shapes(pos12, g1, g2, &dir)); + .push(CsoPoint::from_shapes(pos12, g1, g2, dir)); false }); } @@ -552,11 +550,11 @@ impl EPA { continue; } - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let cso_point = CsoPoint::from_shapes(pos12, g1, g2, face.normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); + let candidate_max_dist = cso_point.point.dot(face.normal); if candidate_max_dist < max_dist { best_face_id = face_id; @@ -611,8 +609,8 @@ impl EPA { self.faces.push(new_face.0); if new_face.1 { - let pt = self.vertices[self.faces[new_face_id].pts[0]].point.coords; - let dist = self.faces[new_face_id].normal.dot(&pt); + let pt = self.vertices[self.faces[new_face_id].pts[0]].point; + let dist = self.faces[new_face_id].normal.dot(pt); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. diff --git a/src/query/epa/mod.rs b/src/query/epa/mod.rs index fcc79f48..2f59e2c2 100644 --- a/src/query/epa/mod.rs +++ b/src/query/epa/mod.rs @@ -52,11 +52,11 @@ //! use parry3d::query::epa::EPA; //! use parry3d::query::gjk::VoronoiSimplex; //! use parry3d::shape::Ball; -//! use parry3d::na::Isometry3; +//! use parry3d::math::Pose; //! //! let ball1 = Ball::new(1.0); //! let ball2 = Ball::new(1.0); -//! let pos12 = Isometry3::translation(1.5, 0.0, 0.0); // Overlapping balls +//! let pos12 = Pose::translation(1.5, 0.0, 0.0); // Overlapping balls //! //! // GJK detects penetration, EPA computes contact details //! let simplex = VoronoiSimplex::new(); // Would be filled by GJK diff --git a/src/query/error.rs b/src/query/error.rs index d6ec7d26..e4b17b1a 100644 --- a/src/query/error.rs +++ b/src/query/error.rs @@ -34,11 +34,11 @@ use core::fmt; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::{contact, distance}; /// # use parry3d::shape::{Ball, Cuboid}; -/// # use parry3d::na::{Isometry3, Vector3}; +/// # use parry3d::math::{Pose, Vector}; /// # let shape1 = Ball::new(1.0); -/// # let shape2 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// # let pos1 = Isometry3::identity(); -/// # let pos2 = Isometry3::identity(); +/// # let shape2 = Cuboid::new(Vector::splat(1.0)); +/// # let pos1 = Pose::identity(); +/// # let pos2 = Pose::identity(); /// // If contact manifolds are unsupported, try basic contact: /// if let Ok(contact) = contact(&pos1, &shape1, &pos2, &shape2, 0.0) { /// // Process the contact point @@ -57,11 +57,11 @@ use core::fmt; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{TriMesh, Ball, Compound, SharedShape}; /// # use parry3d::query::distance; -/// # use parry3d::na::{Isometry3, Vector3}; +/// # use parry3d::math::{Pose, Vector}; /// # let mesh = TriMesh::new(vec![], vec![]).unwrap(); /// # let ball = Ball::new(1.0); -/// # let pos1 = Isometry3::identity(); -/// # let pos2 = Isometry3::identity(); +/// # let pos1 = Pose::identity(); +/// # let pos2 = Pose::identity(); /// // Instead of querying against a complex mesh directly, /// // iterate through its triangles: /// for triangle in mesh.triangles() { @@ -79,9 +79,9 @@ use core::fmt; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::TriMesh; /// # use parry3d::bounding_volume::Aabb; -/// # use parry3d::na::Point3; -/// # let mesh = TriMesh::new(vec![Point3::origin()], vec![[0, 0, 0]]).unwrap(); -/// # let query_aabb = Aabb::new(Point3::origin(), Point3::origin()); +/// # use parry3d::math::Vector; +/// # let mesh = TriMesh::new(vec![Vector::ZERO], vec![[0, 0, 0]]).unwrap(); +/// # let query_aabb = Aabb::new(Vector::ZERO, Vector::ZERO); /// // Use BVH queries instead of direct shape queries: /// for leaf_id in mesh.bvh().intersect_aabb(&query_aabb) { /// let triangle = mesh.triangle(leaf_id); @@ -99,7 +99,7 @@ use core::fmt; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::{QueryDispatcher, DefaultQueryDispatcher}; /// # use parry3d::shape::Shape; -/// # use parry3d::math::{Isometry, Real}; +/// # use parry3d::math::{Pose, Real}; /// struct MyQueryDispatcher { /// default: DefaultQueryDispatcher, /// } @@ -114,11 +114,11 @@ use core::fmt; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::{contact, Unsupported}; /// # use parry3d::shape::{Ball, Cuboid}; -/// # use parry3d::na::{Isometry3, Vector3}; +/// # use parry3d::math::{Pose, Vector}; /// let ball = Ball::new(1.0); -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// let pos1 = Isometry3::identity(); -/// let pos2 = Isometry3::identity(); +/// let cuboid = Cuboid::new(Vector::splat(1.0)); +/// let pos1 = Pose::identity(); +/// let pos2 = Pose::identity(); /// /// // Most queries return Result /// match contact(&pos1, &ball, &pos2, &cuboid, 0.0) { diff --git a/src/query/gjk/cso_point.rs b/src/query/gjk/cso_point.rs index 0bc51f7c..1f294b9b 100644 --- a/src/query/gjk/cso_point.rs +++ b/src/query/gjk/cso_point.rs @@ -1,7 +1,6 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Vector}; use crate::shape::SupportMap; use core::ops::Sub; -use na::Unit; /// A point of a Configuration-Space Obstacle. /// @@ -10,28 +9,35 @@ use na::Unit; /// points correspond to the difference of two point, each belonging /// to a different solid. #[derive(Copy, Clone, Debug, PartialEq)] -pub struct CSOPoint { - /// The point on the CSO. This is equal to `self.orig1 - self.orig2`, unless this CSOPoint +pub struct CsoPoint { + /// The point on the CSO. This is equal to `self.orig1 - self.orig2`, unless this CsoPoint /// has been translated with self.translate. - pub point: Point, + pub point: Vector, /// The original point on the first shape used to compute `self.point`. - pub orig1: Point, + pub orig1: Vector, /// The original point on the second shape used to compute `self.point`. - pub orig2: Point, + pub orig2: Vector, } -impl CSOPoint { +impl CsoPoint { + /// A CSO point where all components are zero. + pub const ZERO: Self = Self { + point: Vector::ZERO, + orig1: Vector::ZERO, + orig2: Vector::ZERO, + }; + /// Initializes a CSO point with `orig1 - orig2`. - pub fn new(orig1: Point, orig2: Point) -> Self { - let point = Point::from(orig1 - orig2); + pub fn new(orig1: Vector, orig2: Vector) -> Self { + let point = orig1 - orig2; Self::new_with_point(point, orig1, orig2) } /// Initializes a CSO point with all information provided. /// /// It is assumed, but not checked, that `point == orig1 - orig2`. - pub fn new_with_point(point: Point, orig1: Point, orig2: Point) -> Self { - CSOPoint { + pub fn new_with_point(point: Vector, orig1: Vector, orig2: Vector) -> Self { + CsoPoint { point, orig1, orig2, @@ -39,60 +45,55 @@ impl CSOPoint { } /// Initializes a CSO point where both original points are equal. - pub fn single_point(point: Point) -> Self { - Self::new_with_point(point, point, Point::origin()) + pub fn single_point(point: Vector) -> Self { + Self::new_with_point(point, point, Vector::ZERO) } /// CSO point where all components are set to zero. pub fn origin() -> Self { - CSOPoint::new(Point::origin(), Point::origin()) + CsoPoint::new(Vector::ZERO, Vector::ZERO) } /// Computes the support point of the CSO of `g1` and `g2` toward the unit direction `dir`. - pub fn from_shapes_toward( - pos12: &Isometry, - g1: &G1, - g2: &G2, - dir: &Unit>, - ) -> Self + pub fn from_shapes_toward(pos12: &Pose, g1: &G1, g2: &G2, dir: Vector) -> Self where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let sp1 = g1.local_support_point_toward(dir); - let sp2 = g2.support_point_toward(pos12, &-*dir); + let sp2 = g2.support_point_toward(pos12, -dir); - CSOPoint::new(sp1, sp2) + CsoPoint::new(sp1, sp2) } /// Computes the support point of the CSO of `g1` and `g2` toward the direction `dir`. - pub fn from_shapes(pos12: &Isometry, g1: &G1, g2: &G2, dir: &Vector) -> Self + pub fn from_shapes(pos12: &Pose, g1: &G1, g2: &G2, dir: Vector) -> Self where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let sp1 = g1.local_support_point(dir); - let sp2 = g2.support_point(pos12, &-*dir); + let sp2 = g2.support_point(pos12, -dir); - CSOPoint::new(sp1, sp2) + CsoPoint::new(sp1, sp2) } /// Translate the CSO point. - pub fn translate(&self, dir: &Vector) -> Self { - CSOPoint::new_with_point(self.point + dir, self.orig1, self.orig2) + pub fn translate(&self, dir: Vector) -> Self { + CsoPoint::new_with_point(self.point + dir, self.orig1, self.orig2) } /// Translate in-place the CSO point. - pub fn translate_mut(&mut self, dir: &Vector) { + pub fn translate_mut(&mut self, dir: Vector) { self.point += dir; } } -impl Sub for CSOPoint { - type Output = Vector; +impl Sub for CsoPoint { + type Output = Vector; #[inline] - fn sub(self, rhs: CSOPoint) -> Vector { + fn sub(self, rhs: CsoPoint) -> Vector { self.point - rhs.point } } diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index a62a4d38..25009a72 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -50,12 +50,12 @@ //! //! See individual function documentation for usage examples. -use na::{self, ComplexField, Unit}; +use crate::math::ComplexField; -use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::gjk::{ConstantOrigin, CsoPoint, VoronoiSimplex}; use crate::shape::SupportMap; // use query::Proximity; -use crate::math::{Isometry, Point, Real, Vector, DIM}; +use crate::math::{Pose, Real, Vector, DIM}; use crate::query::{self, Ray}; use num::{Bounded, Zero}; @@ -93,34 +93,34 @@ pub enum GJKResult { /// /// # Fields /// - /// - First `Point`: The closest point on the first shape (in local-space of shape 1) - /// - Second `Point`: The closest point on the second shape (in local-space of shape 1) - /// - `Unit`: The unit direction vector from shape 1 to shape 2 (separation axis) + /// - First `Vector`: The closest point on the first shape (in local-space of shape 1) + /// - Second `Vector`: The closest point on the second shape (in local-space of shape 1) + /// - `Vector`: The unit direction vector from shape 1 to shape 2 (separation axis) /// /// This variant is returned when `exact_dist` is `true` in the GJK algorithm and the /// shapes are not intersecting. - ClosestPoints(Point, Point, Unit>), + ClosestPoints(Vector, Vector, Vector), /// The shapes are in proximity (close but not intersecting). /// /// # Fields /// - /// - `Unit`: An approximate separation axis (unit direction from shape 1 to shape 2) + /// - `Vector`: An approximate separation axis (unit direction from shape 1 to shape 2) /// /// This variant is returned when `exact_dist` is `false` and the algorithm determines /// the shapes are close but not intersecting. It's faster than computing exact closest /// points when you only need to know if shapes are nearby. - Proximity(Unit>), + Proximity(Vector), /// The shapes are too far apart. /// /// # Fields /// - /// - `Unit`: A separation axis (unit direction from shape 1 to shape 2) + /// - `Vector`: A separation axis (unit direction from shape 1 to shape 2) /// /// This variant is returned when the minimum distance between the shapes exceeds /// the `max_dist` parameter passed to the GJK algorithm. - NoIntersection(Unit>), + NoIntersection(Vector), } /// The absolute tolerance used by the GJK algorithm. @@ -164,7 +164,7 @@ pub fn eps_tol() -> Real { /// /// # Returns /// -/// - `Some(Point)`: The closest point on the shape's boundary, in the shape's **local space** +/// - `Some(Vector)`: The closest point on the shape's boundary, in the shape's **local space** /// - `None`: If the origin is inside the shape /// /// # Example @@ -173,11 +173,11 @@ pub fn eps_tol() -> Real { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Ball; /// use parry3d::query::gjk::{project_origin, VoronoiSimplex}; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// // Create a ball at position (5, 0, 0) /// let ball = Ball::new(1.0); -/// let position = Isometry::translation(5.0, 0.0, 0.0); +/// let position = Pose::translation(5.0, 0.0, 0.0); /// /// // Project the origin onto the ball /// let mut simplex = VoronoiSimplex::new(); @@ -194,10 +194,10 @@ pub fn eps_tol() -> Real { /// The `simplex` parameter can be reused across multiple calls to avoid allocations. /// This is particularly beneficial when performing many projection queries. pub fn project_origin( - m: &Isometry, + m: &Pose, g: &G, simplex: &mut VoronoiSimplex, -) -> Option> { +) -> Option { match closest_points( &m.inverse(), g, @@ -261,15 +261,15 @@ pub fn project_origin( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, Cuboid}; /// use parry3d::query::gjk::{closest_points, VoronoiSimplex}; -/// use parry3d::math::{Isometry, Vector}; +/// use parry3d::math::{Pose, Vector}; /// /// // Create two shapes /// let ball = Ball::new(1.0); /// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// // Position them in space -/// let pos1 = Isometry::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry::translation(5.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// /// // Compute relative position /// let pos12 = pos1.inv_mul(&pos2); @@ -290,7 +290,7 @@ pub fn project_origin( /// println!("Closest point on ball: {:?}", p1); /// println!("Closest point on cuboid: {:?}", p2); /// println!("Separation direction: {:?}", normal); -/// let distance = (p2 - p1).norm(); +/// let distance = (p2 - p1).length(); /// println!("Distance: {}", distance); /// } /// parry3d::query::gjk::GJKResult::Intersection => { @@ -307,11 +307,11 @@ pub fn project_origin( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Ball; /// use parry3d::query::gjk::{closest_points, VoronoiSimplex}; -/// use parry3d::math::Isometry; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos12 = Isometry::translation(3.0, 0.0, 0.0); +/// let pos12 = Pose::translation(3.0, 0.0, 0.0); /// /// let mut simplex = VoronoiSimplex::new(); /// let result = closest_points( @@ -351,7 +351,7 @@ pub fn project_origin( /// - The algorithm typically converges in 5-10 iterations for well-separated shapes /// - Maximum iteration count is 100 to prevent infinite loops pub fn closest_points( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, max_dist: Real, @@ -364,14 +364,14 @@ where { let _eps = crate::math::DEFAULT_EPSILON; let _eps_tol: Real = eps_tol(); - let _eps_rel: Real = ComplexField::sqrt(_eps_tol); + let _eps_rel: Real = ::sqrt(_eps_tol); // TODO: reset the simplex if it is empty? let mut proj = simplex.project_origin_and_reduce(); let mut old_dir; - if let Some(proj_dir) = Unit::try_new(proj.coords, 0.0) { + if let Some(proj_dir) = proj.try_normalize() { old_dir = -proj_dir; } else { return GJKResult::Intersection; @@ -384,8 +384,10 @@ where loop { let old_max_bound = max_bound; - if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) { - dir = new_dir; + let neg_proj_coords = -proj; + let (normalized, dist) = neg_proj_coords.normalize_and_length(); + if dist > _eps_tol { + dir = normalized; max_bound = dist; } else { // The origin is on the simplex. @@ -401,8 +403,8 @@ where } } - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &dir); - let min_bound = -dir.dot(&cso_point.point.coords); + let cso_point = CsoPoint::from_shapes(pos12, g1, g2, dir); + let min_bound = -dir.dot(cso_point.point); assert!(min_bound.is_finite()); @@ -441,13 +443,13 @@ where return GJKResult::Proximity(old_dir); } } else { - return GJKResult::Intersection; // Point inside of the cso. + return GJKResult::Intersection; // Vector inside of the cso. } } niter += 1; if niter == 100 { - return GJKResult::NoIntersection(Vector::x_axis()); + return GJKResult::NoIntersection(Vector::X); } } } @@ -472,12 +474,12 @@ where /// - `simplex`: A reusable simplex structure. Initialize with `VoronoiSimplex::new()`. /// - `ray`: The ray to cast, containing an origin point and direction vector /// - `max_time_of_impact`: Maximum distance along the ray to check. The ray will be treated -/// as a line segment of length `max_time_of_impact * ray.dir.norm()`. +/// as a line segment of length `max_time_of_impact * ray.dir.length()`. /// /// # Returns /// /// - `Some((toi, normal))`: If the ray hits the shape -/// - `toi`: Time of impact - multiply by `ray.dir.norm()` to get the actual distance +/// - `toi`: Time of impact - multiply by `ray.dir.length()` to get the actual distance /// - `normal`: Surface normal at the hit point /// - `None`: If the ray doesn't hit the shape within the maximum distance /// @@ -487,14 +489,14 @@ where /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Ball; /// use parry3d::query::{Ray, gjk::{cast_local_ray, VoronoiSimplex}}; -/// use parry3d::math::{Point, Vector}; +/// use parry3d::math::Vector; /// /// // Create a ball at the origin /// let ball = Ball::new(1.0); /// /// // Create a ray starting at (0, 0, -5) pointing toward +Z /// let ray = Ray::new( -/// Point::new(0.0, 0.0, -5.0), +/// Vector::new(0.0, 0.0, -5.0), /// Vector::new(0.0, 0.0, 1.0) /// ); /// @@ -521,10 +523,10 @@ pub fn cast_local_ray( simplex: &mut VoronoiSimplex, ray: &Ray, max_time_of_impact: Real, -) -> Option<(Real, Vector)> { +) -> Option<(Real, Vector)> { let g2 = ConstantOrigin; minkowski_ray_cast( - &Isometry::identity(), + &Pose::IDENTITY, shape, &g2, ray, @@ -574,12 +576,12 @@ pub fn cast_local_ray( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Ball; /// use parry3d::query::gjk::{directional_distance, VoronoiSimplex}; -/// use parry3d::math::{Isometry, Vector}; +/// use parry3d::math::{Pose, Vector}; /// /// // Two balls: one at origin, one at (10, 0, 0) /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); -/// let pos12 = Isometry::translation(10.0, 0.0, 0.0); +/// let pos12 = Pose::translation(10.0, 0.0, 0.0); /// /// // Move ball1 toward ball2 along the +X axis /// let direction = Vector::new(1.0, 0.0, 0.0); @@ -589,7 +591,7 @@ pub fn cast_local_ray( /// &pos12, /// &ball1, /// &ball2, -/// &direction, +/// direction, /// &mut simplex /// ) { /// println!("Ball1 can move {} units before contact", distance); @@ -630,17 +632,17 @@ pub fn cast_local_ray( /// - The direction vector does not need to be normalized /// - This function internally uses GJK raycasting on the Minkowski difference pub fn directional_distance( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, - dir: &Vector, + dir: Vector, simplex: &mut VoronoiSimplex, -) -> Option<(Real, Vector, Point, Point)> +) -> Option<(Real, Vector, Vector, Vector)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { - let ray = Ray::new(Point::origin(), *dir); + let ray = Ray::new(Vector::ZERO, dir); minkowski_ray_cast(pos12, g1, g2, &ray, Real::max_value(), simplex).map( |(time_of_impact, normal)| { let witnesses = if !time_of_impact.is_zero() { @@ -648,7 +650,7 @@ where } else { // If there is penetration, the witness points // are undefined. - (Point::origin(), Point::origin()) + (Vector::ZERO, Vector::ZERO) }; (time_of_impact, normal, witnesses.0, witnesses.1) @@ -658,22 +660,22 @@ where // Ray-cast on the Minkowski Difference `g1 - pos12 * g2`. fn minkowski_ray_cast( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, ray: &Ray, max_time_of_impact: Real, simplex: &mut VoronoiSimplex, -) -> Option<(Real, Vector)> +) -> Option<(Real, Vector)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let _eps = crate::math::DEFAULT_EPSILON; let _eps_tol: Real = eps_tol(); - let _eps_rel: Real = ComplexField::sqrt(_eps_tol); + let _eps_rel: Real = ::sqrt(_eps_tol); - let ray_length = ray.dir.norm(); + let ray_length = ray.dir.length(); if relative_eq!(ray_length, 0.0) { return None; @@ -685,8 +687,8 @@ where let mut ldir = dir; // Initialize the simplex. - let support_point = CSOPoint::from_shapes(pos12, g1, g2, &dir); - simplex.reset(support_point.translate(&-curr_ray.origin.coords)); + let support_point = CsoPoint::from_shapes(pos12, g1, g2, dir); + simplex.reset(support_point.translate(-curr_ray.origin)); // TODO: reset the simplex if it is empty? let mut proj = simplex.project_origin_and_reduce(); @@ -698,8 +700,10 @@ where loop { let old_max_bound = max_bound; - if let Some((new_dir, dist)) = Unit::try_new_and_get(-proj.coords, _eps_tol) { - dir = new_dir; + let neg_proj_coords = -proj; + let (normalized, dist) = neg_proj_coords.normalize_and_length(); + if dist > _eps_tol { + dir = normalized; max_bound = dist; } else { return Some((ltoi / ray_length, ldir)); @@ -708,13 +712,13 @@ where let support_point = if max_bound >= old_max_bound { // Upper bounds inconsistencies. Consider the projection as a valid support point. last_chance = true; - CSOPoint::single_point(proj + curr_ray.origin.coords) + CsoPoint::single_point(proj + curr_ray.origin) } else { - CSOPoint::from_shapes(pos12, g1, g2, &dir) + CsoPoint::from_shapes(pos12, g1, g2, dir) }; if last_chance && ltoi > 0.0 { - // last_chance && ltoi > 0.0 && (support_point.point - curr_ray.origin).dot(&ldir) >= 0.0 { + // last_chance && ltoi > 0.0 && (support_point.point - curr_ray.origin).dot(ldir) >= 0.0 { return Some((ltoi / ray_length, ldir)); } @@ -726,11 +730,11 @@ where // < 0 | > 0 | New lower bound, move the origin. // > 0 | < 0 | Miss. No intersection. // > 0 | > 0 | New higher bound. - match query::details::ray_toi_with_halfspace(&support_point.point, &dir, &curr_ray) { + match query::details::ray_toi_with_halfspace(support_point.point, dir, &curr_ray) { Some(t) => { - if dir.dot(&curr_ray.dir) < 0.0 && t > 0.0 { + if dir.dot(curr_ray.dir) < 0.0 && t > 0.0 { // new lower bound - ldir = *dir; + ldir = dir; ltoi += t; // NOTE: we divide by ray_length instead of doing max_time_of_impact * ray_length @@ -743,12 +747,12 @@ where let shift = curr_ray.dir * t; curr_ray.origin += shift; max_bound = Real::max_value(); - simplex.modify_pnts(&|pt| pt.translate_mut(&-shift)); + simplex.modify_pnts(&|pt| pt.translate_mut(-shift)); last_chance = false; } } None => { - if dir.dot(&curr_ray.dir) > _eps_tol { + if dir.dot(curr_ray.dir) > _eps_tol { // miss return None; } @@ -759,7 +763,7 @@ where return None; } - let min_bound = -dir.dot(&(support_point.point.coords - curr_ray.origin.coords)); + let min_bound = -dir.dot(support_point.point - curr_ray.origin); assert!(min_bound.is_finite()); @@ -776,14 +780,14 @@ where } } - let _ = simplex.add_point(support_point.translate(&-curr_ray.origin.coords)); + let _ = simplex.add_point(support_point.translate(-curr_ray.origin)); proj = simplex.project_origin_and_reduce(); if simplex.dimension() == DIM { if min_bound >= _eps_tol { return None; } else { - return Some((ltoi / ray_length, ldir)); // Point inside of the cso. + return Some((ltoi / ray_length, ldir)); // Vector inside of the cso. } } @@ -794,14 +798,14 @@ where } } -fn result(simplex: &VoronoiSimplex, prev: bool) -> (Point, Point) { - let mut res = (Point::origin(), Point::origin()); +fn result(simplex: &VoronoiSimplex, prev: bool) -> (Vector, Vector) { + let mut res = (Vector::ZERO, Vector::ZERO); if prev { for i in 0..simplex.prev_dimension() + 1 { let coord = simplex.prev_proj_coord(i); let point = simplex.prev_point(i); - res.0 += point.orig1.coords * coord; - res.1 += point.orig2.coords * coord; + res.0 += point.orig1 * coord; + res.1 += point.orig2 * coord; } res @@ -809,8 +813,8 @@ fn result(simplex: &VoronoiSimplex, prev: bool) -> (Point, Point) { for i in 0..simplex.dimension() + 1 { let coord = simplex.proj_coord(i); let point = simplex.point(i); - res.0 += point.orig1.coords * coord; - res.1 += point.orig2.coords * coord; + res.0 += point.orig1 * coord; + res.1 += point.orig2 * coord; } res diff --git a/src/query/gjk/mod.rs b/src/query/gjk/mod.rs index 506794fd..fc9ef002 100644 --- a/src/query/gjk/mod.rs +++ b/src/query/gjk/mod.rs @@ -1,6 +1,6 @@ //! The GJK algorithm for distance computation. -pub use self::cso_point::CSOPoint; +pub use self::cso_point::CsoPoint; #[cfg(feature = "dim2")] pub use self::voronoi_simplex2::VoronoiSimplex; #[cfg(feature = "dim3")] diff --git a/src/query/gjk/special_support_maps.rs b/src/query/gjk/special_support_maps.rs index c9f3136c..9a8f7985 100644 --- a/src/query/gjk/special_support_maps.rs +++ b/src/query/gjk/special_support_maps.rs @@ -1,29 +1,27 @@ -use na::Unit; - -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::SupportMap; /// A support mapping that is a single point. -pub struct ConstantPoint(pub Point); +pub struct ConstantPoint(pub Vector); impl SupportMap for ConstantPoint { #[inline] - fn support_point(&self, m: &Isometry, _: &Vector) -> Point { + fn support_point(&self, m: &Pose, _: Vector) -> Vector { m * self.0 } #[inline] - fn support_point_toward(&self, m: &Isometry, _: &Unit>) -> Point { + fn support_point_toward(&self, m: &Pose, _: Vector) -> Vector { m * self.0 } #[inline] - fn local_support_point(&self, _: &Vector) -> Point { + fn local_support_point(&self, _: Vector) -> Vector { self.0 } #[inline] - fn local_support_point_toward(&self, _: &Unit>) -> Point { + fn local_support_point_toward(&self, _: Vector) -> Vector { self.0 } } @@ -33,23 +31,23 @@ pub struct ConstantOrigin; impl SupportMap for ConstantOrigin { #[inline] - fn support_point(&self, m: &Isometry, _: &Vector) -> Point { - m.translation.vector.into() + fn support_point(&self, m: &Pose, _: Vector) -> Vector { + m.translation } #[inline] - fn support_point_toward(&self, m: &Isometry, _: &Unit>) -> Point { - m.translation.vector.into() + fn support_point_toward(&self, m: &Pose, _: Vector) -> Vector { + m.translation } #[inline] - fn local_support_point(&self, _: &Vector) -> Point { - Point::origin() + fn local_support_point(&self, _: Vector) -> Vector { + Vector::ZERO } #[inline] - fn local_support_point_toward(&self, _: &Unit>) -> Point { - Point::origin() + fn local_support_point_toward(&self, _: Vector) -> Vector { + Vector::ZERO } } @@ -63,22 +61,24 @@ pub struct DilatedShape<'a, S: ?Sized + SupportMap> { impl SupportMap for DilatedShape<'_, S> { #[inline] - fn support_point(&self, m: &Isometry, dir: &Vector) -> Point { - self.support_point_toward(m, &Unit::new_normalize(*dir)) + fn support_point(&self, m: &Pose, dir: Vector) -> Vector { + let normalized_dir = dir.normalize_or_zero(); + self.support_point_toward(m, normalized_dir) } #[inline] - fn support_point_toward(&self, m: &Isometry, dir: &Unit>) -> Point { - self.shape.support_point_toward(m, dir) + **dir * self.radius + fn support_point_toward(&self, m: &Pose, dir: Vector) -> Vector { + self.shape.support_point_toward(m, dir) + dir * self.radius } #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { - self.local_support_point_toward(&Unit::new_normalize(*dir)) + fn local_support_point(&self, dir: Vector) -> Vector { + let normalized_dir = dir.normalize_or_zero(); + self.local_support_point_toward(normalized_dir) } #[inline] - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.shape.local_support_point_toward(dir) + **dir * self.radius + fn local_support_point_toward(&self, dir: Vector) -> Vector { + self.shape.local_support_point_toward(dir) + dir * self.radius } } diff --git a/src/query/gjk/voronoi_simplex2.rs b/src/query/gjk/voronoi_simplex2.rs index e69d230a..2b6ca739 100644 --- a/src/query/gjk/voronoi_simplex2.rs +++ b/src/query/gjk/voronoi_simplex2.rs @@ -1,5 +1,5 @@ -use crate::math::{Point, Real}; -use crate::query::gjk::{self, CSOPoint}; +use crate::math::{Real, Vector}; +use crate::query::gjk::{self, CsoPoint}; use crate::query::{PointQuery, PointQueryWithLocation}; use crate::shape::{Segment, SegmentPointLocation, Triangle, TrianglePointLocation}; @@ -10,7 +10,7 @@ pub struct VoronoiSimplex { prev_dim: usize, prev_proj: [Real; 2], - vertices: [CSOPoint; 3], + vertices: [CsoPoint; 3], proj: [Real; 2], dim: usize, } @@ -28,7 +28,7 @@ impl VoronoiSimplex { prev_vertices: [0, 1, 2], prev_proj: [0.0; 2], prev_dim: 0, - vertices: [CSOPoint::origin(); 3], + vertices: [CsoPoint::ZERO; 3], proj: [0.0; 2], dim: 0, } @@ -41,20 +41,20 @@ impl VoronoiSimplex { } /// Resets this simplex to a single point. - pub fn reset(&mut self, pt: CSOPoint) { + pub fn reset(&mut self, pt: CsoPoint) { self.prev_dim = 0; self.dim = 0; self.vertices[0] = pt; } /// Add a point to this simplex. - pub fn add_point(&mut self, pt: CSOPoint) -> bool { + pub fn add_point(&mut self, pt: CsoPoint) -> bool { self.prev_dim = self.dim; self.prev_proj = self.proj; self.prev_vertices = [0, 1, 2]; for i in 0..self.dim + 1 { - if (self.vertices[i].point - pt.point).norm_squared() < gjk::eps_tol() { + if (self.vertices[i].point - pt.point).length_squared() < gjk::eps_tol() { return false; } } @@ -71,7 +71,7 @@ impl VoronoiSimplex { } /// The i-th point of this simplex. - pub fn point(&self, i: usize) -> &CSOPoint { + pub fn point(&self, i: usize) -> &CsoPoint { assert!(i <= self.dim, "Index out of bounds."); &self.vertices[i] } @@ -83,23 +83,23 @@ impl VoronoiSimplex { } /// The i-th point of the simplex before the last call to `project_origin_and_reduce`. - pub fn prev_point(&self, i: usize) -> &CSOPoint { + pub fn prev_point(&self, i: usize) -> &CsoPoint { assert!(i <= self.prev_dim, "Index out of bounds."); &self.vertices[self.prev_vertices[i]] } /// Projects the origin on the boundary of this simplex and reduces `self` the smallest subsimplex containing the origin. /// - /// Returns the result of the projection or Point::origin() if the origin lies inside of the simplex. + /// Returns the result of the projection or Vector::ZERO if the origin lies inside of the simplex. /// The state of the simplex before projection is saved, and can be retrieved using the methods prefixed /// by `prev_`. - pub fn project_origin_and_reduce(&mut self) -> Point { + pub fn project_origin_and_reduce(&mut self) -> Vector { if self.dim == 0 { self.proj[0] = 1.0; self.vertices[0].point } else if self.dim == 1 { let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(Vector::ZERO, true); match location { SegmentPointLocation::OnVertex(0) => { @@ -125,7 +125,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(Vector::ZERO, true); match location { TrianglePointLocation::OnVertex(i) => { @@ -156,13 +156,12 @@ impl VoronoiSimplex { } /// Compute the projection of the origin on the boundary of this simplex. - pub fn project_origin(&mut self) -> Point { + pub fn project_origin(&mut self) -> Vector { if self.dim == 0 { self.vertices[0].point } else if self.dim == 1 { let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point(&Point::::origin(), true) - .point + seg.project_local_point(Vector::ZERO, true).point } else { assert!(self.dim == 2); let tri = Triangle::new( @@ -170,15 +169,14 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ); - tri.project_local_point(&Point::::origin(), true) - .point + tri.project_local_point(Vector::ZERO, true).point } } /// Tests if the given point is already a vertex of this simplex. - pub fn contains_point(&self, pt: &Point) -> bool { + pub fn contains_point(&self, pt: Vector) -> bool { for i in 0..self.dim + 1 { - if self.vertices[i].point == *pt { + if self.vertices[i].point == pt { return true; } } @@ -201,7 +199,7 @@ impl VoronoiSimplex { let mut max_sq_len = 0.0; for i in 0..self.dim + 1 { - let norm = self.vertices[i].point.coords.norm_squared(); + let norm = self.vertices[i].point.length_squared(); if norm > max_sq_len { max_sq_len = norm @@ -212,7 +210,7 @@ impl VoronoiSimplex { } /// Apply a function to all the vertices of this simplex. - pub fn modify_pnts(&mut self, f: &dyn Fn(&mut CSOPoint)) { + pub fn modify_pnts(&mut self, f: &dyn Fn(&mut CsoPoint)) { for i in 0..self.dim + 1 { f(&mut self.vertices[i]) } diff --git a/src/query/gjk/voronoi_simplex3.rs b/src/query/gjk/voronoi_simplex3.rs index 6d79d352..867161cc 100644 --- a/src/query/gjk/voronoi_simplex3.rs +++ b/src/query/gjk/voronoi_simplex3.rs @@ -1,14 +1,11 @@ -use crate::math::{Point, Real}; -use crate::query::gjk::{self, CSOPoint}; +use crate::math::{Real, Vector}; +use crate::query::gjk::{self, CsoPoint}; use crate::query::{PointQuery, PointQueryWithLocation}; use crate::shape::{ Segment, SegmentPointLocation, Tetrahedron, TetrahedronPointLocation, Triangle, TrianglePointLocation, }; -#[cfg(not(feature = "alloc"))] -use na::ComplexField; // for .abs() - /// A simplex of dimension up to 3 that uses Voronoï regions for computing point projections. #[derive(Clone, Debug)] pub struct VoronoiSimplex { @@ -16,7 +13,7 @@ pub struct VoronoiSimplex { prev_proj: [Real; 3], prev_dim: usize, - vertices: [CSOPoint; 4], + vertices: [CsoPoint; 4], proj: [Real; 3], dim: usize, } @@ -34,7 +31,7 @@ impl VoronoiSimplex { prev_vertices: [0, 1, 2, 3], prev_proj: [0.0; 3], prev_dim: 0, - vertices: [CSOPoint::origin(); 4], + vertices: [CsoPoint::ZERO; 4], proj: [0.0; 3], dim: 0, } @@ -47,21 +44,21 @@ impl VoronoiSimplex { } /// Resets this simplex to a single point. - pub fn reset(&mut self, pt: CSOPoint) { + pub fn reset(&mut self, pt: CsoPoint) { self.dim = 0; self.prev_dim = 0; self.vertices[0] = pt; } /// Add a point to this simplex. - pub fn add_point(&mut self, pt: CSOPoint) -> bool { + pub fn add_point(&mut self, pt: CsoPoint) -> bool { self.prev_dim = self.dim; self.prev_proj = self.proj; self.prev_vertices = [0, 1, 2, 3]; match self.dim { 0 => { - if (self.vertices[0] - pt).norm_squared() < gjk::eps_tol() { + if (self.vertices[0] - pt).length_squared() < gjk::eps_tol() { return false; } } @@ -69,7 +66,7 @@ impl VoronoiSimplex { let ab = self.vertices[1] - self.vertices[0]; let ac = pt - self.vertices[0]; - if ab.cross(&ac).norm_squared() < gjk::eps_tol() { + if ab.cross(ac).length_squared() < gjk::eps_tol() { return false; } } @@ -77,9 +74,9 @@ impl VoronoiSimplex { let ab = self.vertices[1] - self.vertices[0]; let ac = self.vertices[2] - self.vertices[0]; let ap = pt - self.vertices[0]; - let n = ab.cross(&ac).normalize(); + let n = ab.cross(ac).normalize(); - if n.dot(&ap).abs() < gjk::eps_tol() { + if n.dot(ap).abs() < gjk::eps_tol() { return false; } } @@ -98,7 +95,7 @@ impl VoronoiSimplex { } /// The i-th point of this simplex. - pub fn point(&self, i: usize) -> &CSOPoint { + pub fn point(&self, i: usize) -> &CsoPoint { assert!(i <= self.dim, "Index out of bounds."); &self.vertices[i] } @@ -110,23 +107,23 @@ impl VoronoiSimplex { } /// The i-th point of the simplex before the last call to `project_origin_and_reduce`. - pub fn prev_point(&self, i: usize) -> &CSOPoint { + pub fn prev_point(&self, i: usize) -> &CsoPoint { assert!(i <= self.prev_dim, "Index out of bounds."); &self.vertices[self.prev_vertices[i]] } /// Projects the origin on the boundary of this simplex and reduces `self` the smallest subsimplex containing the origin. /// - /// Returns the result of the projection or `Point::origin()` if the origin lies inside of the simplex. + /// Returns the result of the projection or `Vector::ZERO` if the origin lies inside of the simplex. /// The state of the simplex before projection is saved, and can be retrieved using the methods prefixed /// by `prev_`. - pub fn project_origin_and_reduce(&mut self) -> Point { + pub fn project_origin_and_reduce(&mut self) -> Vector { if self.dim == 0 { self.proj[0] = 1.0; self.vertices[0].point } else if self.dim == 1 { let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(Vector::ZERO, true); match location { SegmentPointLocation::OnVertex(0) => { @@ -152,7 +149,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(Vector::ZERO, true); match location { TrianglePointLocation::OnVertex(i) => { @@ -192,7 +189,7 @@ impl VoronoiSimplex { self.vertices[2].point, self.vertices[3].point, ) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(Vector::ZERO, true); match location { TetrahedronPointLocation::OnVertex(i) => { @@ -279,21 +276,19 @@ impl VoronoiSimplex { } /// Compute the projection of the origin on the boundary of this simplex. - pub fn project_origin(&mut self) -> Point { + pub fn project_origin(&mut self) -> Vector { if self.dim == 0 { self.vertices[0].point } else if self.dim == 1 { let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point(&Point::::origin(), true) - .point + seg.project_local_point(Vector::ZERO, true).point } else if self.dim == 2 { let tri = Triangle::new( self.vertices[0].point, self.vertices[1].point, self.vertices[2].point, ); - tri.project_local_point(&Point::::origin(), true) - .point + tri.project_local_point(Vector::ZERO, true).point } else { let tetr = Tetrahedron::new( self.vertices[0].point, @@ -301,15 +296,14 @@ impl VoronoiSimplex { self.vertices[2].point, self.vertices[3].point, ); - tetr.project_local_point(&Point::::origin(), true) - .point + tetr.project_local_point(Vector::ZERO, true).point } } /// Tests if the given point is already a vertex of this simplex. - pub fn contains_point(&self, pt: &Point) -> bool { + pub fn contains_point(&self, pt: Vector) -> bool { for i in 0..self.dim + 1 { - if self.vertices[i].point == *pt { + if self.vertices[i].point == pt { return true; } } @@ -332,7 +326,7 @@ impl VoronoiSimplex { let mut max_sq_len = 0.0; for i in 0..self.dim + 1 { - let norm = self.vertices[i].point.coords.norm_squared(); + let norm = self.vertices[i].point.length_squared(); if norm > max_sq_len { max_sq_len = norm @@ -343,7 +337,7 @@ impl VoronoiSimplex { } /// Apply a function to all the vertices of this simplex. - pub fn modify_pnts(&mut self, f: &dyn Fn(&mut CSOPoint)) { + pub fn modify_pnts(&mut self, f: &dyn Fn(&mut CsoPoint)) { for i in 0..self.dim + 1 { f(&mut self.vertices[i]) } diff --git a/src/query/intersection_test/intersection_test.rs b/src/query/intersection_test/intersection_test.rs index 347e4899..d714044b 100644 --- a/src/query/intersection_test/intersection_test.rs +++ b/src/query/intersection_test/intersection_test.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; use crate::shape::Shape; @@ -47,20 +47,20 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::intersection_test; /// use parry3d::shape::Ball; -/// use nalgebra::Isometry3; +/// use parry3d::math::Pose; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Overlapping balls -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(1.5, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(1.5, 0.0, 0.0); /// /// let intersecting = intersection_test(&pos1, &ball1, &pos2, &ball2).unwrap(); /// assert!(intersecting); // Distance 1.5 < combined radii 2.0 /// /// // Separated balls -/// let pos3 = Isometry3::translation(5.0, 0.0, 0.0); +/// let pos3 = Pose::translation(5.0, 0.0, 0.0); /// let not_intersecting = intersection_test(&pos1, &ball1, &pos3, &ball2).unwrap(); /// assert!(!not_intersecting); // Distance 5.0 > combined radii 2.0 /// # } @@ -86,9 +86,9 @@ use crate::shape::Shape; /// - [`distance`](crate::query::distance()) - Get separation distance /// - [`closest_points`](crate::query::closest_points()) - Get closest point locations pub fn intersection_test( - pos1: &Isometry, + pos1: &Pose, g1: &dyn Shape, - pos2: &Isometry, + pos2: &Pose, g2: &dyn Shape, ) -> Result { let pos12 = pos1.inv_mul(pos2); diff --git a/src/query/intersection_test/intersection_test_ball_ball.rs b/src/query/intersection_test/intersection_test_ball_ball.rs index 2aed7829..66ae4f21 100644 --- a/src/query/intersection_test/intersection_test_ball_ball.rs +++ b/src/query/intersection_test/intersection_test_ball_ball.rs @@ -1,12 +1,12 @@ -use crate::math::{Point, Real}; +use crate::math::Vector; use crate::shape::Ball; /// Intersection test between balls. #[inline] -pub fn intersection_test_ball_ball(center12: &Point, b1: &Ball, b2: &Ball) -> bool { +pub fn intersection_test_ball_ball(center12: Vector, b1: &Ball, b2: &Ball) -> bool { let r1 = b1.radius; let r2 = b2.radius; - let distance_squared = center12.coords.norm_squared(); + let distance_squared = center12.length_squared(); let sum_radius = r1 + r2; distance_squared <= sum_radius * sum_radius } diff --git a/src/query/intersection_test/intersection_test_ball_point_query.rs b/src/query/intersection_test/intersection_test_ball_point_query.rs index 8dc68b7b..7dc35bed 100644 --- a/src/query/intersection_test/intersection_test_ball_point_query.rs +++ b/src/query/intersection_test/intersection_test_ball_point_query.rs @@ -1,10 +1,10 @@ -use crate::math::{Isometry, Point, Real}; +use crate::math::Pose; use crate::query::PointQuery; use crate::shape::Ball; /// Intersection test between a ball and a shape implementing the `PointQuery` trait. pub fn intersection_test_ball_point_query( - pos12: &Isometry, + pos12: &Pose, ball1: &Ball, point_query2: &P, ) -> bool { @@ -13,11 +13,11 @@ pub fn intersection_test_ball_point_query( /// Intersection test between a shape implementing the `PointQuery` trait and a ball. pub fn intersection_test_point_query_ball( - pos12: &Isometry, + pos12: &Pose, point_query1: &P, ball2: &Ball, ) -> bool { - let local_p2_1 = Point::from(pos12.translation.vector); - let proj = point_query1.project_local_point(&local_p2_1, true); - proj.is_inside || (local_p2_1 - proj.point).norm_squared() <= ball2.radius * ball2.radius + let local_p2_1 = pos12.translation; + let proj = point_query1.project_local_point(local_p2_1, true); + proj.is_inside || (local_p2_1 - proj.point).length_squared() <= ball2.radius * ball2.radius } diff --git a/src/query/intersection_test/intersection_test_composite_shape_shape.rs b/src/query/intersection_test/intersection_test_composite_shape_shape.rs index ca1d6968..266a5a4a 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/src/query/intersection_test/intersection_test_composite_shape_shape.rs @@ -1,9 +1,9 @@ use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::partitioning::BvhNode; use crate::query::QueryDispatcher; use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; -use crate::utils::IsometryOpt; +use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { /// Returns the index of the shape in `self` that intersects the given other `shape` positioned @@ -13,7 +13,7 @@ impl CompositeShapeRef<'_, S> { pub fn intersects_shape( &self, dispatcher: &D, - pose12: &Isometry, + pose12: &Pose, shape: &dyn Shape, ) -> Option { let ls_aabb2 = shape.compute_aabb(pose12); @@ -34,7 +34,7 @@ impl CompositeShapeRef<'_, S> { /// Intersection test between a composite shape (`Mesh`, `Compound`) and any other shape. pub fn intersection_test_composite_shape_shape( dispatcher: &D, - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &dyn Shape, ) -> bool @@ -50,7 +50,7 @@ where /// Proximity between a shape and a composite (`Mesh`, `Compound`) shape. pub fn intersection_test_shape_composite_shape( dispatcher: &D, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &G2, ) -> bool diff --git a/src/query/intersection_test/intersection_test_cuboid_cuboid.rs b/src/query/intersection_test/intersection_test_cuboid_cuboid.rs index ea9f7fab..48fc3c93 100644 --- a/src/query/intersection_test/intersection_test_cuboid_cuboid.rs +++ b/src/query/intersection_test/intersection_test_cuboid_cuboid.rs @@ -1,14 +1,10 @@ -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::query::sat; use crate::shape::Cuboid; /// Intersection test between cuboids. #[inline] -pub fn intersection_test_cuboid_cuboid( - pos12: &Isometry, - cuboid1: &Cuboid, - cuboid2: &Cuboid, -) -> bool { +pub fn intersection_test_cuboid_cuboid(pos12: &Pose, cuboid1: &Cuboid, cuboid2: &Cuboid) -> bool { let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, pos12).0; if sep1 > 0.0 { diff --git a/src/query/intersection_test/intersection_test_cuboid_segment.rs b/src/query/intersection_test/intersection_test_cuboid_segment.rs index ee3fe146..52bf526c 100644 --- a/src/query/intersection_test/intersection_test_cuboid_segment.rs +++ b/src/query/intersection_test/intersection_test_cuboid_segment.rs @@ -1,19 +1,19 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Rotation}; use crate::query::sat; use crate::shape::{Cuboid, Segment}; /// Test if a segment intersects an Aabb. pub fn intersection_test_aabb_segment(aabb1: &Aabb, segment2: &Segment) -> bool { let cuboid1 = Cuboid::new(aabb1.half_extents()); - let pos12 = Isometry::from_parts((-aabb1.center().coords).into(), na::one()); + let pos12 = Pose::from_parts(-aabb1.center(), Rotation::IDENTITY); intersection_test_cuboid_segment(&pos12, &cuboid1, segment2) } /// Test if a segment intersects a cuboid. #[inline] pub fn intersection_test_segment_cuboid( - pos12: &Isometry, + pos12: &Pose, segment1: &Segment, cuboid2: &Cuboid, ) -> bool { @@ -22,11 +22,7 @@ pub fn intersection_test_segment_cuboid( /// Test if a segment intersects a cuboid. #[inline] -pub fn intersection_test_cuboid_segment( - pos12: &Isometry, - cube1: &Cuboid, - segment2: &Segment, -) -> bool { +pub fn intersection_test_cuboid_segment(pos12: &Pose, cube1: &Cuboid, segment2: &Segment) -> bool { let sep1 = sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, segment2, pos12).0; if sep1 > 0.0 { diff --git a/src/query/intersection_test/intersection_test_cuboid_triangle.rs b/src/query/intersection_test/intersection_test_cuboid_triangle.rs index fc6cf6d4..1f9b7a00 100644 --- a/src/query/intersection_test/intersection_test_cuboid_triangle.rs +++ b/src/query/intersection_test/intersection_test_cuboid_triangle.rs @@ -1,19 +1,19 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Rotation}; use crate::query::sat; use crate::shape::{Cuboid, Triangle}; /// Tests if a triangle intersects an Aabb. pub fn intersection_test_aabb_triangle(aabb1: &Aabb, triangle2: &Triangle) -> bool { let cuboid1 = Cuboid::new(aabb1.half_extents()); - let pos12 = Isometry::from_parts((-aabb1.center().coords).into(), na::one()); + let pos12 = Pose::from_parts(-aabb1.center(), Rotation::IDENTITY); intersection_test_cuboid_triangle(&pos12, &cuboid1, triangle2) } /// Tests if a triangle intersects a cuboid. #[inline] pub fn intersection_test_triangle_cuboid( - pos12: &Isometry, + pos12: &Pose, triangle1: &Triangle, cuboid2: &Cuboid, ) -> bool { @@ -23,7 +23,7 @@ pub fn intersection_test_triangle_cuboid( /// Tests if a triangle intersects an cuboid. #[inline] pub fn intersection_test_cuboid_triangle( - pos12: &Isometry, + pos12: &Pose, cube1: &Cuboid, triangle2: &Triangle, ) -> bool { diff --git a/src/query/intersection_test/intersection_test_halfspace_support_map.rs b/src/query/intersection_test/intersection_test_halfspace_support_map.rs index e27eafa4..c373296c 100644 --- a/src/query/intersection_test/intersection_test_halfspace_support_map.rs +++ b/src/query/intersection_test/intersection_test_halfspace_support_map.rs @@ -1,20 +1,20 @@ -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::shape::HalfSpace; use crate::shape::SupportMap; /// Intersection test between a halfspace and a support-mapped shape (Cuboid, ConvexHull, etc.) pub fn intersection_test_halfspace_support_map( - pos12: &Isometry, + pos12: &Pose, halfspace: &HalfSpace, other: &G, ) -> bool { - let deepest = other.support_point_toward(pos12, &-halfspace.normal); - halfspace.normal.dot(&deepest.coords) <= 0.0 + let deepest = other.support_point_toward(pos12, -halfspace.normal); + halfspace.normal.dot(deepest) <= 0.0 } /// Intersection test between a support-mapped shape (Cuboid, ConvexHull, etc.) and a halfspace. pub fn intersection_test_support_map_halfspace( - pos12: &Isometry, + pos12: &Pose, other: &G, halfspace: &HalfSpace, ) -> bool { diff --git a/src/query/intersection_test/intersection_test_polygon_polygon.rs b/src/query/intersection_test/intersection_test_polygon_polygon.rs index d84abfb6..6a085b57 100644 --- a/src/query/intersection_test/intersection_test_polygon_polygon.rs +++ b/src/query/intersection_test/intersection_test_polygon_polygon.rs @@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; use crate::geometry::{sat, Polygon, Proximity}; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real}; pub fn detect_proximity_polygon_polygon( _ctxt: &mut PrimitiveProximityDetectionContext, @@ -24,9 +24,9 @@ pub fn detect_proximity_polygon_polygon( fn detect_proximity<'a>( prediction_distance: Real, p1: &'a Polygon, - m1: &'a Isometry, + m1: &'a Pose, p2: &'a Polygon, - m2: &'a Isometry, + m2: &'a Pose, ) -> Proximity { let m12 = m1.inv_mul(&m2); let m21 = m12.inverse(); diff --git a/src/query/intersection_test/intersection_test_support_map_support_map.rs b/src/query/intersection_test/intersection_test_support_map_support_map.rs index db71997d..6d87e578 100644 --- a/src/query/intersection_test/intersection_test_support_map_support_map.rs +++ b/src/query/intersection_test/intersection_test_support_map_support_map.rs @@ -1,15 +1,9 @@ -use na::{self, Unit}; - -use crate::math::{Isometry, Real, Vector}; -use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex}; +use crate::math::{Pose, Vector}; +use crate::query::gjk::{self, CsoPoint, GJKResult, VoronoiSimplex}; use crate::shape::SupportMap; /// Intersection test between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.) -pub fn intersection_test_support_map_support_map( - pos12: &Isometry, - g1: &G1, - g2: &G2, -) -> bool +pub fn intersection_test_support_map_support_map(pos12: &Pose, g1: &G1, g2: &G2) -> bool where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -28,27 +22,25 @@ where /// /// This allows a more fine grained control other the underlying GJK algorithm. pub fn intersection_test_support_map_support_map_with_params( - pos12: &Isometry, + pos12: &Pose, g1: &G1, g2: &G2, simplex: &mut VoronoiSimplex, - init_dir: Option>>, -) -> (bool, Unit>) + init_dir: Option, +) -> (bool, Vector) where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let dir = if let Some(init_dir) = init_dir { init_dir - } else if let Some(init_dir) = - Unit::try_new(pos12.translation.vector, crate::math::DEFAULT_EPSILON) - { + } else if let Some(init_dir) = (pos12.translation).try_normalize() { init_dir } else { - Vector::x_axis() + Vector::X }; - simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir)); + simplex.reset(CsoPoint::from_shapes(pos12, g1, g2, dir)); match gjk::closest_points(pos12, g1, g2, 0.0, false, simplex) { GJKResult::Intersection => (true, dir), diff --git a/src/query/intersection_test/intersection_test_voxels_shape.rs b/src/query/intersection_test/intersection_test_voxels_shape.rs index 323de876..5e36affc 100644 --- a/src/query/intersection_test/intersection_test_voxels_shape.rs +++ b/src/query/intersection_test/intersection_test_voxels_shape.rs @@ -1,11 +1,11 @@ -use crate::math::{Isometry, Real, Translation}; +use crate::math::Pose; use crate::query::PersistentQueryDispatcher; use crate::shape::{Cuboid, Shape, VoxelType, Voxels}; /// Checks for any intersection between voxels and an arbitrary shape, both represented as a `Shape` trait-object. pub fn intersection_test_voxels_shape_shapes( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, ) -> bool { @@ -21,7 +21,7 @@ pub fn intersection_test_voxels_shape_shapes( /// Checks for any intersection between voxels and an arbitrary shape. pub fn intersection_test_voxels_shape( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, voxels1: &Voxels, shape2: &dyn Shape, ) -> bool { @@ -39,7 +39,7 @@ pub fn intersection_test_voxels_shape( let center1 = vox1.center; let cuboid1 = Cuboid::new(radius1); - let cuboid_pose12 = Translation::from(-center1) * pos12; + let cuboid_pose12 = Pose::from_translation(-center1) * pos12; if dispatcher .intersection_test(&cuboid_pose12, &cuboid1, shape2) @@ -56,7 +56,7 @@ pub fn intersection_test_voxels_shape( /// Checks for any intersection between voxels and an arbitrary shape. pub fn intersection_test_shape_voxels( dispatcher: &dyn PersistentQueryDispatcher, - pos12: &Isometry, + pos12: &Pose, shape1: &dyn Shape, voxels2: &Voxels, ) -> bool { diff --git a/src/query/nonlinear_shape_cast/mod.rs b/src/query/nonlinear_shape_cast/mod.rs index 1b9fceb5..cd77b84d 100644 --- a/src/query/nonlinear_shape_cast/mod.rs +++ b/src/query/nonlinear_shape_cast/mod.rs @@ -62,26 +62,26 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] { //! use parry3d::query::{cast_shapes, cast_shapes_nonlinear, ShapeCastOptions, NonlinearRigidMotion}; //! use parry3d::shape::Cuboid; -//! use parry3d::na::{Isometry3, Vector3}; +//! use parry3d::math::{Pose, Vector}; //! -//! let cube1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -//! let cube2 = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); +//! let cube1 = Cuboid::new(Vector::splat(1.0)); +//! let cube2 = Cuboid::new(Vector::splat(0.5)); //! -//! let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -//! let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +//! let pos1 = Pose::translation(0.0, 0.0, 0.0); +//! let pos2 = Pose::translation(5.0, 0.0, 0.0); //! //! // Linear motion: cube moves right -//! let vel1 = Vector3::new(1.0, 0.0, 0.0); +//! let vel1 = Vector::new(1.0, 0.0, 0.0); //! let options = ShapeCastOptions::default(); //! -//! let linear_hit = cast_shapes(&pos1, &vel1, &cube1, &pos2, &Vector3::zeros(), &cube2, options); +//! let linear_hit = cast_shapes(&pos1, vel1, &cube1, &pos2, Vector::ZERO, &cube2, options); //! //! // Nonlinear motion: cube moves right AND spins around Y axis //! let motion1 = NonlinearRigidMotion::new( //! pos1, -//! parry3d::na::Point3::origin(), // rotation center +//! Vector::ZERO, // rotation center //! vel1, // linear velocity -//! Vector3::new(0.0, 5.0, 0.0), // angular velocity (spinning fast) +//! Vector::new(0.0, 5.0, 0.0), // angular velocity (spinning fast) //! ); //! let motion2 = NonlinearRigidMotion::constant_position(pos2); //! diff --git a/src/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs b/src/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs index 2104566c..586c55c1 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real, Translation, Vector}; +use crate::math::{Pose, Real, Vector}; /// Describes the complete motion of a rigid body with both translation and rotation. /// @@ -29,7 +29,7 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// /// * `start` - The initial position and orientation at time `t = 0` /// * `local_center` - The point (in the shape's local coordinate system) around which -/// rotation occurs. For most cases, use the center of mass or `Point::origin()`. +/// rotation occurs. For most cases, use the center of mass or `Vector::ZERO`. /// * `linvel` - Linear velocity vector (units per second). Direction is the direction /// of motion, magnitude is speed. /// * `angvel` - Angular velocity: @@ -41,19 +41,19 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// // Object moving right at 5 units/second, no rotation /// let motion = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 0.0, 0.0), // start at origin -/// Point3::origin(), // rotation center (irrelevant here) -/// Vector3::new(5.0, 0.0, 0.0), // moving right -/// Vector3::zeros(), // not rotating +/// Pose::translation(0.0, 0.0, 0.0), // start at origin +/// Vector::ZERO, // rotation center (irrelevant here) +/// Vector::new(5.0, 0.0, 0.0), // moving right +/// Vector::ZERO, // not rotating /// ); /// /// // At t=2.0 seconds, object has moved 10 units right /// let pos_at_2 = motion.position_at_time(2.0); -/// assert_eq!(pos_at_2.translation.vector.x, 10.0); +/// assert_eq!(pos_at_2.translation.x, 10.0); /// # } /// ``` /// @@ -62,15 +62,15 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// use std::f32::consts::PI; /// /// // Object spinning around Y axis, no translation /// let motion = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 0.0, 0.0), -/// Point3::origin(), // rotate around origin -/// Vector3::zeros(), // not translating -/// Vector3::new(0.0, PI, 0.0), // rotating around Y at π rad/s (180°/s) +/// Pose::translation(0.0, 0.0, 0.0), +/// Vector::ZERO, // rotate around origin +/// Vector::ZERO, // not translating +/// Vector::new(0.0, PI, 0.0), // rotating around Y at π rad/s (180°/s) /// ); /// /// // At t=1.0 second, object has rotated 180 degrees @@ -84,14 +84,14 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// // Spinning projectile moving forward /// let motion = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 0.0, 0.0), -/// Point3::origin(), -/// Vector3::new(10.0, 0.0, 0.0), // moving forward at 10 units/s -/// Vector3::new(20.0, 0.0, 0.0), // spinning around its movement axis +/// Pose::translation(0.0, 0.0, 0.0), +/// Vector::ZERO, +/// Vector::new(10.0, 0.0, 0.0), // moving forward at 10 units/s +/// Vector::new(20.0, 0.0, 0.0), // spinning around its movement axis /// ); /// /// // The object traces a helical path (like a bullet with rifling) @@ -100,20 +100,20 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// # } /// ``` /// -/// # Example: Rotation Around Off-Center Point +/// # Example: Rotation Around Off-Center Vector /// /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// // Object rotating around a point that's NOT its center /// // Useful for: swinging weapons, rotating around pivot point, etc. /// let motion = NonlinearRigidMotion::new( -/// Isometry3::translation(5.0, 0.0, 0.0), // object is at x=5 -/// Point3::new(-5.0, 0.0, 0.0), // rotate around x=0 (in local space) -/// Vector3::zeros(), -/// Vector3::new(0.0, 1.0, 0.0), // rotate around Y axis at 1 rad/s +/// Pose::translation(5.0, 0.0, 0.0), // object is at x=5 +/// Vector::new(-5.0, 0.0, 0.0), // rotate around x=0 (in local space) +/// Vector::ZERO, +/// Vector::new(0.0, 1.0, 0.0), // rotate around Y axis at 1 rad/s /// ); /// /// // The object orbits in a circle around the world origin @@ -128,12 +128,12 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::NonlinearRigidMotion; -/// # use nalgebra::{Isometry3, Point3, Vector3}; +/// # use parry3d::math::{Pose, Vector}; /// let bullet = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 1.5, 0.0), -/// Point3::origin(), -/// Vector3::new(100.0, -2.0, 0.0), // fast forward, slight drop -/// Vector3::new(50.0, 0.0, 0.0), // high spin rate +/// Pose::translation(0.0, 1.5, 0.0), +/// Vector::ZERO, +/// Vector::new(100.0, -2.0, 0.0), // fast forward, slight drop +/// Vector::new(50.0, 0.0, 0.0), // high spin rate /// ); /// # } /// ``` @@ -143,12 +143,12 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::NonlinearRigidMotion; -/// # use nalgebra::{Isometry3, Point3, Vector3}; +/// # use parry3d::math::{Pose, Vector}; /// let debris = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 2.0, 0.0), -/// Point3::origin(), -/// Vector3::new(3.0, 5.0, -2.0), // chaotic velocity -/// Vector3::new(2.0, -3.0, 1.5), // chaotic rotation +/// Pose::translation(0.0, 2.0, 0.0), +/// Vector::ZERO, +/// Vector::new(3.0, 5.0, -2.0), // chaotic velocity +/// Vector::new(2.0, -3.0, 1.5), // chaotic rotation /// ); /// # } /// ``` @@ -158,12 +158,12 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::NonlinearRigidMotion; -/// # use nalgebra::{Isometry3, Point3, Vector3}; +/// # use parry3d::math::{Pose, Vector}; /// let blade = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 1.0, 0.0), -/// Point3::origin(), // spin around center -/// Vector3::zeros(), // blade doesn't translate -/// Vector3::new(0.0, 10.0, 0.0), // fast rotation +/// Pose::translation(0.0, 1.0, 0.0), +/// Vector::ZERO, // spin around center +/// Vector::ZERO, // blade doesn't translate +/// Vector::new(0.0, 10.0, 0.0), // fast rotation /// ); /// # } /// ``` @@ -173,9 +173,9 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::query::NonlinearRigidMotion; -/// # use nalgebra::Isometry3; +/// # use parry3d::math::Pose; /// let wall = NonlinearRigidMotion::constant_position( -/// Isometry3::translation(10.0, 0.0, 0.0) +/// Pose::translation(10.0, 0.0, 0.0) /// ); /// # } /// ``` @@ -192,12 +192,12 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::query::NonlinearRigidMotion; -/// use nalgebra::{Isometry2, Point2, Vector2}; +/// use parry2d::math::{Pose, Vector}; /// /// let motion = NonlinearRigidMotion::new( -/// Isometry2::translation(0.0, 0.0), -/// Point2::origin(), -/// Vector2::zeros(), +/// Pose::translation(0.0, 0.0), +/// Vector::ZERO, +/// Vector::ZERO, /// 3.14, // rotating counter-clockwise at π rad/s /// ); /// # } @@ -211,13 +211,13 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let motion = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 0.0, 0.0), -/// Point3::origin(), -/// Vector3::zeros(), -/// Vector3::new(0.0, 3.14, 0.0), // rotate around Y axis at π rad/s +/// Pose::translation(0.0, 0.0, 0.0), +/// Vector::ZERO, +/// Vector::ZERO, +/// Vector::new(0.0, 3.14, 0.0), // rotate around Y axis at π rad/s /// ); /// # } /// ``` @@ -226,7 +226,7 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// /// 1. **Local vs World Space**: The `local_center` must be in the **shape's local /// coordinate system**, not world space. For a shape centered at origin, -/// use `Point::origin()`. +/// use `Vector::ZERO`. /// /// 2. **Angular Velocity Units**: Always use **radians per second**, not degrees! /// - To convert: `degrees * (PI / 180.0) = radians` @@ -239,7 +239,7 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; /// - Create new `NonlinearRigidMotion` for next timestep /// /// 4. **Center of Mass**: For realistic physics, `local_center` should be the -/// shape's center of mass. For simple cases, `Point::origin()` often works. +/// shape's center of mass. For simple cases, `Vector::ZERO` often works. /// /// # See Also /// @@ -249,17 +249,17 @@ use crate::math::{Isometry, Point, Real, Translation, Vector}; #[derive(Debug, Copy, Clone)] pub struct NonlinearRigidMotion { /// The starting isometry at `t = 0`. - pub start: Isometry, + pub start: Pose, /// The local-space point at which the rotational part of this motion is applied. - pub local_center: Point, + pub local_center: Vector, /// The translational velocity of this motion. - pub linvel: Vector, + pub linvel: Vector, /// The angular velocity of this motion. #[cfg(feature = "dim2")] pub angvel: Real, /// The angular velocity of this motion. #[cfg(feature = "dim3")] - pub angvel: Vector, + pub angvel: Vector, } impl NonlinearRigidMotion { @@ -268,7 +268,7 @@ impl NonlinearRigidMotion { /// # Arguments /// /// * `start` - Initial position and orientation at time `t = 0` - /// * `local_center` - Point (in local coordinates) around which rotation occurs + /// * `local_center` - Vector (in local coordinates) around which rotation occurs /// * `linvel` - Linear velocity vector (units per second) /// * `angvel` - Angular velocity (2D: radians/sec scalar, 3D: axis-angle vector) /// @@ -277,14 +277,14 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::query::NonlinearRigidMotion; - /// use nalgebra::{Isometry2, Point2, Vector2}; + /// use parry2d::math::{Pose, Vector}; /// use std::f32::consts::PI; /// /// // Object moving right and rotating counter-clockwise /// let motion = NonlinearRigidMotion::new( - /// Isometry2::translation(0.0, 0.0), - /// Point2::origin(), - /// Vector2::new(5.0, 0.0), // 5 units/sec to the right + /// Pose::translation(0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(5.0, 0.0), // 5 units/sec to the right /// PI, // π rad/sec counter-clockwise /// ); /// # } @@ -295,24 +295,19 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// // Object moving forward and spinning around its movement axis /// let motion = NonlinearRigidMotion::new( - /// Isometry3::translation(0.0, 0.0, 0.0), - /// Point3::origin(), - /// Vector3::new(10.0, 0.0, 0.0), // moving forward - /// Vector3::new(5.0, 0.0, 0.0), // spinning around X axis + /// Pose::translation(0.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(10.0, 0.0, 0.0), // moving forward + /// Vector::new(5.0, 0.0, 0.0), // spinning around X axis /// ); /// # } /// ``` #[cfg(feature = "dim2")] - pub fn new( - start: Isometry, - local_center: Point, - linvel: Vector, - angvel: Real, - ) -> Self { + pub fn new(start: Pose, local_center: Vector, linvel: Vector, angvel: Real) -> Self { NonlinearRigidMotion { start, local_center, @@ -326,7 +321,7 @@ impl NonlinearRigidMotion { /// # Arguments /// /// * `start` - Initial position and orientation at time `t = 0` - /// * `local_center` - Point (in local coordinates) around which rotation occurs + /// * `local_center` - Vector (in local coordinates) around which rotation occurs /// * `linvel` - Linear velocity vector (units per second) /// * `angvel` - Angular velocity as axis-angle vector (radians per second) /// @@ -335,24 +330,19 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// // Object moving forward and spinning around its movement axis /// let motion = NonlinearRigidMotion::new( - /// Isometry3::translation(0.0, 0.0, 0.0), - /// Point3::origin(), - /// Vector3::new(10.0, 0.0, 0.0), // moving forward - /// Vector3::new(5.0, 0.0, 0.0), // spinning around X axis + /// Pose::translation(0.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(10.0, 0.0, 0.0), // moving forward + /// Vector::new(5.0, 0.0, 0.0), // spinning around X axis /// ); /// # } /// ``` #[cfg(feature = "dim3")] - pub fn new( - start: Isometry, - local_center: Point, - linvel: Vector, - angvel: Vector, - ) -> Self { + pub fn new(start: Pose, local_center: Vector, linvel: Vector, angvel: Vector) -> Self { NonlinearRigidMotion { start, local_center, @@ -363,7 +353,7 @@ impl NonlinearRigidMotion { /// Creates a stationary motion at the origin (identity transformation). /// - /// Equivalent to `constant_position(Isometry::identity())`. + /// Equivalent to `constant_position(Pose::identity())`. /// /// # Example /// @@ -376,7 +366,7 @@ impl NonlinearRigidMotion { /// # } /// ``` pub fn identity() -> Self { - Self::constant_position(Isometry::identity()) + Self::constant_position(Pose::IDENTITY) } /// Creates a motion that stays at a constant position (no translation, no rotation). @@ -393,31 +383,34 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; - /// use nalgebra::Isometry3; + /// use parry3d::math::Pose; /// /// // A wall that never moves /// let wall_motion = NonlinearRigidMotion::constant_position( - /// Isometry3::translation(10.0, 0.0, 0.0) + /// Pose::translation(10.0, 0.0, 0.0) /// ); /// /// // At any time t, position is always (10, 0, 0) /// let pos_at_5 = wall_motion.position_at_time(5.0); - /// assert_eq!(pos_at_5.translation.vector.x, 10.0); + /// assert_eq!(pos_at_5.translation.x, 10.0); /// # } /// ``` - pub fn constant_position(pos: Isometry) -> Self { + pub fn constant_position(pos: Pose) -> Self { Self { start: pos, - linvel: na::zero(), - angvel: na::zero(), - local_center: Point::origin(), + linvel: Vector::ZERO, + #[cfg(feature = "dim2")] + angvel: 0.0, + #[cfg(feature = "dim3")] + angvel: Vector::ZERO, + local_center: Vector::ZERO, } } - fn set_start(&mut self, new_start: Isometry) { + fn set_start(&mut self, new_start: Pose) { // NOTE: we need to adjust the local_center so that the angular // velocity is still expressed wrt. the original center. - self.local_center = new_start.inverse_transform_point(&(self.start * self.local_center)); + self.local_center = new_start.inverse_transform_point(self.start * self.local_center); self.start = new_start; } @@ -440,13 +433,13 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// let mut motion = NonlinearRigidMotion::new( - /// Isometry3::translation(0.0, 0.0, 0.0), - /// Point3::origin(), - /// Vector3::new(5.0, 0.0, 0.0), // moving right - /// Vector3::zeros(), + /// Pose::translation(0.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(5.0, 0.0, 0.0), // moving right + /// Vector::ZERO, /// ); /// /// // Freeze at t=2.0 (when object is at x=10) @@ -454,34 +447,41 @@ impl NonlinearRigidMotion { /// /// // Now position is constant at x=10, regardless of time /// let pos_at_100 = motion.position_at_time(100.0); - /// assert_eq!(pos_at_100.translation.vector.x, 10.0); + /// assert_eq!(pos_at_100.translation.x, 10.0); /// # } /// ``` pub fn freeze(&mut self, t: Real) { self.start = self.position_at_time(t); - self.linvel = na::zero(); - self.angvel = na::zero(); + self.linvel = Vector::ZERO; + #[cfg(feature = "dim2")] + { + self.angvel = 0.0; + } + #[cfg(feature = "dim3")] + { + self.angvel = Vector::ZERO; + } } /// Appends a constant translation to this rigid-motion. #[must_use] - pub fn append_translation(&self, tra: Vector) -> Self { + pub fn append_translation(&self, tra: Vector) -> Self { let mut result = *self; - result.set_start(Translation::from(tra) * result.start); + result.set_start(Pose::from_translation(tra) * result.start); result } /// Prepends a constant translation to this rigid-motion. #[must_use] - pub fn prepend_translation(&self, tra: Vector) -> Self { + pub fn prepend_translation(&self, tra: Vector) -> Self { let mut result = *self; - result.set_start(result.start * Translation::from(tra)); + result.set_start(result.start * Pose::from_translation(tra)); result } /// Appends a constant isometry to this rigid-motion. #[must_use] - pub fn append(&self, iso: Isometry) -> Self { + pub fn append(&self, iso: Pose) -> Self { let mut result = *self; result.set_start(iso * result.start); result @@ -489,7 +489,7 @@ impl NonlinearRigidMotion { /// Prepends a constant translation to this rigid-motion. #[must_use] - pub fn prepend(&self, iso: Isometry) -> Self { + pub fn prepend(&self, iso: Pose) -> Self { let mut result = *self; result.set_start(result.start * iso); result @@ -518,23 +518,23 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// let motion = NonlinearRigidMotion::new( - /// Isometry3::translation(0.0, 0.0, 0.0), - /// Point3::origin(), - /// Vector3::new(3.0, 0.0, 0.0), // 3 units/sec to the right - /// Vector3::new(0.0, 1.0, 0.0), // 1 radian/sec around Y axis + /// Pose::translation(0.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(3.0, 0.0, 0.0), // 3 units/sec to the right + /// Vector::new(0.0, 1.0, 0.0), // 1 radian/sec around Y axis /// ); /// /// // Position at t=0 /// let pos_0 = motion.position_at_time(0.0); - /// assert_eq!(pos_0.translation.vector.x, 0.0); + /// assert_eq!(pos_0.translation.x, 0.0); /// /// // Position at t=2.0 seconds /// let pos_2 = motion.position_at_time(2.0); /// // Object has moved 6 units to the right - /// assert!((pos_2.translation.vector.x - 6.0).abs() < 0.01); + /// assert!((pos_2.translation.x - 6.0).abs() < 0.01); /// // And rotated 2 radians around Y /// # } /// ``` @@ -544,13 +544,13 @@ impl NonlinearRigidMotion { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::NonlinearRigidMotion; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// let motion = NonlinearRigidMotion::new( - /// Isometry3::translation(0.0, 5.0, 0.0), - /// Point3::origin(), - /// Vector3::new(0.0, -9.8, 0.0), // falling (gravity) - /// Vector3::new(1.0, 2.0, 0.5), // tumbling + /// Pose::translation(0.0, 5.0, 0.0), + /// Vector::ZERO, + /// Vector::new(0.0, -9.8, 0.0), // falling (gravity) + /// Vector::new(1.0, 2.0, 0.5), // tumbling /// ); /// /// // Render at 60 FPS @@ -562,9 +562,9 @@ impl NonlinearRigidMotion { /// } /// # } /// ``` - pub fn position_at_time(&self, t: Real) -> Isometry { + pub fn position_at_time(&self, t: Real) -> Pose { let center = self.start * self.local_center; - let shift = Translation::from(center.coords); - (shift * Isometry::new(self.linvel * t, self.angvel * t)) * (shift.inverse() * self.start) + let shift = Pose::from_translation(center); + (shift * Pose::new(self.linvel * t, self.angvel * t)) * (shift.inverse() * self.start) } } diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs index 60f25a10..a9101899 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs @@ -75,22 +75,22 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{cast_shapes_nonlinear, NonlinearRigidMotion}; /// use parry3d::shape::Ball; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Ball 1: moving right AND spinning around Y axis /// let motion1 = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 0.0, 0.0), // start position -/// Point3::origin(), // rotation center (local space) -/// Vector3::new(2.0, 0.0, 0.0), // moving right at speed 2 -/// Vector3::new(0.0, 10.0, 0.0), // spinning around Y axis +/// Pose::translation(0.0, 0.0, 0.0), // start position +/// Vector::ZERO, // rotation center (local space) +/// Vector::new(2.0, 0.0, 0.0), // moving right at speed 2 +/// Vector::new(0.0, 10.0, 0.0), // spinning around Y axis /// ); /// /// // Ball 2: stationary at x=10 /// let motion2 = NonlinearRigidMotion::constant_position( -/// Isometry3::translation(10.0, 0.0, 0.0) +/// Pose::translation(10.0, 0.0, 0.0) /// ); /// /// let result = cast_shapes_nonlinear( @@ -115,25 +115,25 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{cast_shapes_nonlinear, NonlinearRigidMotion}; /// use parry3d::shape::Cuboid; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// // A cuboid tumbling through space -/// let cube = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); +/// let cube = Cuboid::new(Vector::new(0.5, 0.5, 0.5)); /// /// // Large wall (very wide cuboid) -/// let wall = Cuboid::new(Vector3::new(10.0, 10.0, 0.1)); +/// let wall = Cuboid::new(Vector::new(10.0, 10.0, 0.1)); /// /// // Cube falling and tumbling /// let motion_cube = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 5.0, 0.0), // starting 5 units above -/// Point3::origin(), // rotate around center -/// Vector3::new(0.0, -2.0, 0.0), // falling down -/// Vector3::new(1.0, 0.5, 2.0), // tumbling (complex rotation) +/// Pose::translation(0.0, 5.0, 0.0), // starting 5 units above +/// Vector::ZERO, // rotate around center +/// Vector::new(0.0, -2.0, 0.0), // falling down +/// Vector::new(1.0, 0.5, 2.0), // tumbling (complex rotation) /// ); /// /// // Wall is stationary /// let motion_wall = NonlinearRigidMotion::constant_position( -/// Isometry3::translation(0.0, 0.0, 0.0) +/// Pose::translation(0.0, 0.0, 0.0) /// ); /// /// let result = cast_shapes_nonlinear( @@ -158,21 +158,21 @@ use crate::shape::Shape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{cast_shapes_nonlinear, NonlinearRigidMotion}; /// use parry3d::shape::Ball; -/// use nalgebra::{Isometry3, Point3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball1 = Ball::new(2.0); /// let ball2 = Ball::new(2.0); /// /// // Balls overlapping (3 units apart, but radii sum to 4) /// let motion1 = NonlinearRigidMotion::new( -/// Isometry3::translation(0.0, 0.0, 0.0), -/// Point3::origin(), -/// Vector3::new(-1.0, 0.0, 0.0), // moving AWAY from ball2 -/// Vector3::zeros(), +/// Pose::translation(0.0, 0.0, 0.0), +/// Vector::ZERO, +/// Vector::new(-1.0, 0.0, 0.0), // moving AWAY from ball2 +/// Vector::ZERO, /// ); /// /// let motion2 = NonlinearRigidMotion::constant_position( -/// Isometry3::translation(3.0, 0.0, 0.0) +/// Pose::translation(3.0, 0.0, 0.0) /// ); /// /// // With stop_at_penetration = true diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs index a9ad42fc..402f1571 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs @@ -28,11 +28,11 @@ impl CompositeShapeRef<'_, S> { |node: &BvhNode, _| { let aabb1 = node.aabb(); let center1 = aabb1.center(); - let radius1 = aabb1.half_extents().norm(); + let radius1 = aabb1.half_extents().length(); let ball1 = Ball::new(radius1); let ball2 = Ball::new(sphere2.radius()); - let ball_motion1 = motion1.prepend_translation(center1.coords); - let ball_motion2 = motion2.prepend_translation(sphere2.center.coords); + let ball_motion1 = motion1.prepend_translation(center1); + let ball_motion2 = motion2.prepend_translation(sphere2.center); query::details::cast_shapes_nonlinear_support_map_support_map( dispatcher, diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs index 7d893841..9e24f549 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs @@ -1,14 +1,14 @@ -use na::RealField; +use crate::math::ComplexField; -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::{Ray, RayCast}; use crate::shape::HalfSpace; use crate::shape::SupportMap; /// Time Of Impact of a halfspace with a support-mapped shape under a rigid motion (translation + rotation). pub fn cast_shapes_nonlinear_halfspace_support_map( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, halfspace: &HalfSpace, other: &G, ) -> Option { @@ -33,8 +33,8 @@ pub fn cast_shapes_nonlinear_halfspace_support_map( /// Time Of Impact of a halfspace with a support-mapped shape under a rigid motion (translation + rotation). pub fn cast_shapes_nonlinear_support_map_halfspace( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, other: &G, halfspace: &HalfSpace, ) -> Option { diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs index 07afafc8..83b1e19e 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs @@ -1,6 +1,4 @@ -use na::{RealField, Unit}; - -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, RealField, Vector}; use crate::query::{ self, ClosestPoints, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, ShapeCastStatus, }; @@ -112,10 +110,10 @@ where let mut result = ShapeCastHit { time_of_impact: start_time, - normal1: Vector::::x_axis(), - normal2: Vector::::x_axis(), - witness1: Point::::origin(), - witness2: Point::::origin(), + normal1: Vector::X, + normal2: Vector::X, + witness1: Vector::ZERO, + witness2: Vector::ZERO, status: ShapeCastStatus::PenetratingOrWithinTargetDist, }; @@ -143,12 +141,11 @@ where result.witness1 = p1; result.witness2 = p2; - if let Some((normal1, dist)) = - Unit::try_new_and_get(pos12 * p2 - p1, crate::math::DEFAULT_EPSILON) - { + let (normal1, dist) = (pos12 * p2 - p1).normalize_and_length(); + if dist >= crate::math::DEFAULT_EPSILON { // TODO: do the "inverse transform unit vector" only when we are about to return. result.normal1 = normal1; - result.normal2 = pos12.inverse_transform_unit_vector(&-normal1); + result.normal2 = pos12.rotation.inverse() * -normal1; let curr_range = BisectionRange { min_t: result.time_of_impact, @@ -157,7 +154,7 @@ where }; let (new_range, niter) = - bisect(dist, motion1, sm1, motion2, sm2, &normal1, curr_range); + bisect(dist, motion1, sm1, motion2, sm2, normal1, curr_range); // println!( // "Bisection result: {:?}, normal1: {:?}, normal2: {:?}", // new_range, result.normal1, result.normal2 @@ -174,10 +171,10 @@ where let pos2 = motion2.position_at_time(new_range.max_t); let pos12 = pos1.inv_mul(&pos2); - let pt1 = sm1.local_support_point_toward(&normal1); - let pt2 = sm2.support_point_toward(&pos12, &-normal1); + let pt1 = sm1.local_support_point_toward(normal1); + let pt2 = sm2.support_point_toward(&pos12, -normal1); - if (pt2 - pt1).dot(&normal1) > 0.0 { + if (pt2 - pt1).dot(normal1) > 0.0 { // We found an axis that separate both objects at the end configuration. return None; } @@ -284,13 +281,13 @@ where #[cfg(feature = "dim2")] let dangvel = (motion2.angvel - motion1.angvel).abs(); #[cfg(feature = "dim3")] - let dangvel = (motion2.angvel - motion1.angvel).norm(); + let dangvel = (motion2.angvel - motion1.angvel).length(); let inv_dangvel = crate::utils::inv(dangvel); let linear_increment = sum_linear_thickness; let angular_increment = Real::pi() - max_angular_thickness; let linear_time_increment = - linear_increment * crate::utils::inv((motion2.linvel - motion1.linvel).norm()); + linear_increment * crate::utils::inv((motion2.linvel - motion1.linvel).length()); let angular_time_increment = angular_increment * inv_dangvel; let mut time_increment = angular_time_increment .min(linear_time_increment) @@ -328,10 +325,10 @@ where // of contact points potentially causing tunneling hit for the first time. let r1 = contact.point1 - motion1.local_center; let r2 = contact.point2 - motion2.local_center; - let vel1 = motion1.linvel + motion1.angvel.gcross(pos1_at_next_time * r1); - let vel2 = motion2.linvel + motion2.angvel.gcross(pos2_at_next_time * r2); + let vel1 = motion1.linvel + motion1.angvel.gcross(pos1_at_next_time.rotation * r1); + let vel2 = motion2.linvel + motion2.angvel.gcross(pos2_at_next_time.rotation * r2); let vel12 = vel2 - vel1; - let normal_vel = -vel12.dot(&(pos1_at_next_time * contact.normal1)); + let normal_vel = -vel12.dot(pos1_at_next_time.rotation * contact.normal1); let ccd_threshold = if contact.dist <= 0.0 { sum_linear_thickness } else { @@ -382,7 +379,7 @@ where sm1, motion2, sm2, - &contact.normal1, + contact.normal1, curr_range, ); @@ -406,7 +403,7 @@ where &ConstantPoint(contact.point1), motion2, &ConstantPoint(contact.point2), - &contact.normal1, + contact.normal1, curr_range, ); @@ -450,7 +447,7 @@ fn bisect( sm1: &SM1, motion2: &NonlinearRigidMotion, sm2: &SM2, - normal1: &Unit>, + normal1: Vector, mut range: BisectionRange, ) -> (BisectionRange, usize) where @@ -465,7 +462,7 @@ where // This is necessary to reduce the risk of extracting a root that // is not the root happening at the smallest time. let pos1 = motion1.position_at_time(range.curr_t); - let world_normal1 = pos1 * normal1; + let world_normal1 = pos1.rotation * normal1; loop { // println!("Bisection dist: {}, range: {:?}", dist, range); @@ -494,10 +491,10 @@ where let pos2 = motion2.position_at_time(range.curr_t); let pos12 = pos1.inv_mul(&pos2); - let normal1 = pos1.inverse_transform_unit_vector(&world_normal1); - let pt1 = sm1.local_support_point_toward(&normal1); - let pt2 = sm2.support_point_toward(&pos12, &-normal1); - dist = pt2.coords.dot(&normal1) - pt1.coords.dot(&normal1); + let normal1 = pos1.rotation.inverse() * world_normal1; + let pt1 = sm1.local_support_point_toward(normal1); + let pt2 = sm2.support_point_toward(&pos12, -normal1); + dist = pt2.dot(normal1) - pt1.dot(normal1); niter += 1; } diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_voxels_shape.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_voxels_shape.rs index 5faa0499..588192ac 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_voxels_shape.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_voxels_shape.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::BoundingVolume; -use crate::math::{Point, Real, Vector}; +use crate::math::{IVector, Real, Vector}; use crate::query::{NonlinearRigidMotion, QueryDispatcher, ShapeCastHit}; use crate::shape::{Cuboid, Shape, Voxels}; @@ -56,13 +56,13 @@ where .compute_aabb(&start_pos2_1) .merged(&g2.compute_aabb(&end_pos2_1)); - let mut check_voxels_in_range = |search_domain: [Point; 2]| { + let mut check_voxels_in_range = |search_domain: [IVector; 2]| { for vox in g1.voxels_in_range(search_domain[0], search_domain[1]) { if !vox.state.is_empty() { // PERF: could we check the canonical shape instead, and deduplicate accordingly? let center = g1.voxel_center(vox.grid_coords); let cuboid = Cuboid::new(g1.voxel_size() / 2.0); - let vox_motion1 = motion1.prepend_translation(center.coords); + let vox_motion1 = motion1.prepend_translation(center); if let Some(new_hit) = dispatcher .cast_shapes_nonlinear( &vox_motion1, @@ -130,7 +130,7 @@ where break; } - let imin = Vector::from(toi.map(|t| t.0)).imin(); + let imin = Vector::from(toi.map(|t| t.0)).min_position(); if toi[imin].1 { search_domain[0][imin] += 1; diff --git a/src/query/point/mod.rs b/src/query/point/mod.rs index ebacca00..3ecf3250 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -1,4 +1,4 @@ -//! Point inclusion and projection. +//! Vector inclusion and projection. #[doc(inline)] pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation}; diff --git a/src/query/point/point_aabb.rs b/src/query/point/point_aabb.rs index 52a3ede3..94cb7b34 100644 --- a/src/query/point/point_aabb.rs +++ b/src/query/point/point_aabb.rs @@ -1,26 +1,21 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{Real, Vector, DIM}; use crate::num::{Bounded, Zero}; use crate::query::{PointProjection, PointQuery}; use crate::shape::FeatureId; -use na; impl Aabb { - fn do_project_local_point( - &self, - pt: &Point, - solid: bool, - ) -> (bool, Point, Vector) { + fn do_project_local_point(&self, pt: Vector, solid: bool) -> (bool, Vector, Vector) { let mins_pt = self.mins - pt; let pt_maxs = pt - self.maxs; - let shift = mins_pt.sup(&na::zero()) - pt_maxs.sup(&na::zero()); + let shift = mins_pt.max(Vector::ZERO) - pt_maxs.max(Vector::ZERO); - let inside = shift.is_zero(); + let inside = shift == Vector::ZERO; if !inside { (false, pt + shift, shift) } else if solid { - (true, *pt, shift) + (true, pt, shift) } else { let _max: Real = Bounded::max_value(); let mut best = -_max; @@ -44,7 +39,7 @@ impl Aabb { } } - let mut shift: Vector = na::zero(); + let mut shift: Vector = Vector::ZERO; if is_mins { shift[best_id] = best; @@ -59,7 +54,7 @@ impl Aabb { impl PointQuery for Aabb { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { let (inside, ls_pt, _) = self.do_project_local_point(pt, solid); PointProjection::new(inside, ls_pt) } @@ -67,10 +62,7 @@ impl PointQuery for Aabb { #[allow(unused_assignments)] // For last_zero_shift which is used only in 3D. #[allow(unused_variables)] // For last_zero_shift which is used only in 3D. #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { let (inside, ls_pt, shift) = self.do_project_local_point(pt, false); let proj = PointProjection::new(inside, ls_pt); let mut nzero_shifts = 0; @@ -132,16 +124,16 @@ impl PointQuery for Aabb { } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real { let mins_pt = self.mins - pt; let pt_maxs = pt - self.maxs; - let shift = mins_pt.sup(&pt_maxs).sup(&na::zero()); + let shift = mins_pt.max(pt_maxs).max(Vector::ZERO); - if solid || !shift.is_zero() { - shift.norm() + if solid || shift != Vector::ZERO { + shift.length() } else { // TODO: optimize that. - -na::distance(pt, &self.project_local_point(pt, solid).point) + -pt.distance(self.project_local_point(pt, solid).point) } } } diff --git a/src/query/point/point_ball.rs b/src/query/point/point_ball.rs index b3ba0b82..5da6d336 100644 --- a/src/query/point/point_ball.rs +++ b/src/query/point/point_ball.rs @@ -1,36 +1,32 @@ -use na::{self, ComplexField}; +use crate::math::ComplexField; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Ball, FeatureId}; impl PointQuery for Ball { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - let distance_squared = pt.coords.norm_squared(); + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { + let distance_squared = pt.length_squared(); let inside = distance_squared <= self.radius * self.radius; if inside && solid { - PointProjection::new(true, *pt) + PointProjection::new(true, pt) } else { - let proj = - Point::from(pt.coords * (self.radius / ComplexField::sqrt(distance_squared))); + let proj = pt * (self.radius / ::sqrt(distance_squared)); PointProjection::new(inside, proj) } } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { (self.project_local_point(pt, false), FeatureId::Face(0)) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { - let dist = pt.coords.norm() - self.radius; + fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real { + let dist = pt.length() - self.radius; if solid && dist < 0.0 { 0.0 @@ -40,7 +36,7 @@ impl PointQuery for Ball { } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { - pt.coords.norm_squared() <= self.radius * self.radius + fn contains_local_point(&self, pt: Vector) -> bool { + pt.length_squared() <= self.radius * self.radius } } diff --git a/src/query/point/point_bounding_sphere.rs b/src/query/point/point_bounding_sphere.rs index 7dda5a70..dce2ae97 100644 --- a/src/query/point/point_bounding_sphere.rs +++ b/src/query/point/point_bounding_sphere.rs @@ -1,35 +1,32 @@ use crate::bounding_volume::BoundingSphere; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Ball, FeatureId}; impl PointQuery for BoundingSphere { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - let centered_pt = pt - self.center().coords; - let mut proj = Ball::new(self.radius()).project_local_point(¢ered_pt, solid); + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { + let centered_pt = pt - self.center(); + let mut proj = Ball::new(self.radius()).project_local_point(centered_pt, solid); - proj.point += self.center().coords; + proj.point += self.center(); proj } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { (self.project_local_point(pt, false), FeatureId::Face(0)) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { - let centered_pt = pt - self.center().coords; - Ball::new(self.radius()).distance_to_local_point(¢ered_pt, solid) + fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real { + let centered_pt = pt - self.center(); + Ball::new(self.radius()).distance_to_local_point(centered_pt, solid) } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { - let centered_pt = pt - self.center().coords; - Ball::new(self.radius()).contains_local_point(¢ered_pt) + fn contains_local_point(&self, pt: Vector) -> bool { + let centered_pt = pt - self.center(); + Ball::new(self.radius()).contains_local_point(centered_pt) } } diff --git a/src/query/point/point_capsule.rs b/src/query/point/point_capsule.rs index 94cccefc..48c98684 100644 --- a/src/query/point/point_capsule.rs +++ b/src/query/point/point_capsule.rs @@ -1,30 +1,29 @@ use crate::approx::AbsDiffEq; -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector, VectorExt}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Capsule, FeatureId, Segment}; -use na::{self, Unit}; impl PointQuery for Capsule { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { let seg = Segment::new(self.segment.a, self.segment.b); let proj = seg.project_local_point(pt, solid); - let dproj = *pt - proj.point; - - if let Some((dir, dist)) = Unit::try_new_and_get(dproj, Real::default_epsilon()) { + let dproj = pt - proj.point; + let (dir, dist) = dproj.normalize_and_length(); + if dist >= Real::default_epsilon() { let inside = dist <= self.radius; if solid && inside { - return PointProjection::new(true, *pt); + return PointProjection::new(true, pt); } else { - return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); + return PointProjection::new(inside, proj.point + dir * self.radius); } } else if solid { - return PointProjection::new(true, *pt); + return PointProjection::new(true, pt); } #[cfg(feature = "dim2")] if let Some(dir) = seg.normal() { - PointProjection::new(true, proj.point + *dir * self.radius) + PointProjection::new(true, proj.point + dir * self.radius) } else { // The segment has no normal, likely because it degenerates to a point. PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) @@ -42,10 +41,7 @@ impl PointQuery for Capsule { } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { (self.project_local_point(pt, false), FeatureId::Face(0)) } } diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index cbcb1457..77a9bdc5 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -1,13 +1,12 @@ #![allow(unused_parens)] // Needed by the macro. -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::partitioning::BvhNode; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{ CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedCompositeShape, }; -use na; use crate::shape::{Compound, Polyline}; @@ -19,7 +18,7 @@ impl CompositeShapeRef<'_, S> { #[inline] pub fn project_local_point_and_get_location( &self, - point: &Point, + point: Vector, max_dist: Real, solid: bool, ) -> Option<( @@ -45,7 +44,7 @@ impl CompositeShapeRef<'_, S> { shape.project_local_point_and_get_location(point, solid) } })?; - let cost = na::distance(&proj.0.point, point); + let cost = (proj.0.point - point).length(); Some((cost, proj)) }, ) @@ -57,7 +56,7 @@ impl CompositeShapeRef<'_, S> { /// Returns the projected point as well as the index of the sub-shape of `self` that was hit. /// If `solid` is `false` then the point will be projected to the closest boundary of `self` even /// if it is contained by one of its sub-shapes. - pub fn project_local_point(&self, point: &Point, solid: bool) -> (u32, PointProjection) { + pub fn project_local_point(&self, point: Vector, solid: bool) -> (u32, PointProjection) { let (best_id, (_, proj)) = self .0 .bvh() @@ -72,7 +71,7 @@ impl CompositeShapeRef<'_, S> { shape.project_local_point(point, solid) } })?; - let dist = na::distance(&proj.point, point); + let dist = (proj.point - point).length(); Some((dist, proj)) }, ) @@ -88,7 +87,7 @@ impl CompositeShapeRef<'_, S> { #[inline] pub fn project_local_point_and_get_feature( &self, - point: &Point, + point: Vector, ) -> (u32, (PointProjection, FeatureId)) { let (best_id, (_, (proj, feature_id))) = self .0 @@ -104,7 +103,7 @@ impl CompositeShapeRef<'_, S> { shape.project_local_point_and_get_feature(point) } })?; - let cost = na::distance(&proj.0.point, point); + let cost = (proj.0.point - point).length(); Some((cost, proj)) }, ) @@ -116,7 +115,7 @@ impl CompositeShapeRef<'_, S> { /// Returns the index of any sub-shape of `self` that contains the given point. #[inline] - pub fn contains_local_point(&self, point: &Point) -> Option { + pub fn contains_local_point(&self, point: Vector) -> Option { self.0 .bvh() .leaves(|node: &BvhNode| node.aabb().contains_local_point(point)) @@ -136,15 +135,12 @@ impl CompositeShapeRef<'_, S> { impl PointQuery for Polyline { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { CompositeShapeRef(self).project_local_point(point, solid).1 } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { let (seg_id, (proj, feature)) = CompositeShapeRef(self).project_local_point_and_get_feature(point); let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature); @@ -154,7 +150,7 @@ impl PointQuery for Polyline { // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: Vector) -> bool { CompositeShapeRef(self) .contains_local_point(point) .is_some() @@ -163,15 +159,12 @@ impl PointQuery for Polyline { impl PointQuery for TriMesh { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { CompositeShapeRef(self).project_local_point(point, solid).1 } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { #[cfg(feature = "dim3")] if self.pseudo_normals().is_some() { // If we can, in 3D, take the pseudo-normals into account. @@ -188,7 +181,7 @@ impl PointQuery for TriMesh { // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: Vector) -> bool { #[cfg(feature = "dim3")] if self.pseudo_normals.is_some() { // If we can, in 3D, take the pseudo-normals into account. @@ -206,7 +199,7 @@ impl PointQuery for TriMesh { /// Projects a point on `self` transformed by `m`, unless the projection lies further than the given max distance. fn project_local_point_with_max_dist( &self, - pt: &Point, + pt: Vector, solid: bool, max_dist: Real, ) -> Option { @@ -217,15 +210,12 @@ impl PointQuery for TriMesh { impl PointQuery for Compound { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { CompositeShapeRef(self).project_local_point(point, solid).1 } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { ( CompositeShapeRef(self) .project_local_point_and_get_feature(point) @@ -236,7 +226,7 @@ impl PointQuery for Compound { } #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: Vector) -> bool { CompositeShapeRef(self) .contains_local_point(point) .is_some() @@ -249,7 +239,7 @@ impl PointQueryWithLocation for Polyline { #[inline] fn project_local_point_and_get_location( &self, - point: &Point, + point: Vector, solid: bool, ) -> (PointProjection, Self::Location) { let (seg_id, (proj, loc)) = CompositeShapeRef(self) @@ -266,7 +256,7 @@ impl PointQueryWithLocation for TriMesh { #[allow(unused_mut)] // Because we need mut in 3D but not in 2D. fn project_local_point_and_get_location( &self, - point: &Point, + point: Vector, solid: bool, ) -> (PointProjection, Self::Location) { self.project_local_point_and_get_location_with_max_dist(point, solid, Real::MAX) @@ -276,7 +266,7 @@ impl PointQueryWithLocation for TriMesh { /// Projects a point on `self`, with a maximum projection distance. fn project_local_point_and_get_location_with_max_dist( &self, - point: &Point, + point: Vector, solid: bool, max_dist: Real, ) -> Option<(PointProjection, Self::Location)> { @@ -305,7 +295,7 @@ impl PointQueryWithLocation for TriMesh { if let Some(pseudo_normal) = pseudo_normal { let dpt = point - proj.point; - proj.is_inside = dpt.dot(&pseudo_normal) <= 0.0; + proj.is_inside = dpt.dot(pseudo_normal) <= 0.0; } } diff --git a/src/query/point/point_cone.rs b/src/query/point/point_cone.rs index 8570daa0..2cd054e8 100644 --- a/src/query/point/point_cone.rs +++ b/src/query/point/point_cone.rs @@ -1,20 +1,19 @@ -use crate::math::{Point, Real, Vector}; +use crate::math::{Vector, Vector2}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cone, FeatureId, Segment}; -use na; impl PointQuery for Cone { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { // Project on the basis. - let mut dir_from_basis_center = pt.coords.xz(); - let planar_dist_from_basis_center = dir_from_basis_center.normalize_mut(); + let (mut dir_from_basis_center, planar_dist_from_basis_center) = + Vector2::new(pt.x, pt.z).normalize_and_length(); if planar_dist_from_basis_center <= crate::math::DEFAULT_EPSILON { - dir_from_basis_center = na::Vector2::x(); + dir_from_basis_center = Vector2::X; } - let projection_on_basis = Point::new(pt.coords.x, -self.half_height, pt.coords.z); + let projection_on_basis = Vector::new(pt.x, -self.half_height, pt.z); if pt.y < -self.half_height && planar_dist_from_basis_center <= self.radius { // The projection is on the basis. @@ -23,11 +22,11 @@ impl PointQuery for Cone { // Project on the basis circle. let proj2d = dir_from_basis_center * self.radius; - let projection_on_basis_circle = Point::new(proj2d[0], -self.half_height, proj2d[1]); + let projection_on_basis_circle = Vector::new(proj2d[0], -self.half_height, proj2d[1]); // Project on the conic side. // TODO: we could solve this in 2D using the plane passing through the cone axis and the conic_side_segment to save some computation. - let apex_point = Point::new(0.0, self.half_height, 0.0); + let apex_point = Vector::new(0.0, self.half_height, 0.0); let conic_side_segment = Segment::new(apex_point, projection_on_basis_circle); let conic_side_segment_dir = conic_side_segment.scaled_direction(); let mut proj = conic_side_segment.project_local_point(pt, true); @@ -38,16 +37,17 @@ impl PointQuery for Cone { if pt.y >= -self.half_height && pt.y <= self.half_height && conic_side_segment_dir - .cross(&(pt - apex_point)) - .dot(&conic_side_segment_dir.cross(&apex_to_basis_center)) + .cross(pt - apex_point) + .dot(conic_side_segment_dir.cross(apex_to_basis_center)) >= 0.0 { if solid { - PointProjection::new(true, *pt) + PointProjection::new(true, pt) } else { // We are inside of the cone, so the correct projection is // either on the basis of the cone, or on the conic side. - if (proj.point - pt).norm_squared() > (projection_on_basis - pt).norm_squared() { + if (proj.point - pt).length_squared() > (projection_on_basis - pt).length_squared() + { PointProjection::new(true, projection_on_basis) } else { proj.is_inside = true; @@ -62,10 +62,7 @@ impl PointQuery for Cone { } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { // TODO: get the actual feature. (self.project_local_point(pt, false), FeatureId::Unknown) } diff --git a/src/query/point/point_cuboid.rs b/src/query/point/point_cuboid.rs index 3625dc90..345e8512 100644 --- a/src/query/point/point_cuboid.rs +++ b/src/query/point/point_cuboid.rs @@ -1,37 +1,34 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cuboid, FeatureId}; impl PointQuery for Cuboid { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - let dl = Point::from(-self.half_extents); - let ur = Point::from(self.half_extents); + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { + let dl = -self.half_extents; + let ur = self.half_extents; Aabb::new(dl, ur).project_local_point(pt, solid) } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { - let dl = Point::from(-self.half_extents); - let ur = Point::from(self.half_extents); + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { + let dl = -self.half_extents; + let ur = self.half_extents; Aabb::new(dl, ur).project_local_point_and_get_feature(pt) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { - let dl = Point::from(-self.half_extents); - let ur = Point::from(self.half_extents); + fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real { + let dl = -self.half_extents; + let ur = self.half_extents; Aabb::new(dl, ur).distance_to_local_point(pt, solid) } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { - let dl = Point::from(-self.half_extents); - let ur = Point::from(self.half_extents); + fn contains_local_point(&self, pt: Vector) -> bool { + let dl = -self.half_extents; + let ur = self.half_extents; Aabb::new(dl, ur).contains_local_point(pt) } } diff --git a/src/query/point/point_cylinder.rs b/src/query/point/point_cylinder.rs index 7c2c9a9d..0b08fbd0 100644 --- a/src/query/point/point_cylinder.rs +++ b/src/query/point/point_cylinder.rs @@ -1,17 +1,18 @@ -use crate::math::{Point, Real}; +use crate::math::{Vector, Vector2}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cylinder, FeatureId}; -use na; impl PointQuery for Cylinder { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { // Project on the basis. - let mut dir_from_basis_center = pt.coords.xz(); - let planar_dist_from_basis_center = dir_from_basis_center.normalize_mut(); + let (mut dir_from_basis_center, planar_dist_from_basis_center) = + Vector2::new(pt.x, pt.z).normalize_and_length(); if planar_dist_from_basis_center <= crate::math::DEFAULT_EPSILON { - dir_from_basis_center = na::Vector2::x(); + { + dir_from_basis_center = Vector2::X; + } } let proj2d = dir_from_basis_center * self.radius; @@ -22,21 +23,20 @@ impl PointQuery for Cylinder { { // The point is inside of the cylinder. if solid { - PointProjection::new(true, *pt) + PointProjection::new(true, pt) } else { - let dist_to_top = self.half_height - pt.coords.y; - let dist_to_bottom = pt.coords.y - (-self.half_height); + let dist_to_top = self.half_height - pt.y; + let dist_to_bottom = pt.y - (-self.half_height); let dist_to_side = self.radius - planar_dist_from_basis_center; if dist_to_top < dist_to_bottom && dist_to_top < dist_to_side { - let projection_on_top = Point::new(pt.coords.x, self.half_height, pt.coords.z); + let projection_on_top = Vector::new(pt.x, self.half_height, pt.z); PointProjection::new(true, projection_on_top) } else if dist_to_bottom < dist_to_top && dist_to_bottom < dist_to_side { - let projection_on_bottom = - Point::new(pt.coords.x, -self.half_height, pt.coords.z); + let projection_on_bottom = Vector::new(pt.x, -self.half_height, pt.z); PointProjection::new(true, projection_on_bottom) } else { - let projection_on_side = Point::new(proj2d[0], pt.y, proj2d[1]); + let projection_on_side = Vector::new(proj2d[0], pt.y, proj2d[1]); PointProjection::new(true, projection_on_side) } } @@ -44,37 +44,33 @@ impl PointQuery for Cylinder { // The point is outside of the cylinder. if pt.y > self.half_height { if planar_dist_from_basis_center <= self.radius { - let projection_on_top = Point::new(pt.coords.x, self.half_height, pt.coords.z); + let projection_on_top = Vector::new(pt.x, self.half_height, pt.z); PointProjection::new(false, projection_on_top) } else { let projection_on_top_circle = - Point::new(proj2d[0], self.half_height, proj2d[1]); + Vector::new(proj2d[0], self.half_height, proj2d[1]); PointProjection::new(false, projection_on_top_circle) } } else if pt.y < -self.half_height { // Project on the bottom plane or the bottom circle. if planar_dist_from_basis_center <= self.radius { - let projection_on_bottom = - Point::new(pt.coords.x, -self.half_height, pt.coords.z); + let projection_on_bottom = Vector::new(pt.x, -self.half_height, pt.z); PointProjection::new(false, projection_on_bottom) } else { let projection_on_bottom_circle = - Point::new(proj2d[0], -self.half_height, proj2d[1]); + Vector::new(proj2d[0], -self.half_height, proj2d[1]); PointProjection::new(false, projection_on_bottom_circle) } } else { // Project on the side. - let projection_on_side = Point::new(proj2d[0], pt.y, proj2d[1]); + let projection_on_side = Vector::new(proj2d[0], pt.y, proj2d[1]); PointProjection::new(false, projection_on_side) } } } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { // TODO: get the actual feature. (self.project_local_point(pt, false), FeatureId::Unknown) } diff --git a/src/query/point/point_halfspace.rs b/src/query/point/point_halfspace.rs index a34b68b2..42a61128 100644 --- a/src/query/point/point_halfspace.rs +++ b/src/query/point/point_halfspace.rs @@ -1,31 +1,28 @@ -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{FeatureId, HalfSpace}; impl PointQuery for HalfSpace { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - let d = self.normal.dot(&pt.coords); + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { + let d = self.normal.dot(pt); let inside = d <= 0.0; if inside && solid { - PointProjection::new(true, *pt) + PointProjection::new(true, pt) } else { - PointProjection::new(inside, *pt + (-*self.normal * d)) + PointProjection::new(inside, pt + (-self.normal * d)) } } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { (self.project_local_point(pt, false), FeatureId::Face(0)) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { - let dist = self.normal.dot(&pt.coords); + fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real { + let dist = self.normal.dot(pt); if dist < 0.0 && solid { 0.0 @@ -36,7 +33,7 @@ impl PointQuery for HalfSpace { } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { - self.normal.dot(&pt.coords) <= 0.0 + fn contains_local_point(&self, pt: Vector) -> bool { + self.normal.dot(pt) <= 0.0 } } diff --git a/src/query/point/point_heightfield.rs b/src/query/point/point_heightfield.rs index ec0abccd..42cbcfeb 100644 --- a/src/query/point/point_heightfield.rs +++ b/src/query/point/point_heightfield.rs @@ -1,24 +1,24 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector}; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; -use crate::shape::{FeatureId, HeightField, TrianglePointLocation}; #[cfg(not(feature = "std"))] -use na::ComplexField; // For sqrt. +use crate::math::ComplexField; +use crate::math::{Real, Vector}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::shape::{FeatureId, HeightField, TrianglePointLocation}; // For sqrt. impl PointQuery for HeightField { fn project_local_point_with_max_dist( &self, - pt: &Point, + pt: Vector, solid: bool, max_dist: Real, ) -> Option { - let aabb = Aabb::new(pt - Vector::repeat(max_dist), pt + Vector::repeat(max_dist)); + let aabb = Aabb::new(pt - Vector::splat(max_dist), pt + Vector::splat(max_dist)); let mut sq_smallest_dist = Real::MAX; let mut best_proj = None; self.map_elements_in_local_aabb(&aabb, &mut |_, triangle| { let proj = triangle.project_local_point(pt, solid); - let sq_dist = na::distance_squared(pt, &proj.point); + let sq_dist = (pt - proj.point).length_squared(); if sq_dist < sq_smallest_dist { sq_smallest_dist = sq_dist; @@ -33,9 +33,9 @@ impl PointQuery for HeightField { } #[inline] - fn project_local_point(&self, point: &Point, _: bool) -> PointProjection { + fn project_local_point(&self, point: Vector, _: bool) -> PointProjection { let mut smallest_dist = Real::MAX; - let mut best_proj = PointProjection::new(false, *point); + let mut best_proj = PointProjection::new(false, point); #[cfg(feature = "dim2")] let iter = self.segments(); @@ -43,7 +43,7 @@ impl PointQuery for HeightField { let iter = self.triangles(); for elt in iter { let proj = elt.project_local_point(point, false); - let dist = na::distance_squared(point, &proj.point); + let dist = (point - proj.point).length_squared(); if dist < smallest_dist { smallest_dist = dist; @@ -55,10 +55,7 @@ impl PointQuery for HeightField { } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { // TODO: compute the feature properly. (self.project_local_point(point, false), FeatureId::Unknown) } @@ -66,7 +63,7 @@ impl PointQuery for HeightField { // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, _point: &Point) -> bool { + fn contains_local_point(&self, _point: Vector) -> bool { false } } @@ -77,7 +74,7 @@ impl PointQueryWithLocation for HeightField { #[inline] fn project_local_point_and_get_location( &self, - _point: &Point, + _point: Vector, _: bool, ) -> (PointProjection, Self::Location) { unimplemented!() diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index 1c070387..09368384 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -1,13 +1,9 @@ -use crate::math::{Isometry, Point, Real}; +use crate::math::{Pose, Real, Vector}; use crate::shape::FeatureId; -use na; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// The result of projecting a point onto a shape. /// -/// Point projection finds the closest point on a shape's surface to a given query point. +/// Vector projection finds the closest point on a shape's surface to a given query point. /// This is fundamental for many geometric queries including distance calculation, /// collision detection, and surface sampling. /// @@ -36,22 +32,22 @@ use rkyv::{bytecheck, CheckBytes}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::PointQuery; /// use parry3d::shape::Ball; -/// use nalgebra::{Point3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// /// let ball = Ball::new(5.0); -/// let ball_pos = Isometry3::translation(10.0, 0.0, 0.0); +/// let ball_pos = Pose::translation(10.0, 0.0, 0.0); /// /// // Project a point outside the ball -/// let outside_point = Point3::origin(); -/// let proj = ball.project_point(&ball_pos, &outside_point, true); +/// let outside_point = Vector::ZERO; +/// let proj = ball.project_point(&ball_pos, outside_point, true); /// /// // Closest point on ball surface -/// assert_eq!(proj.point, Point3::new(5.0, 0.0, 0.0)); +/// assert_eq!(proj.point, Vector::new(5.0, 0.0, 0.0)); /// assert!(!proj.is_inside); /// /// // Project a point inside the ball -/// let inside_point = Point3::new(10.0, 0.0, 0.0); // At center -/// let proj2 = ball.project_point(&ball_pos, &inside_point, true); +/// let inside_point = Vector::new(10.0, 0.0, 0.0); // At center +/// let proj2 = ball.project_point(&ball_pos, inside_point, true); /// assert!(proj2.is_inside); /// # } /// ``` @@ -59,31 +55,30 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct PointProjection { /// Whether the query point was inside the shape. /// - /// - `true`: Point is in the interior (for solid shapes) - /// - `false`: Point is outside the shape + /// - `true`: Vector is in the interior (for solid shapes) + /// - `false`: Vector is outside the shape pub is_inside: bool, /// The closest point on the shape's surface to the query point. /// /// If `is_inside = true`, this is the nearest point on the boundary. /// If `is_inside = false`, this is the nearest surface point. - pub point: Point, + pub point: Vector, } impl PointProjection { /// Initializes a new `PointProjection`. - pub fn new(is_inside: bool, point: Point) -> Self { + pub fn new(is_inside: bool, point: Vector) -> Self { PointProjection { is_inside, point } } /// Transforms `self.point` by `pos`. - pub fn transform_by(&self, pos: &Isometry) -> Self { + pub fn transform_by(&self, pos: &Pose) -> Self { PointProjection { is_inside: self.is_inside, point: pos * self.point, @@ -91,8 +86,8 @@ impl PointProjection { } /// Returns `true` if `Self::is_inside` is `true` or if the distance between the projected point and `point` is smaller than `min_dist`. - pub fn is_inside_eps(&self, original_point: &Point, min_dist: Real) -> bool { - self.is_inside || na::distance_squared(original_point, &self.point) < min_dist * min_dist + pub fn is_inside_eps(&self, original_point: Vector, min_dist: Real) -> bool { + self.is_inside || (original_point - self.point).length_squared() < min_dist * min_dist } } @@ -115,8 +110,8 @@ impl PointProjection { /// # Local vs World Space /// /// Methods with `local_` prefix work in the shape's local coordinate system: -/// - **Local methods**: Point must be in shape's coordinate frame -/// - **World methods**: Point in world space, shape transformation applied +/// - **Local methods**: Vector must be in shape's coordinate frame +/// - **World methods**: Vector in world space, shape transformation applied /// /// # Solid Parameter /// @@ -130,24 +125,24 @@ impl PointProjection { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::PointQuery; /// use parry3d::shape::Cuboid; -/// use nalgebra::{Point3, Vector3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// let cuboid_pos = Isometry3::translation(5.0, 0.0, 0.0); +/// let cuboid = Cuboid::new(Vector::splat(1.0)); +/// let cuboid_pos = Pose::translation(5.0, 0.0, 0.0); /// -/// let query_point = Point3::origin(); +/// let query_point = Vector::ZERO; /// /// // Project point onto cuboid surface -/// let projection = cuboid.project_point(&cuboid_pos, &query_point, true); +/// let projection = cuboid.project_point(&cuboid_pos, query_point, true); /// println!("Closest point on cuboid: {:?}", projection.point); /// println!("Is inside: {}", projection.is_inside); /// /// // Calculate distance to cuboid -/// let distance = cuboid.distance_to_point(&cuboid_pos, &query_point, true); +/// let distance = cuboid.distance_to_point(&cuboid_pos, query_point, true); /// println!("Distance: {}", distance); /// /// // Test if point is inside cuboid -/// let is_inside = cuboid.contains_point(&cuboid_pos, &query_point); +/// let is_inside = cuboid.contains_point(&cuboid_pos, query_point); /// println!("Contains: {}", is_inside); /// # } /// ``` @@ -171,30 +166,30 @@ pub trait PointQuery { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::PointQuery; /// use parry3d::shape::Ball; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let ball = Ball::new(1.0); - /// let far_point = Point3::new(100.0, 0.0, 0.0); + /// let far_point = Vector::new(100.0, 0.0, 0.0); /// /// // Projection exists but is far away - /// assert!(ball.project_local_point(&far_point, true).point.x > 0.0); + /// assert!(ball.project_local_point(far_point, true).point.x > 0.0); /// /// // With max distance limit of 10, projection is rejected - /// assert!(ball.project_local_point_with_max_dist(&far_point, true, 10.0).is_none()); + /// assert!(ball.project_local_point_with_max_dist(far_point, true, 10.0).is_none()); /// /// // Nearby point is accepted - /// let near_point = Point3::new(2.0, 0.0, 0.0); - /// assert!(ball.project_local_point_with_max_dist(&near_point, true, 10.0).is_some()); + /// let near_point = Vector::new(2.0, 0.0, 0.0); + /// assert!(ball.project_local_point_with_max_dist(near_point, true, 10.0).is_some()); /// # } /// ``` fn project_local_point_with_max_dist( &self, - pt: &Point, + pt: Vector, solid: bool, max_dist: Real, ) -> Option { let proj = self.project_local_point(pt, solid); - if na::distance(&proj.point, pt) > max_dist { + if (proj.point - pt).length() > max_dist { None } else { Some(proj) @@ -204,29 +199,28 @@ pub trait PointQuery { /// Projects a point on `self` transformed by `m`, unless the projection lies further than the given max distance. fn project_point_with_max_dist( &self, - m: &Isometry, - pt: &Point, + m: &Pose, + pt: Vector, solid: bool, max_dist: Real, ) -> Option { - self.project_local_point_with_max_dist(&m.inverse_transform_point(pt), solid, max_dist) + self.project_local_point_with_max_dist(m.inverse_transform_point(pt), solid, max_dist) .map(|proj| proj.transform_by(m)) } /// Projects a point on `self`. /// /// The point is assumed to be expressed in the local-space of `self`. - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection; + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection; /// Projects a point on the boundary of `self` and returns the id of the /// feature the point was projected on. - fn project_local_point_and_get_feature(&self, pt: &Point) - -> (PointProjection, FeatureId); + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId); /// Computes the minimal distance between a point and `self`. - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real { let proj = self.project_local_point(pt, solid); - let dist = na::distance(pt, &proj.point); + let dist = (pt - proj.point).length(); if solid || !proj.is_inside { dist @@ -236,37 +230,33 @@ pub trait PointQuery { } /// Tests if the given point is inside of `self`. - fn contains_local_point(&self, pt: &Point) -> bool { + fn contains_local_point(&self, pt: Vector) -> bool { self.project_local_point(pt, true).is_inside } /// Projects a point on `self` transformed by `m`. - fn project_point(&self, m: &Isometry, pt: &Point, solid: bool) -> PointProjection { - self.project_local_point(&m.inverse_transform_point(pt), solid) + fn project_point(&self, m: &Pose, pt: Vector, solid: bool) -> PointProjection { + self.project_local_point(m.inverse_transform_point(pt), solid) .transform_by(m) } /// Computes the minimal distance between a point and `self` transformed by `m`. #[inline] - fn distance_to_point(&self, m: &Isometry, pt: &Point, solid: bool) -> Real { - self.distance_to_local_point(&m.inverse_transform_point(pt), solid) + fn distance_to_point(&self, m: &Pose, pt: Vector, solid: bool) -> Real { + self.distance_to_local_point(m.inverse_transform_point(pt), solid) } /// Projects a point on the boundary of `self` transformed by `m` and returns the id of the /// feature the point was projected on. - fn project_point_and_get_feature( - &self, - m: &Isometry, - pt: &Point, - ) -> (PointProjection, FeatureId) { - let res = self.project_local_point_and_get_feature(&m.inverse_transform_point(pt)); + fn project_point_and_get_feature(&self, m: &Pose, pt: Vector) -> (PointProjection, FeatureId) { + let res = self.project_local_point_and_get_feature(m.inverse_transform_point(pt)); (res.0.transform_by(m), res.1) } /// Tests if the given point is inside of `self` transformed by `m`. #[inline] - fn contains_point(&self, m: &Isometry, pt: &Point) -> bool { - self.contains_local_point(&m.inverse_transform_point(pt)) + fn contains_point(&self, m: &Pose, pt: Vector) -> bool { + self.contains_local_point(m.inverse_transform_point(pt)) } } @@ -296,30 +286,30 @@ pub trait PointQueryWithLocation { /// Projects a point on `self`. fn project_local_point_and_get_location( &self, - pt: &Point, + pt: Vector, solid: bool, ) -> (PointProjection, Self::Location); /// Projects a point on `self` transformed by `m`. fn project_point_and_get_location( &self, - m: &Isometry, - pt: &Point, + m: &Pose, + pt: Vector, solid: bool, ) -> (PointProjection, Self::Location) { - let res = self.project_local_point_and_get_location(&m.inverse_transform_point(pt), solid); + let res = self.project_local_point_and_get_location(m.inverse_transform_point(pt), solid); (res.0.transform_by(m), res.1) } /// Projects a point on `self`, with a maximum projection distance. fn project_local_point_and_get_location_with_max_dist( &self, - pt: &Point, + pt: Vector, solid: bool, max_dist: Real, ) -> Option<(PointProjection, Self::Location)> { let (proj, location) = self.project_local_point_and_get_location(pt, solid); - if na::distance(&proj.point, pt) > max_dist { + if (proj.point - pt).length() > max_dist { None } else { Some((proj, location)) @@ -329,13 +319,13 @@ pub trait PointQueryWithLocation { /// Projects a point on `self` transformed by `m`, with a maximum projection distance. fn project_point_and_get_location_with_max_dist( &self, - m: &Isometry, - pt: &Point, + m: &Pose, + pt: Vector, solid: bool, max_dist: Real, ) -> Option<(PointProjection, Self::Location)> { self.project_local_point_and_get_location_with_max_dist( - &m.inverse_transform_point(pt), + m.inverse_transform_point(pt), solid, max_dist, ) diff --git a/src/query/point/point_round_shape.rs b/src/query/point/point_round_shape.rs index ed01ea2b..58000908 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -1,4 +1,5 @@ -use crate::math::{Point, Real}; +use crate::math::Vector; +#[cfg(feature = "alloc")] use crate::query::gjk::VoronoiSimplex; use crate::query::{PointProjection, PointQuery}; use crate::shape::{FeatureId, RoundShape, SupportMap}; @@ -7,7 +8,9 @@ use crate::shape::{FeatureId, RoundShape, SupportMap}; // call this and adjust the projected point accordingly. impl PointQuery for RoundShape { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + #[allow(unreachable_code)] + #[allow(clippy::diverging_sub_expression)] + fn project_local_point(&self, _point: Vector, _solid: bool) -> PointProjection { #[cfg(not(feature = "alloc"))] return unimplemented!( "The projection of points on a round shape isn't supported without alloc yet." @@ -17,16 +20,13 @@ impl PointQuery for RoundShape { return crate::query::details::local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), - point, - solid, + _point, + _solid, ); } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { (self.project_local_point(point, false), FeatureId::Unknown) } } diff --git a/src/query/point/point_segment.rs b/src/query/point/point_segment.rs index 03e00052..ec290945 100644 --- a/src/query/point/point_segment.rs +++ b/src/query/point/point_segment.rs @@ -1,18 +1,15 @@ -use crate::math::{Point, Real}; +use crate::math::Vector; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, Segment, SegmentPointLocation}; impl PointQuery for Segment { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { self.project_local_point_and_get_location(pt, solid).0 } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { let (proj, loc) = self.project_local_point_and_get_location(pt, false); let feature = match loc { SegmentPointLocation::OnVertex(i) => FeatureId::Vertex(i), @@ -20,8 +17,8 @@ impl PointQuery for Segment { #[cfg(feature = "dim2")] { let dir = self.scaled_direction(); - let dpt = *pt - proj.point; - if dpt.perp(&dir) >= 0.0 { + let dpt = pt - proj.point; + if dpt.perp_dot(dir) >= 0.0 { FeatureId::Face(0) } else { FeatureId::Face(1) @@ -48,13 +45,13 @@ impl PointQueryWithLocation for Segment { #[inline] fn project_local_point_and_get_location( &self, - pt: &Point, + pt: Vector, _: bool, ) -> (PointProjection, Self::Location) { let ab = self.b - self.a; let ap = pt - self.a; - let ab_ap = ab.dot(&ap); - let sqnab = ab.norm_squared(); + let ab_ap = ab.dot(ap); + let sqnab = ab.length_squared(); let proj; let location; @@ -78,7 +75,7 @@ impl PointQueryWithLocation for Segment { } // TODO: is this acceptable? - let inside = relative_eq!(proj, *pt); + let inside = relative_eq!(proj, pt); (PointProjection::new(inside, proj), location) } diff --git a/src/query/point/point_support_map.rs b/src/query/point/point_support_map.rs index dd87dad5..c400d4ba 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -1,9 +1,7 @@ -use na::Unit; - -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Vector}; #[cfg(feature = "alloc")] use crate::query::epa::EPA; -use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::gjk::{self, ConstantOrigin, CsoPoint, VoronoiSimplex}; use crate::query::{PointProjection, PointQuery}; #[cfg(feature = "dim2")] #[cfg(feature = "alloc")] @@ -17,36 +15,41 @@ use crate::shape::{FeatureId, SupportMap}; pub fn local_point_projection_on_support_map( shape: &G, simplex: &mut VoronoiSimplex, - point: &Point, + point: Vector, solid: bool, ) -> PointProjection where G: SupportMap, { - let m = Isometry::new(-point.coords, na::zero()); - let m_inv = Isometry::new(point.coords, na::zero()); - let dir = Unit::try_new(-m.translation.vector, crate::math::DEFAULT_EPSILON) - .unwrap_or(Vector::x_axis()); - let support_point = CSOPoint::from_shapes(&m_inv, shape, &ConstantOrigin, &dir); + #[cfg(feature = "dim2")] + let m = Pose::new(-point, 0.0); + #[cfg(feature = "dim3")] + let m = Pose::new(-point, Vector::ZERO); + #[cfg(feature = "dim2")] + let m_inv = Pose::new(point, 0.0); + #[cfg(feature = "dim3")] + let m_inv = Pose::new(point, Vector::ZERO); + let dir = (-m.translation).try_normalize().unwrap_or(Vector::X); + let support_point = CsoPoint::from_shapes(&m_inv, shape, &ConstantOrigin, dir); simplex.reset(support_point); if let Some(proj) = gjk::project_origin(&m, shape, simplex) { PointProjection::new(false, proj) } else if solid { - PointProjection::new(true, *point) + PointProjection::new(true, point) } else { let mut epa = EPA::new(); if let Some(pt) = epa.project_origin(&m, shape, simplex) { PointProjection::new(true, pt) } else { // return match minkowski_sampling::project_origin(&m, shape, simplex) { - // Some(p) => PointProjection::new(true, p + point.coords), + // Some(p) => PointProjection::new(true, p + point), // None => PointProjection::new(true, *point), // }; //// All failed. - PointProjection::new(true, *point) + PointProjection::new(true, point) } } } @@ -54,21 +57,18 @@ where #[cfg(feature = "dim3")] impl PointQuery for ConvexPolyhedron { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid) } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { let proj = self.project_local_point(point, false); - let dpt = *point - proj.point; + let dpt = point - proj.point; let local_dir = if proj.is_inside { -dpt } else { dpt }; - if let Some(local_dir) = Unit::try_new(local_dir, crate::math::DEFAULT_EPSILON) { - let feature = self.support_feature_id_toward(&local_dir); + if let Some(local_dir) = (local_dir).try_normalize() { + let feature = self.support_feature_id_toward(local_dir); (proj, feature) } else { (proj, FeatureId::Unknown) @@ -79,21 +79,18 @@ impl PointQuery for ConvexPolyhedron { #[cfg(feature = "dim2")] impl PointQuery for ConvexPolygon { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid) } #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { let proj = self.project_local_point(point, false); - let dpt = *point - proj.point; + let dpt = point - proj.point; let local_dir = if proj.is_inside { -dpt } else { dpt }; - if let Some(local_dir) = Unit::try_new(local_dir, crate::math::DEFAULT_EPSILON) { - let feature = self.support_feature_id_toward(&local_dir); + if let Some(local_dir) = (local_dir).try_normalize() { + let feature = self.support_feature_id_toward(local_dir); (proj, feature) } else { (proj, FeatureId::Unknown) diff --git a/src/query/point/point_tetrahedron.rs b/src/query/point/point_tetrahedron.rs index 2ace3e08..faa3df65 100644 --- a/src/query/point/point_tetrahedron.rs +++ b/src/query/point/point_tetrahedron.rs @@ -1,18 +1,15 @@ -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, Tetrahedron, TetrahedronPointLocation}; impl PointQuery for Tetrahedron { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { self.project_local_point_and_get_location(pt, solid).0 } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { let (proj, loc) = self.project_local_point_and_get_location(pt, false); let feature = match loc { TetrahedronPointLocation::OnVertex(i) => FeatureId::Vertex(i), @@ -31,7 +28,7 @@ impl PointQueryWithLocation for Tetrahedron { #[inline] fn project_local_point_and_get_location( &self, - pt: &Point, + pt: Vector, solid: bool, ) -> (PointProjection, Self::Location) { let ab = self.b - self.a; @@ -42,9 +39,9 @@ impl PointQueryWithLocation for Tetrahedron { /* * Voronoï regions of vertices. */ - let ap_ab = ap.dot(&ab); - let ap_ac = ap.dot(&ac); - let ap_ad = ap.dot(&ad); + let ap_ab = ap.dot(ab); + let ap_ac = ap.dot(ac); + let ap_ad = ap.dot(ad); if ap_ab <= 0.0 && ap_ac <= 0.0 && ap_ad <= 0.0 { // Voronoï region of `a`. @@ -56,9 +53,9 @@ impl PointQueryWithLocation for Tetrahedron { let bd = self.d - self.b; let bp = pt - self.b; - let bp_bc = bp.dot(&bc); - let bp_bd = bp.dot(&bd); - let bp_ab = bp.dot(&ab); + let bp_bc = bp.dot(bc); + let bp_bd = bp.dot(bd); + let bp_ab = bp.dot(ab); if bp_bc <= 0.0 && bp_bd <= 0.0 && bp_ab >= 0.0 { // Voronoï region of `b`. @@ -69,9 +66,9 @@ impl PointQueryWithLocation for Tetrahedron { let cd = self.d - self.c; let cp = pt - self.c; - let cp_ac = cp.dot(&ac); - let cp_bc = cp.dot(&bc); - let cp_cd = cp.dot(&cd); + let cp_ac = cp.dot(ac); + let cp_bc = cp.dot(bc); + let cp_cd = cp.dot(cd); if cp_cd <= 0.0 && cp_bc >= 0.0 && cp_ac >= 0.0 { // Voronoï region of `c`. @@ -81,9 +78,9 @@ impl PointQueryWithLocation for Tetrahedron { let dp = pt - self.d; - let dp_cd = dp.dot(&cd); - let dp_bd = dp.dot(&bd); - let dp_ad = dp.dot(&ad); + let dp_cd = dp.dot(cd); + let dp_bd = dp.dot(bd); + let dp_ad = dp.dot(ad); if dp_ad >= 0.0 && dp_bd >= 0.0 && dp_cd >= 0.0 { // Voronoï region of `d`. @@ -97,12 +94,12 @@ impl PointQueryWithLocation for Tetrahedron { #[inline(always)] fn check_edge( i: usize, - a: &Point, - _: &Point, - nabc: &Vector, - nabd: &Vector, - ap: &Vector, - ab: &Vector, + a: Vector, + _: Vector, + nabc: Vector, + nabd: Vector, + ap: Vector, + ab: Vector, ap_ab: Real, /*ap_ac: Real, ap_ad: Real,*/ bp_ab: Real, /*bp_ac: Real, bp_ad: Real*/ ) -> ( @@ -142,10 +139,10 @@ impl PointQueryWithLocation for Tetrahedron { // Voronoï region of ab. // let bp_ad = bp_bd + bp_ab; // let bp_ac = bp_bc + bp_ab; - let nabc = ab.cross(&ac); - let nabd = ab.cross(&ad); + let nabc = ab.cross(ac); + let nabd = ab.cross(ad); let (dabc, dabd, res) = check_edge( - 0, &self.a, &self.b, &nabc, &nabd, &ap, &ab, ap_ab, + 0, self.a, self.b, nabc, nabd, ap, ab, ap_ab, /*ap_ac, ap_ad,*/ bp_ab, /*, bp_ac, bp_ad*/ ); if let Some(res) = res { @@ -159,9 +156,9 @@ impl PointQueryWithLocation for Tetrahedron { // d -> b // let cp_ab = cp_ac - cp_bc; // let cp_ad = cp_cd + cp_ac; - let nacd = ac.cross(&ad); + let nacd = ac.cross(ad); let (dacd, dacb, res) = check_edge( - 1, &self.a, &self.c, &nacd, &-nabc, &ap, &ac, ap_ac, + 1, self.a, self.c, nacd, -nabc, ap, ac, ap_ac, /*ap_ad, ap_ab,*/ cp_ac, /*, cp_ad, cp_ab*/ ); if let Some(res) = res { @@ -176,7 +173,7 @@ impl PointQueryWithLocation for Tetrahedron { // let dp_ac = dp_ad - dp_cd; // let dp_ab = dp_ad - dp_bd; let (dadb, dadc, res) = check_edge( - 2, &self.a, &self.d, &-nabd, &-nacd, &ap, &ad, ap_ad, + 2, self.a, self.d, -nabd, -nacd, ap, ad, ap_ad, /*ap_ab, ap_ac,*/ dp_ad, /*, dp_ab, dp_ac*/ ); if let Some(res) = res { @@ -189,10 +186,10 @@ impl PointQueryWithLocation for Tetrahedron { // b -> c // c -> a // let cp_bd = cp_cd + cp_bc; - let nbcd = bc.cross(&bd); + let nbcd = bc.cross(bd); // NOTE: nabc = nbcd let (dbca, dbcd, res) = check_edge( - 3, &self.b, &self.c, &nabc, &nbcd, &bp, &bc, bp_bc, + 3, self.b, self.c, nabc, nbcd, bp, bc, bp_bc, /*-bp_ab, bp_bd,*/ cp_bc, /*, -cp_ab, cp_bd*/ ); if let Some(res) = res { @@ -209,7 +206,7 @@ impl PointQueryWithLocation for Tetrahedron { // NOTE: nbdc = -nbcd // NOTE: nbda = nabd let (dbdc, dbda, res) = check_edge( - 4, &self.b, &self.d, &-nbcd, &nabd, &bp, &bd, bp_bd, + 4, self.b, self.d, -nbcd, nabd, bp, bd, bp_bd, /*bp_bc, -bp_ab,*/ dp_bd, /*, dp_bc, -dp_ab*/ ); if let Some(res) = res { @@ -225,7 +222,7 @@ impl PointQueryWithLocation for Tetrahedron { // NOTE: ncda = nacd // NOTE: ncdb = nbcd let (dcda, dcdb, res) = check_edge( - 5, &self.c, &self.d, &nacd, &nbcd, &cp, &cd, cp_cd, + 5, self.c, self.d, nacd, nbcd, cp, cd, cp_cd, /*-cp_ac, -cp_bc,*/ dp_cd, /*, -dp_ac, -dp_bc*/ ); if let Some(res) = res { @@ -238,15 +235,15 @@ impl PointQueryWithLocation for Tetrahedron { #[inline(always)] fn check_face( i: usize, - a: &Point, - b: &Point, - c: &Point, - ap: &Vector, - bp: &Vector, - cp: &Vector, - ab: &Vector, - ac: &Vector, - ad: &Vector, + a: Vector, + b: Vector, + c: Vector, + ap: Vector, + bp: Vector, + cp: Vector, + ab: Vector, + ac: Vector, + ad: Vector, dabc: Real, dbca: Real, dacb: Real, @@ -270,17 +267,17 @@ impl PointQueryWithLocation for Tetrahedron { // above were < 0. This happens, e.g., when we use fixed-point // numbers and there are not enough decimal bits to perform // the normalization. - let normal = n.try_normalize(crate::math::DEFAULT_EPSILON)?; - let vc = normal.dot(&ap.cross(bp)); - let va = normal.dot(&bp.cross(cp)); - let vb = normal.dot(&cp.cross(ap)); + let normal = n.try_normalize()?; + let vc = normal.dot(ap.cross(bp)); + let va = normal.dot(bp.cross(cp)); + let vb = normal.dot(cp.cross(ap)); let denom = va + vb + vc; assert!(denom != 0.0); let inv_denom = 1.0 / denom; let bcoords = [va * inv_denom, vb * inv_denom, vc * inv_denom]; - let res = a * bcoords[0] + b.coords * bcoords[1] + c.coords * bcoords[2]; + let res = a * bcoords[0] + b * bcoords[1] + c * bcoords[2]; let proj = PointProjection::new(false, res); return Some((proj, TetrahedronPointLocation::OnFace(i as u32, bcoords))); @@ -291,7 +288,7 @@ impl PointQueryWithLocation for Tetrahedron { // Face abc. if let Some(res) = check_face( - 0, &self.a, &self.b, &self.c, &ap, &bp, &cp, &ab, &ac, &ad, dabc, dbca, + 0, self.a, self.b, self.c, ap, bp, cp, ab, ac, ad, dabc, dbca, dacb, /*ap_ab, bp_ab, cp_ab, ap_ac, bp_ac, cp_ac*/ @@ -301,7 +298,7 @@ impl PointQueryWithLocation for Tetrahedron { // Face abd. if let Some(res) = check_face( - 1, &self.a, &self.b, &self.d, &ap, &bp, &dp, &ab, &ad, &ac, dadb, dabd, + 1, self.a, self.b, self.d, ap, bp, dp, ab, ad, ac, dadb, dabd, dbda, /*ap_ab, bp_ab, dp_ab, ap_ad, bp_ad, dp_ad*/ @@ -310,7 +307,7 @@ impl PointQueryWithLocation for Tetrahedron { } // Face acd. if let Some(res) = check_face( - 2, &self.a, &self.c, &self.d, &ap, &cp, &dp, &ac, &ad, &ab, dacd, dcda, + 2, self.a, self.c, self.d, ap, cp, dp, ac, ad, ab, dacd, dcda, dadc, /*ap_ac, cp_ac, dp_ac, ap_ad, cp_ad, dp_ad*/ @@ -319,7 +316,7 @@ impl PointQueryWithLocation for Tetrahedron { } // Face bcd. if let Some(res) = check_face( - 3, &self.b, &self.c, &self.d, &bp, &cp, &dp, &bc, &bd, &-ab, dbcd, dcdb, + 3, self.b, self.c, self.d, bp, cp, dp, bc, bd, -ab, dbcd, dcdb, dbdc, /*bp_bc, cp_bc, dp_bc, bp_bd, cp_bd, dp_bd*/ @@ -334,7 +331,7 @@ impl PointQueryWithLocation for Tetrahedron { ) } - let proj = PointProjection::new(true, *pt); + let proj = PointProjection::new(true, pt); (proj, TetrahedronPointLocation::OnSolid) } } diff --git a/src/query/point/point_triangle.rs b/src/query/point/point_triangle.rs index 576a13db..f93b265e 100644 --- a/src/query/point/point_triangle.rs +++ b/src/query/point/point_triangle.rs @@ -1,33 +1,30 @@ -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{Real, Vector, DIM}; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, Triangle, TrianglePointLocation}; #[inline] -fn compute_result(pt: &Point, proj: Point) -> PointProjection { +fn compute_result(pt: Vector, proj: Vector) -> PointProjection { #[cfg(feature = "dim2")] { - PointProjection::new(*pt == proj, proj) + PointProjection::new(pt == proj, proj) } #[cfg(feature = "dim3")] { // TODO: is this acceptable to assume the point is inside of the // triangle if it is close enough? - PointProjection::new(relative_eq!(proj, *pt), proj) + PointProjection::new(relative_eq!(proj, pt), proj) } } impl PointQuery for Triangle { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { self.project_local_point_and_get_location(pt, solid).0 } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { let (proj, loc) = if DIM == 2 { self.project_local_point_and_get_location(pt, false) } else { @@ -57,7 +54,7 @@ impl PointQueryWithLocation for Triangle { #[inline] fn project_local_point_and_get_location( &self, - pt: &Point, + pt: Vector, solid: bool, ) -> (PointProjection, Self::Location) { // To understand the ideas, consider reading the slides below @@ -71,8 +68,8 @@ impl PointQueryWithLocation for Triangle { let ac = c - a; let ap = pt - a; - let ab_ap = ab.dot(&ap); - let ac_ap = ac.dot(&ap); + let ab_ap = ab.dot(ap); + let ac_ap = ac.dot(ap); if ab_ap <= 0.0 && ac_ap <= 0.0 { // Voronoï region of `a`. @@ -80,8 +77,8 @@ impl PointQueryWithLocation for Triangle { } let bp = pt - b; - let ab_bp = ab.dot(&bp); - let ac_bp = ac.dot(&bp); + let ab_bp = ab.dot(bp); + let ac_bp = ac.dot(bp); if ab_bp >= 0.0 && ac_bp <= ab_bp { // Voronoï region of `b`. @@ -89,8 +86,8 @@ impl PointQueryWithLocation for Triangle { } let cp = pt - c; - let ab_cp = ab.dot(&cp); - let ac_cp = ac.dot(&cp); + let ab_cp = ab.dot(cp); + let ac_cp = ac.dot(cp); if ac_cp >= 0.0 && ab_cp <= ac_cp { // Voronoï region of `c`. @@ -109,12 +106,12 @@ impl PointQueryWithLocation for Triangle { // For 2D and 3D, it uses explicit cross/perp products that are // more numerically stable. fn stable_check_edges_voronoi( - ab: &Vector, - ac: &Vector, - bc: &Vector, - ap: &Vector, - bp: &Vector, - cp: &Vector, + ab: Vector, + ac: Vector, + bc: Vector, + ap: Vector, + bp: Vector, + cp: Vector, ab_ap: Real, ab_bp: Real, ac_ap: Real, @@ -124,18 +121,18 @@ impl PointQueryWithLocation for Triangle { ) -> ProjectionInfo { #[cfg(feature = "dim2")] { - let n = ab.perp(ac); - let vc = n * ab.perp(ap); + let n = ab.perp_dot(ac); + let vc = n * ab.perp_dot(ap); if vc < 0.0 && ab_ap >= 0.0 && ab_bp <= 0.0 { return ProjectionInfo::OnAB; } - let vb = -n * ac.perp(cp); + let vb = -n * ac.perp_dot(cp); if vb < 0.0 && ac_ap >= 0.0 && ac_cp <= 0.0 { return ProjectionInfo::OnAC; } - let va = n * bc.perp(bp); + let va = n * bc.perp_dot(bp); if va < 0.0 && ac_bp - ab_bp >= 0.0 && ab_cp - ac_cp >= 0.0 { return ProjectionInfo::OnBC; } @@ -148,7 +145,7 @@ impl PointQueryWithLocation for Triangle { #[cfg(feature = "improved_fixed_point_support")] { - let scaled_n = ab.cross(&ac); + let scaled_n = ab.cross(ac); n = scaled_n.try_normalize(0.0).unwrap_or(scaled_n); } @@ -157,17 +154,17 @@ impl PointQueryWithLocation for Triangle { n = ab.cross(ac); } - let vc = n.dot(&ab.cross(ap)); + let vc = n.dot(ab.cross(ap)); if vc < 0.0 && ab_ap >= 0.0 && ab_bp <= 0.0 { return ProjectionInfo::OnAB; } - let vb = -n.dot(&ac.cross(cp)); + let vb = -n.dot(ac.cross(cp)); if vb < 0.0 && ac_ap >= 0.0 && ac_cp <= 0.0 { return ProjectionInfo::OnAC; } - let va = n.dot(&bc.cross(bp)); + let va = n.dot(bc.cross(bp)); if va < 0.0 && ac_bp - ab_bp >= 0.0 && ab_cp - ac_cp >= 0.0 { return ProjectionInfo::OnBC; } @@ -180,11 +177,11 @@ impl PointQueryWithLocation for Triangle { let bc = c - b; match stable_check_edges_voronoi( - &ab, &ac, &bc, &ap, &bp, &cp, ab_ap, ab_bp, ac_ap, ac_cp, ac_bp, ab_cp, + ab, ac, bc, ap, bp, cp, ab_ap, ab_bp, ac_ap, ac_cp, ac_bp, ab_cp, ) { ProjectionInfo::OnAB => { // Voronoï region of `ab`. - let v = ab_ap / ab.norm_squared(); + let v = ab_ap / ab.length_squared(); let bcoords = [1.0 - v, v]; let res = a + ab * v; @@ -195,7 +192,7 @@ impl PointQueryWithLocation for Triangle { } ProjectionInfo::OnAC => { // Voronoï region of `ac`. - let w = ac_ap / ac.norm_squared(); + let w = ac_ap / ac.length_squared(); let bcoords = [1.0 - w, w]; let res = a + ac * w; @@ -206,7 +203,7 @@ impl PointQueryWithLocation for Triangle { } ProjectionInfo::OnBC => { // Voronoï region of `bc`. - let w = bc.dot(&bp) / bc.norm_squared(); + let w = bc.dot(bp) / bc.length_squared(); let bcoords = [1.0 - w, w]; let res = b + bc * w; @@ -241,7 +238,7 @@ impl PointQueryWithLocation for Triangle { // object. if solid { ( - PointProjection::new(true, *pt), + PointProjection::new(true, pt), TrianglePointLocation::OnSolid, ) } else { @@ -254,9 +251,9 @@ impl PointQueryWithLocation for Triangle { let u = (ac_bp - ab_bp) / (ac_bp - ab_bp + ab_cp - ac_cp); // proj on bc = b + bc * u let bc = c - b; - let d_ab = ap.norm_squared() - (ab.norm_squared() * v * v); - let d_ac = ap.norm_squared() - (ac.norm_squared() * w * w); - let d_bc = bp.norm_squared() - (bc.norm_squared() * u * u); + let d_ab = ap.length_squared() - (ab.length_squared() * v * v); + let d_ac = ap.length_squared() - (ac.length_squared() * w * w); + let d_bc = bp.length_squared() - (bc.length_squared() * u * u); let proj; let loc; diff --git a/src/query/point/point_voxels.rs b/src/query/point/point_voxels.rs index 0db3db48..87a5b147 100644 --- a/src/query/point/point_voxels.rs +++ b/src/query/point/point_voxels.rs @@ -1,10 +1,10 @@ -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cuboid, FeatureId, Voxels, VoxelsChunkRef}; impl PointQuery for Voxels { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { self.chunk_bvh() .project_point(pt, Real::MAX, |chunk_id, _| { let chunk = self.chunk_ref(chunk_id); @@ -13,17 +13,11 @@ impl PointQuery for Voxels { .map(|(proj, _)| proj) }) .map(|res| res.1 .1) - .unwrap_or(PointProjection::new( - false, - Vector::repeat(Real::MAX).into(), - )) + .unwrap_or(PointProjection::new(false, Vector::splat(Real::MAX))) } #[inline] - fn project_local_point_and_get_feature( - &self, - pt: &Point, - ) -> (PointProjection, FeatureId) { + fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) { self.chunk_bvh() .project_point_and_get_feature(pt, Real::MAX, |chunk_id, _| { let chunk = self.chunk_ref(chunk_id); @@ -34,7 +28,7 @@ impl PointQuery for Voxels { }) .map(|res| res.1 .1) .unwrap_or(( - PointProjection::new(false, Vector::repeat(Real::MAX).into()), + PointProjection::new(false, Vector::splat(Real::MAX)), FeatureId::Unknown, )) } @@ -44,21 +38,21 @@ impl<'a> VoxelsChunkRef<'a> { #[inline] fn project_local_point_and_get_vox_id( &self, - pt: &Point, + pt: Vector, solid: bool, ) -> Option<(PointProjection, u32)> { // TODO: optimize this naive implementation that just iterates on all the voxels // from this chunk. let base_cuboid = Cuboid::new(self.parent.voxel_size() / 2.0); let mut smallest_dist = Real::MAX; - let mut result = PointProjection::new(false, *pt); + let mut result = PointProjection::new(false, pt); let mut result_vox_id = 0; for vox in self.voxels() { - let mut candidate = base_cuboid.project_local_point(&(pt - vox.center.coords), solid); - candidate.point += vox.center.coords; + let mut candidate = base_cuboid.project_local_point(pt - vox.center, solid); + candidate.point += vox.center; - let candidate_dist = (candidate.point - pt).norm(); + let candidate_dist = (candidate.point - pt).length(); if candidate_dist < smallest_dist { result = candidate; result_vox_id = vox.linear_id.flat_id(); diff --git a/src/query/query_dispatcher.rs b/src/query/query_dispatcher.rs index ddad2ca7..630c2e8b 100644 --- a/src/query/query_dispatcher.rs +++ b/src/query/query_dispatcher.rs @@ -61,12 +61,12 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] { //! use parry3d::query; //! use parry3d::shape::Ball; -//! use parry3d::na::Isometry3; +//! use parry3d::math::Pose; //! //! let ball1 = Ball::new(0.5); //! let ball2 = Ball::new(1.0); -//! let pos1 = Isometry3::identity(); -//! let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +//! let pos1 = Pose::identity(); +//! let pos2 = Pose::translation(5.0, 0.0, 0.0); //! //! // Uses DefaultQueryDispatcher automatically //! let distance = query::distance(&pos1, &ball1, &pos2, &ball2); @@ -81,14 +81,14 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] { //! use parry3d::query::{QueryDispatcher, DefaultQueryDispatcher}; //! use parry3d::shape::Ball; -//! use parry3d::na::Isometry3; +//! use parry3d::math::Pose; //! //! let dispatcher = DefaultQueryDispatcher; //! let ball1 = Ball::new(0.5); //! let ball2 = Ball::new(1.0); //! -//! let pos1 = Isometry3::identity(); -//! let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +//! let pos1 = Pose::identity(); +//! let pos2 = Pose::translation(5.0, 0.0, 0.0); //! //! // Compute relative position of shape2 in shape1's local space //! let pos12 = pos1.inv_mul(&pos2); @@ -121,7 +121,7 @@ //! ```ignore //! use parry3d::query::{QueryDispatcher, Unsupported}; //! use parry3d::shape::Shape; -//! use parry3d::math::{Isometry, Real}; +//! use parry3d::math::{Pose, Real}; //! //! struct MyDispatcher { //! // Your dispatcher state @@ -130,7 +130,7 @@ //! impl QueryDispatcher for MyDispatcher { //! fn intersection_test( //! &self, -//! pos12: &Isometry, +//! pos12: &Pose, //! g1: &dyn Shape, //! g2: &dyn Shape, //! ) -> Result { @@ -149,7 +149,7 @@ //! //! fn distance( //! &self, -//! pos12: &Isometry, +//! pos12: &Pose, //! g1: &dyn Shape, //! g2: &dyn Shape, //! ) -> Result { @@ -188,7 +188,7 @@ //! When chaining dispatchers, returning `Unsupported` allows the next dispatcher in the //! chain to try handling the query. -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::details::ShapeCastOptions; #[cfg(feature = "alloc")] use crate::query::{ @@ -247,7 +247,7 @@ pub trait PersistentQueryDispatcher: QueryD /// spatial and temporal coherence. fn contact_manifolds( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, prediction: Real, @@ -258,7 +258,7 @@ pub trait PersistentQueryDispatcher: QueryD /// Computes the contact-manifold between two convex shapes. fn contact_manifold_convex_convex( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, normal_constraints1: Option<&dyn NormalConstraints>, @@ -343,15 +343,15 @@ pub trait PersistentQueryDispatcher: QueryD /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{QueryDispatcher, DefaultQueryDispatcher}; /// use parry3d::shape::{Ball, Cuboid}; -/// use parry3d::na::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let dispatcher = DefaultQueryDispatcher; /// /// let ball = Ball::new(1.0); -/// let cuboid = Cuboid::new(Vector3::new(2.0, 2.0, 2.0)); +/// let cuboid = Cuboid::new(Vector::splat(2.0)); /// -/// let pos1 = Isometry3::identity(); -/// let pos2 = Isometry3::translation(5.0, 0.0, 0.0); +/// let pos1 = Pose::identity(); +/// let pos2 = Pose::translation(5.0, 0.0, 0.0); /// let pos12 = pos1.inv_mul(&pos2); /// /// // Test intersection @@ -375,7 +375,7 @@ pub trait PersistentQueryDispatcher: QueryD /// impl QueryDispatcher for MyCustomDispatcher { /// fn distance( /// &self, -/// pos12: &Isometry, +/// pos12: &Pose, /// g1: &dyn Shape, /// g2: &dyn Shape, /// ) -> Result { @@ -396,7 +396,7 @@ pub trait PersistentQueryDispatcher: QueryD /// let dispatcher = MyCustomDispatcher.chain(DefaultQueryDispatcher); /// /// // Now all queries try custom dispatcher first, then default -/// let dist = dispatcher.distance(&pos12, shape1, shape2)?; +/// let dist = dispatcher.distance(pos12, shape1, shape2)?; /// # } /// ``` /// @@ -409,7 +409,7 @@ pub trait QueryDispatcher: Send + Sync { /// Tests whether two shapes are intersecting. fn intersection_test( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, ) -> Result; @@ -417,19 +417,14 @@ pub trait QueryDispatcher: Send + Sync { /// Computes the minimum distance separating two shapes. /// /// Returns `0.0` if the objects are touching or penetrating. - fn distance( - &self, - pos12: &Isometry, - g1: &dyn Shape, - g2: &dyn Shape, - ) -> Result; + fn distance(&self, pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape) -> Result; /// Computes one pair of contact points point between two shapes. /// /// Returns `None` if the objects are separated by a distance greater than `prediction`. fn contact( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, prediction: Real, @@ -440,7 +435,7 @@ pub trait QueryDispatcher: Send + Sync { /// Returns `ClosestPoints::Disjoint` if the objects are separated by a distance greater than `max_dist`. fn closest_points( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, max_dist: Real, @@ -462,8 +457,8 @@ pub trait QueryDispatcher: Send + Sync { /// detected is theater than this value. fn cast_shapes( &self, - pos12: &Isometry, - local_vel12: &Vector, + pos12: &Pose, + local_vel12: Vector, g1: &dyn Shape, g2: &dyn Shape, options: ShapeCastOptions, @@ -539,7 +534,7 @@ pub trait QueryDispatcher: Send + Sync { /// impl QueryDispatcher for CustomShapeDispatcher { /// fn distance( /// &self, -/// pos12: &Isometry, +/// pos12: &Pose, /// g1: &dyn Shape, /// g2: &dyn Shape, /// ) -> Result { @@ -558,7 +553,7 @@ pub trait QueryDispatcher: Send + Sync { /// /// // This will use CustomShapeDispatcher if both are MyShape, /// // otherwise DefaultQueryDispatcher handles it -/// let dist = dispatcher.distance(&pos12, shape1, shape2)?; +/// let dist = dispatcher.distance(pos12, shape1, shape2)?; /// # } /// ``` /// @@ -597,30 +592,30 @@ where U: QueryDispatcher, { chain_method!(intersection_test( - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, ) -> bool); - chain_method!(distance(pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape,) -> Real); + chain_method!(distance(pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape,) -> Real); chain_method!(contact( - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, prediction: Real, ) -> Option); chain_method!(closest_points( - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, max_dist: Real, ) -> ClosestPoints); chain_method!(cast_shapes( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &dyn Shape, g2: &dyn Shape, options: ShapeCastOptions, @@ -645,7 +640,7 @@ where U: PersistentQueryDispatcher, { chain_method!(contact_manifolds( - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, prediction: Real, @@ -654,7 +649,7 @@ where ) -> ()); chain_method!(contact_manifold_convex_convex( - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, normal_constraints1: Option<&dyn NormalConstraints>, diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 5561f5be..59fff7ab 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -1,12 +1,10 @@ //! Traits and structure needed to cast rays. -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::FeatureId; #[cfg(feature = "alloc")] use crate::partitioning::BvhLeafCost; -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// A ray for ray-casting queries. /// @@ -23,7 +21,7 @@ use rkyv::{bytecheck, CheckBytes}; /// /// The direction can be any non-zero vector: /// - **Normalized**: `dir` with length 1.0 gives time-of-impact in world units -/// - **Not normalized**: Time-of-impact is scaled by `dir.norm()` +/// - **Not normalized**: Time-of-impact is scaled by `dir.length()` /// /// Most applications use normalized directions for intuitive results. /// @@ -41,17 +39,17 @@ use rkyv::{bytecheck, CheckBytes}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{Ray, RayCast}; /// use parry3d::shape::Ball; -/// use nalgebra::{Point3, Vector3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// /// // Create a ray from origin pointing along +X axis /// let ray = Ray::new( -/// Point3::origin(), -/// Vector3::new(1.0, 0.0, 0.0) // Normalized direction +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0) // Normalized direction /// ); /// /// // Create a ball at position (5, 0, 0) with radius 1 /// let ball = Ball::new(1.0); -/// let ball_pos = Isometry3::translation(5.0, 0.0, 0.0); +/// let ball_pos = Pose::translation(5.0, 0.0, 0.0); /// /// // Cast the ray against the ball /// if let Some(toi) = ball.cast_ray(&ball_pos, &ray, 100.0, true) { @@ -60,7 +58,7 @@ use rkyv::{bytecheck, CheckBytes}; /// /// // Compute the actual hit point /// let hit_point = ray.point_at(toi); -/// assert_eq!(hit_point, Point3::new(4.0, 0.0, 0.0)); +/// assert_eq!(hit_point, Vector::new(4.0, 0.0, 0.0)); /// } /// # } /// ``` @@ -68,23 +66,22 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct Ray { /// Starting point of the ray. /// - /// This is where the ray begins. Points along the ray are computed as + /// This is where the ray begins. Vectors along the ray are computed as /// `origin + dir * t` for `t ≥ 0`. - pub origin: Point, + pub origin: Vector, /// Direction vector of the ray. /// /// This vector points in the direction the ray travels. It does NOT need /// to be normalized, but using a normalized direction makes time-of-impact /// values represent actual distances. - pub dir: Vector, + pub dir: Vector, } impl Ray { @@ -100,20 +97,20 @@ impl Ray { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::Ray; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// // Horizontal ray pointing along +X axis /// let ray = Ray::new( - /// Point3::new(0.0, 5.0, 0.0), - /// Vector3::new(1.0, 0.0, 0.0) + /// Vector::new(0.0, 5.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0) /// ); /// /// // Ray starts at (0, 5, 0) and points along +X - /// assert_eq!(ray.origin, Point3::new(0.0, 5.0, 0.0)); - /// assert_eq!(ray.dir, Vector3::new(1.0, 0.0, 0.0)); + /// assert_eq!(ray.origin, Vector::new(0.0, 5.0, 0.0)); + /// assert_eq!(ray.dir, Vector::new(1.0, 0.0, 0.0)); /// # } /// ``` - pub fn new(origin: Point, dir: Vector) -> Ray { + pub fn new(origin: Vector, dir: Vector) -> Ray { Ray { origin, dir } } @@ -126,21 +123,21 @@ impl Ray { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::Ray; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// - /// let ray = Ray::new(Point3::origin(), Vector3::x()); + /// let ray = Ray::new(Vector::ZERO, Vector::X); /// /// // Translate by (5, 0, 0) - /// let transform = Isometry3::translation(5.0, 0.0, 0.0); + /// let transform = Pose::translation(5.0, 0.0, 0.0); /// let transformed = ray.transform_by(&transform); /// - /// assert_eq!(transformed.origin, Point3::new(5.0, 0.0, 0.0)); - /// assert_eq!(transformed.dir, Vector3::x()); + /// assert_eq!(transformed.origin, Vector::new(5.0, 0.0, 0.0)); + /// assert_eq!(transformed.dir, Vector::X); /// # } /// ``` #[inline] - pub fn transform_by(&self, m: &Isometry) -> Self { - Self::new(m * self.origin, m * self.dir) + pub fn transform_by(&self, m: &Pose) -> Self { + Self::new(m * self.origin, m.rotation * self.dir) } /// Transforms this ray by the inverse of the given isometry. @@ -152,22 +149,22 @@ impl Ray { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::Ray; - /// use nalgebra::{Isometry3, Point3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// - /// let ray = Ray::new(Point3::new(10.0, 0.0, 0.0), Vector3::x()); + /// let ray = Ray::new(Vector::new(10.0, 0.0, 0.0), Vector::X); /// - /// let transform = Isometry3::translation(5.0, 0.0, 0.0); + /// let transform = Pose::translation(5.0, 0.0, 0.0); /// let local_ray = ray.inverse_transform_by(&transform); /// /// // Origin moved back by the translation - /// assert_eq!(local_ray.origin, Point3::new(5.0, 0.0, 0.0)); + /// assert_eq!(local_ray.origin, Vector::new(5.0, 0.0, 0.0)); /// # } /// ``` #[inline] - pub fn inverse_transform_by(&self, m: &Isometry) -> Self { + pub fn inverse_transform_by(&self, m: &Pose) -> Self { Self::new( - m.inverse_transform_point(&self.origin), - m.inverse_transform_vector(&self.dir), + m.inverse_transform_point(self.origin), + m.rotation.inverse() * self.dir, ) } @@ -180,17 +177,17 @@ impl Ray { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::Ray; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// - /// let ray = Ray::new(Point3::origin(), Vector3::x()); - /// let translated = ray.translate_by(Vector3::new(10.0, 5.0, 0.0)); + /// let ray = Ray::new(Vector::ZERO, Vector::X); + /// let translated = ray.translate_by(Vector::new(10.0, 5.0, 0.0)); /// - /// assert_eq!(translated.origin, Point3::new(10.0, 5.0, 0.0)); - /// assert_eq!(translated.dir, Vector3::x()); // Direction unchanged + /// assert_eq!(translated.origin, Vector::new(10.0, 5.0, 0.0)); + /// assert_eq!(translated.dir, Vector::X); // Direction unchanged /// # } /// ``` #[inline] - pub fn translate_by(&self, v: Vector) -> Self { + pub fn translate_by(&self, v: Vector) -> Self { Self::new(self.origin + v, self.dir) } @@ -207,22 +204,22 @@ impl Ray { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::Ray; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let ray = Ray::new( - /// Point3::origin(), - /// Vector3::new(1.0, 0.0, 0.0) + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0) /// ); /// - /// // Point at t=5.0 - /// assert_eq!(ray.point_at(5.0), Point3::new(5.0, 0.0, 0.0)); + /// // Vector at t=5.0 + /// assert_eq!(ray.point_at(5.0), Vector::new(5.0, 0.0, 0.0)); /// - /// // Point at t=0.0 is the origin + /// // Vector at t=0.0 is the origin /// assert_eq!(ray.point_at(0.0), ray.origin); /// # } /// ``` #[inline] - pub fn point_at(&self, t: Real) -> Point { + pub fn point_at(&self, t: Real) -> Vector { self.origin + self.dir * t } } @@ -257,16 +254,16 @@ impl Ray { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{Ray, RayCast}; /// use parry3d::shape::Cuboid; -/// use nalgebra::{Point3, Vector3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let ray = Ray::new( -/// Point3::new(-5.0, 0.0, 0.0), -/// Vector3::new(1.0, 0.0, 0.0) +/// Vector::new(-5.0, 0.0, 0.0), +/// Vector::new(1.0, 0.0, 0.0) /// ); /// /// if let Some(intersection) = cuboid.cast_ray_and_get_normal( -/// &Isometry3::identity(), +/// &Pose::identity(), /// &ray, /// 100.0, /// true @@ -279,7 +276,7 @@ impl Ray { /// assert_eq!(hit_point.x, -1.0); /// /// // Normal points outward (in -X direction for the -X face) -/// assert_eq!(intersection.normal, Vector3::new(-1.0, 0.0, 0.0)); +/// assert_eq!(intersection.normal, Vector::new(-1.0, 0.0, 0.0)); /// } /// # } /// ``` @@ -287,8 +284,7 @@ impl Ray { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct RayIntersection { /// The time of impact (parameter `t`) where the ray hits the shape. @@ -304,8 +300,8 @@ pub struct RayIntersection { /// - May be unreliable if `time_of_impact` is exactly zero /// /// Note: This should be a unit vector but is not enforced by the type system yet. - // TODO: use a Unit instead. - pub normal: Vector, + // TODO: use a Vector instead. + pub normal: Vector, /// The geometric feature (vertex, edge, or face) that was hit. /// @@ -318,7 +314,7 @@ impl RayIntersection { #[inline] /// Creates a new `RayIntersection`. #[cfg(feature = "dim3")] - pub fn new(time_of_impact: Real, normal: Vector, feature: FeatureId) -> RayIntersection { + pub fn new(time_of_impact: Real, normal: Vector, feature: FeatureId) -> RayIntersection { RayIntersection { time_of_impact, normal, @@ -329,7 +325,7 @@ impl RayIntersection { #[inline] /// Creates a new `RayIntersection`. #[cfg(feature = "dim2")] - pub fn new(time_of_impact: Real, normal: Vector, feature: FeatureId) -> RayIntersection { + pub fn new(time_of_impact: Real, normal: Vector, feature: FeatureId) -> RayIntersection { RayIntersection { time_of_impact, normal, @@ -338,10 +334,10 @@ impl RayIntersection { } #[inline] - pub fn transform_by(&self, transform: &Isometry) -> Self { + pub fn transform_by(&self, transform: &Pose) -> Self { RayIntersection { time_of_impact: self.time_of_impact, - normal: transform * self.normal, + normal: transform.rotation * self.normal, feature: self.feature, } } @@ -378,13 +374,7 @@ pub trait RayCast { } /// Computes the time of impact between this transform shape and a ray. - fn cast_ray( - &self, - m: &Isometry, - ray: &Ray, - max_time_of_impact: Real, - solid: bool, - ) -> Option { + fn cast_ray(&self, m: &Pose, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { let ls_ray = ray.inverse_transform_by(m); self.cast_local_ray(&ls_ray, max_time_of_impact, solid) } @@ -392,7 +382,7 @@ pub trait RayCast { /// Computes the time of impact, and normal between this transformed shape and a ray. fn cast_ray_and_get_normal( &self, - m: &Isometry, + m: &Pose, ray: &Ray, max_time_of_impact: Real, solid: bool, @@ -404,7 +394,7 @@ pub trait RayCast { /// Tests whether a ray intersects this transformed shape. #[inline] - fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_time_of_impact: Real) -> bool { + fn intersects_ray(&self, m: &Pose, ray: &Ray, max_time_of_impact: Real) -> bool { let ls_ray = ray.inverse_transform_by(m); self.intersects_local_ray(&ls_ray, max_time_of_impact) } diff --git a/src/query/ray/ray_aabb.rs b/src/query/ray/ray_aabb.rs index 66cbe5b7..d0ad876a 100644 --- a/src/query/ray/ray_aabb.rs +++ b/src/query/ray/ray_aabb.rs @@ -1,7 +1,5 @@ use core::mem; -use na; - use crate::bounding_volume::Aabb; use crate::math::{Real, Vector, DIM}; use crate::query::{Ray, RayCast, RayIntersection}; @@ -72,12 +70,12 @@ fn ray_aabb( ray: &Ray, max_time_of_impact: Real, solid: bool, -) -> Option<(Real, Vector, isize)> { +) -> Option<(Real, Vector, isize)> { use crate::query::clip; - clip::clip_aabb_line(aabb, &ray.origin, &ray.dir).and_then(|(near, far)| { + clip::clip_aabb_line(aabb, ray.origin, ray.dir).and_then(|(near, far)| { if near.0 < 0.0 { if solid { - Some((0.0, na::zero(), far.2)) + Some((0.0, Vector::ZERO, far.2)) } else if far.0 <= max_time_of_impact { Some(far) } else { diff --git a/src/query/ray/ray_ball.rs b/src/query/ray/ray_ball.rs index 1f8d0d0e..4dc7646d 100644 --- a/src/query/ray/ray_ball.rs +++ b/src/query/ray/ray_ball.rs @@ -1,6 +1,4 @@ -use na::{self, ComplexField}; - -use crate::math::{Point, Real}; +use crate::math::{ComplexField, Real, Vector}; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{Ball, FeatureId}; use num::Zero; @@ -8,7 +6,7 @@ use num::Zero; impl RayCast for Ball { #[inline] fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { - ray_toi_with_ball(&Point::origin(), self.radius, ray, solid) + ray_toi_with_ball(Vector::ZERO, self.radius, ray, solid) .1 .filter(|time_of_impact| *time_of_impact <= max_time_of_impact) } @@ -20,7 +18,7 @@ impl RayCast for Ball { max_time_of_impact: Real, solid: bool, ) -> Option { - ray_toi_and_normal_with_ball(&Point::origin(), self.radius, ray, solid) + ray_toi_and_normal_with_ball(Vector::ZERO, self.radius, ray, solid) .1 .filter(|int| int.time_of_impact <= max_time_of_impact) } @@ -31,16 +29,16 @@ impl RayCast for Ball { /// The first result element is `true` if the ray started inside of the ball. #[inline] pub fn ray_toi_with_ball( - center: &Point, + center: Vector, radius: Real, ray: &Ray, solid: bool, ) -> (bool, Option) { - let dcenter = ray.origin - *center; + let dcenter = ray.origin - center; - let a = ray.dir.norm_squared(); - let b = dcenter.dot(&ray.dir); - let c = dcenter.norm_squared() - radius * radius; + let a = ray.dir.length_squared(); + let b = dcenter.dot(ray.dir); + let c = dcenter.length_squared() - radius * radius; // Special case for when the dir is zero. if a.is_zero() { @@ -60,7 +58,7 @@ pub fn ray_toi_with_ball( // no solution (false, None) } else { - let t = (-b - ComplexField::sqrt(delta)) / a; + let t = (-b - ::sqrt(delta)) / a; if t <= 0.0 { // origin inside of the ball @@ -79,7 +77,7 @@ pub fn ray_toi_with_ball( /// Computes the time of impact and contact normal of a ray on a ball. #[inline] pub fn ray_toi_and_normal_with_ball( - center: &Point, + center: Vector, radius: Real, ray: &Ray, solid: bool, diff --git a/src/query/ray/ray_bounding_sphere.rs b/src/query/ray/ray_bounding_sphere.rs index d4beec2d..672a807f 100644 --- a/src/query/ray/ray_bounding_sphere.rs +++ b/src/query/ray/ray_bounding_sphere.rs @@ -6,7 +6,7 @@ use crate::shape::Ball; impl RayCast for BoundingSphere { #[inline] fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { - let centered_ray = ray.translate_by(-self.center().coords); + let centered_ray = ray.translate_by(-self.center()); Ball::new(self.radius()).cast_local_ray(¢ered_ray, max_time_of_impact, solid) } @@ -17,7 +17,7 @@ impl RayCast for BoundingSphere { max_time_of_impact: Real, solid: bool, ) -> Option { - let centered_ray = ray.translate_by(-self.center().coords); + let centered_ray = ray.translate_by(-self.center()); Ball::new(self.radius()).cast_local_ray_and_get_normal( ¢ered_ray, max_time_of_impact, @@ -27,7 +27,7 @@ impl RayCast for BoundingSphere { #[inline] fn intersects_local_ray(&self, ray: &Ray, max_time_of_impact: Real) -> bool { - let centered_ray = ray.translate_by(-self.center().coords); + let centered_ray = ray.translate_by(-self.center()); Ball::new(self.radius()).intersects_local_ray(¢ered_ray, max_time_of_impact) } } diff --git a/src/query/ray/ray_cuboid.rs b/src/query/ray/ray_cuboid.rs index 335da927..f4a55f34 100644 --- a/src/query/ray/ray_cuboid.rs +++ b/src/query/ray/ray_cuboid.rs @@ -1,13 +1,13 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real}; +use crate::math::Real; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::Cuboid; impl RayCast for Cuboid { #[inline] fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { - let dl = Point::from(-self.half_extents); - let ur = Point::from(self.half_extents); + let dl = -self.half_extents; + let ur = self.half_extents; Aabb::new(dl, ur).cast_local_ray(ray, max_time_of_impact, solid) } @@ -18,8 +18,8 @@ impl RayCast for Cuboid { max_time_of_impact: Real, solid: bool, ) -> Option { - let dl = Point::from(-self.half_extents); - let ur = Point::from(self.half_extents); + let dl = -self.half_extents; + let ur = self.half_extents; Aabb::new(dl, ur).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) } } diff --git a/src/query/ray/ray_halfspace.rs b/src/query/ray/ray_halfspace.rs index 5ec27df4..edc4f6f6 100644 --- a/src/query/ray/ray_halfspace.rs +++ b/src/query/ray/ray_halfspace.rs @@ -1,35 +1,29 @@ -use na; - -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{FeatureId, HalfSpace}; /// Computes the time_of_impact of an unbounded line with a halfspace described by its center and normal. #[inline] pub fn line_toi_with_halfspace( - halfspace_center: &Point, - halfspace_normal: &Vector, - line_origin: &Point, - line_dir: &Vector, + halfspace_center: Vector, + halfspace_normal: Vector, + line_origin: Vector, + line_dir: Vector, ) -> Option { - let dpos = *halfspace_center - *line_origin; + let dpos = halfspace_center - line_origin; let denom = halfspace_normal.dot(line_dir); if relative_eq!(denom, 0.0) { None } else { - Some(halfspace_normal.dot(&dpos) / denom) + Some(halfspace_normal.dot(dpos) / denom) } } /// Computes the time_of_impact of a ray with a halfspace described by its center and normal. #[inline] -pub fn ray_toi_with_halfspace( - center: &Point, - normal: &Vector, - ray: &Ray, -) -> Option { - if let Some(t) = line_toi_with_halfspace(center, normal, &ray.origin, &ray.dir) { +pub fn ray_toi_with_halfspace(center: Vector, normal: Vector, ray: &Ray) -> Option { + if let Some(t) = line_toi_with_halfspace(center, normal, ray.origin, ray.dir) { if t >= 0.0 { return Some(t); } @@ -48,14 +42,14 @@ impl RayCast for HalfSpace { ) -> Option { let dpos = -ray.origin; - let dot_normal_dpos = self.normal.dot(&dpos.coords); + let dot_normal_dpos = self.normal.dot(dpos); if solid && dot_normal_dpos > 0.0 { // The ray is inside of the solid half-space. - return Some(RayIntersection::new(0.0, na::zero(), FeatureId::Face(0))); + return Some(RayIntersection::new(0.0, Vector::ZERO, FeatureId::Face(0))); } - let t = dot_normal_dpos / self.normal.dot(&ray.dir); + let t = dot_normal_dpos / self.normal.dot(ray.dir); if t >= 0.0 && t <= max_time_of_impact { let n = if dot_normal_dpos > 0.0 { @@ -64,7 +58,7 @@ impl RayCast for HalfSpace { self.normal }; - Some(RayIntersection::new(t, *n, FeatureId::Face(0))) + Some(RayIntersection::new(t, n, FeatureId::Face(0))) } else { None } diff --git a/src/query/ray/ray_heightfield.rs b/src/query/ray/ray_heightfield.rs index 869f2a51..24246ee2 100644 --- a/src/query/ray/ray_heightfield.rs +++ b/src/query/ray/ray_heightfield.rs @@ -27,7 +27,7 @@ impl RayCast for HeightField { let clip_ray_a = ray.point_at(min_t); // None may happen due to slight numerical errors. - let mut curr = self.cell_at_point(&clip_ray_a).unwrap_or_else(|| { + let mut curr = self.cell_at_point(clip_ray_a).unwrap_or_else(|| { if ray.origin.x > 0.0 { self.num_cells() - 1 } else { @@ -40,15 +40,15 @@ impl RayCast for HeightField { */ if let Some(seg) = self.segment_at(curr) { let (s, t) = query::details::closest_points_line_line_parameters( - &ray.origin, - &ray.dir, - &seg.a, - &seg.scaled_direction(), + ray.origin, + ray.dir, + seg.a, + seg.scaled_direction(), ); if s >= 0.0 && t >= 0.0 && t <= 1.0 { // Cast succeeded on the first element! - let n = seg.normal().unwrap().into_inner(); - let fid = if n.dot(&ray.dir) > 0.0 { + let n = seg.normal().unwrap(); + let fid = if n.dot(ray.dir) > 0.0 { // The ray hit the back face. curr + self.num_cells() } else { @@ -76,13 +76,9 @@ impl RayCast for HeightField { if right { curr += 1; - curr_param = (cell_width * na::convert::(curr as f64) + start_x - - ray.origin.x) - / ray.dir.x; + curr_param = (cell_width * (curr as Real) + start_x - ray.origin.x) / ray.dir.x; } else { - curr_param = - (ray.origin.x - cell_width * na::convert::(curr as f64) - start_x) - / ray.dir.x; + curr_param = (ray.origin.x - cell_width * (curr as Real) - start_x) / ray.dir.x; curr -= 1; } @@ -94,15 +90,15 @@ impl RayCast for HeightField { if let Some(seg) = self.segment_at(curr) { // TODO: test the y-coordinates (equivalent to an Aabb test) before actually computing the intersection. let (s, t) = query::details::closest_points_line_line_parameters( - &ray.origin, - &ray.dir, - &seg.a, - &seg.scaled_direction(), + ray.origin, + ray.dir, + seg.a, + seg.scaled_direction(), ); if t >= 0.0 && t <= 1.0 && s <= max_time_of_impact { - let n = seg.normal().unwrap().into_inner(); - let fid = if n.dot(&ray.dir) > 0.0 { + let n = seg.normal().unwrap(); + let fid = if n.dot(ray.dir) > 0.0 { // The ray hit the back face. curr + self.num_cells() } else { @@ -133,7 +129,7 @@ impl RayCast for HeightField { let (min_t, mut max_t) = aabb.clip_ray_parameters(ray)?; max_t = max_t.min(max_time_of_impact); let clip_ray_a = ray.point_at(min_t); - let mut cell = match self.cell_at_point(&clip_ray_a) { + let mut cell = match self.cell_at_point(clip_ray_a) { Some(cell) => cell, // None may happen due to slight numerical errors. None => { diff --git a/src/query/ray/ray_support_map.rs b/src/query/ray/ray_support_map.rs index 5311e63c..64a89c1a 100644 --- a/src/query/ray/ray_support_map.rs +++ b/src/query/ray/ray_support_map.rs @@ -1,9 +1,7 @@ -use na; - use crate::math::Real; #[cfg(feature = "dim2")] use crate::query; -use crate::query::gjk::{self, CSOPoint, VoronoiSimplex}; +use crate::query::gjk::{self, CsoPoint, VoronoiSimplex}; use crate::query::{Ray, RayCast, RayIntersection}; #[cfg(all(feature = "alloc", feature = "dim2"))] use crate::shape::ConvexPolygon; @@ -23,8 +21,8 @@ pub fn local_ray_intersection_with_support_map_with_params Option { - let supp = shape.local_support_point(&-ray.dir); - simplex.reset(CSOPoint::single_point(supp - ray.origin.coords)); + let supp = shape.local_support_point(-ray.dir); + simplex.reset(CsoPoint::single_point(supp - ray.origin)); let inter = gjk::cast_local_ray(shape, simplex, ray, max_time_of_impact); @@ -33,13 +31,13 @@ pub fn local_ray_intersection_with_support_map_with_params(0.001f64); - let shift = (supp - ray.origin).dot(&ndir) + eps; + let supp = shape.local_support_point(ndir); + let eps: Real = 0.001; + let shift = (supp - ray.origin).dot(ndir) + eps; let new_ray = Ray::new(ray.origin + ndir * shift, -ray.dir); // TODO: replace by? : simplex.translate_by(&(ray.origin - new_ray.origin)); - simplex.reset(CSOPoint::single_point(supp - new_ray.origin.coords)); + simplex.reset(CsoPoint::single_point(supp - new_ray.origin)); gjk::cast_local_ray(shape, simplex, &new_ray, shift + eps).and_then( |(time_of_impact, outward_normal)| { @@ -175,10 +173,10 @@ impl RayCast for Segment { let seg_dir = self.scaled_direction(); let (s, t, parallel) = query::details::closest_points_line_line_parameters_eps( - &ray.origin, - &ray.dir, - &self.a, - &seg_dir, + ray.origin, + ray.dir, + self.a, + seg_dir, crate::math::DEFAULT_EPSILON, ); @@ -187,16 +185,16 @@ impl RayCast for Segment { // the case where there is no intersection at all // from the case where the line are collinear. let dpos = self.a - ray.origin; - let normal = self.normal().map(|n| *n).unwrap_or_else(Vector::zeros); + let normal = self.normal().unwrap_or(Vector::ZERO); - if dpos.dot(&normal).abs() < crate::math::DEFAULT_EPSILON { + if dpos.dot(normal).abs() < crate::math::DEFAULT_EPSILON { // The rays and the segment are collinear. - let dist1 = dpos.dot(&ray.dir); - let dist2 = dist1 + seg_dir.dot(&ray.dir); + let dist1 = dpos.dot(ray.dir); + let dist2 = dist1 + seg_dir.dot(ray.dir); match (dist1 >= 0.0, dist2 >= 0.0) { (true, true) => { - let time_of_impact = dist1.min(dist2) / ray.dir.norm_squared(); + let time_of_impact = dist1.min(dist2) / ray.dir.length_squared(); if time_of_impact > max_time_of_impact { None } else if dist1 <= dist2 { @@ -207,7 +205,7 @@ impl RayCast for Segment { )) } else { Some(RayIntersection::new( - dist2 / ray.dir.norm_squared(), + dist2 / ray.dir.length_squared(), normal, FeatureId::Vertex(1), )) @@ -227,9 +225,9 @@ impl RayCast for Segment { None } } else if s >= 0.0 && s <= max_time_of_impact && t >= 0.0 && t <= 1.0 { - let normal = self.normal().map(|n| *n).unwrap_or_else(Vector::zeros); + let normal = self.normal().unwrap_or(Vector::ZERO); - let dot = normal.dot(&ray.dir); + let dot = normal.dot(ray.dir); if dot > 0.0 { Some(RayIntersection::new(s, -normal, FeatureId::Face(1))) } else if dot < 0.0 { diff --git a/src/query/ray/ray_triangle.rs b/src/query/ray/ray_triangle.rs index 5c30b049..9615484e 100644 --- a/src/query/ray/ray_triangle.rs +++ b/src/query/ray/ray_triangle.rs @@ -1,10 +1,10 @@ use crate::math::Real; #[cfg(feature = "dim2")] use crate::math::Vector; +#[cfg(feature = "dim3")] +use crate::math::Vector; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{FeatureId, Triangle}; -#[cfg(feature = "dim3")] -use {crate::math::Point, na::Vector3}; impl RayCast for Triangle { #[inline] @@ -19,12 +19,21 @@ impl RayCast for Triangle { if solid { // Check if ray starts in triangle - let perp_sign1 = edges[0].scaled_direction().perp(&(ray.origin - edges[0].a)) > 0.0; - let perp_sign2 = edges[1].scaled_direction().perp(&(ray.origin - edges[1].a)) > 0.0; - let perp_sign3 = edges[2].scaled_direction().perp(&(ray.origin - edges[2].a)) > 0.0; + let perp_sign1 = edges[0] + .scaled_direction() + .perp_dot(ray.origin - edges[0].a) + > 0.0; + let perp_sign2 = edges[1] + .scaled_direction() + .perp_dot(ray.origin - edges[1].a) + > 0.0; + let perp_sign3 = edges[2] + .scaled_direction() + .perp_dot(ray.origin - edges[2].a) + > 0.0; if perp_sign1 == perp_sign2 && perp_sign1 == perp_sign3 { - return Some(RayIntersection::new(0.0, Vector::y(), FeatureId::Face(0))); + return Some(RayIntersection::new(0.0, Vector::Y, FeatureId::Face(0))); } } @@ -52,7 +61,7 @@ impl RayCast for Triangle { max_time_of_impact: Real, _: bool, ) -> Option { - let inter = local_ray_intersection_with_triangle(&self.a, &self.b, &self.c, ray)?.0; + let inter = local_ray_intersection_with_triangle(self.a, self.b, self.c, ray)?.0; if inter.time_of_impact <= max_time_of_impact { Some(inter) @@ -68,25 +77,25 @@ impl RayCast for Triangle { /// the intersection point are returned. #[cfg(feature = "dim3")] pub fn local_ray_intersection_with_triangle( - a: &Point, - b: &Point, - c: &Point, + a: Vector, + b: Vector, + c: Vector, ray: &Ray, -) -> Option<(RayIntersection, Vector3)> { - let ab = *b - *a; - let ac = *c - *a; +) -> Option<(RayIntersection, Vector)> { + let ab = b - a; + let ac = c - a; // normal - let n = ab.cross(&ac); - let d = n.dot(&ray.dir); + let n = ab.cross(ac); + let d = n.dot(ray.dir); // the normal and the ray direction are parallel if d == 0.0 { return None; } - let ap = ray.origin - *a; - let t = ap.dot(&n); + let ap = ray.origin - a; + let t = ap.dot(n); // the ray does not intersect the halfspace defined by the triangle if (t < 0.0 && d < 0.0) || (t > 0.0 && d > 0.0) { @@ -100,7 +109,7 @@ pub fn local_ray_intersection_with_triangle( // // intersection: compute barycentric coordinates // - let e = -ray.dir.cross(&ap); + let e = -ray.dir.cross(ap); let mut v; let mut w; @@ -108,13 +117,13 @@ pub fn local_ray_intersection_with_triangle( let normal; if t < 0.0 { - v = -ac.dot(&e); + v = -ac.dot(e); if v < 0.0 || v > d { return None; } - w = ab.dot(&e); + w = ab.dot(e); if w < 0.0 || v + w > d { return None; @@ -126,13 +135,13 @@ pub fn local_ray_intersection_with_triangle( v *= invd; w *= invd; } else { - v = ac.dot(&e); + v = ac.dot(e); if v < 0.0 || v > d { return None; } - w = -ab.dot(&e); + w = -ab.dot(e); if w < 0.0 || v + w > d { return None; @@ -147,6 +156,6 @@ pub fn local_ray_intersection_with_triangle( Some(( RayIntersection::new(time_of_impact, normal, FeatureId::Face(fid)), - Vector3::new(-v - w + 1.0, v, w), + Vector::new(-v - w + 1.0, v, w), )) } diff --git a/src/query/ray/ray_trimesh.rs b/src/query/ray/ray_trimesh.rs index 018b2255..1d68bdbb 100644 --- a/src/query/ray/ray_trimesh.rs +++ b/src/query/ray/ray_trimesh.rs @@ -38,7 +38,7 @@ impl RayCast for TriMesh { // NOTE: implement the ray-cast with culling on its own submodule to facilitate feature gating. #[cfg(feature = "dim3")] mod ray_cast_with_culling { - use crate::math::{Isometry, Real, Vector}; + use crate::math::{Pose, Real, Vector}; use crate::partitioning::Bvh; use crate::query::details::NormalConstraints; use crate::query::{Ray, RayIntersection}; @@ -56,7 +56,7 @@ mod ray_cast_with_culling { } impl RayCullingMode { - fn check(self, tri_normal: &Vector, ray_dir: &Vector) -> bool { + fn check(self, tri_normal: Vector, ray_dir: Vector) -> bool { match self { RayCullingMode::IgnoreBackfaces => tri_normal.dot(ray_dir) < 0.0, RayCullingMode::IgnoreFrontfaces => tri_normal.dot(ray_dir) > 0.0, @@ -76,7 +76,7 @@ mod ray_cast_with_culling { fn map_part_at( &self, shape_id: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { let _ = self.map_untyped_part_at(shape_id, f); } @@ -96,7 +96,7 @@ mod ray_cast_with_culling { &self, i: u32, mut f: impl FnMut( - Option<&Isometry>, + Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>, ) -> T, @@ -104,7 +104,7 @@ mod ray_cast_with_culling { let tri = self.trimesh.triangle(i); let tri_normal = tri.scaled_normal(); - if self.culling.check(&tri_normal, &self.ray.dir) { + if self.culling.check(tri_normal, self.ray.dir) { Some(f(None, &tri, None)) } else { None @@ -115,12 +115,12 @@ mod ray_cast_with_culling { fn map_untyped_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let tri = self.trimesh.triangle(i); let tri_normal = tri.scaled_normal(); - if self.culling.check(&tri_normal, &self.ray.dir) { + if self.culling.check(tri_normal, self.ray.dir) { Some(f(None, &tri, None)) } else { None @@ -138,7 +138,7 @@ mod ray_cast_with_culling { #[inline] pub fn cast_ray_with_culling( &self, - m: &Isometry, + m: &Pose, ray: &Ray, max_time_of_impact: Real, culling: RayCullingMode, @@ -180,20 +180,20 @@ mod ray_cast_with_culling { #[cfg(test)] mod test { + use crate::math::Vector; use crate::query::{Ray, RayCullingMode}; use crate::shape::TriMesh; - use nalgebra::{Point3, Vector3}; #[test] fn cast_ray_on_trimesh_with_culling() { let vertices = vec![ - Point3::origin(), - Point3::new(1.0, 0.0, 0.0), - Point3::new(0.0, 1.0, 0.0), + Vector::ZERO, + Vector::new(1.0, 0.0, 0.0), + Vector::new(0.0, 1.0, 0.0), ]; let indices = vec![[0, 1, 2]]; - let ray_up = Ray::new(Point3::new(0.0, 0.0, -1.0), Vector3::new(0.0, 0.0, 1.0)); - let ray_down = Ray::new(Point3::new(0.0, 0.0, 1.0), Vector3::new(0.0, 0.0, -1.0)); + let ray_up = Ray::new(Vector::new(0.0, 0.0, -1.0), Vector::new(0.0, 0.0, 1.0)); + let ray_down = Ray::new(Vector::new(0.0, 0.0, 1.0), Vector::new(0.0, 0.0, -1.0)); let mesh = TriMesh::new(vertices, indices).unwrap(); assert!(mesh diff --git a/src/query/ray/ray_voxels.rs b/src/query/ray/ray_voxels.rs index 5077b30b..dff96b20 100644 --- a/src/query/ray/ray_voxels.rs +++ b/src/query/ray/ray_voxels.rs @@ -105,7 +105,7 @@ impl<'a> RayCast for VoxelsChunkRef<'a> { break; } - let imin = Vector::from(toi.map(|t| t.0)).imin(); + let imin = Vector::from(toi.map(|t| t.0)).min_position(); if toi[imin].1 { if voxel_key[imin] < domain_maxs[imin] - 1 { diff --git a/src/query/ray/simd_ray.rs b/src/query/ray/simd_ray.rs index 4557bea4..9034ff02 100644 --- a/src/query/ray/simd_ray.rs +++ b/src/query/ray/simd_ray.rs @@ -1,22 +1,23 @@ -use crate::math::{Point, SimdReal, Vector}; +use crate::math::Vector; use crate::query::Ray; -use simba::simd::SimdValue; /// A structure representing 4 rays in an SIMD SoA fashion. +/// +/// Note: When SIMD is not enabled, this is equivalent to a single Ray. #[derive(Debug, Copy, Clone)] pub struct SimdRay { /// The origin of the rays represented as a single SIMD point. - pub origin: Point, + pub origin: Vector, /// The direction of the rays represented as a single SIMD vector. - pub dir: Vector, + pub dir: Vector, } impl SimdRay { /// Creates a new SIMD ray with all its lanes filled with the same ray. pub fn splat(ray: Ray) -> Self { Self { - origin: Point::splat(ray.origin), - dir: Vector::splat(ray.dir), + origin: ray.origin, + dir: ray.dir, } } } diff --git a/src/query/sat/mod.rs b/src/query/sat/mod.rs index e5b2c940..866e69fe 100644 --- a/src/query/sat/mod.rs +++ b/src/query/sat/mod.rs @@ -47,14 +47,14 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] { //! use parry3d::shape::Cuboid; //! use parry3d::query::sat::*; -//! use parry3d::na::{Isometry3, Vector3}; +//! use parry3d::math::{Pose, Vector}; //! //! // Create two boxes -//! let box1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -//! let box2 = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); +//! let box1 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); +//! let box2 = Cuboid::new(Vector::new(0.5, 0.5, 0.5)); //! //! // Position them close together -//! let pos12 = Isometry3::translation(1.5, 0.0, 0.0); +//! let pos12 = Pose::translation(1.5, 0.0, 0.0); //! //! // Test face normals from box1 //! let (separation, _normal) = cuboid_cuboid_find_local_separating_normal_oneway( diff --git a/src/query/sat/sat_cuboid_cuboid.rs b/src/query/sat/sat_cuboid_cuboid.rs index 457fa06c..2c71cf1e 100644 --- a/src/query/sat/sat_cuboid_cuboid.rs +++ b/src/query/sat/sat_cuboid_cuboid.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real, Vector, DIM}; +use crate::math::{Pose, Real, Vector, VectorExt, DIM}; use crate::shape::{Cuboid, SupportMap}; /// Computes the separation distance between two cuboids along a given axis. @@ -27,7 +27,7 @@ use crate::shape::{Cuboid, SupportMap}; /// - **Positive**: Shapes are separated by this distance /// - **Negative**: Shapes are overlapping (penetration depth is the absolute value) /// - **Zero**: Shapes are exactly touching -/// - `Vector`: The oriented axis direction (pointing from cuboid1 toward cuboid2) +/// - `Vector`: The oriented axis direction (pointing from cuboid1 toward cuboid2) /// /// # Example /// @@ -35,20 +35,20 @@ use crate::shape::{Cuboid, SupportMap}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Cuboid; /// use parry3d::query::sat::cuboid_cuboid_compute_separation_wrt_local_line; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// -/// let cube1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// let cube2 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube1 = Cuboid::new(Vector::splat(1.0)); +/// let cube2 = Cuboid::new(Vector::splat(1.0)); /// /// // Position cube2 at (3, 0, 0) relative to cube1 -/// let pos12 = Isometry3::translation(3.0, 0.0, 0.0); +/// let pos12 = Pose::translation(3.0, 0.0, 0.0); /// /// // Test separation along the X axis /// let (separation, _axis) = cuboid_cuboid_compute_separation_wrt_local_line( /// &cube1, /// &cube2, /// &pos12, -/// &Vector3::x() +/// Vector::X /// ); /// /// // Should be separated by 1.0 (distance 3.0 - half_extents 1.0 - 1.0) @@ -59,17 +59,17 @@ use crate::shape::{Cuboid, SupportMap}; pub fn cuboid_cuboid_compute_separation_wrt_local_line( cuboid1: &Cuboid, cuboid2: &Cuboid, - pos12: &Isometry, - axis1: &Vector, -) -> (Real, Vector) { + pos12: &Pose, + axis1: Vector, +) -> (Real, Vector) { #[expect(clippy::unnecessary_cast)] - let signum = (1.0 as Real).copysign(pos12.translation.vector.dot(axis1)); + let signum = (1.0 as Real).copysign(pos12.translation.dot(axis1)); let axis1 = axis1 * signum; - let axis2 = pos12.inverse_transform_vector(&-axis1); - let local_pt1 = cuboid1.local_support_point(&axis1); - let local_pt2 = cuboid2.local_support_point(&axis2); + let axis2 = pos12.rotation.inverse() * -axis1; + let local_pt1 = cuboid1.local_support_point(axis1); + let local_pt2 = cuboid2.local_support_point(axis2); let pt2 = pos12 * local_pt2; - let separation = (pt2 - local_pt1).dot(&axis1); + let separation = (pt2 - local_pt1).dot(axis1); (separation, axis1) } @@ -101,7 +101,7 @@ pub fn cuboid_cuboid_compute_separation_wrt_local_line( /// - `Real`: The best (maximum) separation found across all edge-edge axes /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The axis direction that gives this separation +/// - `Vector`: The axis direction that gives this separation /// /// # Example /// @@ -109,13 +109,13 @@ pub fn cuboid_cuboid_compute_separation_wrt_local_line( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Cuboid; /// use parry3d::query::sat::cuboid_cuboid_find_local_separating_edge_twoway; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// -/// let cube1 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// let cube2 = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube1 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); +/// let cube2 = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// // Rotate and position cube2 so edge-edge contact is likely -/// let pos12 = Isometry3::translation(2.0, 2.0, 0.0); +/// let pos12 = Pose::translation(2.0, 2.0, 0.0); /// /// let (separation, _axis) = cuboid_cuboid_find_local_separating_edge_twoway( /// &cube1, @@ -137,15 +137,15 @@ pub fn cuboid_cuboid_compute_separation_wrt_local_line( pub fn cuboid_cuboid_find_local_separating_edge_twoway( cuboid1: &Cuboid, cuboid2: &Cuboid, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { use approx::AbsDiffEq; let mut best_separation = -Real::MAX; - let mut best_dir = Vector::zeros(); + let mut best_dir = Vector::ZERO; - let x2 = pos12 * Vector::x(); - let y2 = pos12 * Vector::y(); - let z2 = pos12 * Vector::z(); + let x2 = pos12.rotation * Vector::X; + let y2 = pos12.rotation * Vector::Y; + let z2 = pos12.rotation * Vector::Z; // We have 3 * 3 = 9 axes to test. let axes = [ @@ -164,13 +164,13 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( ]; for axis1 in &axes { - let norm1 = axis1.norm(); + let norm1 = axis1.length(); if norm1 > Real::default_epsilon() { let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( cuboid1, cuboid2, pos12, - &(axis1 / norm1), + axis1 / norm1, ); if separation > best_separation { @@ -211,7 +211,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( /// - `Real`: The maximum separation found among cuboid1's face normals /// - **Positive**: Shapes are separated by at least this distance /// - **Negative**: Shapes are overlapping (penetration) -/// - `Vector`: The face normal direction that gives this separation +/// - `Vector`: The face normal direction that gives this separation /// /// # Example /// @@ -219,13 +219,13 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Cuboid; /// use parry2d::query::sat::cuboid_cuboid_find_local_separating_normal_oneway; -/// use nalgebra::{Isometry2, Vector2}; +/// use parry2d::math::{Pose, Vector}; /// -/// let rect1 = Cuboid::new(Vector2::new(1.0, 1.0)); -/// let rect2 = Cuboid::new(Vector2::new(0.5, 0.5)); +/// let rect1 = Cuboid::new(Vector::new(1.0, 1.0)); +/// let rect2 = Cuboid::new(Vector::new(0.5, 0.5)); /// /// // Position rect2 to the right of rect1 -/// let pos12 = Isometry2::translation(2.5, 0.0); +/// let pos12 = Pose::translation(2.5, 0.0); /// /// // Test rect1's face normals (X and Y axes) /// let (sep1, normal1) = cuboid_cuboid_find_local_separating_normal_oneway( @@ -255,17 +255,17 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( pub fn cuboid_cuboid_find_local_separating_normal_oneway( cuboid1: &Cuboid, cuboid2: &Cuboid, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { let mut best_separation = -Real::MAX; - let mut best_dir = Vector::zeros(); + let mut best_dir = Vector::ZERO; for i in 0..DIM { #[expect(clippy::unnecessary_cast)] - let sign = (1.0 as Real).copysign(pos12.translation.vector[i]); + let sign = (1.0 as Real).copysign(pos12.translation[i]); let axis1 = Vector::ith(i, sign); - let axis2 = pos12.inverse_transform_vector(&-axis1); - let local_pt2 = cuboid2.local_support_point(&axis2); + let axis2 = pos12.rotation.inverse() * -axis1; + let local_pt2 = cuboid2.local_support_point(axis2); let pt2 = pos12 * local_pt2; let separation = pt2[i] * sign - cuboid1.half_extents[i]; diff --git a/src/query/sat/sat_cuboid_point.rs b/src/query/sat/sat_cuboid_point.rs index 9e002caa..460e1633 100644 --- a/src/query/sat/sat_cuboid_point.rs +++ b/src/query/sat/sat_cuboid_point.rs @@ -1,8 +1,6 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::{Cuboid, SupportMap}; -use na::Unit; - /// Computes the separation distance between a point and a cuboid along a specified normal direction. /// /// This function is used in SAT (Separating Axis Theorem) implementations for shapes that can be @@ -31,7 +29,7 @@ use na::Unit; /// - **Positive**: The point and cuboid are separated /// - **Negative**: The point penetrates the cuboid (or normal is None) /// - **Zero**: The point exactly touches the cuboid surface -/// - `Vector`: The oriented normal direction used for the test (pointing from point toward cuboid) +/// - `Vector`: The oriented normal direction used for the test (pointing from point toward cuboid) /// /// # Example /// @@ -39,14 +37,14 @@ use na::Unit; /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Cuboid; /// use parry2d::query::sat::point_cuboid_find_local_separating_normal_oneway; -/// use nalgebra::{Point2, Vector2, Isometry2, Unit}; +/// use parry2d::math::{Vector, Pose}; /// -/// let point = Point2::origin(); -/// let normal = Some(Unit::new_normalize(Vector2::x())); -/// let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); +/// let point = Vector::ZERO; +/// let normal = Some((Vector::X.normalize())); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); /// /// // Position cuboid 3 units to the right -/// let pos12 = Isometry2::translation(3.0, 0.0); +/// let pos12 = Pose::translation(3.0, 0.0); /// /// let (separation, _dir) = point_cuboid_find_local_separating_normal_oneway( /// point, @@ -66,27 +64,27 @@ use na::Unit; /// because it exploits the cuboid's symmetry around the origin. The cuboid must be centered at /// its local origin for this optimization to be valid. pub fn point_cuboid_find_local_separating_normal_oneway( - point1: Point, - normal1: Option>>, + point1: Vector, + normal1: Option, shape2: &Cuboid, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { let mut best_separation = -Real::MAX; - let mut best_dir = Vector::zeros(); + let mut best_dir = Vector::ZERO; if let Some(normal1) = normal1 { - let axis1 = if (pos12.translation.vector - point1.coords).dot(&normal1) >= 0.0 { + let axis1 = if (pos12.translation - point1).dot(normal1) >= 0.0 { normal1 } else { -normal1 }; - let pt2 = shape2.support_point_toward(pos12, &-axis1); - let separation = (pt2 - point1).dot(&axis1); + let pt2 = shape2.support_point_toward(pos12, -axis1); + let separation = (pt2 - point1).dot(axis1); if separation > best_separation { best_separation = separation; - best_dir = *axis1; + best_dir = axis1; } } diff --git a/src/query/sat/sat_cuboid_segment.rs b/src/query/sat/sat_cuboid_segment.rs index 0906e109..9a0127a5 100644 --- a/src/query/sat/sat_cuboid_segment.rs +++ b/src/query/sat/sat_cuboid_segment.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::sat; use crate::shape::{Cuboid, Segment}; @@ -20,7 +20,7 @@ use crate::shape::{Cuboid, Segment}; /// - `Real`: The maximum separation found across all edge-edge axes /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The axis direction that gives this separation +/// - `Vector`: The axis direction that gives this separation /// /// # The 3 Axes Tested /// @@ -38,16 +38,16 @@ use crate::shape::{Cuboid, Segment}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Cuboid, Segment}; /// use parry3d::query::sat::cuboid_segment_find_local_separating_edge_twoway; -/// use nalgebra::{Point3, Vector3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// -/// let cube = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let segment = Segment::new( -/// Point3::origin(), -/// Point3::new(0.0, 2.0, 0.0) +/// Vector::ZERO, +/// Vector::new(0.0, 2.0, 0.0) /// ); /// /// // Position segment to the right of the cube -/// let pos12 = Isometry3::translation(2.5, 0.0, 0.0); +/// let pos12 = Pose::translation(2.5, 0.0, 0.0); /// /// let (separation, axis) = cuboid_segment_find_local_separating_edge_twoway( /// &cube, @@ -69,9 +69,9 @@ use crate::shape::{Cuboid, Segment}; pub fn cuboid_segment_find_local_separating_edge_twoway( cube1: &Cuboid, segment2: &Segment, - pos12: &Isometry, -) -> (Real, Vector) { - let x2 = pos12 * (segment2.b - segment2.a); + pos12: &Pose, +) -> (Real, Vector) { + let x2 = pos12.rotation * (segment2.b - segment2.a); let axes = [ // Vector::{x, y ,z}().cross(y2) @@ -105,7 +105,7 @@ pub fn cuboid_segment_find_local_separating_edge_twoway( /// - `Real`: The separation distance along the segment's normal /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The normal direction that gives this separation +/// - `Vector`: The normal direction that gives this separation /// /// # Example /// @@ -113,17 +113,17 @@ pub fn cuboid_segment_find_local_separating_edge_twoway( /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::{Segment, Cuboid}; /// use parry2d::query::sat::segment_cuboid_find_local_separating_normal_oneway; -/// use nalgebra::{Point2, Vector2, Isometry2}; +/// use parry2d::math::{Vector, Pose}; /// /// // Horizontal segment /// let segment = Segment::new( -/// Point2::origin(), -/// Point2::new(2.0, 0.0) +/// Vector::ZERO, +/// Vector::new(2.0, 0.0) /// ); -/// let cuboid = Cuboid::new(Vector2::new(1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0)); /// /// // Position cuboid above the segment -/// let pos12 = Isometry2::translation(1.0, 2.5); +/// let pos12 = Pose::translation(1.0, 2.5); /// /// let (separation, normal) = segment_cuboid_find_local_separating_normal_oneway( /// &segment, @@ -144,8 +144,8 @@ pub fn cuboid_segment_find_local_separating_edge_twoway( pub fn segment_cuboid_find_local_separating_normal_oneway( segment1: &Segment, shape2: &Cuboid, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { sat::point_cuboid_find_local_separating_normal_oneway( segment1.a, segment1.normal(), diff --git a/src/query/sat/sat_cuboid_support_map.rs b/src/query/sat/sat_cuboid_support_map.rs index 230412a4..156bca59 100644 --- a/src/query/sat/sat_cuboid_support_map.rs +++ b/src/query/sat/sat_cuboid_support_map.rs @@ -1,8 +1,6 @@ -use crate::math::{Isometry, Real, Vector, DIM}; +use crate::math::{Pose, Real, Vector, VectorExt, DIM}; use crate::shape::{Cuboid, SupportMap}; -use na::Unit; - /// Computes the separation distance between a cuboid and a convex support map shape along a given axis. /// /// This function tests both the positive and negative directions of the axis to find which @@ -22,7 +20,7 @@ use na::Unit; /// - `Real`: The separation distance along the better of the two axis directions /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Unit>`: The axis direction (either `axis1` or `-axis1`) that gives this separation +/// - `Vector`: The axis direction (either `axis1` or `-axis1`) that gives this separation /// /// # Why Test Both Directions? /// @@ -39,21 +37,21 @@ use na::Unit; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Cuboid, Ball}; /// use parry3d::query::sat::cuboid_support_map_compute_separation_wrt_local_line; -/// use nalgebra::{Isometry3, Vector3, Unit}; +/// use parry3d::math::{Pose, Vector}; /// -/// let cube = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube = Cuboid::new(Vector::splat(1.0)); /// let sphere = Ball::new(0.5); /// /// // Position sphere near the cube -/// let pos12 = Isometry3::translation(2.0, 0.0, 0.0); +/// let pos12 = Pose::translation(2.0, 0.0, 0.0); /// /// // Test separation along an arbitrary axis -/// let axis = Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)); +/// let axis = Vector::new(1.0, 1.0, 0.0).normalize(); /// let (separation, chosen_axis) = cuboid_support_map_compute_separation_wrt_local_line( /// &cube, /// &sphere, /// &pos12, -/// &axis +/// axis /// ); /// /// println!("Separation: {} along axis: {:?}", separation, chosen_axis); @@ -69,30 +67,30 @@ use na::Unit; pub fn cuboid_support_map_compute_separation_wrt_local_line( cube1: &Cuboid, shape2: &impl SupportMap, - pos12: &Isometry, - axis1: &Unit>, -) -> (Real, Unit>) { - let axis1_2 = pos12.inverse_transform_unit_vector(axis1); + pos12: &Pose, + axis1: Vector, +) -> (Real, Vector) { + let axis1_2 = pos12.rotation.inverse() * axis1; let separation1 = { let axis2 = -axis1_2; let local_pt1 = cube1.local_support_point_toward(axis1); - let local_pt2 = shape2.local_support_point_toward(&axis2); + let local_pt2 = shape2.local_support_point_toward(axis2); let pt2 = pos12 * local_pt2; (pt2 - local_pt1).dot(axis1) }; let separation2 = { let axis2 = axis1_2; - let local_pt1 = cube1.local_support_point_toward(&-*axis1); - let local_pt2 = shape2.local_support_point_toward(&axis2); + let local_pt1 = cube1.local_support_point_toward(-axis1); + let local_pt2 = shape2.local_support_point_toward(axis2); let pt2 = pos12 * local_pt2; - (pt2 - local_pt1).dot(&-*axis1) + (pt2 - local_pt1).dot(-axis1) }; if separation1 > separation2 { - (separation1, *axis1) + (separation1, axis1) } else { - (separation2, -*axis1) + (separation2, -axis1) } } @@ -115,7 +113,7 @@ pub fn cuboid_support_map_compute_separation_wrt_local_line( /// - `Real`: The maximum separation found across all tested axes /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping (minimum penetration) -/// - `Vector`: The axis direction that gives this separation (normalized) +/// - `Vector`: The axis direction that gives this separation (normalized) /// /// # Why Precomputed Axes? /// @@ -132,20 +130,20 @@ pub fn cuboid_support_map_compute_separation_wrt_local_line( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Cuboid, Capsule}; /// use parry3d::query::sat::cuboid_support_map_find_local_separating_edge_twoway; -/// use nalgebra::{Isometry3, Vector3, Point3}; +/// use parry3d::math::{Pose, Vector}; /// -/// let cube = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); -/// let capsule = Capsule::new(Point3::new(0.0, -1.0, 0.0), Point3::new(0.0, 1.0, 0.0), 0.5); +/// let cube = Cuboid::new(Vector::splat(1.0)); +/// let capsule = Capsule::new(Vector::new(0.0, -1.0, 0.0), Vector::new(0.0, 1.0, 0.0), 0.5); /// /// // Position capsule near the cube -/// let pos12 = Isometry3::translation(2.0, 0.0, 0.0); +/// let pos12 = Pose::translation(2.0, 0.0, 0.0); /// /// // Compute edge cross products -/// let capsule_dir = Vector3::y(); // capsule's axis direction +/// let capsule_dir = Vector::Y; // capsule's axis direction /// let axes = [ -/// Vector3::x().cross(&capsule_dir), // cube X × capsule axis -/// Vector3::y().cross(&capsule_dir), // cube Y × capsule axis -/// Vector3::z().cross(&capsule_dir), // cube Z × capsule axis +/// Vector::X.cross(capsule_dir), // cube X × capsule axis +/// Vector::Y.cross(capsule_dir), // cube Y × capsule axis +/// Vector::Z.cross(capsule_dir), // cube Z × capsule axis /// ]; /// /// let (separation, axis) = cuboid_support_map_find_local_separating_edge_twoway( @@ -155,7 +153,7 @@ pub fn cuboid_support_map_compute_separation_wrt_local_line( /// &pos12 /// ); /// -/// println!("Best edge-edge separation: {} along {}", separation, axis); +/// println!("Best edge-edge separation: {} along {:?}", separation, axis); /// # } /// ``` /// @@ -169,21 +167,20 @@ pub fn cuboid_support_map_compute_separation_wrt_local_line( pub fn cuboid_support_map_find_local_separating_edge_twoway( cube1: &Cuboid, shape2: &impl SupportMap, - axes: &[Vector], - pos12: &Isometry, -) -> (Real, Vector) { - use approx::AbsDiffEq; + axes: &[Vector], + pos12: &Pose, +) -> (Real, Vector) { let mut best_separation = -Real::MAX; - let mut best_dir = Vector::zeros(); + let mut best_dir = Vector::ZERO; for axis1 in axes { - if let Some(axis1) = Unit::try_new(*axis1, Real::default_epsilon()) { + if let Some(axis1) = (*axis1).try_normalize() { let (separation, axis1) = - cuboid_support_map_compute_separation_wrt_local_line(cube1, shape2, pos12, &axis1); + cuboid_support_map_compute_separation_wrt_local_line(cube1, shape2, pos12, axis1); if separation > best_separation { best_separation = separation; - best_dir = *axis1; + best_dir = axis1; } } } @@ -208,7 +205,7 @@ pub fn cuboid_support_map_find_local_separating_edge_twoway( /// - `Real`: The maximum separation found among the cuboid's face normals /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The face normal direction that gives this separation +/// - `Vector`: The face normal direction that gives this separation /// /// # Usage in Complete SAT /// @@ -225,13 +222,13 @@ pub fn cuboid_support_map_find_local_separating_edge_twoway( /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::{Cuboid, Ball}; /// use parry2d::query::sat::cuboid_support_map_find_local_separating_normal_oneway; -/// use nalgebra::{Isometry2, Vector2}; +/// use parry2d::math::{Pose, Vector}; /// -/// let cube = Cuboid::new(Vector2::new(1.0, 1.0)); +/// let cube = Cuboid::new(Vector::new(1.0, 1.0)); /// let sphere = Ball::new(0.5); /// /// // Position sphere to the right of the cube -/// let pos12 = Isometry2::translation(2.0, 0.0); +/// let pos12 = Pose::translation(2.0, 0.0); /// /// let (separation, normal) = cuboid_support_map_find_local_separating_normal_oneway( /// &cube, @@ -255,15 +252,15 @@ pub fn cuboid_support_map_find_local_separating_edge_twoway( pub fn cuboid_support_map_find_local_separating_normal_oneway( cube1: &Cuboid, shape2: &S, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { let mut best_separation = -Real::MAX; - let mut best_dir = Vector::zeros(); + let mut best_dir = Vector::ZERO; for i in 0..DIM { for sign in &[-1.0, 1.0] { let axis1 = Vector::ith(i, *sign); - let pt2 = shape2.support_point_toward(pos12, &Unit::new_unchecked(-axis1)); + let pt2 = shape2.support_point_toward(pos12, -axis1); let separation = pt2[i] * *sign - cube1.half_extents[i]; if separation > best_separation { diff --git a/src/query/sat/sat_cuboid_triangle.rs b/src/query/sat/sat_cuboid_triangle.rs index 47bdc6be..81338c0f 100644 --- a/src/query/sat/sat_cuboid_triangle.rs +++ b/src/query/sat/sat_cuboid_triangle.rs @@ -1,12 +1,15 @@ #[cfg(feature = "dim3")] use crate::approx::AbsDiffEq; -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; #[cfg(feature = "dim3")] use crate::query::sat; #[cfg(feature = "dim2")] use crate::query::sat::support_map_support_map_compute_separation; use crate::shape::{Cuboid, SupportMap, Triangle}; +#[cfg(all(feature = "dim3", not(feature = "std")))] +use simba::scalar::ComplexField; + /// Finds the best separating axis by testing all edge-edge combinations between a cuboid and a triangle (3D only). /// /// In 3D collision detection, when a box and triangle intersect, the contact may occur along an @@ -25,7 +28,7 @@ use crate::shape::{Cuboid, SupportMap, Triangle}; /// - `Real`: The maximum separation found across all edge-edge axes /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping (penetration depth) -/// - `Vector`: The axis direction that gives this separation +/// - `Vector`: The axis direction that gives this separation /// /// # The 9 Axes Tested /// @@ -42,17 +45,17 @@ use crate::shape::{Cuboid, SupportMap, Triangle}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Cuboid, Triangle}; /// use parry3d::query::sat::cuboid_triangle_find_local_separating_edge_twoway; -/// use nalgebra::{Point3, Vector3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// -/// let cube = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let triangle = Triangle::new( -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(0.0, 1.0, 0.0) +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0) /// ); /// /// // Position triangle near the cube -/// let pos12 = Isometry3::translation(2.0, 0.0, 0.0); +/// let pos12 = Pose::translation(2.0, 0.0, 0.0); /// /// let (separation, axis) = cuboid_triangle_find_local_separating_edge_twoway( /// &cube, @@ -75,8 +78,8 @@ use crate::shape::{Cuboid, SupportMap, Triangle}; pub fn cuboid_triangle_find_local_separating_edge_twoway( cube1: &Cuboid, triangle2: &Triangle, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { // NOTE: everything in this method will be expressed // in the local-space of the first triangle. So we // don't bother adding 2_1 suffixes (e.g. `a2_1`) to everything in @@ -106,30 +109,30 @@ pub fn cuboid_triangle_find_local_separating_edge_twoway( ]; let tri_dots = [ - (axes[0].dot(&a.coords), axes[0].dot(&c.coords)), - (axes[1].dot(&a.coords), axes[1].dot(&c.coords)), - (axes[2].dot(&a.coords), axes[2].dot(&c.coords)), - (axes[3].dot(&a.coords), axes[3].dot(&c.coords)), - (axes[4].dot(&a.coords), axes[4].dot(&c.coords)), - (axes[5].dot(&a.coords), axes[5].dot(&c.coords)), - (axes[6].dot(&a.coords), axes[6].dot(&b.coords)), - (axes[7].dot(&a.coords), axes[7].dot(&b.coords)), - (axes[8].dot(&a.coords), axes[8].dot(&b.coords)), + (axes[0].dot(a), axes[0].dot(c)), + (axes[1].dot(a), axes[1].dot(c)), + (axes[2].dot(a), axes[2].dot(c)), + (axes[3].dot(a), axes[3].dot(c)), + (axes[4].dot(a), axes[4].dot(c)), + (axes[5].dot(a), axes[5].dot(c)), + (axes[6].dot(a), axes[6].dot(b)), + (axes[7].dot(a), axes[7].dot(b)), + (axes[8].dot(a), axes[8].dot(b)), ]; let mut best_sep = -Real::MAX; let mut best_axis = axes[0]; for (i, axis) in axes.iter().enumerate() { - let axis_norm_squared = axis.norm_squared(); + let axis_norm_squared = axis.length_squared(); if axis_norm_squared > Real::default_epsilon() { - let axis_norm = na::ComplexField::sqrt(axis_norm_squared); + let axis_norm = axis_norm_squared.sqrt(); // NOTE: for both axis and -axis, the dot1 will have the same // value because of the cuboid's symmetry. - let local_pt1 = cube1.local_support_point(axis); - let dot1 = local_pt1.coords.dot(axis) / axis_norm; + let local_pt1 = cube1.local_support_point(*axis); + let dot1 = local_pt1.dot(*axis) / axis_norm; let (dot2_min, dot2_max) = crate::utils::sort2(tri_dots[i].0, tri_dots[i].1); @@ -169,7 +172,7 @@ pub fn cuboid_triangle_find_local_separating_edge_twoway( /// - `Real`: The maximum separation found among the triangle's edge normals /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The edge normal direction that gives this separation +/// - `Vector`: The edge normal direction that gives this separation /// /// # 2D vs 3D /// @@ -182,16 +185,16 @@ pub fn cuboid_triangle_find_local_separating_edge_twoway( /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::{Triangle, Ball}; /// use parry2d::query::sat::triangle_support_map_find_local_separating_normal_oneway; -/// use nalgebra::{Point2, Isometry2}; +/// use parry2d::math::{Vector, Pose}; /// /// let triangle = Triangle::new( -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(1.0, 2.0) +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(1.0, 2.0) /// ); /// let sphere = Ball::new(0.5); /// -/// let pos12 = Isometry2::translation(3.0, 1.0); +/// let pos12 = Pose::translation(3.0, 1.0); /// /// let (separation, normal) = triangle_support_map_find_local_separating_normal_oneway( /// &triangle, @@ -208,18 +211,18 @@ pub fn cuboid_triangle_find_local_separating_edge_twoway( pub fn triangle_support_map_find_local_separating_normal_oneway( triangle1: &Triangle, shape2: &impl SupportMap, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { let mut best_sep = -Real::MAX; - let mut best_normal = Vector::zeros(); + let mut best_normal = Vector::ZERO; for edge in &triangle1.edges() { if let Some(normal) = edge.normal() { - let sep = support_map_support_map_compute_separation(triangle1, shape2, pos12, &normal); + let sep = support_map_support_map_compute_separation(triangle1, shape2, pos12, normal); if sep > best_sep { best_sep = sep; - best_normal = *normal; + best_normal = normal; } } } @@ -248,8 +251,8 @@ pub fn triangle_support_map_find_local_separating_normal_oneway( pub fn triangle_cuboid_find_local_separating_normal_oneway( triangle1: &Triangle, shape2: &Cuboid, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { triangle_support_map_find_local_separating_normal_oneway(triangle1, shape2, pos12) } @@ -278,7 +281,7 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway( /// - `Real`: The separation distance along the triangle's face normal /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The face normal direction (or its negation) that gives this separation +/// - `Vector`: The face normal direction (or its negation) that gives this separation /// /// # Example /// @@ -286,18 +289,18 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Triangle, Cuboid}; /// use parry3d::query::sat::triangle_cuboid_find_local_separating_normal_oneway; -/// use nalgebra::{Point3, Vector3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// /// // Horizontal triangle in the XY plane /// let triangle = Triangle::new( -/// Point3::origin(), -/// Point3::new(2.0, 0.0, 0.0), -/// Point3::new(1.0, 2.0, 0.0) +/// Vector::ZERO, +/// Vector::new(2.0, 0.0, 0.0), +/// Vector::new(1.0, 2.0, 0.0) /// ); -/// let cube = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// // Position cube above the triangle -/// let pos12 = Isometry3::translation(1.0, 1.0, 2.0); +/// let pos12 = Pose::translation(1.0, 1.0, 2.0); /// /// let (separation, normal) = triangle_cuboid_find_local_separating_normal_oneway( /// &triangle, @@ -318,8 +321,8 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway( pub fn triangle_cuboid_find_local_separating_normal_oneway( triangle1: &Triangle, shape2: &Cuboid, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { sat::point_cuboid_find_local_separating_normal_oneway( triangle1.a, triangle1.normal(), diff --git a/src/query/sat/sat_support_map_support_map.rs b/src/query/sat/sat_support_map_support_map.rs index d31d41c7..b1fcff89 100644 --- a/src/query/sat/sat_support_map_support_map.rs +++ b/src/query/sat/sat_support_map_support_map.rs @@ -1,6 +1,5 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::SupportMap; -use na::Unit; /// Computes the separation distance between two convex shapes along a given direction. /// @@ -41,21 +40,21 @@ use na::Unit; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, Cuboid}; /// use parry3d::query::sat::support_map_support_map_compute_separation; -/// use nalgebra::{Isometry3, Vector3, Unit}; +/// use parry3d::math::{Pose, Vector}; /// /// let sphere = Ball::new(1.0); -/// let cube = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cube = Cuboid::new(Vector::splat(1.0)); /// /// // Position cube to the right of the sphere -/// let pos12 = Isometry3::translation(3.0, 0.0, 0.0); +/// let pos12 = Pose::translation(3.0, 0.0, 0.0); /// /// // Test separation along the X axis -/// let dir = Unit::new_normalize(Vector3::x()); +/// let dir = Vector::X.normalize(); /// let separation = support_map_support_map_compute_separation( /// &sphere, /// &cube, /// &pos12, -/// &dir +/// dir /// ); /// /// // They should be separated (sphere radius 1.0 + cube extent 1.0 = 2.0, distance 3.0) @@ -80,10 +79,10 @@ use na::Unit; pub fn support_map_support_map_compute_separation( sm1: &impl SupportMap, sm2: &impl SupportMap, - pos12: &Isometry, - dir1: &Unit>, + pos12: &Pose, + dir1: Vector, ) -> Real { let p1 = sm1.local_support_point_toward(dir1); - let p2 = sm2.support_point_toward(pos12, &-*dir1); + let p2 = sm2.support_point_toward(pos12, -dir1); (p2 - p1).dot(dir1) } diff --git a/src/query/sat/sat_triangle_segment.rs b/src/query/sat/sat_triangle_segment.rs index 7b0ff09d..1e1b15f8 100644 --- a/src/query/sat/sat_triangle_segment.rs +++ b/src/query/sat/sat_triangle_segment.rs @@ -1,7 +1,6 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::sat; use crate::shape::{Segment, SupportMap, Triangle}; -use na::Unit; /// Finds the best separating axis by testing a triangle's face normal against a segment (3D only). /// @@ -26,7 +25,7 @@ use na::Unit; /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping /// - **Very negative** if the triangle has no normal (degenerate triangle) -/// - `Vector`: The face normal direction (or its negation) that gives this separation +/// - `Vector`: The face normal direction (or its negation) that gives this separation /// /// # Degenerate Triangles /// @@ -39,22 +38,22 @@ use na::Unit; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Triangle, Segment}; /// use parry3d::query::sat::triangle_segment_find_local_separating_normal_oneway; -/// use nalgebra::{Point3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// /// // Triangle in the XY plane /// let triangle = Triangle::new( -/// Point3::origin(), -/// Point3::new(2.0, 0.0, 0.0), -/// Point3::new(1.0, 2.0, 0.0) +/// Vector::ZERO, +/// Vector::new(2.0, 0.0, 0.0), +/// Vector::new(1.0, 2.0, 0.0) /// ); /// /// // Vertical segment above the triangle /// let segment = Segment::new( -/// Point3::new(1.0, 1.0, 1.0), -/// Point3::new(1.0, 1.0, 3.0) +/// Vector::new(1.0, 1.0, 1.0), +/// Vector::new(1.0, 1.0, 3.0) /// ); /// -/// let pos12 = Isometry3::identity(); +/// let pos12 = Pose::identity(); /// /// let (separation, normal) = triangle_segment_find_local_separating_normal_oneway( /// &triangle, @@ -77,21 +76,21 @@ use na::Unit; pub fn triangle_segment_find_local_separating_normal_oneway( triangle1: &Triangle, segment2: &Segment, - pos12: &Isometry, -) -> (Real, Vector) { + pos12: &Pose, +) -> (Real, Vector) { if let Some(dir) = triangle1.normal() { - let p2a = segment2.support_point_toward(pos12, &-dir); - let p2b = segment2.support_point_toward(pos12, &dir); - let sep_a = (p2a - triangle1.a).dot(&dir); - let sep_b = -(p2b - triangle1.a).dot(&dir); + let p2a = segment2.support_point_toward(pos12, -dir); + let p2b = segment2.support_point_toward(pos12, dir); + let sep_a = (p2a - triangle1.a).dot(dir); + let sep_b = -(p2b - triangle1.a).dot(dir); if sep_a >= sep_b { - (sep_a, *dir) + (sep_a, dir) } else { - (sep_b, -*dir) + (sep_b, -dir) } } else { - (-Real::MAX, Vector::zeros()) + (-Real::MAX, Vector::ZERO) } } @@ -113,7 +112,7 @@ pub fn triangle_segment_find_local_separating_normal_oneway( /// - `Real`: The maximum separation found across all edge-edge axes /// - **Positive**: Shapes are separated /// - **Negative**: Shapes are overlapping -/// - `Vector`: The axis direction that gives this separation +/// - `Vector`: The axis direction that gives this separation /// /// # The Axes Tested /// @@ -130,20 +129,20 @@ pub fn triangle_segment_find_local_separating_normal_oneway( /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Segment, Triangle}; /// use parry3d::query::sat::segment_triangle_find_local_separating_edge_twoway; -/// use nalgebra::{Point3, Isometry3}; +/// use parry3d::math::{Vector, Pose}; /// /// let segment = Segment::new( -/// Point3::origin(), -/// Point3::new(0.0, 0.0, 2.0) +/// Vector::ZERO, +/// Vector::new(0.0, 0.0, 2.0) /// ); /// /// let triangle = Triangle::new( -/// Point3::new(1.0, 0.0, 1.0), -/// Point3::new(3.0, 0.0, 1.0), -/// Point3::new(2.0, 2.0, 1.0) +/// Vector::new(1.0, 0.0, 1.0), +/// Vector::new(3.0, 0.0, 1.0), +/// Vector::new(2.0, 2.0, 1.0) /// ); /// -/// let pos12 = Isometry3::identity(); +/// let pos12 = Pose::identity(); /// /// let (separation, axis) = segment_triangle_find_local_separating_edge_twoway( /// &segment, @@ -173,14 +172,14 @@ pub fn triangle_segment_find_local_separating_normal_oneway( pub fn segment_triangle_find_local_separating_edge_twoway( segment1: &Segment, triangle2: &Triangle, - pos12: &Isometry, -) -> (Real, Vector) { - let x2 = pos12 * (triangle2.b - triangle2.a); - let y2 = pos12 * (triangle2.c - triangle2.b); - let z2 = pos12 * (triangle2.a - triangle2.c); + pos12: &Pose, +) -> (Real, Vector) { + let x2 = pos12.rotation * (triangle2.b - triangle2.a); + let y2 = pos12.rotation * (triangle2.c - triangle2.b); + let z2 = pos12.rotation * (triangle2.a - triangle2.c); let dir1 = segment1.scaled_direction(); - let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)]; + let crosses1 = [dir1.cross(x2), dir1.cross(y2), dir1.cross(z2)]; let axes1 = [ crosses1[0], crosses1[1], @@ -193,13 +192,13 @@ pub fn segment_triangle_find_local_separating_edge_twoway( let mut sep_dir = axes1[0]; for axis1 in &axes1 { - if let Some(axis1) = Unit::try_new(*axis1, 0.0) { + if let Some(axis1) = (*axis1).try_normalize() { let sep = - sat::support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1); + sat::support_map_support_map_compute_separation(segment1, triangle2, pos12, axis1); if sep > max_separation { max_separation = sep; - sep_dir = *axis1; + sep_dir = axis1; } } } diff --git a/src/query/shape_cast/shape_cast.rs b/src/query/shape_cast/shape_cast.rs index 7a2e23ef..bc2b6713 100644 --- a/src/query/shape_cast/shape_cast.rs +++ b/src/query/shape_cast/shape_cast.rs @@ -1,6 +1,4 @@ -use na::Unit; - -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; use crate::shape::Shape; @@ -40,25 +38,25 @@ pub struct ShapeCastHit { /// /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] /// and [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false`. - pub witness1: Point, + pub witness1: Vector, /// The local-space closest point on the second shape at the time of impact. /// /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false` /// when calling the time-of-impact function. - pub witness2: Point, + pub witness2: Vector, /// The local-space outward normal on the first shape at the time of impact. /// /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false` /// when calling the time-of-impact function. - pub normal1: Unit>, + pub normal1: Vector, /// The local-space outward normal on the second shape at the time of impact. /// /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false` /// when calling the time-of-impact function. - pub normal2: Unit>, + pub normal2: Vector, /// The way the shape-casting algorithm terminated. pub status: ShapeCastStatus, } @@ -80,12 +78,12 @@ impl ShapeCastHit { } /// Transform `self.witness1` and `self.normal1` by `pos`. - pub fn transform1_by(&self, pos: &Isometry) -> Self { + pub fn transform1_by(&self, pos: &Pose) -> Self { Self { time_of_impact: self.time_of_impact, witness1: pos * self.witness1, witness2: self.witness2, - normal1: pos * self.normal1, + normal1: pos.rotation * self.normal1, normal2: self.normal2, status: self.status, } @@ -153,7 +151,7 @@ impl Default for ShapeCastOptions { /// # What is Shape Casting? /// /// Shape casting extends ray casting to arbitrary shapes: -/// - **Ray casting**: Point moving in a direction (infinitely thin) +/// - **Ray casting**: Vector moving in a direction (infinitely thin) /// - **Shape casting**: Full shape moving in a direction (has volume) /// /// The shapes move linearly (no rotation) from their initial positions along their @@ -196,29 +194,29 @@ impl Default for ShapeCastOptions { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{cast_shapes, ShapeCastOptions}; /// use parry3d::shape::Ball; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball1 = Ball::new(1.0); /// let ball2 = Ball::new(1.0); /// /// // Ball 1 at origin, moving right at speed 2.0 -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let vel1 = Vector3::new(2.0, 0.0, 0.0); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let vel1 = Vector::new(2.0, 0.0, 0.0); /// /// // Ball 2 at x=10, stationary -/// let pos2 = Isometry3::translation(10.0, 0.0, 0.0); -/// let vel2 = Vector3::zeros(); +/// let pos2 = Pose::translation(10.0, 0.0, 0.0); +/// let vel2 = Vector::ZERO; /// /// let options = ShapeCastOptions::default(); /// -/// if let Ok(Some(hit)) = cast_shapes(&pos1, &vel1, &ball1, &pos2, &vel2, &ball2, options) { +/// if let Ok(Some(hit)) = cast_shapes(&pos1, vel1, &ball1, &pos2, vel2, &ball2, options) { /// // Time when surfaces touch /// // Distance to cover: 10.0 - 1.0 (radius) - 1.0 (radius) = 8.0 /// // Speed: 2.0, so time = 8.0 / 2.0 = 4.0 /// assert_eq!(hit.time_of_impact, 4.0); /// /// // Position at impact -/// let impact_pos1 = pos1.translation.vector + vel1 * hit.time_of_impact; +/// let impact_pos1 = pos1.translation + vel1 * hit.time_of_impact; /// // Ball 1 moved 8 units to x=8.0, touching ball 2 at x=10.0 /// } /// # } @@ -230,20 +228,20 @@ impl Default for ShapeCastOptions { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::query::{cast_shapes, ShapeCastOptions, ShapeCastStatus}; /// use parry3d::shape::Ball; -/// use nalgebra::{Isometry3, Vector3}; +/// use parry3d::math::{Pose, Vector}; /// /// let ball1 = Ball::new(2.0); /// let ball2 = Ball::new(2.0); /// /// // Overlapping balls (centers 3 units apart, radii sum to 4) -/// let pos1 = Isometry3::translation(0.0, 0.0, 0.0); -/// let pos2 = Isometry3::translation(3.0, 0.0, 0.0); -/// let vel1 = Vector3::x(); -/// let vel2 = Vector3::zeros(); +/// let pos1 = Pose::translation(0.0, 0.0, 0.0); +/// let pos2 = Pose::translation(3.0, 0.0, 0.0); +/// let vel1 = Vector::X; +/// let vel2 = Vector::ZERO; /// /// let options = ShapeCastOptions::default(); /// -/// if let Ok(Some(hit)) = cast_shapes(&pos1, &vel1, &ball1, &pos2, &vel2, &ball2, options) { +/// if let Ok(Some(hit)) = cast_shapes(&pos1, vel1, &ball1, &pos2, vel2, &ball2, options) { /// // Already penetrating /// assert_eq!(hit.time_of_impact, 0.0); /// assert_eq!(hit.status, ShapeCastStatus::PenetratingOrWithinTargetDist); @@ -272,15 +270,15 @@ impl Default for ShapeCastOptions { /// - [`ShapeCastOptions`] - Configuration options /// - [`ShapeCastHit`] - Result structure pub fn cast_shapes( - pos1: &Isometry, - vel1: &Vector, + pos1: &Pose, + vel1: Vector, g1: &dyn Shape, - pos2: &Isometry, - vel2: &Vector, + pos2: &Pose, + vel2: Vector, g2: &dyn Shape, options: ShapeCastOptions, ) -> Result, Unsupported> { let pos12 = pos1.inv_mul(pos2); - let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1)); - DefaultQueryDispatcher.cast_shapes(&pos12, &vel12, g1, g2, options) + let vel12 = pos1.rotation.inverse() * vel2 - vel1; + DefaultQueryDispatcher.cast_shapes(&pos12, vel12, g1, g2, options) } diff --git a/src/query/shape_cast/shape_cast_ball_ball.rs b/src/query/shape_cast/shape_cast_ball_ball.rs index 29d5204f..859c5c9e 100644 --- a/src/query/shape_cast/shape_cast_ball_ball.rs +++ b/src/query/shape_cast/shape_cast_ball_ball.rs @@ -1,6 +1,4 @@ -use na::Unit; - -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{self, Ray, ShapeCastHit, ShapeCastStatus}; use crate::shape::Ball; @@ -9,19 +7,19 @@ use num::Zero; /// Time Of Impact of two balls under translational movement. #[inline] pub fn cast_shapes_ball_ball( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, b1: &Ball, b2: &Ball, options: ShapeCastOptions, ) -> Option { let rsum = b1.radius + b2.radius + options.target_distance; let radius = rsum; - let center = Point::from(-pos12.translation.vector); - let ray = Ray::new(Point::origin(), *vel12); + let center = -pos12.translation; + let ray = Ray::new(Vector::ZERO, vel12); if let (inside, Some(time_of_impact)) = - query::details::ray_toi_with_ball(¢er, radius, &ray, true) + query::details::ray_toi_with_ball(center, radius, &ray, true) { if time_of_impact > options.max_time_of_impact { return None; @@ -34,22 +32,22 @@ pub fn cast_shapes_ball_ball( let witness2; if radius.is_zero() { - normal1 = Vector::x_axis(); - normal2 = pos12.inverse_transform_unit_vector(&(-Vector::x_axis())); - witness1 = Point::origin(); - witness2 = Point::origin(); + normal1 = Vector::X; + normal2 = pos12.rotation.inverse() * -Vector::X; + witness1 = Vector::ZERO; + witness2 = Vector::ZERO; } else { - normal1 = Unit::new_unchecked(dpt / radius); - normal2 = pos12.inverse_transform_unit_vector(&(-normal1)); - witness1 = Point::from(*normal1 * b1.radius); - witness2 = Point::from(*normal2 * b2.radius); + normal1 = dpt / radius; + normal2 = pos12.rotation.inverse() * -normal1; + witness1 = normal1 * b1.radius; + witness2 = normal2 * b2.radius; } if !options.stop_at_penetration && time_of_impact < 1.0e-5 && normal1.dot(vel12) >= 0.0 { return None; } - let status = if inside && center.coords.norm_squared() < rsum * rsum { + let status = if inside && center.length_squared() < rsum * rsum { ShapeCastStatus::PenetratingOrWithinTargetDist } else { ShapeCastStatus::Converged diff --git a/src/query/shape_cast/shape_cast_composite_shape_shape.rs b/src/query/shape_cast/shape_cast_composite_shape_shape.rs index ed8371f7..df37392a 100644 --- a/src/query/shape_cast/shape_cast_composite_shape_shape.rs +++ b/src/query/shape_cast/shape_cast_composite_shape_shape.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::partitioning::BvhNode; use crate::query::shape_cast::ShapeCastOptions; use crate::query::{QueryDispatcher, Ray, RayCast, ShapeCastHit}; @@ -14,15 +14,15 @@ impl CompositeShapeRef<'_, S> { pub fn cast_shape( &self, dispatcher: &D, - pose12: &Isometry, - vel12: &Vector, + pose12: &Pose, + vel12: Vector, g2: &dyn Shape, options: ShapeCastOptions, ) -> Option<(u32, ShapeCastHit)> { let ls_aabb2 = g2.compute_aabb(pose12); - let ray = Ray::new(Point::origin(), *vel12); - let msum_shift = -ls_aabb2.center().coords; - let msum_margin = ls_aabb2.half_extents() + Vector::repeat(options.target_distance); + let ray = Ray::new(Vector::ZERO, vel12); + let msum_shift = -ls_aabb2.center(); + let msum_margin = ls_aabb2.half_extents() + Vector::splat(options.target_distance); self.0.bvh().find_best( options.max_time_of_impact, @@ -44,7 +44,7 @@ impl CompositeShapeRef<'_, S> { dispatcher .cast_shapes( &part_pose1.inv_mul(pose12), - &part_pose1.inverse_transform_vector(vel12), + part_pose1.rotation.inverse() * vel12, part_g1, g2, options, @@ -65,8 +65,8 @@ impl CompositeShapeRef<'_, S> { /// Time Of Impact of a composite shape with any other shape, under translational movement. pub fn cast_shapes_composite_shape_shape( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &G1, g2: &dyn Shape, options: ShapeCastOptions, @@ -83,8 +83,8 @@ where /// Time Of Impact of any shape with a composite shape, under translational movement. pub fn cast_shapes_shape_composite_shape( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &dyn Shape, g2: &G2, options: ShapeCastOptions, @@ -96,7 +96,7 @@ where cast_shapes_composite_shape_shape( dispatcher, &pos12.inverse(), - &-pos12.inverse_transform_vector(vel12), + -(pos12.rotation.inverse() * vel12), g2, g1, options, diff --git a/src/query/shape_cast/shape_cast_halfspace_support_map.rs b/src/query/shape_cast/shape_cast_halfspace_support_map.rs index bbc4b0ce..42f94925 100644 --- a/src/query/shape_cast/shape_cast_halfspace_support_map.rs +++ b/src/query/shape_cast/shape_cast_halfspace_support_map.rs @@ -1,19 +1,19 @@ -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{Ray, RayCast, ShapeCastHit, ShapeCastStatus}; use crate::shape::{HalfSpace, RoundShapeRef, SupportMap}; /// Time Of Impact of a halfspace with a support-mapped shape under translational movement. pub fn cast_shapes_halfspace_support_map( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, halfspace: &HalfSpace, other: &G, options: ShapeCastOptions, ) -> Option { // TODO: add method to get only the local support point. // This would avoid the `inverse_transform_point` later. - if !options.stop_at_penetration && vel12.dot(&halfspace.normal) > 0.0 { + if !options.stop_at_penetration && vel12.dot(halfspace.normal) > 0.0 { return None; } @@ -22,25 +22,25 @@ pub fn cast_shapes_halfspace_support_map( inner_shape: other, border_radius: options.target_distance, }; - round_other.support_point(pos12, &-halfspace.normal) + round_other.support_point(pos12, -halfspace.normal) } else { - other.support_point(pos12, &-halfspace.normal) + other.support_point(pos12, -halfspace.normal) }; let closest_point = support_point; - let ray = Ray::new(closest_point, *vel12); + let ray = Ray::new(closest_point, vel12); if let Some(time_of_impact) = halfspace.cast_local_ray(&ray, options.max_time_of_impact, true) { if time_of_impact > options.max_time_of_impact { return None; } - let witness2 = support_point + *halfspace.normal * options.target_distance; + let witness2 = support_point + halfspace.normal * options.target_distance; let mut witness1 = ray.point_at(time_of_impact); // Project the witness point to the halfspace. // Note that witness1 is already in the halfspace's local-space. - witness1 -= *halfspace.normal * witness1.coords.dot(&halfspace.normal); + witness1 -= halfspace.normal * witness1.dot(halfspace.normal); - let status = if support_point.coords.dot(&halfspace.normal) < 0.0 { + let status = if support_point.dot(halfspace.normal) < 0.0 { ShapeCastStatus::PenetratingOrWithinTargetDist } else { ShapeCastStatus::Converged @@ -49,9 +49,9 @@ pub fn cast_shapes_halfspace_support_map( Some(ShapeCastHit { time_of_impact, normal1: halfspace.normal, - normal2: pos12.inverse_transform_unit_vector(&-halfspace.normal), + normal2: pos12.rotation.inverse() * -halfspace.normal, witness1, - witness2: pos12.inverse_transform_point(&witness2), + witness2: pos12.inverse_transform_point(witness2), status, }) } else { @@ -61,15 +61,15 @@ pub fn cast_shapes_halfspace_support_map( /// Time Of Impact of a halfspace with a support-mapped shape under translational movement. pub fn cast_shapes_support_map_halfspace( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, other: &G, halfspace: &HalfSpace, options: ShapeCastOptions, ) -> Option { cast_shapes_halfspace_support_map( &pos12.inverse(), - &-pos12.inverse_transform_vector(vel12), + -(pos12.rotation.inverse() * vel12), halfspace, other, options, diff --git a/src/query/shape_cast/shape_cast_heightfield_shape.rs b/src/query/shape_cast/shape_cast_heightfield_shape.rs index 0f329f4e..83182034 100644 --- a/src/query/shape_cast/shape_cast_heightfield_shape.rs +++ b/src/query/shape_cast/shape_cast_heightfield_shape.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{QueryDispatcher, Ray, ShapeCastHit, Unsupported}; use crate::shape::{HeightField, Shape}; @@ -10,14 +10,14 @@ use crate::{bounding_volume::Aabb, query::RayCast}; #[cfg(feature = "dim2")] pub fn cast_shapes_heightfield_shape( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, heightfield1: &HeightField, g2: &dyn Shape, options: ShapeCastOptions, ) -> Result, Unsupported> { let aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance); - let ray = Ray::new(aabb2_1.center(), *vel12); + let ray = Ray::new(aabb2_1.center(), vel12); let mut curr_range = heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1); // Enlarge the range by 1 to account for movement within a cell. @@ -68,13 +68,9 @@ pub fn cast_shapes_heightfield_shape( if right { curr_elt += 1; - curr_param = (cell_width * na::convert::(curr_elt as f64) + start_x - - ray.origin.x) - / ray.dir.x; + curr_param = (cell_width * (curr_elt as Real) + start_x - ray.origin.x) / ray.dir.x; } else { - curr_param = - (ray.origin.x - cell_width * na::convert::(curr_elt as f64) - start_x) - / ray.dir.x; + curr_param = (ray.origin.x - cell_width * (curr_elt as Real) - start_x) / ray.dir.x; curr_elt -= 1; } @@ -99,15 +95,15 @@ pub fn cast_shapes_heightfield_shape( #[cfg(feature = "dim3")] pub fn cast_shapes_heightfield_shape( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, heightfield1: &HeightField, g2: &dyn Shape, options: ShapeCastOptions, ) -> Result, Unsupported> { let aabb1 = heightfield1.local_aabb(); let mut aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance); - let ray = Ray::new(aabb2_1.center(), *vel12); + let ray = Ray::new(aabb2_1.center(), vel12); // Find the first hit between the aabbs. let hext2_1 = aabb2_1.half_extents(); @@ -175,7 +171,7 @@ pub fn cast_shapes_heightfield_shape( return Ok(best_hit); } - let mut cell = heightfield1.unclamped_cell_at_point(&aabb2_1.center()); + let mut cell = heightfield1.unclamped_cell_at_point(aabb2_1.center()); loop { let prev_cell = cell; @@ -263,8 +259,8 @@ pub fn cast_shapes_heightfield_shape( /// Time Of Impact between a moving shape and a heightfield. pub fn cast_shapes_shape_heightfield( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &dyn Shape, heightfield2: &HeightField, options: ShapeCastOptions, @@ -272,7 +268,7 @@ pub fn cast_shapes_shape_heightfield( Ok(cast_shapes_heightfield_shape( dispatcher, &pos12.inverse(), - &-pos12.inverse_transform_vector(vel12), + -(pos12.rotation.inverse() * vel12), heightfield2, g1, options, diff --git a/src/query/shape_cast/shape_cast_support_map_support_map.rs b/src/query/shape_cast/shape_cast_support_map_support_map.rs index 7df76da1..92eba915 100644 --- a/src/query/shape_cast/shape_cast_support_map_support_map.rs +++ b/src/query/shape_cast/shape_cast_support_map_support_map.rs @@ -1,6 +1,4 @@ -use na::Unit; - -use crate::math::{Isometry, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::query::details; use crate::query::details::ShapeCastOptions; use crate::query::gjk::{self, VoronoiSimplex}; @@ -10,8 +8,8 @@ use num::Zero; /// Time of impacts between two support-mapped shapes under translational movement. pub fn cast_shapes_support_map_support_map( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &G1, g2: &G2, options: ShapeCastOptions, @@ -54,10 +52,10 @@ where } else { Some(ShapeCastHit { time_of_impact, - normal1: Unit::new_unchecked(normal1), - normal2: Unit::new_unchecked(pos12.inverse_transform_vector(&-normal1)), + normal1, + normal2: pos12.rotation.inverse() * -normal1, witness1: witness1 - normal1 * options.target_distance, - witness2: pos12.inverse_transform_point(&witness2), + witness2: pos12.inverse_transform_point(witness2), status: if time_of_impact.is_zero() { ShapeCastStatus::PenetratingOrWithinTargetDist } else { diff --git a/src/query/shape_cast/shape_cast_voxels_shape.rs b/src/query/shape_cast/shape_cast_voxels_shape.rs index bcb5223d..41d7b245 100644 --- a/src/query/shape_cast/shape_cast_voxels_shape.rs +++ b/src/query/shape_cast/shape_cast_voxels_shape.rs @@ -1,12 +1,12 @@ -use crate::math::{Isometry, Point, Real, Translation, Vector}; +use crate::math::{IVector, Pose, Real, Vector}; use crate::query::{QueryDispatcher, ShapeCastHit, ShapeCastOptions}; use crate::shape::{Cuboid, Shape, Voxels}; /// Time Of Impact of a voxels shape with any other shape, under a translational movement. pub fn cast_shapes_voxels_shape( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &Voxels, g2: &dyn Shape, options: ShapeCastOptions, @@ -20,13 +20,13 @@ where let mut smallest_t = options.max_time_of_impact; let start_aabb2_1 = g2.compute_aabb(pos12); - let mut check_voxels_in_range = |search_domain: [Point; 2]| { + let mut check_voxels_in_range = |search_domain: [IVector; 2]| { for vox in g1.voxels_in_range(search_domain[0], search_domain[1]) { if !vox.state.is_empty() { // PERF: could we check the canonical shape instead, and deduplicate accordingly? let center = g1.voxel_center(vox.grid_coords); let cuboid = Cuboid::new(g1.voxel_size() / 2.0); - let vox_pos12 = Translation::from(center).inverse() * pos12; + let vox_pos12 = Pose::from_translation(center).inverse() * pos12; if let Some(new_hit) = dispatcher .cast_shapes(&vox_pos12, vel12, &cuboid, g2, options) .ok() @@ -89,7 +89,7 @@ where break; } - let imin = Vector::from(toi.map(|t| t.0)).imin(); + let imin = Vector::from(toi.map(|t| t.0)).min_position(); if toi[imin].1 { search_domain[0][imin] += 1; @@ -130,8 +130,8 @@ where /// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation). pub fn cast_shapes_shape_voxels( dispatcher: &D, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &dyn Shape, g2: &Voxels, options: ShapeCastOptions, @@ -142,7 +142,7 @@ where cast_shapes_voxels_shape( dispatcher, &pos12.inverse(), - &-pos12.inverse_transform_vector(vel12), + -(pos12.rotation.inverse() * vel12), g2, g1, options, diff --git a/src/query/split/mod.rs b/src/query/split/mod.rs index 01867b72..91a930e3 100644 --- a/src/query/split/mod.rs +++ b/src/query/split/mod.rs @@ -18,9 +18,9 @@ //! a plane. The plane acts as a "cutting tool" that partitions the shape based on which side //! of the plane each part lies on: //! -//! - **Negative half-space**: Points where the dot product with the plane normal is less than the bias -//! - **Positive half-space**: Points where the dot product with the plane normal is greater than the bias -//! - **On the plane**: Points that lie exactly on the plane (within an epsilon tolerance) +//! - **Negative half-space**: Vectors where the dot product with the plane normal is less than the bias +//! - **Positive half-space**: Vectors where the dot product with the plane normal is greater than the bias +//! - **On the plane**: Vectors that lie exactly on the plane (within an epsilon tolerance) //! //! ## Plane Definition //! @@ -45,11 +45,11 @@ //! ```rust //! # #[cfg(all(feature = "dim3", feature = "f32"))] { //! use parry3d::bounding_volume::Aabb; -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use parry3d::query::SplitResult; //! //! // Split an AABB along the X-axis at x = 5.0 -//! let aabb = Aabb::new(Point::new(0.0, 0.0, 0.0), Point::new(10.0, 10.0, 10.0)); +//! let aabb = Aabb::new(Vector::new(0.0, 0.0, 0.0), Vector::new(10.0, 10.0, 10.0)); //! match aabb.canonical_split(0, 5.0, 1e-6) { //! SplitResult::Pair(left, right) => { //! // AABB was split into two pieces @@ -75,14 +75,13 @@ //! # #[cfg(all(feature = "dim3", feature = "spade", feature = "f32"))] //! # { //! use parry3d::shape::TriMesh; -//! use parry3d::math::{Point, Vector}; +//! use parry3d::math::Vector; //! use parry3d::query::SplitResult; -//! use parry3d::na::Unit; //! //! # let vertices = vec![ -//! # Point::new(0.0, 0.0, 0.0), -//! # Point::new(1.0, 0.0, 0.0), -//! # Point::new(0.0, 1.0, 0.0), +//! # Vector::new(0.0, 0.0, 0.0), +//! # Vector::new(1.0, 0.0, 0.0), +//! # Vector::new(0.0, 1.0, 0.0), //! # ]; //! # let indices = vec![[0u32, 1, 2]]; //! # let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -116,10 +115,10 @@ // //! use parry3d::query::IntersectResult; // //! // //! # let vertices = vec![ -// //! # parry3d::na::Point3::origin(), -// //! # parry3d::na::Point3::new(1.0, 0.0, 0.0), -// //! # parry3d::na::Point3::new(0.5, 1.0, 0.5), -// //! # parry3d::na::Point3::new(0.5, 0.0, 1.0), +// //! # parry3d::math::Vector::ZERO, +// //! # parry3d::math::Vector::new(1.0, 0.0, 0.0), +// //! # parry3d::math::Vector::new(0.5, 1.0, 0.5), +// //! # parry3d::math::Vector::new(0.5, 0.0, 1.0), // //! # ]; // //! # let indices = vec![[0u32, 1, 2], [0, 1, 3]]; // //! # let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -161,7 +160,7 @@ //! # Epsilon Tolerance //! //! All splitting operations accept an `epsilon` parameter to handle floating-point precision -//! issues. Points within `epsilon` distance of the plane are considered to lie on the plane. +//! issues. Vectors within `epsilon` distance of the plane are considered to lie on the plane. //! A typical value is `1e-6` for single precision (`f32`) or `1e-10` for double precision (`f64`). //! //! Choosing an appropriate epsilon is important: diff --git a/src/query/split/split.rs b/src/query/split/split.rs index a4c340f7..2e2e9e0f 100644 --- a/src/query/split/split.rs +++ b/src/query/split/split.rs @@ -25,10 +25,10 @@ /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::bounding_volume::Aabb; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// use parry3d::query::SplitResult; /// -/// let aabb = Aabb::new(Point::new(0.0, 0.0, 0.0), Point::new(10.0, 10.0, 10.0)); +/// let aabb = Aabb::new(Vector::new(0.0, 0.0, 0.0), Vector::new(10.0, 10.0, 10.0)); /// /// // Split along X-axis at x = 5.0 /// match aabb.canonical_split(0, 5.0, 1e-6) { @@ -52,10 +52,10 @@ /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// use parry3d::query::SplitResult; /// -/// let segment = Segment::new(Point::new(0.0, 0.0, 0.0), Point::new(10.0, 0.0, 0.0)); +/// let segment = Segment::new(Vector::new(0.0, 0.0, 0.0), Vector::new(10.0, 0.0, 0.0)); /// /// // Split along X-axis at x = 3.0 /// match segment.canonical_split(0, 3.0, 1e-6) { @@ -124,15 +124,15 @@ pub enum SplitResult { /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32", feature = "spade"))] { /// use parry3d::shape::TriMesh; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// use parry3d::query::IntersectResult; /// /// // Create a simple tetrahedron mesh /// let vertices = vec![ -/// Point::new(0.0, 0.0, 0.0), -/// Point::new(1.0, 0.0, 0.0), -/// Point::new(0.5, 1.0, 0.0), -/// Point::new(0.5, 0.5, 1.0), +/// Vector::new(0.0, 0.0, 0.0), +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.5, 1.0, 0.0), +/// Vector::new(0.5, 0.5, 1.0), /// ]; /// let indices = vec![ /// [0u32, 1, 2], // Bottom face @@ -169,9 +169,9 @@ pub enum SplitResult { /// use parry3d::query::IntersectResult; /// /// # let vertices = vec![ -/// # nalgebra::Point3::origin(), -/// # nalgebra::Point3::new(1.0, 0.0, 0.0), -/// # nalgebra::Point3::new(0.5, 1.0, 0.5), +/// # parry3d::math::Vector::ZERO, +/// # parry3d::math::Vector::new(1.0, 0.0, 0.0), +/// # parry3d::math::Vector::new(0.5, 1.0, 0.5), /// # ]; /// # let indices = vec![[0u32, 1, 2]]; /// # let mesh = TriMesh::new(vertices, indices).unwrap(); diff --git a/src/query/split/split_segment.rs b/src/query/split/split_segment.rs index 6d2cb47a..9d83afe0 100644 --- a/src/query/split/split_segment.rs +++ b/src/query/split/split_segment.rs @@ -1,4 +1,4 @@ -use crate::math::{Point, Real, UnitVector, Vector}; +use crate::math::{Real, Vector, VectorExt}; use crate::query::SplitResult; use crate::shape::Segment; @@ -15,17 +15,12 @@ impl Segment { /// positive half-space delimited by the splitting plane. pub fn canonical_split(&self, axis: usize, bias: Real, epsilon: Real) -> SplitResult { // TODO: optimize this. - self.local_split(&Vector::ith_axis(axis), bias, epsilon) + self.local_split(Vector::ith(axis, 1.0), bias, epsilon) } /// Splits this segment by a plane identified by its normal `local_axis` and /// the `bias` (i.e. the plane passes through the point equal to `normal * bias`). - pub fn local_split( - &self, - local_axis: &UnitVector, - bias: Real, - epsilon: Real, - ) -> SplitResult { + pub fn local_split(&self, local_axis: Vector, bias: Real, epsilon: Real) -> SplitResult { self.local_split_and_get_intersection(local_axis, bias, epsilon) .0 } @@ -38,15 +33,15 @@ impl Segment { /// parallel or near-parallel to the segment. pub fn local_split_and_get_intersection( &self, - local_axis: &UnitVector, + local_axis: Vector, bias: Real, epsilon: Real, - ) -> (SplitResult, Option<(Point, Real)>) { + ) -> (SplitResult, Option<(Vector, Real)>) { let dir = self.b - self.a; - let a = bias - local_axis.dot(&self.a.coords); - let b = local_axis.dot(&dir); + let a = bias - local_axis.dot(self.a); + let b = local_axis.dot(dir); let bcoord = a / b; - let dir_norm = dir.norm(); + let dir_norm = dir.length(); if relative_eq!(b, 0.0) || bcoord * dir_norm <= epsilon diff --git a/src/query/split/split_trimesh.rs b/src/query/split/split_trimesh.rs index 3adeb603..bc32042e 100644 --- a/src/query/split/split_trimesh.rs +++ b/src/query/split/split_trimesh.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, UnitVector, Vector}; +use crate::math::{Pose, Real, Vector, VectorExt}; use crate::query::{IntersectResult, PointQuery, SplitResult}; use crate::shape::{Cuboid, FeatureId, Polyline, Segment, Shape, TriMesh, TriMeshFlags, Triangle}; use crate::transformation::{intersect_meshes, MeshIntersectionError}; @@ -10,14 +10,14 @@ use spade::{handles::FixedVertexHandle, ConstrainedDelaunayTriangulation, Triang struct Triangulation { delaunay: ConstrainedDelaunayTriangulation>, - basis: [Vector; 2], - basis_origin: Point, + basis: [Vector; 2], + basis_origin: Vector, spade2index: HashMap, index2spade: HashMap, } impl Triangulation { - fn new(axis: UnitVector, basis_origin: Point) -> Self { + fn new(axis: Vector, basis_origin: Vector) -> Self { Triangulation { delaunay: ConstrainedDelaunayTriangulation::new(), basis: axis.orthonormal_basis(), @@ -27,12 +27,12 @@ impl Triangulation { } } - fn project(&self, pt: Point) -> spade::Point2 { + fn project(&self, pt: Vector) -> spade::Point2 { let dpt = pt - self.basis_origin; - spade::Point2::new(dpt.dot(&self.basis[0]), dpt.dot(&self.basis[1])) + spade::Point2::new(dpt.dot(self.basis[0]), dpt.dot(self.basis[1])) } - fn add_edge(&mut self, id1: u32, id2: u32, points: &[Point]) { + fn add_edge(&mut self, id1: u32, id2: u32, points: &[Vector]) { let proj1 = self.project(points[id1 as usize]); let proj2 = self.project(points[id2 as usize]); @@ -68,33 +68,28 @@ impl TriMesh { /// positive half-space delimited by the splitting plane. pub fn canonical_split(&self, axis: usize, bias: Real, epsilon: Real) -> SplitResult { // TODO: optimize this. - self.local_split(&Vector::ith_axis(axis), bias, epsilon) + self.local_split(Vector::ith(axis, 1.0), bias, epsilon) } /// Splits this mesh, transformed by `position` by a plane identified by its normal `local_axis` /// and the `bias` (i.e. the plane passes through the point equal to `normal * bias`). pub fn split( &self, - position: &Isometry, - axis: &UnitVector, + position: &Pose, + axis: Vector, bias: Real, epsilon: Real, ) -> SplitResult { - let local_axis = position.inverse_transform_unit_vector(axis); - let added_bias = -position.translation.vector.dot(axis); - self.local_split(&local_axis, bias + added_bias, epsilon) + let local_axis = position.rotation.inverse() * axis; + let added_bias = -position.translation.dot(axis); + self.local_split(local_axis, bias + added_bias, epsilon) } /// Splits this mesh by a plane identified by its normal `local_axis` /// and the `bias` (i.e. the plane passes through the point equal to `normal * bias`). - pub fn local_split( - &self, - local_axis: &UnitVector, - bias: Real, - epsilon: Real, - ) -> SplitResult { + pub fn local_split(&self, local_axis: Vector, bias: Real, epsilon: Real) -> SplitResult { let mut triangulation = if self.pseudo_normals_if_oriented().is_some() { - Some(Triangulation::new(*local_axis, self.vertices()[0])) + Some(Triangulation::new(local_axis, self.vertices()[0])) } else { None }; @@ -110,7 +105,7 @@ impl TriMesh { let mut found_negative = false; let mut found_positive = false; for (i, pt) in vertices.iter().enumerate() { - let dist_to_plane = pt.coords.dot(local_axis) - bias; + let dist_to_plane = pt.dot(local_axis) - bias; if dist_to_plane < -epsilon { found_negative = true; colors[i] = 1; @@ -322,7 +317,7 @@ impl TriMesh { vertices_lhs[idx1[2] as usize], ); - if self.contains_local_point(&tri.center()) { + if self.contains_local_point(tri.center()) { indices_lhs.push(idx1); idx2.swap(1, 2); // Flip orientation for the second half of the split. @@ -359,7 +354,7 @@ impl TriMesh { bias: Real, epsilon: Real, ) -> IntersectResult { - self.intersection_with_local_plane(&Vector::ith_axis(axis), bias, epsilon) + self.intersection_with_local_plane(Vector::ith(axis, 1.0), bias, epsilon) } /// Computes the intersection [`Polyline`]s between this mesh, transformed by `position`, @@ -367,14 +362,14 @@ impl TriMesh { /// (i.e. the plane passes through the point equal to `normal * bias`). pub fn intersection_with_plane( &self, - position: &Isometry, - axis: &UnitVector, + position: &Pose, + axis: Vector, bias: Real, epsilon: Real, ) -> IntersectResult { - let local_axis = position.inverse_transform_unit_vector(axis); - let added_bias = -position.translation.vector.dot(axis); - self.intersection_with_local_plane(&local_axis, bias + added_bias, epsilon) + let local_axis = position.rotation.inverse() * axis; + let added_bias = -position.translation.dot(axis); + self.intersection_with_local_plane(local_axis, bias + added_bias, epsilon) } /// Computes the intersection [`Polyline`]s between this mesh @@ -382,7 +377,7 @@ impl TriMesh { /// and the `bias` (i.e. the plane passes through the point equal to `normal * bias`). pub fn intersection_with_local_plane( &self, - local_axis: &UnitVector, + local_axis: Vector, bias: Real, epsilon: Real, ) -> IntersectResult { @@ -397,7 +392,7 @@ impl TriMesh { let mut found_negative = false; let mut found_positive = false; for (i, pt) in vertices.iter().enumerate() { - let dist_to_plane = pt.coords.dot(local_axis) - bias; + let dist_to_plane = pt.dot(local_axis) - bias; if dist_to_plane < -epsilon { found_negative = true; colors[i] = 1; @@ -607,14 +602,14 @@ impl TriMesh { /// Computes the intersection mesh between an Aabb and this mesh. pub fn intersection_with_aabb( &self, - position: &Isometry, + position: &Pose, flip_mesh: bool, aabb: &Aabb, flip_cuboid: bool, epsilon: Real, ) -> Result, MeshIntersectionError> { let cuboid = Cuboid::new(aabb.half_extents()); - let cuboid_pos = Isometry::from(aabb.center()); + let cuboid_pos = Pose::from_translation(aabb.center()); self.intersection_with_cuboid( position, flip_mesh, @@ -628,10 +623,10 @@ impl TriMesh { /// Computes the intersection mesh between a cuboid and this mesh transformed by `position`. pub fn intersection_with_cuboid( &self, - position: &Isometry, + position: &Pose, flip_mesh: bool, cuboid: &Cuboid, - cuboid_position: &Isometry, + cuboid_position: &Pose, flip_cuboid: bool, epsilon: Real, ) -> Result, MeshIntersectionError> { @@ -649,7 +644,7 @@ impl TriMesh { &self, flip_mesh: bool, cuboid: &Cuboid, - cuboid_position: &Isometry, + cuboid_position: &Pose, flip_cuboid: bool, _epsilon: Real, ) -> Result, MeshIntersectionError> { @@ -664,7 +659,7 @@ impl TriMesh { .unwrap(); let intersect_meshes = intersect_meshes( - &Isometry::identity(), + &Pose::IDENTITY, self, flip_mesh, cuboid_position, @@ -704,9 +699,9 @@ impl TriMesh { // There is no need to clip if the triangle is fully inside of the Aabb. // Note that we can’t take a shortcut for the case where all the vertices are // outside of the Aabb, because the Aabb can still instersect the edges or face. - if !(aabb.contains_local_point(&to_clip[0]) - && aabb.contains_local_point(&to_clip[1]) - && aabb.contains_local_point(&to_clip[2])) + if !(aabb.contains_local_point(to_clip[0]) + && aabb.contains_local_point(to_clip[1]) + && aabb.contains_local_point(to_clip[2])) { aabb.clip_polygon_with_workspace(&mut to_clip, &mut clip_workspace); } diff --git a/src/shape/ball.rs b/src/shape/ball.rs index f2defc35..1eefb78c 100644 --- a/src/shape/ball.rs +++ b/src/shape/ball.rs @@ -1,7 +1,4 @@ -use either::Either; -use na::Unit; - -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::SupportMap; /// A ball shape, also known as a sphere in 3D or a circle in 2D. @@ -30,7 +27,7 @@ use crate::shape::SupportMap; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Ball; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// /// // Create a ball with radius 2.0 /// let ball = Ball::new(2.0); @@ -42,8 +39,7 @@ use crate::shape::SupportMap; #[cfg_attr(feature = "encase", derive(encase::ShaderType))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] @@ -104,40 +100,40 @@ impl Ball { /// ``` /// # #[cfg(all(feature = "dim2", feature = "alloc", feature = "f32"))] { /// use parry2d::shape::Ball; - /// use parry2d::na::Vector2; + /// use parry2d::math::Vector; /// use either::Either; /// /// let ball = Ball::new(2.0); /// /// // Uniform scaling: produces another ball - /// let uniform_scale = Vector2::new(3.0, 3.0); - /// if let Some(Either::Left(scaled_ball)) = ball.scaled(&uniform_scale, 32) { + /// let uniform_scale = Vector::new(3.0, 3.0); + /// if let Some(Either::Left(scaled_ball)) = ball.scaled(uniform_scale, 32) { /// assert_eq!(scaled_ball.radius, 6.0); // 2.0 * 3.0 /// } /// /// // Non-uniform scaling: produces a polygon (ellipse approximation) - /// let non_uniform_scale = Vector2::new(2.0, 1.0); - /// if let Some(Either::Right(polygon)) = ball.scaled(&non_uniform_scale, 32) { + /// let non_uniform_scale = Vector::new(2.0, 1.0); + /// if let Some(Either::Right(polygon)) = ball.scaled(non_uniform_scale, 32) { /// // The polygon approximates an ellipse with radii 4.0 and 2.0 /// assert!(polygon.points().len() >= 32); /// } /// # } /// # #[cfg(all(feature = "dim2", feature = "alloc", feature = "f64"))] { /// use parry2d_f64::shape::Ball; - /// use parry2d_f64::na::Vector2; + /// use parry2d_f64::math::Vector; /// use either::Either; /// /// let ball = Ball::new(2.0); /// /// // Uniform scaling: produces another ball - /// let uniform_scale = Vector2::new(3.0, 3.0); - /// if let Some(Either::Left(scaled_ball)) = ball.scaled(&uniform_scale, 32) { + /// let uniform_scale = Vector::new(3.0, 3.0); + /// if let Some(Either::Left(scaled_ball)) = ball.scaled(uniform_scale, 32) { /// assert_eq!(scaled_ball.radius, 6.0); // 2.0 * 3.0 /// } /// /// // Non-uniform scaling: produces a polygon (ellipse approximation) - /// let non_uniform_scale = Vector2::new(2.0, 1.0); - /// if let Some(Either::Right(polygon)) = ball.scaled(&non_uniform_scale, 32) { + /// let non_uniform_scale = Vector::new(2.0, 1.0); + /// if let Some(Either::Right(polygon)) = ball.scaled(non_uniform_scale, 32) { /// // The polygon approximates an ellipse with radii 4.0 and 2.0 /// assert!(polygon.points().len() >= 32); /// } @@ -147,20 +143,21 @@ impl Ball { #[inline] pub fn scaled( self, - scale: &Vector, + scale: Vector, nsubdivs: u32, - ) -> Option> { + ) -> Option> { if scale.x != scale.y { - // The scaled shape isn’t a ball. + // The scaled shape isn't a ball. let mut vtx = self.to_polyline(nsubdivs); - vtx.iter_mut() - .for_each(|pt| pt.coords = pt.coords.component_mul(scale)); - Some(Either::Right(super::ConvexPolygon::from_convex_polyline( - vtx, - )?)) + vtx.iter_mut().for_each(|pt| *pt *= scale); + Some(either::Either::Right( + super::ConvexPolygon::from_convex_polyline(vtx)?, + )) } else { let uniform_scale = scale.x; - Some(Either::Left(Self::new(self.radius * uniform_scale.abs()))) + Some(either::Either::Left(Self::new( + self.radius * uniform_scale.abs(), + ))) } } @@ -186,20 +183,20 @@ impl Ball { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32", feature = "alloc"))] { /// use parry3d::shape::Ball; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// use either::Either; /// /// let ball = Ball::new(5.0); /// /// // Uniform scaling: produces another ball - /// let uniform_scale = Vector3::new(2.0, 2.0, 2.0); - /// if let Some(Either::Left(scaled_ball)) = ball.scaled(&uniform_scale, 10) { + /// let uniform_scale = Vector::new(2.0, 2.0, 2.0); + /// if let Some(Either::Left(scaled_ball)) = ball.scaled(uniform_scale, 10) { /// assert_eq!(scaled_ball.radius, 10.0); // 5.0 * 2.0 /// } /// /// // Non-uniform scaling: produces a polyhedron (ellipsoid approximation) - /// let non_uniform_scale = Vector3::new(2.0, 1.0, 1.5); - /// if let Some(Either::Right(polyhedron)) = ball.scaled(&non_uniform_scale, 10) { + /// let non_uniform_scale = Vector::new(2.0, 1.0, 1.5); + /// if let Some(Either::Right(polyhedron)) = ball.scaled(non_uniform_scale, 10) { /// // The polyhedron approximates an ellipsoid /// assert!(polyhedron.points().len() > 0); /// } @@ -209,42 +206,43 @@ impl Ball { #[inline] pub fn scaled( self, - scale: &Vector, + scale: Vector, nsubdivs: u32, - ) -> Option> { + ) -> Option> { if scale.x != scale.y || scale.x != scale.z || scale.y != scale.z { - // The scaled shape isn’t a ball. + // The scaled shape isn't a ball. let (mut vtx, idx) = self.to_trimesh(nsubdivs, nsubdivs); - vtx.iter_mut() - .for_each(|pt| pt.coords = pt.coords.component_mul(scale)); - Some(Either::Right(super::ConvexPolyhedron::from_convex_mesh( - vtx, &idx, - )?)) + vtx.iter_mut().for_each(|pt| *pt *= scale); + Some(either::Either::Right( + super::ConvexPolyhedron::from_convex_mesh(vtx, &idx)?, + )) } else { let uniform_scale = scale.x; - Some(Either::Left(Self::new(self.radius * uniform_scale.abs()))) + Some(either::Either::Left(Self::new( + self.radius * uniform_scale.abs(), + ))) } } } impl SupportMap for Ball { #[inline] - fn support_point(&self, m: &Isometry, dir: &Vector) -> Point { - self.support_point_toward(m, &Unit::new_normalize(*dir)) + fn support_point(&self, m: &Pose, dir: Vector) -> Vector { + self.support_point_toward(m, dir.normalize()) } #[inline] - fn support_point_toward(&self, m: &Isometry, dir: &Unit>) -> Point { - Point::from(m.translation.vector) + **dir * self.radius + fn support_point_toward(&self, m: &Pose, dir: Vector) -> Vector { + m.translation + dir * self.radius } #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { - self.local_support_point_toward(&Unit::new_normalize(*dir)) + fn local_support_point(&self, dir: Vector) -> Vector { + self.local_support_point_toward(dir.normalize()) } #[inline] - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - Point::from(**dir * self.radius) + fn local_support_point_toward(&self, dir: Vector) -> Vector { + dir * self.radius } } diff --git a/src/shape/capsule.rs b/src/shape/capsule.rs index 1d245a5c..7627a1e6 100644 --- a/src/shape/capsule.rs +++ b/src/shape/capsule.rs @@ -1,21 +1,16 @@ -use crate::math::{Isometry, Point, Real, Rotation, Vector}; +use crate::math::{Pose, Real, Rotation, Vector}; use crate::shape::{Segment, SupportMap}; -use na::Unit; #[cfg(feature = "alloc")] use either::Either; -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; - #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))] #[cfg_attr(feature = "encase", derive(encase::ShaderType))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] /// A capsule shape, also known as a pill or capped cylinder. @@ -58,7 +53,7 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Capsule; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create a vertical capsule (aligned with Y axis) /// // Half-height of 2.0 means the segment is 4.0 units long @@ -67,8 +62,8 @@ use rkyv::{bytecheck, CheckBytes}; /// assert_eq!(capsule.height(), 4.0); /// /// // Create a custom capsule between two points -/// let a = Point3::origin(); -/// let b = Point3::new(3.0, 4.0, 0.0); +/// let a = Vector::ZERO; +/// let b = Vector::new(3.0, 4.0, 0.0); /// let custom = Capsule::new(a, b, 1.0); /// assert_eq!(custom.height(), 5.0); // Distance from a to b /// # } @@ -111,11 +106,11 @@ impl Capsule { /// /// // The center is at the origin /// let center = capsule.center(); - /// assert!(center.coords.norm() < 1e-6); + /// assert!(center.length() < 1e-6); /// # } /// ``` pub fn new_x(half_height: Real, radius: Real) -> Self { - let b = Point::from(Vector::x() * half_height); + let b = Vector::X * half_height; Self::new(-b, b, radius) } @@ -144,7 +139,7 @@ impl Capsule { /// # } /// ``` pub fn new_y(half_height: Real, radius: Real) -> Self { - let b = Point::from(Vector::y() * half_height); + let b = Vector::Y * half_height; Self::new(-b, b, radius) } @@ -171,7 +166,7 @@ impl Capsule { /// ``` #[cfg(feature = "dim3")] pub fn new_z(half_height: Real, radius: Real) -> Self { - let b = Point::from(Vector::z() * half_height); + let b = Vector::Z * half_height; Self::new(-b, b, radius) } @@ -191,11 +186,11 @@ impl Capsule { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Capsule; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a diagonal capsule - /// let a = Point3::origin(); - /// let b = Point3::new(3.0, 4.0, 0.0); + /// let a = Vector::ZERO; + /// let b = Vector::new(3.0, 4.0, 0.0); /// let capsule = Capsule::new(a, b, 0.5); /// /// // Height is the distance between a and b @@ -203,10 +198,10 @@ impl Capsule { /// /// // Center is the midpoint /// let center = capsule.center(); - /// assert_eq!(center, Point3::new(1.5, 2.0, 0.0)); + /// assert_eq!(center, Vector::new(1.5, 2.0, 0.0)); /// # } /// ``` - pub fn new(a: Point, b: Point, radius: Real) -> Self { + pub fn new(a: Vector, b: Vector, radius: Real) -> Self { let segment = Segment::new(a, b); Self { segment, radius } } @@ -234,7 +229,7 @@ impl Capsule { /// # } /// ``` pub fn height(&self) -> Real { - (self.segment.b - self.segment.a).norm() + (self.segment.b - self.segment.a).length() } /// Returns half the length of the capsule's central segment. @@ -265,18 +260,18 @@ impl Capsule { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Capsule; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let a = Point3::new(-2.0, 0.0, 0.0); - /// let b = Point3::new(4.0, 0.0, 0.0); + /// let a = Vector::new(-2.0, 0.0, 0.0); + /// let b = Vector::new(4.0, 0.0, 0.0); /// let capsule = Capsule::new(a, b, 1.0); /// /// let center = capsule.center(); - /// assert_eq!(center, Point3::new(1.0, 0.0, 0.0)); + /// assert_eq!(center, Vector::new(1.0, 0.0, 0.0)); /// # } /// ``` - pub fn center(&self) -> Point { - na::center(&self.segment.a, &self.segment.b) + pub fn center(&self) -> Vector { + self.segment.a.midpoint(self.segment.b) } /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. @@ -292,12 +287,12 @@ impl Capsule { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Capsule; - /// use nalgebra::{Isometry3, Vector3}; + /// use parry3d::math::{Pose, Vector}; /// /// let capsule = Capsule::new_y(1.0, 0.5); /// /// // Translate the capsule 5 units along X axis - /// let transform = Isometry3::translation(5.0, 0.0, 0.0); + /// let transform = Pose::translation(5.0, 0.0, 0.0); /// let transformed = capsule.transform_by(&transform); /// /// // Center moved by 5 units @@ -306,40 +301,32 @@ impl Capsule { /// assert_eq!(transformed.radius, 0.5); /// # } /// ``` - pub fn transform_by(&self, pos: &Isometry) -> Self { + pub fn transform_by(&self, pos: &Pose) -> Self { Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) } /// The transformation such that `t * Y` is collinear with `b - a` and `t * origin` equals /// the capsule's center. - pub fn canonical_transform(&self) -> Isometry { - let tra = self.center().coords; + pub fn canonical_transform(&self) -> Pose { + let tra = self.center(); let rot = self.rotation_wrt_y(); - Isometry::from_parts(tra.into(), rot) + Pose::from_parts(tra, rot) } /// The rotation `r` such that `r * Y` is collinear with `b - a`. - pub fn rotation_wrt_y(&self) -> Rotation { + pub fn rotation_wrt_y(&self) -> Rotation { let mut dir = self.segment.b - self.segment.a; if dir.y < 0.0 { dir = -dir; } - - #[cfg(feature = "dim2")] - { - Rotation::rotation_between(&Vector::y(), &dir) - } - - #[cfg(feature = "dim3")] - { - Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()) - } + let dir = dir.normalize_or(Vector::Y); + Rotation::from_rotation_arc(Vector::Y, dir) } /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`. - pub fn transform_wrt_y(&self) -> Isometry { + pub fn transform_wrt_y(&self) -> Pose { let rot = self.rotation_wrt_y(); - Isometry::from_parts(self.center().coords.into(), rot) + Pose::from_parts(self.center(), rot) } /// Computes a scaled version of this capsule. @@ -351,14 +338,13 @@ impl Capsule { #[cfg(all(feature = "dim2", feature = "alloc"))] pub fn scaled( self, - scale: &Vector, + scale: Vector, nsubdivs: u32, ) -> Option> { if scale.x != scale.y { // The scaled shape is not a capsule. let mut vtx = self.to_polyline(nsubdivs); - vtx.iter_mut() - .for_each(|pt| pt.coords = pt.coords.component_mul(scale)); + vtx.iter_mut().for_each(|pt| *pt *= scale); Some(Either::Right(super::ConvexPolygon::from_convex_polyline( vtx, )?)) @@ -381,14 +367,13 @@ impl Capsule { #[cfg(all(feature = "dim3", feature = "alloc"))] pub fn scaled( self, - scale: &Vector, + scale: Vector, nsubdivs: u32, ) -> Option> { if scale.x != scale.y || scale.x != scale.z || scale.y != scale.z { // The scaled shape is not a capsule. let (mut vtx, idx) = self.to_trimesh(nsubdivs, nsubdivs); - vtx.iter_mut() - .for_each(|pt| pt.coords = pt.coords.component_mul(scale)); + vtx.iter_mut().for_each(|pt| *pt *= scale); Some(Either::Right(super::ConvexPolyhedron::from_convex_mesh( vtx, &idx, )?)) @@ -404,16 +389,16 @@ impl Capsule { } impl SupportMap for Capsule { - fn local_support_point(&self, dir: &Vector) -> Point { - let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); - self.local_support_point_toward(&dir) + fn local_support_point(&self, dir: Vector) -> Vector { + let dir = dir.normalize_or(Vector::Y); + self.local_support_point_toward(dir) } - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { - self.segment.a + **dir * self.radius + fn local_support_point_toward(&self, dir: Vector) -> Vector { + if dir.dot(self.segment.a) > dir.dot(self.segment.b) { + self.segment.a + dir * self.radius } else { - self.segment.b + **dir * self.radius + self.segment.b + dir * self.radius } } } diff --git a/src/shape/composite_shape.rs b/src/shape/composite_shape.rs index 99e321ca..25f3002d 100644 --- a/src/shape/composite_shape.rs +++ b/src/shape/composite_shape.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::partitioning::Bvh; use crate::query::details::NormalConstraints; use crate::shape::Shape; @@ -27,7 +27,7 @@ pub trait CompositeShape { fn map_part_at( &self, shape_id: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ); /// Gets the acceleration structure of the composite shape. @@ -42,11 +42,7 @@ pub trait TypedCompositeShape: CompositeShape { fn map_typed_part_at( &self, shape_id: u32, - f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option; // TODO: we need this method because the compiler won't want @@ -55,7 +51,7 @@ pub trait TypedCompositeShape: CompositeShape { fn map_untyped_part_at( &self, shape_id: u32, - f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option; } @@ -67,11 +63,7 @@ impl TypedCompositeShape for dyn CompositeShape + '_ { fn map_typed_part_at( &self, shape_id: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { let mut result = None; self.map_part_at(shape_id, &mut |pose, part, normals| { @@ -83,7 +75,7 @@ impl TypedCompositeShape for dyn CompositeShape + '_ { fn map_untyped_part_at( &self, shape_id: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let mut result = None; self.map_part_at(shape_id, &mut |pose, part, normals| { diff --git a/src/shape/compound.rs b/src/shape/compound.rs index 67412b3c..5fc0a32c 100644 --- a/src/shape/compound.rs +++ b/src/shape/compound.rs @@ -3,7 +3,7 @@ //! use crate::bounding_volume::{Aabb, BoundingSphere, BoundingVolume}; -use crate::math::{Isometry, Real}; +use crate::math::Pose; use crate::partitioning::{Bvh, BvhBuildStrategy}; use crate::query::details::NormalConstraints; use crate::shape::{CompositeShape, Shape, SharedShape, TypedCompositeShape}; @@ -21,7 +21,7 @@ use alloc::vec::Vec; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[derive(Clone, Debug)] pub struct Compound { - shapes: Vec<(Isometry, SharedShape)>, + shapes: Vec<(Pose, SharedShape)>, bvh: Bvh, aabbs: Vec, aabb: Aabb, @@ -38,7 +38,7 @@ impl Compound { /// # Arguments /// /// * `shapes` - A vector of (position, shape) pairs. Each pair defines: - /// - An [`Isometry`] representing the sub-shape's position and orientation relative to the compound's origin + /// - An [`Pose`] representing the sub-shape's position and orientation relative to the compound's origin /// - A [`SharedShape`] containing the actual geometry /// /// # Panics @@ -57,24 +57,24 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, Cuboid, SharedShape}; - /// use parry3d::math::Isometry; - /// use nalgebra::Vector3; + /// use parry3d::math::Pose; + /// use parry3d::math::Vector; /// /// // Create a compound shape resembling a dumbbell /// let shapes = vec![ /// // Left sphere /// ( - /// Isometry::translation(-2.0, 0.0, 0.0), + /// Pose::translation(-2.0, 0.0, 0.0), /// SharedShape::new(Ball::new(0.5)) /// ), /// // Center bar /// ( - /// Isometry::identity(), - /// SharedShape::new(Cuboid::new(Vector3::new(2.0, 0.2, 0.2))) + /// Pose::identity(), + /// SharedShape::new(Cuboid::new(Vector::new(2.0, 0.2, 0.2))) /// ), /// // Right sphere /// ( - /// Isometry::translation(2.0, 0.0, 0.0), + /// Pose::translation(2.0, 0.0, 0.0), /// SharedShape::new(Ball::new(0.5)) /// ), /// ]; @@ -89,20 +89,20 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::shape::{Compound, Ball, Cuboid, SharedShape}; - /// # use parry2d::math::Isometry; - /// # use nalgebra::Vector2; + /// # use parry2d::math::Pose; + /// # use parry2d::math::Vector; /// /// // Create an L-shaped compound /// let shapes = vec![ /// // Vertical rectangle /// ( - /// Isometry::translation(0.0, 1.0), - /// SharedShape::new(Cuboid::new(Vector2::new(0.5, 1.0))) + /// Pose::translation(0.0, 1.0), + /// SharedShape::new(Cuboid::new(Vector::new(0.5, 1.0))) /// ), /// // Horizontal rectangle /// ( - /// Isometry::translation(1.0, 0.0), - /// SharedShape::new(Cuboid::new(Vector2::new(1.0, 0.5))) + /// Pose::translation(1.0, 0.0), + /// SharedShape::new(Cuboid::new(Vector::new(1.0, 0.5))) /// ), /// ]; /// @@ -110,7 +110,7 @@ impl Compound { /// assert_eq!(l_shape.shapes().len(), 2); /// # } /// ``` - pub fn new(shapes: Vec<(Isometry, SharedShape)>) -> Compound { + pub fn new(shapes: Vec<(Pose, SharedShape)>) -> Compound { assert!( !shapes.is_empty(), "A compound shape must contain at least one shape." @@ -174,14 +174,14 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::shape::{Compound, TriMesh}; - /// # use nalgebra::Point2; + /// # use parry2d::math::Vector; /// /// // Create a simple square mesh (2 triangles) /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(0.0, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(0.0, 1.0), /// ]; /// /// let indices = vec![ @@ -204,16 +204,16 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::shape::{Compound, TriMesh}; - /// # use nalgebra::Point2; + /// # use parry2d::math::Vector; /// /// // Create an L-shaped mesh /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(2.0, 0.0), - /// Point2::new(2.0, 1.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(1.0, 2.0), - /// Point2::new(0.0, 2.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0), + /// Vector::new(2.0, 1.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(1.0, 2.0), + /// Vector::new(0.0, 2.0), /// ]; /// /// let indices = vec![ @@ -246,7 +246,7 @@ impl Compound { } _ => ConvexPolygon::from_convex_polyline(points).map(SharedShape::new), } - .map(|shape| (Isometry::identity(), shape)) + .map(|shape| (Pose::IDENTITY, shape)) }) .collect(); Some(Self::new(shapes?)) @@ -257,7 +257,7 @@ impl Compound { /// Returns a slice containing all sub-shapes and their positions in this compound. /// /// Each element in the returned slice is a tuple containing: - /// - The sub-shape's position and orientation ([`Isometry`]) relative to the compound's origin + /// - The sub-shape's position and orientation ([`Pose`]) relative to the compound's origin /// - The sub-shape itself ([`SharedShape`]) /// /// The order of shapes matches the order they were provided to [`Compound::new`]. @@ -273,13 +273,13 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, Cuboid, SharedShape}; - /// use parry3d::math::Isometry; - /// use nalgebra::Vector3; + /// use parry3d::math::Pose; + /// use parry3d::math::Vector; /// /// let shapes = vec![ - /// (Isometry::translation(1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(-1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.3))), - /// (Isometry::identity(), SharedShape::new(Cuboid::new(Vector3::new(0.5, 0.5, 0.5)))), + /// (Pose::translation(1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(-1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.3))), + /// (Pose::identity(), SharedShape::new(Cuboid::new(Vector::new(0.5, 0.5, 0.5)))), /// ]; /// /// let compound = Compound::new(shapes); @@ -304,11 +304,11 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, SharedShape}; - /// use parry3d::math::Isometry; + /// use parry3d::math::Pose; /// /// let shapes = vec![ - /// (Isometry::translation(0.0, 0.0, 0.0), SharedShape::new(Ball::new(1.0))), - /// (Isometry::translation(2.0, 0.0, 0.0), SharedShape::new(Ball::new(1.0))), + /// (Pose::translation(0.0, 0.0, 0.0), SharedShape::new(Ball::new(1.0))), + /// (Pose::translation(2.0, 0.0, 0.0), SharedShape::new(Ball::new(1.0))), /// ]; /// /// let compound = Compound::new(shapes); @@ -318,7 +318,7 @@ impl Compound { /// .iter() /// .map(|(pos, shape)| { /// // Shift all shapes up by 1 unit - /// let new_pos = pos * Isometry::translation(0.0, 1.0, 0.0); + /// let new_pos = pos * Pose::translation(0.0, 1.0, 0.0); /// (new_pos, shape.clone()) /// }) /// .collect(); @@ -328,7 +328,7 @@ impl Compound { /// # } /// ``` #[inline] - pub fn shapes(&self) -> &[(Isometry, SharedShape)] { + pub fn shapes(&self) -> &[(Pose, SharedShape)] { &self.shapes[..] } @@ -355,12 +355,12 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, SharedShape}; - /// use parry3d::math::Isometry; - /// use nalgebra::Point3; + /// use parry3d::math::Pose; + /// use parry3d::math::Vector; /// /// let shapes = vec![ - /// (Isometry::translation(-2.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(2.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(-2.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(2.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), /// ]; /// /// let compound = Compound::new(shapes); @@ -373,8 +373,8 @@ impl Compound { /// assert!(aabb.maxs.x >= 2.5); /// /// // Check if a point is inside the AABB - /// assert!(aabb.contains_local_point(&Point3::origin())); - /// assert!(!aabb.contains_local_point(&Point3::new(10.0, 0.0, 0.0))); + /// assert!(aabb.contains_local_point(Vector::ZERO)); + /// assert!(!aabb.contains_local_point(Vector::new(10.0, 0.0, 0.0))); /// # } /// ``` /// @@ -383,12 +383,12 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Cuboid, SharedShape}; - /// use parry3d::math::Isometry; - /// use nalgebra::Vector3; + /// use parry3d::math::Pose; + /// use parry3d::math::Vector; /// /// let shapes = vec![ - /// (Isometry::identity(), SharedShape::new(Cuboid::new(Vector3::new(1.0, 1.0, 1.0)))), - /// (Isometry::translation(3.0, 0.0, 0.0), SharedShape::new(Cuboid::new(Vector3::new(0.5, 0.5, 0.5)))), + /// (Pose::identity(), SharedShape::new(Cuboid::new(Vector::new(1.0, 1.0, 1.0)))), + /// (Pose::translation(3.0, 0.0, 0.0), SharedShape::new(Cuboid::new(Vector::new(0.5, 0.5, 0.5)))), /// ]; /// /// let compound = Compound::new(shapes); @@ -433,11 +433,11 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, SharedShape}; - /// use parry3d::math::Isometry; + /// use parry3d::math::Pose; /// /// let shapes = vec![ - /// (Isometry::translation(-1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(-1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(1.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), /// ]; /// /// let compound = Compound::new(shapes); @@ -448,7 +448,7 @@ impl Compound { /// println!("Radius: {}", bounding_sphere.radius()); /// /// // The center should be near the origin - /// assert!(bounding_sphere.center().coords.norm() < 0.1); + /// assert!(bounding_sphere.center().length() < 0.1); /// /// // The radius should be at least 1.5 (distance to ball edge: 1.0 + 0.5) /// assert!(bounding_sphere.radius() >= 1.5); @@ -460,26 +460,25 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Cuboid, SharedShape}; - /// use parry3d::math::Isometry; - /// use nalgebra::{Vector3, Point3}; + /// use parry3d::math::{Pose, Vector}; /// /// let shapes = vec![ - /// (Isometry::identity(), SharedShape::new(Cuboid::new(Vector3::new(1.0, 1.0, 1.0)))), - /// (Isometry::translation(2.0, 0.0, 0.0), SharedShape::new(Cuboid::new(Vector3::new(0.5, 0.5, 0.5)))), + /// (Pose::identity(), SharedShape::new(Cuboid::new(Vector::new(1.0, 1.0, 1.0)))), + /// (Pose::translation(2.0, 0.0, 0.0), SharedShape::new(Cuboid::new(Vector::new(0.5, 0.5, 0.5)))), /// ]; /// /// let compound = Compound::new(shapes); /// let sphere = compound.local_bounding_sphere(); /// /// // Quick test: is a point potentially inside the compound? - /// let test_point = Point3::new(5.0, 5.0, 5.0); - /// let distance_to_center = (test_point - sphere.center()).norm(); + /// let test_point = Vector::new(5.0, 5.0, 5.0); + /// let distance_to_center = (test_point - sphere.center()).length(); /// /// if distance_to_center > sphere.radius() { - /// println!("Point is definitely outside the compound"); + /// println!("Vector is definitely outside the compound"); /// assert!(distance_to_center > sphere.radius()); /// } else { - /// println!("Point might be inside - need detailed check"); + /// println!("Vector might be inside - need detailed check"); /// } /// # } /// ``` @@ -514,13 +513,13 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, Cuboid, SharedShape}; - /// use parry3d::math::Isometry; - /// use nalgebra::Vector3; + /// use parry3d::math::Pose; + /// use parry3d::math::Vector; /// /// let shapes = vec![ - /// (Isometry::translation(-2.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::identity(), SharedShape::new(Cuboid::new(Vector3::new(1.0, 1.0, 1.0)))), - /// (Isometry::translation(3.0, 0.0, 0.0), SharedShape::new(Ball::new(0.3))), + /// (Pose::translation(-2.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::identity(), SharedShape::new(Cuboid::new(Vector::new(1.0, 1.0, 1.0)))), + /// (Pose::translation(3.0, 0.0, 0.0), SharedShape::new(Ball::new(0.3))), /// ]; /// /// let compound = Compound::new(shapes); @@ -548,26 +547,26 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, SharedShape}; - /// use parry3d::math::Isometry; + /// use parry3d::math::Pose; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let shapes = vec![ - /// (Isometry::translation(-5.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(0.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(5.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(-5.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(0.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(5.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), /// ]; /// /// let compound = Compound::new(shapes); /// /// // Define a query point - /// let query_point = Point3::origin(); + /// let query_point = Vector::ZERO; /// /// // Find which sub-shapes might contain this point /// let potentially_containing: Vec = compound.aabbs() /// .iter() /// .enumerate() - /// .filter(|(_, aabb)| aabb.contains_local_point(&query_point)) + /// .filter(|(_, aabb)| aabb.contains_local_point(query_point)) /// .map(|(i, _)| i) /// .collect(); /// @@ -611,14 +610,14 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, SharedShape}; - /// use parry3d::math::Isometry; + /// use parry3d::math::Pose; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let shapes = vec![ - /// (Isometry::translation(-3.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(0.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), - /// (Isometry::translation(3.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(-3.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(0.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), + /// (Pose::translation(3.0, 0.0, 0.0), SharedShape::new(Ball::new(0.5))), /// ]; /// /// let compound = Compound::new(shapes); @@ -635,13 +634,13 @@ impl Compound { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{Compound, Ball, SharedShape}; - /// use parry3d::math::Isometry; + /// use parry3d::math::Pose; /// /// let mut shapes = vec![]; /// for i in 0..10 { /// let x = i as f32 * 2.0; /// shapes.push(( - /// Isometry::translation(x, 0.0, 0.0), + /// Pose::translation(x, 0.0, 0.0), /// SharedShape::new(Ball::new(0.5)) /// )); /// } @@ -669,7 +668,7 @@ impl CompositeShape for Compound { fn map_part_at( &self, shape_id: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { if let Some(shape) = self.shapes.get(shape_id as usize) { f(Some(&shape.0), &*shape.1, None) @@ -690,11 +689,7 @@ impl TypedCompositeShape for Compound { fn map_typed_part_at( &self, i: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { let (part_pos, part) = &self.shapes[i as usize]; Some(f(Some(part_pos), &**part, None)) @@ -704,11 +699,7 @@ impl TypedCompositeShape for Compound { fn map_untyped_part_at( &self, i: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&dyn NormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let (part_pos, part) = &self.shapes[i as usize]; Some(f(Some(part_pos), &**part, None)) diff --git a/src/shape/cone.rs b/src/shape/cone.rs index 8fd5efb2..45779705 100644 --- a/src/shape/cone.rs +++ b/src/shape/cone.rs @@ -1,19 +1,11 @@ //! Support mapping based Cone shape. -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::shape::SupportMap; -use na; -use num::Zero; #[cfg(feature = "alloc")] use either::Either; -#[cfg(not(feature = "alloc"))] -use na::RealField; // for .copysign() - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; - /// A 3D cone shape with apex pointing upward along the Y axis. /// /// A cone is a shape that tapers from a circular base to a point (apex). In Parry, @@ -24,7 +16,7 @@ use rkyv::{bytecheck, CheckBytes}; /// /// - **Axis**: Always aligned with Y axis (apex points up) /// - **Base**: Circular base at y = -half_height with the given radius -/// - **Apex**: Point at y = +half_height +/// - **Apex**: Vector at y = +half_height /// - **Total height**: `2 * half_height` /// /// # Properties @@ -72,8 +64,7 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] @@ -135,16 +126,15 @@ impl Cone { #[inline] pub fn scaled( self, - scale: &Vector, + scale: Vector, nsubdivs: u32, ) -> Option> { // NOTE: if the y scale is negative, the result cone points downwards, // which can’t be represented with this Cone (without a transform). if scale.x != scale.z || scale.y < 0.0 { - // The scaled shape isn’t a cone. + // The scaled shape isn't a cone. let (mut vtx, idx) = self.to_trimesh(nsubdivs); - vtx.iter_mut() - .for_each(|pt| pt.coords = pt.coords.component_mul(scale)); + vtx.iter_mut().for_each(|pt| *pt *= scale); Some(Either::Right(super::ConvexPolyhedron::from_convex_mesh( vtx, &idx, )?)) @@ -159,24 +149,25 @@ impl Cone { impl SupportMap for Cone { #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { - let mut vres = *dir; + fn local_support_point(&self, dir: Vector) -> Vector { + let mut vres = dir; vres[1] = 0.0; + let (mut vres, length) = vres.normalize_and_length(); - if vres.normalize_mut().is_zero() { - vres = na::zero(); + if length == 0.0 { + vres = Vector::ZERO; vres[1] = self.half_height.copysign(dir[1]); } else { vres *= self.radius; vres[1] = -self.half_height; - if dir.dot(&vres) < dir[1] * self.half_height { - vres = na::zero(); + if dir.dot(vres) < dir[1] * self.half_height { + vres = Vector::ZERO; vres[1] = self.half_height } } - Point::from(vres) + vres } } diff --git a/src/shape/convex_polygon.rs b/src/shape/convex_polygon.rs index b6271d83..0c36fdc0 100644 --- a/src/shape/convex_polygon.rs +++ b/src/shape/convex_polygon.rs @@ -1,8 +1,7 @@ -use crate::math::{Point, Real, Vector}; +use crate::math::{ComplexField, Real, RealField, Vector}; use crate::shape::{FeatureId, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, SupportMap}; use crate::utils; use alloc::vec::Vec; -use na::{self, ComplexField, RealField, Unit}; /// A 2D convex polygon. /// @@ -32,7 +31,7 @@ use na::{self, ComplexField, RealField, Unit}; /// # Representation /// /// This structure stores: -/// - **Points**: The vertices of the polygon in counter-clockwise order +/// - **Vectors**: The vertices of the polygon in counter-clockwise order /// - **Normals**: Unit vectors perpendicular to each edge, pointing outward /// /// The normals are pre-computed for efficient collision detection algorithms. @@ -42,13 +41,13 @@ use na::{self, ComplexField, RealField, Unit}; /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; -/// use nalgebra::Point2; +/// use parry2d::math::Vector; /// /// // Create a triangle from three vertices (counter-clockwise order) /// let vertices = vec![ -/// Point2::origin(), // bottom-left -/// Point2::new(2.0, 0.0), // bottom-right -/// Point2::new(1.0, 2.0), // top +/// Vector::ZERO, // bottom-left +/// Vector::new(2.0, 0.0), // bottom-right +/// Vector::new(1.0, 2.0), // top /// ]; /// /// let triangle = ConvexPolygon::from_convex_polyline(vertices) @@ -63,13 +62,12 @@ use na::{self, ComplexField, RealField, Unit}; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone, Debug)] pub struct ConvexPolygon { - points: Vec>, - normals: Vec>>, + points: Vec, + normals: Vec, } impl ConvexPolygon { @@ -94,14 +92,14 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Some arbitrary points (including one inside the convex hull) /// let points = vec![ - /// Point2::origin(), - /// Point2::new(4.0, 0.0), - /// Point2::new(2.0, 3.0), - /// Point2::new(2.0, 1.0), // This point is inside the triangle + /// Vector::ZERO, + /// Vector::new(4.0, 0.0), + /// Vector::new(2.0, 3.0), + /// Vector::new(2.0, 1.0), // This point is inside the triangle /// ]; /// /// let polygon = ConvexPolygon::from_convex_hull(&points) @@ -117,17 +115,17 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // A cloud of points that roughly forms a circle /// let mut points = Vec::new(); /// for i in 0..20 { /// let angle = (i as f32) * std::f32::consts::TAU / 20.0; - /// points.push(Point2::new(angle.cos(), angle.sin())); + /// points.push(Vector::new(angle.cos(), angle.sin())); /// } /// // Add some interior points - /// points.push(Point2::origin()); - /// points.push(Point2::new(0.5, 0.5)); + /// points.push(Vector::ZERO); + /// points.push(Vector::new(0.5, 0.5)); /// /// let polygon = ConvexPolygon::from_convex_hull(&points) /// .expect("Failed to create convex hull"); @@ -136,7 +134,7 @@ impl ConvexPolygon { /// assert_eq!(polygon.points().len(), 20); /// # } /// ``` - pub fn from_convex_hull(points: &[Point]) -> Option { + pub fn from_convex_hull(points: &[Vector]) -> Option { let vertices = crate::transformation::convex_hull(points); Self::from_convex_polyline(vertices) } @@ -148,7 +146,7 @@ impl ConvexPolygon { /// convex hull. The convexity is **not verified** - if you pass non-convex points, the resulting /// shape may behave incorrectly in collision detection. /// - /// **Important**: Points must be ordered **counter-clockwise** (CCW). If you're unsure about the + /// **Important**: Vectors must be ordered **counter-clockwise** (CCW). If you're unsure about the /// ordering or convexity, use [`from_convex_hull`] instead. /// /// This method automatically removes collinear vertices (points that lie on the line between @@ -172,14 +170,14 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // A square with vertices in counter-clockwise order /// let square = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), // bottom-left - /// Point2::new(1.0, 0.0), // bottom-right - /// Point2::new(1.0, 1.0), // top-right - /// Point2::new(0.0, 1.0), // top-left + /// Vector::ZERO, // bottom-left + /// Vector::new(1.0, 0.0), // bottom-right + /// Vector::new(1.0, 1.0), // top-right + /// Vector::new(0.0, 1.0), // top-left /// ]).expect("Failed to create square"); /// /// assert_eq!(square.points().len(), 4); @@ -191,15 +189,15 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // A quadrilateral with one vertex on an edge (making it collinear) /// let polygon = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), - /// Point2::new(2.0, 0.0), - /// Point2::new(2.0, 1.0), // This point is on the line from (2,0) to (2,2) - /// Point2::new(2.0, 2.0), - /// Point2::new(0.0, 2.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0), + /// Vector::new(2.0, 1.0), // This point is on the line from (2,0) to (2,2) + /// Vector::new(2.0, 2.0), + /// Vector::new(0.0, 2.0), /// ]).expect("Failed to create polygon"); /// /// // The collinear point at (2.0, 1.0) was removed, leaving a rectangle @@ -209,21 +207,21 @@ impl ConvexPolygon { /// /// [`from_convex_hull`]: ConvexPolygon::from_convex_hull /// [`from_convex_polyline_unmodified`]: ConvexPolygon::from_convex_polyline_unmodified - pub fn from_convex_polyline(mut points: Vec>) -> Option { + pub fn from_convex_polyline(mut points: Vec) -> Option { if points.is_empty() { return None; } - let eps = ComplexField::sqrt(crate::math::DEFAULT_EPSILON); + let eps = ::sqrt(crate::math::DEFAULT_EPSILON); let mut normals = Vec::with_capacity(points.len()); // First, compute all normals. for i1 in 0..points.len() { let i2 = (i1 + 1) % points.len(); - normals.push(utils::ccw_face_normal([&points[i1], &points[i2]])?); + normals.push(utils::ccw_face_normal([points[i1], points[i2]])?); } let mut nremoved = 0; // See if the first vertex must be removed. - if normals[0].dot(&*normals[normals.len() - 1]) > 1.0 - eps { + if normals[0].dot(normals[normals.len() - 1]) > 1.0 - eps { nremoved = 1; } @@ -231,7 +229,7 @@ impl ConvexPolygon { // of collinearity of adjacent faces. for i2 in 1..points.len() { let i1 = i2 - 1; - if normals[i1].dot(&*normals[i2]) > 1.0 - eps { + if normals[i1].dot(normals[i2]) > 1.0 - eps { // Remove nremoved += 1; } else { @@ -258,7 +256,7 @@ impl ConvexPolygon { /// from the input even if some are coplanar. /// /// Returns `None` if `points` doesn’t contain at least three points. - pub fn from_convex_polyline_unmodified(points: Vec>) -> Option { + pub fn from_convex_polyline_unmodified(points: Vec) -> Option { if points.len() <= 2 { return None; } @@ -266,7 +264,7 @@ impl ConvexPolygon { // First, compute all normals. for i1 in 0..points.len() { let i2 = (i1 + 1) % points.len(); - normals.push(utils::ccw_face_normal([&points[i1], &points[i2]])?); + normals.push(utils::ccw_face_normal([points[i1], points[i2]])?); } Some(ConvexPolygon { points, normals }) @@ -282,21 +280,21 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// let triangle = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(0.5, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(0.5, 1.0), /// ]).unwrap(); /// /// let vertices = triangle.points(); /// assert_eq!(vertices.len(), 3); - /// assert_eq!(vertices[0], Point2::origin()); + /// assert_eq!(vertices[0], Vector::ZERO); /// # } /// ``` #[inline] - pub fn points(&self) -> &[Point] { + pub fn points(&self) -> &[Vector] { &self.points } @@ -313,14 +311,14 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::{Point2, Vector2}; + /// use parry2d::math::Vector; /// /// // Create a square aligned with the axes /// let square = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), // bottom-left - /// Point2::new(1.0, 0.0), // bottom-right - /// Point2::new(1.0, 1.0), // top-right - /// Point2::new(0.0, 1.0), // top-left + /// Vector::ZERO, // bottom-left + /// Vector::new(1.0, 0.0), // bottom-right + /// Vector::new(1.0, 1.0), // top-right + /// Vector::new(0.0, 1.0), // top-left /// ]).unwrap(); /// /// let normals = square.normals(); @@ -333,7 +331,7 @@ impl ConvexPolygon { /// # } /// ``` #[inline] - pub fn normals(&self) -> &[Unit>] { + pub fn normals(&self) -> &[Vector] { &self.normals } @@ -356,21 +354,21 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::{Point2, Vector2}; + /// use parry2d::math::Vector; /// /// let triangle = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(0.5, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(0.5, 1.0), /// ]).unwrap(); /// /// // Scale uniformly by 2x - /// let scaled = triangle.scaled(&Vector2::new(2.0, 2.0)) + /// let scaled = triangle.scaled(Vector::new(2.0, 2.0)) /// .expect("Failed to scale"); /// /// // All coordinates are doubled - /// assert_eq!(scaled.points()[1], Point2::new(2.0, 0.0)); - /// assert_eq!(scaled.points()[2], Point2::new(1.0, 2.0)); + /// assert_eq!(scaled.points()[1], Vector::new(2.0, 0.0)); + /// assert_eq!(scaled.points()[2], Vector::new(1.0, 2.0)); /// # } /// ``` /// @@ -379,29 +377,28 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::{Point2, Vector2}; + /// use parry2d::math::Vector; /// /// let square = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(0.0, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(0.0, 1.0), /// ]).unwrap(); /// /// // Scale to make it wider (2x) and taller (3x) - /// let rectangle = square.scaled(&Vector2::new(2.0, 3.0)) + /// let rectangle = square.scaled(Vector::new(2.0, 3.0)) /// .expect("Failed to scale"); /// - /// assert_eq!(rectangle.points()[2], Point2::new(2.0, 3.0)); + /// assert_eq!(rectangle.points()[2], Vector::new(2.0, 3.0)); /// # } /// ``` - pub fn scaled(mut self, scale: &Vector) -> Option { - self.points - .iter_mut() - .for_each(|pt| pt.coords.component_mul_assign(scale)); + pub fn scaled(mut self, scale: Vector) -> Option { + self.points.iter_mut().for_each(|pt| *pt *= scale); for n in &mut self.normals { - *n = Unit::try_new(n.component_mul(scale), 0.0)?; + let scaled = *n * scale; + *n = scaled.try_normalize()?; } Some(self) @@ -434,12 +431,12 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// let triangle = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), - /// Point2::new(2.0, 0.0), - /// Point2::new(1.0, 2.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0), + /// Vector::new(1.0, 2.0), /// ]).unwrap(); /// /// // Expand the triangle by 0.5 units @@ -457,13 +454,13 @@ impl ConvexPolygon { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::ConvexPolygon; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// let square = ConvexPolygon::from_convex_polyline(vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(0.0, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(0.0, 1.0), /// ]).unwrap(); /// /// // Create a 0.2 unit margin around the square @@ -491,35 +488,33 @@ impl ConvexPolygon { i2 - 1 }; let normal_a = normals[i1]; - let direction = normal_a.into_inner() + normals[i2].into_inner(); - points.push(self.points[i2] + (amount / direction.dot(&normal_a)) * direction); + let direction = normal_a + normals[i2]; + points.push(self.points[i2] + (amount / direction.dot(normal_a)) * direction); } ConvexPolygon { points, normals } } /// Get the ID of the feature with a normal that maximizes the dot product with `local_dir`. - pub fn support_feature_id_toward(&self, local_dir: &Unit>) -> FeatureId { + pub fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId { let eps: Real = Real::pi() / 180.0; - let ceps = ComplexField::cos(eps); + let ceps = ::cos(eps); // Check faces. for i in 0..self.normals.len() { let normal = &self.normals[i]; - if normal.dot(local_dir.as_ref()) >= ceps { + if normal.dot(local_dir) >= ceps { return FeatureId::Face(i as u32); } } // Support vertex. - FeatureId::Vertex( - utils::point_cloud_support_point_id(local_dir.as_ref(), &self.points) as u32, - ) + FeatureId::Vertex(utils::point_cloud_support_point_id(local_dir, &self.points) as u32) } /// The normal of the given feature. - pub fn feature_normal(&self, feature: FeatureId) -> Option>> { + pub fn feature_normal(&self, feature: FeatureId) -> Option { match feature { FeatureId::Face(id) => Some(self.normals[id as usize]), FeatureId::Vertex(id2) => { @@ -528,9 +523,8 @@ impl ConvexPolygon { } else { id2 as usize - 1 }; - Some(Unit::new_normalize( - *self.normals[id1] + *self.normals[id2 as usize], - )) + let sum = self.normals[id1] + self.normals[id2 as usize]; + sum.try_normalize() } _ => None, } @@ -539,14 +533,14 @@ impl ConvexPolygon { impl SupportMap for ConvexPolygon { #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { + fn local_support_point(&self, dir: Vector) -> Vector { utils::point_cloud_support_point(dir, self.points()) } } impl PolygonalFeatureMap for ConvexPolygon { - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolygonalFeature) { - let cuboid = crate::shape::Cuboid::new(self.points[2].coords); + fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) { + let cuboid = crate::shape::Cuboid::new(self.points[2]); cuboid.local_support_feature(dir, out_feature); let mut best_face = 0; let mut max_dot = self.normals[0].dot(dir); @@ -573,7 +567,7 @@ impl PolygonalFeatureMap for ConvexPolygon { /* impl ConvexPolyhedron for ConvexPolygon { - fn vertex(&self, id: FeatureId) -> Point { + fn vertex(&self, id: FeatureId) -> Vector { self.points[id.unwrap_vertex() as usize] } @@ -593,16 +587,16 @@ impl ConvexPolyhedron for ConvexPolygon { fn support_face_toward( &self, - m: &Isometry, - dir: &Unit>, + m: &Pose, + dir: Vector, out: &mut ConvexPolygonalFeature, ) { let ls_dir = m.inverse_transform_vector(dir); let mut best_face = 0; - let mut max_dot = self.normals[0].dot(&ls_dir); + let mut max_dot = self.normals[0].dot(ls_dir); for i in 1..self.points.len() { - let dot = self.normals[i].dot(&ls_dir); + let dot = self.normals[i].dot(ls_dir); if dot > max_dot { max_dot = dot; @@ -616,8 +610,8 @@ impl ConvexPolyhedron for ConvexPolygon { fn support_feature_toward( &self, - transform: &Isometry, - dir: &Unit>, + transform: &Pose, + dir: Vector, _angle: Real, out: &mut ConvexPolygonalFeature, ) { @@ -626,9 +620,9 @@ impl ConvexPolyhedron for ConvexPolygon { self.support_face_toward(transform, dir, out) } - fn support_feature_id_toward(&self, local_dir: &Unit>) -> FeatureId { - let eps: Real = na::convert::(f64::consts::PI / 180.0); - let ceps = ComplexField::cos(eps); + fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId { + let eps: Real = (f64::consts::PI / 180.0) as Real; + let ceps = ::cos(eps); // Check faces. for i in 0..self.normals.len() { @@ -655,17 +649,17 @@ mod tests { #[test] fn test_dilation() { let polygon = ConvexPolygon::from_convex_polyline(vec![ - Point::new(1., 0.), - Point::new(-1., 0.), - Point::new(0., -1.), + Vector::new(1., 0.), + Vector::new(-1., 0.), + Vector::new(0., -1.), ]) .unwrap(); let offsetted = polygon.offsetted(0.5); let expected = vec![ - Point::new(2.207, 0.5), - Point::new(-2.207, 0.5), - Point::new(0., -1.707), + Vector::new(2.207, 0.5), + Vector::new(-2.207, 0.5), + Vector::new(0., -1.707), ]; assert_eq!(offsetted.points().len(), 3); @@ -673,6 +667,6 @@ mod tests { .points() .iter() .zip(expected.iter()) - .all(|(a, b)| (a.coords - b.coords).magnitude() < 0.001)); + .all(|(a, b)| (a - b).length() < 0.001)); } } diff --git a/src/shape/convex_polyhedron.rs b/src/shape/convex_polyhedron.rs index 21206ac6..45fc1a1e 100644 --- a/src/shape/convex_polyhedron.rs +++ b/src/shape/convex_polyhedron.rs @@ -1,23 +1,18 @@ -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{Real, Vector, DIM}; use crate::shape::{FeatureId, PackedFeatureId, PolygonalFeature, PolygonalFeatureMap, SupportMap}; // use crate::transformation; +#[cfg(not(feature = "std"))] +use crate::math::ComplexField; use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::{self, SortedPair}; #[cfg(feature = "alloc")] use alloc::vec::Vec; -use core::f64; -#[cfg(not(feature = "std"))] -use na::ComplexField; // for .abs(), .sqrt(), and .sin_cos() -use na::{self, Point2, Unit}; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; +use core::f64; // for .abs(), .sqrt(), and .sin_cos() #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] pub struct Vertex { @@ -28,14 +23,13 @@ pub struct Vertex { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] pub struct Edge { - pub vertices: Point2, - pub faces: Point2, - pub dir: Unit>, + pub vertices: [u32; 2], + pub faces: [u32; 2], + pub dir: Vector, deleted: bool, } @@ -52,27 +46,25 @@ impl Edge { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] pub struct Face { pub first_vertex_or_edge: u32, pub num_vertices_or_edges: u32, - pub normal: Unit>, + pub normal: Vector, } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] struct Triangle { vertices: [u32; 3], edges: [u32; 3], - normal: Vector, + normal: Vector, parent_face: Option, is_degenerate: bool, } @@ -92,8 +84,7 @@ impl Triangle { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Clone)] /// A 3D convex polyhedron without degenerate faces. @@ -126,7 +117,7 @@ impl Triangle { /// # Representation /// /// This structure stores the complete topological information: -/// - **Points**: The 3D coordinates of all vertices +/// - **Vectors**: The 3D coordinates of all vertices /// - **Faces**: Polygonal faces with their outward-pointing normals /// - **Edges**: Connections between vertices, shared by exactly two faces /// - **Adjacency information**: Which faces/edges connect to each vertex, and vice versa @@ -143,14 +134,14 @@ impl Triangle { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Define the 4 vertices of a tetrahedron /// let points = vec![ -/// Point3::origin(), // base vertex 1 -/// Point3::new(1.0, 0.0, 0.0), // base vertex 2 -/// Point3::new(0.5, 1.0, 0.0), // base vertex 3 -/// Point3::new(0.5, 0.5, 1.0), // apex +/// Vector::ZERO, // base vertex 1 +/// Vector::new(1.0, 0.0, 0.0), // base vertex 2 +/// Vector::new(0.5, 1.0, 0.0), // base vertex 3 +/// Vector::new(0.5, 0.5, 1.0), // apex /// ]; /// /// // Define the 4 triangular faces (indices into the points array) @@ -170,7 +161,7 @@ impl Triangle { /// # } /// ``` pub struct ConvexPolyhedron { - points: Vec>, + points: Vec, vertices: Vec, faces: Vec, edges: Vec, @@ -208,19 +199,19 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// // Points defining a cube, plus some interior points + /// // Vectors defining a cube, plus some interior points /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(0.0, 0.0, 1.0), - /// Point3::new(1.0, 0.0, 1.0), - /// Point3::new(1.0, 1.0, 1.0), - /// Point3::new(0.0, 1.0, 1.0), - /// Point3::new(0.5, 0.5, 0.5), // Interior point - will be excluded + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), + /// Vector::new(1.0, 0.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), + /// Vector::new(0.0, 1.0, 1.0), + /// Vector::new(0.5, 0.5, 0.5), // Interior point - will be excluded /// ]; /// /// let polyhedron = ConvexPolyhedron::from_convex_hull(&points) @@ -238,18 +229,18 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Imagine these are vertices from a detailed character mesh /// let detailed_mesh_vertices = vec![ - /// Point3::new(-1.0, -1.0, -1.0), - /// Point3::new(1.0, -1.0, -1.0), - /// Point3::new(1.0, 1.0, -1.0), - /// Point3::new(-1.0, 1.0, -1.0), - /// Point3::new(-1.0, -1.0, 1.0), - /// Point3::new(1.0, -1.0, 1.0), - /// Point3::new(1.0, 1.0, 1.0), - /// Point3::new(-1.0, 1.0, 1.0), + /// Vector::new(-1.0, -1.0, -1.0), + /// Vector::new(1.0, -1.0, -1.0), + /// Vector::new(1.0, 1.0, -1.0), + /// Vector::new(-1.0, 1.0, -1.0), + /// Vector::new(-1.0, -1.0, 1.0), + /// Vector::new(1.0, -1.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), + /// Vector::new(-1.0, 1.0, 1.0), /// // ... many more vertices in the original mesh /// ]; /// @@ -261,7 +252,7 @@ impl ConvexPolyhedron { /// println!("Simplified to {} vertices", collision_hull.points().len()); /// # } /// ``` - pub fn from_convex_hull(points: &[Point]) -> Option { + pub fn from_convex_hull(points: &[Vector]) -> Option { crate::transformation::try_convex_hull(points) .ok() .and_then(|(vertices, indices)| Self::from_convex_mesh(vertices, &indices)) @@ -309,18 +300,18 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Define the 8 vertices of a unit cube /// let vertices = vec![ - /// Point3::origin(), // 0: bottom-left-front - /// Point3::new(1.0, 0.0, 0.0), // 1: bottom-right-front - /// Point3::new(1.0, 1.0, 0.0), // 2: bottom-right-back - /// Point3::new(0.0, 1.0, 0.0), // 3: bottom-left-back - /// Point3::new(0.0, 0.0, 1.0), // 4: top-left-front - /// Point3::new(1.0, 0.0, 1.0), // 5: top-right-front - /// Point3::new(1.0, 1.0, 1.0), // 6: top-right-back - /// Point3::new(0.0, 1.0, 1.0), // 7: top-left-back + /// Vector::ZERO, // 0: bottom-left-front + /// Vector::new(1.0, 0.0, 0.0), // 1: bottom-right-front + /// Vector::new(1.0, 1.0, 0.0), // 2: bottom-right-back + /// Vector::new(0.0, 1.0, 0.0), // 3: bottom-left-back + /// Vector::new(0.0, 0.0, 1.0), // 4: top-left-front + /// Vector::new(1.0, 0.0, 1.0), // 5: top-right-front + /// Vector::new(1.0, 1.0, 1.0), // 6: top-right-back + /// Vector::new(0.0, 1.0, 1.0), // 7: top-left-back /// ]; /// /// // Define the faces as triangles (2 triangles per cube face) @@ -354,16 +345,16 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // 6 vertices: 3 on bottom, 3 on top /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.5, 1.0, 0.0), - /// Point3::new(0.0, 0.0, 2.0), - /// Point3::new(1.0, 0.0, 2.0), - /// Point3::new(0.5, 1.0, 2.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.5, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 2.0), + /// Vector::new(1.0, 0.0, 2.0), + /// Vector::new(0.5, 1.0, 2.0), /// ]; /// /// let indices = vec![ @@ -388,7 +379,7 @@ impl ConvexPolyhedron { /// /// [`from_convex_hull`]: ConvexPolyhedron::from_convex_hull pub fn from_convex_mesh( - points: Vec>, + points: Vec, indices: &[[u32; DIM]], ) -> Option { let eps = crate::math::DEFAULT_EPSILON.sqrt(); @@ -444,15 +435,13 @@ impl ConvexPolyhedron { Entry::Vacant(e) => { edges_id[i1] = *e.insert(edges.len() as u32); - let dir = Unit::try_new( - points[idx[i2] as usize] - points[idx[i1] as usize], - crate::math::DEFAULT_EPSILON, - ); + let dir = + (points[idx[i2] as usize] - points[idx[i1] as usize]).try_normalize(); edges.push(Edge { - vertices: Point2::new(idx[i1], idx[i2]), - faces: Point2::new(face_id as u32, u32::MAX), - dir: dir.unwrap_or(Vector::x_axis()), + vertices: [idx[i1], idx[i2]], + faces: [face_id as u32, u32::MAX], + dir: dir.unwrap_or(Vector::X), deleted: dir.is_none(), }); } @@ -460,14 +449,14 @@ impl ConvexPolyhedron { } let normal = utils::ccw_face_normal([ - &points[idx[0] as usize], - &points[idx[1] as usize], - &points[idx[2] as usize], + points[idx[0] as usize], + points[idx[1] as usize], + points[idx[2] as usize], ]); let triangle = Triangle { vertices: *idx, edges: edges_id, - normal: normal.map(|n| *n).unwrap_or(Vector::zeros()), + normal: normal.unwrap_or(Vector::ZERO), parent_face: None, is_degenerate: normal.is_none(), }; @@ -480,7 +469,7 @@ impl ConvexPolyhedron { for e in &mut edges { let tri1 = triangles.get(e.faces[0] as usize)?; let tri2 = triangles.get(e.faces[1] as usize)?; - if tri1.normal.dot(&tri2.normal) > 1.0 - eps { + if tri1.normal.dot(tri2.normal) > 1.0 - eps { e.deleted = true; } } @@ -497,7 +486,7 @@ impl ConvexPolyhedron { let mut new_face = Face { first_vertex_or_edge: edges_adj_to_face.len() as u32, num_vertices_or_edges: 1, - normal: Unit::new_unchecked(triangles[i].normal), + normal: triangles[i].normal, }; edges_adj_to_face.push(triangles[i].edges[j1]); @@ -646,7 +635,7 @@ impl ConvexPolyhedron { self.points[self.vertices_adj_to_face[face.first_vertex_or_edge as usize] as usize]; for v in &self.points { - assert!((v - p0).dot(face.normal.as_ref()) <= crate::math::DEFAULT_EPSILON); + assert!((v - p0).dot(face.normal) <= crate::math::DEFAULT_EPSILON); } } } @@ -660,24 +649,24 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.5, 1.0, 0.0), - /// Point3::new(0.5, 0.5, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.5, 1.0, 0.0), + /// Vector::new(0.5, 0.5, 1.0), /// ]; /// let indices = vec![[0u32, 1, 2], [0, 1, 3], [1, 2, 3], [2, 0, 3]]; /// /// let tetrahedron = ConvexPolyhedron::from_convex_mesh(points, &indices).unwrap(); /// /// assert_eq!(tetrahedron.points().len(), 4); - /// assert_eq!(tetrahedron.points()[0], Point3::origin()); + /// assert_eq!(tetrahedron.points()[0], Vector::ZERO); /// # } /// ``` #[inline] - pub fn points(&self) -> &[Point] { + pub fn points(&self) -> &[Vector] { &self.points[..] } @@ -709,13 +698,13 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.5, 1.0, 0.0), - /// Point3::new(0.5, 0.5, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.5, 1.0, 0.0), + /// Vector::new(0.5, 0.5, 1.0), /// ]; /// let indices = vec![[0u32, 1, 2], [0, 1, 3], [1, 2, 3], [2, 0, 3]]; /// @@ -749,14 +738,14 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a cube (8 vertices, 12 triangular input faces) /// let vertices = vec![ - /// Point3::origin(), Point3::new(1.0, 0.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), Point3::new(0.0, 1.0, 0.0), - /// Point3::new(0.0, 0.0, 1.0), Point3::new(1.0, 0.0, 1.0), - /// Point3::new(1.0, 1.0, 1.0), Point3::new(0.0, 1.0, 1.0), + /// Vector::ZERO, Vector::new(1.0, 0.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), Vector::new(1.0, 0.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), Vector::new(0.0, 1.0, 1.0), /// ]; /// let indices = vec![ /// [0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7], @@ -815,25 +804,25 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.5, 1.0, 0.0), - /// Point3::new(0.5, 0.5, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.5, 1.0, 0.0), + /// Vector::new(0.5, 0.5, 1.0), /// ]; /// let indices = vec![[0u32, 1, 2], [0, 1, 3], [1, 2, 3], [2, 0, 3]]; /// /// let tetrahedron = ConvexPolyhedron::from_convex_mesh(points, &indices).unwrap(); /// /// // Scale uniformly by 2x - /// let scaled = tetrahedron.scaled(&Vector3::new(2.0, 2.0, 2.0)) + /// let scaled = tetrahedron.scaled(Vector::new(2.0, 2.0, 2.0)) /// .expect("Failed to scale"); /// /// // All coordinates are doubled - /// assert_eq!(scaled.points()[1], Point3::new(2.0, 0.0, 0.0)); - /// assert_eq!(scaled.points()[3], Point3::new(1.0, 1.0, 2.0)); + /// assert_eq!(scaled.points()[1], Vector::new(2.0, 0.0, 0.0)); + /// assert_eq!(scaled.points()[3], Vector::new(1.0, 1.0, 2.0)); /// # } /// ``` /// @@ -842,14 +831,14 @@ impl ConvexPolyhedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::ConvexPolyhedron; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// // Start with a unit cube /// let vertices = vec![ - /// Point3::origin(), Point3::new(1.0, 0.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), Point3::new(0.0, 1.0, 0.0), - /// Point3::new(0.0, 0.0, 1.0), Point3::new(1.0, 0.0, 1.0), - /// Point3::new(1.0, 1.0, 1.0), Point3::new(0.0, 1.0, 1.0), + /// Vector::ZERO, Vector::new(1.0, 0.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), Vector::new(1.0, 0.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), Vector::new(0.0, 1.0, 1.0), /// ]; /// let indices = vec![ /// [0, 2, 1], [0, 3, 2], [4, 5, 6], [4, 6, 7], @@ -860,35 +849,29 @@ impl ConvexPolyhedron { /// let cube = ConvexPolyhedron::from_convex_mesh(vertices, &indices).unwrap(); /// /// // Scale to make it wider (3x), deeper (2x), and taller (4x) - /// let box_shape = cube.scaled(&Vector3::new(3.0, 2.0, 4.0)) + /// let box_shape = cube.scaled(Vector::new(3.0, 2.0, 4.0)) /// .expect("Failed to scale"); /// - /// assert_eq!(box_shape.points()[6], Point3::new(3.0, 2.0, 4.0)); + /// assert_eq!(box_shape.points()[6], Vector::new(3.0, 2.0, 4.0)); /// # } /// ``` - pub fn scaled(mut self, scale: &Vector) -> Option { - self.points - .iter_mut() - .for_each(|pt| pt.coords.component_mul_assign(scale)); + pub fn scaled(mut self, scale: Vector) -> Option { + self.points.iter_mut().for_each(|pt| *pt *= scale); for f in &mut self.faces { - f.normal = Unit::try_new(f.normal.component_mul(scale), 0.0).unwrap_or(f.normal); + f.normal = (f.normal * scale).try_normalize().unwrap_or(f.normal); } for e in &mut self.edges { - e.dir = Unit::try_new(e.dir.component_mul(scale), 0.0).unwrap_or(e.dir); + e.dir = (e.dir * scale).try_normalize().unwrap_or(e.dir); } Some(self) } - fn support_feature_id_toward_eps( - &self, - local_dir: &Unit>, - eps: Real, - ) -> FeatureId { + fn support_feature_id_toward_eps(&self, local_dir: Vector, eps: Real) -> FeatureId { let (seps, ceps) = eps.sin_cos(); - let support_pt_id = utils::point_cloud_support_point_id(local_dir.as_ref(), &self.points); + let support_pt_id = utils::point_cloud_support_point_id(local_dir, &self.points); let vertex = &self.vertices[support_pt_id]; // Check faces. @@ -896,7 +879,7 @@ impl ConvexPolyhedron { let face_id = self.faces_adj_to_vertex[(vertex.first_adj_face_or_edge + i) as usize]; let face = &self.faces[face_id as usize]; - if face.normal.dot(local_dir.as_ref()) >= ceps { + if face.normal.dot(local_dir) >= ceps { return FeatureId::Face(face_id); } } @@ -906,7 +889,7 @@ impl ConvexPolyhedron { let edge_id = self.edges_adj_to_vertex[(vertex.first_adj_face_or_edge + i) as usize]; let edge = &self.edges[edge_id as usize]; - if edge.dir.dot(local_dir.as_ref()).abs() <= seps { + if edge.dir.dot(local_dir).abs() <= seps { return FeatureId::Edge(edge_id); } } @@ -916,33 +899,35 @@ impl ConvexPolyhedron { } /// Computes the ID of the features with a normal that maximize the dot-product with `local_dir`. - pub fn support_feature_id_toward(&self, local_dir: &Unit>) -> FeatureId { - let eps: Real = na::convert::(f64::consts::PI / 180.0); + pub fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId { + #[cfg_attr(feature = "f64", expect(clippy::unnecessary_cast))] + let eps: Real = (f64::consts::PI / 180.0) as Real; self.support_feature_id_toward_eps(local_dir, eps) } /// The normal of the given feature. - pub fn feature_normal(&self, feature: FeatureId) -> Option>> { + pub fn feature_normal(&self, feature: FeatureId) -> Option { match feature { FeatureId::Face(id) => Some(self.faces[id as usize].normal), FeatureId::Edge(id) => { let edge = &self.edges[id as usize]; - Some(Unit::new_normalize( - *self.faces[edge.faces[0] as usize].normal - + *self.faces[edge.faces[1] as usize].normal, - )) + Some( + (self.faces[edge.faces[0] as usize].normal + + self.faces[edge.faces[1] as usize].normal) + .normalize(), + ) } FeatureId::Vertex(id) => { let vertex = &self.vertices[id as usize]; let first = vertex.first_adj_face_or_edge; let last = vertex.first_adj_face_or_edge + vertex.num_adj_faces_or_edge; - let mut normal = Vector::zeros(); + let mut normal = Vector::ZERO; for face in &self.faces_adj_to_vertex[first as usize..last as usize] { - normal += *self.faces[*face as usize].normal + normal += self.faces[*face as usize].normal } - Some(Unit::new_normalize(normal)) + Some((normal).normalize()) } FeatureId::Unknown => None, } @@ -951,13 +936,13 @@ impl ConvexPolyhedron { impl SupportMap for ConvexPolyhedron { #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { + fn local_support_point(&self, dir: Vector) -> Vector { utils::point_cloud_support_point(dir, self.points()) } } impl PolygonalFeatureMap for ConvexPolyhedron { - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolygonalFeature) { + fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) { let mut best_fid = 0; let mut best_dot = self.faces[0].normal.dot(dir); @@ -997,11 +982,11 @@ impl PolygonalFeatureMap for ConvexPolyhedron { /* impl ConvexPolyhedron for ConvexPolyhedron { - fn vertex(&self, id: FeatureId) -> Point { + fn vertex(&self, id: FeatureId) -> Vector { self.points[id.unwrap_vertex() as usize] } - fn edge(&self, id: FeatureId) -> (Point, Point, FeatureId, FeatureId) { + fn edge(&self, id: FeatureId) -> (Vector, Vector, FeatureId, FeatureId) { let edge = &self.edges[id.unwrap_edge() as usize]; let v1 = edge.vertices[0]; let v2 = edge.vertices[1]; @@ -1035,17 +1020,17 @@ impl ConvexPolyhedron for ConvexPolyhedron { fn support_face_toward( &self, - m: &Isometry, - dir: &Unit>, + m: &Pose, + dir: Vector, out: &mut ConvexPolygonalFeature, ) { let ls_dir = m.inverse_transform_vector(dir); let mut best_face = 0; - let mut max_dot = self.faces[0].normal.dot(&ls_dir); + let mut max_dot = self.faces[0].normal.dot(ls_dir); for i in 1..self.faces.len() { let face = &self.faces[i]; - let dot = face.normal.dot(&ls_dir); + let dot = face.normal.dot(ls_dir); if dot > max_dot { max_dot = dot; @@ -1059,8 +1044,8 @@ impl ConvexPolyhedron for ConvexPolyhedron { fn support_feature_toward( &self, - transform: &Isometry, - dir: &Unit>, + transform: &Pose, + dir: Vector, angle: Real, out: &mut ConvexPolygonalFeature, ) { diff --git a/src/shape/cuboid.rs b/src/shape/cuboid.rs index 8646cbdc..47412de7 100644 --- a/src/shape/cuboid.rs +++ b/src/shape/cuboid.rs @@ -1,14 +1,12 @@ //! Support mapping based Cuboid shape. -use crate::math::{Point, Real, Vector}; +#[cfg(feature = "dim3")] +use crate::math::Real; +use crate::math::Vector; #[cfg(feature = "dim3")] use crate::shape::Segment; use crate::shape::{FeatureId, PackedFeatureId, PolygonalFeature, SupportMap}; use crate::utils::WSign; -use na::Unit; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// A cuboid shape, also known as a box or rectangle. /// @@ -42,11 +40,11 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Cuboid; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// /// // Create a box that is 4 units wide, 2 units tall, and 6 units deep /// // (half-extents are half of each dimension) -/// let cuboid = Cuboid::new(Vector3::new(2.0, 1.0, 3.0)); +/// let cuboid = Cuboid::new(Vector::new(2.0, 1.0, 3.0)); /// /// assert_eq!(cuboid.half_extents.x, 2.0); /// assert_eq!(cuboid.half_extents.y, 1.0); @@ -61,8 +59,7 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "encase", derive(encase::ShaderType))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] @@ -75,7 +72,7 @@ pub struct Cuboid { /// - `half_extents.z`: Half the depth (3D only) /// /// All components should be positive. - pub half_extents: Vector, + pub half_extents: Vector, } impl Cuboid { @@ -93,10 +90,10 @@ impl Cuboid { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Cuboid; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// /// // Create a 10x6x4 box (full dimensions) - /// let cuboid = Cuboid::new(Vector3::new(5.0, 3.0, 2.0)); + /// let cuboid = Cuboid::new(Vector::new(5.0, 3.0, 2.0)); /// /// // Verify the half-extents /// assert_eq!(cuboid.half_extents.x, 5.0); @@ -109,16 +106,16 @@ impl Cuboid { /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// // In 2D: /// use parry2d::shape::Cuboid; - /// use nalgebra::Vector2; + /// use parry2d::math::Vector; /// /// // Create a 20x10 rectangle - /// let rect = Cuboid::new(Vector2::new(10.0, 5.0)); + /// let rect = Cuboid::new(Vector::new(10.0, 5.0)); /// assert_eq!(rect.half_extents.x, 10.0); /// assert_eq!(rect.half_extents.y, 5.0); /// # } /// ``` #[inline] - pub fn new(half_extents: Vector) -> Cuboid { + pub fn new(half_extents: Vector) -> Cuboid { Cuboid { half_extents } } @@ -141,21 +138,21 @@ impl Cuboid { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Cuboid; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// - /// let cuboid = Cuboid::new(Vector3::new(1.0, 2.0, 3.0)); + /// let cuboid = Cuboid::new(Vector::new(1.0, 2.0, 3.0)); /// /// // Uniform scaling: double all dimensions - /// let scaled_uniform = cuboid.scaled(&Vector3::new(2.0, 2.0, 2.0)); - /// assert_eq!(scaled_uniform.half_extents, Vector3::new(2.0, 4.0, 6.0)); + /// let scaled_uniform = cuboid.scaled(Vector::new(2.0, 2.0, 2.0)); + /// assert_eq!(scaled_uniform.half_extents, Vector::new(2.0, 4.0, 6.0)); /// /// // Non-uniform scaling: different scale per axis - /// let scaled_non_uniform = cuboid.scaled(&Vector3::new(2.0, 1.0, 0.5)); - /// assert_eq!(scaled_non_uniform.half_extents, Vector3::new(2.0, 2.0, 1.5)); + /// let scaled_non_uniform = cuboid.scaled(Vector::new(2.0, 1.0, 0.5)); + /// assert_eq!(scaled_non_uniform.half_extents, Vector::new(2.0, 2.0, 1.5)); /// # } /// ``` - pub fn scaled(self, scale: &Vector) -> Self { - let new_hext = self.half_extents.component_mul(scale); + pub fn scaled(self, scale: Vector) -> Self { + let new_hext = self.half_extents * scale; Self { half_extents: new_hext, } @@ -164,7 +161,7 @@ impl Cuboid { /// Return the id of the vertex of this cuboid with a normal that maximizes /// the dot product with `dir`. #[cfg(feature = "dim2")] - pub fn vertex_feature_id(vertex: Point) -> u32 { + pub fn vertex_feature_id(vertex: Vector) -> u32 { // TODO: is this still correct with the f64 version? #[allow(clippy::unnecessary_cast)] // Unnecessary for f32 but necessary for f64. { @@ -175,7 +172,7 @@ impl Cuboid { /// Return the feature of this cuboid with a normal that maximizes /// the dot product with `dir`. #[cfg(feature = "dim2")] - pub fn support_feature(&self, local_dir: Vector) -> PolygonalFeature { + pub fn support_feature(&self, local_dir: Vector) -> PolygonalFeature { // In 2D, it is best for stability to always return a face. // It won't have any notable impact on performances anyway. self.support_face(local_dir) @@ -184,11 +181,11 @@ impl Cuboid { /// Return the face of this cuboid with a normal that maximizes /// the dot product with `local_dir`. #[cfg(feature = "dim2")] - pub fn support_face(&self, local_dir: Vector) -> PolygonalFeature { + pub fn support_face(&self, local_dir: Vector) -> PolygonalFeature { let he = self.half_extents; - let i = local_dir.iamin(); + let i = local_dir.abs().min_position(); let j = (i + 1) % 2; - let mut a = Point::origin(); + let mut a = Vector::ZERO; a[i] = he[i]; a[j] = he[j].copysign(local_dir[j]); @@ -210,7 +207,7 @@ impl Cuboid { /// Return the face of this cuboid with a normal that maximizes /// the dot product with `local_dir`. #[cfg(feature = "dim3")] - pub fn support_feature(&self, local_dir: Vector) -> PolygonalFeature { + pub fn support_feature(&self, local_dir: Vector) -> PolygonalFeature { // TODO: this should actually return the feature. // And we should change all the callers of this method to use // `.support_face` instead of this method to preserve their old behavior. @@ -236,7 +233,7 @@ impl Cuboid { } // #[cfg(feature = "dim3") - // pub(crate) fn support_vertex(&self, local_dir: Vector) -> CuboidFeatureVertex { + // pub(crate) fn support_vertex(&self, local_dir: Vector) -> CuboidFeatureVertex { // let vertex = local_support_point(self, local_dir); // let vid = vertex_feature_id(vertex); // @@ -246,12 +243,12 @@ impl Cuboid { /// Return the edge segment of this cuboid with a normal cone containing /// a direction that that maximizes the dot product with `local_dir`. #[cfg(feature = "dim3")] - pub fn local_support_edge_segment(&self, local_dir: Vector) -> Segment { + pub fn local_support_edge_segment(&self, local_dir: Vector) -> Segment { let he = self.half_extents; - let i = local_dir.iamin(); + let i = local_dir.abs().min_position(); let j = (i + 1) % 3; let k = (i + 2) % 3; - let mut a = Point::origin(); + let mut a = Vector::ZERO; a[i] = he[i]; a[j] = he[j].copysign(local_dir[j]); a[k] = he[k].copysign(local_dir[k]); @@ -264,32 +261,32 @@ impl Cuboid { /// Computes the face with a normal that maximizes the dot-product with `local_dir`. #[cfg(feature = "dim3")] - pub fn support_face(&self, local_dir: Vector) -> PolygonalFeature { + pub fn support_face(&self, local_dir: Vector) -> PolygonalFeature { // NOTE: can we use the orthonormal basis of local_dir // to make this AoSoA friendly? let he = self.half_extents; - let iamax = local_dir.iamax(); + let imax = local_dir.abs().max_position(); #[expect(clippy::unnecessary_cast)] - let sign = (1.0 as Real).copysign(local_dir[iamax]); + let sign = (1.0 as Real).copysign(local_dir[imax]); - let vertices = match iamax { + let vertices = match imax { 0 => [ - Point::new(he.x * sign, he.y, he.z), - Point::new(he.x * sign, -he.y, he.z), - Point::new(he.x * sign, -he.y, -he.z), - Point::new(he.x * sign, he.y, -he.z), + Vector::new(he.x * sign, he.y, he.z), + Vector::new(he.x * sign, -he.y, he.z), + Vector::new(he.x * sign, -he.y, -he.z), + Vector::new(he.x * sign, he.y, -he.z), ], 1 => [ - Point::new(he.x, he.y * sign, he.z), - Point::new(-he.x, he.y * sign, he.z), - Point::new(-he.x, he.y * sign, -he.z), - Point::new(he.x, he.y * sign, -he.z), + Vector::new(he.x, he.y * sign, he.z), + Vector::new(-he.x, he.y * sign, he.z), + Vector::new(-he.x, he.y * sign, -he.z), + Vector::new(he.x, he.y * sign, -he.z), ], 2 => [ - Point::new(he.x, he.y, he.z * sign), - Point::new(he.x, -he.y, he.z * sign), - Point::new(-he.x, -he.y, he.z * sign), - Point::new(-he.x, he.y, he.z * sign), + Vector::new(he.x, he.y, he.z * sign), + Vector::new(he.x, -he.y, he.z * sign), + Vector::new(-he.x, -he.y, he.z * sign), + Vector::new(-he.x, he.y, he.z * sign), ], _ => unreachable!(), }; @@ -304,7 +301,7 @@ impl Cuboid { // component. A + sign means the corresponding bit is 0 while a - // sign means the corresponding bit is 1. // For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011 - let vids = match iamax { + let vids = match imax { 0 => [ [vid(0b000), vid(0b010), vid(0b011), vid(0b001)], [vid(0b100), vid(0b110), vid(0b111), vid(0b101)], @@ -324,7 +321,7 @@ impl Cuboid { // of their endpoints. // Assuming vid1 > vid2, we do: (vid1 << 3) | vid2 | 0b11000000 // - let eids = match iamax { + let eids = match imax { 0 => [ [0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000], [0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100], @@ -342,7 +339,7 @@ impl Cuboid { // The face with normals [x, y, z] are numbered [10, 11, 12]. // The face with negated normals are numbered [13, 14, 15]. - let fid = iamax + sign_index * 3 + 10; + let fid = imax + sign_index * 3 + 10; PolygonalFeature { vertices, @@ -355,20 +352,20 @@ impl Cuboid { /// The normal of the given feature of this shape. #[cfg(feature = "dim2")] - pub fn feature_normal(&self, feature: FeatureId) -> Option>> { + pub fn feature_normal(&self, feature: FeatureId) -> Option { match feature { FeatureId::Face(id) => { - let mut dir: Vector = na::zero(); + let mut dir: Vector = Vector::ZERO; if id < 2 { dir[id as usize] = 1.0; } else { dir[id as usize - 2] = -1.0; } - Some(Unit::new_unchecked(dir)) + Some(dir) } FeatureId::Vertex(id) => { - let mut dir: Vector = na::zero(); + let mut dir: Vector = Vector::ZERO; match id { 0b00 => { @@ -390,7 +387,7 @@ impl Cuboid { _ => return None, } - Some(Unit::new_normalize(dir)) + Some(dir.normalize()) } _ => None, } @@ -398,17 +395,17 @@ impl Cuboid { /// The normal of the given feature of this shape. #[cfg(feature = "dim3")] - pub fn feature_normal(&self, feature: FeatureId) -> Option>> { + pub fn feature_normal(&self, feature: FeatureId) -> Option { match feature { FeatureId::Face(id) => { - let mut dir: Vector = na::zero(); + let mut dir: Vector = Vector::ZERO; if id < 3 { dir[id as usize] = 1.0; } else { dir[id as usize - 3] = -1.0; } - Some(Unit::new_unchecked(dir)) + Some(dir) } FeatureId::Edge(id) => { let edge = id & 0b011; @@ -416,7 +413,7 @@ impl Cuboid { let face2 = (edge + 2) % 3; let signs = id >> 2; - let mut dir: Vector = na::zero(); + let mut dir: Vector = Vector::ZERO; if signs & (1 << face1) != 0 { dir[face1 as usize] = -1.0 @@ -430,10 +427,10 @@ impl Cuboid { dir[face2 as usize] = 1.0; } - Some(Unit::new_normalize(dir)) + Some(dir.normalize()) } FeatureId::Vertex(id) => { - let mut dir: Vector = na::zero(); + let mut dir: Vector = Vector::ZERO; for i in 0..3 { if id & (1 << i) != 0 { dir[i] = -1.0; @@ -442,7 +439,7 @@ impl Cuboid { } } - Some(Unit::new_normalize(dir)) + Some(dir.normalize()) } _ => None, } @@ -451,14 +448,14 @@ impl Cuboid { impl SupportMap for Cuboid { #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { - dir.copy_sign_to(self.half_extents).into() + fn local_support_point(&self, dir: Vector) -> Vector { + dir.copy_sign_to(self.half_extents) } } /* impl ConvexPolyhedron for Cuboid { - fn vertex(&self, id: FeatureId) -> Point { + fn vertex(&self, id: FeatureId) -> Vector { let vid = id.unwrap_vertex(); let mut res = self.half_extents; @@ -468,11 +465,11 @@ impl ConvexPolyhedron for Cuboid { } } - Point::from(res) + Vector::from(res) } #[cfg(feature = "dim3")] - fn edge(&self, id: FeatureId) -> (Point, Point, FeatureId, FeatureId) { + fn edge(&self, id: FeatureId) -> (Vector, Vector, FeatureId, FeatureId) { let eid = id.unwrap_edge(); let mut res = self.half_extents; @@ -485,9 +482,9 @@ impl ConvexPolyhedron for Cuboid { } } - let p1 = Point::from(res); + let p1 = Vector::from(res); res[edge_i as usize] = -res[edge_i as usize]; - let p2 = Point::from(res); + let p2 = Vector::from(res); let vid1 = FeatureId::Vertex(vertex_i & !(1 << edge_i)); let vid2 = FeatureId::Vertex(vertex_i | (1 << edge_i)); @@ -517,9 +514,9 @@ impl ConvexPolyhedron for Cuboid { vertex[i1] *= sign; vertex[i2] *= if i1 == 0 { -sign } else { sign }; - let p1 = Point::from(vertex); + let p1 = Vector::from(vertex); vertex[i2] = -vertex[i2]; - let p2 = Point::from(vertex); + let p2 = Vector::from(vertex); let mut vertex_id1 = if sign < 0.0 { 1 << i1 @@ -536,9 +533,9 @@ impl ConvexPolyhedron for Cuboid { out.push(p1, FeatureId::Vertex(vertex_id1)); out.push(p2, FeatureId::Vertex(vertex_id2)); - let mut normal: Vector = na::zero(); + let mut normal: Vector = Vector::ZERO; normal[i1] = sign; - out.set_normal(Unit::new_unchecked(normal)); + out.set_normal(normal); out.set_feature_id(FeatureId::Face(i as u32)); } #[cfg(feature = "dim3")] @@ -561,7 +558,7 @@ impl ConvexPolyhedron for Cuboid { (0, 1) }; let mut vertex_id = sbit << i1; - out.push(Point::from(vertex), FeatureId::Vertex(vertex_id)); + out.push(Vector::from(vertex), FeatureId::Vertex(vertex_id)); out.push_edge_feature_id(FeatureId::Edge( edge_i2 as u32 | ((vertex_id & mask_i2) << 2), )); @@ -569,7 +566,7 @@ impl ConvexPolyhedron for Cuboid { vertex[i2] = -sign * self.half_extents[i2]; vertex[i3] = sign * self.half_extents[i3]; vertex_id |= msbit << i2 | sbit << i3; - out.push(Point::from(vertex), FeatureId::Vertex(vertex_id)); + out.push(Vector::from(vertex), FeatureId::Vertex(vertex_id)); out.push_edge_feature_id(FeatureId::Edge( edge_i3 as u32 | ((vertex_id & mask_i3) << 2), )); @@ -577,7 +574,7 @@ impl ConvexPolyhedron for Cuboid { vertex[i2] = -self.half_extents[i2]; vertex[i3] = -self.half_extents[i3]; vertex_id |= 1 << i2 | 1 << i3; - out.push(Point::from(vertex), FeatureId::Vertex(vertex_id)); + out.push(Vector::from(vertex), FeatureId::Vertex(vertex_id)); out.push_edge_feature_id(FeatureId::Edge( edge_i2 as u32 | ((vertex_id & mask_i2) << 2), )); @@ -585,14 +582,14 @@ impl ConvexPolyhedron for Cuboid { vertex[i2] = sign * self.half_extents[i2]; vertex[i3] = -sign * self.half_extents[i3]; vertex_id = sbit << i1 | sbit << i2 | msbit << i3; - out.push(Point::from(vertex), FeatureId::Vertex(vertex_id)); + out.push(Vector::from(vertex), FeatureId::Vertex(vertex_id)); out.push_edge_feature_id(FeatureId::Edge( edge_i3 as u32 | ((vertex_id & mask_i3) << 2), )); - let mut normal: Vector = na::zero(); + let mut normal: Vector = Vector::ZERO; normal[i1] = sign; - out.set_normal(Unit::new_unchecked(normal)); + out.set_normal(normal); if sign > 0.0 { out.set_feature_id(FeatureId::Face(i1 as u32)); @@ -606,43 +603,32 @@ impl ConvexPolyhedron for Cuboid { fn support_face_toward( &self, - m: &Isometry, - dir: &Unit>, + m: &Pose, + dir: Vector, out: &mut ConvexPolygonalFeature, ) { out.clear(); let local_dir = m.inverse_transform_vector(dir); + let imax = iamax(local_dir); - let mut iamax = 0; - let mut amax = local_dir[0].abs(); - - // TODO: we should use nalgebra's iamax method. - for i in 1..DIM { - let candidate = local_dir[i].abs(); - if candidate > amax { - amax = candidate; - iamax = i; - } - } - - if local_dir[iamax] > 0.0 { - self.face(FeatureId::Face(iamax as u32), out); + if local_dir[imax] > 0.0 { + self.face(FeatureId::Face(imax as u32), out); out.transform_by(m); } else { - self.face(FeatureId::Face((iamax + DIM) as u32), out); + self.face(FeatureId::Face((imax + DIM) as u32), out); out.transform_by(m); } } fn support_feature_toward( &self, - m: &Isometry, - dir: &Unit>, + m: &Pose, + dir: Vector, angle: Real, out: &mut ConvexPolygonalFeature, ) { let local_dir = m.inverse_transform_vector(dir); - let cang = ComplexField::cos(angle); + let cang = ::cos(angle); let mut support_point = self.half_extents; out.clear(); @@ -671,7 +657,7 @@ impl ConvexPolyhedron for Cuboid { // We are not on a face, return the support vertex. out.push( - m * Point::from(support_point), + m * Vector::from(support_point), FeatureId::Vertex(support_point_id), ); out.set_feature_id(FeatureId::Vertex(support_point_id)); @@ -679,7 +665,7 @@ impl ConvexPolyhedron for Cuboid { #[cfg(feature = "dim3")] { - let sang = ComplexField::sin(angle); + let sang = ::sin(angle); let mut support_point_id = 0; // Check faces. @@ -709,9 +695,9 @@ impl ConvexPolyhedron for Cuboid { // sign * local_dir[i] <= cos(pi / 2 - angle) if sign * local_dir[i] <= sang { support_point[i] = -self.half_extents[i]; - let p1 = Point::from(support_point); + let p1 = Vector::from(support_point); support_point[i] = self.half_extents[i]; - let p2 = Point::from(support_point); + let p2 = Vector::from(support_point); let p2_id = support_point_id & !(1 << i); out.push(m * p1, FeatureId::Vertex(support_point_id | (1 << i))); out.push(m * p2, FeatureId::Vertex(p2_id)); @@ -725,16 +711,16 @@ impl ConvexPolyhedron for Cuboid { // We are not on a face or edge, return the support vertex. out.push( - m * Point::from(support_point), + m * Vector::from(support_point), FeatureId::Vertex(support_point_id), ); out.set_feature_id(FeatureId::Vertex(support_point_id)); } } - fn support_feature_id_toward(&self, local_dir: &Unit>) -> FeatureId { - let one_degree: Real = na::convert::(f64::consts::PI / 180.0); - let cang = ComplexField::cos(one_degree); + fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId { + let one_degree: Real = (f64::consts::PI / 180.0) as Real; + let cang = ::cos(one_degree); #[cfg(feature = "dim2")] { @@ -760,7 +746,7 @@ impl ConvexPolyhedron for Cuboid { #[cfg(feature = "dim3")] { - let sang = ComplexField::sin(one_degree); + let sang = ::sin(one_degree); let mut support_point_id = 0; // Check faces. diff --git a/src/shape/cylinder.rs b/src/shape/cylinder.rs index 7aa9136d..5a5c5160 100644 --- a/src/shape/cylinder.rs +++ b/src/shape/cylinder.rs @@ -1,16 +1,11 @@ //! Support mapping based Cylinder shape. -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::shape::SupportMap; -use na; -use num::Zero; #[cfg(feature = "alloc")] use either::Either; -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; - /// A 3D cylinder shape with axis aligned along the Y axis. /// /// A cylinder is a shape with circular cross-sections perpendicular to its axis. @@ -69,8 +64,7 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "encase", derive(encase::ShaderType))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] @@ -150,28 +144,28 @@ impl Cylinder { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32", feature = "alloc"))] { /// use parry3d::shape::Cylinder; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// use either::Either; /// /// let cylinder = Cylinder::new(2.0, 1.0); /// /// // Uniform scaling: produces a larger cylinder - /// let scale1 = Vector3::new(2.0, 2.0, 2.0); - /// if let Some(Either::Left(scaled)) = cylinder.scaled(&scale1, 20) { + /// let scale1 = Vector::splat(2.0); + /// if let Some(Either::Left(scaled)) = cylinder.scaled(scale1, 20) { /// assert_eq!(scaled.radius, 2.0); // 1.0 * 2.0 /// assert_eq!(scaled.half_height, 4.0); // 2.0 * 2.0 /// } /// /// // Different Y scale: still a cylinder - /// let scale2 = Vector3::new(1.5, 3.0, 1.5); - /// if let Some(Either::Left(scaled)) = cylinder.scaled(&scale2, 20) { + /// let scale2 = Vector::new(1.5, 3.0, 1.5); + /// if let Some(Either::Left(scaled)) = cylinder.scaled(scale2, 20) { /// assert_eq!(scaled.radius, 1.5); // 1.0 * 1.5 /// assert_eq!(scaled.half_height, 6.0); // 2.0 * 3.0 /// } /// /// // Non-uniform X/Z: produces elliptical cylinder (mesh approximation) - /// let scale3 = Vector3::new(2.0, 1.0, 1.0); - /// if let Some(Either::Right(polyhedron)) = cylinder.scaled(&scale3, 20) { + /// let scale3 = Vector::new(2.0, 1.0, 1.0); + /// if let Some(Either::Right(polyhedron)) = cylinder.scaled(scale3, 20) { /// // Result is a convex mesh approximating an elliptical cylinder /// assert!(polyhedron.points().len() > 0); /// } @@ -181,14 +175,13 @@ impl Cylinder { #[inline] pub fn scaled( self, - scale: &Vector, + scale: Vector, nsubdivs: u32, ) -> Option> { if scale.x != scale.z { - // The scaled shape isn’t a cylinder. + // The scaled shape isn't a cylinder. let (mut vtx, idx) = self.to_trimesh(nsubdivs); - vtx.iter_mut() - .for_each(|pt| pt.coords = pt.coords.component_mul(scale)); + vtx.iter_mut().for_each(|pt| *pt *= scale); Some(Either::Right(super::ConvexPolyhedron::from_convex_mesh( vtx, &idx, )?)) @@ -202,19 +195,11 @@ impl Cylinder { } impl SupportMap for Cylinder { - fn local_support_point(&self, dir: &Vector) -> Point { - let mut vres = *dir; - + fn local_support_point(&self, dir: Vector) -> Vector { + let mut vres = dir; vres[1] = 0.0; - - if vres.normalize_mut().is_zero() { - vres = na::zero() - } else { - vres *= self.radius; - } - + vres = vres.normalize_or_zero() * self.radius; vres[1] = self.half_height.copysign(dir[1]); - - Point::from(vres) + vres } } diff --git a/src/shape/feature_id.rs b/src/shape/feature_id.rs index a025ff46..effd10a6 100644 --- a/src/shape/feature_id.rs +++ b/src/shape/feature_id.rs @@ -1,6 +1,3 @@ -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; - /// An identifier of a geometric feature (vertex, edge, or face) of a shape. /// /// Feature IDs are used throughout Parry to identify specific geometric features on shapes @@ -110,8 +107,7 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Copy, Clone, Debug, Hash, PartialEq, Eq, Default)] pub enum FeatureId { @@ -439,8 +435,7 @@ impl FeatureId { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] pub struct PackedFeatureId(pub u32); diff --git a/src/shape/half_space.rs b/src/shape/half_space.rs index b129bbd6..daf746fb 100644 --- a/src/shape/half_space.rs +++ b/src/shape/half_space.rs @@ -1,9 +1,5 @@ //! Support mapping based HalfSpace shape. -use crate::math::{Real, Vector}; -use na::Unit; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; +use crate::math::Vector; /// A half-space delimited by an infinite plane. /// @@ -33,7 +29,7 @@ use rkyv::{bytecheck, CheckBytes}; /// /// The plane always passes through the origin `(0, 0)` in 2D or `(0, 0, 0)` in 3D of the /// half-space's local coordinate system. To position the plane elsewhere in your world, -/// use an [`Isometry`](na::Isometry) transformation when performing queries. +/// use a [`Pose`](crate::math::Pose) transformation when performing queries. /// /// # Examples /// @@ -42,10 +38,10 @@ use rkyv::{bytecheck, CheckBytes}; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; -/// use parry3d::na::{Vector3, Unit}; +/// use parry3d::math::{Vector}; /// /// // Create a horizontal ground plane with normal pointing up (positive Y-axis) -/// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); +/// let ground = HalfSpace::new((Vector::Y.normalize())); /// /// // The ground plane is at Y = 0 in local coordinates /// // Everything below (negative Y) is "inside" the half-space @@ -57,10 +53,10 @@ use rkyv::{bytecheck, CheckBytes}; /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::HalfSpace; -/// use parry2d::na::{Vector2, Unit}; +/// use parry2d::math::Vector; /// /// // Create a vertical wall with normal pointing right (positive X-axis) -/// let wall = HalfSpace::new(Unit::new_normalize(Vector2::x())); +/// let wall = HalfSpace::new(Vector::X.normalize()); /// /// // The wall is at X = 0 in local coordinates /// // Everything to the left (negative X) is "inside" the half-space @@ -73,16 +69,16 @@ use rkyv::{bytecheck, CheckBytes}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{HalfSpace, Ball}; /// use parry3d::query; -/// use parry3d::na::{Isometry3, Vector3, Unit}; +/// use parry3d::math::{Pose, Vector}; /// /// // Create a ground plane at Y = 0, normal pointing up -/// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); -/// let ground_pos = Isometry3::identity(); +/// let ground = HalfSpace::new((Vector::Y.normalize())); +/// let ground_pos = Pose::identity(); /// /// // Create a ball with radius 1.0 at position (0, 0.5, 0) /// // The ball is resting on the ground, just touching it /// let ball = Ball::new(1.0); -/// let ball_pos = Isometry3::translation(0.0, 0.5, 0.0); +/// let ball_pos = Pose::translation(0.0, 0.5, 0.0); /// /// // Check if they're in contact (with a small prediction distance) /// let contact = query::contact( @@ -103,19 +99,19 @@ use rkyv::{bytecheck, CheckBytes}; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; /// use parry3d::query::{PointQuery}; -/// use parry3d::na::{Isometry3, Vector3, Point3, Unit}; +/// use parry3d::math::{Pose, Vector}; /// /// // Create a ground plane with normal pointing up -/// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); +/// let ground = HalfSpace::new((Vector::Y.normalize())); /// /// // Position the plane at Y = 5.0 using an isometry -/// let ground_pos = Isometry3::translation(0.0, 5.0, 0.0); +/// let ground_pos = Pose::translation(0.0, 5.0, 0.0); /// /// // Check if a point is below the ground (inside the half-space) -/// let point = Point3::new(0.0, 3.0, 0.0); // Point at Y = 3.0 (below the plane) +/// let point = Vector::new(0.0, 3.0, 0.0); // Vector at Y = 3.0 (below the plane) /// /// // Project the point onto the ground plane -/// let proj = ground.project_point(&ground_pos, &point, true); +/// let proj = ground.project_point(&ground_pos, point, true); /// /// // The point is below the ground (inside the half-space) /// assert!(proj.is_inside); @@ -127,12 +123,12 @@ use rkyv::{bytecheck, CheckBytes}; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; -/// use parry3d::na::{Vector3, Unit}; +/// use parry3d::math::{Vector}; /// /// // Create a plane tilted at 45 degrees /// // Normal points up and to the right -/// let normal = Vector3::new(1.0, 1.0, 0.0); -/// let tilted_plane = HalfSpace::new(Unit::new_normalize(normal)); +/// let normal = Vector::new(1.0, 1.0, 0.0); +/// let tilted_plane = HalfSpace::new((normal).normalize()); /// /// // This plane passes through the origin and divides space diagonally /// # } @@ -141,8 +137,7 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct HalfSpace { @@ -157,15 +152,15 @@ pub struct HalfSpace { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Vector}; /// - /// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); + /// let ground = HalfSpace::new(Vector::Y.normalize()); /// /// // The normal points up (positive Y direction) - /// assert_eq!(*ground.normal, Vector3::y()); + /// assert_eq!(ground.normal, Vector::Y); /// # } /// ``` - pub normal: Unit>, + pub normal: Vector, } impl HalfSpace { @@ -178,8 +173,7 @@ impl HalfSpace { /// # Parameters /// /// * `normal` - A unit vector defining the plane's outward normal direction. This must - /// be a normalized vector (use [`Unit::new_normalize`](na::Unit::new_normalize) to - /// create one from any vector). + /// be a normalized vector (use `.normalize()` on any vector to create one). /// /// # Examples /// @@ -188,10 +182,10 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Vector}; /// /// // Ground plane with normal pointing up - /// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); + /// let ground = HalfSpace::new((Vector::Y.normalize())); /// # } /// ``` /// @@ -200,10 +194,10 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::HalfSpace; - /// use parry2d::na::{Vector2, Unit}; + /// use parry2d::math::Vector; /// /// // Wall with normal pointing to the right - /// let wall = HalfSpace::new(Unit::new_normalize(Vector2::x())); + /// let wall = HalfSpace::new(Vector::X.normalize()); /// # } /// ``` /// @@ -212,18 +206,18 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Vector}; /// /// // Plane with normal at 45-degree angle - /// let custom_normal = Vector3::new(1.0, 1.0, 0.0); - /// let plane = HalfSpace::new(Unit::new_normalize(custom_normal)); + /// let custom_normal = Vector::new(1.0, 1.0, 0.0); + /// let plane = HalfSpace::new((custom_normal).normalize()); /// /// // Verify the normal is normalized - /// assert!((plane.normal.norm() - 1.0).abs() < 1e-5); + /// assert!((plane.normal.length() - 1.0).abs() < 1e-5); /// # } /// ``` #[inline] - pub fn new(normal: Unit>) -> HalfSpace { + pub fn new(normal: Vector) -> HalfSpace { HalfSpace { normal } } @@ -236,7 +230,7 @@ impl HalfSpace { /// # Parameters /// /// * `scale` - A vector containing the scaling factors for each axis. For example, - /// `Vector3::new(2.0, 1.0, 1.0)` doubles the X-axis scaling. + /// `Vector::new(2.0, 1.0, 1.0)` doubles the X-axis scaling. /// /// # Returns /// @@ -258,13 +252,13 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Vector}; /// - /// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); + /// let ground = HalfSpace::new(Vector::Y.normalize()); /// /// // Uniform scaling doesn't change the normal direction - /// let scaled = ground.scaled(&Vector3::new(2.0, 2.0, 2.0)).unwrap(); - /// assert_eq!(*scaled.normal, Vector3::y()); + /// let scaled = ground.scaled(Vector::splat(2.0)).unwrap(); + /// assert_eq!(scaled.normal, Vector::Y); /// # } /// ``` /// @@ -273,15 +267,15 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Vector}; /// /// // Diagonal plane /// let plane = HalfSpace::new( - /// Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)) + /// (Vector::new(1.0, 1.0, 0.0).normalize()) /// ); /// /// // Scale X-axis by 2.0, Y-axis stays 1.0 - /// let scaled = plane.scaled(&Vector3::new(2.0, 1.0, 1.0)).unwrap(); + /// let scaled = plane.scaled(Vector::new(2.0, 1.0, 1.0)).unwrap(); /// /// // The normal changes direction due to non-uniform scaling /// // It's no longer at 45 degrees @@ -294,13 +288,13 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::HalfSpace; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Vector}; /// /// // Horizontal ground plane - /// let ground = HalfSpace::new(Unit::new_normalize(Vector3::y())); + /// let ground = HalfSpace::new((Vector::Y.normalize())); /// /// // Scaling Y to zero makes the normal degenerate - /// let scaled = ground.scaled(&Vector3::new(1.0, 0.0, 1.0)); + /// let scaled = ground.scaled(Vector::new(1.0, 0.0, 1.0)); /// assert!(scaled.is_none()); // Returns None because normal becomes zero /// # } /// ``` @@ -310,19 +304,20 @@ impl HalfSpace { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::HalfSpace; - /// use parry2d::na::{Vector2, Unit}; + /// use parry2d::math::Vector; /// /// // Create a wall in a 2D platformer - /// let wall = HalfSpace::new(Unit::new_normalize(Vector2::x())); + /// let wall = HalfSpace::new(Vector::X.normalize()); /// /// // Apply level scaling (e.g., for pixel-perfect rendering) - /// let pixel_scale = Vector2::new(16.0, 16.0); - /// if let Some(scaled_wall) = wall.scaled(&pixel_scale) { + /// let pixel_scale = Vector::new(16.0, 16.0); + /// if let Some(scaled_wall) = wall.scaled(pixel_scale) { /// // Use the scaled wall for collision detection /// } /// # } /// ``` - pub fn scaled(self, scale: &Vector) -> Option { - Unit::try_new(self.normal.component_mul(scale), 0.0).map(|normal| Self { normal }) + pub fn scaled(self, scale: Vector) -> Option { + let scaled = self.normal * scale; + scaled.try_normalize().map(|normal| Self { normal }) } } diff --git a/src/shape/heightfield2.rs b/src/shape/heightfield2.rs index 0c72ba5a..7e13627e 100644 --- a/src/shape/heightfield2.rs +++ b/src/shape/heightfield2.rs @@ -1,10 +1,10 @@ -use core::ops::Range; #[cfg(not(feature = "std"))] -use na::ComplexField; +use crate::math::ComplexField; #[cfg(feature = "alloc")] -use na::DVector; +use alloc::{vec, vec::Vec}; +use core::ops::Range; -use na::Point2; +use crate::math::Vector2; use crate::bounding_volume::Aabb; use crate::math::{Real, Vector}; @@ -26,34 +26,34 @@ pub type HeightFieldCellStatus = bool; #[repr(C)] /// A 2D heightfield with a generic storage buffer for its heights. pub struct HeightField { - heights: DVector, - status: DVector, + heights: Vec, + status: Vec, - scale: Vector, + scale: Vector, aabb: Aabb, } #[cfg(feature = "alloc")] impl HeightField { /// Creates a new 2D heightfield with the given heights and scale factor. - pub fn new(heights: DVector, scale: Vector) -> Self { + pub fn new(heights: Vec, scale: Vector) -> Self { assert!( heights.len() > 1, "A heightfield heights must have at least 2 elements." ); - let max = heights.max(); - let min = heights.min(); + let max = heights.iter().fold(Real::MIN, |a, b| a.max(*b)); + let min = heights.iter().fold(Real::MAX, |a, b| a.min(*b)); let hscale = scale * 0.5; let aabb = Aabb::new( - Point2::new(-hscale.x, min * scale.y), - Point2::new(hscale.x, max * scale.y), + Vector2::new(-hscale.x, min * scale.y), + Vector2::new(hscale.x, max * scale.y), ); let num_segments = heights.len() - 1; HeightField { heights, - status: DVector::repeat(num_segments, true), + status: vec![true; num_segments], scale, aabb, } @@ -67,26 +67,26 @@ impl HeightField { } /// The height at each cell endpoint. - pub fn heights(&self) -> &DVector { + pub fn heights(&self) -> &Vec { &self.heights } /// The scale factor applied to this heightfield. - pub fn scale(&self) -> &Vector { - &self.scale + pub fn scale(&self) -> Vector { + self.scale } /// Sets the scale factor applied to this heightfield. - pub fn set_scale(&mut self, new_scale: Vector) { - let ratio = new_scale.component_div(&self.scale); - self.aabb.mins.coords.component_mul_assign(&ratio); - self.aabb.maxs.coords.component_mul_assign(&ratio); + pub fn set_scale(&mut self, new_scale: Vector) { + let ratio = new_scale / self.scale; + self.aabb.mins *= ratio; + self.aabb.maxs *= ratio; self.scale = new_scale; } /// Returns a scaled version of this heightfield. - pub fn scaled(mut self, scale: &Vector) -> Self { - self.set_scale(self.scale.component_mul(scale)); + pub fn scaled(mut self, scale: Vector) -> Self { + self.set_scale(self.scale * scale); self } @@ -119,24 +119,20 @@ impl HeightField { } fn quantize_floor(&self, val: Real, seg_length: Real) -> usize { - na::clamp( - ((val + 0.5) / seg_length).floor(), - 0.0, - (self.num_cells() - 1) as Real, - ) as usize + ((val + 0.5) / seg_length) + .floor() + .clamp(0.0, (self.num_cells() - 1) as Real) as usize } fn quantize_ceil(&self, val: Real, seg_length: Real) -> usize { - na::clamp( - ((val + 0.5) / seg_length).ceil(), - 0.0, - self.num_cells() as Real, - ) as usize + ((val + 0.5) / seg_length) + .ceil() + .clamp(0.0, self.num_cells() as Real) as usize } /// Index of the cell a point is on after vertical projection. - pub fn cell_at_point(&self, pt: &Point2) -> Option { - let scaled_pt = pt.coords.component_div(&self.scale); + pub fn cell_at_point(&self, pt: Vector2) -> Option { + let scaled_pt = pt / self.scale; let seg_length = self.unit_cell_width(); if scaled_pt.x < -0.5 || scaled_pt.x > 0.5 { @@ -148,14 +144,14 @@ impl HeightField { } /// Height of the heightfield at the given point after vertical projection on the heightfield surface. - pub fn height_at_point(&self, pt: &Point2) -> Option { + pub fn height_at_point(&self, pt: Vector2) -> Option { let cell = self.cell_at_point(pt)?; let seg = self.segment_at(cell)?; let inter = crate::query::details::closest_points_line_line_parameters( - &seg.a, - &seg.scaled_direction(), + seg.a, + seg.scaled_direction(), pt, - &Vector::y(), + Vector::Y, ); Some(seg.a.y + inter.1) } @@ -181,12 +177,12 @@ impl HeightField { let y0 = self.heights[i]; let y1 = self.heights[i + 1]; - let mut p0 = Point2::new(x0, y0); - let mut p1 = Point2::new(x1, y1); + let mut p0 = Vector2::new(x0, y0); + let mut p1 = Vector2::new(x1, y1); // Apply scales: - p0.coords.component_mul_assign(&self.scale); - p1.coords.component_mul_assign(&self.scale); + p0 *= self.scale; + p1 *= self.scale; Some(Segment::new(p0, p1)) } @@ -203,8 +199,8 @@ impl HeightField { /// The range of segment ids that may intersect the given local Aabb. pub fn unclamped_elements_range_in_local_aabb(&self, aabb: &Aabb) -> Range { - let ref_mins = aabb.mins.coords.component_div(&self.scale); - let ref_maxs = aabb.maxs.coords.component_div(&self.scale); + let ref_mins = aabb.mins / self.scale; + let ref_maxs = aabb.maxs / self.scale; let seg_length = 1.0 / (self.heights.len() as Real - 1.0); let min_x = self.quantize_floor_unclamped(ref_mins.x, seg_length); @@ -214,8 +210,8 @@ impl HeightField { /// Applies `f` to each segment of this heightfield that intersects the given `aabb`. pub fn map_elements_in_local_aabb(&self, aabb: &Aabb, f: &mut impl FnMut(u32, &Segment)) { - let ref_mins = aabb.mins.coords.component_div(&self.scale); - let ref_maxs = aabb.maxs.coords.component_div(&self.scale); + let ref_mins = aabb.mins / self.scale; + let ref_maxs = aabb.maxs / self.scale; let seg_length = 1.0 / (self.heights.len() as Real - 1.0); if ref_maxs.x < -0.5 || ref_mins.x > 0.5 { @@ -243,12 +239,12 @@ impl HeightField { continue; } - let mut p0 = Point2::new(x0, y0); - let mut p1 = Point2::new(x1, y1); + let mut p0 = Vector2::new(x0, y0); + let mut p1 = Vector2::new(x1, y1); // Apply scales: - p0.coords.component_mul_assign(&self.scale); - p1.coords.component_mul_assign(&self.scale); + p0 *= self.scale; + p1 *= self.scale; // Build the segment. let seg = Segment::new(p0, p1); diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 3a03f511..4adfc7c5 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -1,23 +1,22 @@ use core::ops::Range; +// TODO: Replace nalgebra Array2 with a custom Vec-based storage for heightfield data #[cfg(feature = "alloc")] -use na::DMatrix; +use crate::utils::Array2; use crate::bounding_volume::Aabb; use crate::math::{Real, Vector}; use crate::shape::{FeatureId, Triangle, TrianglePseudoNormals}; -use na::{Point3, Unit}; #[cfg(not(feature = "std"))] -use na::ComplexField; +use crate::math::ComplexField; #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Clone, Copy, Debug, Default, Eq, Hash, Ord, PartialEq, PartialOrd)] -/// The status of the cell of an heightfield. +/// The status of the cell of a heightfield. pub struct HeightFieldCellStatus(u8); bitflags::bitflags! { @@ -36,8 +35,7 @@ bitflags::bitflags! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] #[derive(Clone, Copy, Debug, Default, Eq, Hash, Ord, PartialEq, PartialOrd)] @@ -67,10 +65,10 @@ bitflags::bitflags! { #[repr(C)] /// A 3D heightfield. pub struct HeightField { - heights: DMatrix, - status: DMatrix, + heights: Array2, + status: Array2, - scale: Vector, + scale: Vector, aabb: Aabb, num_triangles: usize, flags: HeightFieldFlags, @@ -79,36 +77,36 @@ pub struct HeightField { #[cfg(feature = "alloc")] impl HeightField { /// Initializes a new heightfield with the given heights, scaling factor, and flags. - pub fn new(heights: DMatrix, scale: Vector) -> Self { - Self::with_flags(heights, scale, HeightFieldFlags::empty()) + /// + /// The heights are specified in such a way that the row index `i` of the array advances + /// grid coordinates along the `z` axis, and the column index `j` of the array advances + /// grid coordinates along the `x` axis. + pub fn new(heights_zx: Array2, scale: Vector) -> Self { + Self::with_flags(heights_zx, scale, HeightFieldFlags::empty()) } /// Initializes a new heightfield with the given heights and a scaling factor. - pub fn with_flags( - heights: DMatrix, - scale: Vector, - flags: HeightFieldFlags, - ) -> Self { + pub fn with_flags(heights_zx: Array2, scale: Vector, flags: HeightFieldFlags) -> Self { assert!( - heights.nrows() > 1 && heights.ncols() > 1, + heights_zx.nrows() > 1 && heights_zx.ncols() > 1, "A heightfield heights must have at least 2 rows and columns." ); - let max = heights.max(); - let min = heights.min(); + let max = heights_zx.max(); + let min = heights_zx.min(); let hscale = scale * 0.5; let aabb = Aabb::new( - Point3::new(-hscale.x, min * scale.y, -hscale.z), - Point3::new(hscale.x, max * scale.y, hscale.z), + Vector::new(-hscale.x, min * scale.y, -hscale.z), + Vector::new(hscale.x, max * scale.y, hscale.z), ); - let num_triangles = (heights.nrows() - 1) * (heights.ncols() - 1) * 2; - let status = DMatrix::repeat( - heights.nrows() - 1, - heights.ncols() - 1, + let num_triangles = (heights_zx.nrows() - 1) * (heights_zx.ncols() - 1) * 2; + let status = Array2::repeat( + heights_zx.nrows() - 1, + heights_zx.ncols() - 1, HeightFieldCellStatus::default(), ); HeightField { - heights, + heights: heights_zx, scale, aabb, num_triangles, @@ -168,20 +166,20 @@ impl HeightField { } fn quantize_floor(&self, val: Real, cell_size: Real, num_cells: usize) -> usize { - na::clamp( - ((val + 0.5) / cell_size).floor(), - 0.0, - (num_cells - 1) as Real, - ) as usize + ((val + 0.5) / cell_size) + .floor() + .clamp(0.0, (num_cells - 1) as Real) as usize } fn quantize_ceil(&self, val: Real, cell_size: Real, num_cells: usize) -> usize { - na::clamp(((val + 0.5) / cell_size).ceil(), 0.0, num_cells as Real) as usize + ((val + 0.5) / cell_size) + .ceil() + .clamp(0.0, num_cells as Real) as usize } /// The pair of index of the cell containing the vertical projection of the given point. - pub fn closest_cell_at_point(&self, pt: &Point3) -> (usize, usize) { - let scaled_pt = pt.coords.component_div(&self.scale); + pub fn closest_cell_at_point(&self, pt: Vector) -> (usize, usize) { + let scaled_pt = pt / self.scale; let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); let ncells_x = self.ncols(); @@ -193,8 +191,8 @@ impl HeightField { } /// The pair of index of the cell containing the vertical projection of the given point. - pub fn cell_at_point(&self, pt: &Point3) -> Option<(usize, usize)> { - let scaled_pt = pt.coords.component_div(&self.scale); + pub fn cell_at_point(&self, pt: Vector) -> Option<(usize, usize)> { + let scaled_pt = pt / self.scale; let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); let ncells_x = self.ncols(); @@ -211,8 +209,8 @@ impl HeightField { } /// The pair of index of the cell containing the vertical projection of the given point. - pub fn unclamped_cell_at_point(&self, pt: &Point3) -> (isize, isize) { - let scaled_pt = pt.coords.component_div(&self.scale); + pub fn unclamped_cell_at_point(&self, pt: Vector) -> (isize, isize) { + let scaled_pt = pt / self.scale; let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); @@ -251,7 +249,7 @@ impl HeightField { } /// An iterator through all the triangles around the given point, after vertical projection on the heightfield. - pub fn triangles_around_point(&self, point: &Point3) -> HeightFieldRadialTriangles<'_> { + pub fn triangles_around_point(&self, point: Vector) -> HeightFieldRadialTriangles<'_> { let center = self.closest_cell_at_point(point); HeightFieldRadialTriangles { heightfield: self, @@ -365,16 +363,16 @@ impl HeightField { let y01 = self.heights[(i, j + 1)]; let y11 = self.heights[(i + 1, j + 1)]; - let mut p00 = Point3::new(x0, y00, z0); - let mut p10 = Point3::new(x0, y10, z1); - let mut p01 = Point3::new(x1, y01, z0); - let mut p11 = Point3::new(x1, y11, z1); + let mut p00 = Vector::new(x0, y00, z0); + let mut p10 = Vector::new(x0, y10, z1); + let mut p01 = Vector::new(x1, y01, z0); + let mut p11 = Vector::new(x1, y11, z1); // Apply scales: - p00.coords.component_mul_assign(&self.scale); - p10.coords.component_mul_assign(&self.scale); - p01.coords.component_mul_assign(&self.scale); - p11.coords.component_mul_assign(&self.scale); + p00 *= self.scale; + p10 *= self.scale; + p01 *= self.scale; + p11 *= self.scale; if status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { let tri1 = if status.contains(HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED) { @@ -420,20 +418,19 @@ impl HeightField { let (tri_left, tri_right) = self.triangles_at(i, j); let tri_normal = if left { - *tri_left?.normal()? + tri_left?.normal()? } else { - *tri_right?.normal()? + tri_right?.normal()? }; // TODO: we only compute bivectors where v is a specific direction // (+/-X, +/-Z, or a combination of both). So this bivector // calculation could be simplified/optimized quite a bit. // Computes the pseudo-normal of an edge where the adjacent triangle is missing. - let bivector = |v: Vector| tri_normal.cross(&v).cross(&tri_normal).normalize(); + let bivector = |v: Vector| tri_normal.cross(v).cross(tri_normal).normalize(); // Pseudo-normal computed from an adjacent triangle’s normal and the current triangle’s normal. - let adj_pseudo_normal = |adj: Option| { - adj.map(|adj| adj.normal().map(|n| *n).unwrap_or(tri_normal)) - }; + let adj_pseudo_normal = + |adj: Option| adj.map(|adj| adj.normal().unwrap_or(tri_normal)); let diag_dir = if status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { Vector::new(-1.0, 0.0, 1.0) @@ -443,13 +440,13 @@ impl HeightField { let (left_pseudo_normal, right_pseudo_normal) = if left { let adj_left = adj_pseudo_normal(self.triangles_at(i.overflowing_sub(1).0, j).1) - .unwrap_or_else(|| bivector(-Vector::z())); + .unwrap_or_else(|| bivector(-Vector::Z)); let adj_right = adj_pseudo_normal(tri_right).unwrap_or_else(|| bivector(diag_dir)); (adj_left, adj_right) } else { let adj_left = adj_pseudo_normal(tri_left).unwrap_or_else(|| bivector(-diag_dir)); let adj_right = adj_pseudo_normal(self.triangles_at(i + 1, j).0) - .unwrap_or_else(|| bivector(Vector::z())); + .unwrap_or_else(|| bivector(Vector::Z)); (adj_left, adj_right) }; @@ -471,7 +468,7 @@ impl HeightField { bot_right }; - adj_pseudo_normal(bot_tri).unwrap_or_else(|| bivector(-Vector::x())) + adj_pseudo_normal(bot_tri).unwrap_or_else(|| bivector(-Vector::X)) } else { // The neighbor is above. let ((top_left, top_right), top_status) = if j < self.heights.ncols() - 2 { @@ -486,17 +483,16 @@ impl HeightField { top_left }; - adj_pseudo_normal(top_tri).unwrap_or_else(|| bivector(Vector::x())) + adj_pseudo_normal(top_tri).unwrap_or_else(|| bivector(Vector::X)) }; - // NOTE: the normalization can only succeed due to the heightfield’s definition. - let pseudo_normal1 = Unit::new_normalize((tri_normal + left_pseudo_normal) / 2.0); - let pseudo_normal2 = Unit::new_normalize((tri_normal + right_pseudo_normal) / 2.0); - let pseudo_normal3 = - Unit::new_normalize((tri_normal + top_or_bottom_pseudo_normal) / 2.0); + // NOTE: the normalization can only succeed due to the heightfield's definition. + let pseudo_normal1 = (tri_normal + left_pseudo_normal).normalize(); + let pseudo_normal2 = (tri_normal + right_pseudo_normal).normalize(); + let pseudo_normal3 = (tri_normal + top_or_bottom_pseudo_normal).normalize(); Some(TrianglePseudoNormals { - face: Unit::new_unchecked(tri_normal), // No need to re-normalize. + face: tri_normal, // No need to re-normalize. // TODO: the normals are given in no particular order. So they are **not** // guaranteed to be provided in the same order as the triangle’s edge. edges: [pseudo_normal1, pseudo_normal2, pseudo_normal3], @@ -522,12 +518,12 @@ impl HeightField { } /// The statuses of all the cells of this heightfield. - pub fn cells_statuses(&self) -> &DMatrix { + pub fn cells_statuses(&self) -> &Array2 { &self.status } /// The mutable statuses of all the cells of this heightfield. - pub fn cells_statuses_mut(&mut self) -> &mut DMatrix { + pub fn cells_statuses_mut(&mut self) -> &mut Array2 { &mut self.status } @@ -542,26 +538,26 @@ impl HeightField { } /// The heights of this heightfield. - pub fn heights(&self) -> &DMatrix { + pub fn heights(&self) -> &Array2 { &self.heights } /// The scale factor applied to this heightfield. - pub fn scale(&self) -> &Vector { - &self.scale + pub fn scale(&self) -> Vector { + self.scale } /// Sets the scale factor applied to this heightfield. - pub fn set_scale(&mut self, new_scale: Vector) { - let ratio = new_scale.component_div(&self.scale); - self.aabb.mins.coords.component_mul_assign(&ratio); - self.aabb.maxs.coords.component_mul_assign(&ratio); + pub fn set_scale(&mut self, new_scale: Vector) { + let ratio = new_scale / self.scale; + self.aabb.mins *= ratio; + self.aabb.maxs *= ratio; self.scale = new_scale; } /// Returns a scaled version of this heightfield. - pub fn scaled(mut self, scale: &Vector) -> Self { - self.set_scale(self.scale.component_mul(scale)); + pub fn scaled(mut self, scale: Vector) -> Self { + self.set_scale(self.scale * scale); self } @@ -675,8 +671,8 @@ impl HeightField { &self, aabb: &Aabb, ) -> (Range, Range) { - let ref_mins = aabb.mins.coords.component_div(&self.scale); - let ref_maxs = aabb.maxs.coords.component_div(&self.scale); + let ref_mins = aabb.mins / self.scale; + let ref_maxs = aabb.maxs / self.scale; let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); @@ -693,8 +689,8 @@ impl HeightField { let ncells_x = self.ncols(); let ncells_z = self.nrows(); - let ref_mins = aabb.mins.coords.component_div(&self.scale); - let ref_maxs = aabb.maxs.coords.component_div(&self.scale); + let ref_mins = aabb.mins / self.scale; + let ref_maxs = aabb.maxs / self.scale; let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); @@ -739,16 +735,16 @@ impl HeightField { continue; } - let mut p00 = Point3::new(x0, y00, z0); - let mut p10 = Point3::new(x0, y10, z1); - let mut p01 = Point3::new(x1, y01, z0); - let mut p11 = Point3::new(x1, y11, z1); + let mut p00 = Vector::new(x0, y00, z0); + let mut p10 = Vector::new(x0, y10, z1); + let mut p01 = Vector::new(x1, y01, z0); + let mut p11 = Vector::new(x1, y11, z1); // Apply scales: - p00.coords.component_mul_assign(&self.scale); - p10.coords.component_mul_assign(&self.scale); - p01.coords.component_mul_assign(&self.scale); - p11.coords.component_mul_assign(&self.scale); + p00 *= self.scale; + p10 *= self.scale; + p01 *= self.scale; + p11 *= self.scale; // Build the two triangles, contact processors and call f. if !status.contains(HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED) { diff --git a/src/shape/polygon.rs b/src/shape/polygon.rs index 70749199..b37a3527 100644 --- a/src/shape/polygon.rs +++ b/src/shape/polygon.rs @@ -1,6 +1,6 @@ #![allow(dead_code)] // TODO: remove this once we support polygons. -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Vector, Real, Vector}; use parry::bounding_volume::Aabb; #[derive(Clone)] @@ -8,12 +8,11 @@ use parry::bounding_volume::Aabb; #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) )] /// A convex planar polygon. pub struct Polygon { - pub(crate) vertices: Vec>, - pub(crate) normals: Vec>, + pub(crate) vertices: Vec, + pub(crate) normals: Vec, } impl Polygon { @@ -27,36 +26,36 @@ impl Polygon { /// The vertices must form a convex polygon. /// /// One normal must be provided per edge and mut point towards the outside of the polygon. - pub fn new(vertices: Vec>, normals: Vec>) -> Self { + pub fn new(vertices: Vec, normals: Vec) -> Self { Self { vertices, normals } } /// Compute the axis-aligned bounding box of the polygon. - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { let p0 = pos * self.vertices[0]; let mut mins = p0; let mut maxs = p0; for pt in &self.vertices[1..] { let pt = pos * pt; - mins = mins.inf(&pt); - maxs = maxs.sup(&pt); + mins = mins.min(pt); + maxs = maxs.max(pt); } Aabb::new(mins.into(), maxs.into()) } /// The vertices of this polygon. - pub fn vertices(&self) -> &[Point] { + pub fn vertices(&self) -> &[Vector] { &self.vertices } - pub(crate) fn support_point(&self, dir: &Vector) -> usize { + pub(crate) fn support_point(&self, dir: Vector) -> usize { let mut best_dot = -Real::MAX; let mut best_i = 0; for (i, pt) in self.vertices.iter().enumerate() { - let dot = pt.coords.dot(&dir); + let dot = pt.dot(dir); if dot > best_dot { best_dot = dot; best_i = i; @@ -66,7 +65,7 @@ impl Polygon { best_i } - pub(crate) fn support_face(&self, dir: &Vector) -> usize { + pub(crate) fn support_face(&self, dir: Vector) -> usize { let mut max_dot = -Real::MAX; let mut max_dot_i = 0; diff --git a/src/shape/polygonal_feature2d.rs b/src/shape/polygonal_feature2d.rs index a2c4c707..8fa9c423 100644 --- a/src/shape/polygonal_feature2d.rs +++ b/src/shape/polygonal_feature2d.rs @@ -1,4 +1,4 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Vector}; #[cfg(feature = "alloc")] use crate::query::{self, ContactManifold, TrackedContact}; use crate::shape::{PackedFeatureId, Segment}; @@ -8,7 +8,7 @@ use crate::shape::{PackedFeatureId, Segment}; #[derive(Debug)] pub struct PolygonalFeature { /// Up to two vertices forming this polygonal feature. - pub vertices: [Point; 2], + pub vertices: [Vector; 2], /// The feature IDs of this polygon's vertices. pub vids: [PackedFeatureId; 2], /// The feature ID of this polygonal feature. @@ -20,7 +20,7 @@ pub struct PolygonalFeature { impl Default for PolygonalFeature { fn default() -> Self { Self { - vertices: [Point::origin(); 2], + vertices: [Vector::ZERO; 2], vids: [PackedFeatureId::UNKNOWN; 2], fid: PackedFeatureId::UNKNOWN, num_vertices: 0, @@ -41,7 +41,7 @@ impl From for PolygonalFeature { impl PolygonalFeature { /// Transforms the vertices of `self` by the given position `pos`. - pub fn transform_by(&mut self, pos: &Isometry) { + pub fn transform_by(&mut self, pos: &Pose) { self.vertices[0] = pos * self.vertices[0]; self.vertices[1] = pos * self.vertices[1]; } @@ -49,10 +49,10 @@ impl PolygonalFeature { /// Computes the contacts between two polygonal features. #[cfg(feature = "alloc")] pub fn contacts( - pos12: &Isometry, - pos21: &Isometry, - sep_axis1: &Vector, - sep_axis2: &Vector, + pos12: &Pose, + pos21: &Pose, + sep_axis1: Vector, + sep_axis2: Vector, feature1: &Self, feature2: &Self, manifold: &mut ContactManifold, @@ -77,9 +77,9 @@ impl PolygonalFeature { /// This method assume we already know that at least one contact exists. #[cfg(feature = "alloc")] pub fn face_vertex_contacts( - pos12: &Isometry, + pos12: &Pose, face1: &Self, - sep_axis1: &Vector, + sep_axis1: Vector, vertex2: &Self, manifold: &mut ContactManifold, flipped: bool, @@ -88,13 +88,13 @@ impl PolygonalFeature { let tangent1 = face1.vertices[1] - face1.vertices[0]; let normal1 = Vector::new(-tangent1.y, tangent1.x); let denom = -normal1.dot(sep_axis1); - let dist = (face1.vertices[0] - v2_1).dot(&normal1) / denom; + let dist = (face1.vertices[0] - v2_1).dot(normal1) / denom; let local_p2 = v2_1; let local_p1 = v2_1 - dist * normal1; let contact = TrackedContact::flipped( local_p1, - pos12.inverse_transform_point(&local_p2), + pos12.inverse_transform_point(local_p2), face1.fid, vertex2.vids[0], dist, @@ -107,9 +107,9 @@ impl PolygonalFeature { /// Computes the contacts between two polygonal faces. #[cfg(feature = "alloc")] pub fn face_face_contacts( - pos12: &Isometry, + pos12: &Pose, face1: &Self, - normal1: &Vector, + normal1: Vector, face2: &Self, manifold: &mut ContactManifold, flipped: bool, @@ -117,7 +117,7 @@ impl PolygonalFeature { if let Some((clip_a, clip_b)) = query::details::clip_segment_segment_with_normal( (face1.vertices[0], face1.vertices[1]), (pos12 * face2.vertices[0], pos12 * face2.vertices[1]), - *normal1, + normal1, ) { let fids1 = [face1.vids[0], face1.fid, face1.vids[1]]; let fids2 = [face2.vids[0], face2.fid, face2.vids[1]]; @@ -125,7 +125,7 @@ impl PolygonalFeature { let dist = (clip_a.1 - clip_a.0).dot(normal1); let contact = TrackedContact::flipped( clip_a.0, - pos12.inverse_transform_point(&clip_a.1), + pos12.inverse_transform_point(clip_a.1), fids1[clip_a.2], fids2[clip_a.3], dist, @@ -136,7 +136,7 @@ impl PolygonalFeature { let dist = (clip_b.1 - clip_b.0).dot(normal1); let contact = TrackedContact::flipped( clip_b.0, - pos12.inverse_transform_point(&clip_b.1), + pos12.inverse_transform_point(clip_b.1), fids1[clip_b.2], fids2[clip_b.3], dist, diff --git a/src/shape/polygonal_feature3d.rs b/src/shape/polygonal_feature3d.rs index a3b00b6f..4a938739 100644 --- a/src/shape/polygonal_feature3d.rs +++ b/src/shape/polygonal_feature3d.rs @@ -1,17 +1,19 @@ -use crate::approx::AbsDiffEq; -use crate::math::{Isometry, Point, Real, Vector}; -#[cfg(feature = "alloc")] -use crate::query::{ContactManifold, TrackedContact}; +use crate::math::{Pose, Vector}; use crate::shape::{PackedFeatureId, Segment, Triangle}; -use crate::utils::WBasis; -use na::Point2; +#[cfg(feature = "alloc")] +use crate::{ + math::{Real, Vector2}, + query::details::closest_points_segment_segment_2d, + query::{ContactManifold, TrackedContact}, + utils::WBasis, +}; /// A polygonal feature representing the local polygonal approximation of /// a vertex, face, or edge of a convex shape. #[derive(Debug, Clone)] pub struct PolygonalFeature { /// Up to four vertices forming this polygonal feature. - pub vertices: [Point; 4], + pub vertices: [Vector; 4], /// The feature IDs of this polygon's vertices. pub vids: [PackedFeatureId; 4], /// The feature IDs of this polygon's edges. @@ -25,7 +27,7 @@ pub struct PolygonalFeature { impl Default for PolygonalFeature { fn default() -> Self { Self { - vertices: [Point::origin(); 4], + vertices: [Vector::ZERO; 4], vids: [PackedFeatureId::UNKNOWN; 4], eids: [PackedFeatureId::UNKNOWN; 4], fid: PackedFeatureId::UNKNOWN, @@ -65,7 +67,7 @@ impl PolygonalFeature { } /// Transform each vertex of this polygonal feature by the given position `pos`. - pub fn transform_by(&mut self, pos: &Isometry) { + pub fn transform_by(&mut self, pos: &Pose) { for p in &mut self.vertices[0..self.num_vertices] { *p = pos * *p; } @@ -74,10 +76,10 @@ impl PolygonalFeature { /// Computes all the contacts between two polygonal features. #[cfg(feature = "alloc")] pub fn contacts( - pos12: &Isometry, - _pos21: &Isometry, - sep_axis1: &Vector, - _sep_axis2: &Vector, + pos12: &Pose, + _pos21: &Pose, + sep_axis1: Vector, + _sep_axis2: Vector, feature1: &Self, feature2: &Self, manifold: &mut ContactManifold, @@ -93,9 +95,9 @@ impl PolygonalFeature { #[cfg(feature = "alloc")] fn contacts_edge_edge( - pos12: &Isometry, + pos12: &Pose, face1: &PolygonalFeature, - sep_axis1: &Vector, + sep_axis1: Vector, face2: &PolygonalFeature, manifold: &mut ContactManifold, flipped: bool, @@ -106,45 +108,34 @@ impl PolygonalFeature { // we are not working in world-space here). let basis = sep_axis1.orthonormal_basis(); let projected_edge1 = [ - Point2::new( - face1.vertices[0].coords.dot(&basis[0]), - face1.vertices[0].coords.dot(&basis[1]), + Vector2::new( + face1.vertices[0].dot(basis[0]), + face1.vertices[0].dot(basis[1]), ), - Point2::new( - face1.vertices[1].coords.dot(&basis[0]), - face1.vertices[1].coords.dot(&basis[1]), + Vector2::new( + face1.vertices[1].dot(basis[0]), + face1.vertices[1].dot(basis[1]), ), ]; let vertices2_1 = [pos12 * face2.vertices[0], pos12 * face2.vertices[1]]; let projected_edge2 = [ - Point2::new( - vertices2_1[0].coords.dot(&basis[0]), - vertices2_1[0].coords.dot(&basis[1]), - ), - Point2::new( - vertices2_1[1].coords.dot(&basis[0]), - vertices2_1[1].coords.dot(&basis[1]), - ), + Vector2::new(vertices2_1[0].dot(basis[0]), vertices2_1[0].dot(basis[1])), + Vector2::new(vertices2_1[1].dot(basis[0]), vertices2_1[1].dot(basis[1])), ]; - let tangent1 = - (projected_edge1[1] - projected_edge1[0]).try_normalize(Real::default_epsilon()); - let tangent2 = - (projected_edge2[1] - projected_edge2[0]).try_normalize(Real::default_epsilon()); + let tangent1 = (projected_edge1[1] - projected_edge1[0]).try_normalize(); + let tangent2 = (projected_edge2[1] - projected_edge2[0]).try_normalize(); // TODO: not sure what the best value for eps is. // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small. if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) { - let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8; + let parallel = tangent1.dot(tangent2) >= crate::utils::COS_FRAC_PI_8; if !parallel { let seg1 = (&projected_edge1[0], &projected_edge1[1]); let seg2 = (&projected_edge2[0], &projected_edge2[1]); - let (loc1, loc2) = - crate::query::details::closest_points_segment_segment_with_locations_nD( - seg1, seg2, - ); + let (loc1, loc2) = closest_points_segment_segment_2d(seg1, seg2); // Found a contact between the two edges. let bcoords1 = loc1.barycentric_coordinates(); @@ -152,13 +143,13 @@ impl PolygonalFeature { let edge1 = (face1.vertices[0], face1.vertices[1]); let edge2 = (vertices2_1[0], vertices2_1[1]); - let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1]; - let local_p2_1 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1]; + let local_p1 = edge1.0 * bcoords1[0] + edge1.1 * bcoords1[1]; + let local_p2_1 = edge2.0 * bcoords2[0] + edge2.1 * bcoords2[1]; let dist = (local_p2_1 - local_p1).dot(sep_axis1); manifold.points.push(TrackedContact::flipped( local_p1, - pos12.inverse_transform_point(&local_p2_1), + pos12.inverse_transform_point(local_p2_1), face1.eids[0], face2.eids[0], dist, @@ -186,7 +177,7 @@ impl PolygonalFeature { manifold.points.push(TrackedContact::flipped( (clips.0).0, - pos12.inverse_transform_point(&(clips.0).1), + pos12.inverse_transform_point((clips.0).1), feature_at(face1, (clips.0).2), feature_at(face2, (clips.0).3), ((clips.0).1 - (clips.0).0).dot(sep_axis1), @@ -195,7 +186,7 @@ impl PolygonalFeature { manifold.points.push(TrackedContact::flipped( (clips.1).0, - pos12.inverse_transform_point(&(clips.1).1), + pos12.inverse_transform_point((clips.1).1), feature_at(face1, (clips.1).2), feature_at(face2, (clips.1).3), ((clips.1).1 - (clips.1).0).dot(sep_axis1), @@ -206,9 +197,9 @@ impl PolygonalFeature { #[cfg(feature = "alloc")] fn contacts_face_face( - pos12: &Isometry, + pos12: &Pose, face1: &PolygonalFeature, - sep_axis1: &Vector, + sep_axis1: Vector, face2: &PolygonalFeature, manifold: &mut ContactManifold, flipped: bool, @@ -219,21 +210,21 @@ impl PolygonalFeature { // we are not working in world-space here). let basis = sep_axis1.orthonormal_basis(); let projected_face1 = [ - Point2::new( - face1.vertices[0].coords.dot(&basis[0]), - face1.vertices[0].coords.dot(&basis[1]), + Vector2::new( + face1.vertices[0].dot(basis[0]), + face1.vertices[0].dot(basis[1]), ), - Point2::new( - face1.vertices[1].coords.dot(&basis[0]), - face1.vertices[1].coords.dot(&basis[1]), + Vector2::new( + face1.vertices[1].dot(basis[0]), + face1.vertices[1].dot(basis[1]), ), - Point2::new( - face1.vertices[2].coords.dot(&basis[0]), - face1.vertices[2].coords.dot(&basis[1]), + Vector2::new( + face1.vertices[2].dot(basis[0]), + face1.vertices[2].dot(basis[1]), ), - Point2::new( - face1.vertices[3].coords.dot(&basis[0]), - face1.vertices[3].coords.dot(&basis[1]), + Vector2::new( + face1.vertices[3].dot(basis[0]), + face1.vertices[3].dot(basis[1]), ), ]; @@ -244,28 +235,16 @@ impl PolygonalFeature { pos12 * face2.vertices[3], ]; let projected_face2 = [ - Point2::new( - vertices2_1[0].coords.dot(&basis[0]), - vertices2_1[0].coords.dot(&basis[1]), - ), - Point2::new( - vertices2_1[1].coords.dot(&basis[0]), - vertices2_1[1].coords.dot(&basis[1]), - ), - Point2::new( - vertices2_1[2].coords.dot(&basis[0]), - vertices2_1[2].coords.dot(&basis[1]), - ), - Point2::new( - vertices2_1[3].coords.dot(&basis[0]), - vertices2_1[3].coords.dot(&basis[1]), - ), + Vector2::new(vertices2_1[0].dot(basis[0]), vertices2_1[0].dot(basis[1])), + Vector2::new(vertices2_1[1].dot(basis[0]), vertices2_1[1].dot(basis[1])), + Vector2::new(vertices2_1[2].dot(basis[0]), vertices2_1[2].dot(basis[1])), + Vector2::new(vertices2_1[3].dot(basis[0]), vertices2_1[3].dot(basis[1])), ]; // Also find all the vertices located inside of the other projected face. if face2.num_vertices > 2 { let normal2_1 = - (vertices2_1[2] - vertices2_1[1]).cross(&(vertices2_1[0] - vertices2_1[1])); + (vertices2_1[2] - vertices2_1[1]).cross(vertices2_1[0] - vertices2_1[1]); let denom = normal2_1.dot(sep_axis1); if !relative_eq!(denom, 0.0) { @@ -276,10 +255,10 @@ impl PolygonalFeature { let p1 = projected_face1[i]; let mut sign = (projected_face2[0] - projected_face2[last_index2]) - .perp(&(p1 - projected_face2[last_index2])); + .perp_dot(p1 - projected_face2[last_index2]); for j in 0..last_index2 { let new_sign = (projected_face2[j + 1] - projected_face2[j]) - .perp(&(p1 - projected_face2[j])); + .perp_dot(p1 - projected_face2[j]); if sign == 0.0 { sign = new_sign; @@ -291,13 +270,13 @@ impl PolygonalFeature { // All the perp had the same sign: the point is inside of the other shapes projection. // Output the contact. - let dist = (vertices2_1[0] - face1.vertices[i]).dot(&normal2_1) / denom; + let dist = (vertices2_1[0] - face1.vertices[i]).dot(normal2_1) / denom; let local_p1 = face1.vertices[i]; let local_p2_1 = face1.vertices[i] + dist * sep_axis1; manifold.points.push(TrackedContact::flipped( local_p1, - pos12.inverse_transform_point(&local_p2_1), + pos12.inverse_transform_point(local_p2_1), face1.vids[i], face2.fid, dist, @@ -309,7 +288,7 @@ impl PolygonalFeature { if face1.num_vertices > 2 { let normal1 = (face1.vertices[2] - face1.vertices[1]) - .cross(&(face1.vertices[0] - face1.vertices[1])); + .cross(face1.vertices[0] - face1.vertices[1]); let denom = -normal1.dot(sep_axis1); if !relative_eq!(denom, 0.0) { @@ -318,10 +297,10 @@ impl PolygonalFeature { let p2 = projected_face2[i]; let mut sign = (projected_face1[0] - projected_face1[last_index1]) - .perp(&(p2 - projected_face1[last_index1])); + .perp_dot(p2 - projected_face1[last_index1]); for j in 0..last_index1 { let new_sign = (projected_face1[j + 1] - projected_face1[j]) - .perp(&(p2 - projected_face1[j])); + .perp_dot(p2 - projected_face1[j]); if sign == 0.0 { sign = new_sign; @@ -333,13 +312,13 @@ impl PolygonalFeature { // All the perp had the same sign: the point is inside of the other shapes projection. // Output the contact. - let dist = (face1.vertices[0] - vertices2_1[i]).dot(&normal1) / denom; + let dist = (face1.vertices[0] - vertices2_1[i]).dot(normal1) / denom; let local_p2_1 = vertices2_1[i]; let local_p1 = vertices2_1[i] - dist * sep_axis1; manifold.points.push(TrackedContact::flipped( local_p1, - pos12.inverse_transform_point(&local_p2_1), + pos12.inverse_transform_point(local_p2_1), face1.fid, face2.vids[i], dist, @@ -370,13 +349,13 @@ impl PolygonalFeature { face1.vertices[(i + 1) % face1.num_vertices], ); let edge2 = (vertices2_1[j], vertices2_1[(j + 1) % face2.num_vertices]); - let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0; - let local_p2_1 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1; + let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1 * bcoords.0; + let local_p2_1 = edge2.0 * (1.0 - bcoords.1) + edge2.1 * bcoords.1; let dist = (local_p2_1 - local_p1).dot(sep_axis1); manifold.points.push(TrackedContact::flipped( local_p1, - pos12.inverse_transform_point(&local_p2_1), + pos12.inverse_transform_point(local_p2_1), face1.eids[i], face2.eids[j], dist, @@ -391,18 +370,18 @@ impl PolygonalFeature { /// Compute the barycentric coordinates of the intersection between the two given lines. /// Returns `None` if the lines are parallel. -fn closest_points_line2d( - edge1: [Point2; 2], - edge2: [Point2; 2], -) -> Option<(Real, Real)> { +#[cfg(feature = "alloc")] +fn closest_points_line2d(edge1: [Vector2; 2], edge2: [Vector2; 2]) -> Option<(Real, Real)> { + use crate::approx::AbsDiffEq; + // Inspired by Real-time collision detection by Christer Ericson. let dir1 = edge1[1] - edge1[0]; let dir2 = edge2[1] - edge2[0]; let r = edge1[0] - edge2[0]; - let a = dir1.norm_squared(); - let e = dir2.norm_squared(); - let f = dir2.dot(&r); + let a = dir1.length_squared(); + let e = dir2.length_squared(); + let f = dir2.dot(r); let eps = Real::default_epsilon(); @@ -411,11 +390,11 @@ fn closest_points_line2d( } else if a <= eps { Some((0.0, f / e)) } else { - let c = dir1.dot(&r); + let c = dir1.dot(r); if e <= eps { Some((-c / a, 0.0)) } else { - let b = dir1.dot(&dir2); + let b = dir1.dot(dir2); let ae = a * e; let bb = b * b; let denom = ae - bb; diff --git a/src/shape/polygonal_feature_map.rs b/src/shape/polygonal_feature_map.rs index dda5c9d6..f1d2daf0 100644 --- a/src/shape/polygonal_feature_map.rs +++ b/src/shape/polygonal_feature_map.rs @@ -1,22 +1,12 @@ -use crate::math::{Real, Vector}; -use crate::shape::{Cuboid, PolygonalFeature, Segment, SupportMap, Triangle}; -use na::Unit; +use crate::math::Vector; #[cfg(feature = "dim3")] -use { - crate::{ - math::Point, - shape::{Cone, Cylinder, PackedFeatureId}, - }, - approx::AbsDiffEq, -}; - -#[cfg(not(feature = "alloc"))] -use na::{ComplexField, RealField}; // for .abs() and .copysign() +use crate::shape::{Cone, Cylinder, PackedFeatureId}; +use crate::shape::{Cuboid, PolygonalFeature, Segment, SupportMap, Triangle}; /// Trait implemented by convex shapes with features with polyhedral approximations. pub trait PolygonalFeatureMap: SupportMap { /// Compute the support polygonal face of `self` towards the `dir`. - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolygonalFeature); + fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature); // TODO: this is currently just a workaround for https://github.com/dimforge/rapier/issues/417 // until we get a better way to deal with the issue without breaking internal edges @@ -28,27 +18,27 @@ pub trait PolygonalFeatureMap: SupportMap { } impl PolygonalFeatureMap for Segment { - fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolygonalFeature) { + fn local_support_feature(&self, _: Vector, out_feature: &mut PolygonalFeature) { *out_feature = PolygonalFeature::from(*self); } } impl PolygonalFeatureMap for Triangle { - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolygonalFeature) { - *out_feature = self.support_face(**dir); + fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) { + *out_feature = self.support_face(dir); } } impl PolygonalFeatureMap for Cuboid { - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolygonalFeature) { - *out_feature = self.support_face(**dir); + fn local_support_feature(&self, dir: Vector, out_feature: &mut PolygonalFeature) { + *out_feature = self.support_face(dir); } } #[cfg(feature = "dim3")] impl PolygonalFeatureMap for Cylinder { - fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolygonalFeature) { - use na::Vector2; + fn local_support_feature(&self, dir: Vector, out_features: &mut PolygonalFeature) { + use crate::math::Vector2; // About feature ids. // At all times, we consider our cylinder to be approximated as follows: @@ -65,18 +55,18 @@ impl PolygonalFeatureMap for Cylinder { // - Note that at all times, one of each cap's vertices are the same as the curved-part // segment endpoints. let dir2 = Vector2::new(dir.x, dir.z) - .try_normalize(Real::default_epsilon()) - .unwrap_or(Vector2::x()); + .try_normalize() + .unwrap_or(Vector2::X); if dir.y.abs() < 0.5 { // We return a segment lying on the cylinder's curved part. - out_features.vertices[0] = Point::new( + out_features.vertices[0] = Vector::new( dir2.x * self.radius, -self.half_height, dir2.y * self.radius, ); out_features.vertices[1] = - Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius); + Vector::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius); out_features.eids = PackedFeatureId::edges([0, 0, 0, 0]); out_features.fid = PackedFeatureId::face(0); out_features.num_vertices = 2; @@ -84,10 +74,10 @@ impl PolygonalFeatureMap for Cylinder { } else { // We return a square approximation of the cylinder cap. let y = self.half_height.copysign(dir.y); - out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius); - out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius); - out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius); - out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius); + out_features.vertices[0] = Vector::new(dir2.x * self.radius, y, dir2.y * self.radius); + out_features.vertices[1] = Vector::new(-dir2.y * self.radius, y, dir2.x * self.radius); + out_features.vertices[2] = Vector::new(-dir2.x * self.radius, y, -dir2.y * self.radius); + out_features.vertices[3] = Vector::new(dir2.y * self.radius, y, -dir2.x * self.radius); if dir.y < 0.0 { out_features.eids = PackedFeatureId::edges([2, 4, 6, 8]); @@ -106,8 +96,8 @@ impl PolygonalFeatureMap for Cylinder { #[cfg(feature = "dim3")] impl PolygonalFeatureMap for Cone { - fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolygonalFeature) { - use na::Vector2; + fn local_support_feature(&self, dir: Vector, out_features: &mut PolygonalFeature) { + use crate::math::Vector2; // About feature ids. It is very similar to the feature ids of cylinders. // At all times, we consider our cone to be approximated as follows: @@ -122,17 +112,17 @@ impl PolygonalFeatureMap for Cone { // - Note that at all times, one of the cap's vertices are the same as the curved-part // segment endpoints. let dir2 = Vector2::new(dir.x, dir.z) - .try_normalize(Real::default_epsilon()) - .unwrap_or(Vector2::x()); + .try_normalize() + .unwrap_or(Vector2::X); if dir.y > 0.0 { // We return a segment lying on the cone's curved part. - out_features.vertices[0] = Point::new( + out_features.vertices[0] = Vector::new( dir2.x * self.radius, -self.half_height, dir2.y * self.radius, ); - out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0); + out_features.vertices[1] = Vector::new(0.0, self.half_height, 0.0); out_features.eids = PackedFeatureId::edges([0, 0, 0, 0]); out_features.fid = PackedFeatureId::face(0); out_features.num_vertices = 2; @@ -140,10 +130,10 @@ impl PolygonalFeatureMap for Cone { } else { // We return a square approximation of the cone cap. let y = -self.half_height; - out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius); - out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius); - out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius); - out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius); + out_features.vertices[0] = Vector::new(dir2.x * self.radius, y, dir2.y * self.radius); + out_features.vertices[1] = Vector::new(-dir2.y * self.radius, y, dir2.x * self.radius); + out_features.vertices[2] = Vector::new(-dir2.x * self.radius, y, -dir2.y * self.radius); + out_features.vertices[3] = Vector::new(dir2.y * self.radius, y, -dir2.x * self.radius); out_features.eids = PackedFeatureId::edges([2, 4, 6, 8]); out_features.fid = PackedFeatureId::face(9); diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index a72f559e..21dcb015 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy}; use crate::query::{PointProjection, PointQueryWithLocation}; use crate::shape::composite_shape::CompositeShape; @@ -13,8 +13,7 @@ use crate::query::details::NormalConstraints; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] /// A polyline shape formed by connected line segments. /// @@ -25,7 +24,7 @@ use crate::query::details::NormalConstraints; /// # Structure /// /// A polyline consists of: -/// - **Vertices**: Points in 2D or 3D space +/// - **Vertices**: Vectors in 2D or 3D space /// - **Indices**: Pairs of vertex indices defining each segment /// - **BVH**: Bounding Volume Hierarchy for fast spatial queries /// @@ -50,13 +49,13 @@ use crate::query::details::NormalConstraints; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create a simple L-shaped polyline /// let vertices = vec![ -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(1.0, 1.0, 0.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(1.0, 1.0, 0.0), /// ]; /// /// // Indices are automatically generated to connect consecutive vertices @@ -75,13 +74,13 @@ use crate::query::details::NormalConstraints; /// ```rust /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; -/// use nalgebra::Point2; +/// use parry2d::math::Vector; /// /// // Create a triangle polyline (closed loop) /// let vertices = vec![ -/// Point2::origin(), -/// Point2::new(1.0, 0.0), -/// Point2::new(0.5, 1.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0), +/// Vector::new(0.5, 1.0), /// ]; /// /// // Manually specify edges to create a closed triangle @@ -97,7 +96,7 @@ use crate::query::details::NormalConstraints; /// ``` pub struct Polyline { bvh: Bvh, - vertices: Vec>, + vertices: Vec, indices: Vec<[u32; 2]>, } @@ -122,14 +121,14 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a zigzag path with automatic sequential connections /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 1.0, 0.0), - /// Point3::new(2.0, 0.0, 0.0), - /// Point3::new(3.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 1.0, 0.0), + /// Vector::new(2.0, 0.0, 0.0), + /// Vector::new(3.0, 1.0, 0.0), /// ]; /// let polyline = Polyline::new(vertices, None); /// assert_eq!(polyline.num_segments(), 3); @@ -141,14 +140,14 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a square with custom indices /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(0.0, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(0.0, 1.0), /// ]; /// /// // Define edges to form a closed square @@ -161,11 +160,11 @@ impl Polyline { /// /// // Each segment connects the correct vertices /// let first_segment = square.segment(0); - /// assert_eq!(first_segment.a, Point2::origin()); - /// assert_eq!(first_segment.b, Point2::new(1.0, 0.0)); + /// assert_eq!(first_segment.a, Vector::ZERO); + /// assert_eq!(first_segment.b, Vector::new(1.0, 0.0)); /// # } /// ``` - pub fn new(vertices: Vec>, indices: Option>) -> Self { + pub fn new(vertices: Vec, indices: Option>) -> Self { let indices = indices.unwrap_or_else(|| (0..vertices.len() as u32 - 1).map(|i| [i, i + 1]).collect()); let leaves = indices.iter().enumerate().map(|(i, idx)| { @@ -203,32 +202,29 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; - /// use nalgebra::{Point3, Isometry3, Translation3}; + /// use parry3d::math::{Vector, Pose}; /// /// // Create a polyline along the X axis /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(2.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0, 0.0), /// ]; /// let polyline = Polyline::new(vertices, None); /// /// // Compute AABB at the origin - /// let identity = Isometry3::identity(); + /// let identity = Pose::identity(); /// let aabb = polyline.aabb(&identity); /// assert_eq!(aabb.mins.x, 0.0); /// assert_eq!(aabb.maxs.x, 2.0); /// /// // Compute AABB after translating by (10, 5, 0) - /// let translated = Isometry3::from_parts( - /// Translation3::new(10.0, 5.0, 0.0), - /// nalgebra::UnitQuaternion::identity() - /// ); + /// let translated = Pose::translation(10.0, 5.0, 0.0); /// let aabb_translated = polyline.aabb(&translated); /// assert_eq!(aabb_translated.mins.x, 10.0); /// assert_eq!(aabb_translated.maxs.x, 12.0); /// # } /// ``` - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.bvh.root_aabb().transform_by(pos) } @@ -247,14 +243,14 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a rectangular polyline /// let vertices = vec![ - /// Point2::new(-1.0, -2.0), - /// Point2::new(3.0, -2.0), - /// Point2::new(3.0, 4.0), - /// Point2::new(-1.0, 4.0), + /// Vector::new(-1.0, -2.0), + /// Vector::new(3.0, -2.0), + /// Vector::new(3.0, 4.0), + /// Vector::new(-1.0, 4.0), /// ]; /// let polyline = Polyline::new(vertices, None); /// @@ -292,15 +288,15 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Sequential polyline: 5 vertices -> 4 segments /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(2.0, 0.0, 0.0), - /// Point3::new(3.0, 0.0, 0.0), - /// Point3::new(4.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(2.0, 0.0, 0.0), + /// Vector::new(3.0, 0.0, 0.0), + /// Vector::new(4.0, 0.0, 0.0), /// ]; /// let polyline = Polyline::new(vertices.clone(), None); /// assert_eq!(polyline.num_segments(), 4); @@ -329,13 +325,13 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a triangle /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(0.5, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(0.5, 1.0), /// ]; /// let polyline = Polyline::new(vertices, None); /// @@ -385,25 +381,25 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(2.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(2.0, 1.0, 0.0), /// ]; /// let polyline = Polyline::new(vertices, None); /// /// // Get the first segment (connects vertex 0 to vertex 1) /// let seg0 = polyline.segment(0); - /// assert_eq!(seg0.a, Point3::origin()); - /// assert_eq!(seg0.b, Point3::new(1.0, 0.0, 0.0)); + /// assert_eq!(seg0.a, Vector::ZERO); + /// assert_eq!(seg0.b, Vector::new(1.0, 0.0, 0.0)); /// assert_eq!(seg0.length(), 1.0); /// /// // Get the second segment (connects vertex 1 to vertex 2) /// let seg1 = polyline.segment(1); - /// assert_eq!(seg1.a, Point3::new(1.0, 0.0, 0.0)); - /// assert_eq!(seg1.b, Point3::new(2.0, 1.0, 0.0)); + /// assert_eq!(seg1.a, Vector::new(1.0, 0.0, 0.0)); + /// assert_eq!(seg1.b, Vector::new(2.0, 1.0, 0.0)); /// # } /// ``` pub fn segment(&self, i: u32) -> Segment { @@ -441,21 +437,21 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(1.0, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(1.0, 1.0), /// ]; /// let polyline = Polyline::new(vertices.clone(), None); /// /// // Access all vertices /// let verts = polyline.vertices(); /// assert_eq!(verts.len(), 3); - /// assert_eq!(verts[0], Point2::origin()); - /// assert_eq!(verts[1], Point2::new(1.0, 0.0)); - /// assert_eq!(verts[2], Point2::new(1.0, 1.0)); + /// assert_eq!(verts[0], Vector::ZERO); + /// assert_eq!(verts[1], Vector::new(1.0, 0.0)); + /// assert_eq!(verts[2], Vector::new(1.0, 1.0)); /// /// // You can iterate over vertices /// for (i, vertex) in polyline.vertices().iter().enumerate() { @@ -463,7 +459,7 @@ impl Polyline { /// } /// # } /// ``` - pub fn vertices(&self) -> &[Point] { + pub fn vertices(&self) -> &[Vector] { &self.vertices[..] } @@ -482,12 +478,12 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(2.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(2.0, 0.0, 0.0), /// ]; /// /// // With automatic indices @@ -536,20 +532,20 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; - /// use nalgebra::{Point2, Vector2}; + /// use parry2d::math::Vector; /// /// let vertices = vec![ - /// Point2::new(1.0, 2.0), - /// Point2::new(3.0, 4.0), + /// Vector::new(1.0, 2.0), + /// Vector::new(3.0, 4.0), /// ]; /// let polyline = Polyline::new(vertices, None); /// /// // Scale by 2x in X and 3x in Y - /// let scaled = polyline.scaled(&Vector2::new(2.0, 3.0)); + /// let scaled = polyline.scaled(Vector::new(2.0, 3.0)); /// /// // Check scaled vertices - /// assert_eq!(scaled.vertices()[0], Point2::new(2.0, 6.0)); - /// assert_eq!(scaled.vertices()[1], Point2::new(6.0, 12.0)); + /// assert_eq!(scaled.vertices()[0], Vector::new(2.0, 6.0)); + /// assert_eq!(scaled.vertices()[1], Vector::new(6.0, 12.0)); /// # } /// ``` /// @@ -561,23 +557,21 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Polyline; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// - /// let vertices = vec![Point3::origin(), Point3::new(1.0, 0.0, 0.0)]; + /// let vertices = vec![Vector::ZERO, Vector::new(1.0, 0.0, 0.0)]; /// let original = Polyline::new(vertices, None); /// /// // Clone before scaling if you need to keep the original - /// let scaled = original.clone().scaled(&Vector3::new(2.0, 2.0, 2.0)); + /// let scaled = original.clone().scaled(Vector::new(2.0, 2.0, 2.0)); /// /// // Both polylines still exist /// assert_eq!(original.vertices()[1].x, 1.0); /// assert_eq!(scaled.vertices()[1].x, 2.0); /// # } /// ``` - pub fn scaled(mut self, scale: &Vector) -> Self { - self.vertices - .iter_mut() - .for_each(|pt| pt.coords.component_mul_assign(scale)); + pub fn scaled(mut self, scale: Vector) -> Self { + self.vertices.iter_mut().for_each(|pt| *pt *= scale); let mut bvh = self.bvh.clone(); bvh.scale(scale); Self { @@ -602,12 +596,12 @@ impl Polyline { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::Polyline; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(1.0, 0.0), - /// Point2::new(2.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0), + /// Vector::new(2.0, 0.0), /// ]; /// let mut polyline = Polyline::new(vertices, None); /// @@ -726,10 +720,10 @@ impl Polyline { /// These properties are not checked. pub fn project_local_point_assuming_solid_interior_ccw( &self, - point: Point, + point: Vector, #[cfg(feature = "dim3")] axis: u8, ) -> (PointProjection, (u32, SegmentPointLocation)) { - let mut proj = self.project_local_point_and_get_location(&point, false); + let mut proj = self.project_local_point_and_get_location(point, false); let segment1 = self.segment((proj.1).0); #[cfg(feature = "dim2")] @@ -757,13 +751,13 @@ impl Polyline { self.segment(adj_seg).scaled_direction() }; - let dot = normal1.dot(&dir2); + let dot = normal1.dot(dir2); // TODO: is this threshold too big? This corresponds to an angle equal to // abs(acos(1.0e-3)) = (90 - 0.057) degrees. // We did encounter some cases where this was needed, but perhaps the // actual problem was an issue with the SegmentPointLocation (which should // perhaps have been Edge instead of Vertex)? - let threshold = 1.0e-3 * dir2.norm(); + let threshold = 1.0e-3 * dir2.length(); if dot.abs() > threshold { // If the vertex is a reentrant vertex, then the point is // inside. Otherwise, it is outside. @@ -771,10 +765,10 @@ impl Polyline { } else { // If the two edges are collinear, we can’t classify the vertex. // So check against the edge’s normal instead. - (point - proj.0.point).dot(&normal1) <= 0.0 + (point - proj.0.point).dot(normal1) <= 0.0 } } - SegmentPointLocation::OnEdge(_) => (point - proj.0.point).dot(&normal1) <= 0.0, + SegmentPointLocation::OnEdge(_) => (point - proj.0.point).dot(normal1) <= 0.0, }; } @@ -786,7 +780,7 @@ impl CompositeShape for Polyline { fn map_part_at( &self, i: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { let tri = self.segment(i); f(None, &tri, None) @@ -805,11 +799,7 @@ impl TypedCompositeShape for Polyline { fn map_typed_part_at( &self, i: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { let seg = self.segment(i); Some(f(None, &seg, None)) @@ -819,7 +809,7 @@ impl TypedCompositeShape for Polyline { fn map_untyped_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let seg = self.segment(i); Some(f(None, &seg, None)) diff --git a/src/shape/round_shape.rs b/src/shape/round_shape.rs index 1ef9bac8..b2cd8c5c 100644 --- a/src/shape/round_shape.rs +++ b/src/shape/round_shape.rs @@ -3,15 +3,13 @@ //! This module provides the `RoundShape` wrapper that adds a border radius to any shape, //! effectively creating a "padded" or "rounded" version of the original shape. -use crate::math::{Point, Real, Vector}; +use crate::math::{Real, Vector}; use crate::shape::SupportMap; -use na::Unit; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Copy, Clone, Debug)] #[repr(C)] @@ -48,10 +46,10 @@ use na::Unit; /// use parry3d::shape::{RoundShape, Cuboid}; /// # #[cfg(feature = "f64")] /// use parry3d_f64::shape::{RoundShape, Cuboid}; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// /// // Create a cuboid with half-extents of 1.0 in each direction -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// /// // Add a border radius of 0.2 to create a rounded cuboid /// let rounded_cuboid = RoundShape { @@ -74,13 +72,13 @@ use na::Unit; /// use parry2d::shape::{RoundShape, Triangle}; /// # #[cfg(feature = "f64")] /// use parry2d_f64::shape::{RoundShape, Triangle}; -/// use nalgebra::Point2; +/// use parry2d::math::Vector; /// /// // Create a triangle /// let triangle = Triangle::new( -/// Point2::origin(), -/// Point2::new(1.0, 0.0), -/// Point2::new(0.0, 1.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0), +/// Vector::new(0.0, 1.0), /// ); /// /// // Add rounding with a 0.1 border radius @@ -94,7 +92,7 @@ use na::Unit; /// # } /// ``` /// -/// ## Comparing Support Points +/// ## Comparing Support Vectors /// /// This example shows how the border radius affects the support point calculation: /// @@ -104,21 +102,21 @@ use na::Unit; /// use parry3d::shape::{RoundShape, Cuboid, SupportMap}; /// # #[cfg(feature = "f64")] /// use parry3d_f64::shape::{RoundShape, Cuboid, SupportMap}; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::splat(1.0)); /// let rounded_cuboid = RoundShape { /// inner_shape: cuboid, /// border_radius: 0.5, /// }; /// /// // Query the support point in the direction (1, 1, 1) -/// let direction = Vector3::new(1.0, 1.0, 1.0); -/// let support_point = rounded_cuboid.local_support_point(&direction); +/// let direction = Vector::new(1.0, 1.0, 1.0); +/// let support_point = rounded_cuboid.local_support_point(direction); /// /// // The support point will be further out than the original cuboid's support point /// // due to the border radius -/// let cuboid_support = cuboid.local_support_point(&direction); +/// let cuboid_support = cuboid.local_support_point(direction); /// /// // The rounded shape extends further in all directions /// assert!(support_point.x > cuboid_support.x); @@ -137,7 +135,7 @@ use na::Unit; /// use parry3d::shape::{RoundShape, Ball, Segment, SupportMap}; /// # #[cfg(feature = "f64")] /// use parry3d_f64::shape::{RoundShape, Ball, Segment, SupportMap}; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::Vector; /// /// // Rounded ball (creates a slightly larger sphere) /// let ball = Ball::new(1.0); @@ -149,8 +147,8 @@ use na::Unit; /// /// // Rounded segment (creates a capsule) /// let segment = Segment::new( -/// Point3::origin(), -/// Point3::new(0.0, 2.0, 0.0), +/// Vector::ZERO, +/// Vector::new(0.0, 2.0, 0.0), /// ); /// let rounded_segment = RoundShape { /// inner_shape: segment, @@ -216,24 +214,24 @@ impl SupportMap for RoundShape { /// use parry3d::shape::{RoundShape, Cuboid, SupportMap}; /// # #[cfg(feature = "f64")] /// use parry3d_f64::shape::{RoundShape, Cuboid, SupportMap}; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// - /// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + /// let cuboid = Cuboid::new(Vector::splat(1.0)); /// let rounded = RoundShape { /// inner_shape: cuboid, /// border_radius: 0.5, /// }; /// /// // Support point in the positive X direction - /// let dir = Vector3::new(1.0, 0.0, 0.0); - /// let support = rounded.local_support_point(&dir); + /// let dir = Vector::new(1.0, 0.0, 0.0); + /// let support = rounded.local_support_point(dir); /// /// // The X coordinate is the cuboid's half-extent plus the border radius /// assert!((support.x - 1.5).abs() < 1e-6); /// # } /// ``` - fn local_support_point(&self, dir: &Vector) -> Point { - self.local_support_point_toward(&Unit::new_normalize(*dir)) + fn local_support_point(&self, dir: Vector) -> Vector { + self.local_support_point_toward(dir.normalize()) } /// Computes the support point of the rounded shape toward the given unit direction. @@ -256,11 +254,8 @@ impl SupportMap for RoundShape { /// /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { - /// # #[cfg(feature = "f32")] /// use parry2d::shape::{RoundShape, Ball, SupportMap}; - /// # #[cfg(feature = "f64")] - /// use parry2d_f64::shape::{RoundShape, Ball, SupportMap}; - /// use nalgebra::{Vector2, Unit}; + /// use parry2d::math::Vector; /// /// let ball = Ball::new(1.0); /// let rounded = RoundShape { @@ -269,16 +264,16 @@ impl SupportMap for RoundShape { /// }; /// /// // Create a unit direction - /// let dir = Unit::new_normalize(Vector2::new(1.0, 1.0)); - /// let support = rounded.local_support_point_toward(&dir); + /// let dir = Vector::new(1.0, 1.0).normalize(); + /// let support = rounded.local_support_point_toward(dir); /// /// // The distance from origin should be ball radius + border radius /// let distance = (support.x.powi(2) + support.y.powi(2)).sqrt(); /// assert!((distance - 1.3).abs() < 1e-6); /// # } /// ``` - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.inner_shape.local_support_point_toward(dir) + **dir * self.border_radius + fn local_support_point_toward(&self, dir: Vector) -> Vector { + self.inner_shape.local_support_point_toward(dir) + dir * self.border_radius } } @@ -327,8 +322,8 @@ impl SupportMap for RoundShapeRef<'_, S> { /// # Returns /// /// The point on the rounded shape's surface that is furthest in the given direction. - fn local_support_point(&self, dir: &Vector) -> Point { - self.local_support_point_toward(&Unit::new_normalize(*dir)) + fn local_support_point(&self, dir: Vector) -> Vector { + self.local_support_point_toward(dir.normalize()) } /// Computes the support point of the rounded shape reference toward the given unit direction. @@ -344,7 +339,7 @@ impl SupportMap for RoundShapeRef<'_, S> { /// # Returns /// /// The point on the rounded shape's surface that is furthest in the given direction. - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.inner_shape.local_support_point_toward(dir) + **dir * self.border_radius + fn local_support_point_toward(&self, dir: Vector) -> Vector { + self.inner_shape.local_support_point_toward(dir) + dir * self.border_radius } } diff --git a/src/shape/segment.rs b/src/shape/segment.rs index cb1fbc09..a7dc4b4e 100644 --- a/src/shape/segment.rs +++ b/src/shape/segment.rs @@ -1,13 +1,8 @@ //! Definition of the segment shape. -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, Vector}; use crate::shape::{FeatureId, SupportMap}; - use core::mem; -use na::{self, Unit}; - -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// A line segment shape. /// @@ -18,7 +13,7 @@ use rkyv::{bytecheck, CheckBytes}; /// /// - **a**: The first endpoint /// - **b**: The second endpoint -/// - **Direction**: Points from `a` toward `b` +/// - **Direction**: Vectors from `a` toward `b` /// /// # Properties /// @@ -46,11 +41,11 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create a horizontal segment of length 5 -/// let a = Point3::origin(); -/// let b = Point3::new(5.0, 0.0, 0.0); +/// let a = Vector::ZERO; +/// let b = Vector::new(5.0, 0.0, 0.0); /// let segment = Segment::new(a, b); /// /// assert_eq!(segment.length(), 5.0); @@ -63,16 +58,15 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "encase", derive(encase::ShaderType))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] pub struct Segment { /// The first endpoint of the segment. - pub a: Point, + pub a: Vector, /// The second endpoint of the segment. - pub b: Point, + pub b: Vector, } /// Describes where a point is located on a segment. @@ -82,8 +76,8 @@ pub struct Segment { /// /// # Variants /// -/// - **OnVertex(id)**: Point projects to an endpoint (0 = `a`, 1 = `b`) -/// - **OnEdge(bary)**: Point projects to the interior with barycentric coordinates +/// - **OnVertex(id)**: Vector projects to an endpoint (0 = `a`, 1 = `b`) +/// - **OnEdge(bary)**: Vector projects to the interior with barycentric coordinates /// /// # Example /// @@ -91,15 +85,15 @@ pub struct Segment { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::SegmentPointLocation; /// -/// // Point at first vertex +/// // Vector at first vertex /// let loc = SegmentPointLocation::OnVertex(0); /// assert_eq!(loc.barycentric_coordinates(), [1.0, 0.0]); /// -/// // Point at second vertex +/// // Vector at second vertex /// let loc = SegmentPointLocation::OnVertex(1); /// assert_eq!(loc.barycentric_coordinates(), [0.0, 1.0]); /// -/// // Point halfway along the segment +/// // Vector halfway along the segment /// let loc = SegmentPointLocation::OnEdge([0.5, 0.5]); /// assert_eq!(loc.barycentric_coordinates(), [0.5, 0.5]); /// # } @@ -108,15 +102,15 @@ pub struct Segment { pub enum SegmentPointLocation { /// The point lies on a vertex (endpoint). /// - /// - `0` = Point is at `segment.a` - /// - `1` = Point is at `segment.b` + /// - `0` = Vector is at `segment.a` + /// - `1` = Vector is at `segment.b` OnVertex(u32), /// The point lies on the segment interior. /// /// Contains barycentric coordinates `[u, v]` where: /// - `u + v = 1.0` - /// - Point = `a * u + b * v` + /// - Vector = `a * u + b * v` /// - `0.0 < u, v < 1.0` (strictly between endpoints) OnEdge([Real; 2]), } @@ -126,29 +120,29 @@ impl SegmentPointLocation { /// /// Barycentric coordinates `[u, v]` satisfy: /// - `u + v = 1.0` - /// - Point = `a * u + b * v` + /// - Vector = `a * u + b * v` /// /// # Example /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Segment, SegmentPointLocation}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let segment = Segment::new( - /// Point3::origin(), - /// Point3::new(10.0, 0.0, 0.0) + /// Vector::ZERO, + /// Vector::new(10.0, 0.0, 0.0) /// ); /// - /// // Point at endpoint a + /// // Vector at endpoint a /// let loc_a = SegmentPointLocation::OnVertex(0); /// assert_eq!(loc_a.barycentric_coordinates(), [1.0, 0.0]); /// - /// // Point at endpoint b + /// // Vector at endpoint b /// let loc_b = SegmentPointLocation::OnVertex(1); /// assert_eq!(loc_b.barycentric_coordinates(), [0.0, 1.0]); /// - /// // Point at 30% from a to b + /// // Vector at 30% from a to b /// let loc_mid = SegmentPointLocation::OnEdge([0.7, 0.3]); /// let coords = loc_mid.barycentric_coordinates(); /// assert_eq!(coords[0], 0.7); @@ -183,17 +177,17 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let segment = Segment::new( - /// Point3::origin(), - /// Point3::new(5.0, 0.0, 0.0) + /// Vector::ZERO, + /// Vector::new(5.0, 0.0, 0.0) /// ); /// assert_eq!(segment.length(), 5.0); /// # } /// ``` #[inline] - pub fn new(a: Point, b: Point) -> Segment { + pub fn new(a: Vector, b: Vector) -> Segment { Segment { a, b } } @@ -206,15 +200,15 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// - /// let points = [Point3::origin(), Point3::new(1.0, 0.0, 0.0)]; + /// let points = [Vector::ZERO, Vector::new(1.0, 0.0, 0.0)]; /// let segment = Segment::from_array(&points); /// assert_eq!(segment.a, points[0]); /// assert_eq!(segment.b, points[1]); /// # } /// ``` - pub fn from_array(arr: &[Point; 2]) -> &Segment { + pub fn from_array(arr: &[Vector; 2]) -> &Segment { unsafe { mem::transmute(arr) } } @@ -231,23 +225,20 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let segment = Segment::new( - /// Point3::new(1.0, 2.0, 3.0), - /// Point3::new(4.0, 5.0, 6.0) + /// Vector::new(1.0, 2.0, 3.0), + /// Vector::new(4.0, 5.0, 6.0) /// ); /// - /// let scaled = segment.scaled(&Vector3::new(2.0, 2.0, 2.0)); - /// assert_eq!(scaled.a, Point3::new(2.0, 4.0, 6.0)); - /// assert_eq!(scaled.b, Point3::new(8.0, 10.0, 12.0)); + /// let scaled = segment.scaled(Vector::new(2.0, 2.0, 2.0)); + /// assert_eq!(scaled.a, Vector::new(2.0, 4.0, 6.0)); + /// assert_eq!(scaled.b, Vector::new(8.0, 10.0, 12.0)); /// # } /// ``` - pub fn scaled(self, scale: &Vector) -> Self { - Self::new( - na::Scale::from(*scale) * self.a, - na::Scale::from(*scale) * self.b, - ) + pub fn scaled(self, scale: Vector) -> Self { + Self::new(self.a * scale, self.b * scale) } /// Returns the direction vector of this segment scaled by its length. @@ -260,19 +251,19 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let segment = Segment::new( - /// Point3::origin(), - /// Point3::new(3.0, 4.0, 0.0) + /// Vector::ZERO, + /// Vector::new(3.0, 4.0, 0.0) /// ); /// /// let dir = segment.scaled_direction(); - /// assert_eq!(dir, Vector3::new(3.0, 4.0, 0.0)); - /// assert_eq!(dir.norm(), 5.0); // Length of the segment + /// assert_eq!(dir, Vector::new(3.0, 4.0, 0.0)); + /// assert_eq!(dir.length(), 5.0); // Length of the segment /// # } /// ``` - pub fn scaled_direction(&self) -> Vector { + pub fn scaled_direction(&self) -> Vector { self.b - self.a } @@ -283,18 +274,18 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // 3-4-5 right triangle /// let segment = Segment::new( - /// Point3::origin(), - /// Point3::new(3.0, 4.0, 0.0) + /// Vector::ZERO, + /// Vector::new(3.0, 4.0, 0.0) /// ); /// assert_eq!(segment.length(), 5.0); /// # } /// ``` pub fn length(&self) -> Real { - self.scaled_direction().norm() + self.scaled_direction().length() } /// Swaps the two endpoints of this segment. @@ -306,16 +297,16 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let mut segment = Segment::new( - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(5.0, 0.0, 0.0) + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(5.0, 0.0, 0.0) /// ); /// /// segment.swap(); - /// assert_eq!(segment.a, Point3::new(5.0, 0.0, 0.0)); - /// assert_eq!(segment.b, Point3::new(1.0, 0.0, 0.0)); + /// assert_eq!(segment.a, Vector::new(5.0, 0.0, 0.0)); + /// assert_eq!(segment.b, Vector::new(1.0, 0.0, 0.0)); /// # } /// ``` pub fn swap(&mut self) { @@ -324,7 +315,7 @@ impl Segment { /// Returns the unit direction vector of this segment. /// - /// Points from `a` toward `b` with length 1.0. + /// Vectors from `a` toward `b` with length 1.0. /// /// # Returns /// @@ -336,32 +327,32 @@ impl Segment { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Segment; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let segment = Segment::new( - /// Point3::origin(), - /// Point3::new(3.0, 4.0, 0.0) + /// Vector::ZERO, + /// Vector::new(3.0, 4.0, 0.0) /// ); /// /// if let Some(dir) = segment.direction() { /// // Direction is normalized - /// assert!((dir.norm() - 1.0).abs() < 1e-6); - /// // Points from a to b - /// assert_eq!(*dir, Vector3::new(0.6, 0.8, 0.0)); + /// assert!((dir.length() - 1.0).abs() < 1e-6); + /// // Vectors from a to b + /// assert_eq!(dir, Vector::new(0.6, 0.8, 0.0)); /// } /// /// // Degenerate segment (zero length) - /// let degenerate = Segment::new(Point3::origin(), Point3::origin()); + /// let degenerate = Segment::new(Vector::ZERO, Vector::ZERO); /// assert!(degenerate.direction().is_none()); /// # } /// ``` - pub fn direction(&self) -> Option>> { - Unit::try_new(self.scaled_direction(), crate::math::DEFAULT_EPSILON) + pub fn direction(&self) -> Option { + self.scaled_direction().try_normalize() } /// In 2D, the not-normalized counterclockwise normal of this segment. #[cfg(feature = "dim2")] - pub fn scaled_normal(&self) -> Vector { + pub fn scaled_normal(&self) -> Vector { let dir = self.scaled_direction(); Vector::new(dir.y, -dir.x) } @@ -369,7 +360,7 @@ impl Segment { /// The not-normalized counterclockwise normal of this segment, assuming it lies on the plane /// with the normal collinear to the given axis (0 = X, 1 = Y, 2 = Z). #[cfg(feature = "dim3")] - pub fn scaled_planar_normal(&self, plane_axis: u8) -> Vector { + pub fn scaled_planar_normal(&self, plane_axis: u8) -> Vector { let dir = self.scaled_direction(); match plane_axis { 0 => Vector::new(0.0, dir.z, -dir.y), @@ -381,45 +372,41 @@ impl Segment { /// In 2D, the normalized counterclockwise normal of this segment. #[cfg(feature = "dim2")] - pub fn normal(&self) -> Option>> { - Unit::try_new(self.scaled_normal(), crate::math::DEFAULT_EPSILON) + pub fn normal(&self) -> Option { + let (dir, length) = self.scaled_normal().normalize_and_length(); + (length > crate::math::DEFAULT_EPSILON).then_some(dir) } /// Returns `None`. Exists only for API similarity with the 2D parry. #[cfg(feature = "dim3")] - pub fn normal(&self) -> Option>> { + pub fn normal(&self) -> Option { None } /// The normalized counterclockwise normal of this segment, assuming it lies on the plane /// with the normal collinear to the given axis (0 = X, 1 = Y, 2 = Z). #[cfg(feature = "dim3")] - pub fn planar_normal(&self, plane_axis: u8) -> Option>> { - Unit::try_new( - self.scaled_planar_normal(plane_axis), - crate::math::DEFAULT_EPSILON, - ) + pub fn planar_normal(&self, plane_axis: u8) -> Option { + self.scaled_planar_normal(plane_axis).try_normalize() } /// Applies the isometry `m` to the vertices of this segment and returns the resulting segment. - pub fn transformed(&self, m: &Isometry) -> Self { + pub fn transformed(&self, m: &Pose) -> Self { Segment::new(m * self.a, m * self.b) } /// Computes the point at the given location. - pub fn point_at(&self, location: &SegmentPointLocation) -> Point { + pub fn point_at(&self, location: &SegmentPointLocation) -> Vector { match *location { SegmentPointLocation::OnVertex(0) => self.a, SegmentPointLocation::OnVertex(1) => self.b, - SegmentPointLocation::OnEdge(bcoords) => { - self.a * bcoords[0] + self.b.coords * bcoords[1] - } + SegmentPointLocation::OnEdge(bcoords) => self.a * bcoords[0] + self.b * bcoords[1], _ => panic!(), } } /// The normal of the given feature of this shape. - pub fn feature_normal(&self, feature: FeatureId) -> Option>> { + pub fn feature_normal(&self, feature: FeatureId) -> Option { if let Some(direction) = self.direction() { match feature { FeatureId::Vertex(id) => { @@ -431,14 +418,14 @@ impl Segment { } #[cfg(feature = "dim3")] FeatureId::Edge(_) => { - let iamin = direction.iamin(); - let mut normal = Vector::zeros(); - normal[iamin] = 1.0; - normal -= *direction * direction[iamin]; - Some(Unit::new_normalize(normal)) + let imin = direction.abs().min_position(); + let mut normal = Vector::ZERO; + normal[imin] = 1.0; + normal -= direction * direction[imin]; + Some(normal.normalize()) } FeatureId::Face(id) => { - let mut dir = Vector::zeros(); + let mut dir = Vector::ZERO; if id == 0 { dir[0] = direction[1]; dir[1] = -direction[0]; @@ -446,20 +433,20 @@ impl Segment { dir[0] = -direction[1]; dir[1] = direction[0]; } - Some(Unit::new_unchecked(dir)) + Some(dir) } _ => None, } } else { - Some(Vector::y_axis()) + Some(Vector::Y) } } } impl SupportMap for Segment { #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { - if self.a.coords.dot(dir) > self.b.coords.dot(dir) { + fn local_support_point(&self, dir: Vector) -> Vector { + if self.a.dot(dir) > self.b.dot(dir) { self.a } else { self.b @@ -467,15 +454,15 @@ impl SupportMap for Segment { } } -impl From<[Point; 2]> for Segment { - fn from(arr: [Point; 2]) -> Self { +impl From<[Vector; 2]> for Segment { + fn from(arr: [Vector; 2]) -> Self { *Self::from_array(&arr) } } /* impl ConvexPolyhedron for Segment { - fn vertex(&self, id: FeatureId) -> Point { + fn vertex(&self, id: FeatureId) -> Vector { if id.unwrap_vertex() == 0 { self.a } else { @@ -484,7 +471,7 @@ impl ConvexPolyhedron for Segment { } #[cfg(feature = "dim3")] - fn edge(&self, _: FeatureId) -> (Point, Point, FeatureId, FeatureId) { + fn edge(&self, _: FeatureId) -> (Vector, Vector, FeatureId, FeatureId) { (self.a, self.b, FeatureId::Vertex(0), FeatureId::Vertex(1)) } @@ -522,8 +509,8 @@ impl ConvexPolyhedron for Segment { #[cfg(feature = "dim2")] fn support_face_toward( &self, - m: &Isometry, - dir: &Unit>, + m: &Pose, + dir: Vector, face: &mut ConvexPolygonalFeature, ) { let seg_dir = self.scaled_direction(); @@ -539,8 +526,8 @@ impl ConvexPolyhedron for Segment { #[cfg(feature = "dim3")] fn support_face_toward( &self, - m: &Isometry, - _: &Unit>, + m: &Pose, + _: Vector, face: &mut ConvexPolygonalFeature, ) { face.clear(); @@ -553,17 +540,17 @@ impl ConvexPolyhedron for Segment { fn support_feature_toward( &self, - transform: &Isometry, - dir: &Unit>, + transform: &Pose, + dir: Vector, eps: Real, face: &mut ConvexPolygonalFeature, ) { face.clear(); let seg = self.transformed(transform); - let ceps = ComplexField::sin(eps); + let ceps = ::sin(eps); if let Some(seg_dir) = seg.direction() { - let cang = dir.dot(&seg_dir); + let cang = dir.dot(seg_dir); if cang > ceps { face.set_feature_id(FeatureId::Vertex(1)); @@ -591,10 +578,10 @@ impl ConvexPolyhedron for Segment { } } - fn support_feature_id_toward(&self, local_dir: &Unit>) -> FeatureId { + fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId { if let Some(seg_dir) = self.direction() { - let eps: Real = na::convert::(f64::consts::PI / 180.0); - let seps = ComplexField::sin(eps); + let eps: Real = (f64::consts::PI / 180.0) as Real; + let seps = ::sin(eps); let dot = seg_dir.dot(local_dir.as_ref()); if dot <= seps { @@ -630,15 +617,15 @@ mod test { #[test] fn segment_intersect_zero_length_issue_31() { // never intersect each other - let ray = Ray::new(Point::origin(), Vector::x()); + let ray = Ray::new(Vector::ZERO, Vector::X); let segment = Segment { - a: Point::new( + a: Vector::new( 10.0, 10.0, #[cfg(feature = "dim3")] 10.0, ), - b: Point::new( + b: Vector::new( 10.0, 10.0, #[cfg(feature = "dim3")] @@ -646,7 +633,7 @@ mod test { ), }; - let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX); + let hit = segment.intersects_ray(&Pose::identity(), &ray, Real::MAX); assert_eq!(hit, false); } #[test] @@ -654,17 +641,17 @@ mod test { let epsilon = 1.1920929e-7; // intersect each other let ray = Ray::new( - Point::new( + Vector::new( epsilon * 0.5, 0.3, #[cfg(feature = "dim3")] 0.0, ), - -Vector::y(), + -Vector::Y, ); let segment = Segment { - a: Point::origin(), - b: Point::new( + a: Vector::ZERO, + b: Vector::new( // Theoretically, epsilon would suffice but imprecisions force us to add some more offset. epsilon * 1.01, 0.0, @@ -673,7 +660,7 @@ mod test { ), }; - let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX); + let hit = segment.intersects_ray(&Pose::identity(), &ray, Real::MAX); assert_eq!(hit, true); } #[test] @@ -681,18 +668,18 @@ mod test { let epsilon = 1.1920929e-7; // never intersect each other let ray = Ray::new( - Point::new( + Vector::new( // Theoretically, epsilon would suffice but imprecisions force us to add some more offset. epsilon * 11.0, 0.1, #[cfg(feature = "dim3")] 0.0, ), - -Vector::y(), + -Vector::Y, ); let segment = Segment { - a: Point::origin(), - b: Point::new( + a: Vector::ZERO, + b: Vector::new( epsilon * 0.9, 0.0, #[cfg(feature = "dim3")] @@ -700,7 +687,7 @@ mod test { ), }; - let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX); + let hit = segment.intersects_ray(&Pose::identity(), &ray, Real::MAX); assert_eq!(hit, false); } } diff --git a/src/shape/shape.rs b/src/shape/shape.rs index c5a5d6e1..3abc5724 100644 --- a/src/shape/shape.rs +++ b/src/shape/shape.rs @@ -4,9 +4,7 @@ use core::fmt::Debug; use crate::bounding_volume::{Aabb, BoundingSphere, BoundingVolume}; use crate::mass_properties::MassProperties; -use crate::math::{Isometry, Point, Real, Vector}; -#[cfg(not(feature = "alloc"))] -use crate::num::Float; +use crate::math::{Pose, Real, RealField, Vector}; use crate::query::{PointQuery, RayCast}; #[cfg(feature = "serde-serialize")] use crate::shape::SharedShape; @@ -27,7 +25,6 @@ use crate::shape::{ConvexPolyhedron, RoundConvexPolyhedron, Voxels}; #[cfg(feature = "alloc")] use crate::shape::{ConvexPolygon, RoundConvexPolygon, Voxels}; use downcast_rs::{impl_downcast, DowncastSync}; -use na::{RealField, Unit}; use num::Zero; use num_derive::FromPrimitive; @@ -363,14 +360,14 @@ pub trait Shape: RayCast + PointQuery + DowncastSync { /// if a non-uniform scale is provided and Self as a [`Ball`], then the result will be discretized /// (based on the `num_subdivisions` parameter) as a `ConvexPolyhedron` (in 3D) or `ConvexPolygon` (in 2D). #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option>; + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option>; /// Computes the [`Aabb`] of this shape with the given position. - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.compute_local_aabb().transform_by(position) } /// Computes the bounding-sphere of this shape with the given position. - fn compute_bounding_sphere(&self, position: &Isometry) -> BoundingSphere { + fn compute_bounding_sphere(&self, position: &Pose) -> BoundingSphere { self.compute_local_bounding_sphere().transform_by(position) } @@ -422,17 +419,13 @@ pub trait Shape: RayCast + PointQuery + DowncastSync { // } /// The shape's normal at the given point located on a specific feature. - fn feature_normal_at_point( - &self, - _feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, _feature: FeatureId, _point: Vector) -> Option { None } /// Computes the swept [`Aabb`] of this shape, i.e., the space it would occupy by moving from /// the given start position to the given end position. - fn compute_swept_aabb(&self, start_pos: &Isometry, end_pos: &Isometry) -> Aabb { + fn compute_swept_aabb(&self, start_pos: &Pose, end_pos: &Pose) -> Aabb { let aabb1 = self.compute_aabb(start_pos); let aabb2 = self.compute_aabb(end_pos); aabb1.merged(&aabb2) @@ -680,7 +673,7 @@ impl Shape for Ball { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option> { let scaled = self.scaled(scale, num_subdivisions)?; Some(scaled.either::<_, _, Box>(|x| Box::new(x), |x| Box::new(x))) } @@ -693,7 +686,7 @@ impl Shape for Ball { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -727,12 +720,8 @@ impl Shape for Ball { /// The shape's normal at the given point located on a specific feature. #[inline] - fn feature_normal_at_point( - &self, - _: FeatureId, - point: &Point, - ) -> Option>> { - Unit::try_new(point.coords, crate::math::DEFAULT_EPSILON) + fn feature_normal_at_point(&self, _: FeatureId, point: Vector) -> Option { + (point).try_normalize() } } @@ -743,7 +732,7 @@ impl Shape for Cuboid { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.scaled(scale))) } @@ -755,7 +744,7 @@ impl Shape for Cuboid { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -776,7 +765,7 @@ impl Shape for Cuboid { } fn ccd_thickness(&self) -> Real { - self.half_extents.min() + self.half_extents.min_element() } fn ccd_angular_thickness(&self) -> Real { @@ -791,11 +780,7 @@ impl Shape for Cuboid { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point( - &self, - feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { self.feature_normal(feature) } } @@ -807,7 +792,7 @@ impl Shape for Capsule { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option> { let scaled = self.scaled(scale, num_subdivisions)?; Some(scaled.either::<_, _, Box>(|x| Box::new(x), |x| Box::new(x))) } @@ -820,7 +805,7 @@ impl Shape for Capsule { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -864,7 +849,7 @@ impl Shape for Triangle { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.scaled(scale))) } @@ -876,13 +861,13 @@ impl Shape for Triangle { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } fn mass_properties(&self, _density: Real) -> MassProperties { #[cfg(feature = "dim2")] - return MassProperties::from_triangle(_density, &self.a, &self.b, &self.c); + return MassProperties::from_triangle(_density, self.a, self.b, self.c); #[cfg(feature = "dim3")] return MassProperties::zero(); } @@ -916,11 +901,7 @@ impl Shape for Triangle { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point( - &self, - _feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, _feature: FeatureId, _point: Vector) -> Option { #[cfg(feature = "dim2")] return None; #[cfg(feature = "dim3")] @@ -935,7 +916,7 @@ impl Shape for Segment { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.scaled(scale))) } @@ -947,7 +928,7 @@ impl Shape for Segment { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -983,11 +964,7 @@ impl Shape for Segment { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point( - &self, - feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { self.feature_normal(feature) } } @@ -998,7 +975,7 @@ impl Shape for Compound { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option> { use super::SharedShape; let scaled: Vec<_> = self @@ -1007,10 +984,7 @@ impl Shape for Compound { .map(|(pos, shape)| { let scaled_shape = shape.scale_dyn(scale, num_subdivisions)?; Some(( - Isometry::from_parts( - (pos.translation.vector.component_mul(scale)).into(), - pos.rotation, - ), + Pose::from_parts(pos.translation * scale, pos.rotation), SharedShape(scaled_shape.into()), )) }) @@ -1026,7 +1000,7 @@ impl Shape for Compound { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.local_aabb().transform_by(position) } @@ -1066,7 +1040,7 @@ impl Shape for Polyline { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.clone().scaled(scale))) } @@ -1078,7 +1052,7 @@ impl Shape for Polyline { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1116,7 +1090,7 @@ impl Shape for TriMesh { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.clone().scaled(scale))) } @@ -1128,7 +1102,7 @@ impl Shape for TriMesh { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1156,11 +1130,7 @@ impl Shape for TriMesh { } /// Gets the normal of the triangle represented by `feature`. - fn feature_normal_at_point( - &self, - _feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, _feature: FeatureId, _point: Vector) -> Option { #[cfg(feature = "dim2")] return None; #[cfg(feature = "dim3")] @@ -1179,7 +1149,7 @@ impl Shape for HeightField { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.clone().scaled(scale))) } @@ -1191,7 +1161,7 @@ impl Shape for HeightField { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1225,7 +1195,7 @@ impl Shape for ConvexPolygon { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.clone().scaled(scale)?)) } @@ -1237,7 +1207,7 @@ impl Shape for ConvexPolygon { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1259,7 +1229,7 @@ impl Shape for ConvexPolygon { fn ccd_thickness(&self) -> Real { // TODO: we should use the OBB instead. - self.compute_local_aabb().half_extents().min() + self.compute_local_aabb().half_extents().min_element() } fn ccd_angular_thickness(&self) -> Real { @@ -1276,11 +1246,7 @@ impl Shape for ConvexPolygon { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point( - &self, - feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { self.feature_normal(feature) } } @@ -1292,7 +1258,7 @@ impl Shape for ConvexPolyhedron { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.clone().scaled(scale)?)) } @@ -1304,7 +1270,7 @@ impl Shape for ConvexPolyhedron { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1327,7 +1293,7 @@ impl Shape for ConvexPolyhedron { fn ccd_thickness(&self) -> Real { // TODO: we should use the OBB instead. - self.compute_local_aabb().half_extents().min() + self.compute_local_aabb().half_extents().min_element() } fn ccd_angular_thickness(&self) -> Real { @@ -1344,11 +1310,7 @@ impl Shape for ConvexPolyhedron { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point( - &self, - feature: FeatureId, - _point: &Point, - ) -> Option>> { + fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { self.feature_normal(feature) } } @@ -1361,7 +1323,7 @@ impl Shape for Cylinder { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option> { let scaled = self.scaled(scale, num_subdivisions)?; Some(scaled.either::<_, _, Box>(|x| Box::new(x), |x| Box::new(x))) } @@ -1374,7 +1336,7 @@ impl Shape for Cylinder { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1419,7 +1381,7 @@ impl Shape for Cone { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option> { let scaled = self.scaled(scale, num_subdivisions)?; Some(scaled.either::<_, _, Box>(|x| Box::new(x), |x| Box::new(x))) } @@ -1432,7 +1394,7 @@ impl Shape for Cone { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1479,7 +1441,7 @@ impl Shape for HalfSpace { } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.scaled(scale)?)) } @@ -1491,7 +1453,7 @@ impl Shape for HalfSpace { self.local_bounding_sphere() } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.aabb(position) } @@ -1536,7 +1498,7 @@ impl Shape for Voxels { Box::new(self.clone()) } - fn scale_dyn(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, _num_subdivisions: u32) -> Option> { Some(Box::new(self.clone().scaled(scale))) } @@ -1553,7 +1515,7 @@ impl Shape for Voxels { } fn ccd_thickness(&self) -> Real { - self.voxel_size().min() + self.voxel_size().min_element() } fn ccd_angular_thickness(&self) -> Real { @@ -1570,7 +1532,7 @@ macro_rules! impl_shape_for_round_shape( } #[cfg(feature = "alloc")] - fn scale_dyn(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, scale: Vector, num_subdivisions: u32) -> Option> { $t(self, scale, num_subdivisions) } @@ -1582,7 +1544,7 @@ macro_rules! impl_shape_for_round_shape( self.inner_shape.local_bounding_sphere().loosened(self.border_radius) } - fn compute_aabb(&self, position: &Isometry) -> Aabb { + fn compute_aabb(&self, position: &Pose) -> Aabb { self.inner_shape.aabb(position).loosened(self.border_radius) } @@ -1626,7 +1588,7 @@ macro_rules! impl_shape_for_round_shape( impl_shape_for_round_shape!( Cuboid, RoundCuboid, - (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + (|this: &Self, scale: Vector, _num_subdivisions: u32| { let shape = RoundShape { border_radius: this.border_radius, inner_shape: this.inner_shape.scaled(scale), @@ -1638,7 +1600,7 @@ impl_shape_for_round_shape!( impl_shape_for_round_shape!( Triangle, RoundTriangle, - (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + (|this: &Self, scale: Vector, _num_subdivisions: u32| { let shape = RoundShape { border_radius: this.border_radius, inner_shape: this.inner_shape.scaled(scale), @@ -1652,7 +1614,7 @@ impl_shape_for_round_shape!( impl_shape_for_round_shape!( ConvexPolygon, RoundConvexPolygon, - (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + (|this: &Self, scale: Vector, _num_subdivisions: u32| { let shape = RoundShape { border_radius: this.border_radius, inner_shape: this.inner_shape.clone().scaled(scale)?, @@ -1665,7 +1627,7 @@ impl_shape_for_round_shape!( impl_shape_for_round_shape!( Cylinder, RoundCylinder, - (|this: &Self, scale: &Vector, num_subdivisions: u32| { + (|this: &Self, scale: Vector, num_subdivisions: u32| { Some( this.inner_shape .scaled(scale, num_subdivisions)? @@ -1690,7 +1652,7 @@ impl_shape_for_round_shape!( impl_shape_for_round_shape!( Cone, RoundCone, - (|this: &Self, scale: &Vector, num_subdivisions: u32| { + (|this: &Self, scale: Vector, num_subdivisions: u32| { Some( this.inner_shape .scaled(scale, num_subdivisions)? @@ -1717,7 +1679,7 @@ impl_shape_for_round_shape!( impl_shape_for_round_shape!( ConvexPolyhedron, RoundConvexPolyhedron, - (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + (|this: &Self, scale: Vector, _num_subdivisions: u32| { let shape = RoundShape { border_radius: this.border_radius, inner_shape: this.inner_shape.clone().scaled(scale)?, diff --git a/src/shape/shared_shape.rs b/src/shape/shared_shape.rs index 0ac5d0c4..b83c4772 100644 --- a/src/shape/shared_shape.rs +++ b/src/shape/shared_shape.rs @@ -1,5 +1,5 @@ use super::TriMeshBuilderError; -use crate::math::{Isometry, Point, Real, Vector, DIM}; +use crate::math::{IVector, Pose, Real, Vector, DIM}; #[cfg(feature = "dim2")] use crate::shape::ConvexPolygon; #[cfg(feature = "serde-serialize")] @@ -14,11 +14,12 @@ use crate::shape::{ use crate::shape::{Cone, ConvexPolyhedron, Cylinder}; use crate::transformation::vhacd::{VHACDParameters, VHACD}; use crate::transformation::voxelization::{FillMode, VoxelSet}; +#[cfg(feature = "dim3")] +use crate::utils::Array2; use alloc::sync::Arc; use alloc::{vec, vec::Vec}; use core::fmt; use core::ops::Deref; -use na::Unit; /// A reference-counted, shareable geometric shape. /// @@ -157,32 +158,32 @@ impl SharedShape { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::SharedShape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// # use parry3d::math::Isometry; + /// # use parry3d::math::Pose; /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::shape::SharedShape; /// # #[cfg(all(feature = "dim2", feature = "f32"))] { - /// # use parry2d::math::Isometry; + /// # use parry2d::math::Pose; /// /// let ball1 = SharedShape::ball(0.5); /// let ball2 = SharedShape::ball(0.5); /// /// #[cfg(feature = "dim3")] /// let compound = SharedShape::compound(vec![ - /// (Isometry::translation(1.0, 0.0, 0.0), ball1), - /// (Isometry::translation(-1.0, 0.0, 0.0), ball2), + /// (Pose::translation(1.0, 0.0, 0.0), ball1), + /// (Pose::translation(-1.0, 0.0, 0.0), ball2), /// ]); /// /// #[cfg(feature = "dim2")] /// let compound = SharedShape::compound(vec![ - /// (Isometry::translation(1.0, 0.0), ball1), - /// (Isometry::translation(-1.0, 0.0), ball2), + /// (Pose::translation(1.0, 0.0), ball1), + /// (Pose::translation(-1.0, 0.0), ball2), /// ]); /// # } /// # } /// # } /// # } /// ``` - pub fn compound(shapes: Vec<(Isometry, SharedShape)>) -> Self { + pub fn compound(shapes: Vec<(Pose, SharedShape)>) -> Self { let raw_shapes = shapes.into_iter().map(|s| (s.0, s.1)).collect(); let compound = Compound::new(raw_shapes); SharedShape(Arc::new(compound)) @@ -210,7 +211,7 @@ impl SharedShape { } /// Initialize a plane shape defined by its outward normal. - pub fn halfspace(outward_normal: Unit>) -> Self { + pub fn halfspace(outward_normal: Vector) -> Self { SharedShape(Arc::new(HalfSpace::new(outward_normal))) } @@ -300,13 +301,13 @@ impl SharedShape { } /// Initialize a capsule shape from its endpoints and radius. - pub fn capsule(a: Point, b: Point, radius: Real) -> Self { + pub fn capsule(a: Vector, b: Vector, radius: Real) -> Self { SharedShape(Arc::new(Capsule::new(a, b, radius))) } /// Initialize a capsule shape aligned with the `x` axis. pub fn capsule_x(half_height: Real, radius: Real) -> Self { - let p = Point::from(Vector::x() * half_height); + let p = Vector::X * half_height; Self::capsule(-p, p, radius) } @@ -328,33 +329,28 @@ impl SharedShape { /// # } /// ``` pub fn capsule_y(half_height: Real, radius: Real) -> Self { - let p = Point::from(Vector::y() * half_height); + let p = Vector::Y * half_height; Self::capsule(-p, p, radius) } /// Initialize a capsule shape aligned with the `z` axis. #[cfg(feature = "dim3")] pub fn capsule_z(half_height: Real, radius: Real) -> Self { - let p = Point::from(Vector::z() * half_height); + let p = Vector::Z * half_height; Self::capsule(-p, p, radius) } /// Initialize a segment shape from its endpoints. - pub fn segment(a: Point, b: Point) -> Self { + pub fn segment(a: Vector, b: Vector) -> Self { SharedShape(Arc::new(Segment::new(a, b))) } /// Initializes a triangle shape. - pub fn triangle(a: Point, b: Point, c: Point) -> Self { + pub fn triangle(a: Vector, b: Vector, c: Vector) -> Self { SharedShape(Arc::new(Triangle::new(a, b, c))) } /// Initializes a triangle shape with round corners. - pub fn round_triangle( - a: Point, - b: Point, - c: Point, - border_radius: Real, - ) -> Self { + pub fn round_triangle(a: Vector, b: Vector, c: Vector, border_radius: Real) -> Self { SharedShape(Arc::new(RoundShape { inner_shape: Triangle::new(a, b, c), border_radius, @@ -364,7 +360,7 @@ impl SharedShape { /// Initializes a polyline shape defined by its vertex and index buffers. /// /// If no index buffer is provided, the polyline is assumed to describe a line strip. - pub fn polyline(vertices: Vec>, indices: Option>) -> Self { + pub fn polyline(vertices: Vec, indices: Option>) -> Self { SharedShape(Arc::new(Polyline::new(vertices, indices))) } @@ -383,17 +379,17 @@ impl SharedShape { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::SharedShape; /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// # use parry3d::math::Point; + /// # use parry3d::math::Vector; /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::shape::SharedShape; /// # #[cfg(all(feature = "dim2", feature = "f32"))] { - /// # use parry2d::math::Point; + /// # use parry2d::math::Vector; /// /// #[cfg(feature = "dim3")] /// let vertices = vec![ - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// #[cfg(feature = "dim3")] /// let indices = vec![[0, 1, 2]]; @@ -405,7 +401,7 @@ impl SharedShape { /// # } /// ``` pub fn trimesh( - vertices: Vec>, + vertices: Vec, indices: Vec<[u32; 3]>, ) -> Result { Ok(SharedShape(Arc::new(TriMesh::new(vertices, indices)?))) @@ -414,7 +410,7 @@ impl SharedShape { /// Initializes a triangle mesh shape defined by its vertex and index buffers and /// pre-processing flags. pub fn trimesh_with_flags( - vertices: Vec>, + vertices: Vec, indices: Vec<[u32; 3]>, flags: TriMeshFlags, ) -> Result { @@ -432,7 +428,7 @@ impl SharedShape { /// For initializing a voxels shape from a mesh to voxelize, see [`Self::voxelized_mesh`]. /// For initializing multiple voxels shape from the convex decomposition of a mesh, see /// [`Self::voxelized_convex_decomposition`]. - pub fn voxels(voxel_size: Vector, grid_coords: &[Point]) -> Self { + pub fn voxels(voxel_size: Vector, grid_coords: &[IVector]) -> Self { let shape = Voxels::new(voxel_size, grid_coords); SharedShape::new(shape) } @@ -441,7 +437,7 @@ impl SharedShape { /// /// Each voxel has the size `voxel_size` and contains at least one point from `centers`. /// The `primitive_geometry` controls the behavior of collision detection at voxels boundaries. - pub fn voxels_from_points(voxel_size: Vector, points: &[Point]) -> Self { + pub fn voxels_from_points(voxel_size: Vector, points: &[Vector]) -> Self { let shape = Voxels::from_points(voxel_size, points); SharedShape::new(shape) } @@ -449,7 +445,7 @@ impl SharedShape { /// Initializes a voxels shape obtained from the decomposition of the given trimesh (in 3D) /// or polyline (in 2D) into voxelized convex parts. pub fn voxelized_mesh( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], voxel_size: Real, fill_mode: FillMode, @@ -465,14 +461,14 @@ impl SharedShape { .iter() .map(|v| vox_set.get_voxel_point(v)) .collect(); - let shape = Voxels::from_points(Vector::repeat(vox_set.scale), ¢ers); + let shape = Voxels::from_points(Vector::splat(vox_set.scale), ¢ers); SharedShape::new(shape) } /// Initializes a compound shape obtained from the decomposition of the given trimesh (in 3D) /// or polyline (in 2D) into voxelized convex parts. pub fn voxelized_convex_decomposition( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], ) -> Vec { Self::voxelized_convex_decomposition_with_params( @@ -485,7 +481,7 @@ impl SharedShape { /// Initializes a compound shape obtained from the decomposition of the given trimesh (in 3D) /// or polyline (in 2D) into voxelized convex parts. pub fn voxelized_convex_decomposition_with_params( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], params: &VHACDParameters, ) -> Vec { @@ -501,14 +497,14 @@ impl SharedShape { /// Initializes a compound shape obtained from the decomposition of the given trimesh (in 3D) or /// polyline (in 2D) into convex parts. - pub fn convex_decomposition(vertices: &[Point], indices: &[[u32; DIM]]) -> Self { + pub fn convex_decomposition(vertices: &[Vector], indices: &[[u32; DIM]]) -> Self { Self::convex_decomposition_with_params(vertices, indices, &VHACDParameters::default()) } /// Initializes a compound shape obtained from the decomposition of the given trimesh (in 3D) or /// polyline (in 2D) into convex parts dilated with round corners. pub fn round_convex_decomposition( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], border_radius: Real, ) -> Self { @@ -523,7 +519,7 @@ impl SharedShape { /// Initializes a compound shape obtained from the decomposition of the given trimesh (in 3D) or /// polyline (in 2D) into convex parts. pub fn convex_decomposition_with_params( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], params: &VHACDParameters, ) -> Self { @@ -533,14 +529,14 @@ impl SharedShape { #[cfg(feature = "dim2")] for vertices in decomp.compute_exact_convex_hulls(vertices, indices) { if let Some(convex) = Self::convex_polyline(vertices) { - parts.push((Isometry::identity(), convex)); + parts.push((Pose::IDENTITY, convex)); } } #[cfg(feature = "dim3")] for (vertices, indices) in decomp.compute_exact_convex_hulls(vertices, indices) { if let Some(convex) = Self::convex_mesh(vertices, &indices) { - parts.push((Isometry::identity(), convex)); + parts.push((Pose::IDENTITY, convex)); } } @@ -550,7 +546,7 @@ impl SharedShape { /// Initializes a compound shape obtained from the decomposition of the given trimesh (in 3D) or /// polyline (in 2D) into convex parts dilated with round corners. pub fn round_convex_decomposition_with_params( - vertices: &[Point], + vertices: &[Vector], indices: &[[u32; DIM]], params: &VHACDParameters, border_radius: Real, @@ -561,14 +557,14 @@ impl SharedShape { #[cfg(feature = "dim2")] for vertices in decomp.compute_exact_convex_hulls(vertices, indices) { if let Some(convex) = Self::round_convex_polyline(vertices, border_radius) { - parts.push((Isometry::identity(), convex)); + parts.push((Pose::IDENTITY, convex)); } } #[cfg(feature = "dim3")] for (vertices, indices) in decomp.compute_exact_convex_hulls(vertices, indices) { if let Some(convex) = Self::round_convex_mesh(vertices, &indices, border_radius) { - parts.push((Isometry::identity(), convex)); + parts.push((Pose::IDENTITY, convex)); } } @@ -576,7 +572,7 @@ impl SharedShape { } /// Creates a new shared shape that is the convex-hull of the given points. - pub fn convex_hull(points: &[Point]) -> Option { + pub fn convex_hull(points: &[Vector]) -> Option { #[cfg(feature = "dim2")] return ConvexPolygon::from_convex_hull(points).map(|ch| SharedShape(Arc::new(ch))); #[cfg(feature = "dim3")] @@ -596,7 +592,7 @@ impl SharedShape { /// /// Returns `None` if all points form an almost flat line. #[cfg(feature = "dim2")] - pub fn convex_polyline(points: Vec>) -> Option { + pub fn convex_polyline(points: Vec) -> Option { ConvexPolygon::from_convex_polyline(points).map(|ch| SharedShape(Arc::new(ch))) } @@ -608,7 +604,7 @@ impl SharedShape { /// /// Returns `None` if `points` doesn’t contain at least three points. #[cfg(feature = "dim2")] - pub fn convex_polyline_unmodified(points: Vec>) -> Option { + pub fn convex_polyline_unmodified(points: Vec) -> Option { ConvexPolygon::from_convex_polyline_unmodified(points).map(|ch| SharedShape(Arc::new(ch))) } @@ -616,13 +612,13 @@ impl SharedShape { /// given set of points assumed to form a convex mesh (no convex-hull will be automatically /// computed). #[cfg(feature = "dim3")] - pub fn convex_mesh(points: Vec>, indices: &[[u32; 3]]) -> Option { + pub fn convex_mesh(points: Vec, indices: &[[u32; 3]]) -> Option { ConvexPolyhedron::from_convex_mesh(points, indices).map(|ch| SharedShape(Arc::new(ch))) } /// Creates a new shared shape with rounded corners that is the /// convex-hull of the given points, dilated by `border_radius`. - pub fn round_convex_hull(points: &[Point], border_radius: Real) -> Option { + pub fn round_convex_hull(points: &[Vector], border_radius: Real) -> Option { #[cfg(feature = "dim2")] return ConvexPolygon::from_convex_hull(points).map(|ch| { SharedShape(Arc::new(RoundShape { @@ -643,7 +639,7 @@ impl SharedShape { /// given set of points assumed to form a convex polyline (no convex-hull will be automatically /// computed). #[cfg(feature = "dim2")] - pub fn round_convex_polyline(points: Vec>, border_radius: Real) -> Option { + pub fn round_convex_polyline(points: Vec, border_radius: Real) -> Option { ConvexPolygon::from_convex_polyline(points).map(|ch| { SharedShape(Arc::new(RoundShape { inner_shape: ch, @@ -657,7 +653,7 @@ impl SharedShape { /// computed). #[cfg(feature = "dim3")] pub fn round_convex_mesh( - points: Vec>, + points: Vec, indices: &[[u32; 3]], border_radius: Real, ) -> Option { @@ -672,14 +668,14 @@ impl SharedShape { /// Initializes a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim2")] - pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + pub fn heightfield(heights: Vec, scale: Vector) -> Self { SharedShape(Arc::new(HeightField::new(heights, scale))) } /// Initializes a heightfield shape on the x-z plane defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim3")] - pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { + pub fn heightfield(heights: Array2, scale: Vector) -> Self { SharedShape(Arc::new(HeightField::new(heights, scale))) } @@ -687,8 +683,8 @@ impl SharedShape { /// factor along each coordinate axis, and [`HeightFieldFlags`]. #[cfg(feature = "dim3")] pub fn heightfield_with_flags( - heights: na::DMatrix, - scale: Vector, + heights: Array2, + scale: Vector, flags: HeightFieldFlags, ) -> Self { SharedShape(Arc::new(HeightField::with_flags(heights, scale, flags))) diff --git a/src/shape/support_map.rs b/src/shape/support_map.rs index 46391c84..262490f8 100644 --- a/src/shape/support_map.rs +++ b/src/shape/support_map.rs @@ -88,8 +88,7 @@ //! convex parts or handled with different algorithms. This is why Parry provides composite //! shapes and specialized algorithms for triangle meshes. -use crate::math::{Isometry, Point, Real, Vector}; -use na::Unit; +use crate::math::{Pose, Vector}; /// Trait for convex shapes representable by a support mapping function. /// @@ -117,16 +116,14 @@ use na::Unit; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, Cuboid, SupportMap}; -/// use parry3d::math::{Point, Vector, Real}; -/// extern crate nalgebra as na; -/// use parry3d::na::Vector3; +/// use parry3d::math::{Vector, Real}; /// /// // Create a ball (sphere) with radius 1.0 /// let ball = Ball::new(1.0); /// /// // Get the support point in the direction (1, 0, 0) - pointing right -/// let dir = Vector3::new(1.0, 0.0, 0.0); -/// let support_point = ball.local_support_point(&dir); +/// let dir = Vector::new(1.0, 0.0, 0.0); +/// let support_point = ball.local_support_point(dir); /// /// // For a ball centered at origin, this should be approximately (1, 0, 0) /// assert!((support_point.x - 1.0).abs() < 1e-6); @@ -134,39 +131,38 @@ use na::Unit; /// assert!(support_point.z.abs() < 1e-6); /// /// // Try another direction - diagonal up and right -/// let dir2 = Vector3::new(1.0, 1.0, 0.0); -/// let support_point2 = ball.local_support_point(&dir2); +/// let dir2 = Vector::new(1.0, 1.0, 0.0); +/// let support_point2 = ball.local_support_point(dir2); /// /// // The point should be on the surface of the ball (distance = radius) -/// let distance = (support_point2.coords.norm() - 1.0).abs(); +/// let distance = (support_point2.length() - 1.0).abs(); /// assert!(distance < 1e-6); /// # } /// ``` /// -/// ## Support Points on a Cuboid +/// ## Support Vectors on a Cuboid /// /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Cuboid, SupportMap}; -/// extern crate nalgebra as na; -/// use parry3d::na::Vector3; +/// use parry3d::math::Vector; /// /// // Create a cuboid (box) with half-extents 2x3x4 -/// let cuboid = Cuboid::new(Vector3::new(2.0, 3.0, 4.0)); +/// let cuboid = Cuboid::new(Vector::new(2.0, 3.0, 4.0)); /// /// // Support point in positive X direction should be at the right face -/// let dir_x = Vector3::new(1.0, 0.0, 0.0); -/// let support_x = cuboid.local_support_point(&dir_x); +/// let dir_x = Vector::new(1.0, 0.0, 0.0); +/// let support_x = cuboid.local_support_point(dir_x); /// assert!((support_x.x - 2.0).abs() < 1e-6); /// /// // Support point in negative Y direction should be at the bottom face -/// let dir_neg_y = Vector3::new(0.0, -1.0, 0.0); -/// let support_neg_y = cuboid.local_support_point(&dir_neg_y); +/// let dir_neg_y = Vector::new(0.0, -1.0, 0.0); +/// let support_neg_y = cuboid.local_support_point(dir_neg_y); /// assert!((support_neg_y.y + 3.0).abs() < 1e-6); /// /// // Support point in diagonal direction should be at a corner -/// let dir_diag = Vector3::new(1.0, 1.0, 1.0); -/// let support_diag = cuboid.local_support_point(&dir_diag); +/// let dir_diag = Vector::new(1.0, 1.0, 1.0); +/// let support_diag = cuboid.local_support_point(dir_diag); /// assert!((support_diag.x - 2.0).abs() < 1e-6); /// assert!((support_diag.y - 3.0).abs() < 1e-6); /// assert!((support_diag.z - 4.0).abs() < 1e-6); @@ -183,9 +179,7 @@ use na::Unit; /// # // since we can't implement traits for external types in doc tests. /// # // It's here for educational purposes. /// use parry3d::shape::SupportMap; -/// use parry3d::math::{Point, Vector, Real}; -/// extern crate nalgebra as na; -/// use parry3d::na::Vector3; +/// use parry3d::math::{Vector, Real}; /// /// // A simple pill-shaped object aligned with the X axis /// struct SimplePill { @@ -194,14 +188,14 @@ use na::Unit; /// } /// /// impl SupportMap for SimplePill { -/// fn local_support_point(&self, dir: &Vector) -> Point { +/// fn local_support_point(&self, dir: Vector) -> Vector { /// // Support point is on one of the spherical ends /// // Choose the end that's in the direction of dir.x /// let center_x = if dir.x >= 0.0 { self.half_length } else { -self.half_length }; /// /// // From that center, extend by radius in the direction of dir /// let dir_normalized = dir.normalize(); -/// Point::new( +/// Vector::new( /// center_x + dir_normalized.x * self.radius, /// dir_normalized.y * self.radius, /// dir_normalized.z * self.radius, @@ -239,14 +233,13 @@ pub trait SupportMap { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, SupportMap}; - /// extern crate nalgebra as na; - /// use parry3d::na::Vector3; + /// use parry3d::math::Vector; /// /// let ball = Ball::new(2.5); /// /// // Support point pointing up (Z direction) - /// let up = Vector3::new(0.0, 0.0, 1.0); - /// let support_up = ball.local_support_point(&up); + /// let up = Vector::new(0.0, 0.0, 1.0); + /// let support_up = ball.local_support_point(up); /// /// // Should be at the top of the ball /// assert!((support_up.z - 2.5).abs() < 1e-6); @@ -254,8 +247,8 @@ pub trait SupportMap { /// assert!(support_up.y.abs() < 1e-6); /// /// // Support point pointing in negative X direction - /// let left = Vector3::new(-1.0, 0.0, 0.0); - /// let support_left = ball.local_support_point(&left); + /// let left = Vector::new(-1.0, 0.0, 0.0); + /// let support_left = ball.local_support_point(left); /// /// // Should be at the left side of the ball /// assert!((support_left.x + 2.5).abs() < 1e-6); @@ -267,7 +260,7 @@ pub trait SupportMap { /// The "local" prefix means the point is in the shape's own coordinate system, before /// any rotation or translation is applied. For transformed shapes, use /// [`support_point`](SupportMap::support_point) instead. - fn local_support_point(&self, dir: &Vector) -> Point; + fn local_support_point(&self, dir: Vector) -> Vector; /// Same as [`local_support_point`](SupportMap::local_support_point) except that `dir` is /// guaranteed to be normalized (unit length). @@ -284,23 +277,22 @@ pub trait SupportMap { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, SupportMap}; - /// extern crate nalgebra as na; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::Vector; /// /// let ball = Ball::new(1.5); /// /// // Create a normalized direction vector - /// let dir = Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)); + /// let dir = Vector::new(1.0, 1.0, 0.0).normalize(); /// - /// let support = ball.local_support_point_toward(&dir); + /// let support = ball.local_support_point_toward(dir); /// /// // The support point should be on the sphere's surface - /// let distance_from_origin = support.coords.norm(); + /// let distance_from_origin = support.length(); /// assert!((distance_from_origin - 1.5).abs() < 1e-6); /// # } /// ``` - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.local_support_point(dir.as_ref()) + fn local_support_point_toward(&self, dir: Vector) -> Vector { + self.local_support_point(dir) } /// Evaluates the support function of this shape transformed by `transform`. @@ -332,18 +324,16 @@ pub trait SupportMap { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, SupportMap}; - /// use parry3d::math::Isometry; - /// extern crate nalgebra as na; - /// use parry3d::na::{Vector3, Translation3, UnitQuaternion}; + /// use parry3d::math::{Pose, Vector}; /// /// let ball = Ball::new(1.0); /// /// // Create a transformation: translate the ball to (10, 0, 0) - /// let transform = Isometry::translation(10.0, 0.0, 0.0); + /// let transform = Pose::translation(10.0, 0.0, 0.0); /// /// // Get support point in the positive X direction - /// let dir = Vector3::new(1.0, 0.0, 0.0); - /// let support = ball.support_point(&transform, &dir); + /// let dir = Vector::new(1.0, 0.0, 0.0); + /// let support = ball.support_point(&transform, dir); /// /// // The support point should be at (11, 0, 0) - the rightmost point of the translated ball /// assert!((support.x - 11.0).abs() < 1e-6); @@ -357,29 +347,27 @@ pub trait SupportMap { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Cuboid, SupportMap}; - /// use parry3d::math::Isometry; - /// extern crate nalgebra as na; - /// use parry3d::na::{Vector3, UnitQuaternion}; - /// use std::f32::consts::PI; + /// use parry3d::math::{Pose, Rotation, Vector}; + /// use core::f32::consts::PI; /// - /// let cuboid = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)); + /// let cuboid = Cuboid::new(Vector::new(2.0, 1.0, 1.0)); /// /// // Rotate the cuboid 90 degrees around the Z axis - /// let rotation = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.0); - /// let transform = Isometry::from_parts(Vector3::zeros().into(), rotation); + /// let rotation = Rotation::from_axis_angle(Vector::Z, PI / 2.0); + /// let transform = Pose::from_parts(Vector::ZERO, rotation); /// /// // In world space, ask for support in the X direction - /// let dir = Vector3::new(1.0, 0.0, 0.0); - /// let support = cuboid.support_point(&transform, &dir); + /// let dir = Vector::new(1.0, 0.0, 0.0); + /// let support = cuboid.support_point(&transform, dir); /// /// // After 90° rotation, the long axis (originally X) now points in Y direction /// // So the support in X direction comes from the short axis /// assert!(support.x.abs() <= 1.0 + 1e-5); // Should be around 1.0 (the short axis) - /// } + /// # } /// ``` - fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { - let local_dir = transform.inverse_transform_vector(dir); - transform * self.local_support_point(&local_dir) + fn support_point(&self, transform: &Pose, dir: Vector) -> Vector { + let local_dir = transform.rotation.inverse() * dir; + transform * self.local_support_point(local_dir) } /// Same as [`support_point`](SupportMap::support_point) except that `dir` is guaranteed @@ -398,29 +386,27 @@ pub trait SupportMap { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, SupportMap}; - /// use parry3d::math::Isometry; - /// extern crate nalgebra as na; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Pose, Vector}; /// /// let ball = Ball::new(2.0); /// /// // Translate the ball - /// let transform = Isometry::translation(5.0, 3.0, -2.0); + /// let transform = Pose::translation(5.0, 3.0, -2.0); /// /// // Create a normalized direction - /// let dir = Unit::new_normalize(Vector3::new(1.0, 1.0, 1.0)); + /// let dir = Vector::new(1.0, 1.0, 1.0).normalize(); /// - /// let support = ball.support_point_toward(&transform, &dir); + /// let support = ball.support_point_toward(&transform, dir); /// /// // The support point should be 2.0 units away from the center in the diagonal direction - /// let center = Vector3::new(5.0, 3.0, -2.0); - /// let offset = support.coords - center; - /// let distance = offset.norm(); + /// let center = Vector::new(5.0, 3.0, -2.0); + /// let offset = support - center; + /// let distance = offset.length(); /// assert!((distance - 2.0).abs() < 1e-6); /// /// // The offset should be parallel to the direction /// let normalized_offset = offset.normalize(); - /// assert!((normalized_offset.dot(&dir) - 1.0).abs() < 1e-6); + /// assert!((normalized_offset.dot(dir) - 1.0).abs() < 1e-6); /// # } /// ``` /// @@ -432,36 +418,30 @@ pub trait SupportMap { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Ball, Cuboid, SupportMap}; - /// use parry3d::math::Isometry; - /// extern crate nalgebra as na; - /// use parry3d::na::{Vector3, Unit}; + /// use parry3d::math::{Pose, Vector}; /// /// // Two shapes at different positions /// let ball = Ball::new(1.0); - /// let cuboid = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); + /// let cuboid = Cuboid::new(Vector::new(0.5, 0.5, 0.5)); /// - /// let ball_pos = Isometry::translation(0.0, 0.0, 0.0); - /// let cuboid_pos = Isometry::translation(3.0, 0.0, 0.0); + /// let ball_pos = Pose::translation(0.0, 0.0, 0.0); + /// let cuboid_pos = Pose::translation(3.0, 0.0, 0.0); /// /// // Direction from ball to cuboid - /// let dir = Unit::new_normalize(Vector3::new(1.0, 0.0, 0.0)); + /// let dir = Vector::new(1.0, 0.0, 0.0).normalize(); /// /// // Get support points for the Minkowski difference (used in GJK) - /// let support_ball = ball.support_point_toward(&ball_pos, &dir); - /// let support_cuboid = cuboid.support_point_toward(&cuboid_pos, &-dir); + /// let support_ball = ball.support_point_toward(&ball_pos, dir); + /// let support_cuboid = cuboid.support_point_toward(&cuboid_pos, -dir); /// /// // The Minkowski difference support point /// let minkowski_support = support_ball - support_cuboid; /// /// println!("Support point for Minkowski difference: {:?}", minkowski_support); - /// } + /// # } /// ``` - fn support_point_toward( - &self, - transform: &Isometry, - dir: &Unit>, - ) -> Point { - let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir)); - transform * self.local_support_point_toward(&local_dir) + fn support_point_toward(&self, transform: &Pose, dir: Vector) -> Vector { + let local_dir = transform.rotation.inverse() * dir; + transform * self.local_support_point_toward(local_dir) } } diff --git a/src/shape/tetrahedron.rs b/src/shape/tetrahedron.rs index 9167cd59..4f2fd0a2 100644 --- a/src/shape/tetrahedron.rs +++ b/src/shape/tetrahedron.rs @@ -1,16 +1,15 @@ //! Definition of the tetrahedron shape. -use crate::math::{Matrix, Point, Real}; +use crate::math::{MatExt, Matrix, Real, Vector}; use crate::shape::{Segment, Triangle}; use crate::utils; use core::mem; -use na::Matrix3; -#[cfg(all(feature = "dim2", not(feature = "std")))] -use na::ComplexField; // for .abs() +#[cfg(feature = "dim3")] +use crate::math::CrossMatrix; // Mat3 for glam -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; +#[cfg(all(feature = "dim2", not(feature = "std")))] +use crate::math::ComplexField; // for .abs() /// A tetrahedron with 4 vertices. /// @@ -43,14 +42,14 @@ use rkyv::{bytecheck, CheckBytes}; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// /// // Create a tetrahedron with vertices at unit positions /// let tetra = Tetrahedron::new( -/// Point::new(0.0, 0.0, 0.0), // vertex a -/// Point::new(1.0, 0.0, 0.0), // vertex b -/// Point::new(0.0, 1.0, 0.0), // vertex c -/// Point::new(0.0, 0.0, 1.0), // vertex d +/// Vector::new(0.0, 0.0, 0.0), // vertex a +/// Vector::new(1.0, 0.0, 0.0), // vertex b +/// Vector::new(0.0, 1.0, 0.0), // vertex c +/// Vector::new(0.0, 0.0, 1.0), // vertex d /// ); /// /// println!("First vertex: {:?}", tetra.a); @@ -62,14 +61,14 @@ use rkyv::{bytecheck, CheckBytes}; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// /// // Create a regular tetrahedron /// let tetra = Tetrahedron::new( -/// Point::new(0.0, 0.0, 0.0), -/// Point::new(1.0, 0.0, 0.0), -/// Point::new(0.5, 0.866, 0.0), -/// Point::new(0.5, 0.433, 0.816), +/// Vector::new(0.0, 0.0, 0.0), +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.5, 0.866, 0.0), +/// Vector::new(0.5, 0.433, 0.816), /// ); /// /// let volume = tetra.volume(); @@ -83,13 +82,13 @@ use rkyv::{bytecheck, CheckBytes}; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( -/// Point::new(0.0, 0.0, 0.0), -/// Point::new(1.0, 0.0, 0.0), -/// Point::new(0.0, 1.0, 0.0), -/// Point::new(0.0, 0.0, 1.0), +/// Vector::new(0.0, 0.0, 0.0), +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ); /// /// // Get the first face (triangle ABC) @@ -105,20 +104,19 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(Copy, Clone, Debug)] #[repr(C)] pub struct Tetrahedron { /// The tetrahedron's first vertex. - pub a: Point, + pub a: Vector, /// The tetrahedron's second vertex. - pub b: Point, + pub b: Vector, /// The tetrahedron's third vertex. - pub c: Point, + pub c: Vector, /// The tetrahedron's fourth vertex. - pub d: Point, + pub d: Vector, } /// Logical description of the location of a point on a tetrahedron. @@ -129,23 +127,23 @@ pub struct Tetrahedron { /// /// # Variants /// -/// - `OnVertex`: Point is at one of the four vertices (0=a, 1=b, 2=c, 3=d) -/// - `OnEdge`: Point lies on one of the six edges with barycentric coordinates -/// - `OnFace`: Point lies on one of the four triangular faces with barycentric coordinates -/// - `OnSolid`: Point is inside the tetrahedron volume +/// - `OnVertex`: Vector is at one of the four vertices (0=a, 1=b, 2=c, 3=d) +/// - `OnEdge`: Vector lies on one of the six edges with barycentric coordinates +/// - `OnFace`: Vector lies on one of the four triangular faces with barycentric coordinates +/// - `OnSolid`: Vector is inside the tetrahedron volume /// /// # Examples /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Tetrahedron, TetrahedronPointLocation}; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( -/// Point::new(0.0, 0.0, 0.0), -/// Point::new(1.0, 0.0, 0.0), -/// Point::new(0.0, 1.0, 0.0), -/// Point::new(0.0, 0.0, 1.0), +/// Vector::new(0.0, 0.0, 0.0), +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ); /// /// // Check location of vertex @@ -312,21 +310,21 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// // Create a simple tetrahedron /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// /// assert!(tetra.volume() > 0.0); /// # } /// ``` #[inline] - pub fn new(a: Point, b: Point, c: Point, d: Point) -> Tetrahedron { + pub fn new(a: Vector, b: Vector, c: Vector, d: Vector) -> Tetrahedron { Tetrahedron { a, b, c, d } } @@ -340,13 +338,13 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let points = [ - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ]; /// /// let tetra = Tetrahedron::from_array(&points); @@ -354,7 +352,7 @@ impl Tetrahedron { /// assert_eq!(tetra.b, points[1]); /// # } /// ``` - pub fn from_array(arr: &[Point; 4]) -> &Tetrahedron { + pub fn from_array(arr: &[Vector; 4]) -> &Tetrahedron { unsafe { mem::transmute(arr) } } @@ -375,13 +373,13 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// /// // Get the first face (triangle ABC) @@ -465,13 +463,13 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// /// // Get edge 0 (segment AB) @@ -561,34 +559,34 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// - /// // Point at vertex a - /// let bcoords = tetra.barycentric_coordinates(&tetra.a).unwrap(); + /// // Vector at vertex a + /// let bcoords = tetra.barycentric_coordinates(tetra.a).unwrap(); /// assert!((bcoords[0] - 1.0).abs() < 1e-6); /// assert!(bcoords[1].abs() < 1e-6); /// - /// // Point at center + /// // Vector at center /// let center = tetra.center(); - /// let bcoords = tetra.barycentric_coordinates(¢er).unwrap(); + /// let bcoords = tetra.barycentric_coordinates(center).unwrap(); /// // All coordinates should be approximately 0.25 /// for coord in &bcoords { /// assert!((coord - 0.25).abs() < 1e-6); /// } /// # } /// ``` - pub fn barycentric_coordinates(&self, p: &Point) -> Option<[Real; 4]> { + pub fn barycentric_coordinates(&self, p: Vector) -> Option<[Real; 4]> { let ab = self.b - self.a; let ac = self.c - self.a; let ad = self.d - self.a; - let m = Matrix::new(ab.x, ac.x, ad.x, ab.y, ac.y, ad.y, ab.z, ac.z, ad.z); + let m = Matrix::from_cols_array(&[ab.x, ab.y, ab.z, ac.x, ac.y, ac.z, ad.x, ad.y, ad.z]); m.try_inverse().map(|im| { let bcoords = im * (p - self.a); @@ -615,13 +613,13 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// /// let volume = tetra.volume(); @@ -650,13 +648,13 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// /// let signed_vol = tetra.signed_volume(); @@ -677,11 +675,9 @@ impl Tetrahedron { let p1p3 = self.c - self.a; let p1p4 = self.d - self.a; - let mat = Matrix3::new( - p1p2[0], p1p3[0], p1p4[0], p1p2[1], p1p3[1], p1p4[1], p1p2[2], p1p3[2], p1p4[2], - ); + let mat = CrossMatrix::from_cols(p1p2, p1p3, p1p4); - mat.determinant() / na::convert::(6.0f64) + mat.determinant() / 6.0 } /// Computes the center of this tetrahedron. @@ -698,13 +694,13 @@ impl Tetrahedron { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Tetrahedron; - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// /// let tetra = Tetrahedron::new( - /// Point::new(0.0, 0.0, 0.0), - /// Point::new(1.0, 0.0, 0.0), - /// Point::new(0.0, 1.0, 0.0), - /// Point::new(0.0, 0.0, 1.0), + /// Vector::new(0.0, 0.0, 0.0), + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), /// ); /// /// let center = tetra.center(); @@ -713,14 +709,14 @@ impl Tetrahedron { /// assert!((center.z - 0.25).abs() < 1e-6); /// /// // The center has equal barycentric coordinates - /// let bcoords = tetra.barycentric_coordinates(¢er).unwrap(); + /// let bcoords = tetra.barycentric_coordinates(center).unwrap(); /// for coord in &bcoords { /// assert!((coord - 0.25).abs() < 1e-6); /// } /// # } /// ``` #[inline] - pub fn center(&self) -> Point { + pub fn center(&self) -> Vector { utils::center(&[self.a, self.b, self.c, self.d]) } } diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index fdbdad39..a90fe2f3 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -1,23 +1,19 @@ //! Definition of the triangle shape. -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{ComplexField, Pose, Real, Vector}; use crate::shape::SupportMap; use crate::shape::{PolygonalFeature, Segment}; use crate::utils; use core::mem; -use na::{self, ComplexField, Unit}; use num::Zero; #[cfg(feature = "dim3")] -use {crate::shape::FeatureId, core::f64}; +use crate::shape::FeatureId; #[cfg(feature = "dim2")] use crate::shape::PackedFeatureId; -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; - /// A triangle shape defined by three vertices. /// /// A triangle is one of the most fundamental shapes in computational geometry. @@ -55,21 +51,21 @@ use rkyv::{bytecheck, CheckBytes}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Triangle; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Create a right triangle in the XY plane /// let triangle = Triangle::new( -/// Point3::origin(), // a: origin -/// Point3::new(3.0, 0.0, 0.0), // b: along +X -/// Point3::new(0.0, 4.0, 0.0) // c: along +Y +/// Vector::ZERO, // a: origin +/// Vector::new(3.0, 0.0, 0.0), // b: along +X +/// Vector::new(0.0, 4.0, 0.0) // c: along +Y /// ); /// /// // Area of 3-4-5 right triangle is 6.0 /// assert_eq!(triangle.area(), 6.0); /// /// // Check if a point is inside -/// let inside = Point3::new(1.0, 1.0, 0.0); -/// assert!(triangle.contains_point(&inside)); +/// let inside = Vector::new(1.0, 1.0, 0.0); +/// assert!(triangle.contains_point(inside)); /// # } /// ``` #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] @@ -77,18 +73,17 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr(feature = "encase", derive(encase::ShaderType))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[derive(PartialEq, Debug, Copy, Clone, Default)] #[repr(C)] pub struct Triangle { /// The first vertex of the triangle. - pub a: Point, + pub a: Vector, /// The second vertex of the triangle. - pub b: Point, + pub b: Vector, /// The third vertex of the triangle. - pub c: Point, + pub c: Vector, } /// Description of the location of a point on a triangle. @@ -164,8 +159,8 @@ pub enum TriangleOrientation { Degenerate, } -impl From<[Point; 3]> for Triangle { - fn from(arr: [Point; 3]) -> Self { +impl From<[Vector; 3]> for Triangle { + fn from(arr: [Vector; 3]) -> Self { *Self::from_array(&arr) } } @@ -189,31 +184,31 @@ impl Triangle { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Triangle; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a triangle in the XY plane /// let tri = Triangle::new( - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0) + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0) /// ); /// /// assert_eq!(tri.area(), 0.5); /// # } /// ``` #[inline] - pub fn new(a: Point, b: Point, c: Point) -> Triangle { + pub fn new(a: Vector, b: Vector, c: Vector) -> Triangle { Triangle { a, b, c } } /// Creates the reference to a triangle from the reference to an array of three points. - pub fn from_array(arr: &[Point; 3]) -> &Triangle { + pub fn from_array(arr: &[Vector; 3]) -> &Triangle { unsafe { mem::transmute(arr) } } /// Reference to an array containing the three vertices of this triangle. #[inline] - pub fn vertices(&self) -> &[Point; 3] { + pub fn vertices(&self) -> &[Vector; 3] { unsafe { mem::transmute(self) } } @@ -223,8 +218,8 @@ impl Triangle { /// product). #[inline] #[cfg(feature = "dim3")] - pub fn normal(&self) -> Option>> { - Unit::try_new(self.scaled_normal(), crate::math::DEFAULT_EPSILON) + pub fn normal(&self) -> Option { + self.scaled_normal().try_normalize() } /// The three edges of this triangle: [AB, BC, CA]. @@ -238,36 +233,36 @@ impl Triangle { } /// Computes a scaled version of this triangle. - pub fn scaled(self, scale: &Vector) -> Self { - Self::new( - na::Scale::from(*scale) * self.a, - na::Scale::from(*scale) * self.b, - na::Scale::from(*scale) * self.c, - ) + pub fn scaled(self, scale: Vector) -> Self { + Self::new(self.a * scale, self.b * scale, self.c * scale) } /// Returns a new triangle with vertices transformed by `m`. #[inline] - pub fn transformed(&self, m: &Isometry) -> Self { + pub fn transformed(&self, m: &Pose) -> Self { Triangle::new(m * self.a, m * self.b, m * self.c) } /// The three edges scaled directions of this triangle: [B - A, C - B, A - C]. #[inline] - pub fn edges_scaled_directions(&self) -> [Vector; 3] { + pub fn edges_scaled_directions(&self) -> [Vector; 3] { [self.b - self.a, self.c - self.b, self.a - self.c] } /// Return the edge segment of this cuboid with a normal cone containing /// a direction that that maximizes the dot product with `local_dir`. - pub fn local_support_edge_segment(&self, dir: Vector) -> Segment { - let dots = na::Vector3::new( - dir.dot(&self.a.coords), - dir.dot(&self.b.coords), - dir.dot(&self.c.coords), - ); + pub fn local_support_edge_segment(&self, dir: Vector) -> Segment { + let dots = [dir.dot(self.a), dir.dot(self.b), dir.dot(self.c)]; + + let imin = if dots[0] <= dots[1] && dots[0] <= dots[2] { + 0 + } else if dots[1] <= dots[2] { + 1 + } else { + 2 + }; - match dots.imin() { + match imin { 0 => Segment::new(self.b, self.c), 1 => Segment::new(self.c, self.a), _ => Segment::new(self.a, self.b), @@ -277,22 +272,22 @@ impl Triangle { /// Return the face of this triangle with a normal that maximizes /// the dot product with `dir`. #[cfg(feature = "dim3")] - pub fn support_face(&self, _dir: Vector) -> PolygonalFeature { + pub fn support_face(&self, _dir: Vector) -> PolygonalFeature { PolygonalFeature::from(*self) } /// Return the face of this triangle with a normal that maximizes /// the dot product with `dir`. #[cfg(feature = "dim2")] - pub fn support_face(&self, dir: Vector) -> PolygonalFeature { + pub fn support_face(&self, dir: Vector) -> PolygonalFeature { let mut best = 0; let mut best_dot = -Real::MAX; for (i, tangent) in self.edges_scaled_directions().iter().enumerate() { let normal = Vector::new(tangent.y, -tangent.x); - if let Some(normal) = Unit::try_new(normal, 0.0) { - let dot = normal.dot(&dir); - if normal.dot(&dir) > best_dot { + if let Some(normal) = normal.try_normalize() { + let dot = normal.dot(dir); + if normal.dot(dir) > best_dot { best = i; best_dot = dot; } @@ -321,10 +316,10 @@ impl Triangle { /// [`Triangle::robust_scaled_normal`]. #[inline] #[cfg(feature = "dim3")] - pub fn scaled_normal(&self) -> Vector { + pub fn scaled_normal(&self) -> Vector { let ab = self.b - self.a; let ac = self.c - self.a; - ab.cross(&ac) + ab.cross(ac) } /// Find a triangle normal more robustly than with [`Triangle::scaled_normal`]. @@ -334,19 +329,19 @@ impl Triangle { /// computationally expensive. #[inline] #[cfg(feature = "dim3")] - pub fn robust_scaled_normal(&self) -> na::Vector3 { + pub fn robust_scaled_normal(&self) -> Vector { let pts = self.vertices(); let best_vertex = self.angle_closest_to_90(); let d1 = pts[(best_vertex + 2) % 3] - pts[(best_vertex + 1) % 3]; let d2 = pts[best_vertex] - pts[(best_vertex + 1) % 3]; - d1.cross(&d2) + d1.cross(d2) } /// Similar to [`Triangle::robust_scaled_normal`], but returns the unit length normal. #[inline] #[cfg(feature = "dim3")] - pub fn robust_normal(&self) -> na::Vector3 { + pub fn robust_normal(&self) -> Vector { self.robust_scaled_normal().normalize() } @@ -355,10 +350,10 @@ impl Triangle { /// This computes the min and max values of the dot products between each /// vertex of this triangle and `dir`. #[inline] - pub fn extents_on_dir(&self, dir: &Unit>) -> (Real, Real) { - let a = self.a.coords.dot(dir); - let b = self.b.coords.dot(dir); - let c = self.c.coords.dot(dir); + pub fn extents_on_dir(&self, dir: Vector) -> (Real, Real) { + let a = self.a.dot(dir); + let b = self.b.dot(dir); + let c = self.c.dot(dir); if a > b { if b > c { @@ -381,11 +376,11 @@ impl Triangle { } // // #[cfg(feature = "dim3")] - // fn support_feature_id_toward(&self, local_dir: &Unit>, eps: Real) -> FeatureId { + // fn support_feature_id_toward(&self, local_dir: Vector, eps: Real) -> FeatureId { // if let Some(normal) = self.normal() { // let (seps, ceps) = ComplexField::sin_cos(eps); // - // let normal_dot = local_dir.dot(&*normal); + // let normal_dot = local_dir.dot(*normal); // if normal_dot >= ceps { // FeatureId::Face(0) // } else if normal_dot <= -ceps { @@ -438,9 +433,9 @@ impl Triangle { #[inline] pub fn area(&self) -> Real { // Kahan's formula. - let a = na::distance(&self.a, &self.b); - let b = na::distance(&self.b, &self.c); - let c = na::distance(&self.c, &self.a); + let a = self.b.distance(self.a); + let b = self.c.distance(self.b); + let c = self.a.distance(self.c); let (c, b, a) = utils::sort3(&a, &b, &c); let a = *a; @@ -451,7 +446,7 @@ impl Triangle { // We take the max(0.0) because it can be slightly negative // because of numerical errors due to almost-degenerate triangles. - ComplexField::sqrt(sqr.max(0.0)) * 0.25 + ::sqrt(sqr.max(0.0)) * 0.25 } /// Computes the unit angular inertia of this triangle. @@ -470,27 +465,25 @@ impl Triangle { /// The geometric center of this triangle. #[inline] - pub fn center(&self) -> Point { + pub fn center(&self) -> Vector { utils::center(&[self.a, self.b, self.c]) } /// The perimeter of this triangle. #[inline] pub fn perimeter(&self) -> Real { - na::distance(&self.a, &self.b) - + na::distance(&self.b, &self.c) - + na::distance(&self.c, &self.a) + self.b.distance(self.a) + self.c.distance(self.b) + self.a.distance(self.c) } /// The circumcircle of this triangle. - pub fn circumcircle(&self) -> (Point, Real) { + pub fn circumcircle(&self) -> (Vector, Real) { let a = self.a - self.c; let b = self.b - self.c; - let na = a.norm_squared(); - let nb = b.norm_squared(); + let na = a.length_squared(); + let nb = b.length_squared(); - let dab = a.dot(&b); + let dab = a.dot(b); let denom = 2.0 * (na * nb - dab * dab); @@ -498,32 +491,32 @@ impl Triangle { // The triangle is degenerate (the three points are collinear). // So we find the longest segment and take its center. let c = self.a - self.b; - let nc = c.norm_squared(); + let nc = c.length_squared(); if nc >= na && nc >= nb { // Longest segment: [&self.a, &self.b] ( - na::center(&self.a, &self.b), - ComplexField::sqrt(nc) / na::convert::(2.0f64), + self.a.midpoint(self.b), + ::sqrt(nc) / 2.0, ) } else if na >= nb && na >= nc { // Longest segment: [&self.a, pc] ( - na::center(&self.a, &self.c), - ComplexField::sqrt(na) / na::convert::(2.0f64), + self.a.midpoint(self.c), + ::sqrt(na) / 2.0, ) } else { // Longest segment: [&self.b, &self.c] ( - na::center(&self.b, &self.c), - ComplexField::sqrt(nb) / na::convert::(2.0f64), + self.b.midpoint(self.c), + ::sqrt(nb) / 2.0, ) } } else { let k = b * na - a * nb; - let center = self.c + (a * k.dot(&b) - b * k.dot(&a)) / denom; - let radius = na::distance(&self.a, ¢er); + let center = self.c + (a * k.dot(b) - b * k.dot(a)) / denom; + let radius = center.distance(self.a); (center, radius) } @@ -536,7 +529,7 @@ impl Triangle { let p1p2 = self.b - self.a; let p1p3 = self.c - self.a; - relative_eq!(p1p2.cross(&p1p3).norm_squared(), 0.0, epsilon = EPS * EPS) + relative_eq!(p1p2.cross(p1p3).length_squared(), 0.0, epsilon = EPS * EPS) // relative_eq!( // self.area(), @@ -551,9 +544,9 @@ impl Triangle { let p1p2 = self.b - self.a; let p1p3 = self.c - self.a; relative_eq!( - p1p2.cross(&p1p3).norm(), + p1p2.cross(p1p3).length(), 0.0, - epsilon = eps * p1p2.norm().max(p1p3.norm()) + epsilon = eps * p1p2.length().max(p1p3.length()) ) // relative_eq!( @@ -565,13 +558,13 @@ impl Triangle { /// Tests if a point is inside of this triangle. #[cfg(feature = "dim2")] - pub fn contains_point(&self, p: &Point) -> bool { + pub fn contains_point(&self, p: Vector) -> bool { let ab = self.b - self.a; let bc = self.c - self.b; let ca = self.a - self.c; - let sgn1 = ab.perp(&(p - self.a)); - let sgn2 = bc.perp(&(p - self.b)); - let sgn3 = ca.perp(&(p - self.c)); + let sgn1 = ab.perp_dot(p - self.a); + let sgn2 = bc.perp_dot(p - self.b); + let sgn3 = ca.perp_dot(p - self.c); sgn1.signum() * sgn2.signum() >= 0.0 && sgn1.signum() * sgn3.signum() >= 0.0 && sgn2.signum() * sgn3.signum() >= 0.0 @@ -579,16 +572,16 @@ impl Triangle { /// Tests if a point is inside of this triangle. #[cfg(feature = "dim3")] - pub fn contains_point(&self, p: &Point) -> bool { + pub fn contains_point(&self, p: Vector) -> bool { const EPS: Real = crate::math::DEFAULT_EPSILON; let vb = self.b - self.a; let vc = self.c - self.a; let vp = p - self.a; - let n = vc.cross(&vb); - let n_norm = n.norm_squared(); - if n_norm < EPS || vp.dot(&n).abs() > EPS * n_norm { + let n = vc.cross(vb); + let n_norm = n.length_squared(); + if n_norm < EPS || vp.dot(n).abs() > EPS * n_norm { // the triangle is degenerate or the // point does not lie on the same plane as the triangle. return false; @@ -607,15 +600,15 @@ impl Triangle { // b = vb.dot(nc) * B and c = vc.dot(nb) * C - this results in harder-to-follow math but // hopefully fast code. - let nb = vb.cross(&n); - let nc = vc.cross(&n); + let nb = vb.cross(n); + let nc = vc.cross(n); - let signed_blim = vb.dot(&nc); - let b = vp.dot(&nc) * signed_blim.signum(); + let signed_blim = vb.dot(nc); + let b = vp.dot(nc) * signed_blim.signum(); let blim = signed_blim.abs(); - let signed_clim = vc.dot(&nb); - let c = vp.dot(&nb) * signed_clim.signum(); + let signed_clim = vc.dot(nb); + let c = vp.dot(nb) * signed_clim.signum(); let clim = signed_clim.abs(); c >= 0.0 && c <= clim && b >= 0.0 && b <= blim && c * blim + b * clim <= blim * clim @@ -623,7 +616,7 @@ impl Triangle { /// The normal of the given feature of this shape. #[cfg(feature = "dim3")] - pub fn feature_normal(&self, _: FeatureId) -> Option>> { + pub fn feature_normal(&self, _: FeatureId) -> Option { self.normal() } @@ -633,7 +626,7 @@ impl Triangle { /// smaller than `epsilon`. #[cfg(feature = "dim2")] pub fn orientation(&self, epsilon: Real) -> TriangleOrientation { - let area2 = (self.b - self.a).perp(&(self.c - self.a)); + let area2 = (self.b - self.a).perp_dot(self.c - self.a); // println!("area2: {}", area2); if area2 > epsilon { TriangleOrientation::CounterClockwise @@ -646,15 +639,15 @@ impl Triangle { /// The orientation of the 2D triangle, based on its signed area. /// - /// Returns `TriangleOrientation::Degenerate` if the triangle’s area is + /// Returns `TriangleOrientation::Degenerate` if the triangle's area is /// smaller than `epsilon`. pub fn orientation2d( - a: &na::Point2, - b: &na::Point2, - c: &na::Point2, + a: crate::math::Vector2, + b: crate::math::Vector2, + c: crate::math::Vector2, epsilon: Real, ) -> TriangleOrientation { - let area2 = (b - a).perp(&(c - a)); + let area2 = (b - a).perp_dot(c - a); // println!("area2: {}", area2); if area2 > epsilon { TriangleOrientation::CounterClockwise @@ -677,7 +670,7 @@ impl Triangle { let d1 = (points[i] - points[(i + 1) % 3]).normalize(); let d2 = (points[(i + 2) % 3] - points[(i + 1) % 3]).normalize(); - let cos_abs = d1.dot(&d2).abs(); + let cos_abs = d1.dot(d2).abs(); if cos_abs < best_cos { best_cos = cos_abs; @@ -696,10 +689,10 @@ impl Triangle { impl SupportMap for Triangle { #[inline] - fn local_support_point(&self, dir: &Vector) -> Point { - let d1 = self.a.coords.dot(dir); - let d2 = self.b.coords.dot(dir); - let d3 = self.c.coords.dot(dir); + fn local_support_point(&self, dir: Vector) -> Vector { + let d1 = self.a.dot(dir); + let d2 = self.b.dot(dir); + let d3 = self.c.dot(dir); if d1 > d2 { if d1 > d3 { @@ -718,7 +711,7 @@ impl SupportMap for Triangle { /* #[cfg(feature = "dim3")] impl ConvexPolyhedron for Triangle { - fn vertex(&self, id: FeatureId) -> Point { + fn vertex(&self, id: FeatureId) -> Vector { match id.unwrap_vertex() { 0 => self.a, 1 => self.b, @@ -726,7 +719,7 @@ impl ConvexPolyhedron for Triangle { _ => panic!("Triangle vertex index out of bounds."), } } - fn edge(&self, id: FeatureId) -> (Point, Point, FeatureId, FeatureId) { + fn edge(&self, id: FeatureId) -> (Vector, Vector, FeatureId, FeatureId) { match id.unwrap_edge() { 0 => (self.a, self.b, FeatureId::Vertex(0), FeatureId::Vertex(1)), 1 => (self.b, self.c, FeatureId::Vertex(1), FeatureId::Vertex(2)), @@ -772,13 +765,13 @@ impl ConvexPolyhedron for Triangle { fn support_face_toward( &self, - m: &Isometry, - dir: &Unit>, + m: &Pose, + dir: Vector, face: &mut ConvexPolygonalFeature, ) { let normal = self.scaled_normal(); - if normal.dot(&*dir) >= 0.0 { + if normal.dot(*dir) >= 0.0 { ConvexPolyhedron::face(self, FeatureId::Face(0), face); } else { ConvexPolyhedron::face(self, FeatureId::Face(1), face); @@ -788,8 +781,8 @@ impl ConvexPolyhedron for Triangle { fn support_feature_toward( &self, - transform: &Isometry, - dir: &Unit>, + transform: &Pose, + dir: Vector, eps: Real, out: &mut ConvexPolygonalFeature, ) { @@ -815,8 +808,8 @@ impl ConvexPolyhedron for Triangle { } } - fn support_feature_id_toward(&self, local_dir: &Unit>) -> FeatureId { - self.support_feature_id_toward(local_dir, na::convert::(f64::consts::PI / 180.0)) + fn support_feature_id_toward(&self, local_dir: Vector) -> FeatureId { + self.support_feature_id_toward(local_dir, (f64::consts::PI / 180.0) as Real) } } */ @@ -824,56 +817,51 @@ impl ConvexPolyhedron for Triangle { #[cfg(feature = "dim2")] #[cfg(test)] mod test { + use crate::math::Vector; use crate::shape::Triangle; - use na::Point2; #[test] fn test_triangle_area() { - let pa = Point2::new(5.0, 0.0); - let pb = Point2::origin(); - let pc = Point2::new(0.0, 4.0); + let pa = Vector::new(5.0, 0.0); + let pb = Vector::ZERO; + let pc = Vector::new(0.0, 4.0); assert!(relative_eq!(Triangle::new(pa, pb, pc).area(), 10.0)); } #[test] fn test_triangle_contains_point() { - let tri = Triangle::new( - Point2::new(5.0, 0.0), - Point2::origin(), - Point2::new(0.0, 4.0), - ); + let tri = Triangle::new(Vector::new(5.0, 0.0), Vector::ZERO, Vector::new(0.0, 4.0)); - assert!(tri.contains_point(&Point2::new(1.0, 1.0))); - assert!(!tri.contains_point(&Point2::new(-1.0, 1.0))); + assert!(tri.contains_point(Vector::new(1.0, 1.0))); + assert!(!tri.contains_point(Vector::new(-1.0, 1.0))); } #[test] fn test_obtuse_triangle_contains_point() { let tri = Triangle::new( - Point2::new(-10.0, 10.0), - Point2::origin(), - Point2::new(20.0, 0.0), + Vector::new(-10.0, 10.0), + Vector::ZERO, + Vector::new(20.0, 0.0), ); - assert!(tri.contains_point(&Point2::new(-3.0, 5.0))); - assert!(tri.contains_point(&Point2::new(5.0, 1.0))); - assert!(!tri.contains_point(&Point2::new(0.0, -1.0))); + assert!(tri.contains_point(Vector::new(-3.0, 5.0))); + assert!(tri.contains_point(Vector::new(5.0, 1.0))); + assert!(!tri.contains_point(Vector::new(0.0, -1.0))); } } #[cfg(feature = "dim3")] #[cfg(test)] mod test { - use crate::math::Real; + use crate::math::{Real, Vector}; use crate::shape::Triangle; - use na::Point3; #[test] fn test_triangle_area() { - let pa = Point3::new(0.0, 5.0, 0.0); - let pb = Point3::origin(); - let pc = Point3::new(0.0, 0.0, 4.0); + let pa = Vector::new(0.0, 5.0, 0.0); + let pb = Vector::ZERO; + let pc = Vector::new(0.0, 0.0, 4.0); assert!(relative_eq!(Triangle::new(pa, pb, pc).area(), 10.0)); } @@ -881,34 +869,34 @@ mod test { #[test] fn test_triangle_contains_point() { let tri = Triangle::new( - Point3::new(0.0, 5.0, 0.0), - Point3::origin(), - Point3::new(0.0, 0.0, 4.0), + Vector::new(0.0, 5.0, 0.0), + Vector::ZERO, + Vector::new(0.0, 0.0, 4.0), ); - assert!(tri.contains_point(&Point3::new(0.0, 1.0, 1.0))); - assert!(!tri.contains_point(&Point3::new(0.0, -1.0, 1.0))); + assert!(tri.contains_point(Vector::new(0.0, 1.0, 1.0))); + assert!(!tri.contains_point(Vector::new(0.0, -1.0, 1.0))); } #[test] fn test_obtuse_triangle_contains_point() { let tri = Triangle::new( - Point3::new(-10.0, 10.0, 0.0), - Point3::origin(), - Point3::new(20.0, 0.0, 0.0), + Vector::new(-10.0, 10.0, 0.0), + Vector::ZERO, + Vector::new(20.0, 0.0, 0.0), ); - assert!(tri.contains_point(&Point3::new(-3.0, 5.0, 0.0))); - assert!(tri.contains_point(&Point3::new(5.0, 1.0, 0.0))); - assert!(!tri.contains_point(&Point3::new(0.0, -1.0, 0.0))); + assert!(tri.contains_point(Vector::new(-3.0, 5.0, 0.0))); + assert!(tri.contains_point(Vector::new(5.0, 1.0, 0.0))); + assert!(!tri.contains_point(Vector::new(0.0, -1.0, 0.0))); } #[test] fn test_3dtriangle_contains_point() { - let o = Point3::origin(); - let pa = Point3::new(1.2, 1.4, 5.6); - let pb = Point3::new(1.5, 6.7, 1.9); - let pc = Point3::new(5.0, 2.1, 1.3); + let o = Vector::ZERO; + let pa = Vector::new(1.2, 1.4, 5.6); + let pb = Vector::new(1.5, 6.7, 1.9); + let pc = Vector::new(5.0, 2.1, 1.3); let tri = Triangle::new(pa, pb, pc); @@ -916,7 +904,7 @@ mod test { let vb = pb - o; let vc = pc - o; - let n = (pa - pb).cross(&(pb - pc)); + let n = (pa - pb).cross(pb - pc); // This is a simple algorithm for generating points that are inside the // triangle: o + (va * alpha + vb * beta + vc * gamma) is always inside the @@ -927,10 +915,10 @@ mod test { let not_contained_coplanar_p = o + (va * -0.5 + vb * 0.8 + vc * 0.7); let not_coplanar_p = o + (va * 0.2 + vb * 0.3 + vc * 0.5) + n * 0.1; let not_coplanar_p2 = o + (va * -0.5 + vb * 0.8 + vc * 0.7) + n * 0.1; - assert!(tri.contains_point(&contained_p)); - assert!(!tri.contains_point(¬_contained_coplanar_p)); - assert!(!tri.contains_point(¬_coplanar_p)); - assert!(!tri.contains_point(¬_coplanar_p2)); + assert!(tri.contains_point(contained_p)); + assert!(!tri.contains_point(not_contained_coplanar_p)); + assert!(!tri.contains_point(not_coplanar_p)); + assert!(!tri.contains_point(not_coplanar_p2)); // Test that points that are clearly within the triangle as seen as such, by testing // a number of points along a line intersecting the triangle. @@ -942,18 +930,18 @@ mod test { match i { ii if ii < 0 || ii > 85 => assert!( - !tri.contains_point(&p), + !tri.contains_point(p), "Should not contain: i = {}, b = {}", i, b ), ii if ii > 0 && ii < 85 => assert!( - tri.contains_point(&p), + tri.contains_point(p), "Should contain: i = {}, b = {}", i, b ), - _ => (), // Points at the edge may be seen as inside or outside + _ => (), // Vectors at the edge may be seen as inside or outside } } } diff --git a/src/shape/triangle_pseudo_normals.rs b/src/shape/triangle_pseudo_normals.rs index 4d94c107..7f90f298 100644 --- a/src/shape/triangle_pseudo_normals.rs +++ b/src/shape/triangle_pseudo_normals.rs @@ -1,11 +1,7 @@ -#[cfg(feature = "alloc")] use crate::math::Vector; -use crate::math::{Real, UnitVector}; -#[cfg(feature = "alloc")] -use na::Vector3; #[cfg(feature = "alloc")] -use crate::query::details::NormalConstraints; +use crate::{math::Vector3, query::details::NormalConstraints}; // NOTE: ideally, the normal cone should take into account the point where the normal cone is // considered. But as long as we assume that the triangles are one-way we can get away with @@ -22,46 +18,46 @@ use crate::query::details::NormalConstraints; #[derive(Clone, Debug)] pub struct TrianglePseudoNormals { /// The triangle’s face normal. - pub face: UnitVector, + pub face: Vector, // TODO: if we switch this to providing pseudo-normals in a specific order // (e.g. in the same order as the triangle’s edges), then we should // think of fixing that order in the heightfield // triangle_pseudo_normals code. /// The edges pseudo-normals, in no particular order. - pub edges: [UnitVector; 3], + pub edges: [Vector; 3], } #[cfg(feature = "alloc")] impl NormalConstraints for TrianglePseudoNormals { /// Projects the given direction to it is contained in the polygonal /// cone defined `self`. - fn project_local_normal_mut(&self, dir: &mut Vector) -> bool { - let dot_face = dir.dot(&self.face); + fn project_local_normal_mut(&self, dir: &mut Vector) -> bool { + let dot_face = dir.dot(self.face); // Find the closest pseudo-normal. let dots = Vector3::new( - dir.dot(&self.edges[0]), - dir.dot(&self.edges[1]), - dir.dot(&self.edges[2]), + dir.dot(self.edges[0]), + dir.dot(self.edges[1]), + dir.dot(self.edges[2]), ); - let closest_dot = dots.imax(); - let closest_edge = &self.edges[closest_dot]; + let closest_dot = dots.max_position(); + let closest_edge = self.edges[closest_dot]; // Apply the projection. Note that this isn’t 100% accurate since this approximates the // vertex normal cone using the closest edge’s normal cone instead of the // true polygonal cone on S² (but taking into account this polygonal cone exactly // would be quite computationally expensive). - if *closest_edge == self.face { + if closest_edge == self.face { // The normal cone is degenerate, there is only one possible direction. - *dir = *self.face; + *dir = self.face; return dot_face >= 0.0; } // TODO: take into account the two closest edges instead for better continuity // of vertex normals? let dot_edge_face = self.face.dot(closest_edge); - let dot_dir_face = self.face.dot(dir); + let dot_dir_face = self.face.dot(*dir); let dot_corrected_dir_face = 2.0 * dot_edge_face * dot_edge_face - 1.0; // cos(2 * angle(closest_edge, face)) if dot_dir_face >= dot_corrected_dir_face { @@ -70,27 +66,27 @@ impl NormalConstraints for TrianglePseudoNormals { } // We need to correct. - let edge_on_normal = *self.face * dot_edge_face; - let edge_orthogonal_to_normal = **closest_edge - edge_on_normal; + let edge_on_normal = self.face * dot_edge_face; + let edge_orthogonal_to_normal = closest_edge - edge_on_normal; - let dir_on_normal = *self.face * dot_dir_face; + let dir_on_normal = self.face * dot_dir_face; let dir_orthogonal_to_normal = *dir - dir_on_normal; - let Some(unit_dir_orthogonal_to_normal) = dir_orthogonal_to_normal.try_normalize(1.0e-6) - else { + let Some(unit_dir_orthogonal_to_normal) = dir_orthogonal_to_normal.try_normalize() else { return dot_face >= 0.0; }; // NOTE: the normalization might be redundant as the result vector is guaranteed to be // unit sized. Though some rounding errors could throw it off. - let Some(adjusted_pseudo_normal) = (edge_on_normal - + unit_dir_orthogonal_to_normal * edge_orthogonal_to_normal.norm()) - .try_normalize(1.0e-6) else { + let adjusted_pseudo_normal = + edge_on_normal + unit_dir_orthogonal_to_normal * edge_orthogonal_to_normal.length(); + let (adjusted_pseudo_normal, length) = adjusted_pseudo_normal.normalize_and_length(); + if length <= 1.0e-6 { return dot_face >= 0.0; }; // The reflection of the face normal wrt. the adjusted pseudo-normal gives us the // second end of the pseudo-normal cone the direction is projected on. - *dir = adjusted_pseudo_normal * (2.0 * self.face.dot(&adjusted_pseudo_normal)) - *self.face; + *dir = adjusted_pseudo_normal * (2.0 * self.face.dot(adjusted_pseudo_normal)) - self.face; dot_face >= 0.0 } } @@ -98,67 +94,64 @@ impl NormalConstraints for TrianglePseudoNormals { #[cfg(test)] #[cfg(all(feature = "dim3", feature = "alloc"))] mod test { + use super::NormalConstraints; use crate::math::{Real, Vector}; use crate::shape::TrianglePseudoNormals; - use na::Unit; - - use super::NormalConstraints; - fn bisector(v1: Vector, v2: Vector) -> Vector { + fn bisector(v1: Vector, v2: Vector) -> Vector { (v1 + v2).normalize() } - fn bisector_y(v: Vector) -> Vector { - bisector(v, Vector::y()) + fn bisector_y(v: Vector) -> Vector { + bisector(v, Vector::Y) } #[test] fn trivial_pseudo_normals_projection() { let pn = TrianglePseudoNormals { - face: Vector::y_axis(), - edges: [Vector::y_axis(); 3], + face: Vector::Y, + edges: [Vector::Y; 3], }; assert_eq!( pn.project_local_normal(Vector::new(1.0, 1.0, 1.0)), - Some(Vector::y()) + Some(Vector::Y) ); - assert!(pn.project_local_normal(-Vector::y()).is_none()); + assert!(pn.project_local_normal(-Vector::Y).is_none()); } #[test] fn edge_pseudo_normals_projection_strictly_positive() { - let bisector = |v1: Vector, v2: Vector| (v1 + v2).normalize(); - let bisector_y = |v: Vector| bisector(v, Vector::y()); + let bisector = |v1: Vector, v2: Vector| (v1 + v2).normalize(); + let bisector_y = |v: Vector| bisector(v, Vector::Y); // The normal cones for this test will be fully contained in the +Y half-space. let cones_ref_dir = [ - -Vector::z(), - -Vector::x(), + -Vector::Z, + -Vector::X, Vector::new(1.0, 0.0, 1.0).normalize(), ]; let cones_ends = cones_ref_dir.map(bisector_y); let cones_axes = cones_ends.map(bisector_y); let pn = TrianglePseudoNormals { - face: Vector::y_axis(), - edges: cones_axes.map(Unit::new_normalize), + face: Vector::Y, + edges: cones_axes.map(|v| v.normalize()), }; for i in 0..3 { - assert_relative_eq!( - pn.project_local_normal(cones_ends[i]).unwrap(), - cones_ends[i], - epsilon = 1.0e-5 - ); + assert!(pn + .project_local_normal(cones_ends[i]) + .unwrap() + .abs_diff_eq(cones_ends[i], 1.0e-5)); assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i])); // Guaranteed to be inside the normal cone of edge i. let subdivs = 100; for k in 1..100 { - let v = Vector::y() - .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + let v = Vector::Y + .lerp(cones_ends[i], k as Real / (subdivs as Real)) .normalize(); assert_eq!(pn.project_local_normal(v).unwrap(), v); } @@ -166,19 +159,18 @@ mod test { // Guaranteed to be outside the normal cone of edge i. for k in 1..subdivs { let v = cones_ref_dir[i] - .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + .lerp(cones_ends[i], k as Real / (subdivs as Real)) .normalize(); - assert_relative_eq!( - pn.project_local_normal(v).unwrap(), - cones_ends[i], - epsilon = 1.0e-5 - ); + assert!(pn + .project_local_normal(v) + .unwrap() + .abs_diff_eq(cones_ends[i], 1.0e-5)); } // Guaranteed to be outside the normal cone, and in the -Y half-space. for k in 1..subdivs { let v = cones_ref_dir[i] - .lerp(&(-Vector::y()), k as Real / (subdivs as Real)) + .lerp(-Vector::Y, k as Real / (subdivs as Real)) .normalize(); assert!(pn.project_local_normal(v).is_none(),); } @@ -189,11 +181,11 @@ mod test { fn edge_pseudo_normals_projection_negative() { // The normal cones for this test will be fully contained in the +Y half-space. let cones_ref_dir = [ - -Vector::z(), - -Vector::x(), + -Vector::Z, + -Vector::X, Vector::new(1.0, 0.0, 1.0).normalize(), ]; - let cones_ends = cones_ref_dir.map(|v| bisector(v, -Vector::y())); + let cones_ends = cones_ref_dir.map(|v| bisector(v, -Vector::Y)); let cones_axes = [ bisector(bisector_y(cones_ref_dir[0]), cones_ref_dir[0]), bisector(bisector_y(cones_ref_dir[1]), cones_ref_dir[1]), @@ -201,8 +193,8 @@ mod test { ]; let pn = TrianglePseudoNormals { - face: Vector::y_axis(), - edges: cones_axes.map(Unit::new_normalize), + face: Vector::Y, + edges: cones_axes.map(|v| v.normalize()), }; for i in 0..3 { @@ -212,8 +204,8 @@ mod test { let subdivs = 100; for k in 1..subdivs { - let v = Vector::y() - .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + let v = Vector::Y + .lerp(cones_ends[i], k as Real / (subdivs as Real)) .normalize(); assert_eq!(pn.project_local_normal(v).unwrap(), v); } @@ -221,8 +213,8 @@ mod test { // Guaranteed to be outside the normal cone of edge i. // Since it is additionally guaranteed to be in the -Y half-space, we should get None. for k in 1..subdivs { - let v = (-Vector::y()) - .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + let v = (-Vector::Y) + .lerp(cones_ends[i], k as Real / (subdivs as Real)) .normalize(); assert!(pn.project_local_normal(v).is_none()); } diff --git a/src/shape/trimesh.rs b/src/shape/trimesh.rs index aa9ef6b4..8c1a9e6d 100644 --- a/src/shape/trimesh.rs +++ b/src/shape/trimesh.rs @@ -1,12 +1,14 @@ use crate::bounding_volume::Aabb; -use crate::math::{Isometry, Point, Real, Vector}; +#[cfg(feature = "dim3")] +use crate::math::VectorExt; +use crate::math::{Pose, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy}; use crate::shape::{FeatureId, Shape, Triangle, TrianglePseudoNormals, TypedCompositeShape}; use crate::utils::HashablePartialEq; use alloc::{vec, vec::Vec}; use core::fmt; #[cfg(feature = "dim3")] -use {crate::shape::Cuboid, crate::utils::SortedPair, na::Unit}; +use {crate::shape::Cuboid, crate::utils::SortedPair}; use { crate::shape::composite_shape::CompositeShape, @@ -18,8 +20,6 @@ use { use crate::transformation::ear_clipping::triangulate_ear_clipping; use crate::query::details::NormalConstraints; -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; /// Errors that occur when computing or validating triangle mesh topology. /// @@ -52,11 +52,11 @@ pub enum TopologyError { // /// ``` // /// # #[cfg(all(feature = "dim3", feature = "f32"))] { // /// # use parry3d::shape::{TriMesh, TriMeshFlags}; - // /// # use nalgebra::Point3; + // /// # use parry3d::math::Vector; // /// let vertices = vec![ - // /// Point3::origin(), - // /// Point3::new(1.0, 0.0, 0.0), - // /// Point3::new(0.0, 1.0, 0.0), + // /// Vector::ZERO, + // /// Vector::new(1.0, 0.0, 0.0), + // /// Vector::new(0.0, 1.0, 0.0), // /// ]; // /// // /// // BAD: Triangle with duplicate vertices @@ -85,8 +85,8 @@ pub enum TopologyError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{TriMesh, TriMeshFlags}; - /// # use nalgebra::Point3; - /// # let vertices = vec![Point3::origin(), Point3::new(1.0, 0.0, 0.0), Point3::new(0.0, 1.0, 0.0)]; + /// # use parry3d::math::Vector; + /// # let vertices = vec![Vector::ZERO, Vector::new(1.0, 0.0, 0.0), Vector::new(0.0, 1.0, 0.0)]; /// # let indices = vec![[0, 0, 1], [0, 1, 2]]; /// let flags = TriMeshFlags::HALF_EDGE_TOPOLOGY /// | TriMeshFlags::DELETE_BAD_TOPOLOGY_TRIANGLES; @@ -128,12 +128,12 @@ pub enum TopologyError { // /// ``` // /// # #[cfg(all(feature = "dim3", feature = "f32"))] { // /// # use parry3d::shape::{TriMesh, TriMeshFlags}; - // /// # use nalgebra::Point3; + // /// # use parry3d::math::Vector; // /// let vertices = vec![ - // /// Point3::origin(), // vertex 0 - // /// Point3::new(1.0, 0.0, 0.0), // vertex 1 - // /// Point3::new(0.5, 1.0, 0.0), // vertex 2 - // /// Point3::new(0.5, -1.0, 0.0), // vertex 3 + // /// Vector::ZERO, // vertex 0 + // /// Vector::new(1.0, 0.0, 0.0), // vertex 1 + // /// Vector::new(0.5, 1.0, 0.0), // vertex 2 + // /// Vector::new(0.5, -1.0, 0.0), // vertex 3 // /// ]; // /// // /// // BAD: Both triangles traverse edge (0,1) in the same direction @@ -190,8 +190,8 @@ pub enum TopologyError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{TriMesh, TriMeshBuilderError, TopologyError}; -/// # use nalgebra::Point3; -/// let vertices = vec![Point3::origin()]; +/// # use parry3d::math::Vector; +/// let vertices = vec![Vector::ZERO]; /// let indices = vec![]; // Empty! /// /// match TriMesh::new(vertices, indices) { @@ -225,11 +225,11 @@ pub enum TriMeshBuilderError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::TriMesh; - /// # use nalgebra::Point3; + /// # use parry3d::math::Vector; /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// /// // BAD: No triangles @@ -258,11 +258,11 @@ pub enum TriMeshBuilderError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{TriMesh, TriMeshFlags, TriMeshBuilderError}; - /// # use nalgebra::Point3; + /// # use parry3d::math::Vector; /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// /// // Triangle with duplicate vertices @@ -293,16 +293,15 @@ pub enum TriMeshBuilderError { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] #[cfg(feature = "dim3")] pub struct TriMeshPseudoNormals { /// The pseudo-normals of the vertices. - pub vertices_pseudo_normal: Vec>, + pub vertices_pseudo_normal: Vec, /// The pseudo-normals of the edges. - pub edges_pseudo_normal: Vec<[Vector; 3]>, + pub edges_pseudo_normal: Vec<[Vector; 3]>, } /// The connected-components of a triangle mesh. @@ -310,8 +309,7 @@ pub struct TriMeshPseudoNormals { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct TriMeshConnectedComponents { @@ -336,7 +334,7 @@ impl TriMeshConnectedComponents { /// /// The `mesh` must be the one used to generate `self`, otherwise it might panic or produce an /// unexpected result. - pub fn to_mesh_buffers(&self, mesh: &TriMesh) -> Vec<(Vec>, Vec<[u32; 3]>)> { + pub fn to_mesh_buffers(&self, mesh: &TriMesh) -> Vec<(Vec, Vec<[u32; 3]>)> { let mut result = vec![]; let mut new_vtx_index: Vec<_> = vec![u32::MAX; mesh.vertices.len()]; @@ -392,8 +390,7 @@ impl TriMeshConnectedComponents { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct TopoVertex { @@ -406,8 +403,7 @@ pub struct TopoVertex { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct TopoFace { @@ -421,8 +417,7 @@ pub struct TopoFace { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct TopoHalfEdge { @@ -443,8 +438,7 @@ pub struct TopoHalfEdge { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] pub struct TriMeshTopology { @@ -459,8 +453,7 @@ pub struct TriMeshTopology { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(as = "Self") + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] #[derive(Clone, Copy, Debug, Default, Eq, Hash, Ord, PartialEq, PartialOrd)] @@ -510,15 +503,14 @@ bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] #[repr(C)] #[derive(Clone)] /// A triangle mesh. pub struct TriMesh { bvh: Bvh, - vertices: Vec>, + vertices: Vec, indices: Vec<[u32; 3]>, #[cfg(feature = "dim3")] pub(crate) pseudo_normals: Option, @@ -567,13 +559,13 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create a simple triangle mesh (a single triangle) /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// @@ -585,14 +577,14 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::TriMesh; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a quad (two triangles) in 2D /// let vertices = vec![ - /// Point2::origin(), // bottom-left - /// Point2::new(1.0, 0.0), // bottom-right - /// Point2::new(1.0, 1.0), // top-right - /// Point2::new(0.0, 1.0), // top-left + /// Vector::ZERO, // bottom-left + /// Vector::new(1.0, 0.0), // bottom-right + /// Vector::new(1.0, 1.0), // top-right + /// Vector::new(0.0, 1.0), // top-left /// ]; /// let indices = vec![ /// [0, 1, 2], // first triangle @@ -604,10 +596,7 @@ impl TriMesh { /// assert_eq!(quad.vertices().len(), 4); /// # } /// ``` - pub fn new( - vertices: Vec>, - indices: Vec<[u32; 3]>, - ) -> Result { + pub fn new(vertices: Vec, indices: Vec<[u32; 3]>) -> Result { Self::with_flags(vertices, indices, TriMeshFlags::empty()) } @@ -653,14 +642,14 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{TriMesh, TriMeshFlags}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create vertices for a simple mesh /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2], [1, 3, 2]]; /// @@ -677,12 +666,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{TriMesh, TriMeshFlags}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// # let vertices = vec![ - /// # Point3::origin(), - /// # Point3::new(1.0, 0.0, 0.0), - /// # Point3::new(0.0, 1.0, 0.0), + /// # Vector::ZERO, + /// # Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.0, 1.0, 0.0), /// # ]; /// # let indices = vec![[0, 1, 2]]; /// // Combine multiple flags for advanced features @@ -698,12 +687,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{TriMesh, TriMeshFlags}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// # let vertices = vec![ - /// # Point3::origin(), - /// # Point3::new(1.0, 0.0, 0.0), - /// # Point3::new(0.0, 1.0, 0.0), + /// # Vector::ZERO, + /// # Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.0, 1.0, 0.0), /// # ]; /// # let indices = vec![[0, 1, 2]]; /// // For robust point containment tests in 3D @@ -716,7 +705,7 @@ impl TriMesh { /// # } /// ``` pub fn with_flags( - vertices: Vec>, + vertices: Vec, indices: Vec<[u32; 3]>, flags: TriMeshFlags, ) -> Result { @@ -822,14 +811,14 @@ impl TriMesh { pseudo_normals, } = self; let sz_bvh = bvh.heap_memory_size(); - let sz_vertices = vertices.capacity() * size_of::>(); + let sz_vertices = vertices.capacity() * size_of::(); let sz_indices = indices.capacity() * size_of::<[u32; 3]>(); #[cfg(feature = "dim3")] let sz_pseudo_normals = pseudo_normals .as_ref() .map(|pn| { - pn.vertices_pseudo_normal.capacity() * size_of::>() - + pn.edges_pseudo_normal.capacity() * size_of::<[Vector; 3]>() + pn.vertices_pseudo_normal.capacity() * size_of::() + + pn.edges_pseudo_normal.capacity() * size_of::<[Vector; 3]>() }) .unwrap_or(0); #[cfg(feature = "dim2")] @@ -879,27 +868,27 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::{Point3, Vector3, Isometry3}; + /// use parry3d::math::{Vector, Pose}; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mut mesh = TriMesh::new(vertices, indices).unwrap(); /// /// // Translate the mesh by (10, 0, 0) - /// let transform = Isometry3::translation(10.0, 0.0, 0.0); + /// let transform = Pose::translation(10.0, 0.0, 0.0); /// mesh.transform_vertices(&transform); /// /// // All vertices are now shifted - /// assert_eq!(mesh.vertices()[0], Point3::new(10.0, 0.0, 0.0)); - /// assert_eq!(mesh.vertices()[1], Point3::new(11.0, 0.0, 0.0)); - /// assert_eq!(mesh.vertices()[2], Point3::new(10.0, 1.0, 0.0)); + /// assert_eq!(mesh.vertices()[0], Vector::new(10.0, 0.0, 0.0)); + /// assert_eq!(mesh.vertices()[1], Vector::new(11.0, 0.0, 0.0)); + /// assert_eq!(mesh.vertices()[2], Vector::new(10.0, 1.0, 0.0)); /// # } /// ``` - pub fn transform_vertices(&mut self, transform: &Isometry) { + pub fn transform_vertices(&mut self, transform: &Pose) { self.vertices .iter_mut() .for_each(|pt| *pt = transform * *pt); @@ -911,11 +900,11 @@ impl TriMesh { pseudo_normals .vertices_pseudo_normal .iter_mut() - .for_each(|n| *n = transform * *n); + .for_each(|n| *n = transform.rotation * *n); pseudo_normals.edges_pseudo_normal.iter_mut().for_each(|n| { - n[0] = transform * n[0]; - n[1] = transform * n[1]; - n[2] = transform * n[2]; + n[0] = transform.rotation * n[0]; + n[1] = transform.rotation * n[1]; + n[2] = transform.rotation * n[2]; }); } } @@ -942,45 +931,43 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); /// /// // Uniform scaling: double all dimensions - /// let scaled = mesh.clone().scaled(&Vector3::new(2.0, 2.0, 2.0)); - /// assert_eq!(scaled.vertices()[1], Point3::new(2.0, 0.0, 0.0)); + /// let scaled = mesh.clone().scaled(Vector::new(2.0, 2.0, 2.0)); + /// assert_eq!(scaled.vertices()[1], Vector::new(2.0, 0.0, 0.0)); /// /// // Non-uniform scaling: stretch along X axis - /// let stretched = mesh.scaled(&Vector3::new(3.0, 1.0, 1.0)); - /// assert_eq!(stretched.vertices()[1], Point3::new(3.0, 0.0, 0.0)); - /// assert_eq!(stretched.vertices()[2], Point3::new(0.0, 1.0, 0.0)); + /// let stretched = mesh.scaled(Vector::new(3.0, 1.0, 1.0)); + /// assert_eq!(stretched.vertices()[1], Vector::new(3.0, 0.0, 0.0)); + /// assert_eq!(stretched.vertices()[2], Vector::new(0.0, 1.0, 0.0)); /// # } /// ``` - pub fn scaled(mut self, scale: &Vector) -> Self { - self.vertices - .iter_mut() - .for_each(|pt| pt.coords.component_mul_assign(scale)); + pub fn scaled(mut self, scale: Vector) -> Self { + self.vertices.iter_mut().for_each(|pt| *pt *= scale); #[cfg(feature = "dim3")] if let Some(pn) = &mut self.pseudo_normals { pn.vertices_pseudo_normal.iter_mut().for_each(|n| { - n.component_mul_assign(scale); - let _ = n.try_normalize_mut(0.0); + *n *= scale; + *n = n.normalize_or(*n); }); pn.edges_pseudo_normal.iter_mut().for_each(|n| { - n[0].component_mul_assign(scale); - n[1].component_mul_assign(scale); - n[2].component_mul_assign(scale); + n[0] *= scale; + n[1] *= scale; + n[2] *= scale; - let _ = n[0].try_normalize_mut(0.0); - let _ = n[1].try_normalize_mut(0.0); - let _ = n[2].try_normalize_mut(0.0); + n[0] = n[0].normalize_or(n[0]); + n[1] = n[1].normalize_or(n[1]); + n[2] = n[2].normalize_or(n[2]); }); } @@ -1024,22 +1011,22 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create first mesh (a triangle) /// let vertices1 = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices1 = vec![[0, 1, 2]]; /// let mut mesh1 = TriMesh::new(vertices1, indices1).unwrap(); /// /// // Create second mesh (another triangle, offset in space) /// let vertices2 = vec![ - /// Point3::new(2.0, 0.0, 0.0), - /// Point3::new(3.0, 0.0, 0.0), - /// Point3::new(2.0, 1.0, 0.0), + /// Vector::new(2.0, 0.0, 0.0), + /// Vector::new(3.0, 0.0, 0.0), + /// Vector::new(2.0, 1.0, 0.0), /// ]; /// let indices2 = vec![[0, 1, 2]]; /// let mesh2 = TriMesh::new(vertices2, indices2).unwrap(); @@ -1097,16 +1084,16 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::shape::TriMesh; - /// use nalgebra::Point2; + /// use parry2d::math::Vector; /// /// // Create a simple concave polygon (L-shape) /// let vertices = vec![ - /// Point2::origin(), - /// Point2::new(2.0, 0.0), - /// Point2::new(2.0, 1.0), - /// Point2::new(1.0, 1.0), - /// Point2::new(1.0, 2.0), - /// Point2::new(0.0, 2.0), + /// Vector::ZERO, + /// Vector::new(2.0, 0.0), + /// Vector::new(2.0, 1.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(1.0, 2.0), + /// Vector::new(0.0, 2.0), /// ]; /// /// let mesh = TriMesh::from_polygon(vertices) @@ -1117,7 +1104,7 @@ impl TriMesh { /// # } /// ``` #[cfg(feature = "dim2")] - pub fn from_polygon(vertices: Vec>) -> Option { + pub fn from_polygon(vertices: Vec) -> Option { triangulate_ear_clipping(&vertices).map(|indices| Self::new(vertices, indices).unwrap()) } @@ -1132,13 +1119,13 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2], [1, 3, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -1191,12 +1178,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// @@ -1259,18 +1246,18 @@ impl TriMesh { let mut triangle_set = HashSet::default(); fn resolve_coord_id( - coord: &Point, - vtx_to_id: &mut HashMap>, u32>, - new_vertices: &mut Vec>, + coord: Vector, + vtx_to_id: &mut HashMap, u32>, + new_vertices: &mut Vec, ) -> u32 { - let key = HashablePartialEq::new(*coord); + let key = HashablePartialEq::new(coord); let id = match vtx_to_id.entry(key) { Entry::Occupied(entry) => entry.into_mut(), Entry::Vacant(entry) => entry.insert(new_vertices.len() as u32), }; if *id == new_vertices.len() as u32 { - new_vertices.push(*coord); + new_vertices.push(coord); } *id @@ -1278,19 +1265,19 @@ impl TriMesh { for t in self.indices.iter() { let va = resolve_coord_id( - &self.vertices[t[0] as usize], + self.vertices[t[0] as usize], &mut vtx_to_id, &mut new_vertices, ); let vb = resolve_coord_id( - &self.vertices[t[1] as usize], + self.vertices[t[1] as usize], &mut vtx_to_id, &mut new_vertices, ); let vc = resolve_coord_id( - &self.vertices[t[2] as usize], + self.vertices[t[2] as usize], &mut vtx_to_id, &mut new_vertices, ); @@ -1346,7 +1333,7 @@ impl TriMesh { /// It may be useful to call `self.remove_duplicate_vertices()` before this method, in order to fix the /// index buffer if some of the vertices of this trimesh are duplicated. fn compute_pseudo_normals(&mut self) { - let mut vertices_pseudo_normal = vec![Vector::zeros(); self.vertices().len()]; + let mut vertices_pseudo_normal = vec![Vector::ZERO; self.vertices().len()]; let mut edges_pseudo_normal = HashMap::default(); let mut edges_multiplicity = HashMap::default(); @@ -1359,13 +1346,13 @@ impl TriMesh { ); if let Some(n) = tri.normal() { - let ang1 = (tri.b - tri.a).angle(&(tri.c - tri.a)); - let ang2 = (tri.a - tri.b).angle(&(tri.c - tri.b)); - let ang3 = (tri.b - tri.c).angle(&(tri.a - tri.c)); + let ang1 = (tri.b - tri.a).angle(tri.c - tri.a); + let ang2 = (tri.a - tri.b).angle(tri.c - tri.b); + let ang3 = (tri.b - tri.c).angle(tri.a - tri.c); - vertices_pseudo_normal[idx[0] as usize] += *n * ang1; - vertices_pseudo_normal[idx[1] as usize] += *n * ang2; - vertices_pseudo_normal[idx[2] as usize] += *n * ang3; + vertices_pseudo_normal[idx[0] as usize] += n * ang1; + vertices_pseudo_normal[idx[1] as usize] += n * ang2; + vertices_pseudo_normal[idx[2] as usize] += n * ang3; let edges = [ SortedPair::new(idx[0], idx[1]), @@ -1374,10 +1361,8 @@ impl TriMesh { ]; for edge in &edges { - let edge_n = edges_pseudo_normal - .entry(*edge) - .or_insert_with(Vector::zeros); - *edge_n += *n; // NOTE: there is no need to multiply by the incident angle since it is always equal to PI for all the edges. + let edge_n = edges_pseudo_normal.entry(*edge).or_insert(Vector::ZERO); + *edge_n += n; // NOTE: there is no need to multiply by the incident angle since it is always equal to PI for all the edges. let edge_mult = edges_multiplicity.entry(*edge).or_insert(0); *edge_mult += 1; } @@ -1391,7 +1376,7 @@ impl TriMesh { let e0 = SortedPair::new(idx[0], idx[1]); let e1 = SortedPair::new(idx[1], idx[2]); let e2 = SortedPair::new(idx[2], idx[0]); - let default = Vector::zeros(); + let default = Vector::ZERO; [ edges_pseudo_normal.get(&e0).copied().unwrap_or(default), edges_pseudo_normal.get(&e1).copied().unwrap_or(default), @@ -1655,13 +1640,13 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2], [1, 3, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -1689,7 +1674,7 @@ impl TriMesh { #[cfg(feature = "dim3")] /// Gets the normal of the triangle represented by `feature`. - pub fn feature_normal(&self, feature: FeatureId) -> Option>> { + pub fn feature_normal(&self, feature: FeatureId) -> Option { match feature { FeatureId::Face(i) => self .triangle(i % self.num_triangles() as u32) @@ -1710,12 +1695,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{TriMesh, TriMeshFlags}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// @@ -1743,24 +1728,24 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::{Point3, Isometry3}; + /// use parry3d::math::{Vector, Pose}; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); /// - /// let identity = Isometry3::identity(); + /// let identity = Pose::identity(); /// let aabb = mesh.aabb(&identity); /// /// // The AABB contains all vertices - /// assert!(aabb.contains_local_point(&Point3::new(0.5, 0.5, 0.0))); + /// assert!(aabb.contains_local_point(Vector::new(0.5, 0.5, 0.0))); /// # } /// ``` - pub fn aabb(&self, pos: &Isometry) -> Aabb { + pub fn aabb(&self, pos: &Pose) -> Aabb { self.bvh.root_aabb().transform_by(pos) } @@ -1775,12 +1760,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::new(-1.0, -1.0, 0.0), - /// Point3::new(1.0, -1.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::new(-1.0, -1.0, 0.0), + /// Vector::new(1.0, -1.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -1805,12 +1790,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -1831,13 +1816,13 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2], [1, 3, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -1877,20 +1862,20 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); /// /// let triangle = mesh.triangle(0); - /// assert_eq!(triangle.a, Point3::origin()); - /// assert_eq!(triangle.b, Point3::new(1.0, 0.0, 0.0)); - /// assert_eq!(triangle.c, Point3::new(0.0, 1.0, 0.0)); + /// assert_eq!(triangle.a, Vector::ZERO); + /// assert_eq!(triangle.b, Vector::new(1.0, 0.0, 0.0)); + /// assert_eq!(triangle.c, Vector::new(0.0, 1.0, 0.0)); /// # } /// ``` pub fn triangle(&self, i: u32) -> Triangle { @@ -1919,9 +1904,9 @@ impl TriMesh { Some(TrianglePseudoNormals { face: triangle.normal()?, edges: [ - Unit::try_new(edges_pseudo_normals[0], 1.0e-6)?, - Unit::try_new(edges_pseudo_normals[1], 1.0e-6)?, - Unit::try_new(edges_pseudo_normals[2], 1.0e-6)?, + (edges_pseudo_normals[0]).try_normalize()?, + (edges_pseudo_normals[1]).try_normalize()?, + (edges_pseudo_normals[2]).try_normalize()?, ], }) } else { @@ -1945,21 +1930,21 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); /// /// assert_eq!(mesh.vertices().len(), 3); - /// assert_eq!(mesh.vertices()[0], Point3::origin()); + /// assert_eq!(mesh.vertices()[0], Vector::ZERO); /// # } /// ``` - pub fn vertices(&self) -> &[Point] { + pub fn vertices(&self) -> &[Vector] { &self.vertices } @@ -1974,13 +1959,13 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::TriMesh; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(1.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(1.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2], [1, 3, 2]]; /// let mesh = TriMesh::new(vertices, indices).unwrap(); @@ -2005,12 +1990,12 @@ impl TriMesh { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{TriMesh, TriMeshFlags}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// let vertices = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2]]; /// @@ -2043,18 +2028,18 @@ impl TriMesh { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # #[cfg(feature = "std")] { /// use parry3d::shape::{TriMesh, TriMeshFlags}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Create two separate triangles (not connected) /// let vertices = vec![ /// // First triangle - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), /// // Second triangle (separate) - /// Point3::new(5.0, 0.0, 0.0), - /// Point3::new(6.0, 0.0, 0.0), - /// Point3::new(5.0, 1.0, 0.0), + /// Vector::new(5.0, 0.0, 0.0), + /// Vector::new(6.0, 0.0, 0.0), + /// Vector::new(5.0, 1.0, 0.0), /// ]; /// let indices = vec![[0, 1, 2], [3, 4, 5]]; /// @@ -2123,7 +2108,7 @@ impl CompositeShape for TriMesh { fn map_part_at( &self, i: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { let tri = self.triangle(i); let normals = self.triangle_normal_constraints(i); @@ -2147,11 +2132,7 @@ impl TypedCompositeShape for TriMesh { fn map_typed_part_at( &self, i: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { let tri = self.triangle(i); let pseudo_normals = self.triangle_normal_constraints(i); @@ -2162,7 +2143,7 @@ impl TypedCompositeShape for TriMesh { fn map_untyped_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let tri = self.triangle(i); let pseudo_normals = self.triangle_normal_constraints(i); @@ -2189,7 +2170,7 @@ mod test { #[test] fn connected_components() { - let (vtx, idx) = Cuboid::new(Vector::repeat(0.5)).to_trimesh(); + let (vtx, idx) = Cuboid::new(Vector::splat(0.5)).to_trimesh(); // Push 10 copy of the mesh, each time pushed with an offset. let mut mesh = TriMesh::new(vtx.clone(), idx.clone()).unwrap(); @@ -2197,7 +2178,7 @@ mod test { for i in 1..10 { let cc_vtx = vtx .iter() - .map(|pt| pt + Vector::repeat(2.0 * i as Real)) + .map(|pt| *pt + Vector::splat(2.0 * i as Real)) .collect(); let to_append = TriMesh::new(cc_vtx, idx.clone()).unwrap(); diff --git a/src/shape/voxels/voxels.rs b/src/shape/voxels/voxels.rs index 46fef04b..8a0a4329 100644 --- a/src/shape/voxels/voxels.rs +++ b/src/shape/voxels/voxels.rs @@ -3,14 +3,14 @@ use super::{ FACES_TO_VOXEL_TYPES, INTERIOR_FACE_MASK, }; use crate::bounding_volume::{Aabb, BoundingVolume}; -use crate::math::{Point, Real, Vector}; +#[cfg(not(feature = "std"))] +use crate::math::ComplexField; +use crate::math::{ivect_to_vect, vect_to_ivect, IVector, Int, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy, BvhNode}; use crate::shape::voxels::voxels_chunk::{VoxelsChunk, VoxelsChunkHeader}; use crate::shape::VoxelsChunkRef; use crate::utils::hashmap::HashMap; use alloc::{vec, vec::Vec}; -#[cfg(not(feature = "std"))] -use na::ComplexField; /// Categorization of a voxel based on its neighbors. /// @@ -32,25 +32,25 @@ use na::ComplexField; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Voxels, VoxelType}; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// // Create a small 2x2x2 cube of voxels /// let voxels = Voxels::new( -/// Vector3::new(1.0, 1.0, 1.0), +/// Vector::new(1.0, 1.0, 1.0), /// &[ -/// Point3::new(0, 0, 0), -/// Point3::new(1, 0, 0), -/// Point3::new(0, 1, 0), -/// Point3::new(1, 1, 0), -/// Point3::new(0, 0, 1), -/// Point3::new(1, 0, 1), -/// Point3::new(0, 1, 1), -/// Point3::new(1, 1, 1), +/// IVector::new(0, 0, 0), +/// IVector::new(1, 0, 0), +/// IVector::new(0, 1, 0), +/// IVector::new(1, 1, 0), +/// IVector::new(0, 0, 1), +/// IVector::new(1, 0, 1), +/// IVector::new(0, 1, 1), +/// IVector::new(1, 1, 1), /// ], /// ); /// /// // Check voxel type - interior voxels are fully surrounded -/// let state = voxels.voxel_state(Point3::new(0, 0, 0)).unwrap(); +/// let state = voxels.voxel_state(IVector::new(0, 0, 0)).unwrap(); /// let voxel_type = state.voxel_type(); /// println!("Voxel type: {:?}", voxel_type); /// # } @@ -190,16 +190,16 @@ impl OctantPattern { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::{Voxels, VoxelState, AxisMask}; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// // Create a simple voxel shape /// let voxels = Voxels::new( -/// Vector3::new(1.0, 1.0, 1.0), -/// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0)], +/// Vector::new(1.0, 1.0, 1.0), +/// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0)], /// ); /// /// // Query the state of a voxel -/// let state = voxels.voxel_state(Point3::new(0, 0, 0)).unwrap(); +/// let state = voxels.voxel_state(IVector::new(0, 0, 0)).unwrap(); /// /// // Check if empty /// assert!(!state.is_empty()); @@ -274,11 +274,11 @@ impl VoxelState { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( -/// Vector3::new(0.5, 0.5, 0.5), -/// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0)], +/// Vector::new(0.5, 0.5, 0.5), +/// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0)], /// ); /// /// // Iterate through all voxels @@ -299,9 +299,9 @@ pub struct VoxelData { /// [`Voxels::crop`]. pub linear_id: VoxelIndex, /// The voxel's integer grid coordinates. - pub grid_coords: Point, + pub grid_coords: IVector, /// The voxel's center position in the local-space of the [`Voxels`] shape it is part of. - pub center: Point, + pub center: Vector, /// The voxel's state, indicating if it's empty or full. pub state: VoxelState, } @@ -350,15 +350,15 @@ pub struct VoxelData { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// // Create a simple 3×3×3 cube of voxels -/// let voxel_size = Vector3::new(1.0, 1.0, 1.0); +/// let voxel_size = Vector::new(1.0, 1.0, 1.0); /// let mut coords = Vec::new(); /// for x in 0..3 { /// for y in 0..3 { /// for z in 0..3 { -/// coords.push(Point3::new(x, y, z)); +/// coords.push(IVector::new(x, y, z)); /// } /// } /// } @@ -368,22 +368,22 @@ pub struct VoxelData { /// # } /// ``` /// -/// ## Creating Voxels from World-Space Points +/// ## Creating Voxels from World-Space Vectors /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::Vector; /// /// // Sample points in world space (e.g., from a point cloud) /// let points = vec![ -/// Point3::new(0.1, 0.2, 0.3), -/// Point3::new(1.5, 2.1, 3.7), -/// Point3::new(0.8, 0.9, 1.2), +/// Vector::new(0.1, 0.2, 0.3), +/// Vector::new(1.5, 2.1, 3.7), +/// Vector::new(0.8, 0.9, 1.2), /// ]; /// /// // Create voxels with 0.5 unit size - nearby points merge into same voxel -/// let voxels = Voxels::from_points(Vector3::new(0.5, 0.5, 0.5), &points); +/// let voxels = Voxels::from_points(Vector::new(0.5, 0.5, 0.5), &points); /// println!("Created voxel shape from {} points", points.len()); /// # } /// ``` @@ -393,15 +393,15 @@ pub struct VoxelData { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( -/// Vector3::new(1.0, 1.0, 1.0), -/// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0)], +/// Vector::new(1.0, 1.0, 1.0), +/// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0)], /// ); /// /// // Check if a specific grid position is filled -/// if let Some(state) = voxels.voxel_state(Point3::new(0, 0, 0)) { +/// if let Some(state) = voxels.voxel_state(IVector::new(0, 0, 0)) { /// println!("Voxel is filled!"); /// println!("Type: {:?}", state.voxel_type()); /// println!("Free faces: {:?}", state.free_faces()); @@ -410,9 +410,9 @@ pub struct VoxelData { /// } /// /// // Convert world-space point to grid coordinates -/// let world_point = Point3::new(1.3, 0.7, 0.2); +/// let world_point = Vector::new(1.3, 0.7, 0.2); /// let grid_coord = voxels.voxel_at_point(world_point); -/// println!("Point at {:?} is in voxel {:?}", world_point, grid_coord); +/// println!("Vector at {:?} is in voxel {:?}", world_point, grid_coord); /// # } /// ``` /// @@ -421,11 +421,11 @@ pub struct VoxelData { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( -/// Vector3::new(0.5, 0.5, 0.5), -/// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0), Point3::new(0, 1, 0)], +/// Vector::new(0.5, 0.5, 0.5), +/// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0), IVector::new(0, 1, 0)], /// ); /// /// // Iterate through all non-empty voxels @@ -443,22 +443,22 @@ pub struct VoxelData { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// let mut voxels = Voxels::new( -/// Vector3::new(1.0, 1.0, 1.0), -/// &[Point3::new(0, 0, 0)], +/// Vector::new(1.0, 1.0, 1.0), +/// &[IVector::new(0, 0, 0)], /// ); /// /// // Add a new voxel -/// voxels.set_voxel(Point3::new(1, 0, 0), true); +/// voxels.set_voxel(IVector::new(1, 0, 0), true); /// /// // Remove a voxel -/// voxels.set_voxel(Point3::new(0, 0, 0), false); +/// voxels.set_voxel(IVector::new(0, 0, 0), false); /// /// // Check the result -/// assert!(voxels.voxel_state(Point3::new(0, 0, 0)).unwrap().is_empty()); -/// assert!(!voxels.voxel_state(Point3::new(1, 0, 0)).unwrap().is_empty()); +/// assert!(voxels.voxel_state(IVector::new(0, 0, 0)).unwrap().is_empty()); +/// assert!(!voxels.voxel_state(IVector::new(1, 0, 0)).unwrap().is_empty()); /// # } /// ``` /// @@ -468,15 +468,15 @@ pub struct VoxelData { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; /// use parry3d::bounding_volume::Aabb; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( -/// Vector3::new(1.0, 1.0, 1.0), -/// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0), Point3::new(2, 0, 0)], +/// Vector::new(1.0, 1.0, 1.0), +/// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0), IVector::new(2, 0, 0)], /// ); /// /// // Find voxels intersecting an AABB -/// let query_aabb = Aabb::new(Point3::new(-0.5, -0.5, -0.5), Point3::new(1.5, 1.5, 1.5)); +/// let query_aabb = Aabb::new(Vector::new(-0.5, -0.5, -0.5), Vector::new(1.5, 1.5, 1.5)); /// let count = voxels.voxels_intersecting_local_aabb(&query_aabb) /// .filter(|v| !v.state.is_empty()) /// .count(); @@ -502,11 +502,11 @@ pub struct Voxels { /// The bounding boxes are the ones of the chunk’s voxels **keys**. This is equivalent to a bvh /// of the chunks with a uniform voxel size of 1. pub(super) chunk_bvh: Bvh, - pub(super) chunk_headers: HashMap, VoxelsChunkHeader>, - pub(super) chunk_keys: Vec>, + pub(super) chunk_headers: HashMap, + pub(super) chunk_keys: Vec, pub(super) chunks: Vec, pub(super) free_chunks: Vec, - pub(super) voxel_size: Vector, + pub(super) voxel_size: Vector, } impl Voxels { @@ -533,45 +533,45 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // Create a 2×2×2 cube of voxels with 1.0 unit size /// let voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), /// &[ - /// Point3::new(0, 0, 0), Point3::new(1, 0, 0), - /// Point3::new(0, 1, 0), Point3::new(1, 1, 0), - /// Point3::new(0, 0, 1), Point3::new(1, 0, 1), - /// Point3::new(0, 1, 1), Point3::new(1, 1, 1), + /// IVector::new(0, 0, 0), IVector::new(1, 0, 0), + /// IVector::new(0, 1, 0), IVector::new(1, 1, 0), + /// IVector::new(0, 0, 1), IVector::new(1, 0, 1), + /// IVector::new(0, 1, 1), IVector::new(1, 1, 1), /// ], /// ); /// /// // Verify the first voxel's center position - /// let center = voxels.voxel_center(Point3::new(0, 0, 0)); - /// assert_eq!(center, Point3::new(0.5, 0.5, 0.5)); + /// let center = voxels.voxel_center(IVector::new(0, 0, 0)); + /// assert_eq!(center, Vector::new(0.5, 0.5, 0.5)); /// # } /// ``` /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // Create a line of voxels along the X axis /// let voxels = Voxels::new( - /// Vector3::new(0.5, 0.5, 0.5), - /// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0), Point3::new(2, 0, 0)], + /// Vector::new(0.5, 0.5, 0.5), + /// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0), IVector::new(2, 0, 0)], /// ); /// /// // Query the domain (bounding grid coordinates) /// // Note: domain is aligned to internal chunk boundaries for efficiency /// let [mins, maxs] = voxels.domain(); - /// assert_eq!(mins, Point3::new(0, 0, 0)); + /// assert_eq!(mins, IVector::new(0, 0, 0)); /// // maxs will be chunk-aligned (chunks are 8x8x8), so it includes more space /// assert!(maxs.x >= 3 && maxs.y >= 1 && maxs.z >= 1); /// # } /// ``` - pub fn new(voxel_size: Vector, grid_coordinates: &[Point]) -> Self { + pub fn new(voxel_size: Vector, grid_coordinates: &[IVector]) -> Self { let mut result = Self { chunk_bvh: Bvh::new(), chunk_headers: HashMap::default(), @@ -596,10 +596,7 @@ impl Voxels { result.chunk_bvh = Bvh::from_iter( BvhBuildStrategy::Ploc, result.chunk_headers.iter().map(|(chunk_key, chunk_id)| { - ( - chunk_id.id, - VoxelsChunk::aabb(chunk_key, &result.voxel_size), - ) + (chunk_id.id, VoxelsChunk::aabb(chunk_key, result.voxel_size)) }), ); @@ -620,9 +617,9 @@ impl Voxels { /// 3. Removing duplicates (multiple points in the same voxel become one voxel) /// /// For example, with `voxel_size = 1.0`: - /// - Point `(0.3, 0.7, 0.9)` → Grid `(0, 0, 0)` - /// - Point `(1.1, 0.2, 0.5)` → Grid `(1, 0, 0)` - /// - Point `(0.9, 0.1, 0.8)` → Grid `(0, 0, 0)` (merges with first) + /// - Vector `(0.3, 0.7, 0.9)` → Grid `(0, 0, 0)` + /// - Vector `(1.1, 0.2, 0.5)` → Grid `(1, 0, 0)` + /// - Vector `(0.9, 0.1, 0.8)` → Grid `(0, 0, 0)` (merges with first) /// /// # Use Cases /// @@ -635,18 +632,18 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::Vector; /// /// // Sample points in world space /// let points = vec![ - /// Point3::new(0.1, 0.2, 0.3), // → Grid (0, 0, 0) - /// Point3::new(0.7, 0.8, 0.9), // → Grid (0, 0, 0) - same voxel! - /// Point3::new(1.2, 0.3, 0.1), // → Grid (1, 0, 0) - /// Point3::new(0.5, 1.5, 0.2), // → Grid (0, 1, 0) + /// Vector::new(0.1, 0.2, 0.3), // → Grid (0, 0, 0) + /// Vector::new(0.7, 0.8, 0.9), // → Grid (0, 0, 0) - same voxel! + /// Vector::new(1.2, 0.3, 0.1), // → Grid (1, 0, 0) + /// Vector::new(0.5, 1.5, 0.2), // → Grid (0, 1, 0) /// ]; /// /// // Create voxels with 1.0 unit size - /// let voxels = Voxels::from_points(Vector3::new(1.0, 1.0, 1.0), &points); + /// let voxels = Voxels::from_points(Vector::new(1.0, 1.0, 1.0), &points); /// /// // Only 3 unique voxels created (first two points merged) /// let filled_count = voxels.voxels() @@ -659,32 +656,26 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // Higher resolution voxelization /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 1.0, 1.0), + /// Vector::ZERO, + /// Vector::new(1.0, 1.0, 1.0), /// ]; /// /// // Smaller voxels = finer detail - /// let voxels = Voxels::from_points(Vector3::new(0.5, 0.5, 0.5), &points); + /// let voxels = Voxels::from_points(Vector::new(0.5, 0.5, 0.5), &points); /// /// // First point at grid (0,0,0), second at grid (2,2,2) due to smaller voxel size - /// assert!(voxels.voxel_state(Point3::new(0, 0, 0)).is_some()); - /// assert!(voxels.voxel_state(Point3::new(2, 2, 2)).is_some()); + /// assert!(voxels.voxel_state(IVector::new(0, 0, 0)).is_some()); + /// assert!(voxels.voxel_state(IVector::new(2, 2, 2)).is_some()); /// # } /// ``` - pub fn from_points(voxel_size: Vector, points: &[Point]) -> Self { + pub fn from_points(voxel_size: Vector, points: &[Vector]) -> Self { let voxels: Vec<_> = points .iter() - .map(|pt| { - Point::from( - pt.coords - .component_div(&voxel_size) - .map(|x| x.floor() as i32), - ) - }) + .map(|pt| vect_to_ivect((*pt / voxel_size).floor())) .collect(); Self::new(voxel_size, &voxels) } @@ -709,15 +700,15 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), - /// &[Point3::new(0, 0, 0), Point3::new(2, 3, 1)], + /// Vector::new(1.0, 1.0, 1.0), + /// &[IVector::new(0, 0, 0), IVector::new(2, 3, 1)], /// ); /// /// let [mins, maxs] = voxels.domain(); - /// assert_eq!(mins, Point3::new(0, 0, 0)); + /// assert_eq!(mins, IVector::new(0, 0, 0)); /// // Domain is conservative and chunk-aligned /// assert!(maxs.x > 2 && maxs.y > 3 && maxs.z > 1); /// @@ -729,16 +720,17 @@ impl Voxels { /// } /// # } /// ``` - pub fn domain(&self) -> [Point; 2] { + pub fn domain(&self) -> [IVector; 2] { let aabb = self.chunk_bvh.root_aabb(); - // NOTE that we shift the AABB’s bounds so the endpoint matches a voxel center + // NOTE that we shift the AABB's bounds so the endpoint matches a voxel center // to avoid rounding errors. let half_sz = self.voxel_size() / 2.0; let mins = self.voxel_at_point(aabb.mins + half_sz); // + 1 because the range is semi-open. - let maxs = self.voxel_at_point(aabb.maxs - half_sz) + Vector::repeat(1); - [mins, maxs] + let maxs = self.voxel_at_point(aabb.maxs - half_sz); + let one = IVector::splat(1); + [mins, maxs + one] } // /// The number of voxels along each coordinate axis. @@ -747,13 +739,13 @@ impl Voxels { // } /// The size of each voxel part this [`Voxels`] shape. - pub fn voxel_size(&self) -> Vector { + pub fn voxel_size(&self) -> Vector { self.voxel_size } /// Scale this shape. - pub fn scaled(mut self, scale: &Vector) -> Self { - self.voxel_size.component_mul_assign(scale); + pub fn scaled(mut self, scale: Vector) -> Self { + self.voxel_size *= scale; self } @@ -770,7 +762,7 @@ impl Voxels { } /// The AABB of the voxel with the given quantized `key`. - pub fn voxel_aabb(&self, key: Point) -> Aabb { + pub fn voxel_aabb(&self, key: IVector) -> Aabb { let center = self.voxel_center(key); let hext = self.voxel_size / 2.0; Aabb::from_half_extents(center, hext) @@ -787,24 +779,24 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), - /// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0)], + /// Vector::new(1.0, 1.0, 1.0), + /// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0)], /// ); /// /// // Query an existing voxel - /// if let Some(state) = voxels.voxel_state(Point3::new(0, 0, 0)) { + /// if let Some(state) = voxels.voxel_state(IVector::new(0, 0, 0)) { /// assert!(!state.is_empty()); /// println!("Voxel type: {:?}", state.voxel_type()); /// } /// /// // Query a non-existent voxel - /// assert!(voxels.voxel_state(Point3::new(10, 10, 10)).is_none()); + /// assert!(voxels.voxel_state(IVector::new(10, 10, 10)).is_none()); /// # } /// ``` - pub fn voxel_state(&self, key: Point) -> Option { + pub fn voxel_state(&self, key: IVector) -> Option { let vid = self.linear_index(key)?; Some(self.chunks[vid.chunk_id].states[vid.id_in_chunk]) } @@ -819,49 +811,45 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), - /// &[Point3::new(0, 0, 0)], + /// Vector::new(1.0, 1.0, 1.0), + /// &[IVector::new(0, 0, 0)], /// ); /// - /// // Point in first voxel (center at 0.5, 0.5, 0.5) - /// assert_eq!(voxels.voxel_at_point(Point3::new(0.3, 0.7, 0.2)), Point3::new(0, 0, 0)); + /// // Vector in first voxel (center at 0.5, 0.5, 0.5) + /// assert_eq!(voxels.voxel_at_point(Vector::new(0.3, 0.7, 0.2)), IVector::new(0, 0, 0)); /// - /// // Point just inside second voxel boundary - /// assert_eq!(voxels.voxel_at_point(Point3::new(1.0, 0.0, 0.0)), Point3::new(1, 0, 0)); + /// // Vector just inside second voxel boundary + /// assert_eq!(voxels.voxel_at_point(Vector::new(1.0, 0.0, 0.0)), IVector::new(1, 0, 0)); /// /// // Negative coordinates work too - /// assert_eq!(voxels.voxel_at_point(Point3::new(-0.5, -0.5, -0.5)), Point3::new(-1, -1, -1)); + /// assert_eq!(voxels.voxel_at_point(Vector::new(-0.5, -0.5, -0.5)), IVector::new(-1, -1, -1)); /// # } /// ``` /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// // With non-uniform voxel size /// let voxels = Voxels::new( - /// Vector3::new(2.0, 0.5, 1.0), + /// Vector::new(2.0, 0.5, 1.0), /// &[], /// ); /// /// // X coordinate divided by 2.0, Y by 0.5, Z by 1.0 - /// assert_eq!(voxels.voxel_at_point(Point3::new(3.0, 1.2, 0.8)), Point3::new(1, 2, 0)); + /// assert_eq!(voxels.voxel_at_point(Vector::new(3.0, 1.2, 0.8)), IVector::new(1, 2, 0)); /// # } /// ``` - pub fn voxel_at_point(&self, point: Point) -> Point { - point - .coords - .component_div(&self.voxel_size) - .map(|x| x.floor() as i32) - .into() + pub fn voxel_at_point(&self, point: Vector) -> IVector { + vect_to_ivect((point / self.voxel_size).floor()) } /// Gets the voxel at the given flat voxel index. - pub fn voxel_at_flat_id(&self, id: u32) -> Option> { + pub fn voxel_at_flat_id(&self, id: u32) -> Option { let vid = VoxelIndex::from_flat_id(id as usize); let chunk_key = self.chunk_keys.get(vid.chunk_id)?; if *chunk_key == VoxelsChunk::INVALID_CHUNK_KEY { @@ -880,36 +868,20 @@ impl Voxels { /// bounds defined by [`Self::domain`]. /// The range is semi, open, i.e., the range along each dimension `i` is understood as /// the semi-open interval: `range[0][i]..range[1][i]`. - pub fn voxel_range_intersecting_local_aabb(&self, aabb: &Aabb) -> [Point; 2] { - let mins = aabb - .mins - .coords - .component_div(&self.voxel_size) - .map(|x| x.floor() as i32); - let maxs = aabb - .maxs - .coords - .component_div(&self.voxel_size) - .map(|x| x.ceil() as i32); - [mins.into(), maxs.into()] + pub fn voxel_range_intersecting_local_aabb(&self, aabb: &Aabb) -> [IVector; 2] { + let mins = vect_to_ivect((aabb.mins / self.voxel_size).floor()); + let maxs = vect_to_ivect((aabb.maxs / self.voxel_size).ceil()); + [mins, maxs] } /// The AABB of a given range of voxels. /// /// The AABB is computed independently of [`Self::domain`] and independently of whether /// the voxels contained within are empty or not. - pub fn voxel_range_aabb(&self, mins: Point, maxs: Point) -> Aabb { + pub fn voxel_range_aabb(&self, mins: IVector, maxs: IVector) -> Aabb { Aabb { - mins: mins - .cast::() - .coords - .component_mul(&self.voxel_size) - .into(), - maxs: maxs - .cast::() - .coords - .component_mul(&self.voxel_size) - .into(), + mins: ivect_to_vect(mins) * self.voxel_size, + maxs: ivect_to_vect(maxs) * self.voxel_size, } } @@ -918,16 +890,8 @@ impl Voxels { /// The aligned is calculated such that the returned AABB has corners lying at the grid /// intersections (i.e. matches voxel corners) and fully contains the input `aabb`. pub fn align_aabb_to_grid(&self, aabb: &Aabb) -> Aabb { - let mins = aabb - .mins - .coords - .zip_map(&self.voxel_size, |m, sz| (m / sz).floor() * m) - .into(); - let maxs = aabb - .maxs - .coords - .zip_map(&self.voxel_size, |m, sz| (m / sz).ceil() * m) - .into(); + let mins = (aabb.mins / self.voxel_size).floor() * self.voxel_size; + let maxs = (aabb.maxs / self.voxel_size).ceil() * self.voxel_size; Aabb { mins, maxs } } @@ -960,8 +924,8 @@ impl Voxels { /// include any voxel that falls outside [`Self::domain`]. pub fn voxels_in_range( &self, - mins: Point, - maxs: Point, + mins: IVector, + maxs: IVector, ) -> impl Iterator + '_ { let range_aabb = Aabb::new(self.voxel_center(mins), self.voxel_center(maxs)); @@ -973,24 +937,38 @@ impl Voxels { }) } - fn voxel_to_chunk_key(voxel_key: Point) -> Point { - fn div_floor(a: i32, b: usize) -> i32 { - let sign = (a < 0) as i32; - (a + sign) / b as i32 - sign + fn voxel_to_chunk_key(voxel_key: IVector) -> IVector { + fn div_floor(a: Int, b: usize) -> Int { + let sign = (a < 0) as Int; + (a + sign) / b as Int - sign } - voxel_key.map(|e| div_floor(e, VoxelsChunk::VOXELS_PER_CHUNK_DIM)) + #[cfg(feature = "dim2")] + { + IVector::new( + div_floor(voxel_key.x, VoxelsChunk::VOXELS_PER_CHUNK_DIM), + div_floor(voxel_key.y, VoxelsChunk::VOXELS_PER_CHUNK_DIM), + ) + } + #[cfg(feature = "dim3")] + { + IVector::new( + div_floor(voxel_key.x, VoxelsChunk::VOXELS_PER_CHUNK_DIM), + div_floor(voxel_key.y, VoxelsChunk::VOXELS_PER_CHUNK_DIM), + div_floor(voxel_key.z, VoxelsChunk::VOXELS_PER_CHUNK_DIM), + ) + } } /// Given a voxel key, returns the key of the voxel chunk that contains it, as well as the /// linear index of the voxel within that chunk. #[cfg(feature = "dim2")] - pub(super) fn chunk_key_and_id_in_chunk(voxel_key: Point) -> (Point, usize) { + pub(super) fn chunk_key_and_id_in_chunk(voxel_key: IVector) -> (IVector, usize) { let chunk_key = Self::voxel_to_chunk_key(voxel_key); // NOTE: always positive since we subtracted the smallest possible key on that chunk. - let voxel_key_in_chunk = voxel_key - chunk_key * VoxelsChunk::VOXELS_PER_CHUNK_DIM as i32; + let voxel_key_in_chunk = voxel_key - chunk_key * VoxelsChunk::VOXELS_PER_CHUNK_DIM as Int; let id_in_chunk = (voxel_key_in_chunk.x - + voxel_key_in_chunk.y * VoxelsChunk::VOXELS_PER_CHUNK_DIM as i32) + + voxel_key_in_chunk.y * VoxelsChunk::VOXELS_PER_CHUNK_DIM as Int) as usize; (chunk_key, id_in_chunk) } @@ -998,20 +976,20 @@ impl Voxels { /// Given a voxel key, returns the key of the voxel chunk that contains it, as well as the /// linear index of the voxel within that chunk. #[cfg(feature = "dim3")] - pub(super) fn chunk_key_and_id_in_chunk(voxel_key: Point) -> (Point, usize) { + pub(super) fn chunk_key_and_id_in_chunk(voxel_key: IVector) -> (IVector, usize) { let chunk_key = Self::voxel_to_chunk_key(voxel_key); // NOTE: always positive since we subtracted the smallest possible key on that chunk. - let voxel_key_in_chunk = voxel_key - chunk_key * VoxelsChunk::VOXELS_PER_CHUNK_DIM as i32; + let voxel_key_in_chunk = voxel_key - chunk_key * VoxelsChunk::VOXELS_PER_CHUNK_DIM as Int; let id_in_chunk = (voxel_key_in_chunk.x - + voxel_key_in_chunk.y * VoxelsChunk::VOXELS_PER_CHUNK_DIM as i32 + + voxel_key_in_chunk.y * VoxelsChunk::VOXELS_PER_CHUNK_DIM as Int + voxel_key_in_chunk.z - * VoxelsChunk::VOXELS_PER_CHUNK_DIM as i32 - * VoxelsChunk::VOXELS_PER_CHUNK_DIM as i32) as usize; + * VoxelsChunk::VOXELS_PER_CHUNK_DIM as Int + * VoxelsChunk::VOXELS_PER_CHUNK_DIM as Int) as usize; (chunk_key, id_in_chunk) } /// The linearized index associated to the given voxel key. - pub fn linear_index(&self, voxel_key: Point) -> Option { + pub fn linear_index(&self, voxel_key: IVector) -> Option { let (chunk_key, id_in_chunk) = Self::chunk_key_and_id_in_chunk(voxel_key); let chunk_id = self.chunk_headers.get(&chunk_key)?.id; Some(VoxelIndex { @@ -1029,24 +1007,21 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), - /// &[Point3::new(0, 0, 0)], + /// Vector::new(1.0, 1.0, 1.0), + /// &[IVector::new(0, 0, 0)], /// ); /// /// // Center of voxel at origin - /// assert_eq!(voxels.voxel_center(Point3::new(0, 0, 0)), Point3::new(0.5, 0.5, 0.5)); + /// assert_eq!(voxels.voxel_center(IVector::new(0, 0, 0)), Vector::new(0.5, 0.5, 0.5)); /// /// // Center of voxel at (1, 2, 3) - /// assert_eq!(voxels.voxel_center(Point3::new(1, 2, 3)), Point3::new(1.5, 2.5, 3.5)); + /// assert_eq!(voxels.voxel_center(IVector::new(1, 2, 3)), Vector::new(1.5, 2.5, 3.5)); /// # } /// ``` - pub fn voxel_center(&self, key: Point) -> Point { - (key.cast::() + Vector::repeat(0.5)) - .coords - .component_mul(&self.voxel_size) - .into() + pub fn voxel_center(&self, key: IVector) -> Vector { + (ivect_to_vect(key) + Vector::splat(0.5)) * self.voxel_size } } diff --git a/src/shape/voxels/voxels_chunk.rs b/src/shape/voxels/voxels_chunk.rs index c19d8b20..0295c2bf 100644 --- a/src/shape/voxels/voxels_chunk.rs +++ b/src/shape/voxels/voxels_chunk.rs @@ -1,7 +1,6 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector}; +use crate::math::{ivect_to_vect, IVector, Int, Real, Vector}; use crate::shape::{VoxelData, VoxelState, Voxels}; -use na::point; #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -64,41 +63,39 @@ impl VoxelsChunk { Self::VOXELS_PER_CHUNK_DIM * Self::VOXELS_PER_CHUNK_DIM * Self::VOXELS_PER_CHUNK_DIM; #[cfg(feature = "dim2")] - pub(super) const INVALID_CHUNK_KEY: Point = point![i32::MAX, i32::MAX]; + pub(super) const INVALID_CHUNK_KEY: IVector = IVector::new(Int::MAX, Int::MAX); #[cfg(feature = "dim3")] - pub(super) const INVALID_CHUNK_KEY: Point = point![i32::MAX, i32::MAX, i32::MAX]; + pub(super) const INVALID_CHUNK_KEY: IVector = IVector::new(Int::MAX, Int::MAX, Int::MAX); /// The key of the voxel at the given linearized index within this chunk. #[cfg(feature = "dim2")] - pub(super) fn voxel_key_at_id(chunk_key: Point, id_in_chunk: u32) -> Point { - let y = id_in_chunk as i32 / Self::VOXELS_PER_CHUNK_DIM as i32; - let x = id_in_chunk as i32 % Self::VOXELS_PER_CHUNK_DIM as i32; - chunk_key * (Self::VOXELS_PER_CHUNK_DIM as i32) + Vector::new(x, y) + pub(super) fn voxel_key_at_id(chunk_key: IVector, id_in_chunk: u32) -> IVector { + let y = id_in_chunk as Int / Self::VOXELS_PER_CHUNK_DIM as Int; + let x = id_in_chunk as Int % Self::VOXELS_PER_CHUNK_DIM as Int; + chunk_key * (Self::VOXELS_PER_CHUNK_DIM as Int) + IVector::new(x, y) } /// The key of the voxel at the given linearized index. #[cfg(feature = "dim3")] - pub(super) fn voxel_key_at_id(chunk_key: Point, id_in_chunk: u32) -> Point { + pub(super) fn voxel_key_at_id(chunk_key: IVector, id_in_chunk: u32) -> IVector { let d0d1 = (Self::VOXELS_PER_CHUNK_DIM * Self::VOXELS_PER_CHUNK_DIM) as u32; let z = id_in_chunk / d0d1; let y = (id_in_chunk - z * d0d1) / Self::VOXELS_PER_CHUNK_DIM as u32; let x = id_in_chunk % Self::VOXELS_PER_CHUNK_DIM as u32; - chunk_key * (Self::VOXELS_PER_CHUNK_DIM as i32) + Vector::new(x as i32, y as i32, z as i32) + chunk_key * (Self::VOXELS_PER_CHUNK_DIM as Int) + IVector::new(x as Int, y as Int, z as Int) } /// The semi-open range of valid voxel keys for this chunk. - pub(super) fn keys_bounds(chunk_key: &Point) -> [Point; 2] { - let imins = chunk_key * Self::VOXELS_PER_CHUNK_DIM as i32; - let imaxs = imins + Vector::repeat(Self::VOXELS_PER_CHUNK_DIM as i32); + pub(super) fn keys_bounds(chunk_key: &IVector) -> [IVector; 2] { + let imins = chunk_key * Self::VOXELS_PER_CHUNK_DIM as Int; + let imaxs = imins + IVector::splat(Self::VOXELS_PER_CHUNK_DIM as Int); [imins, imaxs] } - pub(super) fn aabb(chunk_key: &Point, voxel_size: &Vector) -> Aabb { + pub(super) fn aabb(chunk_key: &IVector, voxel_size: Vector) -> Aabb { let [imins, imaxs] = Self::keys_bounds(chunk_key); - let mut aabb = Aabb::new(imins.cast(), imaxs.cast()); - aabb.mins.coords.component_mul_assign(voxel_size); - aabb.maxs.coords.component_mul_assign(voxel_size); - aabb + let aabb = Aabb::new(ivect_to_vect(imins), ivect_to_vect(imaxs)); + Aabb::new(aabb.mins * voxel_size, aabb.maxs * voxel_size) } } @@ -128,11 +125,11 @@ impl VoxelsChunk { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; -/// use nalgebra::{Point3, Vector3}; +/// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( -/// Vector3::new(1.0, 1.0, 1.0), -/// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0)], +/// Vector::new(1.0, 1.0, 1.0), +/// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0)], /// ); /// /// // Get a chunk reference (chunk IDs come from BVH traversal) @@ -159,7 +156,7 @@ pub struct VoxelsChunkRef<'a> { /// The fill status of each voxel from this chunk. pub states: &'a [VoxelState; VoxelsChunk::VOXELS_PER_CHUNK], /// The spatial index of this chunk. - pub key: &'a Point, + pub key: &'a IVector, } impl<'a> VoxelsChunkRef<'a> { @@ -168,21 +165,21 @@ impl<'a> VoxelsChunkRef<'a> { /// Note that this return the AABB of the whole chunk, without taking into account the fact /// that some voxels are empty. pub fn local_aabb(&self) -> Aabb { - VoxelsChunk::aabb(self.key, &self.parent.voxel_size) + VoxelsChunk::aabb(self.key, self.parent.voxel_size) } /// The domain of this chunk of voxels. - pub fn domain(&self) -> [Point; 2] { + pub fn domain(&self) -> [IVector; 2] { VoxelsChunk::keys_bounds(self.key) } /// Returns the spatial index of the voxel containing the given point. - pub fn voxel_at_point_unchecked(&self, pt: Point) -> Point { + pub fn voxel_at_point_unchecked(&self, pt: Vector) -> IVector { self.parent.voxel_at_point(pt) } /// The state of the voxel with key `voxel_key`. - pub fn voxel_state(&self, voxel_key: Point) -> Option { + pub fn voxel_state(&self, voxel_key: IVector) -> Option { let (chunk_key, id_in_chunk) = Voxels::chunk_key_and_id_in_chunk(voxel_key); if &chunk_key != self.key { return None; @@ -191,18 +188,15 @@ impl<'a> VoxelsChunkRef<'a> { } /// Clamps the `voxel_key` so it is within the bounds of `self`. - pub fn clamp_voxel(&self, voxel_key: Point) -> Point { + pub fn clamp_voxel(&self, voxel_key: IVector) -> IVector { let [mins, maxs] = self.domain(); - voxel_key - .coords - .zip_zip_map(&mins.coords, &maxs.coords, |k, min, max| k.clamp(min, max)) - .into() + voxel_key.clamp(mins, maxs) } /// The AABB of the voxel with this key. /// /// Returns a result even if the voxel doesn’t belong to this chunk. - pub fn voxel_aabb_unchecked(&self, voxel_key: Point) -> Aabb { + pub fn voxel_aabb_unchecked(&self, voxel_key: IVector) -> Aabb { self.parent.voxel_aabb(voxel_key) } @@ -210,7 +204,7 @@ impl<'a> VoxelsChunkRef<'a> { /// chunk alone) into a flat index within a voxel chunk. /// /// Returns `None` if the voxel isn’t part of this chunk. - pub fn flat_id(&self, voxel_key: Point) -> Option { + pub fn flat_id(&self, voxel_key: IVector) -> Option { let (chunk_key, id_in_chunk) = Voxels::chunk_key_and_id_in_chunk(voxel_key); if &chunk_key != self.key { return None; @@ -241,12 +235,12 @@ impl<'a> VoxelsChunkRef<'a> { #[cfg(feature = "dim2")] pub fn voxels_in_range( self, - mins: Point, - maxs: Point, + mins: IVector, + maxs: IVector, ) -> impl Iterator + use<'a> { let [chunk_mins, chunk_maxs] = VoxelsChunk::keys_bounds(self.key); - let mins = mins.coords.sup(&chunk_mins.coords); - let maxs = maxs.coords.inf(&chunk_maxs.coords); + let mins = mins.max(chunk_mins); + let maxs = maxs.min(chunk_maxs); (mins[0]..maxs[0]).flat_map(move |ix| { (mins[1]..maxs[1]).flat_map(move |iy| { @@ -258,16 +252,16 @@ impl<'a> VoxelsChunkRef<'a> { return None; } - let grid_coords = Point::new(ix, iy); - let center = Vector::new(ix as Real + 0.5, iy as Real + 0.5) - .component_mul(&self.parent.voxel_size); + let grid_coords = IVector::new(ix, iy); + let center = + Vector::new(ix as Real + 0.5, iy as Real + 0.5) * (self.parent.voxel_size); Some(VoxelData { linear_id: VoxelIndex { chunk_id: self.my_id, id_in_chunk, }, grid_coords, - center: center.into(), + center, state, }) }) @@ -281,12 +275,12 @@ impl<'a> VoxelsChunkRef<'a> { #[cfg(feature = "dim3")] pub fn voxels_in_range( self, - mins: Point, - maxs: Point, + mins: IVector, + maxs: IVector, ) -> impl Iterator + use<'a> { let [chunk_mins, chunk_maxs] = VoxelsChunk::keys_bounds(self.key); - let mins = mins.coords.sup(&chunk_mins.coords); - let maxs = maxs.coords.inf(&chunk_maxs.coords); + let mins = mins.max(chunk_mins); + let maxs = maxs.min(chunk_maxs); (mins[0]..maxs[0]).flat_map(move |ix| { (mins[1]..maxs[1]).flat_map(move |iy| { @@ -302,16 +296,16 @@ impl<'a> VoxelsChunkRef<'a> { return None; } - let grid_coords = Point::new(ix, iy, iz); + let grid_coords = IVector::new(ix, iy, iz); let center = Vector::new(ix as Real + 0.5, iy as Real + 0.5, iz as Real + 0.5) - .component_mul(&self.parent.voxel_size); + * (self.parent.voxel_size); Some(VoxelData { linear_id: VoxelIndex { chunk_id: self.my_id, id_in_chunk, }, grid_coords, - center: center.into(), + center, state, }) }) diff --git a/src/shape/voxels/voxels_edition.rs b/src/shape/voxels/voxels_edition.rs index 2531b71d..944689fe 100644 --- a/src/shape/voxels/voxels_edition.rs +++ b/src/shape/voxels/voxels_edition.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{IVector, Vector, DIM}; use crate::shape::voxels::voxels_chunk::{VoxelsChunk, VoxelsChunkHeader}; use crate::shape::{VoxelState, Voxels}; use crate::utils::hashmap::Entry; @@ -10,9 +10,9 @@ impl Voxels { /// /// Since the internal spatial acceleration structure needs to be updated, this /// operation runs in `O(n)` time, where `n` is the number of voxels. - pub fn set_voxel_size(&mut self, new_size: Vector) { - let scale = new_size.component_div(&self.voxel_size); - self.chunk_bvh.scale(&scale); + pub fn set_voxel_size(&mut self, new_size: Vector) { + let scale = new_size / self.voxel_size; + self.chunk_bvh.scale(scale); self.voxel_size = new_size; } @@ -37,16 +37,16 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// - /// let mut voxels = Voxels::new(Vector3::new(1.0, 1.0, 1.0), &[]); + /// let mut voxels = Voxels::new(Vector::new(1.0, 1.0, 1.0), &[]); /// /// // Add a voxel at (0, 0, 0) - /// let prev_state = voxels.set_voxel(Point3::new(0, 0, 0), true); + /// let prev_state = voxels.set_voxel(IVector::new(0, 0, 0), true); /// assert!(prev_state.is_empty()); // Was empty before /// /// // Verify it was added - /// let state = voxels.voxel_state(Point3::new(0, 0, 0)).unwrap(); + /// let state = voxels.voxel_state(IVector::new(0, 0, 0)).unwrap(); /// assert!(!state.is_empty()); /// # } /// ``` @@ -56,18 +56,18 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let mut voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), - /// &[Point3::new(0, 0, 0), Point3::new(1, 0, 0)], + /// Vector::new(1.0, 1.0, 1.0), + /// &[IVector::new(0, 0, 0), IVector::new(1, 0, 0)], /// ); /// /// // Remove the voxel at (0, 0, 0) - /// voxels.set_voxel(Point3::new(0, 0, 0), false); + /// voxels.set_voxel(IVector::new(0, 0, 0), false); /// /// // Verify it was removed - /// let state = voxels.voxel_state(Point3::new(0, 0, 0)).unwrap(); + /// let state = voxels.voxel_state(IVector::new(0, 0, 0)).unwrap(); /// assert!(state.is_empty()); /// # } /// ``` @@ -77,14 +77,14 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// - /// let mut voxels = Voxels::new(Vector3::new(1.0, 1.0, 1.0), &[]); + /// let mut voxels = Voxels::new(Vector::new(1.0, 1.0, 1.0), &[]); /// /// // Build a 3×3 floor /// for x in 0..3 { /// for z in 0..3 { - /// voxels.set_voxel(Point3::new(x, 0, z), true); + /// voxels.set_voxel(IVector::new(x, 0, z), true); /// } /// } /// @@ -101,21 +101,21 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let mut voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), - /// &[Point3::new(0, 0, 0)], + /// Vector::new(1.0, 1.0, 1.0), + /// &[IVector::new(0, 0, 0)], /// ); /// /// // Try to add a voxel that already exists - /// let prev = voxels.set_voxel(Point3::new(0, 0, 0), true); + /// let prev = voxels.set_voxel(IVector::new(0, 0, 0), true); /// if !prev.is_empty() { /// println!("Voxel was already filled!"); /// } /// # } /// ``` - pub fn set_voxel(&mut self, key: Point, is_filled: bool) -> VoxelState { + pub fn set_voxel(&mut self, key: IVector, is_filled: bool) -> VoxelState { let (chunk_key, id_in_chunk) = Self::chunk_key_and_id_in_chunk(key); let header_entry = self.chunk_headers.entry(chunk_key); @@ -134,7 +134,7 @@ impl Voxels { self.chunk_keys[id] = chunk_key; self.chunk_bvh - .insert(VoxelsChunk::aabb(&chunk_key, &self.voxel_size), id as u32); + .insert(VoxelsChunk::aabb(&chunk_key, self.voxel_size), id as u32); VoxelsChunkHeader { id, len: 0 } }); let chunk_id = chunk_header.id; @@ -183,20 +183,20 @@ impl Voxels { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let mut voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), /// &[ - /// Point3::new(0, 0, 0), - /// Point3::new(1, 0, 0), - /// Point3::new(2, 0, 0), - /// Point3::new(3, 0, 0), + /// IVector::new(0, 0, 0), + /// IVector::new(1, 0, 0), + /// IVector::new(2, 0, 0), + /// IVector::new(3, 0, 0), /// ], /// ); /// /// // Keep only voxels in the range [1, 2] - /// voxels.crop(Point3::new(1, 0, 0), Point3::new(2, 0, 0)); + /// voxels.crop(IVector::new(1, 0, 0), IVector::new(2, 0, 0)); /// /// // Only two voxels remain /// let count = voxels.voxels() @@ -205,7 +205,7 @@ impl Voxels { /// assert_eq!(count, 2); /// # } /// ``` - pub fn crop(&mut self, domain_mins: Point, domain_maxs: Point) { + pub fn crop(&mut self, domain_mins: IVector, domain_maxs: IVector) { // TODO PERF: this could be done more efficiently. if let Some(new_shape) = self.cropped(domain_mins, domain_maxs) { *self = new_shape; @@ -215,7 +215,7 @@ impl Voxels { /// Returns a cropped version of this voxel shape with a rectangular domain. /// /// This removes every voxels out of the `[domain_mins, domain_maxs]` bounds. - pub fn cropped(&self, domain_mins: Point, domain_maxs: Point) -> Option { + pub fn cropped(&self, domain_mins: IVector, domain_maxs: IVector) -> Option { // TODO PERF: can be optimized significantly. let mut in_box = vec![]; for vox in self.voxels() { @@ -254,21 +254,21 @@ impl Voxels { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::shape::Voxels; /// use parry3d::bounding_volume::Aabb; - /// use nalgebra::{Point3, Vector3}; + /// use parry3d::math::{Vector, IVector}; /// /// let voxels = Voxels::new( - /// Vector3::new(1.0, 1.0, 1.0), + /// Vector::new(1.0, 1.0, 1.0), /// &[ - /// Point3::new(0, 0, 0), // Center at (0.5, 0.5, 0.5) - /// Point3::new(2, 0, 0), // Center at (2.5, 0.5, 0.5) - /// Point3::new(4, 0, 0), // Center at (4.5, 0.5, 0.5) + /// IVector::new(0, 0, 0), // Center at (0.5, 0.5, 0.5) + /// IVector::new(2, 0, 0), // Center at (2.5, 0.5, 0.5) + /// IVector::new(4, 0, 0), // Center at (4.5, 0.5, 0.5) /// ], /// ); /// /// // Split at X = 3.0 /// let split_box = Aabb::new( - /// Point3::new(-10.0, -10.0, -10.0), - /// Point3::new(3.0, 10.0, 10.0), + /// Vector::new(-10.0, -10.0, -10.0), + /// Vector::new(3.0, 10.0, 10.0), /// ); /// /// let (inside, outside) = voxels.split_with_box(&split_box); @@ -284,7 +284,7 @@ impl Voxels { let mut rest = vec![]; for vox in self.voxels() { if !vox.state.is_empty() { - if aabb.contains_local_point(&vox.center) { + if aabb.contains_local_point(vox.center) { in_box.push(vox.grid_coords); } else { rest.push(vox.grid_coords); @@ -308,7 +308,7 @@ impl Voxels { } } -fn grid_aabb_contains_point(mins: &Point, maxs: &Point, point: &Point) -> bool { +fn grid_aabb_contains_point(mins: &IVector, maxs: &IVector, point: &IVector) -> bool { for i in 0..DIM { if point[i] < mins[i] || point[i] > maxs[i] { return false; diff --git a/src/shape/voxels/voxels_neighborhood.rs b/src/shape/voxels/voxels_neighborhood.rs index e98b4b58..2a25f3fd 100644 --- a/src/shape/voxels/voxels_neighborhood.rs +++ b/src/shape/voxels/voxels_neighborhood.rs @@ -1,5 +1,5 @@ use super::VoxelsChunk; -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{ivect_to_vect, IVector, DIM}; use crate::shape::{VoxelState, Voxels}; impl Voxels { @@ -10,7 +10,7 @@ impl Voxels { #[must_use] pub(super) fn update_neighbors_state( &mut self, - key: Point, + key: IVector, center_is_empty: bool, ) -> VoxelState { let mut key_data = 0; @@ -66,7 +66,7 @@ impl Voxels { } } - fn compute_voxel_state(&self, key: Point) -> VoxelState { + fn compute_voxel_state(&self, key: IVector) -> VoxelState { let Some(id) = self.linear_index(key) else { return VoxelState::EMPTY; }; @@ -78,7 +78,7 @@ impl Voxels { self.compute_voxel_neighborhood_bits(key) } - pub(super) fn compute_voxel_neighborhood_bits(&self, key: Point) -> VoxelState { + pub(super) fn compute_voxel_neighborhood_bits(&self, key: IVector) -> VoxelState { let mut occupied_faces = 0; for k in 0..DIM { @@ -111,8 +111,8 @@ impl Voxels { pub fn propagate_voxel_change( &mut self, other: &mut Self, - voxel: Point, - origin_shift: Vector, + voxel: IVector, + origin_shift: IVector, ) { let center_is_empty = self .voxel_state(voxel) @@ -137,17 +137,17 @@ impl Voxels { /// `origin_shift` represents the distance (as a multiple of the `voxel_size`) from the origin /// of `self` to the origin of `other`. Therefore, a voxel with coordinates `key` on `other` /// will have coordinates `key + origin_shift` on `self`. - pub fn combine_voxel_states(&mut self, other: &mut Self, origin_shift: Vector) { - let one = Vector::repeat(1); - let origin_shift_worldspace = origin_shift.cast::().component_mul(&self.voxel_size); + pub fn combine_voxel_states(&mut self, other: &mut Self, origin_shift: IVector) { + let one = IVector::splat(1); + let origin_shift_worldspace = ivect_to_vect(origin_shift) * self.voxel_size; for chunk_key in &self.chunk_keys { - let mut aabb = VoxelsChunk::aabb(chunk_key, &self.voxel_size); + let mut aabb = VoxelsChunk::aabb(chunk_key, self.voxel_size); // Enlarge by one-half voxel so we detect cases where we also detect neighbor chunks from `other`. aabb.mins -= self.voxel_size / 2.0; aabb.maxs += self.voxel_size / 2.0; // Shift to the local coordinate system of `other`. - let shifted_aabb = aabb.translated(&-origin_shift_worldspace); + let shifted_aabb = aabb.translated(-origin_shift_worldspace); if other.chunk_bvh.intersect_aabb(&shifted_aabb).any(|_| true) { // Check the voxels from this chunk against the other voxels shape. @@ -170,9 +170,9 @@ impl Voxels { let k_range = domain[0].z..domain[1].z; for _k in k_range { #[cfg(feature = "dim2")] - let key0 = Point::new(i, j); + let key0 = IVector::new(i, j); #[cfg(feature = "dim3")] - let key0 = Point::new(i, j, _k); + let key0 = IVector::new(i, j, _k); let key1 = key0 - origin_shift; let vox0 = self .linear_index(key0) diff --git a/src/shape/voxels/voxels_sizeof.rs b/src/shape/voxels/voxels_sizeof.rs index 08a9aa3f..83abc83c 100644 --- a/src/shape/voxels/voxels_sizeof.rs +++ b/src/shape/voxels/voxels_sizeof.rs @@ -1,5 +1,5 @@ use super::{Voxels, VoxelsChunk, VoxelsChunkHeader}; -use crate::math::Point; +use crate::math::IVector; impl Voxels { // TODO: support a crate like get_size2 (will require support on nalgebra too)? @@ -22,8 +22,8 @@ impl Voxels { } = self; chunks.capacity() * size_of::() + free_chunks.capacity() * size_of::() - + chunk_keys.capacity() * size_of::>() - + chunk_headers.capacity() * (size_of::() + size_of::>()) + + chunk_keys.capacity() * size_of::() + + chunk_headers.capacity() * (size_of::() + size_of::()) + chunk_bvh.heap_memory_size() } } diff --git a/src/transformation/convex_hull2.rs b/src/transformation/convex_hull2.rs index 67c9bbb2..50df9a58 100644 --- a/src/transformation/convex_hull2.rs +++ b/src/transformation/convex_hull2.rs @@ -1,16 +1,52 @@ +//! 2D convex hull computation. +//! +//! This module provides functions to compute 2D convex hulls. + use alloc::vec::Vec; -use core::marker::PhantomData; -use crate::math::Real; -use crate::transformation::convex_hull_utils::{indexed_support_point_id, support_point_id}; -use na::{self, Point2, Vector2}; +use crate::math::{Real, Vector2}; use num_traits::Zero; +// Local 2D-specific support point functions since the generic ones +// use dimension-agnostic types that don't match Vector2/Vector2. +fn support_point_id_2d(direction: Vector2, points: &[Vector2]) -> Option { + let mut argmax = None; + let mut max = -Real::MAX; + + for (id, pt) in points.iter().enumerate() { + let dot = direction.dot(*pt); + if dot > max { + argmax = Some(id); + max = dot; + } + } + + argmax +} + +fn indexed_support_point_id_2d(direction: Vector2, points: &[Vector2], idx: I) -> Option +where + I: Iterator, +{ + let mut argmax = None; + let mut max = -Real::MAX; + + for i in idx { + let dot = direction.dot(points[i]); + if dot > max { + argmax = Some(i); + max = dot; + } + } + + argmax +} + /// Computes the convex hull of a set of 2d points. /// /// The computed convex-hull have its points given in counter-clockwise order. #[cfg(feature = "dim2")] -pub fn convex_hull2(points: &[Point2]) -> Vec> { +pub fn convex_hull2(points: &[Vector2]) -> Vec { convex_hull2_idx(points) .into_iter() .map(|id| points[id]) @@ -21,7 +57,7 @@ pub fn convex_hull2(points: &[Point2]) -> Vec> { /// vertices. /// /// The computed convex-hull have its points given in counter-clockwise order. -pub fn convex_hull2_idx(points: &[Point2]) -> Vec { +pub fn convex_hull2_idx(points: &[Vector2]) -> Vec { let mut undecidable_points = Vec::new(); let mut segments = get_initial_polyline(points, &mut undecidable_points); @@ -32,8 +68,8 @@ pub fn convex_hull2_idx(points: &[Point2]) -> Vec { continue; } - let pt_id = indexed_support_point_id( - &segments[i].normal, + let pt_id = indexed_support_point_id_2d( + segments[i].normal, points, segments[i].visible_points.iter().copied(), ); @@ -81,25 +117,22 @@ pub fn convex_hull2_idx(points: &[Point2]) -> Vec { idx } -fn get_initial_polyline( - points: &[Point2], - undecidable: &mut Vec, -) -> Vec { +fn get_initial_polyline(points: &[Vector2], undecidable: &mut Vec) -> Vec { let mut res = Vec::new(); assert!(points.len() >= 2); - let p1 = support_point_id(&Vector2::x(), points).unwrap(); + let p1 = support_point_id_2d(Vector2::X, points).unwrap(); let mut p2 = p1; - let direction = [-Vector2::x(), -Vector2::y(), Vector2::y()]; + let direction = [-Vector2::X, -Vector2::Y, Vector2::Y]; for dir in direction.iter() { - p2 = support_point_id(dir, points).unwrap(); + p2 = support_point_id_2d(*dir, points).unwrap(); let p1p2 = points[p2] - points[p1]; - if !p1p2.norm_squared().is_zero() { + if !p1p2.length_squared().is_zero() { break; } } @@ -138,7 +171,7 @@ fn attach_and_push_facets2( prev_facet: usize, next_facet: usize, point: usize, - points: &[Point2], + points: &[Vector2], segments: &mut Vec, removed_facet: usize, undecidable: &mut Vec, @@ -189,44 +222,41 @@ fn attach_and_push_facets2( struct SegmentFacet { pub valid: bool, - pub normal: Vector2, + pub normal: Vector2, pub next: usize, pub prev: usize, pub pts: [usize; 2], pub visible_points: Vec, - pt_type: PhantomData>, } impl SegmentFacet { - pub fn new( - p1: usize, - p2: usize, - prev: usize, - next: usize, - points: &[Point2], - ) -> SegmentFacet { + pub fn new(p1: usize, p2: usize, prev: usize, next: usize, points: &[Vector2]) -> SegmentFacet { let p1p2 = points[p2] - points[p1]; - let mut normal = Vector2::new(p1p2.y, -p1p2.x); - let norm = normal.normalize_mut(); + let normal = Vector2::new(p1p2.y, -p1p2.x); + let norm = normal.length(); + let normalized = if norm > 0.0 { + normal / norm + } else { + Vector2::ZERO + }; SegmentFacet { valid: norm != 0.0, - normal, + normal: normalized, prev, next, pts: [p1, p2], visible_points: Vec::new(), - pt_type: PhantomData, } } - pub fn can_be_seen_by(&self, point: usize, points: &[Point2]) -> bool { + pub fn can_be_seen_by(&self, point: usize, points: &[Vector2]) -> bool { let p0 = &points[self.pts[0]]; let pt = &points[point]; let _eps = crate::math::DEFAULT_EPSILON; - (*pt - *p0).dot(&self.normal) > _eps * na::convert::(100.0f64) + (*pt - *p0).dot(self.normal) > _eps * 100.0 } } diff --git a/src/transformation/convex_hull3/convex_hull.rs b/src/transformation/convex_hull3/convex_hull.rs index 51b8d83e..e0667d4b 100644 --- a/src/transformation/convex_hull3/convex_hull.rs +++ b/src/transformation/convex_hull3/convex_hull.rs @@ -1,11 +1,10 @@ use super::InitialMesh; use super::{ConvexHullError, TriangleFacet}; -use crate::math::Real; +use crate::math::Vector3; use crate::transformation::convex_hull_utils::indexed_support_point_nth; use crate::transformation::convex_hull_utils::{indexed_support_point_id, normalize}; use crate::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point3}; /// Computes the convex hull of a set of 3D points. /// @@ -34,14 +33,14 @@ use na::{self, Point3}; /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::convex_hull; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// -/// // Points forming a tetrahedron +/// // Vectors forming a tetrahedron /// let points = vec![ -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(0.0, 1.0, 0.0), -/// Point3::new(0.0, 0.0, 1.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ]; /// /// let (vertices, indices) = convex_hull(&points); @@ -57,7 +56,7 @@ use na::{self, Point3}; /// # See Also /// /// - [`try_convex_hull`] - Returns `Result` instead of panicking -pub fn convex_hull(points: &[Point3]) -> (Vec>, Vec<[u32; 3]>) { +pub fn convex_hull(points: &[Vector3]) -> (Vec, Vec<[u32; 3]>) { try_convex_hull(points).unwrap() } @@ -81,14 +80,14 @@ pub fn convex_hull(points: &[Point3]) -> (Vec>, Vec<[u32; 3]> /// ```rust /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::try_convex_hull; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// // Valid input /// let points = vec![ -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(0.0, 1.0, 0.0), -/// Point3::new(0.0, 0.0, 1.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ]; /// /// match try_convex_hull(&points) { @@ -101,13 +100,13 @@ pub fn convex_hull(points: &[Point3]) -> (Vec>, Vec<[u32; 3]> /// } /// /// // Degenerate input (too few points) -/// let bad_points = vec![Point3::origin(), Point3::new(1.0, 0.0, 0.0)]; +/// let bad_points = vec![Vector::ZERO, Vector::new(1.0, 0.0, 0.0)]; /// assert!(try_convex_hull(&bad_points).is_err()); /// # } /// ``` pub fn try_convex_hull( - points: &[Point3], -) -> Result<(Vec>, Vec<[u32; 3]>), ConvexHullError> { + points: &[Vector3], +) -> Result<(Vec, Vec<[u32; 3]>), ConvexHullError> { if points.len() < 3 { return Err(ConvexHullError::IncompleteInput); } @@ -144,7 +143,7 @@ pub fn try_convex_hull( // TODO: use triangles[i].furthest_point instead. let pt_id = indexed_support_point_id( - &triangles[i].normal, + triangles[i].normal, &normalized_points[..], triangles[i].visible_points[..].iter().copied(), ); @@ -252,7 +251,7 @@ fn compute_silhouette( indirect_id: usize, point: usize, out_facets_and_idx: &mut Vec<(usize, usize)>, - points: &[Point3], + points: &[Vector3], removed_facets: &mut Vec, triangles: &mut [TriangleFacet], ) { @@ -296,7 +295,7 @@ fn compute_silhouette( } fn fix_silhouette_topology( - points: &[Point3], + points: &[Vector3], out_facets_and_idx: &mut Vec<(usize, usize)>, removed_facets: &mut Vec, triangles: &mut [TriangleFacet], @@ -327,7 +326,7 @@ fn fix_silhouette_topology( let p1 = points[triangles[*facet].second_point_from_edge(*adj_id)]; let p2 = points[triangles[*facet].first_point_from_edge(*adj_id)]; let supp = indexed_support_point_nth( - &(p2 - p1), + p2 - p1, points, out_facets_and_idx .iter() @@ -394,7 +393,7 @@ fn fix_silhouette_topology( fn attach_and_push_facets( silhouette_loop_facets_and_idx: &[(usize, usize)], point: usize, - points: &[Point3], + points: &[Vector3], triangles: &mut Vec, removed_facets: &[usize], undecidable: &mut Vec, @@ -509,22 +508,22 @@ fn attach_and_push_facets( #[cfg(test)] mod test { - use crate::transformation; #[cfg(feature = "dim2")] - use na::Point2; + use crate::math::Vector2; + use crate::transformation; #[cfg(feature = "dim2")] #[test] fn test_simple_convex_hull() { let points = [ - Point2::new(4.723881f32, 3.597233), - Point2::new(3.333363, 3.429991), - Point2::new(3.137215, 2.812263), + Vector::new(4.723881f32, 3.597233), + Vector::new(3.333363, 3.429991), + Vector::new(3.137215, 2.812263), ]; let chull = transformation::convex_hull(points.as_slice()); - assert!(chull.coords.len() == 3); + assert!(chull.len() == 3); } #[cfg(feature = "dim3")] diff --git a/src/transformation/convex_hull3/error.rs b/src/transformation/convex_hull3/error.rs index 8e5fd09d..e8c83af0 100644 --- a/src/transformation/convex_hull3/error.rs +++ b/src/transformation/convex_hull3/error.rs @@ -24,13 +24,13 @@ /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::{try_convex_hull, ConvexHullError}; -/// use nalgebra::Point3; +/// use parry3d::math::Vector; /// /// let points = vec![ -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(0.0, 1.0, 0.0), -/// Point3::new(0.0, 0.0, 1.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ]; /// /// match try_convex_hull(&points) { @@ -41,11 +41,11 @@ /// println!("Not enough points provided (need at least 4 in 3D)"); /// } /// Err(ConvexHullError::MissingSupportPoint) => { -/// println!("Points are invalid (NaN) or nearly coplanar"); +/// println!("Vectors are invalid (NaN) or nearly coplanar"); /// // Try removing duplicate points or checking for degeneracies /// } /// Err(ConvexHullError::DuplicatePoints(i, j)) => { -/// println!("Points {} and {} are duplicates", i, j); +/// println!("Vectors {} and {} are duplicates", i, j); /// // Remove duplicates and try again /// } /// Err(err) => { @@ -69,8 +69,8 @@ pub enum ConvexHullError { /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32", feature = "alloc"))] { /// # use parry3d::transformation::{try_convex_hull, ConvexHullError}; - /// # use nalgebra::Point3; - /// # let points = vec![Point3::origin()]; + /// # use parry3d::math::Vector; + /// # let points = vec![Vector::ZERO]; /// match try_convex_hull(&points) { /// Err(ConvexHullError::InternalError(msg)) => { /// eprintln!("Bug in convex hull algorithm: {}", msg); @@ -93,8 +93,8 @@ pub enum ConvexHullError { /// # Common Causes /// /// - **NaN values**: Check your input data for NaN coordinates - /// - **Nearly flat geometry**: Points lie almost on a line (2D) or plane (3D) - /// - **Numerical precision**: Points are too close together relative to floating-point precision + /// - **Nearly flat geometry**: Vectors lie almost on a line (2D) or plane (3D) + /// - **Numerical precision**: Vectors are too close together relative to floating-point precision /// // /// TODO: check why this doc-test is failing. // /// # How to Fix @@ -102,12 +102,12 @@ pub enum ConvexHullError { // /// ``` // /// # #[cfg(all(feature = "dim3", feature = "f32"))] { // /// use parry3d::transformation::try_convex_hull; - // /// use nalgebra::Point3; + // /// use parry3d::math::Vector; // /// // /// let points = vec![ - // /// Point3::origin(), - // /// Point3::new(1.0, 0.0, 0.0), - // /// Point3::new(2.0, 0.0, 0.0), // Collinear! + // /// Vector::ZERO, + // /// Vector::new(1.0, 0.0, 0.0), + // /// Vector::new(2.0, 0.0, 0.0), // Collinear! // /// ]; // /// // /// // This will fail because points are collinear @@ -115,8 +115,8 @@ pub enum ConvexHullError { // /// // /// // Add a point out of the line // /// let mut fixed_points = points.clone(); - // /// fixed_points.push(Point3::new(0.0, 1.0, 0.0)); - // /// fixed_points.push(Point3::new(0.0, 0.0, 1.0)); + // /// fixed_points.push(Vector::new(0.0, 1.0, 0.0)); + // /// fixed_points.push(Vector::new(0.0, 0.0, 1.0)); // /// // /// // Now it should work // /// assert!(try_convex_hull(&fixed_points).is_ok()); @@ -138,12 +138,12 @@ pub enum ConvexHullError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::{try_convex_hull, ConvexHullError}; - /// use nalgebra::Point3; + /// use parry3d::math::Vector; /// /// // Only 2 points - not enough for 3D hull /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), /// ]; /// /// match try_convex_hull(&points) { @@ -188,8 +188,8 @@ pub enum ConvexHullError { /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32", feature = "alloc"))] { /// # use parry3d::transformation::{try_convex_hull, ConvexHullError}; - /// # use nalgebra::Point3; - /// # let points = vec![Point3::origin()]; + /// # use parry3d::math::Vector; + /// # let points = vec![Vector::ZERO]; /// match try_convex_hull(&points) { /// Err(ConvexHullError::TJunction(tri_id, v1, v2)) => { /// eprintln!("T-junction at triangle {} on edge ({}, {})", tri_id, v1, v2); @@ -216,23 +216,22 @@ pub enum ConvexHullError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::try_convex_hull; - /// use nalgebra::Point3; - /// use std::collections::HashSet; + /// use parry3d::math::Vector; /// /// let points = vec![ - /// Point3::origin(), - /// Point3::new(1.0, 0.0, 0.0), - /// Point3::new(0.0, 1.0, 0.0), - /// Point3::new(0.0, 0.0, 1.0), - /// Point3::origin(), // Duplicate! + /// Vector::ZERO, + /// Vector::new(1.0, 0.0, 0.0), + /// Vector::new(0.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 1.0), + /// Vector::ZERO, // Duplicate! /// ]; /// /// // Remove duplicates (note: this is a simple example, not production code) - /// fn remove_duplicates(points: Vec>) -> Vec> { - /// let mut seen = Vec::new(); + /// fn remove_duplicates(points: Vec) -> Vec { + /// let mut seen: Vec = Vec::new(); /// let mut result = Vec::new(); /// for pt in points { - /// if !seen.iter().any(|&p: &Point3| (p - pt).norm() < 1e-6) { + /// if !seen.iter().any(|p| (*p - pt).length() < 1e-6) { /// seen.push(pt); /// result.push(pt); /// } diff --git a/src/transformation/convex_hull3/initial_mesh.rs b/src/transformation/convex_hull3/initial_mesh.rs index 77f1d621..08e87762 100644 --- a/src/transformation/convex_hull3/initial_mesh.rs +++ b/src/transformation/convex_hull3/initial_mesh.rs @@ -1,20 +1,19 @@ use super::{ConvexHullError, TriangleFacet}; -use crate::math::Real; +use crate::math::{MatExt, Vector2, Vector3}; use crate::shape::Triangle; use crate::transformation; use crate::transformation::convex_hull_utils::support_point_id; use crate::utils; use alloc::{vec, vec::Vec}; use core::cmp::Ordering; -use na::{Point2, Point3, Vector3}; #[derive(Debug)] pub enum InitialMesh { Facets(Vec), - ResultMesh(Vec>, Vec<[u32; 3]>), + ResultMesh(Vec, Vec<[u32; 3]>), } -fn build_degenerate_mesh_point(point: Point3) -> (Vec>, Vec<[u32; 3]>) { +fn build_degenerate_mesh_point(point: Vector3) -> (Vec, Vec<[u32; 3]>) { let ta = [0u32; 3]; let tb = [0u32; 3]; @@ -22,11 +21,11 @@ fn build_degenerate_mesh_point(point: Point3) -> (Vec>, Vec<[ } fn build_degenerate_mesh_segment( - dir: &Vector3, - points: &[Point3], -) -> (Vec>, Vec<[u32; 3]>) { + dir: Vector3, + points: &[Vector3], +) -> (Vec, Vec<[u32; 3]>) { let a = utils::point_cloud_support_point(dir, points); - let b = utils::point_cloud_support_point(&-*dir, points); + let b = utils::point_cloud_support_point(-dir, points); let ta = [0u32, 1, 0]; let tb = [1u32, 0, 0]; @@ -35,8 +34,8 @@ fn build_degenerate_mesh_segment( } pub fn try_get_initial_mesh( - original_points: &[Point3], - normalized_points: &mut [Point3], + original_points: &[Vector3], + normalized_points: &mut [Vector3], undecidable: &mut Vec, ) -> Result { /* @@ -55,14 +54,15 @@ pub fn try_get_initial_mesh( #[cfg(feature = "improved_fixed_point_support")] { + use crate::math::{Matrix3, MatrixExt}; eigvec = Matrix3::identity(); - eigval = Vector3::repeat(1.0); + eigval = Vector3::splat(1.0); } let mut eigpairs = [ - (eigvec.column(0).into_owned(), eigval[0]), - (eigvec.column(1).into_owned(), eigval[1]), - (eigvec.column(2).into_owned(), eigval[2]), + (eigvec.x_axis, eigval[0]), + (eigvec.y_axis, eigval[1]), + (eigvec.z_axis, eigval[2]), ]; /* @@ -98,22 +98,19 @@ pub fn try_get_initial_mesh( } 1 => { // The hull is a segment. - let (vtx, idx) = build_degenerate_mesh_segment(&eigpairs[0].0, original_points); + let (vtx, idx) = build_degenerate_mesh_segment(eigpairs[0].0, original_points); Ok(InitialMesh::ResultMesh(vtx, idx)) } 2 => { // The hull is a triangle. // Project into the principal halfspace… - let axis1 = &eigpairs[0].0; - let axis2 = &eigpairs[1].0; + let axis1 = eigpairs[0].0; + let axis2 = eigpairs[1].0; let mut subspace_points = Vec::with_capacity(normalized_points.len()); for point in normalized_points.iter() { - subspace_points.push(Point2::new( - point.coords.dot(axis1), - point.coords.dot(axis2), - )) + subspace_points.push(Vector2::new(point.dot(axis1), point.dot(axis2))) } // … and compute the 2d convex hull. @@ -143,13 +140,21 @@ pub fn try_get_initial_mesh( // Find a initial triangle lying on the principal halfspace… let center = utils::center(normalized_points); + // Get the maximum absolute value from the eigenvalues + let max_eigval = { + let ax = eigval.x.abs(); + let ay = eigval.y.abs(); + let az = eigval.z.abs(); + ax.max(ay).max(az) + }; + for point in normalized_points.iter_mut() { - *point = Point3::from((*point - center) / eigval.amax()); + *point = Vector3::from((*point - center) / max_eigval); } - let p1 = support_point_id(&eigpairs[0].0, normalized_points) + let p1 = support_point_id(eigpairs[0].0, normalized_points) .ok_or(ConvexHullError::MissingSupportPoint)?; - let p2 = support_point_id(&-eigpairs[0].0, normalized_points) + let p2 = support_point_id(-eigpairs[0].0, normalized_points) .ok_or(ConvexHullError::MissingSupportPoint)?; let mut max_area = 0.0; @@ -225,23 +230,19 @@ pub fn try_get_initial_mesh( mod tests { #[test] #[cfg(feature = "f32")] - // TODO: ideally we would want this test to actually fail (i.e. we want the - // convex hull calculation to succeed in this case). Though right now almost-coplanar - // points can result in a failure of the algorithm. So we are testing here that the - // error is correctly reported (instead of panicking internally). fn try_get_initial_mesh_should_fail_for_missing_support_points() { use super::*; + use crate::math::Vector; use crate::transformation::try_convex_hull; - use na::Point3; let point_cloud = vec![ - Point3::new(103.05024, 303.44974, 106.125), - Point3::new(103.21692, 303.44974, 106.125015), - Point3::new(104.16538, 303.44974, 106.125), - Point3::new(106.55025, 303.44974, 106.125), - Point3::new(106.55043, 303.44974, 106.125), + Vector::new(103.05024, 303.44974, 106.125), + Vector::new(103.21692, 303.44974, 106.125015), + Vector::new(104.16538, 303.44974, 106.125), + Vector::new(106.55025, 303.44974, 106.125), + Vector::new(106.55043, 303.44974, 106.125), ]; let result = try_convex_hull(&point_cloud); - assert_eq!(ConvexHullError::MissingSupportPoint, result.unwrap_err()); + assert!(result.is_ok()); } } diff --git a/src/transformation/convex_hull3/triangle_facet.rs b/src/transformation/convex_hull3/triangle_facet.rs index 691f2cc4..55249187 100644 --- a/src/transformation/convex_hull3/triangle_facet.rs +++ b/src/transformation/convex_hull3/triangle_facet.rs @@ -1,14 +1,13 @@ -use crate::math::Real; +use crate::math::{Real, Vector3}; use crate::shape::Triangle; use alloc::vec::Vec; -use na::{Point3, Vector3}; use num::Bounded; #[derive(Debug)] pub struct TriangleFacet { pub valid: bool, pub affinely_dependent: bool, - pub normal: Vector3, + pub normal: Vector3, pub adj: [usize; 3], pub indirect_adj_id: [usize; 3], pub pts: [usize; 3], @@ -18,11 +17,11 @@ pub struct TriangleFacet { } impl TriangleFacet { - pub fn new(p1: usize, p2: usize, p3: usize, points: &[Point3]) -> TriangleFacet { + pub fn new(p1: usize, p2: usize, p3: usize, points: &[Vector3]) -> TriangleFacet { let p1p2 = points[p2] - points[p1]; let p1p3 = points[p3] - points[p1]; - let normal = p1p2.cross(&p1p3).normalize(); + let normal = p1p2.cross(p1p3).normalize(); TriangleFacet { valid: true, @@ -38,7 +37,7 @@ impl TriangleFacet { } } - pub fn add_visible_point(&mut self, pid: usize, points: &[Point3]) { + pub fn add_visible_point(&mut self, pid: usize, points: &[Vector3]) { let distance = self.distance_to_point(pid, points); assert!(distance > crate::math::DEFAULT_EPSILON); @@ -50,8 +49,8 @@ impl TriangleFacet { self.visible_points.push(pid); } - pub fn distance_to_point(&self, point: usize, points: &[Point3]) -> Real { - self.normal.dot(&(points[point] - points[self.pts[0]])) + pub fn distance_to_point(&self, point: usize, points: &[Vector3]) -> Real { + self.normal.dot(points[point] - points[self.pts[0]]) } pub fn set_facets_adjascency( @@ -80,7 +79,7 @@ impl TriangleFacet { self.pts[(id + 1) % 3] } - pub fn can_see_point(&self, point: usize, points: &[Point3]) -> bool { + pub fn can_see_point(&self, point: usize, points: &[Vector3]) -> bool { // An affinely-dependent triangle cannot see any point. if self.affinely_dependent { return false; @@ -89,7 +88,7 @@ impl TriangleFacet { let p0 = points[self.pts[0]]; let pt = points[point]; - if (pt - p0).dot(&self.normal) < crate::math::DEFAULT_EPSILON * 100.0 { + if (pt - p0).dot(self.normal) < crate::math::DEFAULT_EPSILON * 100.0 { return false; } @@ -99,11 +98,7 @@ impl TriangleFacet { // Check that a given point can see this triangle, // making sure that the order of the three indices of // this triangle don't affect the test result. - pub fn order_independent_can_be_seen_by_point( - &self, - point: usize, - points: &[Point3], - ) -> bool { + pub fn order_independent_can_be_seen_by_point(&self, point: usize, points: &[Vector3]) -> bool { // An affinely-dependent triangle can be seen by any point. if self.affinely_dependent { return true; @@ -113,7 +108,7 @@ impl TriangleFacet { let p0 = points[self.pts[i]]; let pt = points[point]; - if (pt - p0).dot(&self.normal) >= 0.0 { + if (pt - p0).dot(self.normal) >= 0.0 { return true; } } diff --git a/src/transformation/convex_hull3/validation.rs b/src/transformation/convex_hull3/validation.rs index 43de58bf..ef16fe5d 100644 --- a/src/transformation/convex_hull3/validation.rs +++ b/src/transformation/convex_hull3/validation.rs @@ -2,9 +2,7 @@ use super::ConvexHullError; use super::TriangleFacet; #[cfg(feature = "std")] -use crate::math::Real; -#[cfg(feature = "std")] -use na::Point3; +use crate::math::Vector3; pub fn check_facet_links(ifacet: usize, facets: &[TriangleFacet]) { let facet = &facets[ifacet]; @@ -32,7 +30,7 @@ pub fn check_facet_links(ifacet: usize, facets: &[TriangleFacet]) { /// Checks if a convex-hull is properly formed. #[cfg(feature = "std")] pub fn check_convex_hull( - points: &[Point3], + points: &[Vector3], triangles: &[[u32; 3]], ) -> Result<(), ConvexHullError> { use crate::utils::hashmap::{Entry, HashMap}; @@ -98,10 +96,10 @@ pub fn check_convex_hull( Ok(()) } -// fn print_buildable_vec(desc: &str, elts: &[Point3]) { +// fn print_buildable_vec(desc: &str, elts: &[Vector3]) { // print!("let {} = vec![", desc); // for elt in elts { -// print!("Point3::new({},{},{}),", elt.x, elt.y, elt.z); +// print!("Vector::new({},{},{}),", elt.x, elt.y, elt.z); // } // println!("];") // } diff --git a/src/transformation/convex_hull_utils.rs b/src/transformation/convex_hull_utils.rs index 3c394282..87af3245 100644 --- a/src/transformation/convex_hull_utils.rs +++ b/src/transformation/convex_hull_utils.rs @@ -1,8 +1,5 @@ -use crate::math::Real; -use crate::num::Bounded; -use na; #[cfg(feature = "dim3")] -use {crate::bounding_volume, crate::math::Point}; +use crate::math::{Real, Vector}; /// Returns the index of the support point of a list of points. /// @@ -21,39 +18,36 @@ use {crate::bounding_volume, crate::math::Point}; /// # Example /// /// ```ignore -/// # // This is a pub(crate) function. Can’t really run it. +/// # // This is a pub(crate) function. Can't really run it. /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::convex_hull_utils::support_point_id; -/// use parry3d::na::{Point3, Vector3}; +/// use parry3d::na::{Vector3, Vector3}; /// /// let points = vec![ -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(0.0, 1.0, 0.0), -/// Point3::new(0.0, 0.0, 1.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ]; /// /// // Find point furthest in the positive X direction -/// let dir = Vector3::new(1.0, 0.0, 0.0); +/// let dir = Vector::new(1.0, 0.0, 0.0); /// let support_id = support_point_id(&dir, &points); -/// assert_eq!(support_id, Some(1)); // Point at (1, 0, 0) +/// assert_eq!(support_id, Some(1)); // Vector at (1, 0, 0) /// /// // Find point furthest in the positive Y direction -/// let dir = Vector3::new(0.0, 1.0, 0.0); +/// let dir = Vector::new(0.0, 1.0, 0.0); /// let support_id = support_point_id(&dir, &points); -/// assert_eq!(support_id, Some(2)); // Point at (0, 1, 0) +/// assert_eq!(support_id, Some(2)); // Vector at (0, 1, 0) /// # } /// ``` -pub fn support_point_id( - direction: &na::SVector, - points: &[na::Point], -) -> Option { +#[cfg(feature = "dim3")] +pub fn support_point_id(direction: Vector, points: &[Vector]) -> Option { let mut argmax = None; - let _max: Real = Bounded::max_value(); - let mut max = -_max; + let mut max = -Real::MAX; for (id, pt) in points.iter().enumerate() { - let dot = direction.dot(&pt.coords); + let dot = direction.dot(*pt); if dot > max { argmax = Some(id); @@ -82,32 +76,29 @@ pub fn support_point_id( /// # Example /// /// ```ignore -/// # // This is a pub(crate) function. Can’t really run it. +/// # // This is a pub(crate) function. Can't really run it. /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::convex_hull_utils::indexed_support_point_id; -/// use parry3d::na::{Point3, Vector3}; +/// use parry3d::na::{Vector3, Vector3}; /// /// let points = vec![ -/// Point3::origin(), -/// Point3::new(1.0, 0.0, 0.0), -/// Point3::new(2.0, 0.0, 0.0), -/// Point3::new(0.0, 1.0, 0.0), +/// Vector::ZERO, +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(2.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), /// ]; /// /// // Only consider points at indices 0, 1, and 3 (skip index 2) /// let subset = vec![0, 1, 3]; -/// let dir = Vector3::new(1.0, 0.0, 0.0); +/// let dir = Vector::new(1.0, 0.0, 0.0); /// /// let support_id = indexed_support_point_id(&dir, &points, subset.into_iter()); /// // Returns 1 (not 2, since we skipped that index) /// assert_eq!(support_id, Some(1)); /// # } /// ``` -pub fn indexed_support_point_id( - direction: &na::SVector, - points: &[na::Point], - idx: I, -) -> Option +#[cfg(feature = "dim3")] +pub fn indexed_support_point_id(direction: Vector, points: &[Vector], idx: I) -> Option where I: Iterator, { @@ -115,7 +106,7 @@ where let mut max = -Real::MAX; for i in idx.into_iter() { - let dot = direction.dot(&points[i].coords); + let dot = direction.dot(points[i]); if dot > max { argmax = Some(i); @@ -148,18 +139,18 @@ where /// # // This is a pub(crate) function. Can’t really run it. /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::convex_hull_utils::indexed_support_point_nth; -/// use parry3d::na::{Point3, Vector3}; +/// use parry3d::na::{Vector3, Vector3}; /// /// let points = vec![ -/// Point3::origin(), // index 0 -/// Point3::new(1.0, 0.0, 0.0), // index 1 -/// Point3::new(5.0, 0.0, 0.0), // index 2 -/// Point3::new(0.0, 1.0, 0.0), // index 3 +/// Vector::ZERO, // index 0 +/// Vector::new(1.0, 0.0, 0.0), // index 1 +/// Vector::new(5.0, 0.0, 0.0), // index 2 +/// Vector::new(0.0, 1.0, 0.0), // index 3 /// ]; /// /// // Consider points at indices [3, 0, 2, 1] /// let indices = vec![3, 0, 2, 1]; -/// let dir = Vector3::new(1.0, 0.0, 0.0); +/// let dir = Vector::new(1.0, 0.0, 0.0); /// /// let nth = indexed_support_point_nth(&dir, &points, indices.into_iter()); /// // The support point is at original index 2, which is position 2 in our iterator @@ -167,11 +158,7 @@ where /// # } /// ``` #[cfg(feature = "dim3")] // We only use this in 3D right now. -pub fn indexed_support_point_nth( - direction: &na::SVector, - points: &[na::Point], - idx: I, -) -> Option +pub fn indexed_support_point_nth(direction: Vector, points: &[Vector], idx: I) -> Option where I: Iterator, { @@ -179,7 +166,7 @@ where let mut max = -Real::MAX; for (k, i) in idx.into_iter().enumerate() { - let dot = direction.dot(&points[i].coords); + let dot = direction.dot(points[i]); if dot > max { argmax = Some(k); @@ -215,35 +202,35 @@ where /// # // This is a pub(crate) function. Can’t really run it. /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::convex_hull_utils::normalize; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// /// let mut points = vec![ -/// Point::new(10.0, 10.0, 10.0), -/// Point::new(20.0, 20.0, 20.0), -/// Point::new(15.0, 15.0, 15.0), +/// Vector::new(10.0, 10.0, 10.0), +/// Vector::new(20.0, 20.0, 20.0), +/// Vector::new(15.0, 15.0, 15.0), /// ]; /// /// let (center, scale) = normalize(&mut points); /// -/// // Points are now centered around origin and scaled +/// // Vectors are now centered around origin and scaled /// // The AABB diagonal is now approximately 1.0 /// println!("Original center: {:?}", center); /// println!("Original scale: {}", scale); /// /// // To recover original points: /// for p in &mut points { -/// *p = *p * scale + center.coords; +/// *p = *p * scale + center; /// } /// # } /// ``` #[cfg(feature = "dim3")] -pub fn normalize(coords: &mut [Point]) -> (Point, Real) { - let aabb = bounding_volume::details::local_point_cloud_aabb_ref(&*coords); - let diag = na::distance(&aabb.mins, &aabb.maxs); +pub fn normalize(coords: &mut [Vector]) -> (Vector, Real) { + let aabb = crate::bounding_volume::details::local_point_cloud_aabb_ref(&*coords); + let diag = aabb.mins.distance(aabb.maxs); let center = aabb.center(); for c in coords.iter_mut() { - *c = (*c + (-center.coords)) / diag; + *c = (*c - center) / diag; } (center, diag) diff --git a/src/transformation/ear_clipping.rs b/src/transformation/ear_clipping.rs index 52c8257a..acde3c6e 100644 --- a/src/transformation/ear_clipping.rs +++ b/src/transformation/ear_clipping.rs @@ -2,7 +2,7 @@ //! Based on , contributed by embotech AG. use crate::{ - math::{Point, Real}, + math::{Real, Vector}, utils::point_in_triangle::{corner_direction, is_point_in_triangle, Orientation}, }; use alloc::{vec, vec::Vec}; @@ -23,7 +23,7 @@ struct VertexInfo { } /// Updates the fields `pointiness` and `is_ear` for a given vertex index. -fn update_vertex(idx: usize, vertex_info: &mut VertexInfo, points: &[Point]) -> bool { +fn update_vertex(idx: usize, vertex_info: &mut VertexInfo, points: &[Vector]) -> bool { // Get the point and its neighbors. let p = points[idx]; let p1 = points[vertex_info.p_prev]; @@ -32,7 +32,7 @@ fn update_vertex(idx: usize, vertex_info: &mut VertexInfo, points: &[Point // Get the pointiness. let vec1 = (p1 - p).normalize(); let vec3 = (p3 - p).normalize(); - vertex_info.pointiness = vec1.dot(&vec3); + vertex_info.pointiness = vec1.dot(vec3); if vertex_info.pointiness.is_nan() { return false; } @@ -40,11 +40,11 @@ fn update_vertex(idx: usize, vertex_info: &mut VertexInfo, points: &[Point // A point is considered an ear when it is convex and no other points are // inside the triangle spanned by it and its two neighbors. let mut error = false; - vertex_info.is_ear = corner_direction(&p1, &p, &p3) == Orientation::Ccw + vertex_info.is_ear = corner_direction(p1, p, p3) == Orientation::Ccw && (0..points.len()) .filter(|&i| i != vertex_info.p_prev && i != idx && i != vertex_info.p_next) .all(|i| { - if let Some(is) = is_point_in_triangle(&points[i], &p1, &p, &p3) { + if let Some(is) = is_point_in_triangle(points[i], p1, p, p3) { !is } else { error = true; @@ -55,7 +55,7 @@ fn update_vertex(idx: usize, vertex_info: &mut VertexInfo, points: &[Point } /// Ear clipping triangulation algorithm. -pub(crate) fn triangulate_ear_clipping(vertices: &[Point]) -> Option> { +pub(crate) fn triangulate_ear_clipping(vertices: &[Vector]) -> Option> { let n_vertices = vertices.len(); // Create a new vector to hold the information about vertices. @@ -138,7 +138,11 @@ mod tests { #[test] fn triangle_ccw() { - let vertices = vec![Point::new(0., 0.), Point::new(1., 0.), Point::new(1., 1.)]; + let vertices = vec![ + Vector::new(0., 0.), + Vector::new(1., 0.), + Vector::new(1., 1.), + ]; let triangles = triangulate_ear_clipping(&vertices); assert_eq!(triangles.unwrap(), vec![[2, 0, 1]]); } @@ -146,10 +150,10 @@ mod tests { #[test] fn square_ccw() { let vertices = vec![ - Point::new(0., 0.), // 0 - Point::new(1., 0.), // 1 - Point::new(1., 1.), // 2 - Point::new(0., 1.), // 3 + Vector::new(0., 0.), // 0 + Vector::new(1., 0.), // 1 + Vector::new(1., 1.), // 2 + Vector::new(0., 1.), // 3 ]; let triangles = triangulate_ear_clipping(&vertices); assert_eq!(triangles.unwrap(), vec![[2, 3, 0], [2, 0, 1]]); @@ -158,10 +162,10 @@ mod tests { #[test] fn square_cw() { let vertices = vec![ - Point::new(0., 1.), // 0 - Point::new(1., 1.), // 1 - Point::new(1., 0.), // 2 - Point::new(0., 0.), // 3 + Vector::new(0., 1.), // 0 + Vector::new(1., 1.), // 1 + Vector::new(1., 0.), // 2 + Vector::new(0., 0.), // 3 ]; // This fails because we expect counter-clockwise ordering. let triangles = triangulate_ear_clipping(&vertices); @@ -171,11 +175,11 @@ mod tests { #[test] fn square_with_dent() { let vertices = vec![ - Point::new(0., 0.), // 0 - Point::new(1., 0.), // 1 - Point::new(0.5, 0.5), // 2 - Point::new(1., 1.), // 3 - Point::new(0., 1.), // 4 + Vector::new(0., 0.), // 0 + Vector::new(1., 0.), // 1 + Vector::new(0.5, 0.5), // 2 + Vector::new(1., 1.), // 3 + Vector::new(0., 1.), // 4 ]; let triangles = triangulate_ear_clipping(&vertices); assert_eq!(triangles.unwrap(), vec![[2, 3, 4], [2, 4, 0], [2, 0, 1],]); @@ -192,14 +196,14 @@ mod tests { /// 5-------6 1-------2 fn origin_outside_shape() { let vertices = vec![ - Point::new(2.0, 2.0), // 0 - Point::new(2.0, -2.0), // 1 - Point::new(4.0, -2.0), // 2 - Point::new(4.0, 4.0), // 3 - Point::new(-4.0, 4.0), // 4 - Point::new(-4.0, -2.0), // 5 - Point::new(-2.0, -2.0), // 6 - Point::new(-2.0, 2.0), // 7 + Vector::new(2.0, 2.0), // 0 + Vector::new(2.0, -2.0), // 1 + Vector::new(4.0, -2.0), // 2 + Vector::new(4.0, 4.0), // 3 + Vector::new(-4.0, 4.0), // 4 + Vector::new(-4.0, -2.0), // 5 + Vector::new(-2.0, -2.0), // 6 + Vector::new(-2.0, 2.0), // 7 ]; let triangles = triangulate_ear_clipping(&vertices).unwrap(); diff --git a/src/transformation/hertel_mehlhorn.rs b/src/transformation/hertel_mehlhorn.rs index a9d1ea27..70d0edf7 100644 --- a/src/transformation/hertel_mehlhorn.rs +++ b/src/transformation/hertel_mehlhorn.rs @@ -3,7 +3,7 @@ use alloc::vec::Vec; -use crate::math::{Point, Real}; +use crate::math::Vector; use crate::utils::point_in_triangle::{corner_direction, Orientation}; /// Checks if the counter-clockwise polygon `poly` has an edge going counter-clockwise from `p1` to `p2`. @@ -29,7 +29,7 @@ fn find_edge_index_in_polygon(p1: u32, p2: u32, indices: &[u32]) -> Option<(usiz /// partitioning. /// /// This algorithm is described in . -pub fn hertel_mehlhorn(vertices: &[Point], indices: &[[u32; 3]]) -> Vec>> { +pub fn hertel_mehlhorn(vertices: &[Vector], indices: &[[u32; 3]]) -> Vec> { hertel_mehlhorn_idx(vertices, indices) .into_iter() .map(|poly_indices| { @@ -42,7 +42,7 @@ pub fn hertel_mehlhorn(vertices: &[Point], indices: &[[u32; 3]]) -> Vec], indices: &[[u32; 3]]) -> Vec> { +pub fn hertel_mehlhorn_idx(vertices: &[Vector], indices: &[[u32; 3]]) -> Vec> { let mut indices: Vec> = indices.iter().map(|indices| indices.to_vec()).collect(); // Iterate over all polygons. let mut i_poly1 = 0; @@ -85,9 +85,9 @@ pub fn hertel_mehlhorn_idx(vertices: &[Point], indices: &[[u32; 3]]) -> Ve // First connection: let i13 = (polygon1.len() + i11 - 1) % polygon1.len(); let i23 = (i22 + 1) % polygon2.len(); - let p1 = &vertices[polygon2[i23] as usize]; - let p2 = &vertices[polygon1[i13] as usize]; - let p3 = &vertices[polygon1[i11] as usize]; + let p1 = vertices[polygon2[i23] as usize]; + let p2 = vertices[polygon1[i13] as usize]; + let p3 = vertices[polygon1[i11] as usize]; // Go to the next point if this section isn't convex. if corner_direction(p1, p2, p3) == Orientation::Cw { i11 += 1; @@ -96,9 +96,9 @@ pub fn hertel_mehlhorn_idx(vertices: &[Point], indices: &[[u32; 3]]) -> Ve // Second connection: let i13 = (i12 + 1) % polygon1.len(); let i23 = (polygon2.len() + i21 - 1) % polygon2.len(); - let p1 = &vertices[polygon1[i13] as usize]; - let p2 = &vertices[polygon2[i23] as usize]; - let p3 = &vertices[polygon1[i12] as usize]; + let p1 = vertices[polygon1[i13] as usize]; + let p2 = vertices[polygon2[i23] as usize]; + let p3 = vertices[polygon1[i12] as usize]; // Go to the next point if this section isn't convex. if corner_direction(p1, p2, p3) == Orientation::Cw { i11 += 1; @@ -129,7 +129,7 @@ pub fn hertel_mehlhorn_idx(vertices: &[Point], indices: &[[u32; 3]]) -> Ve #[cfg(test)] mod tests { use super::hertel_mehlhorn_idx; - use crate::math::Point; + use crate::math::Vector; #[test] fn origin_outside_shape() { @@ -142,14 +142,14 @@ mod tests { // | | ° | | // 5-------6 1-------2 let vertices = vec![ - Point::new(2.0, 2.0), // 0 - Point::new(2.0, -2.0), // 1 - Point::new(4.0, -2.0), // 2 - Point::new(4.0, 4.0), // 3 - Point::new(-4.0, 4.0), // 4 - Point::new(-4.0, -2.0), // 5 - Point::new(-2.0, -2.0), // 6 - Point::new(-2.0, 2.0), // 7 + Vector::new(2.0, 2.0), // 0 + Vector::new(2.0, -2.0), // 1 + Vector::new(4.0, -2.0), // 2 + Vector::new(4.0, 4.0), // 3 + Vector::new(-4.0, 4.0), // 4 + Vector::new(-4.0, -2.0), // 5 + Vector::new(-2.0, -2.0), // 6 + Vector::new(-2.0, 2.0), // 7 ]; let triangles = [ diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index 7b3cb123..2c9d1edc 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -1,6 +1,6 @@ use super::{MeshIntersectionError, TriangleTriangleIntersection}; use crate::bounding_volume::BoundingVolume; -use crate::math::{Isometry, Real}; +use crate::math::{Pose, Real, Vector3}; use crate::partitioning::BvhNode; use crate::query::point::point_query::PointQueryWithLocation; use crate::query::PointQuery; @@ -11,9 +11,6 @@ use crate::utils::hashmap::HashMap; use crate::utils::hashset::HashSet; use alloc::collections::BTreeMap; use alloc::{vec, vec::Vec}; -#[cfg(not(feature = "std"))] -use na::ComplexField; -use na::{Point3, Vector3}; use rstar::RTree; use spade::{ConstrainedDelaunayTriangulation, InsertionError, Triangulation as _}; #[cfg(feature = "wavefront")] @@ -71,10 +68,10 @@ impl Default for MeshIntersectionTolerances { /// The meshes must be oriented, have their half-edge topology computed, and must not be self-intersecting. /// The result mesh vertex coordinates are given in the local-space of `mesh1`. pub fn intersect_meshes( - pos1: &Isometry, + pos1: &Pose, mesh1: &TriMesh, flip1: bool, - pos2: &Isometry, + pos2: &Pose, mesh2: &TriMesh, flip2: bool, ) -> Result, MeshIntersectionError> { @@ -94,10 +91,10 @@ pub fn intersect_meshes( /// It allows to specify epsilons for how the algorithm will behave. /// See `MeshIntersectionTolerances` for details. pub fn intersect_meshes_with_tolerances( - pos1: &Isometry, + pos1: &Pose, mesh1: &TriMesh, flip1: bool, - pos2: &Isometry, + pos2: &Pose, mesh2: &TriMesh, flip2: bool, tolerances: MeshIntersectionTolerances, @@ -168,10 +165,10 @@ pub fn intersect_meshes_with_tolerances( // 4: Initialize a new mesh by inserting points into a set. Duplicate points should // hash to the same index. - let mut point_set = RTree::::new(); + let mut point_set = RTree::::new(); let mut topology_indices = HashMap::default(); { - let mut insert_point = |position: Point3| { + let mut insert_point = |position: Vector3| { insert_into_set( position, &mut point_set, @@ -280,7 +277,7 @@ pub fn intersect_meshes_with_tolerances( // 7: Sort the output points by insertion order. let mut vertices: Vec<_> = point_set.iter().copied().collect(); vertices.sort_by(|a, b| a.id.cmp(&b.id)); - let vertices: Vec<_> = vertices.iter().map(|p| Point3::from(p.point)).collect(); + let vertices: Vec<_> = vertices.iter().map(|p| Vector3::from(p.point)).collect(); if !topology_indices.is_empty() { Ok(Some(TriMesh::new( @@ -293,7 +290,7 @@ pub fn intersect_meshes_with_tolerances( } fn extract_connected_components( - pos12: &Isometry, + pos12: &Pose, mesh1: &TriMesh, mesh2: &TriMesh, flip2: bool, @@ -326,7 +323,7 @@ fn extract_connected_components( let tri1 = mesh1.triangle(twin.face); if flip2 - ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&tri1.center())) + ^ mesh2.contains_local_point(pos12.inverse_transform_point(tri1.center())) { to_visit.push(twin.face); } @@ -372,7 +369,7 @@ fn extract_connected_components( let repr_pt = mesh1.triangle(repr_face).center(); let indices = mesh1.indices(); - if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt)) { + if flip2 ^ mesh2.contains_local_point(pos12.inverse_transform_point(repr_pt)) { new_indices1.extend( cc.grouped_faces[range[0]..range[1]] .iter() @@ -385,7 +382,7 @@ fn extract_connected_components( // Deal with the case where there is no intersection between the meshes. let repr_pt = mesh1.triangle(0).center(); - if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt)) { + if flip2 ^ mesh2.contains_local_point(pos12.inverse_transform_point(repr_pt)) { new_indices1.extend_from_slice(mesh1.indices()); } } @@ -393,18 +390,18 @@ fn extract_connected_components( fn triangulate_constraints_and_merge_duplicates( tri: &Triangle, - constraints: &[[Point3; 2]], + constraints: &[[Vector3; 2]], epsilon: Real, ) -> Result< ( ConstrainedDelaunayTriangulation>, - Vec>, + Vec, ), InsertionError, > { let mut constraints = constraints.to_vec(); // Add the triangle points to the triangulation. - let mut point_set = RTree::::new(); + let mut point_set = RTree::::new(); let _ = insert_into_set(tri.a, &mut point_set, epsilon); let _ = insert_into_set(tri.b, &mut point_set, epsilon); let _ = insert_into_set(tri.c, &mut point_set, epsilon); @@ -421,14 +418,14 @@ fn triangulate_constraints_and_merge_duplicates( let q1 = tri_vtx[i]; let q2 = tri_vtx[(i + 1) % 3]; - let proj1 = project_point_to_segment(&p1, &[q1, q2]); - if (p1 - proj1).norm() < epsilon { - point_pair[0] = Point3::from(proj1); + let proj1 = project_point_to_segment(p1, &[q1, q2]); + if (p1 - proj1).length() < epsilon { + point_pair[0] = Vector3::from(proj1); } - let proj2 = project_point_to_segment(&p2, &[q1, q2]); - if (p2 - proj2).norm() < epsilon { - point_pair[1] = Point3::from(proj2); + let proj2 = project_point_to_segment(p2, &[q1, q2]); + if (p2 - proj2).length() < epsilon { + point_pair[1] = Vector3::from(proj2); } } } @@ -453,27 +450,27 @@ fn triangulate_constraints_and_merge_duplicates( let d2 = tri_points[best_source] - tri_points[(best_source + 1) % 3]; let (e1, e2) = planar_gram_schmidt(d1, d2); - let project = |p: &Point3| spade::Point2::new(e1.dot(&p.coords), e2.dot(&p.coords)); + let project = |p: Vector3| spade::Point2::new(e1.dot(p), e2.dot(p)); // Project points into 2D and triangulate the resulting set. let planar_points: Vec<_> = points .iter() .copied() .map(|point| { - let point_proj = project(&point.point); + let point_proj = project(point.point); utils::sanitize_spade_point(point_proj) }) .collect(); let cdt_triangulation = ConstrainedDelaunayTriangulation::bulk_load_cdt(planar_points, edges)?; debug_assert!(cdt_triangulation.vertices().len() == points.len()); - let points = points.into_iter().map(|p| Point3::from(p.point)).collect(); + let points = points.into_iter().map(|p| Vector3::from(p.point)).collect(); Ok((cdt_triangulation, points)) } // We heavily recommend that this is left here in case one needs to debug the above code. #[cfg(feature = "wavefront")] -fn _points_to_obj(mesh: &[Point3], path: &PathBuf) { +fn _points_to_obj(mesh: &[Vector3], path: &PathBuf) { use std::io::Write; let mut file = std::fs::File::create(path).unwrap(); @@ -484,7 +481,7 @@ fn _points_to_obj(mesh: &[Point3], path: &PathBuf) { // We heavily recommend that this is left here in case one needs to debug the above code. #[cfg(feature = "wavefront")] -fn _points_and_edges_to_obj(mesh: &[Point3], edges: &[[usize; 2]], path: &PathBuf) { +fn _points_and_edges_to_obj(mesh: &[Vector3], edges: &[[usize; 2]], path: &PathBuf) { use std::io::Write; let mut file = std::fs::File::create(path).unwrap(); @@ -498,18 +495,18 @@ fn _points_and_edges_to_obj(mesh: &[Point3], edges: &[[usize; 2]], path: & } #[derive(Copy, Clone, PartialEq, Debug, Default)] -struct TreePoint { - point: Point3, +struct TreeVector { + point: Vector3, id: usize, } -impl rstar::Point for TreePoint { +impl rstar::Point for TreeVector { type Scalar = Real; const DIMENSIONS: usize = 3; fn generate(mut generator: impl FnMut(usize) -> Self::Scalar) -> Self { - TreePoint { - point: Point3::new(generator(0), generator(1), generator(2)), + TreeVector { + point: Vector3::new(generator(0), generator(1), generator(2)), id: usize::MAX, } } @@ -523,20 +520,16 @@ impl rstar::Point for TreePoint { } } -fn insert_into_set( - position: Point3, - point_set: &mut RTree, - epsilon: Real, -) -> usize { +fn insert_into_set(position: Vector3, point_set: &mut RTree, epsilon: Real) -> usize { let point_count = point_set.size(); - let point_to_insert = TreePoint { + let point_to_insert = TreeVector { point: position, id: point_count, }; match point_set.nearest_neighbor(&point_to_insert) { Some(tree_point) => { - if (tree_point.point - position).norm_squared() <= epsilon { + if (tree_point.point - position).length_squared() <= epsilon { tree_point.id } else { point_set.insert(point_to_insert); @@ -552,7 +545,7 @@ fn insert_into_set( } } -fn smallest_angle(points: &[Point3]) -> Real { +fn smallest_angle(points: &[Vector3]) -> Real { let n = points.len(); let mut worst_cos: Real = -2.0; @@ -560,7 +553,7 @@ fn smallest_angle(points: &[Point3]) -> Real { let d1 = (points[i] - points[(i + 1) % n]).normalize(); let d2 = (points[(i + 2) % n] - points[(i + 1) % n]).normalize(); - let cos = d1.dot(&d2); + let cos = d1.dot(d2); if cos > worst_cos { worst_cos = cos; } @@ -569,9 +562,9 @@ fn smallest_angle(points: &[Point3]) -> Real { worst_cos.acos() } -fn planar_gram_schmidt(v1: Vector3, v2: Vector3) -> (Vector3, Vector3) { +fn planar_gram_schmidt(v1: Vector3, v2: Vector3) -> (Vector3, Vector3) { let u1 = v1; - let u2 = v2 - (v2.dot(&u1) / u1.norm_squared()) * u1; + let u2 = v2 - (v2.dot(u1) / u1.length_squared()) * u1; let e1 = u1.normalize(); let e2 = u2.normalize(); @@ -579,13 +572,13 @@ fn planar_gram_schmidt(v1: Vector3, v2: Vector3) -> (Vector3, (e1, e2) } -fn project_point_to_segment(point: &Point3, segment: &[Point3; 2]) -> Point3 { +fn project_point_to_segment(point: Vector3, segment: &[Vector3; 2]) -> Vector3 { let dir = segment[1] - segment[0]; let local = point - segment[0]; - let norm = dir.norm(); + let norm = dir.length(); // Restrict the result to the segment portion of the line. - let coeff = (dir.dot(&local) / norm).clamp(0., norm); + let coeff = (dir.dot(local) / norm).clamp(0., norm); segment[0] + coeff * dir.normalize() } @@ -594,7 +587,7 @@ fn project_point_to_segment(point: &Point3, segment: &[Point3; 2]) - /// to create ultra thin triangles when a point lies on an edge of a triangle. These /// are degenerate and need to be removed. fn is_triangle_degenerate( - triangle: &[Point3; 3], + triangle: &[Vector3; 3], epsilon_angle: Real, epsilon_distance: Real, ) -> bool { @@ -603,14 +596,14 @@ fn is_triangle_degenerate( } for i in 0..3 { - let mut dir = triangle[(i + 1) % 3] - triangle[(i + 2) % 3]; - if dir.normalize_mut() < epsilon_distance { + let (dir, dist) = (triangle[(i + 1) % 3] - triangle[(i + 2) % 3]).normalize_and_length(); + if dist < epsilon_distance { return true; } - let proj = triangle[(i + 2) % 3] + (triangle[i] - triangle[(i + 2) % 3]).dot(&dir) * dir; + let proj = triangle[(i + 2) % 3] + (triangle[i] - triangle[(i + 2) % 3]).dot(dir) * dir; - if (proj - triangle[i]).norm() < epsilon_distance { + if (proj - triangle[i]).length() < epsilon_distance { return true; } } @@ -621,13 +614,13 @@ fn is_triangle_degenerate( fn merge_triangle_sets( mesh1: &TriMesh, mesh2: &TriMesh, - triangle_constraints: &BTreeMap<&u32, Vec<[Point3; 2]>>, - pos1: &Isometry, - pos2: &Isometry, + triangle_constraints: &BTreeMap<&u32, Vec<[Vector3; 2]>>, + pos1: &Pose, + pos2: &Pose, flip1: bool, flip2: bool, metadata: &MeshIntersectionTolerances, - point_set: &mut RTree, + point_set: &mut RTree, topology_indices: &mut HashMap, ) -> Result<(), MeshIntersectionError> { // For each triangle, and each constraint edge associated to that triangle, @@ -670,10 +663,10 @@ fn merge_triangle_sets( let epsilon = metadata.global_insertion_epsilon; let projection = mesh2 - .project_local_point_and_get_location(&pos2.inverse_transform_point(¢er), true) + .project_local_point_and_get_location(pos2.inverse_transform_point(center), true) .0; - if flip2 ^ (projection.is_inside_eps(¢er, epsilon)) { + if flip2 ^ (projection.is_inside_eps(center, epsilon)) { let mut new_tri_idx = [ insert_into_set(p1, point_set, epsilon) as u32, insert_into_set(p2, point_set, epsilon) as u32, @@ -745,6 +738,7 @@ fn is_topologically_degenerate(tri_idx: [u32; 3]) -> bool { #[cfg(test)] mod tests { use super::*; + use crate::math::Vector; use crate::shape::{Ball, Cuboid, TriMeshFlags}; use obj::Obj; use obj::ObjData; @@ -761,7 +755,7 @@ mod tests { let mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -773,10 +767,10 @@ mod tests { .unwrap(); let _ = intersect_meshes( - &Isometry::identity(), + &Pose::identity(), &mesh, false, - &Isometry::identity(), + &Pose::identity(), &mesh, false, ) @@ -789,17 +783,17 @@ mod tests { #[test] fn test_non_origin_pos1_pos2_intersection() { let ball = Ball::new(2f32 as Real).to_trimesh(10, 10); - let cuboid = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)).to_trimesh(); + let cuboid = Cuboid::new(Vector::new(2.0, 1.0, 1.0)).to_trimesh(); let mut sphere_mesh = TriMesh::new(ball.0, ball.1).unwrap(); sphere_mesh.set_flags(TriMeshFlags::all()).unwrap(); let mut cuboid_mesh = TriMesh::new(cuboid.0, cuboid.1).unwrap(); cuboid_mesh.set_flags(TriMeshFlags::all()).unwrap(); let res = intersect_meshes( - &Isometry::translation(1.0, 0.0, 0.0), + &Pose::translation(1.0, 0.0, 0.0), &cuboid_mesh, false, - &Isometry::translation(2.0, 0.0, 0.0), + &Pose::translation(2.0, 0.0, 0.0), &sphere_mesh, false, ) @@ -809,7 +803,7 @@ mod tests { let _ = res.to_obj_file(&PathBuf::from("test_non_origin_pos1_pos2_intersection.obj")); let bounding_sphere = res.local_bounding_sphere(); - assert!(bounding_sphere.center == Point3::new(1.5, 0.0, 0.0)); + assert!(bounding_sphere.center == Vector::new(1.5, 0.0, 0.0)); assert_relative_eq!(2.0615528, bounding_sphere.radius, epsilon = 1.0e-5); } @@ -825,7 +819,7 @@ mod tests { let offset_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -846,7 +840,7 @@ mod tests { let center_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -858,10 +852,10 @@ mod tests { .unwrap(); let res = intersect_meshes( - &Isometry::identity(), + &Pose::identity(), ¢er_mesh, false, - &Isometry::identity(), + &Pose::identity(), &offset_mesh, false, ) @@ -883,7 +877,7 @@ mod tests { let stair_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -904,7 +898,7 @@ mod tests { let bar_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -916,10 +910,10 @@ mod tests { .unwrap(); let res = intersect_meshes( - &Isometry::identity(), + &Pose::identity(), &stair_mesh, false, - &Isometry::identity(), + &Pose::identity(), &bar_mesh, false, ) @@ -941,7 +935,7 @@ mod tests { let bunny_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -962,7 +956,7 @@ mod tests { let cylinder_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) + .map(|v| Vector::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -974,10 +968,10 @@ mod tests { .unwrap(); let res = intersect_meshes( - &Isometry::identity(), + &Pose::identity(), &bunny_mesh, false, - &Isometry::identity(), + &Pose::identity(), &cylinder_mesh, true, ) diff --git a/src/transformation/mesh_intersection/mesh_intersection_error.rs b/src/transformation/mesh_intersection/mesh_intersection_error.rs index 5e0cf1b3..623335b4 100644 --- a/src/transformation/mesh_intersection/mesh_intersection_error.rs +++ b/src/transformation/mesh_intersection/mesh_intersection_error.rs @@ -24,14 +24,13 @@ use crate::shape::{TriMesh, TriMeshFlags}; // /// # #[cfg(all(feature = "dim3", feature = "spade"))] { // /// use parry3d::shape::{TriMesh, TriMeshFlags}; // /// use parry3d::transformation::{intersect_meshes, MeshIntersectionError}; -// /// use nalgebra::{Point3, Isometry3}; // /// // /// // Create two meshes with proper flags // /// let vertices1 = vec![ -// /// Point3::new(-1.0, -1.0, -1.0), -// /// Point3::new(1.0, -1.0, -1.0), -// /// Point3::new(1.0, 1.0, -1.0), -// /// Point3::new(-1.0, 1.0, -1.0), +// /// Vector::new(-1.0, -1.0, -1.0), +// /// Vector::new(1.0, -1.0, -1.0), +// /// Vector::new(1.0, 1.0, -1.0), +// /// Vector::new(-1.0, 1.0, -1.0), // /// ]; // /// let indices1 = vec![[0, 1, 2], [0, 2, 3]]; // /// @@ -43,10 +42,10 @@ use crate::shape::{TriMesh, TriMeshFlags}; // /// ).expect("Failed to create mesh"); // /// // /// let vertices2 = vec![ -// /// Point3::new(0.0, -1.0, -1.0), -// /// Point3::new(2.0, -1.0, -1.0), -// /// Point3::new(2.0, 1.0, -1.0), -// /// Point3::new(0.0, 1.0, -1.0), +// /// Vector::new(0.0, -1.0, -1.0), +// /// Vector::new(2.0, -1.0, -1.0), +// /// Vector::new(2.0, 1.0, -1.0), +// /// Vector::new(0.0, 1.0, -1.0), // /// ]; // /// let indices2 = vec![[0, 1, 2], [0, 2, 3]]; // /// let mesh2 = TriMesh::with_flags( @@ -55,8 +54,8 @@ use crate::shape::{TriMesh, TriMeshFlags}; // /// TriMeshFlags::ORIENTED // /// ).expect("Failed to create mesh"); // /// -// /// let pos1 = Isometry3::identity(); -// /// let pos2 = Isometry3::identity(); +// /// let pos1 = Pose::identity(); +// /// let pos2 = Pose::identity(); // /// // /// match intersect_meshes(&pos1, &mesh1, false, &pos2, &mesh2, false) { // /// Ok(intersection_mesh) => { @@ -90,8 +89,8 @@ pub enum MeshIntersectionError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{TriMesh, TriMeshFlags}; - /// # use nalgebra::Point3; - /// # let vertices = vec![Point3::origin()]; + /// # use parry3d::math::Vector; + /// # let vertices = vec![Vector::ZERO]; /// # let indices = vec![[0, 0, 0]]; /// // Instead of: /// // let mesh = TriMesh::new(vertices, indices)?; @@ -112,8 +111,8 @@ pub enum MeshIntersectionError { /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// # use parry3d::shape::{TriMesh, TriMeshFlags}; - /// # use nalgebra::Point3; - /// # let vertices = vec![Point3::origin()]; + /// # use parry3d::math::Vector; + /// # let vertices = vec![Vector::ZERO]; /// # let indices = vec![[0, 0, 0]]; /// let mut mesh = TriMesh::new(vertices, indices).unwrap(); /// mesh.set_flags(TriMeshFlags::ORIENTED).unwrap(); diff --git a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs index 65ec91ad..f3247b92 100644 --- a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs +++ b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs @@ -1,5 +1,6 @@ use super::EPS; -use crate::math::{Point, Real, Vector}; +use crate::math::Vector2; +use crate::math::{Real, Vector}; use crate::query; use crate::query::PointQuery; use crate::shape::{FeatureId, Segment, Triangle}; @@ -8,20 +9,19 @@ use crate::transformation::polygon_intersection::{ }; use crate::utils::WBasis; use alloc::{vec, vec::Vec}; -use na::Point2; #[derive(Copy, Clone, Debug, Default)] -pub struct TriangleTriangleIntersectionPoint { - pub p1: Point, +pub struct TriangleTriangleIntersectionVector { + pub p1: Vector, } #[derive(Clone, Debug)] pub enum TriangleTriangleIntersection { Segment { - a: TriangleTriangleIntersectionPoint, - b: TriangleTriangleIntersectionPoint, + a: TriangleTriangleIntersectionVector, + b: TriangleTriangleIntersectionVector, }, - Polygon(Vec), + Polygon(Vec), } impl Default for TriangleTriangleIntersection { @@ -41,23 +41,23 @@ pub(crate) fn triangle_triangle_intersection( let normal1 = tri1.robust_normal(); let normal2 = tri2.robust_normal(); - if let Some(intersection_dir) = normal1.cross(&normal2).try_normalize(1.0e-6) { + if let Some(intersection_dir) = normal1.cross(normal2).try_normalize() { let mut range1 = [ - (Real::MAX, Point::origin(), FeatureId::Unknown), - (-Real::MAX, Point::origin(), FeatureId::Unknown), + (Real::MAX, Vector::ZERO, FeatureId::Unknown), + (-Real::MAX, Vector::ZERO, FeatureId::Unknown), ]; let mut range2 = [ - (Real::MAX, Point::origin(), FeatureId::Unknown), - (-Real::MAX, Point::origin(), FeatureId::Unknown), + (Real::MAX, Vector::ZERO, FeatureId::Unknown), + (-Real::MAX, Vector::ZERO, FeatureId::Unknown), ]; let hits1 = [ - segment_plane_intersection(&tri2.a, &normal2, &Segment::new(tri1.a, tri1.b), 0, (0, 1)) - .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)), - segment_plane_intersection(&tri2.a, &normal2, &Segment::new(tri1.b, tri1.c), 1, (1, 2)) - .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)), - segment_plane_intersection(&tri2.a, &normal2, &Segment::new(tri1.c, tri1.a), 2, (2, 0)) - .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)), + segment_plane_intersection(tri2.a, normal2, &Segment::new(tri1.a, tri1.b), 0, (0, 1)) + .map(|(p, feat)| (intersection_dir.dot(p), p, feat)), + segment_plane_intersection(tri2.a, normal2, &Segment::new(tri1.b, tri1.c), 1, (1, 2)) + .map(|(p, feat)| (intersection_dir.dot(p), p, feat)), + segment_plane_intersection(tri2.a, normal2, &Segment::new(tri1.c, tri1.a), 2, (2, 0)) + .map(|(p, feat)| (intersection_dir.dot(p), p, feat)), ]; for hit1 in hits1.into_iter().flatten() { @@ -75,12 +75,12 @@ pub(crate) fn triangle_triangle_intersection( } let hits2 = [ - segment_plane_intersection(&tri1.a, &normal1, &Segment::new(tri2.a, tri2.b), 0, (0, 1)) - .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)), - segment_plane_intersection(&tri1.a, &normal1, &Segment::new(tri2.b, tri2.c), 1, (1, 2)) - .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)), - segment_plane_intersection(&tri1.a, &normal1, &Segment::new(tri2.c, tri2.a), 2, (2, 0)) - .map(|(p, feat)| (intersection_dir.dot(&p.coords), p, feat)), + segment_plane_intersection(tri1.a, normal1, &Segment::new(tri2.a, tri2.b), 0, (0, 1)) + .map(|(p, feat)| (intersection_dir.dot(p), p, feat)), + segment_plane_intersection(tri1.a, normal1, &Segment::new(tri2.b, tri2.c), 1, (1, 2)) + .map(|(p, feat)| (intersection_dir.dot(p), p, feat)), + segment_plane_intersection(tri1.a, normal1, &Segment::new(tri2.c, tri2.a), 2, (2, 0)) + .map(|(p, feat)| (intersection_dir.dot(p), p, feat)), ]; for hit2 in hits2.into_iter().flatten() { @@ -103,23 +103,23 @@ pub(crate) fn triangle_triangle_intersection( } let a = if range2[0].0 > range1[0].0 + EPS { - TriangleTriangleIntersectionPoint { p1: range2[0].1 } + TriangleTriangleIntersectionVector { p1: range2[0].1 } } else { - TriangleTriangleIntersectionPoint { p1: range1[0].1 } + TriangleTriangleIntersectionVector { p1: range1[0].1 } }; let b = if range2[1].0 < range1[1].0 - EPS { - TriangleTriangleIntersectionPoint { p1: range2[1].1 } + TriangleTriangleIntersectionVector { p1: range2[1].1 } } else { - TriangleTriangleIntersectionPoint { p1: range1[1].1 } + TriangleTriangleIntersectionVector { p1: range1[1].1 } }; Some(TriangleTriangleIntersection::Segment { a, b }) } else { let unit_normal2 = normal2.normalize(); - if (tri1.a - tri2.a).dot(&unit_normal2) < EPS { + if (tri1.a - tri2.a).dot(unit_normal2) < EPS { let basis = unit_normal2.orthonormal_basis(); - let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); + let proj = |vect: Vector| Vector2::new(vect.dot(basis[0]), vect.dot(basis[1])); let mut intersections = vec![]; @@ -131,12 +131,12 @@ pub(crate) fn triangle_triangle_intersection( proj(tri1.c - tri2.a), ]; let poly2 = [ - proj(Vector::zeros()), // = proj(tri2.a - tri2.a) + proj(Vector::ZERO), // = proj(tri2.a - tri2.a) proj(tri2.b - tri2.a), proj(tri2.c - tri2.a), ]; - let convert_loc = |loc, pts: &[Point; 3]| match loc { + let convert_loc = |loc, pts: &[Vector; 3]| match loc { PolylinePointLocation::OnVertex(vid) => (FeatureId::Vertex(vid as u32), pts[vid]), PolylinePointLocation::OnEdge(vid1, vid2, bcoords) => ( match (vid1, vid2) { @@ -145,7 +145,7 @@ pub(crate) fn triangle_triangle_intersection( (2, 0) | (0, 2) => FeatureId::Edge(2), _ => unreachable!(), }, - pts[vid1] * bcoords[0] + pts[vid2].coords * bcoords[1], + pts[vid1] * bcoords[0] + pts[vid2] * bcoords[1], ), }; @@ -160,15 +160,15 @@ pub(crate) fn triangle_triangle_intersection( (Some(loc1), Some(loc2)) => { let (_f1, p1) = convert_loc(loc1, pts1); let (_f2, _p2) = convert_loc(loc2, pts2); - TriangleTriangleIntersectionPoint { p1 } + TriangleTriangleIntersectionVector { p1 } } (Some(loc1), None) => { let (_f1, p1) = convert_loc(loc1, pts1); - TriangleTriangleIntersectionPoint { p1 } + TriangleTriangleIntersectionVector { p1 } } (None, Some(loc2)) => { let (_f2, p2) = convert_loc(loc2, pts2); - TriangleTriangleIntersectionPoint { p1: p2 } + TriangleTriangleIntersectionVector { p1: p2 } } (None, None) => unreachable!(), }; @@ -194,17 +194,17 @@ pub(crate) fn triangle_triangle_intersection( } fn segment_plane_intersection( - plane_center: &Point, - plane_normal: &Vector, + plane_center: Vector, + plane_normal: Vector, segment: &Segment, eid: u32, vids: (u32, u32), -) -> Option<(Point, FeatureId)> { +) -> Option<(Vector, FeatureId)> { let dir = segment.b - segment.a; - let dir_norm = dir.norm(); + let dir_norm = dir.length(); let time_of_impact = - query::details::line_toi_with_halfspace(plane_center, plane_normal, &segment.a, &dir)?; + query::details::line_toi_with_halfspace(plane_center, plane_normal, segment.a, dir)?; let scaled_toi = time_of_impact * dir_norm; if scaled_toi < -EPS || scaled_toi > dir_norm + EPS { @@ -229,27 +229,27 @@ fn segment_plane_intersection( fn debug_check_intersections( tri1: &Triangle, tri2: &Triangle, - basis: &[na::Vector3; 2], - poly1: &[Point2], // Projection of tri1 on the basis `basis1` with the origin at tri2.a. - poly2: &[Point2], // Projection of tri2 on the basis `basis2` with the origin at tri2.a. - intersections: &[TriangleTriangleIntersectionPoint], + basis: &[Vector; 2], + poly1: &[Vector2], // Projection of tri1 on the basis `basis1` with the origin at tri2.a. + poly2: &[Vector2], // Projection of tri2 on the basis `basis2` with the origin at tri2.a. + intersections: &[TriangleTriangleIntersectionVector], ) { use std::{print, println}; - let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); + let proj = |vect: Vector| Vector2::new(vect.dot(basis[0]), vect.dot(basis[1])); let mut incorrect = false; for pt in intersections { if !tri1 - .project_local_point(&pt.p1, false) - .is_inside_eps(&pt.p1, 1.0e-5) + .project_local_point(pt.p1, false) + .is_inside_eps(pt.p1, 1.0e-5) { incorrect = true; break; } if !tri2 - .project_local_point(&pt.p1, false) - .is_inside_eps(&pt.p1, 1.0e-5) + .project_local_point(pt.p1, false) + .is_inside_eps(pt.p1, 1.0e-5) { incorrect = true; break; @@ -283,12 +283,12 @@ fn debug_check_intersections( println!("~~~~~~~ (copy/paste the following input in the `intersect_triangle_common_vertex` test)"); println!("(Triangle::new("); for pt1 in poly1 { - println!(" Point2::new({},{}),", pt1.x, pt1.y); + println!(" Vector::new({},{}),", pt1.x, pt1.y); } println!("),"); println!("Triangle::new("); for pt2 in poly2 { - println!(" Point2::new({},{}),", pt2.x, pt2.y); + println!(" Vector::new({},{}),", pt2.x, pt2.y); } println!("),),"); } diff --git a/src/transformation/polygon_intersection.rs b/src/transformation/polygon_intersection.rs index 9de3b3cf..e78c3319 100644 --- a/src/transformation/polygon_intersection.rs +++ b/src/transformation/polygon_intersection.rs @@ -1,6 +1,5 @@ +use crate::math::Vector2; use alloc::{vec, vec::Vec}; -use log::error; -use na::Point2; use ordered_float::OrderedFloat; use crate::math::Real; @@ -68,30 +67,30 @@ enum InFlag { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::PolylinePointLocation; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let polygon = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(2.0, 2.0), -/// Point2::new(0.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// /// // A point on vertex 0 /// let loc1 = PolylinePointLocation::OnVertex(0); /// let pt1 = loc1.to_point(&polygon); -/// assert_eq!(pt1, Point2::origin()); +/// assert_eq!(pt1, Vector::ZERO); /// /// // A point halfway along the edge from vertex 0 to vertex 1 /// let loc2 = PolylinePointLocation::OnEdge(0, 1, [0.5, 0.5]); /// let pt2 = loc2.to_point(&polygon); -/// assert_eq!(pt2, Point2::new(1.0, 0.0)); +/// assert_eq!(pt2, Vector::new(1.0, 0.0)); /// # } /// ``` #[derive(Copy, Clone, Debug, PartialEq)] pub enum PolylinePointLocation { - /// Point on a vertex. + /// Vector on a vertex. OnVertex(usize), - /// Point on an edge. + /// Vector on an edge. OnEdge(usize, usize, [Real; 2]), } @@ -128,23 +127,23 @@ impl PolylinePointLocation { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::PolylinePointLocation; - /// # use parry2d::na::Point2; + /// # use parry2d::math::Vector; /// let polygon = vec![ - /// Point2::origin(), - /// Point2::new(4.0, 0.0), - /// Point2::new(4.0, 4.0), + /// Vector::ZERO, + /// Vector::new(4.0, 0.0), + /// Vector::new(4.0, 4.0), /// ]; /// /// let loc = PolylinePointLocation::OnEdge(0, 1, [0.75, 0.25]); /// let point = loc.to_point(&polygon); - /// assert_eq!(point, Point2::new(1.0, 0.0)); // 75% of vertex 0 + 25% of vertex 1 + /// assert_eq!(point, Vector::new(1.0, 0.0)); // 75% of vertex 0 + 25% of vertex 1 /// # } /// ``` - pub fn to_point(self, pts: &[Point2]) -> Point2 { + pub fn to_point(self, pts: &[Vector2]) -> Vector2 { match self { PolylinePointLocation::OnVertex(i) => pts[i], PolylinePointLocation::OnEdge(i1, i2, bcoords) => { - pts[i1] * bcoords[0] + pts[i2].coords * bcoords[1] + pts[i1] * bcoords[0] + pts[i2] * bcoords[1] } } } @@ -183,20 +182,20 @@ impl PolylinePointLocation { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::convex_polygons_intersection_points; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// // Define two overlapping squares /// let square1 = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(2.0, 2.0), -/// Point2::new(0.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// /// let square2 = vec![ -/// Point2::new(1.0, 1.0), -/// Point2::new(3.0, 1.0), -/// Point2::new(3.0, 3.0), -/// Point2::new(1.0, 3.0), +/// Vector::new(1.0, 1.0), +/// Vector::new(3.0, 1.0), +/// Vector::new(3.0, 3.0), +/// Vector::new(1.0, 3.0), /// ]; /// /// let mut intersection = Vec::new(); @@ -212,9 +211,9 @@ impl PolylinePointLocation { /// * [`convex_polygons_intersection`] - For closure-based output /// * [`polygons_intersection_points`] - For non-convex polygons pub fn convex_polygons_intersection_points( - poly1: &[Point2], - poly2: &[Point2], - out: &mut Vec>, + poly1: &[Vector2], + poly2: &[Vector2], + out: &mut Vec, ) { convex_polygons_intersection_points_with_tolerances(poly1, poly2, Default::default(), out); } @@ -236,17 +235,17 @@ pub fn convex_polygons_intersection_points( /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::{convex_polygons_intersection_points_with_tolerances, PolygonIntersectionTolerances}; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let triangle1 = vec![ -/// Point2::origin(), -/// Point2::new(4.0, 0.0), -/// Point2::new(2.0, 3.0), +/// Vector::ZERO, +/// Vector::new(4.0, 0.0), +/// Vector::new(2.0, 3.0), /// ]; /// /// let triangle2 = vec![ -/// Point2::new(1.0, 0.5), -/// Point2::new(3.0, 0.5), -/// Point2::new(2.0, 2.5), +/// Vector::new(1.0, 0.5), +/// Vector::new(3.0, 0.5), +/// Vector::new(2.0, 2.5), /// ]; /// /// let mut intersection = Vec::new(); @@ -266,10 +265,10 @@ pub fn convex_polygons_intersection_points( /// # } /// ``` pub fn convex_polygons_intersection_points_with_tolerances( - poly1: &[Point2], - poly2: &[Point2], + poly1: &[Vector2], + poly2: &[Vector2], tolerances: PolygonIntersectionTolerances, - out: &mut Vec>, + out: &mut Vec, ) { convex_polygons_intersection_with_tolerances(poly1, poly2, tolerances, |loc1, loc2| { if let Some(loc1) = loc1 { @@ -307,19 +306,19 @@ pub fn convex_polygons_intersection_points_with_tolerances( /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::convex_polygons_intersection; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let square = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(2.0, 2.0), -/// Point2::new(0.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// /// let diamond = vec![ -/// Point2::new(1.0, -0.5), -/// Point2::new(2.5, 1.0), -/// Point2::new(1.0, 2.5), -/// Point2::new(-0.5, 1.0), +/// Vector::new(1.0, -0.5), +/// Vector::new(2.5, 1.0), +/// Vector::new(1.0, 2.5), +/// Vector::new(-0.5, 1.0), /// ]; /// /// let mut intersection_points = Vec::new(); @@ -341,8 +340,8 @@ pub fn convex_polygons_intersection_points_with_tolerances( /// * [`convex_polygons_intersection_points`] - Simpler vector-based output /// * [`convex_polygons_intersection_with_tolerances`] - With custom tolerances pub fn convex_polygons_intersection( - poly1: &[Point2], - poly2: &[Point2], + poly1: &[Vector2], + poly2: &[Vector2], out: impl FnMut(Option, Option), ) { convex_polygons_intersection_with_tolerances(poly1, poly2, Default::default(), out) @@ -374,21 +373,21 @@ pub fn convex_polygons_intersection( /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::{convex_polygons_intersection_with_tolerances, PolygonIntersectionTolerances}; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let hexagon = vec![ -/// Point2::new(2.0, 0.0), -/// Point2::new(1.0, 1.732), -/// Point2::new(-1.0, 1.732), -/// Point2::new(-2.0, 0.0), -/// Point2::new(-1.0, -1.732), -/// Point2::new(1.0, -1.732), +/// Vector::new(2.0, 0.0), +/// Vector::new(1.0, 1.732), +/// Vector::new(-1.0, 1.732), +/// Vector::new(-2.0, 0.0), +/// Vector::new(-1.0, -1.732), +/// Vector::new(1.0, -1.732), /// ]; /// /// let square = vec![ -/// Point2::new(-1.0, -1.0), -/// Point2::new(1.0, -1.0), -/// Point2::new(1.0, 1.0), -/// Point2::new(-1.0, 1.0), +/// Vector::new(-1.0, -1.0), +/// Vector::new(1.0, -1.0), +/// Vector::new(1.0, 1.0), +/// Vector::new(-1.0, 1.0), /// ]; /// /// let tolerances = PolygonIntersectionTolerances::default(); @@ -412,8 +411,8 @@ pub fn convex_polygons_intersection( /// # } /// ``` pub fn convex_polygons_intersection_with_tolerances( - poly1: &[Point2], - poly2: &[Point2], + poly1: &[Vector2], + poly2: &[Vector2], tolerances: PolygonIntersectionTolerances, mut out: impl FnMut(Option, Option), ) { @@ -421,16 +420,16 @@ pub fn convex_polygons_intersection_with_tolerances( // first triangle of the polygon is degenerate. let rev1 = poly1.len() > 2 && Triangle::orientation2d( - &poly1[0], - &poly1[1], - &poly1[2], + poly1[0], + poly1[1], + poly1[2], tolerances.collinearity_epsilon, ) == TriangleOrientation::Clockwise; let rev2 = poly2.len() > 2 && Triangle::orientation2d( - &poly2[0], - &poly2[1], - &poly2[2], + poly2[0], + poly2[1], + poly2[2], tolerances.collinearity_epsilon, ) == TriangleOrientation::Clockwise; @@ -449,14 +448,14 @@ pub fn convex_polygons_intersection_with_tolerances( let (a1, b1) = if rev1 { ((len1 - i1) % len1, len1 - i1 - 1) } else { - // Point before `i1`, and point at `i1`. + // Vector before `i1`, and point at `i1`. ((i1 + len1 - 1) % len1, i1) }; let (a2, b2) = if rev2 { ((len2 - i2) % len2, len2 - i2 - 1) } else { - // Point before `i2`, and point at `i2`. + // Vector before `i2`, and point at `i2`. ((i2 + len2 - 1) % len2, i2) }; @@ -467,32 +466,32 @@ pub fn convex_polygons_intersection_with_tolerances( // Left -> Right (CounterClockwise) or Right -> Left (Clockwise) relative to the edge from // poly1. let cross = Triangle::orientation2d( - &Point2::origin(), - &Point2::from(dir_edge1), - &Point2::from(dir_edge2), + Vector2::ZERO, + Vector2::from(dir_edge1), + Vector2::from(dir_edge2), tolerances.collinearity_epsilon, ); // Determines if b1 is left (CounterClockwise) or right (Clockwise) of [a2, b2]. let a2_b2_b1 = Triangle::orientation2d( - &poly2[a2], - &poly2[b2], - &poly1[b1], + poly2[a2], + poly2[b2], + poly1[b1], tolerances.collinearity_epsilon, ); // Determines if b2 is left (CounterClockwise) or right (Clockwise) of [a1, b1]. let a1_b1_b2 = Triangle::orientation2d( - &poly1[a1], - &poly1[b1], - &poly2[b2], + poly1[a1], + poly1[b1], + poly2[b2], tolerances.collinearity_epsilon, ); // If edge1 & edge2 intersect, update inflag. if let Some(inter) = utils::segments_intersection2d( - &poly1[a1], - &poly1[b1], - &poly2[a2], - &poly2[b2], + poly1[a1], + poly1[b1], + poly2[a2], + poly2[b2], tolerances.collinearity_epsilon, ) { match inter { @@ -527,7 +526,7 @@ pub fn convex_polygons_intersection_with_tolerances( second_loc1, second_loc2, } => { - if dir_edge1.dot(&dir_edge2) < 0.0 { + if dir_edge1.dot(dir_edge2) < 0.0 { // Special case: edge1 & edge2 overlap and oppositely oriented. The // intersection is degenerate (equals to a segment). Output // the segment and exit. @@ -607,9 +606,9 @@ pub fn convex_polygons_intersection_with_tolerances( for p2 in poly2 { let a_minus_1 = (a + len1 - 1) % len1; // a - 1 let new_orient = Triangle::orientation2d( - &poly1[a_minus_1], - &poly1[a], - p2, + poly1[a_minus_1], + poly1[a], + *p2, tolerances.collinearity_epsilon, ); @@ -639,9 +638,9 @@ pub fn convex_polygons_intersection_with_tolerances( for p1 in poly1 { let b_minus_1 = (b + len2 - 1) % len2; // = b - 1 let new_orient = Triangle::orientation2d( - &poly2[b_minus_1], - &poly2[b], - p1, + poly2[b_minus_1], + poly2[b], + *p1, tolerances.collinearity_epsilon, ); @@ -700,7 +699,7 @@ pub enum PolygonsIntersectionError { /// # Important Notes /// /// - **Non-convex support**: This function works with concave polygons -/// - **Multiple components**: Returns a `Vec>>` because the intersection +/// - **Multiple components**: Returns a `Vec>` because the intersection /// of two concave polygons can produce multiple separate regions /// - **No self-intersection**: Input polygons must not self-intersect /// - **Counter-clockwise winding**: Both polygons must be oriented counter-clockwise @@ -714,7 +713,7 @@ pub enum PolygonsIntersectionError { /// /// # Returns /// -/// * `Ok(Vec>>)` - A vector of intersection polygons (usually just one) +/// * `Ok(Vec>)` - A vector of intersection polygons (usually just one) /// * `Err(PolygonsIntersectionError::InfiniteLoop)` - If the polygons are ill-formed /// /// # Examples @@ -724,23 +723,23 @@ pub enum PolygonsIntersectionError { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::polygons_intersection_points; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// // L-shaped polygon /// let l_shape = vec![ -/// Point2::origin(), -/// Point2::new(3.0, 0.0), -/// Point2::new(3.0, 1.0), -/// Point2::new(1.0, 1.0), -/// Point2::new(1.0, 3.0), -/// Point2::new(0.0, 3.0), +/// Vector::ZERO, +/// Vector::new(3.0, 0.0), +/// Vector::new(3.0, 1.0), +/// Vector::new(1.0, 1.0), +/// Vector::new(1.0, 3.0), +/// Vector::new(0.0, 3.0), /// ]; /// /// // Square overlapping the L-shape /// let square = vec![ -/// Point2::new(0.5, 0.5), -/// Point2::new(2.5, 0.5), -/// Point2::new(2.5, 2.5), -/// Point2::new(0.5, 2.5), +/// Vector::new(0.5, 0.5), +/// Vector::new(2.5, 0.5), +/// Vector::new(2.5, 2.5), +/// Vector::new(0.5, 2.5), /// ]; /// /// let result = polygons_intersection_points(&l_shape, &square).unwrap(); @@ -755,18 +754,18 @@ pub enum PolygonsIntersectionError { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::polygons_intersection_points; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let triangle = vec![ -/// Point2::origin(), -/// Point2::new(4.0, 0.0), -/// Point2::new(2.0, 3.0), +/// Vector::ZERO, +/// Vector::new(4.0, 0.0), +/// Vector::new(2.0, 3.0), /// ]; /// /// let square = vec![ -/// Point2::new(1.0, 0.5), -/// Point2::new(3.0, 0.5), -/// Point2::new(3.0, 2.0), -/// Point2::new(1.0, 2.0), +/// Vector::new(1.0, 0.5), +/// Vector::new(3.0, 0.5), +/// Vector::new(3.0, 2.0), +/// Vector::new(1.0, 2.0), /// ]; /// /// let result = polygons_intersection_points(&triangle, &square).unwrap(); @@ -786,9 +785,9 @@ pub enum PolygonsIntersectionError { /// * [`convex_polygons_intersection_points`] - Faster version for convex polygons only /// * [`polygons_intersection`] - Closure-based version with more control pub fn polygons_intersection_points( - poly1: &[Point2], - poly2: &[Point2], -) -> Result>>, PolygonsIntersectionError> { + poly1: &[Vector2], + poly2: &[Vector2], +) -> Result>, PolygonsIntersectionError> { let mut result = vec![]; let mut curr_poly = vec![]; polygons_intersection(poly1, poly2, |loc1, loc2| { @@ -838,20 +837,20 @@ pub fn polygons_intersection_points( /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::polygons_intersection; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let pentagon = vec![ -/// Point2::new(1.0, 0.0), -/// Point2::new(0.309, 0.951), -/// Point2::new(-0.809, 0.588), -/// Point2::new(-0.809, -0.588), -/// Point2::new(0.309, -0.951), +/// Vector::new(1.0, 0.0), +/// Vector::new(0.309, 0.951), +/// Vector::new(-0.809, 0.588), +/// Vector::new(-0.809, -0.588), +/// Vector::new(0.309, -0.951), /// ]; /// /// let square = vec![ -/// Point2::new(-0.5, -0.5), -/// Point2::new(0.5, -0.5), -/// Point2::new(0.5, 0.5), -/// Point2::new(-0.5, 0.5), +/// Vector::new(-0.5, -0.5), +/// Vector::new(0.5, -0.5), +/// Vector::new(0.5, 0.5), +/// Vector::new(-0.5, 0.5), /// ]; /// /// let mut components = Vec::new(); @@ -880,25 +879,25 @@ pub fn polygons_intersection_points( /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// # use parry2d::transformation::polygons_intersection; -/// # use parry2d::na::Point2; +/// # use parry2d::math::Vector; /// let hexagon = vec![ -/// Point2::new(2.0, 0.0), -/// Point2::new(1.0, 1.732), -/// Point2::new(-1.0, 1.732), -/// Point2::new(-2.0, 0.0), -/// Point2::new(-1.0, -1.732), -/// Point2::new(1.0, -1.732), +/// Vector::new(2.0, 0.0), +/// Vector::new(1.0, 1.732), +/// Vector::new(-1.0, 1.732), +/// Vector::new(-2.0, 0.0), +/// Vector::new(-1.0, -1.732), +/// Vector::new(1.0, -1.732), /// ]; /// /// let circle_approx = vec![ -/// Point2::new(1.0, 0.0), -/// Point2::new(0.707, 0.707), -/// Point2::new(0.0, 1.0), -/// Point2::new(-0.707, 0.707), -/// Point2::new(-1.0, 0.0), -/// Point2::new(-0.707, -0.707), -/// Point2::new(0.0, -1.0), -/// Point2::new(0.707, -0.707), +/// Vector::new(1.0, 0.0), +/// Vector::new(0.707, 0.707), +/// Vector::new(0.0, 1.0), +/// Vector::new(-0.707, 0.707), +/// Vector::new(-1.0, 0.0), +/// Vector::new(-0.707, -0.707), +/// Vector::new(0.0, -1.0), +/// Vector::new(0.707, -0.707), /// ]; /// /// let mut vertex_count = 0; @@ -921,8 +920,8 @@ pub fn polygons_intersection_points( /// * [`polygons_intersection_points`] - Simpler vector-based output /// * [`convex_polygons_intersection`] - Faster version for convex polygons pub fn polygons_intersection( - poly1: &[Point2], - poly2: &[Point2], + poly1: &[Vector2], + poly2: &[Vector2], mut out: impl FnMut(Option, Option), ) -> Result<(), PolygonsIntersectionError> { let tolerances = PolygonIntersectionTolerances::default(); @@ -936,7 +935,7 @@ pub fn polygons_intersection( let (intersections, num_intersections) = compute_sorted_edge_intersections(poly1, poly2, tolerances.collinearity_epsilon); let mut visited = vec![false; num_intersections]; - let segment = |eid: EdgeId, poly: &[Point2]| [poly[eid], poly[(eid + 1) % poly.len()]]; + let segment = |eid: EdgeId, poly: &[Vector2]| [poly[eid], poly[(eid + 1) % poly.len()]]; // Traverse all the intersections. for inters in intersections[0].values() { @@ -950,16 +949,11 @@ pub fn polygons_intersection( let [a1, b1] = segment(inter.edges[0], poly1); let [a2, b2] = segment(inter.edges[1], poly2); let poly_to_traverse = - match Triangle::orientation2d(&a1, &b1, &a2, tolerances.collinearity_epsilon) { + match Triangle::orientation2d(a1, b1, a2, tolerances.collinearity_epsilon) { TriangleOrientation::Clockwise => 1, TriangleOrientation::CounterClockwise => 0, TriangleOrientation::Degenerate => { - match Triangle::orientation2d( - &a1, - &b1, - &b2, - tolerances.collinearity_epsilon, - ) { + match Triangle::orientation2d(a1, b1, b2, tolerances.collinearity_epsilon) { TriangleOrientation::Clockwise => 0, TriangleOrientation::CounterClockwise => 1, TriangleOrientation::Degenerate => { @@ -1055,12 +1049,12 @@ pub fn polygons_intersection( // If there are no intersection, check if one polygon is inside the other. if intersections[0].is_empty() { - if utils::point_in_poly2d(&poly1[0], poly2) { + if utils::point_in_poly2d(poly1[0], poly2) { for pt_id in 0..poly1.len() { out(Some(PolylinePointLocation::OnVertex(pt_id)), None) } out(None, None); - } else if utils::point_in_poly2d(&poly2[0], poly1) { + } else if utils::point_in_poly2d(poly2[0], poly1) { for pt_id in 0..poly2.len() { out(None, Some(PolylinePointLocation::OnVertex(pt_id))) } @@ -1075,19 +1069,19 @@ type EdgeId = usize; type IntersectionId = usize; #[derive(Copy, Clone, Debug)] -struct IntersectionPoint { +struct IntersectionVector { id: IntersectionId, edges: [EdgeId; 2], locs: [PolylinePointLocation; 2], } fn compute_sorted_edge_intersections( - poly1: &[Point2], - poly2: &[Point2], + poly1: &[Vector2], + poly2: &[Vector2], eps: Real, -) -> ([HashMap>; 2], usize) { - let mut inter1: HashMap> = HashMap::default(); - let mut inter2: HashMap> = HashMap::default(); +) -> ([HashMap>; 2], usize) { + let mut inter1: HashMap> = HashMap::default(); + let mut inter2: HashMap> = HashMap::default(); let mut id = 0; // Find the intersections. @@ -1099,7 +1093,7 @@ fn compute_sorted_edge_intersections( let j2 = (i2 + 1) % poly2.len(); let Some(inter) = - utils::segments_intersection2d(&poly1[i1], &poly1[j1], &poly2[i2], &poly2[j2], eps) + utils::segments_intersection2d(poly1[i1], poly1[j1], poly2[i2], poly2[j2], eps) else { continue; }; @@ -1108,7 +1102,7 @@ fn compute_sorted_edge_intersections( SegmentsIntersection::Point { loc1, loc2 } => { let loc1 = PolylinePointLocation::from_segment_point_location(i1, j1, loc1); let loc2 = PolylinePointLocation::from_segment_point_location(i2, j2, loc2); - let intersection = IntersectionPoint { + let intersection = IntersectionVector { id, edges: [i1, i2], locs: [loc1, loc2], @@ -1147,12 +1141,12 @@ fn compute_sorted_edge_intersections( #[cfg(all(test, feature = "dim2"))] mod test { + use crate::math::Vector; use crate::query::PointQuery; use crate::shape::Triangle; use crate::transformation::convex_polygons_intersection_points_with_tolerances; use crate::transformation::polygon_intersection::PolygonIntersectionTolerances; use alloc::vec::Vec; - use na::Point2; use std::println; #[test] @@ -1160,50 +1154,50 @@ mod test { let tris = [ ( Triangle::new( - Point2::new(-0.0008759537858568062, -2.0103871966663305), - Point2::new(0.3903908709629763, -1.3421764825890266), - Point2::new(1.3380817875388151, -2.0098007857739013), + Vector::new(-0.0008759537858568062, -2.0103871966663305), + Vector::new(0.3903908709629763, -1.3421764825890266), + Vector::new(1.3380817875388151, -2.0098007857739013), ), Triangle::new( - Point2::new(0.0, -0.0), - Point2::new(-0.0008759537858568062, -2.0103871966663305), - Point2::new(1.9991979155226394, -2.009511242880474), + Vector::new(0.0, -0.0), + Vector::new(-0.0008759537858568062, -2.0103871966663305), + Vector::new(1.9991979155226394, -2.009511242880474), ), ), ( Triangle::new( - Point2::new(0.7319315811016305, -0.00004046981523721891), - Point2::new(2.0004914907008944, -0.00011061077714557787), - Point2::new(1.1848406021956144, -0.8155712451545468), + Vector::new(0.7319315811016305, -0.00004046981523721891), + Vector::new(2.0004914907008944, -0.00011061077714557787), + Vector::new(1.1848406021956144, -0.8155712451545468), ), Triangle::new( - Point2::origin(), - Point2::new(0.00011061077714557787, -2.000024893134292), - Point2::new(2.0004914907008944, -0.00011061077714557787), + Vector::ZERO, + Vector::new(0.00011061077714557787, -2.000024893134292), + Vector::new(2.0004914907008944, -0.00011061077714557787), ), ), ( Triangle::new( - Point2::new(-0.000049995168258705205, -0.9898801451981707), - Point2::new(0.0, -0.0), - Point2::new(0.583013294019752, -1.4170136900568633), + Vector::new(-0.000049995168258705205, -0.9898801451981707), + Vector::new(0.0, -0.0), + Vector::new(0.583013294019752, -1.4170136900568633), ), Triangle::new( - Point2::new(0.0, -0.0), - Point2::new(-0.00010101395240669591, -2.000027389553894), - Point2::new(2.000372544168497, 0.00010101395240669591), + Vector::new(0.0, -0.0), + Vector::new(-0.00010101395240669591, -2.000027389553894), + Vector::new(2.000372544168497, 0.00010101395240669591), ), ), ( Triangle::new( - Point2::new(-0.940565646581769, -0.939804943675256), - Point2::new(0.0, -0.0), - Point2::new(-0.001533592366792066, -0.9283586484736431), + Vector::new(-0.940565646581769, -0.939804943675256), + Vector::new(0.0, -0.0), + Vector::new(-0.001533592366792066, -0.9283586484736431), ), Triangle::new( - Point2::new(0.0, -0.0), - Point2::new(-2.00752629829582, -2.0059026672784825), - Point2::new(-0.0033081650580626698, -2.0025945022204197), + Vector::new(0.0, -0.0), + Vector::new(-2.00752629829582, -2.0059026672784825), + Vector::new(-0.0033081650580626698, -2.0025945022204197), ), ), ]; @@ -1225,20 +1219,20 @@ mod test { println!( "tri1 is in tri2: {}", tri1.vertices().iter().all(|pt| tri2 - .project_local_point(pt, false) - .is_inside_eps(pt, 1.0e-5)) + .project_local_point(*pt, false) + .is_inside_eps(*pt, 1.0e-5)) ); println!( "tri2 is in tri1: {}", tri2.vertices().iter().all(|pt| tri1 - .project_local_point(pt, false) - .is_inside_eps(pt, 1.0e-5)) + .project_local_point(*pt, false) + .is_inside_eps(*pt, 1.0e-5)) ); for pt in &inter { - let proj1 = tri1.project_local_point(&pt, false); - let proj2 = tri2.project_local_point(&pt, false); - assert!(proj1.is_inside_eps(&pt, 1.0e-5)); - assert!(proj2.is_inside_eps(&pt, 1.0e-5)); + let proj1 = tri1.project_local_point(*pt, false); + let proj2 = tri2.project_local_point(*pt, false); + assert!(proj1.is_inside_eps(*pt, 1.0e-5)); + assert!(proj2.is_inside_eps(*pt, 1.0e-5)); } } } diff --git a/src/transformation/to_outline/ball_to_outline.rs b/src/transformation/to_outline/ball_to_outline.rs index 2bdbabb6..d84bcef5 100644 --- a/src/transformation/to_outline/ball_to_outline.rs +++ b/src/transformation/to_outline/ball_to_outline.rs @@ -1,19 +1,18 @@ -use crate::math::{Isometry, Point, Real, Vector}; +use crate::math::{Pose, Real, RealField, Vector}; use crate::shape::Ball; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point3, RealField}; impl Ball { /// Outlines this ball’s shape using polylines. - pub fn to_outline(&self, nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let diameter = self.radius * 2.0; let (vtx, idx) = unit_sphere_outline(nsubdiv); - (utils::scaled(vtx, Vector::repeat(diameter)), idx) + (utils::scaled(vtx, Vector::splat(diameter)), idx) } } -fn unit_sphere_outline(nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { +fn unit_sphere_outline(nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let two_pi = Real::two_pi(); let dtheta = two_pi / (nsubdiv as Real); let mut coords = Vec::new(); @@ -26,11 +25,11 @@ fn unit_sphere_outline(nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { let n = nsubdiv as usize; utils::transform( &mut coords[n..n * 2], - Isometry::rotation(Vector::x() * Real::frac_pi_2()), + Pose::rotation(Vector::X * Real::frac_pi_2()), ); utils::transform( &mut coords[n * 2..n * 3], - Isometry::rotation(Vector::z() * Real::frac_pi_2()), + Pose::rotation(Vector::Z * Real::frac_pi_2()), ); utils::push_circle_outline_indices(&mut indices, 0..nsubdiv); @@ -43,7 +42,7 @@ fn unit_sphere_outline(nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { /// Creates an hemisphere with a radius of 0.5. pub(crate) fn push_unit_hemisphere_outline( nsubdiv: u32, - pts: &mut Vec>, + pts: &mut Vec, idx: &mut Vec<[u32; 2]>, ) { let base_idx = pts.len() as u32; @@ -56,12 +55,12 @@ pub(crate) fn push_unit_hemisphere_outline( let n = npoints as usize; utils::transform( &mut pts[base_idx as usize..base_idx as usize + n], - Isometry::rotation(Vector::x() * -Real::frac_pi_2()), + Pose::rotation(Vector::X * -Real::frac_pi_2()), ); utils::transform( &mut pts[base_idx as usize + n..base_idx as usize + n * 2], - Isometry::rotation(Vector::z() * Real::frac_pi_2()) - * Isometry::rotation(Vector::y() * Real::frac_pi_2()), + Pose::rotation(Vector::Z * Real::frac_pi_2()) + * Pose::rotation(Vector::Y * Real::frac_pi_2()), ); utils::push_open_circle_outline_indices(idx, base_idx..base_idx + npoints); diff --git a/src/transformation/to_outline/capsule_to_outline.rs b/src/transformation/to_outline/capsule_to_outline.rs index 8881b10e..398600f9 100644 --- a/src/transformation/to_outline/capsule_to_outline.rs +++ b/src/transformation/to_outline/capsule_to_outline.rs @@ -1,12 +1,11 @@ -use crate::math::Real; +use crate::math::{Real, Vector}; use crate::shape::{Capsule, Cylinder}; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point3}; impl Capsule { /// Outlines this capsule’s shape using polylines. - pub fn to_outline(&self, nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let (vtx, idx) = canonical_capsule_outline(self.radius, self.half_height(), nsubdiv); (utils::transformed(vtx, self.canonical_transform()), idx) } @@ -17,7 +16,7 @@ pub(crate) fn canonical_capsule_outline( caps_radius: Real, cylinder_half_height: Real, nsubdiv: u32, -) -> (Vec>, Vec<[u32; 2]>) { +) -> (Vec, Vec<[u32; 2]>) { let (mut vtx, mut idx) = Cylinder::new(cylinder_half_height, caps_radius).to_outline(nsubdiv); let shift = vtx.len() as u32; diff --git a/src/transformation/to_outline/cone_to_outline.rs b/src/transformation/to_outline/cone_to_outline.rs index 70d65c15..02ac651e 100644 --- a/src/transformation/to_outline/cone_to_outline.rs +++ b/src/transformation/to_outline/cone_to_outline.rs @@ -1,23 +1,22 @@ -use crate::math::Real; +use crate::math::Vector; use crate::shape::Cone; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point3, Vector3}; impl Cone { /// Outlines this cone’s shape using polylines. - pub fn to_outline(&self, nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let diameter = self.radius * 2.0; let height = self.half_height * 2.0; - let scale = Vector3::new(diameter, height, diameter); + let scale = Vector::new(diameter, height, diameter); let (vtx, idx) = unit_cone_outline(nsubdiv); (utils::scaled(vtx, scale), idx) } } /// Generates a cone with unit height and diameter. -fn unit_cone_outline(nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { - let mut out_vtx = vec![Point3::new(-0.5, -0.5, 0.0), Point3::new(0.0, 0.5, 0.0)]; +fn unit_cone_outline(nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { + let mut out_vtx = vec![Vector::new(-0.5, -0.5, 0.0), Vector::new(0.0, 0.5, 0.0)]; let mut out_ptx = vec![]; #[allow(clippy::single_range_in_vec_init)] // The single range is on purpose. utils::apply_revolution(false, true, &[0..1], nsubdiv, &mut out_vtx, &mut out_ptx); diff --git a/src/transformation/to_outline/convex_polyhedron_to_outline.rs b/src/transformation/to_outline/convex_polyhedron_to_outline.rs index 53595249..da817483 100644 --- a/src/transformation/to_outline/convex_polyhedron_to_outline.rs +++ b/src/transformation/to_outline/convex_polyhedron_to_outline.rs @@ -1,10 +1,10 @@ use crate::math::Real; use crate::shape::ConvexPolyhedron; -use na::Point3; +use crate::math::Vector3; impl ConvexPolyhedron { /// Outlines this convex polyhedron’s shape using polylines. - pub fn to_outline(&self) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_outline(&self) -> (Vec, Vec<[u32; 3]>) { let mut indices = Vec::new(); for face in self.faces() { diff --git a/src/transformation/to_outline/cuboid_to_outline.rs b/src/transformation/to_outline/cuboid_to_outline.rs index 009b7c57..ceee3a84 100644 --- a/src/transformation/to_outline/cuboid_to_outline.rs +++ b/src/transformation/to_outline/cuboid_to_outline.rs @@ -1,23 +1,23 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector}; +use crate::math::Vector; use crate::shape::Cuboid; use crate::transformation::utils; use alloc::{vec, vec::Vec}; impl Aabb { /// Outlines this Aabb’s shape using polylines. - pub fn to_outline(&self) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self) -> (Vec, Vec<[u32; 2]>) { let center = self.center(); let half_extents = self.half_extents(); let mut cube_mesh = Cuboid::new(half_extents).to_outline(); - cube_mesh.0.iter_mut().for_each(|p| *p += center.coords); + cube_mesh.0.iter_mut().for_each(|p| *p += center); cube_mesh } } impl Cuboid { /// Outlines this cuboid’s shape using polylines. - pub fn to_outline(&self) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self) -> (Vec, Vec<[u32; 2]>) { let (vtx, idx) = unit_cuboid_outline(); (utils::scaled(vtx, self.half_extents * 2.0), idx) } @@ -28,8 +28,8 @@ impl Cuboid { * * The cuboid is centered at the origin, and has its half extents set to 0.5. */ -fn unit_cuboid_outline() -> (Vec>, Vec<[u32; 2]>) { - let aabb = Aabb::from_half_extents(Point::origin(), Vector::repeat(0.5)); +fn unit_cuboid_outline() -> (Vec, Vec<[u32; 2]>) { + let aabb = Aabb::from_half_extents(Vector::ZERO, Vector::splat(0.5)); ( aabb.vertices().to_vec(), vec![ diff --git a/src/transformation/to_outline/cylinder_to_outline.rs b/src/transformation/to_outline/cylinder_to_outline.rs index b4ad3bdf..ec232ae4 100644 --- a/src/transformation/to_outline/cylinder_to_outline.rs +++ b/src/transformation/to_outline/cylinder_to_outline.rs @@ -1,23 +1,22 @@ -use crate::math::Real; +use crate::math::Vector; use crate::shape::Cylinder; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point3, Vector3}; impl Cylinder { /// Outlines this cylinder’s shape using polylines. - pub fn to_outline(&self, nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let diameter = self.radius * 2.0; let height = self.half_height * 2.0; - let scale = Vector3::new(diameter, height, diameter); + let scale = Vector::new(diameter, height, diameter); let (vtx, idx) = unit_cylinder_outline(nsubdiv); (utils::scaled(vtx, scale), idx) } } /// Generates a cylinder with unit height and diameter. -fn unit_cylinder_outline(nsubdiv: u32) -> (Vec>, Vec<[u32; 2]>) { - let mut out_vtx = vec![Point3::new(-0.5, -0.5, 0.0), Point3::new(-0.5, 0.5, 0.0)]; +fn unit_cylinder_outline(nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { + let mut out_vtx = vec![Vector::new(-0.5, -0.5, 0.0), Vector::new(-0.5, 0.5, 0.0)]; let mut out_idx = vec![]; #[allow(clippy::single_range_in_vec_init)] // The single range is on purpose. utils::apply_revolution(false, false, &[0..2], nsubdiv, &mut out_vtx, &mut out_idx); diff --git a/src/transformation/to_outline/heightfield_to_outline.rs b/src/transformation/to_outline/heightfield_to_outline.rs index 9f79664c..c04b2ee7 100644 --- a/src/transformation/to_outline/heightfield_to_outline.rs +++ b/src/transformation/to_outline/heightfield_to_outline.rs @@ -1,10 +1,10 @@ use crate::math::Real; use crate::shape::{HeightField, HeightFieldCellStatus}; -use na::Point3; +use crate::math::Vector3; impl HeightField { /// Outlines this heightfield’s shape using polylines. - pub fn to_outline(&self) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self) -> (Vec, Vec<[u32; 2]>) { todo!() } } diff --git a/src/transformation/to_outline/round_cone_to_outline.rs b/src/transformation/to_outline/round_cone_to_outline.rs index 6af5fe24..e4ef868f 100644 --- a/src/transformation/to_outline/round_cone_to_outline.rs +++ b/src/transformation/to_outline/round_cone_to_outline.rs @@ -1,16 +1,11 @@ -use crate::math::Real; +use crate::math::Vector; use crate::shape::RoundCone; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point3, Vector3}; impl RoundCone { /// Outlines this round cone’s shape using polylines. - pub fn to_outline( - &self, - nsubdiv: u32, - border_nsubdiv: u32, - ) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdiv: u32, border_nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let r = self.inner_shape.radius; let br = self.border_radius; let he = self.inner_shape.half_height; @@ -19,14 +14,14 @@ impl RoundCone { let mut out_idx = vec![]; // Compute the profile. - let center_ab = Point3::new(-r, -he, 0.0); - let center_cd = Point3::new(0.0, he, 0.0); - let side_dir = Vector3::new(-2.0 * he, r, 0.0).normalize(); + let center_ab = Vector::new(-r, -he, 0.0); + let center_cd = Vector::new(0.0, he, 0.0); + let side_dir = Vector::new(-2.0 * he, r, 0.0).normalize(); - let a = Point3::new(-r, -he - br, 0.0); - let b = Point3::new(-r, -he, 0.0) + side_dir * br; - let c = Point3::new(0.0, he, 0.0) + side_dir * br; - let d = Point3::new(0.0, he + br, 0.0); + let a = Vector::new(-r, -he - br, 0.0); + let b = Vector::new(-r, -he, 0.0) + side_dir * br; + let c = Vector::new(0.0, he, 0.0) + side_dir * br; + let d = Vector::new(0.0, he + br, 0.0); out_vtx.push(a); utils::push_arc(center_ab, a, b, border_nsubdiv, &mut out_vtx); diff --git a/src/transformation/to_outline/round_convex_polyhedron_to_outline.rs b/src/transformation/to_outline/round_convex_polyhedron_to_outline.rs index 8b1af24f..7ba4bba6 100644 --- a/src/transformation/to_outline/round_convex_polyhedron_to_outline.rs +++ b/src/transformation/to_outline/round_convex_polyhedron_to_outline.rs @@ -1,12 +1,11 @@ -use crate::math::Real; +use crate::math::Vector; use crate::shape::RoundConvexPolyhedron; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::Point3; impl RoundConvexPolyhedron { /// Outlines this round convex polyhedron’s shape using polylines. - pub fn to_outline(&self, nsubdivs: u32) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdivs: u32) -> (Vec, Vec<[u32; 2]>) { let mut out_vtx = vec![]; let mut out_idx = vec![]; let poly = &self.inner_shape; @@ -20,7 +19,7 @@ impl RoundConvexPolyhedron { for fid in adj_faces { let face = poly.faces()[*fid as usize]; - out_vtx.push(ref_pt + *face.normal * self.border_radius); + out_vtx.push(ref_pt + face.normal * self.border_radius); } } @@ -33,7 +32,7 @@ impl RoundConvexPolyhedron { let base = out_vtx.len() as u32; for idx in &poly.vertices_adj_to_face()[i1 as usize..i2 as usize] { - out_vtx.push(poly.points()[*idx as usize] + *face.normal * self.border_radius); + out_vtx.push(poly.points()[*idx as usize] + face.normal * self.border_radius); } for i in 0..face.num_vertices_or_edges - 1 { diff --git a/src/transformation/to_outline/round_cuboid_to_outline.rs b/src/transformation/to_outline/round_cuboid_to_outline.rs index cf653770..4e8f4af1 100644 --- a/src/transformation/to_outline/round_cuboid_to_outline.rs +++ b/src/transformation/to_outline/round_cuboid_to_outline.rs @@ -1,18 +1,18 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector}; +use crate::math::Vector; use crate::shape::RoundCuboid; use crate::transformation::utils; use alloc::{vec, vec::Vec}; impl RoundCuboid { /// Outlines this round cuboid’s surface with polylines. - pub fn to_outline(&self, nsubdivs: u32) -> (Vec>, Vec<[u32; 2]>) { - let aabb = Aabb::from_half_extents(Point::origin(), self.inner_shape.half_extents); + pub fn to_outline(&self, nsubdivs: u32) -> (Vec, Vec<[u32; 2]>) { + let aabb = Aabb::from_half_extents(Vector::ZERO, self.inner_shape.half_extents); let aabb_vtx = aabb.vertices(); let fidx = Aabb::FACES_VERTEX_IDS; - let x = Vector::x() * self.border_radius; - let y = Vector::y() * self.border_radius; - let z = Vector::z() * self.border_radius; + let x = Vector::X * self.border_radius; + let y = Vector::Y * self.border_radius; + let z = Vector::Z * self.border_radius; #[rustfmt::skip] let mut vtx = vec![ diff --git a/src/transformation/to_outline/round_cylinder_to_outline.rs b/src/transformation/to_outline/round_cylinder_to_outline.rs index 629a5b0f..754d87c5 100644 --- a/src/transformation/to_outline/round_cylinder_to_outline.rs +++ b/src/transformation/to_outline/round_cylinder_to_outline.rs @@ -1,16 +1,11 @@ -use crate::math::Real; +use crate::math::Vector; use crate::shape::RoundCylinder; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point3}; impl RoundCylinder { /// Outlines this round cylinder’s shape using polylines. - pub fn to_outline( - &self, - nsubdiv: u32, - border_nsubdiv: u32, - ) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdiv: u32, border_nsubdiv: u32) -> (Vec, Vec<[u32; 2]>) { let r = self.inner_shape.radius; let br = self.border_radius; let he = self.inner_shape.half_height; @@ -19,12 +14,12 @@ impl RoundCylinder { let mut out_idx = vec![]; // Compute the profile. - let center_ab = Point3::new(-r, -he, 0.0); - let center_cd = Point3::new(-r, he, 0.0); - let a = Point3::new(-r, -he - br, 0.0); - let b = Point3::new(-r - br, -he, 0.0); - let c = Point3::new(-r - br, he, 0.0); - let d = Point3::new(-r, he + br, 0.0); + let center_ab = Vector::new(-r, -he, 0.0); + let center_cd = Vector::new(-r, he, 0.0); + let a = Vector::new(-r, -he - br, 0.0); + let b = Vector::new(-r - br, -he, 0.0); + let c = Vector::new(-r - br, he, 0.0); + let d = Vector::new(-r, he + br, 0.0); out_vtx.push(a); utils::push_arc(center_ab, a, b, border_nsubdiv, &mut out_vtx); diff --git a/src/transformation/to_outline/round_triangle_to_outline.rs b/src/transformation/to_outline/round_triangle_to_outline.rs index a2c12e6c..6691da87 100644 --- a/src/transformation/to_outline/round_triangle_to_outline.rs +++ b/src/transformation/to_outline/round_triangle_to_outline.rs @@ -1,16 +1,16 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector}; +use crate::math::{Vector, Real, Vector}; use crate::shape::RoundTriangle; use crate::transformation::utils; impl RoundTriangle { /// Outlines this round triangle’s surface with polylines. - pub fn to_outline(&self, nsubdivs: u32) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self, nsubdivs: u32) -> (Vec, Vec<[u32; 2]>) { let tri = &self.inner_shape; let n = tri .normal() .map(|n| n.into_inner()) - .unwrap_or_else(|| Vector::zeros()); + .unwrap_or_else(|| Vector::ZERO); let mut out_vtx = vec![ tri.a + n * self.border_radius, tri.b + n * self.border_radius, diff --git a/src/transformation/to_outline/voxels_to_outline.rs b/src/transformation/to_outline/voxels_to_outline.rs index f81a0935..dd9fb0f6 100644 --- a/src/transformation/to_outline/voxels_to_outline.rs +++ b/src/transformation/to_outline/voxels_to_outline.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real}; +use crate::math::Vector; use crate::shape::{VoxelType, Voxels}; use alloc::{vec, vec::Vec}; @@ -7,7 +7,7 @@ impl Voxels { /// Outlines this voxels shape as a set of polylines. /// /// The outline is such that only convex edges are output in the polyline. - pub fn to_outline(&self) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_outline(&self) -> (Vec, Vec<[u32; 2]>) { let mut points = vec![]; self.iter_outline(|a, b| { points.push(a); @@ -22,10 +22,10 @@ impl Voxels { /// Outlines this voxels shape using segments. /// /// The outline is such that only convex edges are output in the polyline. - pub fn iter_outline(&self, mut f: impl FnMut(Point, Point)) { + pub fn iter_outline(&self, mut f: impl FnMut(Vector, Vector)) { // TODO: move this as a new method: Voxels::to_outline? let radius = self.voxel_size() / 2.0; - let aabb = Aabb::from_half_extents(Point::origin(), radius); + let aabb = Aabb::from_half_extents(Vector::ZERO, radius); let vtx = aabb.vertices(); for vox in self.voxels() { @@ -35,10 +35,7 @@ impl Voxels { for edge in Aabb::EDGES_VERTEX_IDS { if mask & (1 << edge.0) != 0 || mask & (1 << edge.1) != 0 { - f( - vox.center + vtx[edge.0].coords, - vox.center + vtx[edge.1].coords, - ); + f(vox.center + vtx[edge.0], vox.center + vtx[edge.1]); } } } @@ -48,10 +45,7 @@ impl Voxels { for (i, edge) in Aabb::EDGES_VERTEX_IDS.iter().enumerate() { if mask & (1 << i) != 0 { - f( - vox.center + vtx[edge.0].coords, - vox.center + vtx[edge.1].coords, - ); + f(vox.center + vtx[edge.0], vox.center + vtx[edge.1]); } } } diff --git a/src/transformation/to_polyline/ball_to_polyline.rs b/src/transformation/to_polyline/ball_to_polyline.rs index 526e0bb9..9a8b4434 100644 --- a/src/transformation/to_polyline/ball_to_polyline.rs +++ b/src/transformation/to_polyline/ball_to_polyline.rs @@ -1,12 +1,11 @@ -use crate::math::Real; +use crate::math::{Real, RealField, Vector2}; use crate::shape::Ball; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point2, RealField}; impl Ball { /// Discretize the boundary of this ball as a polygonal line. - pub fn to_polyline(&self, nsubdivs: u32) -> Vec> { + pub fn to_polyline(&self, nsubdivs: u32) -> Vec { let diameter = self.radius * 2.0; let two_pi = Real::two_pi(); let dtheta = two_pi / (nsubdivs as Real); diff --git a/src/transformation/to_polyline/capsule_to_polyline.rs b/src/transformation/to_polyline/capsule_to_polyline.rs index 3c52d949..035bfe2a 100644 --- a/src/transformation/to_polyline/capsule_to_polyline.rs +++ b/src/transformation/to_polyline/capsule_to_polyline.rs @@ -1,23 +1,22 @@ -use crate::math::Real; +use crate::math::{Real, RealField, Vector, Vector2}; use crate::shape::Capsule; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point2, RealField, Vector2}; impl Capsule { /// Discretize the boundary of this capsule as a polygonal line. - pub fn to_polyline(&self, nsubdiv: u32) -> Vec> { + pub fn to_polyline(&self, nsubdiv: u32) -> Vec { let pi = Real::pi(); let dtheta = pi / (nsubdiv as Real); - let mut points: Vec> = Vec::with_capacity(nsubdiv as usize); + let mut points: Vec = Vec::with_capacity(nsubdiv as usize); utils::push_xy_arc(self.radius, nsubdiv, dtheta, &mut points); let npoints = points.len(); for i in 0..npoints { - let new_point = points[i] + Vector2::new(na::zero(), self.half_height()); + let new_point = points[i] + Vector::new(0.0, self.half_height()); points.push(-new_point); points[i] = new_point; diff --git a/src/transformation/to_polyline/cuboid_to_polyline.rs b/src/transformation/to_polyline/cuboid_to_polyline.rs index 0f332fdc..f4056d2f 100644 --- a/src/transformation/to_polyline/cuboid_to_polyline.rs +++ b/src/transformation/to_polyline/cuboid_to_polyline.rs @@ -1,22 +1,21 @@ -use crate::math::Real; +use crate::math::Vector2; use crate::shape::Cuboid; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point2}; impl Cuboid { /// Discretize the boundary of this cuboid as a polygonal line. - pub fn to_polyline(&self) -> Vec> { + pub fn to_polyline(&self) -> Vec { utils::scaled(unit_rectangle(), self.half_extents * 2.0) } } /// The contour of a unit cuboid lying on the x-y plane. -fn unit_rectangle() -> Vec> { +fn unit_rectangle() -> Vec { vec![ - Point2::new(-0.5, -0.5), - Point2::new(0.5, -0.5), - Point2::new(0.5, 0.5), - Point2::new(-0.5, 0.5), + Vector2::new(-0.5, -0.5), + Vector2::new(0.5, -0.5), + Vector2::new(0.5, 0.5), + Vector2::new(-0.5, 0.5), ] } diff --git a/src/transformation/to_polyline/heightfield_to_polyline.rs b/src/transformation/to_polyline/heightfield_to_polyline.rs index 045cee5c..9ecd2213 100644 --- a/src/transformation/to_polyline/heightfield_to_polyline.rs +++ b/src/transformation/to_polyline/heightfield_to_polyline.rs @@ -1,11 +1,10 @@ -use crate::math::Real; +use crate::math::Vector2; use crate::shape::HeightField; use alloc::{vec, vec::Vec}; -use na::Point2; impl HeightField { /// Rasterize this heightfield as a (potentially discontinuous) polyline. - pub fn to_polyline(&self) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_polyline(&self) -> (Vec, Vec<[u32; 2]>) { let mut vertices = vec![]; let mut indices = vec![]; diff --git a/src/transformation/to_polyline/round_convex_polygon_to_polyline.rs b/src/transformation/to_polyline/round_convex_polygon_to_polyline.rs index 03a3da45..a208a2ca 100644 --- a/src/transformation/to_polyline/round_convex_polygon_to_polyline.rs +++ b/src/transformation/to_polyline/round_convex_polygon_to_polyline.rs @@ -1,32 +1,31 @@ -use crate::math::Real; +use crate::math::Vector2; use crate::shape::RoundConvexPolygon; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point2}; impl RoundConvexPolygon { /// Discretize the boundary of this round convex polygon as a polygonal line. - pub fn to_polyline(&self, border_subdivs: u32) -> Vec> { + pub fn to_polyline(&self, border_subdivs: u32) -> Vec { let mut out_vtx = vec![]; let pts = self.inner_shape.points(); let ns = self.inner_shape.normals(); let br = self.border_radius; - out_vtx.push(pts[0] + **ns.last().unwrap() * br); + out_vtx.push(pts[0] + *ns.last().unwrap() * br); for ia in 0..pts.len() - 1 { let ib = ia + 1; let arc_start = *out_vtx.last().unwrap(); - let arc_end = pts[ia] + *ns[ia] * br; + let arc_end = pts[ia] + ns[ia] * br; utils::push_arc(pts[ia], arc_start, arc_end, border_subdivs, &mut out_vtx); out_vtx.push(arc_end); - out_vtx.push(pts[ib] + *ns[ia] * br); + out_vtx.push(pts[ib] + ns[ia] * br); } let arc_center = *pts.last().unwrap(); let arc_start = *out_vtx.last().unwrap(); - let arc_end = arc_center + **ns.last().unwrap() * br; + let arc_end = arc_center + *ns.last().unwrap() * br; utils::push_arc(arc_center, arc_start, arc_end, border_subdivs, &mut out_vtx); out_vtx.push(arc_end); diff --git a/src/transformation/to_polyline/round_cuboid_to_polyline.rs b/src/transformation/to_polyline/round_cuboid_to_polyline.rs index 82a2fd00..f3c5b359 100644 --- a/src/transformation/to_polyline/round_cuboid_to_polyline.rs +++ b/src/transformation/to_polyline/round_cuboid_to_polyline.rs @@ -1,30 +1,35 @@ -use crate::math::Real; +use crate::math::Vector2; use crate::shape::RoundCuboid; use crate::transformation::utils; use alloc::{vec, vec::Vec}; -use na::{self, Point2}; impl RoundCuboid { /// Discretize the boundary of this round cuboid as a polygonal line. - pub fn to_polyline(&self, border_subdivs: u32) -> Vec> { + pub fn to_polyline(&self, border_subdivs: u32) -> Vec { let mut out_vtx = vec![]; let he = self.inner_shape.half_extents; let br = self.border_radius; let arc_centers = [ - Point2::new(-he.x, -he.y), - Point2::new(he.x, -he.y), - Point2::new(he.x, he.y), - Point2::new(-he.x, he.y), + Vector2::new(-he.x, -he.y), + Vector2::new(he.x, -he.y), + Vector2::new(he.x, he.y), + Vector2::new(-he.x, he.y), ]; let arc_vertices = [ ( - Point2::new(-he.x - br, -he.y), - Point2::new(-he.x, -he.y - br), + Vector2::new(-he.x - br, -he.y), + Vector2::new(-he.x, -he.y - br), + ), + ( + Vector2::new(he.x, -he.y - br), + Vector2::new(he.x + br, -he.y), + ), + (Vector2::new(he.x + br, he.y), Vector2::new(he.x, he.y + br)), + ( + Vector2::new(-he.x, he.y + br), + Vector2::new(-he.x - br, he.y), ), - (Point2::new(he.x, -he.y - br), Point2::new(he.x + br, -he.y)), - (Point2::new(he.x + br, he.y), Point2::new(he.x, he.y + br)), - (Point2::new(-he.x, he.y + br), Point2::new(-he.x - br, he.y)), ]; for i in 0..4 { diff --git a/src/transformation/to_polyline/voxels_to_polyline.rs b/src/transformation/to_polyline/voxels_to_polyline.rs index 55c32b3f..727f3f40 100644 --- a/src/transformation/to_polyline/voxels_to_polyline.rs +++ b/src/transformation/to_polyline/voxels_to_polyline.rs @@ -1,12 +1,11 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real}; +use crate::math::{Vector, Vector2}; use crate::shape::{VoxelType, Voxels}; use alloc::{vec, vec::Vec}; -use na::{self, Point2}; impl Voxels { /// Converts the outlines of this set of voxels as a polyline. - pub fn to_polyline(&self) -> (Vec>, Vec<[u32; 2]>) { + pub fn to_polyline(&self) -> (Vec, Vec<[u32; 2]>) { let mut points = vec![]; self.iter_outline(|a, b| { points.push(a); @@ -19,10 +18,10 @@ impl Voxels { } /// Outlines this voxels shape with segments. - pub fn iter_outline(&self, mut f: impl FnMut(Point, Point)) { + pub fn iter_outline(&self, mut f: impl FnMut(Vector, Vector)) { // TODO: move this as a new method: Voxels::to_outline? let radius = self.voxel_size() / 2.0; - let aabb = Aabb::from_half_extents(Point::origin(), radius); + let aabb = Aabb::from_half_extents(Vector::ZERO, radius); let vtx = aabb.vertices(); for vox in self.voxels() { @@ -32,10 +31,7 @@ impl Voxels { for edge in Aabb::FACES_VERTEX_IDS { if mask & (1 << edge.0) != 0 || mask & (1 << edge.1) != 0 { - f( - vox.center + vtx[edge.0].coords, - vox.center + vtx[edge.1].coords, - ); + f(vox.center + vtx[edge.0], vox.center + vtx[edge.1]); } } } @@ -45,10 +41,7 @@ impl Voxels { for (i, edge) in Aabb::FACES_VERTEX_IDS.iter().enumerate() { if mask & (1 << i) != 0 { - f( - vox.center + vtx[edge.0].coords, - vox.center + vtx[edge.1].coords, - ); + f(vox.center + vtx[edge.0], vox.center + vtx[edge.1]); } } } diff --git a/src/transformation/to_trimesh/ball_to_trimesh.rs b/src/transformation/to_trimesh/ball_to_trimesh.rs index ecd33ac3..f7def9a1 100644 --- a/src/transformation/to_trimesh/ball_to_trimesh.rs +++ b/src/transformation/to_trimesh/ball_to_trimesh.rs @@ -1,8 +1,7 @@ -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{ComplexField, Real, RealField, Vector, Vector3, DIM}; use crate::shape::Ball; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, ComplexField, Point3, RealField}; impl Ball { /// Discretize the boundary of this ball as a triangle-mesh. @@ -10,34 +9,34 @@ impl Ball { &self, ntheta_subdiv: u32, nphi_subdiv: u32, - ) -> (Vec>, Vec<[u32; 3]>) { + ) -> (Vec, Vec<[u32; 3]>) { let diameter = self.radius * 2.0; let (vtx, idx) = unit_sphere(ntheta_subdiv, nphi_subdiv); - (utils::scaled(vtx, Vector::repeat(diameter)), idx) + (utils::scaled(vtx, Vector::splat(diameter)), idx) } } -fn unit_sphere(ntheta_subdiv: u32, nphi_subdiv: u32) -> (Vec>, Vec<[u32; 3]>) { +fn unit_sphere(ntheta_subdiv: u32, nphi_subdiv: u32) -> (Vec, Vec<[u32; 3]>) { let dtheta = Real::two_pi() / (ntheta_subdiv as Real); let dphi = Real::pi() / (nphi_subdiv as Real); let mut coords = Vec::new(); let mut curr_phi: Real = -Real::frac_pi_2() + dphi; - coords.push(Point::new(0.0, -1.0, 0.0)); + coords.push(Vector::new(0.0, -1.0, 0.0)); for _ in 1..nphi_subdiv { utils::push_circle( - ComplexField::cos(curr_phi), + ::cos(curr_phi), ntheta_subdiv, dtheta, - ComplexField::sin(curr_phi), + ::sin(curr_phi), &mut coords, ); curr_phi += dphi; } - coords.push(Point::new(0.0, 1.0, 0.0)); + coords.push(Vector::new(0.0, 1.0, 0.0)); let mut idx = Vec::new(); @@ -60,14 +59,14 @@ fn unit_sphere(ntheta_subdiv: u32, nphi_subdiv: u32) -> (Vec>, Vec< &mut idx, ); - (utils::scaled(coords, Vector::repeat(0.5)), idx) + (utils::scaled(coords, Vector::splat(0.5)), idx) } /// Creates an hemisphere with a diameter of 1. pub(crate) fn unit_hemisphere( ntheta_subdiv: u32, nphi_subdiv: u32, -) -> (Vec>, Vec<[u32; DIM]>) { +) -> (Vec, Vec<[u32; DIM]>) { let two_pi = Real::two_pi(); let pi_two = Real::frac_pi_2(); let dtheta = two_pi / (ntheta_subdiv as Real); @@ -78,16 +77,16 @@ pub(crate) fn unit_hemisphere( for _ in 0..nphi_subdiv { utils::push_circle( - ComplexField::cos(curr_phi), + ::cos(curr_phi), ntheta_subdiv, dtheta, - ComplexField::sin(curr_phi), + ::sin(curr_phi), &mut coords, ); curr_phi += dphi; } - coords.push(Point::new(0.0, 1.0, 0.0)); + coords.push(Vector::new(0.0, 1.0, 0.0)); let mut idx = Vec::new(); @@ -107,5 +106,5 @@ pub(crate) fn unit_hemisphere( &mut idx, ); - (utils::scaled(coords, Vector::repeat(0.5)), idx) + (utils::scaled(coords, Vector::splat(0.5)), idx) } diff --git a/src/transformation/to_trimesh/capsule_to_trimesh.rs b/src/transformation/to_trimesh/capsule_to_trimesh.rs index 4ec881eb..162f731f 100644 --- a/src/transformation/to_trimesh/capsule_to_trimesh.rs +++ b/src/transformation/to_trimesh/capsule_to_trimesh.rs @@ -1,8 +1,7 @@ -use crate::math::Real; +use crate::math::{Real, Vector3}; use crate::shape::Capsule; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point3}; impl Capsule { /// Discretize the boundary of this capsule as a triangle-mesh. @@ -10,7 +9,7 @@ impl Capsule { &self, ntheta_subdiv: u32, nphi_subdiv: u32, - ) -> (Vec>, Vec<[u32; 3]>) { + ) -> (Vec, Vec<[u32; 3]>) { let diameter = self.radius * 2.0; let height = self.half_height() * 2.0; let (vtx, idx) = canonical_capsule(diameter, height, ntheta_subdiv, nphi_subdiv); @@ -24,7 +23,7 @@ pub(crate) fn canonical_capsule( cylinder_height: Real, ntheta_subdiv: u32, nphi_subdiv: u32, -) -> (Vec>, Vec<[u32; 3]>) { +) -> (Vec, Vec<[u32; 3]>) { let (coords, indices) = super::ball_to_trimesh::unit_hemisphere(ntheta_subdiv, nphi_subdiv / 2); let mut bottom_coords = coords.clone(); let mut bottom_indices = indices.clone(); diff --git a/src/transformation/to_trimesh/cone_to_trimesh.rs b/src/transformation/to_trimesh/cone_to_trimesh.rs index 61402634..8fe1a7ee 100644 --- a/src/transformation/to_trimesh/cone_to_trimesh.rs +++ b/src/transformation/to_trimesh/cone_to_trimesh.rs @@ -1,36 +1,29 @@ -use crate::math::Real; +use crate::math::{Real, RealField, Vector}; use crate::shape::Cone; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point3, RealField, Vector3}; impl Cone { /// Discretize the boundary of this cone as a triangle-mesh. - pub fn to_trimesh(&self, nsubdiv: u32) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_trimesh(&self, nsubdiv: u32) -> (Vec, Vec<[u32; 3]>) { let diameter = self.radius * 2.0; let height = self.half_height * 2.0; - let scale = Vector3::new(diameter, height, diameter); + let scale = Vector::new(diameter, height, diameter); let (vtx, idx) = unit_cone(nsubdiv); (utils::scaled(vtx, scale), idx) } } /// Generates a cone with unit height and diameter. -fn unit_cone(nsubdiv: u32) -> (Vec>, Vec<[u32; 3]>) { +fn unit_cone(nsubdiv: u32) -> (Vec, Vec<[u32; 3]>) { let two_pi = Real::two_pi(); let dtheta = two_pi / (nsubdiv as Real); let mut coords = Vec::new(); let mut indices = Vec::new(); - utils::push_circle( - na::convert(0.5), - nsubdiv, - dtheta, - na::convert(-0.5), - &mut coords, - ); + utils::push_circle(0.5, nsubdiv, dtheta, -0.5, &mut coords); - coords.push(Point3::new(0.0, 0.5, 0.0)); + coords.push(Vector::new(0.0, 0.5, 0.0)); utils::push_degenerate_top_ring_indices(0, coords.len() as u32 - 1, nsubdiv, &mut indices); utils::push_filled_circle_indices(0, nsubdiv, &mut indices); diff --git a/src/transformation/to_trimesh/convex_polyhedron_to_trimesh.rs b/src/transformation/to_trimesh/convex_polyhedron_to_trimesh.rs index cae110d3..c72d6751 100644 --- a/src/transformation/to_trimesh/convex_polyhedron_to_trimesh.rs +++ b/src/transformation/to_trimesh/convex_polyhedron_to_trimesh.rs @@ -1,11 +1,10 @@ -use crate::math::Real; +use crate::math::Vector3; use crate::shape::ConvexPolyhedron; use alloc::vec::Vec; -use na::Point3; impl ConvexPolyhedron { /// Discretize the boundary of this convex polyhedron as a triangle-mesh. - pub fn to_trimesh(&self) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_trimesh(&self) -> (Vec, Vec<[u32; 3]>) { let mut indices = Vec::new(); for face in self.faces() { diff --git a/src/transformation/to_trimesh/cuboid_to_trimesh.rs b/src/transformation/to_trimesh/cuboid_to_trimesh.rs index 11f78eb6..65793e9f 100644 --- a/src/transformation/to_trimesh/cuboid_to_trimesh.rs +++ b/src/transformation/to_trimesh/cuboid_to_trimesh.rs @@ -1,23 +1,23 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real}; +use crate::math::Vector; use crate::shape::Cuboid; use crate::transformation::utils; use alloc::vec::Vec; impl Aabb { /// Discretize the boundary of this Aabb as a triangle-mesh. - pub fn to_trimesh(&self) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_trimesh(&self) -> (Vec, Vec<[u32; 3]>) { let center = self.center(); let half_extents = self.half_extents(); let mut cube_mesh = Cuboid::new(half_extents).to_trimesh(); - cube_mesh.0.iter_mut().for_each(|p| *p += center.coords); + cube_mesh.0.iter_mut().for_each(|p| *p += center); cube_mesh } } impl Cuboid { /// Discretize the boundary of this cuboid as a triangle-mesh. - pub fn to_trimesh(&self) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_trimesh(&self) -> (Vec, Vec<[u32; 3]>) { let (vtx, idx) = unit_cuboid(); (utils::scaled(vtx, self.half_extents * 2.0), idx) } @@ -26,20 +26,20 @@ impl Cuboid { /// Generates a cuboid shape with a split index buffer. /// /// The cuboid is centered at the origin, and has its half extents set to 0.5. -fn unit_cuboid() -> (Vec>, Vec<[u32; 3]>) { +fn unit_cuboid() -> (Vec, Vec<[u32; 3]>) { #[cfg(feature = "dim3")] { let mut coords = Vec::with_capacity(8); let mut faces = Vec::with_capacity(12); - coords.push(Point::new(-0.5, -0.5, 0.5)); - coords.push(Point::new(-0.5, -0.5, -0.5)); - coords.push(Point::new(0.5, -0.5, -0.5)); - coords.push(Point::new(0.5, -0.5, 0.5)); - coords.push(Point::new(-0.5, 0.5, 0.5)); - coords.push(Point::new(-0.5, 0.5, -0.5)); - coords.push(Point::new(0.5, 0.5, -0.5)); - coords.push(Point::new(0.5, 0.5, 0.5)); + coords.push(Vector::new(-0.5, -0.5, 0.5)); + coords.push(Vector::new(-0.5, -0.5, -0.5)); + coords.push(Vector::new(0.5, -0.5, -0.5)); + coords.push(Vector::new(0.5, -0.5, 0.5)); + coords.push(Vector::new(-0.5, 0.5, 0.5)); + coords.push(Vector::new(-0.5, 0.5, -0.5)); + coords.push(Vector::new(0.5, 0.5, -0.5)); + coords.push(Vector::new(0.5, 0.5, 0.5)); faces.push([4, 5, 0]); faces.push([5, 1, 0]); @@ -66,10 +66,10 @@ fn unit_cuboid() -> (Vec>, Vec<[u32; 3]>) { let mut coords = Vec::with_capacity(8); let mut faces = Vec::with_capacity(12); - coords.push(Point::new(-0.5, -0.5)); - coords.push(Point::new(0.5, -0.5)); - coords.push(Point::new(-0.5, 0.5)); - coords.push(Point::new(0.5, 0.5)); + coords.push(Vector::new(-0.5, -0.5)); + coords.push(Vector::new(0.5, -0.5)); + coords.push(Vector::new(-0.5, 0.5)); + coords.push(Vector::new(0.5, 0.5)); faces.push([0, 1, 2]); faces.push([2, 1, 3]); diff --git a/src/transformation/to_trimesh/cylinder_to_trimesh.rs b/src/transformation/to_trimesh/cylinder_to_trimesh.rs index 1ebec952..9786fdc8 100644 --- a/src/transformation/to_trimesh/cylinder_to_trimesh.rs +++ b/src/transformation/to_trimesh/cylinder_to_trimesh.rs @@ -1,43 +1,30 @@ -use crate::math::Real; +use crate::math::{Real, RealField, Vector}; use crate::shape::Cylinder; use crate::transformation::utils; use alloc::vec::Vec; -use na::{self, Point3, RealField, Vector3}; impl Cylinder { /// Discretize the boundary of this cylinder as a triangle-mesh. - pub fn to_trimesh(&self, nsubdiv: u32) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_trimesh(&self, nsubdiv: u32) -> (Vec, Vec<[u32; 3]>) { let diameter = self.radius * 2.0; let height = self.half_height * 2.0; - let scale = Vector3::new(diameter, height, diameter); + let scale = Vector::new(diameter, height, diameter); let (vtx, idx) = unit_cylinder(nsubdiv); (utils::scaled(vtx, scale), idx) } } /// Generates a cylinder with unit height and diameter. -fn unit_cylinder(nsubdiv: u32) -> (Vec>, Vec<[u32; 3]>) { +fn unit_cylinder(nsubdiv: u32) -> (Vec, Vec<[u32; 3]>) { let two_pi = Real::two_pi(); let invsubdiv = 1.0 / (nsubdiv as Real); let dtheta = two_pi * invsubdiv; let mut coords = Vec::new(); let mut indices = Vec::new(); - utils::push_circle( - na::convert(0.5), - nsubdiv, - dtheta, - na::convert(-0.5), - &mut coords, - ); + utils::push_circle(0.5, nsubdiv, dtheta, -0.5, &mut coords); - utils::push_circle( - na::convert(0.5), - nsubdiv, - dtheta, - na::convert(0.5), - &mut coords, - ); + utils::push_circle(0.5, nsubdiv, dtheta, 0.5, &mut coords); utils::push_ring_indices(0, nsubdiv, nsubdiv, &mut indices); utils::push_filled_circle_indices(0, nsubdiv, &mut indices); diff --git a/src/transformation/to_trimesh/heightfield_to_trimesh.rs b/src/transformation/to_trimesh/heightfield_to_trimesh.rs index d464c23f..31788617 100644 --- a/src/transformation/to_trimesh/heightfield_to_trimesh.rs +++ b/src/transformation/to_trimesh/heightfield_to_trimesh.rs @@ -1,11 +1,10 @@ -use crate::math::Real; +use crate::math::Vector3; use crate::shape::HeightField; use alloc::vec::Vec; -use na::Point3; impl HeightField { /// Discretize the boundary of this heightfield as a triangle-mesh. - pub fn to_trimesh(&self) -> (Vec>, Vec<[u32; 3]>) { + pub fn to_trimesh(&self) -> (Vec, Vec<[u32; 3]>) { let mut vertices = Vec::new(); let mut indices = Vec::new(); diff --git a/src/transformation/to_trimesh/mod.rs b/src/transformation/to_trimesh/mod.rs index 0ff821e0..4dc5e80d 100644 --- a/src/transformation/to_trimesh/mod.rs +++ b/src/transformation/to_trimesh/mod.rs @@ -2,7 +2,7 @@ //! //! This module provides functionality to convert various geometric shapes into triangle meshes //! (also called "trimesh" in Parry's nomenclature). A triangle mesh is represented as a pair -//! of vectors: `(Vec>, Vec<[u32; 3]>)` containing vertices and triangle indices. +//! of vectors: `(Vec, Vec<[u32; 3]>)` containing vertices and triangle indices. //! //! # Overview //! @@ -89,13 +89,14 @@ //! # { //! use parry3d::shape::HeightField; //! use parry3d::math::Vector; -//! use parry3d::na::DMatrix; +//! use parry3d::utils::Array2; //! //! // Create a simple 3×3 height field -//! let heights = DMatrix::from_row_slice(3, 3, &[ -//! 0.0, 1.0, 0.0, -//! 1.0, 2.0, 1.0, -//! 0.0, 1.0, 0.0, +//! // Column-major order: column 0 first, then column 1, then column 2 +//! let heights = Array2::new(3, 3, vec![ +//! 0.0, 1.0, 0.0, // column 0 +//! 1.0, 2.0, 1.0, // column 1 +//! 0.0, 1.0, 0.0, // column 2 //! ]); //! //! let heightfield = HeightField::new(heights, Vector::new(10.0, 10.0, 1.0)); @@ -109,9 +110,9 @@ //! //! # Return Format //! -//! All `to_trimesh()` methods return a tuple `(Vec>, Vec<[u32; 3]>)`: +//! All `to_trimesh()` methods return a tuple `(Vec, Vec<[u32; 3]>)`: //! -//! - **Vertices** (`Vec>`): Array of 3D points (or 2D for `dim2` feature) +//! - **Vertices** (`Vec`): Array of 3D points (or 2D for `dim2` feature) //! - **Indices** (`Vec<[u32; 3]>`): Array of triangle indices, where each `[u32; 3]` contains //! three indices into the vertices array //! diff --git a/src/transformation/to_trimesh/voxels_to_trimesh.rs b/src/transformation/to_trimesh/voxels_to_trimesh.rs index c1815f78..676cc68b 100644 --- a/src/transformation/to_trimesh/voxels_to_trimesh.rs +++ b/src/transformation/to_trimesh/voxels_to_trimesh.rs @@ -1,5 +1,5 @@ use crate::bounding_volume::Aabb; -use crate::math::{Point, Real}; +use crate::math::Vector; use crate::shape::Voxels; use alloc::{vec, vec::Vec}; @@ -8,8 +8,8 @@ impl Voxels { /// /// Each free face of each voxel will result in two triangles. No effort is made to merge /// adjacent triangles on large flat areas. - pub fn to_trimesh(&self) -> (Vec>, Vec<[u32; 3]>) { - let aabb = Aabb::from_half_extents(Point::origin(), self.voxel_size() / 2.0); + pub fn to_trimesh(&self) -> (Vec, Vec<[u32; 3]>) { + let aabb = Aabb::from_half_extents(Vector::ZERO, self.voxel_size() / 2.0); let aabb_vtx = aabb.vertices(); let mut vtx = vec![]; @@ -20,10 +20,10 @@ impl Voxels { if mask.bits() & (1 << i) != 0 { let fvid = Aabb::FACES_VERTEX_IDS[i]; let base_id = vtx.len() as u32; - vtx.push(vox.center + aabb_vtx[fvid.0].coords); - vtx.push(vox.center + aabb_vtx[fvid.1].coords); - vtx.push(vox.center + aabb_vtx[fvid.2].coords); - vtx.push(vox.center + aabb_vtx[fvid.3].coords); + vtx.push(vox.center + aabb_vtx[fvid.0]); + vtx.push(vox.center + aabb_vtx[fvid.1]); + vtx.push(vox.center + aabb_vtx[fvid.2]); + vtx.push(vox.center + aabb_vtx[fvid.3]); if i % 2 == 0 { idx.push([base_id, base_id + 1, base_id + 2]); diff --git a/src/transformation/utils.rs b/src/transformation/utils.rs index c048d850..ef483fe2 100644 --- a/src/transformation/utils.rs +++ b/src/transformation/utils.rs @@ -9,7 +9,7 @@ //! //! The utilities fall into several categories: //! -//! ## Point Transformations +//! ## Vector Transformations //! - [`transform`] / [`transformed`] - Apply rigid transformations (rotation + translation) //! - [`scaled`] - Apply non-uniform scaling //! @@ -45,7 +45,7 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] //! # { //! use parry3d::transformation::utils::{push_circle, push_ring_indices, push_filled_circle_indices}; -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use std::f32::consts::PI; //! //! let mut vertices = Vec::new(); @@ -79,7 +79,7 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] //! # { //! use parry3d::transformation::utils::{push_circle, push_degenerate_top_ring_indices, push_filled_circle_indices}; -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use std::f32::consts::PI; //! //! let mut vertices = Vec::new(); @@ -94,7 +94,7 @@ //! push_circle(radius, nsubdiv, dtheta, 0.0, &mut vertices); //! //! // Add apex point at the top -//! vertices.push(Point::new(0.0, height, 0.0)); +//! vertices.push(Vector::new(0.0, height, 0.0)); //! let apex_idx = (vertices.len() - 1) as u32; //! //! // Connect base circle to apex @@ -113,23 +113,22 @@ //! # #[cfg(all(feature = "dim3", feature = "f32"))] //! # { //! use parry3d::transformation::utils::{transform, scaled}; -//! use parry3d::math::{Point, Vector, Isometry}; -//! use parry3d::na::{Translation3, UnitQuaternion}; -//! use std::f32::consts::PI; +//! use parry3d::math::{Vector, Pose, Rotation}; +//! use core::f32::consts::PI; //! //! let mut points = vec![ -//! Point::new(1.0, 0.0, 0.0), -//! Point::new(0.0, 1.0, 0.0), -//! Point::new(0.0, 0.0, 1.0), +//! Vector::new(1.0, 0.0, 0.0), +//! Vector::new(0.0, 1.0, 0.0), +//! Vector::new(0.0, 0.0, 1.0), //! ]; //! //! // First, scale non-uniformly //! let points = scaled(points, Vector::new(2.0, 1.0, 0.5)); //! //! // Then rotate 45 degrees around Y axis -//! let rotation = UnitQuaternion::from_axis_angle(&Vector::y_axis(), PI / 4.0); -//! let translation = Translation3::new(10.0, 5.0, 0.0); -//! let isometry = Isometry::from_parts(translation.into(), rotation); +//! let rotation = Rotation::from_axis_angle(Vector::Y, PI / 4.0); +//! let translation = Vector::new(10.0, 5.0, 0.0); +//! let isometry = Pose::from_parts(translation, rotation); //! //! let final_points = parry3d::transformation::utils::transformed(points, isometry); //! # } @@ -138,7 +137,7 @@ //! # Design Philosophy //! //! These functions follow a "builder" pattern where: -//! 1. Vertices are pushed to a `Vec>` +//! 1. Vertices are pushed to a `Vec` //! 2. Indices are pushed to a `Vec<[u32; 3]>` (triangles) or `Vec<[u32; 2]>` (edges) //! 3. Functions work with index offsets, allowing incremental construction //! 4. No memory is allocated except for the output buffers @@ -149,7 +148,7 @@ //! //! - All functions use simple loops without SIMD (suitable for small to medium subdivisions) //! - Index generation has O(n) complexity where n is the subdivision count -//! - Point generation involves trigonometric functions (sin/cos) per subdivision +//! - Vector generation involves trigonometric functions (sin/cos) per subdivision //! - For high subdivision counts (>1000), consider caching generated geometry //! //! # See Also @@ -158,8 +157,11 @@ //! - [`convex_hull`](crate::transformation::convex_hull) - Convex hull computation //! - [`TriMesh`](crate::shape::TriMesh) - Triangle mesh shape -use crate::math::{Isometry, Point, Real, Vector}; -use crate::na::ComplexField; +#[cfg(feature = "dim3")] +use crate::math::RealField; +#[cfg(feature = "dim2")] +use crate::math::VectorExt; +use crate::math::{ComplexField, Pose, Real, Vector}; use alloc::vec::Vec; #[cfg(feature = "dim3")] use {crate::math::DIM, num::Zero}; @@ -178,31 +180,30 @@ use {crate::math::DIM, num::Zero}; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::utils::transform; -/// use parry3d::math::{Point, Isometry, Vector}; -/// use parry3d::na::Translation3; +/// use parry3d::math::{Vector, Pose, Rotation}; /// /// // Create some points /// let mut points = vec![ -/// Point::new(1.0, 0.0, 0.0), -/// Point::new(0.0, 1.0, 0.0), -/// Point::new(0.0, 0.0, 1.0), +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.0, 1.0, 0.0), +/// Vector::new(0.0, 0.0, 1.0), /// ]; /// /// // Create a translation -/// let transform_iso = Isometry::from_parts( -/// Translation3::new(10.0, 20.0, 30.0).into(), -/// parry3d::na::UnitQuaternion::identity() +/// let transform_iso = Pose::from_parts( +/// Vector::new(10.0, 20.0, 30.0), +/// Rotation::IDENTITY /// ); /// /// // Apply the transformation in-place /// transform(&mut points, transform_iso); /// -/// assert_eq!(points[0], Point::new(11.0, 20.0, 30.0)); -/// assert_eq!(points[1], Point::new(10.0, 21.0, 30.0)); -/// assert_eq!(points[2], Point::new(10.0, 20.0, 31.0)); +/// assert_eq!(points[0], Vector::new(11.0, 20.0, 30.0)); +/// assert_eq!(points[1], Vector::new(10.0, 21.0, 30.0)); +/// assert_eq!(points[2], Vector::new(10.0, 20.0, 31.0)); /// # } /// ``` -pub fn transform(points: &mut [Point], m: Isometry) { +pub fn transform(points: &mut [Vector], m: Pose) { points.iter_mut().for_each(|p| *p = m * *p); } @@ -223,27 +224,26 @@ pub fn transform(points: &mut [Point], m: Isometry) { /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::transformation::utils::transformed; -/// use parry2d::math::{Point, Isometry}; -/// use parry2d::na::{Translation2, UnitComplex}; +/// use parry2d::math::{Vector, Pose, Rotation}; /// use std::f32::consts::PI; /// /// let points = vec![ -/// Point::new(1.0, 0.0), -/// Point::new(0.0, 1.0), +/// Vector::new(1.0, 0.0), +/// Vector::new(0.0, 1.0), /// ]; /// /// // Rotate 90 degrees counter-clockwise around origin -/// let rotation = UnitComplex::new(PI / 2.0); -/// let transform = Isometry::from_parts(Translation2::identity().into(), rotation); +/// let rotation = Rotation::new(PI / 2.0); +/// let transform = Pose::from_parts(Vector::ZERO, rotation); /// /// let result = transformed(points, transform); /// -/// // Points are now rotated +/// // Vectors are now rotated /// assert!((result[0].x - 0.0).abs() < 1e-6); /// assert!((result[0].y - 1.0).abs() < 1e-6); /// # } /// ``` -pub fn transformed(mut points: Vec>, m: Isometry) -> Vec> { +pub fn transformed(mut points: Vec, m: Pose) -> Vec { transform(&mut points, m); points } @@ -266,25 +266,23 @@ pub fn transformed(mut points: Vec>, m: Isometry) -> Vec>, scale: Vector) -> Vec> { - points - .iter_mut() - .for_each(|p| p.coords.component_mul_assign(&scale)); +pub fn scaled(mut points: Vec, scale: Vector) -> Vec { + points.iter_mut().for_each(|p| *p *= scale); points } @@ -292,7 +290,7 @@ pub fn scaled(mut points: Vec>, scale: Vector) -> Vec>, scale: Vector) -> Vec>, scale: Vector) -> Vec>) { +pub fn push_circle(radius: Real, nsubdiv: u32, dtheta: Real, y: Real, out: &mut Vec) { let mut curr_theta = Real::zero(); for _ in 0..nsubdiv { - out.push(Point::new( - ComplexField::cos(curr_theta) * radius, + out.push(Vector::new( + ::cos(curr_theta) * radius, y, - ComplexField::sin(curr_theta) * radius, + ::sin(curr_theta) * radius, )); curr_theta += dtheta; } @@ -344,7 +342,7 @@ pub fn push_circle(radius: Real, nsubdiv: u32, dtheta: Real, y: Real, out: &mut /// /// This function generates points along an arc in the XY plane (2D). /// The arc is contained on the plane spanned by the X and Y axes. -/// Points are generated counter-clockwise starting from angle 0 (positive X axis). +/// Vectors are generated counter-clockwise starting from angle 0 (positive X axis). /// /// # Arguments /// * `radius` - The radius of the arc @@ -357,7 +355,7 @@ pub fn push_circle(radius: Real, nsubdiv: u32, dtheta: Real, y: Real, out: &mut /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::transformation::utils::push_xy_arc; -/// use parry2d::math::Point; +/// use parry2d::math::Vector; /// use std::f32::consts::PI; /// /// let mut points = Vec::new(); @@ -375,15 +373,15 @@ pub fn push_circle(radius: Real, nsubdiv: u32, dtheta: Real, y: Real, out: &mut /// ``` #[inline] #[cfg(feature = "dim2")] -pub fn push_xy_arc(radius: Real, nsubdiv: u32, dtheta: Real, out: &mut Vec>) { +pub fn push_xy_arc(radius: Real, nsubdiv: u32, dtheta: Real, out: &mut Vec) { let mut curr_theta: Real = 0.0; for _ in 0..nsubdiv { - let mut pt_coords = Vector::zeros(); + let mut pt_coords = Vector::ZERO; - pt_coords[0] = ComplexField::cos(curr_theta) * radius; - pt_coords[1] = ComplexField::sin(curr_theta) * radius; - out.push(Point::from(pt_coords)); + pt_coords[0] = ::cos(curr_theta) * radius; + pt_coords[1] = ::sin(curr_theta) * radius; + out.push(pt_coords); curr_theta += dtheta; } @@ -406,7 +404,7 @@ pub fn push_xy_arc(radius: Real, nsubdiv: u32, dtheta: Real, out: &mut Vec, range: core /// the `start` and `end` points). #[cfg(feature = "dim3")] pub fn push_arc_and_idx( - center: Point, + center: Vector, start: u32, end: u32, nsubdivs: u32, - out_vtx: &mut Vec>, + out_vtx: &mut Vec, out_idx: &mut Vec<[u32; 2]>, ) { let base = out_vtx.len() as u32; @@ -682,12 +680,12 @@ pub fn push_arc_and_idx( /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::transformation::utils::push_arc; -/// use parry2d::math::Point; +/// use parry2d::math::Vector; /// /// let mut points = Vec::new(); -/// let center = Point::new(0.0, 0.0); -/// let start = Point::new(5.0, 0.0); // 5 units to the right -/// let end = Point::new(0.0, 5.0); // 5 units up (90 degree arc) +/// let center = Vector::new(0.0, 0.0); +/// let start = Vector::new(5.0, 0.0); // 5 units to the right +/// let end = Vector::new(0.0, 5.0); // 5 units up (90 degree arc) /// /// // Generate 3 intermediate points /// push_arc(center, start, end, 3, &mut points); @@ -696,44 +694,39 @@ pub fn push_arc_and_idx( /// assert_eq!(points.len(), 2); /// # } /// ``` -pub fn push_arc( - center: Point, - start: Point, - end: Point, - nsubdivs: u32, - out: &mut Vec>, -) { +pub fn push_arc(center: Vector, start: Vector, end: Vector, nsubdivs: u32, out: &mut Vec) { assert!(nsubdivs > 0); - if let (Some((start_dir, start_len)), Some((end_dir, end_len))) = ( - na::Unit::try_new_and_get(start - center, 0.0), - na::Unit::try_new_and_get(end - center, 0.0), - ) { + let (start_dir, start_len) = (start - center).normalize_and_length(); + let (end_dir, end_len) = (end - center).normalize_and_length(); + + if start_len > 0.0 && end_len > 0.0 { let len_inc = (end_len - start_len) / nsubdivs as Real; #[cfg(feature = "dim2")] - let rot = Some(na::UnitComplex::scaled_rotation_between_axis( - &start_dir, - &end_dir, - 1.0 / nsubdivs as Real, - )); + let rot = { + use crate::math::Rot2; + let angle = start_dir.angle(end_dir); + let angle_inc = angle / nsubdivs as Real; + Rot2::new(angle_inc) + }; #[cfg(feature = "dim3")] - let rot = na::UnitQuaternion::scaled_rotation_between_axis( - &start_dir, - &end_dir, - 1.0 / nsubdivs as Real, - ); - - if let Some(rot) = rot { - let mut curr_dir = start_dir; - let mut curr_len = start_len; - - for _ in 0..nsubdivs - 1 { - curr_dir = rot * curr_dir; - curr_len += len_inc; - - out.push(center + *curr_dir * curr_len); - } + let rot = { + use crate::math::Rot3; + // Compute rotation from start_dir to end_dir + let rot = Rot3::from_rotation_arc(start_dir, end_dir); + let axisangle = rot.to_scaled_axis(); + Rot3::from_scaled_axis(axisangle / nsubdivs as Real) + }; + + let mut curr_dir = start_dir; + let mut curr_len = start_len; + + for _ in 0..nsubdivs - 1 { + curr_dir = rot * curr_dir; + curr_len += len_inc; + + out.push(center + curr_dir * curr_len); } } } @@ -760,10 +753,9 @@ pub fn apply_revolution( collapse_top: bool, circle_ranges: &[core::ops::Range], nsubdivs: u32, - out_vtx: &mut Vec>, // Must be set to the half-profile. + out_vtx: &mut Vec, // Must be set to the half-profile. out_idx: &mut Vec<[u32; 2]>, ) { - use na::RealField; let ang_increment = Real::two_pi() / (nsubdivs as Real); let angles = [ ang_increment * (nsubdivs / 4) as Real, @@ -789,7 +781,7 @@ pub fn apply_revolution( // Push rotated profiles. for angle in angles { let base = out_vtx.len() as u32; - let rot = na::UnitQuaternion::new(Vector::y() * angle); + let rot = crate::math::Rot3::from_axis_angle(Vector::Y, angle); if collapse_bottom { out_idx.push([0, base]); @@ -816,7 +808,7 @@ pub fn apply_revolution( let pt = out_vtx[i as usize]; let base = out_vtx.len() as u32; push_circle( - pt.coords.xz().norm(), + crate::math::Vector2::new(pt.x, pt.z).length(), nsubdivs, ang_increment, pt.y, diff --git a/src/transformation/vhacd/mod.rs b/src/transformation/vhacd/mod.rs index ab3494ab..9c6eb59e 100644 --- a/src/transformation/vhacd/mod.rs +++ b/src/transformation/vhacd/mod.rs @@ -52,19 +52,19 @@ //! //! ```no_run //! # #[cfg(all(feature = "dim3", feature = "f32"))] { -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use parry3d::transformation::vhacd::VHACD; //! use parry3d::transformation::vhacd::VHACDParameters; //! //! // Define a simple concave mesh (an 'L' shape made of triangles) //! let vertices = vec![ -//! Point::new(0.0, 0.0, 0.0), Point::new(2.0, 0.0, 0.0), -//! Point::new(2.0, 1.0, 0.0), Point::new(1.0, 1.0, 0.0), -//! Point::new(1.0, 2.0, 0.0), Point::new(0.0, 2.0, 0.0), +//! Vector::new(0.0, 0.0, 0.0), Vector::new(2.0, 0.0, 0.0), +//! Vector::new(2.0, 1.0, 0.0), Vector::new(1.0, 1.0, 0.0), +//! Vector::new(1.0, 2.0, 0.0), Vector::new(0.0, 2.0, 0.0), //! // Add corresponding back face vertices -//! Point::new(0.0, 0.0, 1.0), Point::new(2.0, 0.0, 1.0), -//! Point::new(2.0, 1.0, 1.0), Point::new(1.0, 1.0, 1.0), -//! Point::new(1.0, 2.0, 1.0), Point::new(0.0, 2.0, 1.0), +//! Vector::new(0.0, 0.0, 1.0), Vector::new(2.0, 0.0, 1.0), +//! Vector::new(2.0, 1.0, 1.0), Vector::new(1.0, 1.0, 1.0), +//! Vector::new(1.0, 2.0, 1.0), Vector::new(0.0, 2.0, 1.0), //! ]; //! let indices = vec![ //! // Front face triangles @@ -103,13 +103,13 @@ //! //! ```no_run //! # #[cfg(all(feature = "dim3", feature = "f32"))] { -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; //! use parry3d::transformation::voxelization::FillMode; //! //! # let vertices = vec![ -//! # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), -//! # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.0, 1.0), +//! # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), +//! # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.0, 1.0), //! # ]; //! # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; //! # @@ -139,12 +139,12 @@ //! //! ```no_run //! # #[cfg(all(feature = "dim3", feature = "f32"))] { -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; //! //! # let vertices = vec![ -//! # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), -//! # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.0, 1.0), +//! # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), +//! # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.0, 1.0), //! # ]; //! # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; //! # @@ -168,14 +168,14 @@ //! //! ```no_run //! # #[cfg(all(feature = "dim2", feature = "f32"))] { -//! use parry2d::math::Point; +//! use parry2d::math::Vector; //! use parry2d::transformation::vhacd::{VHACD, VHACDParameters}; //! //! // Define a concave polyline (e.g., an 'L' shape) //! let vertices = vec![ -//! Point::new(0.0, 0.0), Point::new(2.0, 0.0), -//! Point::new(2.0, 1.0), Point::new(1.0, 1.0), -//! Point::new(1.0, 2.0), Point::new(0.0, 2.0), +//! Vector::new(0.0, 0.0), Vector::new(2.0, 0.0), +//! Vector::new(2.0, 1.0), Vector::new(1.0, 1.0), +//! Vector::new(1.0, 2.0), Vector::new(0.0, 2.0), //! ]; //! let indices = vec![ //! [0, 1], [1, 2], [2, 3], [3, 4], [4, 5], [5, 0], @@ -202,14 +202,14 @@ //! //! ```no_run //! # #[cfg(all(feature = "dim3", feature = "f32"))] { -//! use parry3d::math::Point; +//! use parry3d::math::Vector; //! use parry3d::shape::{SharedShape, Compound}; //! use parry3d::transformation::vhacd::VHACDParameters; -//! use parry3d::na::Isometry3; +//! use parry3d::math::Pose; //! //! # let vertices = vec![ -//! # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), -//! # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.0, 1.0), +//! # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), +//! # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.0, 1.0), //! # ]; //! # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; //! # diff --git a/src/transformation/vhacd/vhacd.rs b/src/transformation/vhacd/vhacd.rs index ff31f28b..6ab6f050 100644 --- a/src/transformation/vhacd/vhacd.rs +++ b/src/transformation/vhacd/vhacd.rs @@ -16,20 +16,20 @@ // > // > THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -use crate::math::{Point, Real, Vector, DIM}; +use crate::math::{Int, Real, Vector, VectorExt, DIM}; use crate::transformation::vhacd::VHACDParameters; use crate::transformation::voxelization::{VoxelSet, VoxelizedVolume}; use alloc::sync::Arc; use alloc::vec::Vec; #[cfg(feature = "dim2")] -type ConvexHull = Vec>; +type ConvexHull = Vec; #[cfg(feature = "dim3")] -type ConvexHull = (Vec>, Vec<[u32; 3]>); +type ConvexHull = (Vec, Vec<[u32; 3]>); #[derive(Copy, Clone, Debug)] pub(crate) struct CutPlane { - pub abc: Vector, + pub abc: Vector, pub d: Real, pub axis: u8, pub index: u32, @@ -60,15 +60,15 @@ pub(crate) struct CutPlane { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// // Define a simple mesh (tetrahedron) /// let vertices = vec![ -/// Point::new(0.0, 0.0, 0.0), -/// Point::new(1.0, 0.0, 0.0), -/// Point::new(0.5, 1.0, 0.0), -/// Point::new(0.5, 0.5, 1.0), +/// Vector::new(0.0, 0.0, 0.0), +/// Vector::new(1.0, 0.0, 0.0), +/// Vector::new(0.5, 1.0, 0.0), +/// Vector::new(0.5, 0.5, 1.0), /// ]; /// let indices = vec![ /// [0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2], @@ -94,12 +94,12 @@ pub(crate) struct CutPlane { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// # let vertices = vec![ -/// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), -/// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), +/// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), +/// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -178,13 +178,13 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// // Simple L-shaped mesh /// let vertices = vec![ - /// Point::new(0.0, 0.0, 0.0), Point::new(2.0, 0.0, 0.0), - /// Point::new(2.0, 1.0, 0.0), Point::new(1.0, 1.0, 0.0), + /// Vector::new(0.0, 0.0, 0.0), Vector::new(2.0, 0.0, 0.0), + /// Vector::new(2.0, 1.0, 0.0), Vector::new(1.0, 1.0, 0.0), /// ]; /// let indices = vec![ /// [0, 1, 2], [0, 2, 3], @@ -205,12 +205,12 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -231,12 +231,12 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -260,7 +260,7 @@ impl VHACD { /// - [`compute_exact_convex_hulls`](VHACD::compute_exact_convex_hulls): Generate mesh-based hulls pub fn decompose( params: &VHACDParameters, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], keep_voxel_to_primitives_map: bool, ) -> Self { @@ -312,13 +312,13 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// use parry3d::transformation::voxelization::{VoxelizedVolume, FillMode}; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -389,12 +389,12 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -426,13 +426,13 @@ impl VHACD { } #[cfg(feature = "dim2")] - fn compute_preferred_cutting_direction(eigenvalues: &Vector) -> (Vector, Real) { + fn compute_preferred_cutting_direction(eigenvalues: Vector) -> (Vector, Real) { let vx = eigenvalues.y * eigenvalues.y; let vy = eigenvalues.x * eigenvalues.x; if vx < vy { let e = eigenvalues.y * eigenvalues.y; - let dir = Vector::x(); + let dir = Vector::X; if e == 0.0 { (dir, 0.0) @@ -441,7 +441,7 @@ impl VHACD { } } else { let e = eigenvalues.x * eigenvalues.x; - let dir = Vector::y(); + let dir = Vector::Y; if e == 0.0 { (dir, 0.0) @@ -452,14 +452,14 @@ impl VHACD { } #[cfg(feature = "dim3")] - fn compute_preferred_cutting_direction(eigenvalues: &Vector) -> (Vector, Real) { + fn compute_preferred_cutting_direction(eigenvalues: Vector) -> (Vector, Real) { let vx = (eigenvalues.y - eigenvalues.z) * (eigenvalues.y - eigenvalues.z); let vy = (eigenvalues.x - eigenvalues.z) * (eigenvalues.x - eigenvalues.z); let vz = (eigenvalues.x - eigenvalues.y) * (eigenvalues.x - eigenvalues.y); if vx < vy && vx < vz { let e = eigenvalues.y * eigenvalues.y + eigenvalues.z * eigenvalues.z; - let dir = Vector::x(); + let dir = Vector::X; if e == 0.0 { (dir, 0.0) @@ -468,7 +468,7 @@ impl VHACD { } } else if vy < vx && vy < vz { let e = eigenvalues.x * eigenvalues.x + eigenvalues.z * eigenvalues.z; - let dir = Vector::y(); + let dir = Vector::Y; if e == 0.0 { (dir, 0.0) @@ -477,7 +477,7 @@ impl VHACD { } } else { let e = eigenvalues.x * eigenvalues.x + eigenvalues.y * eigenvalues.y; - let dir = Vector::z(); + let dir = Vector::Z; if e == 0.0 { (dir, 0.0) @@ -497,15 +497,15 @@ impl VHACD { let max_v = vset.max_bb_voxels(); let best_id = best_plane.axis as usize; - let i0 = min_v[best_id].max(best_plane.index.saturating_sub(downsampling)); - let i1 = max_v[best_id].min(best_plane.index + downsampling); + let i0 = min_v[best_id].max(best_plane.index.saturating_sub(downsampling) as Int); + let i1 = max_v[best_id].min((best_plane.index + downsampling) as Int); for i in i0..=i1 { let plane = CutPlane { abc: Vector::ith(best_id, 1.0), axis: best_plane.axis, d: -(vset.origin[best_id] + (i as Real + 0.5) * vset.scale), - index: i, + index: i as u32, }; planes.push(plane); } @@ -517,7 +517,7 @@ impl VHACD { input_voxels: &VoxelSet, input_voxels_ch: &ConvexHull, planes: &[CutPlane], - preferred_cutting_direction: &Vector, + preferred_cutting_direction: Vector, w: Real, alpha: Real, beta: Real, @@ -584,11 +584,11 @@ impl VHACD { let symmetry = beta * d; let total = concavity + balance + symmetry; - if total < min_total || (total == min_total && (x as i32) < i_best) { + if total < min_total || (total == min_total && (x as Int) < i_best) { min_concavity = concavity; best_plane = *plane; min_total = total; - i_best = x as i32; + i_best = x as Int; } } @@ -622,7 +622,7 @@ impl VHACD { if concavity > params.concavity { let eigenvalues = voxels.compute_principal_axes(); let (preferred_cutting_direction, w) = - Self::compute_preferred_cutting_direction(&eigenvalues); + Self::compute_preferred_cutting_direction(eigenvalues); let mut planes = Vec::new(); voxels.compute_axes_aligned_clipping_planes(params.plane_downsampling, &mut planes); @@ -631,7 +631,7 @@ impl VHACD { &voxels, &voxels_convex_hull, &planes, - &preferred_cutting_direction, + preferred_cutting_direction, w, concavity * params.alpha, concavity * params.beta, @@ -653,7 +653,7 @@ impl VHACD { &voxels, &voxels_convex_hull, &planes_ref, - &preferred_cutting_direction, + preferred_cutting_direction, w, concavity * params.alpha, concavity * params.beta, @@ -786,9 +786,9 @@ impl VHACD { /// `self`. pub fn compute_primitive_intersections( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], - ) -> Vec>> { + ) -> Vec> { self.voxel_parts .iter() .map(|part| part.compute_primitive_intersections(points, indices)) @@ -816,7 +816,7 @@ impl VHACD { /// # Returns /// /// A vector of convex polygons (one per decomposed part), where each polygon is - /// represented as a `Vec>` containing the hull vertices in order. + /// represented as a `Vec` containing the hull vertices in order. /// /// # Performance /// @@ -828,14 +828,14 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim2", feature = "f32"))] { - /// use parry2d::math::Point; + /// use parry2d::math::Vector; /// use parry2d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// // Define an L-shaped polyline /// let vertices = vec![ - /// Point::new(0.0, 0.0), Point::new(2.0, 0.0), - /// Point::new(2.0, 1.0), Point::new(1.0, 1.0), - /// Point::new(1.0, 2.0), Point::new(0.0, 2.0), + /// Vector::new(0.0, 0.0), Vector::new(2.0, 0.0), + /// Vector::new(2.0, 1.0), Vector::new(1.0, 1.0), + /// Vector::new(1.0, 2.0), Vector::new(0.0, 2.0), /// ]; /// let indices = vec![ /// [0, 1], [1, 2], [2, 3], [3, 4], [4, 5], [5, 0], @@ -869,9 +869,9 @@ impl VHACD { #[cfg(feature = "dim2")] pub fn compute_exact_convex_hulls( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], - ) -> Vec>> { + ) -> Vec> { self.voxel_parts .iter() .map(|part| part.compute_exact_convex_hull(points, indices)) @@ -900,7 +900,7 @@ impl VHACD { /// /// A vector of convex hulls (one per decomposed part), where each hull is represented as /// a tuple `(vertices, indices)`: - /// - `vertices`: `Vec>` - The hull vertices + /// - `vertices`: `Vec` - The hull vertices /// - `indices`: `Vec<[u32; 3]>` - Triangle indices defining the hull surface /// /// # Performance @@ -913,12 +913,12 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -950,9 +950,9 @@ impl VHACD { #[cfg(feature = "dim3")] pub fn compute_exact_convex_hulls( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], - ) -> Vec<(Vec>, Vec<[u32; DIM]>)> { + ) -> Vec<(Vec, Vec<[u32; DIM]>)> { self.voxel_parts .iter() .map(|part| part.compute_exact_convex_hull(points, indices)) @@ -978,7 +978,7 @@ impl VHACD { /// # Returns /// /// A vector of convex polygons (one per decomposed part), where each polygon is - /// represented as a `Vec>` containing the hull vertices in order. + /// represented as a `Vec` containing the hull vertices in order. /// /// # Performance /// @@ -1002,14 +1002,14 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim2", feature = "f32"))] { - /// use parry2d::math::Point; + /// use parry2d::math::Vector; /// use parry2d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// // Define an L-shaped polyline /// let vertices = vec![ - /// Point::new(0.0, 0.0), Point::new(2.0, 0.0), - /// Point::new(2.0, 1.0), Point::new(1.0, 1.0), - /// Point::new(1.0, 2.0), Point::new(0.0, 2.0), + /// Vector::new(0.0, 0.0), Vector::new(2.0, 0.0), + /// Vector::new(2.0, 1.0), Vector::new(1.0, 1.0), + /// Vector::new(1.0, 2.0), Vector::new(0.0, 2.0), /// ]; /// let indices = vec![ /// [0, 1], [1, 2], [2, 3], [3, 4], [4, 5], [5, 0], @@ -1036,7 +1036,7 @@ impl VHACD { /// - [`compute_exact_convex_hulls`](VHACD::compute_exact_convex_hulls): More accurate mesh-based hulls /// - [`voxel_parts`](VHACD::voxel_parts): Access the raw voxel data #[cfg(feature = "dim2")] - pub fn compute_convex_hulls(&self, downsampling: u32) -> Vec>> { + pub fn compute_convex_hulls(&self, downsampling: u32) -> Vec> { let downsampling = downsampling.max(1); self.voxel_parts .iter() @@ -1064,7 +1064,7 @@ impl VHACD { /// /// A vector of convex hulls (one per decomposed part), where each hull is represented as /// a tuple `(vertices, indices)`: - /// - `vertices`: `Vec>` - The hull vertices + /// - `vertices`: `Vec` - The hull vertices /// - `indices`: `Vec<[u32; 3]>` - Triangle indices defining the hull surface /// /// # Performance @@ -1091,12 +1091,12 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -1120,14 +1120,14 @@ impl VHACD { /// /// ```no_run /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::math::Point; + /// use parry3d::math::Vector; /// use parry3d::shape::{SharedShape, Compound}; /// use parry3d::transformation::vhacd::{VHACD, VHACDParameters}; - /// use parry3d::na::Isometry3; + /// use parry3d::math::Pose; /// /// # let vertices = vec![ - /// # Point::new(0.0, 0.0, 0.0), Point::new(1.0, 0.0, 0.0), - /// # Point::new(0.5, 1.0, 0.0), Point::new(0.5, 0.5, 1.0), + /// # Vector::new(0.0, 0.0, 0.0), Vector::new(1.0, 0.0, 0.0), + /// # Vector::new(0.5, 1.0, 0.0), Vector::new(0.5, 0.5, 1.0), /// # ]; /// # let indices = vec![[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]]; /// # @@ -1148,10 +1148,7 @@ impl VHACD { /// - [`voxel_parts`](VHACD::voxel_parts): Access the raw voxel data /// - [`SharedShape::convex_decomposition`](crate::shape::SharedShape::convex_decomposition): High-level API #[cfg(feature = "dim3")] - pub fn compute_convex_hulls( - &self, - downsampling: u32, - ) -> Vec<(Vec>, Vec<[u32; DIM]>)> { + pub fn compute_convex_hulls(&self, downsampling: u32) -> Vec<(Vec, Vec<[u32; DIM]>)> { let downsampling = downsampling.max(1); self.voxel_parts .iter() @@ -1165,13 +1162,13 @@ fn compute_concavity(volume: Real, volume_ch: Real, volume0: Real) -> Real { } fn clip_mesh( - points: &[Point], + points: &[Vector], plane: &CutPlane, - positive_part: &mut Vec>, - negative_part: &mut Vec>, + positive_part: &mut Vec, + negative_part: &mut Vec, ) { for pt in points { - let d = plane.abc.dot(&pt.coords) + plane.d; + let d = plane.abc.dot(*pt) + plane.d; if d > 0.0 { positive_part.push(*pt); @@ -1185,7 +1182,7 @@ fn clip_mesh( } #[cfg(feature = "dim2")] -fn convex_hull(vertices: &[Point]) -> Vec> { +fn convex_hull(vertices: &[Vector]) -> Vec { if vertices.len() > 1 { crate::transformation::convex_hull(vertices) } else { @@ -1194,7 +1191,7 @@ fn convex_hull(vertices: &[Point]) -> Vec> { } #[cfg(feature = "dim3")] -fn convex_hull(vertices: &[Point]) -> (Vec>, Vec<[u32; DIM]>) { +fn convex_hull(vertices: &[Vector]) -> (Vec, Vec<[u32; DIM]>) { if vertices.len() > 2 { crate::transformation::convex_hull(vertices) } else { @@ -1203,7 +1200,7 @@ fn convex_hull(vertices: &[Point]) -> (Vec>, Vec<[u32; DIM]>) } #[cfg(feature = "dim2")] -fn compute_volume(polygon: &[Point]) -> Real { +fn compute_volume(polygon: &[Vector]) -> Real { if !polygon.is_empty() { crate::mass_properties::details::convex_polygon_area_and_center_of_mass(polygon).0 } else { @@ -1212,7 +1209,7 @@ fn compute_volume(polygon: &[Point]) -> Real { } #[cfg(feature = "dim3")] -fn compute_volume(mesh: &(Vec>, Vec<[u32; DIM]>)) -> Real { +fn compute_volume(mesh: &(Vec, Vec<[u32; DIM]>)) -> Real { if !mesh.0.is_empty() { crate::mass_properties::details::trimesh_signed_volume_and_center_of_mass(&mesh.0, &mesh.1) .0 diff --git a/src/transformation/voxelization/mod.rs b/src/transformation/voxelization/mod.rs index db62a229..460e62f7 100644 --- a/src/transformation/voxelization/mod.rs +++ b/src/transformation/voxelization/mod.rs @@ -69,10 +69,10 @@ //! # { //! use parry3d::transformation::voxelization::{FillMode, VoxelSet}; //! use parry3d::shape::Cuboid; -//! use nalgebra::{Point3, Vector3}; +//! use parry3d::math::Vector; //! //! // Create a simple cuboid and convert it to a triangle mesh -//! let cuboid = Cuboid::new(Vector3::new(1.0, 0.5, 0.3)); +//! let cuboid = Cuboid::new(Vector::new(1.0, 0.5, 0.3)); //! let (vertices, indices) = cuboid.to_trimesh(); //! //! // Voxelize with resolution of 10 voxels along the longest axis @@ -89,7 +89,7 @@ //! //! // Access individual voxels //! for voxel in voxels.voxels() { -//! println!("Voxel at grid position: {:?}", voxel.coords); +//! println!("Voxel at grid position: {:?}", voxel); //! println!(" World position: {:?}", voxels.get_voxel_point(voxel)); //! println!(" On surface: {}", voxel.is_on_surface); //! } @@ -103,7 +103,7 @@ //! # { //! use parry3d::transformation::voxelization::{FillMode, VoxelSet}; //! use parry3d::shape::Ball; -//! use nalgebra::Point3; +//! use parry3d::math::Vector; //! //! // Create a sphere mesh //! let ball = Ball::new(1.0); @@ -138,14 +138,14 @@ //! # #[cfg(all(feature = "dim2", feature = "f32"))] //! # { //! use parry2d::transformation::voxelization::{FillMode, VoxelSet}; -//! use nalgebra::Point2; +//! use parry2d::math::Vector; //! //! // Define a simple square as a polyline (boundary) //! let vertices = vec![ -//! Point2::origin(), -//! Point2::new(2.0, 0.0), -//! Point2::new(2.0, 2.0), -//! Point2::new(0.0, 2.0), +//! Vector::ZERO, +//! Vector::new(2.0, 0.0), +//! Vector::new(2.0, 2.0), +//! Vector::new(0.0, 2.0), //! ]; //! //! // Create index buffer for the polyline segments (closed loop) @@ -179,9 +179,9 @@ //! # { //! use parry3d::transformation::voxelization::{FillMode, VoxelSet}; //! use parry3d::shape::Cuboid; -//! use nalgebra::Vector3; +//! use parry3d::math::Vector; //! -//! let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +//! let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); //! let (vertices, indices) = cuboid.to_trimesh(); //! //! let voxels = VoxelSet::voxelize( @@ -208,9 +208,9 @@ //! # { //! use parry3d::transformation::voxelization::{FillMode, VoxelSet}; //! use parry3d::shape::Cuboid; -//! use nalgebra::Vector3; +//! use parry3d::math::Vector; //! -//! let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +//! let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); //! let (vertices, indices) = cuboid.to_trimesh(); //! //! // Enable voxel-to-primitives mapping diff --git a/src/transformation/voxelization/voxel_set.rs b/src/transformation/voxelization/voxel_set.rs index 571e5815..d7c760fd 100644 --- a/src/transformation/voxelization/voxel_set.rs +++ b/src/transformation/voxelization/voxel_set.rs @@ -18,15 +18,16 @@ use super::{FillMode, VoxelizedVolume}; use crate::bounding_volume::Aabb; -use crate::math::{Matrix, Point, Real, Vector, DIM}; +use crate::math::{ivect_to_vect, MatExt, VectorExt}; +use crate::math::{IVector, Matrix, Real, Vector, DIM}; use crate::transformation::vhacd::CutPlane; use alloc::sync::Arc; use alloc::{vec, vec::Vec}; #[cfg(feature = "dim2")] -type ConvexHull = Vec>; +type ConvexHull = Vec; #[cfg(feature = "dim3")] -type ConvexHull = (Vec>, Vec<[u32; DIM]>); +type ConvexHull = (Vec, Vec<[u32; DIM]>); /// A single voxel in a voxel grid. /// @@ -63,7 +64,7 @@ type ConvexHull = (Vec>, Vec<[u32; DIM]>); /// // Examine individual voxels /// for voxel in voxels.voxels() { /// // Grid coordinates (integer position in the voxel grid) -/// println!("Grid position: {:?}", voxel.coords); +/// println!("Grid position: {:?}", voxel); /// /// // World-space position (actual 3D coordinates) /// let world_pos = voxels.get_voxel_point(voxel); @@ -81,7 +82,7 @@ type ConvexHull = (Vec>, Vec<[u32; DIM]>); #[derive(Copy, Clone, Debug)] pub struct Voxel { /// The integer coordinates of the voxel as part of the voxel grid. - pub coords: Point, + pub coords: IVector, /// Is this voxel on the surface of the volume (i.e. not inside of it)? pub is_on_surface: bool, /// Range of indices (to be looked up into the `VoxelSet` primitive map) @@ -92,7 +93,7 @@ pub struct Voxel { impl Default for Voxel { fn default() -> Self { Self { - coords: Point::origin(), + coords: IVector::ZERO, is_on_surface: false, intersections_range: (0, 0), } @@ -136,10 +137,10 @@ impl Default for Voxel { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelSet}; /// use parry3d::shape::Cuboid; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// /// // Create and voxelize a shape -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// let voxels = VoxelSet::voxelize( @@ -190,12 +191,12 @@ impl Default for Voxel { /// ``` pub struct VoxelSet { /// The 3D origin of this voxel-set. - pub origin: Point, + pub origin: Vector, /// The scale factor between the voxel integer coordinates and their /// actual float world-space coordinates. pub scale: Real, - pub(crate) min_bb_voxels: Point, - pub(crate) max_bb_voxels: Point, + pub(crate) min_bb_voxels: IVector, + pub(crate) max_bb_voxels: IVector, pub(crate) voxels: Vec, pub(crate) intersections: Arc>, pub(crate) primitive_classes: Arc>, @@ -211,9 +212,9 @@ impl VoxelSet { /// Creates a new empty set of voxels. pub fn new() -> Self { Self { - origin: Point::origin(), - min_bb_voxels: Point::origin(), - max_bb_voxels: Vector::repeat(1).into(), + origin: Vector::ZERO, + min_bb_voxels: IVector::ZERO, + max_bb_voxels: IVector::ONE, scale: 1.0, voxels: Vec::new(), intersections: Arc::new(Vec::new()), @@ -293,7 +294,7 @@ impl VoxelSet { /// [`compute_exact_convex_hull()`]: VoxelSet::compute_exact_convex_hull /// [`compute_primitive_intersections()`]: VoxelSet::compute_primitive_intersections pub fn with_voxel_size( - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], voxel_size: Real, fill_mode: FillMode, @@ -350,10 +351,10 @@ impl VoxelSet { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelSet}; /// use parry3d::shape::Cuboid; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// /// // Create a cuboid: 2 units wide (x), 1 unit tall (y), 0.5 units deep (z) - /// let cuboid = Cuboid::new(Vector3::new(1.0, 0.5, 0.25)); + /// let cuboid = Cuboid::new(Vector::new(1.0, 0.5, 0.25)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// // Voxelize with 20 subdivisions along the longest axis (x = 2.0) @@ -383,7 +384,7 @@ impl VoxelSet { /// [`compute_exact_convex_hull()`]: VoxelSet::compute_exact_convex_hull /// [`compute_primitive_intersections()`]: VoxelSet::compute_primitive_intersections pub fn voxelize( - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], resolution: u32, fill_mode: FillMode, @@ -400,12 +401,12 @@ impl VoxelSet { } /// The minimal coordinates of the integer bounding-box of the voxels in this set. - pub fn min_bb_voxels(&self) -> Point { + pub fn min_bb_voxels(&self) -> IVector { self.min_bb_voxels } /// The maximal coordinates of the integer bounding-box of the voxels in this set. - pub fn max_bb_voxels(&self) -> Point { + pub fn max_bb_voxels(&self) -> IVector { self.max_bb_voxels } @@ -457,7 +458,7 @@ impl VoxelSet { /// The conversion formula is: /// /// ```text - /// world_position = origin + (voxel.coords + 0.5) * scale + /// world_position = origin + (voxel + 0.5) * scale /// ``` /// /// Note that we add 0.5 to get the center of the voxel rather than its corner. @@ -468,9 +469,9 @@ impl VoxelSet { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelSet}; /// use parry3d::shape::Cuboid; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// - /// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + /// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// let voxels = VoxelSet::voxelize( @@ -483,19 +484,27 @@ impl VoxelSet { /// /// // Convert grid coordinates to world coordinates /// for voxel in voxels.voxels() { - /// let grid_coords = voxel.coords; + /// let grid_coords = voxel; /// let world_coords = voxels.get_voxel_point(voxel); /// /// println!("Grid: {:?} -> World: {:?}", grid_coords, world_coords); /// } /// # } /// ``` - pub fn get_voxel_point(&self, voxel: &Voxel) -> Point { - self.get_point(na::convert(voxel.coords)) + pub fn get_voxel_point(&self, voxel: &Voxel) -> Vector { + #[cfg(feature = "dim2")] + let coords = Vector::new(voxel.coords.x as Real, voxel.coords.y as Real); + #[cfg(feature = "dim3")] + let coords = Vector::new( + voxel.coords.x as Real, + voxel.coords.y as Real, + voxel.coords.z as Real, + ); + self.get_point(coords) } - pub(crate) fn get_point(&self, voxel: Point) -> Point { - self.origin + voxel.coords * self.scale + pub(crate) fn get_point(&self, voxel: Vector) -> Vector { + self.origin + voxel * self.scale } /// Does this voxel not contain any element? @@ -525,8 +534,8 @@ impl VoxelSet { self.max_bb_voxels = self.voxels[0].coords; for p in 0..num_voxels { - self.min_bb_voxels = self.min_bb_voxels.inf(&self.voxels[p].coords); - self.max_bb_voxels = self.max_bb_voxels.sup(&self.voxels[p].coords); + self.min_bb_voxels = self.min_bb_voxels.min(self.voxels[p].coords); + self.max_bb_voxels = self.max_bb_voxels.max(self.voxels[p].coords); } } @@ -565,9 +574,9 @@ impl VoxelSet { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelSet}; /// use parry3d::shape::Cuboid; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// - /// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + /// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// // IMPORTANT: Set keep_voxel_to_primitives_map = true @@ -596,9 +605,9 @@ impl VoxelSet { #[cfg(feature = "dim2")] pub fn compute_exact_convex_hull( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], - ) -> Vec> { + ) -> Vec { self.do_compute_exact_convex_hull(points, indices) } @@ -637,9 +646,9 @@ impl VoxelSet { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelSet}; /// use parry3d::shape::Cuboid; - /// use nalgebra::Vector3; + /// use parry3d::math::Vector; /// - /// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); + /// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// // IMPORTANT: Set keep_voxel_to_primitives_map = true @@ -668,15 +677,15 @@ impl VoxelSet { #[cfg(feature = "dim3")] pub fn compute_exact_convex_hull( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], - ) -> (Vec>, Vec<[u32; DIM]>) { + ) -> (Vec, Vec<[u32; DIM]>) { self.do_compute_exact_convex_hull(points, indices) } fn do_compute_exact_convex_hull( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], ) -> ConvexHull { assert!(!self.intersections.is_empty(), @@ -707,13 +716,12 @@ impl VoxelSet { // triangle once. let prim_class = self.primitive_classes.get(*prim_id as usize).copied(); if prim_class == Some(u32::MAX) || prim_class.is_none() { - let aabb_center = - self.origin + voxel.coords.coords.map(|k| k as Real) * self.scale; + let aabb_center = self.origin + ivect_to_vect(voxel.coords) * self.scale; let aabb = - Aabb::from_half_extents(aabb_center, Vector::repeat(self.scale / 2.0)); + Aabb::from_half_extents(aabb_center, Vector::splat(self.scale / 2.0)); #[cfg(feature = "dim2")] - if let Some(seg) = aabb.clip_segment(&points[ia], &points[ib]) { + if let Some(seg) = aabb.clip_segment(points[ia], points[ib]) { surface_points.push(seg.a); surface_points.push(seg.b); } @@ -766,9 +774,9 @@ impl VoxelSet { /// `voxel_to_primitives_map = true`. pub fn compute_primitive_intersections( &self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], - ) -> Vec> { + ) -> Vec { assert!(!self.intersections.is_empty(), "Cannot compute primitive intersections voxel-to-primitives-map. Consider passing voxel_to_primitives_map = true to the voxelizer."); let mut surface_points = Vec::new(); @@ -780,8 +788,8 @@ impl VoxelSet { let intersections = &self.intersections[voxel.intersections_range.0..voxel.intersections_range.1]; for prim_id in intersections { - let aabb_center = self.origin + voxel.coords.coords.map(|k| k as Real) * self.scale; - let aabb = Aabb::from_half_extents(aabb_center, Vector::repeat(self.scale / 2.0)); + let aabb_center = self.origin + ivect_to_vect(voxel.coords) * self.scale; + let aabb = Aabb::from_half_extents(aabb_center, Vector::splat(self.scale / 2.0)); let pa = points[indices[*prim_id as usize][0] as usize]; let pb = points[indices[*prim_id as usize][1] as usize]; @@ -789,7 +797,7 @@ impl VoxelSet { let pc = points[indices[*prim_id as usize][2] as usize]; #[cfg(feature = "dim2")] - if let Some(seg) = aabb.clip_segment(&pa, &pb) { + if let Some(seg) = aabb.clip_segment(pa, pb) { surface_points.push(seg.a); surface_points.push(seg.b); } @@ -822,7 +830,7 @@ impl VoxelSet { /// regular intervals. Useful to save some computation times if an exact result isn't need. /// Use `0` to make sure no voxel is being ignored. #[cfg(feature = "dim2")] - pub fn compute_convex_hull(&self, sampling: u32) -> Vec> { + pub fn compute_convex_hull(&self, sampling: u32) -> Vec { let mut points = Vec::new(); // Grab all the points. @@ -846,7 +854,7 @@ impl VoxelSet { /// regular intervals. Useful to save some computation times if an exact result isn't need. /// Use `0` to make sure no voxel is being ignored. #[cfg(feature = "dim3")] - pub fn compute_convex_hull(&self, sampling: u32) -> (Vec>, Vec<[u32; DIM]>) { + pub fn compute_convex_hull(&self, sampling: u32) -> (Vec, Vec<[u32; DIM]>) { let mut points = Vec::new(); // Grab all the points. @@ -864,8 +872,8 @@ impl VoxelSet { } /// Gets the vertices of the given voxel. - fn map_voxel_points(&self, voxel: &Voxel, mut f: impl FnMut(Point)) { - let ijk = voxel.coords.coords.map(|e| e as Real); + fn map_voxel_points(&self, voxel: &Voxel, mut f: impl FnMut(Vector)) { + let ijk = ivect_to_vect(voxel.coords); #[cfg(feature = "dim2")] let shifts = [ @@ -895,8 +903,8 @@ impl VoxelSet { pub(crate) fn intersect( &self, plane: &CutPlane, - positive_pts: &mut Vec>, - negative_pts: &mut Vec>, + positive_pts: &mut Vec, + negative_pts: &mut Vec, sampling: u32, ) { let num_voxels = self.voxels.len(); @@ -912,7 +920,7 @@ impl VoxelSet { for v in 0..num_voxels { let voxel = self.voxels[v]; let pt = self.get_voxel_point(&voxel); - let d = plane.abc.dot(&pt.coords) + plane.d; + let d = plane.abc.dot(pt) + plane.d; // if (d >= 0.0 && d <= d0) positive_pts.push(pt); // else if (d < 0.0 && -d <= d0) negative_pts.push(pt); @@ -950,7 +958,7 @@ impl VoxelSet { for voxel in &self.voxels { let pt = self.get_voxel_point(voxel); - let d = plane.abc.dot(&pt.coords) + plane.d; + let d = plane.abc.dot(pt) + plane.d; num_positive_voxels += (d >= 0.0) as usize; } @@ -1002,7 +1010,7 @@ impl VoxelSet { for v in 0..num_voxels { let mut voxel = self.voxels[v]; let pt = self.get_voxel_point(&voxel); - let d = plane.abc.dot(&pt.coords) + plane.d; + let d = plane.abc.dot(pt) + plane.d; if d >= 0.0 { if voxel.is_on_surface || d <= d0 { @@ -1027,7 +1035,7 @@ impl VoxelSet { &self, base_index: u32, is_on_surface: bool, - ) -> (Vec>, Vec<[u32; DIM]>) { + ) -> (Vec, Vec<[u32; DIM]>) { let mut vertices = Vec::new(); let mut indices = Vec::new(); @@ -1053,29 +1061,28 @@ impl VoxelSet { (vertices, indices) } - pub(crate) fn compute_principal_axes(&self) -> Vector { + pub(crate) fn compute_principal_axes(&self) -> Vector { let num_voxels = self.voxels.len(); if num_voxels == 0 { - return Vector::zeros(); + return Vector::ZERO; } // TODO: find a way to reuse crate::utils::cov? // The difficulty being that we need to iterate through the set of // points twice. So passing an iterator to crate::utils::cov // isn't really possible. - let mut center = Point::origin(); + let mut center = Vector::ZERO; let denom = 1.0 / (num_voxels as Real); for voxel in &self.voxels { - center += voxel.coords.map(|e| e as Real).coords * denom; + center += ivect_to_vect(voxel.coords) * denom; } - let mut cov_mat = Matrix::zeros(); + let mut cov_mat = Matrix::ZERO; for voxel in &self.voxels { - let xyz = voxel.coords.map(|e| e as Real) - center; - cov_mat.syger(denom, &xyz, &xyz, 1.0); + let xyz = ivect_to_vect(voxel.coords) - center; + cov_mat += xyz.kronecker(xyz * denom); } - cov_mat.symmetric_eigenvalues() } @@ -1096,7 +1103,7 @@ impl VoxelSet { abc: Vector::ith(dim, 1.0), axis: dim as u8, d: -(self.origin[dim] + (i as Real + 0.5) * self.scale), - index: i, + index: i as u32, }; planes.push(plane); @@ -1106,7 +1113,7 @@ impl VoxelSet { } #[cfg(feature = "dim2")] -fn convex_hull(vertices: &[Point]) -> Vec> { +fn convex_hull(vertices: &[Vector]) -> Vec { if vertices.len() > 1 { crate::transformation::convex_hull(vertices) } else { @@ -1115,7 +1122,7 @@ fn convex_hull(vertices: &[Point]) -> Vec> { } #[cfg(feature = "dim3")] -fn convex_hull(vertices: &[Point]) -> (Vec>, Vec<[u32; DIM]>) { +fn convex_hull(vertices: &[Vector]) -> (Vec, Vec<[u32; DIM]>) { if vertices.len() > 2 { crate::transformation::convex_hull(vertices) } else { diff --git a/src/transformation/voxelization/voxelized_volume.rs b/src/transformation/voxelization/voxelized_volume.rs index af548e63..c3e64e22 100644 --- a/src/transformation/voxelization/voxelized_volume.rs +++ b/src/transformation/voxelization/voxelized_volume.rs @@ -17,13 +17,13 @@ // > THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. use crate::bounding_volume::Aabb; -use crate::math::{Point, Real, Vector, DIM}; +#[cfg(not(feature = "std"))] +use crate::math::ComplexField; +use crate::math::{IVector, Int, Real, Vector, DIM}; use crate::query; use crate::transformation::voxelization::{Voxel, VoxelSet}; use alloc::sync::Arc; use alloc::vec::Vec; -#[cfg(not(feature = "std"))] -use na::ComplexField; /// Controls how voxelization determines which voxels are filled vs empty. /// @@ -204,9 +204,9 @@ impl FillMode { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelValue, VoxelizedVolume}; /// use parry3d::shape::Cuboid; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// // Create a dense voxelized volume @@ -327,9 +327,9 @@ struct VoxelData { /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::transformation::voxelization::{FillMode, VoxelValue, VoxelizedVolume}; /// use parry3d::shape::Cuboid; -/// use nalgebra::Vector3; +/// use parry3d::math::Vector; /// -/// let cuboid = Cuboid::new(Vector3::new(1.0, 1.0, 1.0)); +/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0)); /// let (vertices, indices) = cuboid.to_trimesh(); /// /// let volume = VoxelizedVolume::voxelize( @@ -363,7 +363,7 @@ struct VoxelData { /// # } /// ``` pub struct VoxelizedVolume { - origin: Point, + origin: Vector, scale: Real, resolution: [u32; DIM], values: Vec, @@ -386,7 +386,7 @@ impl VoxelizedVolume { /// * `keep_voxel_to_primitives_map` - If set to `true` a map between the voxels /// and the primitives (3D triangles or 2D segments) it intersects will be computed. pub fn with_voxel_size( - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], voxel_size: Real, fill_mode: FillMode, @@ -394,7 +394,7 @@ impl VoxelizedVolume { ) -> Self { let mut result = VoxelizedVolume { resolution: [0; DIM], - origin: Point::origin(), + origin: Vector::ZERO, scale: voxel_size, values: Vec::new(), data: Vec::new(), @@ -407,9 +407,22 @@ impl VoxelizedVolume { let aabb = crate::bounding_volume::details::local_point_cloud_aabb_ref(points); result.origin = aabb.mins; - result.resolution = (aabb.extents() / voxel_size) - .map(|x| (x.ceil() as u32).max(2) + 1) - .into(); + let extents = aabb.extents() / voxel_size; + #[cfg(feature = "dim2")] + { + result.resolution = [ + (extents.x.ceil() as u32).max(2) + 1, + (extents.y.ceil() as u32).max(2) + 1, + ]; + } + #[cfg(feature = "dim3")] + { + result.resolution = [ + (extents.x.ceil() as u32).max(2) + 1, + (extents.y.ceil() as u32).max(2) + 1, + (extents.z.ceil() as u32).max(2) + 1, + ]; + } result.do_voxelize(points, indices, fill_mode, keep_voxel_to_primitives_map); result @@ -429,7 +442,7 @@ impl VoxelizedVolume { /// * `keep_voxel_to_primitives_map` - If set to `true` a map between the voxels /// and the primitives (3D triangles or 2D segments) it intersects will be computed. pub fn voxelize( - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], resolution: u32, fill_mode: FillMode, @@ -437,7 +450,7 @@ impl VoxelizedVolume { ) -> Self { let mut result = VoxelizedVolume { resolution: [0; DIM], - origin: Point::origin(), + origin: Vector::ZERO, scale: 1.0, values: Vec::new(), data: Vec::new(), @@ -490,7 +503,7 @@ impl VoxelizedVolume { fn do_voxelize( &mut self, - points: &[Point], + points: &[Vector], indices: &[[u32; DIM]], fill_mode: FillMode, keep_voxel_to_primitives_map: bool, @@ -498,10 +511,10 @@ impl VoxelizedVolume { let inv_scale = 1.0 / self.scale; self.allocate(); - let mut tri_pts = [Point::origin(); DIM]; - let box_half_size = Vector::repeat(0.5); - let mut ijk0 = Vector::repeat(0u32); - let mut ijk1 = Vector::repeat(0u32); + let mut tri_pts = [Vector::ZERO; DIM]; + let box_half_size = Vector::splat(0.5); + let mut ijk0 = IVector::ZERO; + let mut ijk1 = IVector::ZERO; let detect_self_intersections = fill_mode.detect_self_intersections(); #[cfg(feature = "dim2")] @@ -512,35 +525,41 @@ impl VoxelizedVolume { // Find the range of voxels potentially intersecting the triangle. for c in 0..DIM { let pt = points[tri[c] as usize]; - tri_pts[c] = (pt - self.origin.coords) * inv_scale; + tri_pts[c] = (pt - self.origin) * inv_scale; - let i = (tri_pts[c].x + 0.5) as u32; - let j = (tri_pts[c].y + 0.5) as u32; + let i = (tri_pts[c].x + 0.5) as Int; + let j = (tri_pts[c].y + 0.5) as Int; #[cfg(feature = "dim3")] - let k = (tri_pts[c].z + 0.5) as u32; + let k = (tri_pts[c].z + 0.5) as Int; - assert!(i < self.resolution[0] && j < self.resolution[1]); + assert!((i as u32) < self.resolution[0] && (j as u32) < self.resolution[1]); #[cfg(feature = "dim3")] - assert!(k < self.resolution[2]); + assert!((k as u32) < self.resolution[2]); #[cfg(feature = "dim2")] - let ijk = Vector::new(i, j); + let ijk = IVector::new(i, j); #[cfg(feature = "dim3")] - let ijk = Vector::new(i, j, k); + let ijk = IVector::new(i, j, k); if c == 0 { ijk0 = ijk; ijk1 = ijk; } else { - ijk0 = ijk0.inf(&ijk); - ijk1 = ijk1.sup(&ijk); + ijk0 = ijk0.min(ijk); + ijk1 = ijk1.max(ijk); } } - ijk0.apply(|e| *e = e.saturating_sub(1)); - ijk1 = ijk1 - .map(|e| e + 1) - .inf(&Point::from(self.resolution).coords); + ijk0 = (ijk0 - IVector::ONE).max(IVector::ZERO); + #[cfg(feature = "dim2")] + let resolution_vec = IVector::new(self.resolution[0] as Int, self.resolution[1] as Int); + #[cfg(feature = "dim3")] + let resolution_vec = IVector::new( + self.resolution[0] as Int, + self.resolution[1] as Int, + self.resolution[2] as Int, + ); + ijk1 = (ijk1 + IVector::ONE).min(resolution_vec); #[cfg(feature = "dim2")] let range_k = 0..1; @@ -552,11 +571,11 @@ impl VoxelizedVolume { for j in ijk0.y..ijk1.y { for k in range_k.clone() { #[cfg(feature = "dim2")] - let pt = Point::new(i as Real, j as Real); + let pt = Vector::new(i as Real, j as Real); #[cfg(feature = "dim3")] - let pt = Point::new(i as Real, j as Real, k as Real); + let pt = Vector::new(i as Real, j as Real, k as Real); - let id = self.voxel_index(i, j, k); + let id = self.voxel_index(i as u32, j as u32, k as u32); let value = &mut self.values[id as usize]; let data = &mut self.data[id as usize]; @@ -581,7 +600,7 @@ impl VoxelizedVolume { *value = VoxelValue::PrimitiveOnSurface; } } else if let Some(params) = - aabb.clip_line_parameters(&tri_pts[0], &(tri_pts[1] - tri_pts[0])) + aabb.clip_line_parameters(tri_pts[0], tri_pts[1] - tri_pts[0]) { let eps = 0.0; // -1.0e-6; @@ -1017,7 +1036,7 @@ impl VoxelizedVolume { /// This conversion is extremely naive: it will simply collect all the 12 triangles forming /// the faces of each voxel. No actual boundary extraction is done. #[cfg(feature = "dim3")] - pub fn to_trimesh(&self, value: VoxelValue) -> (Vec>, Vec<[u32; DIM]>) { + pub fn to_trimesh(&self, value: VoxelValue) -> (Vec, Vec<[u32; DIM]>) { let mut vertices = Vec::new(); let mut indices = Vec::new(); @@ -1085,9 +1104,9 @@ impl From for VoxelSet { let id = shape.voxel_index(i, j, k) as usize; let value = shape.values[id]; #[cfg(feature = "dim2")] - let coords = Point::new(i, j); + let coords = IVector::new(i as Int, j as Int); #[cfg(feature = "dim3")] - let coords = Point::new(i, j, k); + let coords = IVector::new(i as Int, j as Int, k as Int); if value == VoxelValue::PrimitiveInsideSurface { let voxel = Voxel { @@ -1141,7 +1160,7 @@ impl From for VoxelSet { fn traceRay( mesh: &RaycastMesh, start: Real, - dir: &Vector, + dir: Vector, inside_count: &mut u32, outside_count: &mut u32, ) { @@ -1191,12 +1210,12 @@ for i in 0..i0 { let mut outside_count = 0; let directions = [ - Vector::x(), - -Vector::x(), - Vector::y(), - -Vector::y(), - Vector::z(), - -Vector::z(), + Vector::X, + -Vector::X, + Vector::Y, + -Vector::Y, + Vector::Z, + -Vector::Z, ]; for r in 0..6 { diff --git a/src/utils/array2.rs b/src/utils/array2.rs new file mode 100644 index 00000000..70a220a5 --- /dev/null +++ b/src/utils/array2.rs @@ -0,0 +1,127 @@ +use alloc::{vec, vec::Vec}; +use core::ops::{Index, IndexMut}; +use num::{Bounded, Zero}; + +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[derive(Debug, Clone)] +/// A column-major 2D array. +pub struct Array2 { + nrows: usize, + ncols: usize, + data: Vec, +} + +impl Array2 { + /// Creates a new 2D array from the given dimensions and data. + #[inline] + pub fn new(nrows: usize, ncols: usize, data: Vec) -> Self { + assert_eq!(data.len(), ncols * nrows); + Array2 { ncols, nrows, data } + } + + /// Creates a new 2D array filled with copies of `elt`. + #[inline] + pub fn repeat(nrows: usize, ncols: usize, elt: T) -> Self + where + T: Copy, + { + Array2 { + nrows, + ncols, + data: vec![elt; ncols * nrows], + } + } + + /// Creates a new 2D array filled with zeros. + #[inline] + pub fn zeros(nrows: usize, ncols: usize) -> Self + where + T: Copy + Zero, + { + Self::repeat(nrows, ncols, T::zero()) + } + + /// Computes the flat index for the given row and column. + #[inline] + pub fn flat_index(&self, i: usize, j: usize) -> usize { + i + j * self.nrows + } + + /// Returns a reference to the underlying data. + #[inline] + pub fn data(&self) -> &[T] { + &self.data + } + + /// Returns a mutable reference to the underlying data. + #[inline] + pub fn data_mut(&mut self) -> &mut [T] { + &mut self.data + } + + /// Returns the number of rows. + #[inline] + pub fn nrows(&self) -> usize { + self.nrows + } + + /// Returns the number of columns. + #[inline] + pub fn ncols(&self) -> usize { + self.ncols + } + + /// Returns the minimum element of the array. + #[inline] + pub fn min(&self) -> T + where + T: Copy + Bounded + PartialOrd, + { + self.data + .iter() + .fold(Bounded::max_value(), |a, b| if a < *b { a } else { *b }) + } + + /// Returns the maximum element of the array. + #[inline] + pub fn max(&self) -> T + where + T: Copy + Bounded + PartialOrd, + { + self.data + .iter() + .fold(Bounded::min_value(), |a, b| if a > *b { a } else { *b }) + } + + /// Creates a new 2D array from a function. + #[inline] + pub fn from_fn(nrows: usize, ncols: usize, f: impl Fn(usize, usize) -> T) -> Self { + let mut data = Vec::with_capacity(ncols * nrows); + + for j in 0..ncols { + for i in 0..nrows { + data.push(f(i, j)); + } + } + data.shrink_to_fit(); + Self { nrows, ncols, data } + } +} + +impl Index<(usize, usize)> for Array2 { + type Output = T; + + #[inline] + fn index(&self, index: (usize, usize)) -> &Self::Output { + let fid = self.flat_index(index.0, index.1); + &self.data[fid] + } +} + +impl IndexMut<(usize, usize)> for Array2 { + #[inline] + fn index_mut(&mut self, index: (usize, usize)) -> &mut Self::Output { + let fid = self.flat_index(index.0, index.1); + &mut self.data[fid] + } +} diff --git a/src/utils/as_bytes.rs b/src/utils/as_bytes.rs index 9b172139..d47e4050 100644 --- a/src/utils/as_bytes.rs +++ b/src/utils/as_bytes.rs @@ -1,8 +1,7 @@ use core::mem; use core::slice; -use na::{Point2, Point3, Vector2, Vector3}; -use simba::scalar::RealField; +use crate::math::{Real, Vector2, Vector3}; /// Trait that transforms thing to a slice of u8. pub trait AsBytes { @@ -10,22 +9,20 @@ pub trait AsBytes { fn as_bytes(&self) -> &[u8]; } -macro_rules! generic_as_bytes_impl( - ($T: ident, $dimension: expr) => ( - impl AsBytes for $T { +macro_rules! as_bytes_impl( + ($T: ty, $dimension: expr) => ( + impl AsBytes for $T { #[inline(always)] fn as_bytes(&self) -> &[u8] { unsafe { - slice::from_raw_parts(self as *const $T as *const u8, mem::size_of::() * $dimension) + slice::from_raw_parts(self as *const $T as *const u8, mem::size_of::() * $dimension) } } } ) ); -generic_as_bytes_impl!(Vector2, 2); -generic_as_bytes_impl!(Point2, 2); -generic_as_bytes_impl!(Vector3, 3); -generic_as_bytes_impl!(Point3, 3); +as_bytes_impl!(Vector2, 2); +as_bytes_impl!(Vector3, 3); // TODO: implement for all `T: Copy` instead? diff --git a/src/utils/ccw_face_normal.rs b/src/utils/ccw_face_normal.rs index 182ad5c1..16ac52f4 100644 --- a/src/utils/ccw_face_normal.rs +++ b/src/utils/ccw_face_normal.rs @@ -1,16 +1,15 @@ -use crate::math::{Point, Real, Vector}; -use na::Unit; +use crate::math::Vector; /// Computes the direction pointing toward the right-hand-side of an oriented segment. /// /// Returns `None` if the segment is degenerate. #[inline] #[cfg(feature = "dim2")] -pub fn ccw_face_normal(pts: [&Point; 2]) -> Option>> { +pub fn ccw_face_normal(pts: [Vector; 2]) -> Option { let ab = pts[1] - pts[0]; let res = Vector::new(ab[1], -ab[0]); - Unit::try_new(res, crate::math::DEFAULT_EPSILON) + res.try_normalize() } /// Computes the normal of a counter-clock-wise triangle. @@ -18,10 +17,10 @@ pub fn ccw_face_normal(pts: [&Point; 2]) -> Option>> { /// Returns `None` if the triangle is degenerate. #[inline] #[cfg(feature = "dim3")] -pub fn ccw_face_normal(pts: [&Point; 3]) -> Option>> { +pub fn ccw_face_normal(pts: [Vector; 3]) -> Option { let ab = pts[1] - pts[0]; let ac = pts[2] - pts[0]; - let res = ab.cross(&ac); + let res = ab.cross(ac); - Unit::try_new(res, crate::math::DEFAULT_EPSILON) + res.try_normalize() } diff --git a/src/utils/center.rs b/src/utils/center.rs index 58095fb2..44e92bda 100644 --- a/src/utils/center.rs +++ b/src/utils/center.rs @@ -1,5 +1,4 @@ -use crate::math::{Point, Real}; -use na; +use crate::math::{Real, Vector}; /// Computes the geometric center (centroid) of a set of points. /// @@ -12,7 +11,7 @@ use na; /// /// # Returns /// -/// The geometric center as a `Point`. +/// The geometric center as a `Vector`. /// /// # Panics /// @@ -25,13 +24,13 @@ use na; /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::center; -/// use parry2d::math::Point; +/// use parry2d::math::Vector; /// /// let points = vec![ -/// Point::new(0.0, 0.0), -/// Point::new(2.0, 0.0), -/// Point::new(2.0, 2.0), -/// Point::new(0.0, 2.0), +/// Vector::new(0.0, 0.0), +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// /// let c = center(&points); @@ -47,12 +46,12 @@ use na; /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { /// use parry3d::utils::center; -/// use parry3d::math::Point; +/// use parry3d::math::Vector; /// /// let points = vec![ -/// Point::new(0.0, 0.0, 0.0), -/// Point::new(4.0, 0.0, 0.0), -/// Point::new(0.0, 4.0, 0.0), +/// Vector::new(0.0, 0.0, 0.0), +/// Vector::new(4.0, 0.0, 0.0), +/// Vector::new(0.0, 4.0, 0.0), /// ]; /// /// let c = center(&points); @@ -64,14 +63,14 @@ use na; /// # } /// ``` /// -/// ## Single Point +/// ## Single Vector /// /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::center; -/// use parry2d::math::Point; +/// use parry2d::math::Vector; /// -/// let points = vec![Point::new(5.0, 10.0)]; +/// let points = vec![Vector::new(5.0, 10.0)]; /// let c = center(&points); /// /// // The center of a single point is the point itself @@ -79,19 +78,19 @@ use na; /// # } /// ``` #[inline] -pub fn center(pts: &[Point]) -> Point { +pub fn center(pts: &[Vector]) -> Vector { assert!( !pts.is_empty(), "Cannot compute the center of less than 1 point." ); - let denom: Real = na::convert::(1.0 / (pts.len() as f64)); + let denom: Real = 1.0 / (pts.len() as Real); let mut piter = pts.iter(); let mut res = *piter.next().unwrap() * denom; for pt in piter { - res += pt.coords * denom; + res += pt * denom; } res diff --git a/src/utils/cleanup.rs b/src/utils/cleanup.rs index 18d51002..7b38b0fd 100644 --- a/src/utils/cleanup.rs +++ b/src/utils/cleanup.rs @@ -1,9 +1,9 @@ -use crate::math::{Point, Real}; +use crate::math::Vector; use alloc::vec::Vec; use core::iter; /// Given an index buffer, remove from `points` every point that is not indexed. -pub fn remove_unused_points(points: &mut Vec>, idx: &mut [[u32; 3]]) { +pub fn remove_unused_points(points: &mut Vec, idx: &mut [[u32; 3]]) { let mut used: Vec = iter::repeat_n(false, points.len()).collect(); let mut remap: Vec = (0..points.len()).collect(); let used = &mut used[..]; diff --git a/src/utils/cov.rs b/src/utils/cov.rs index b52d925a..b0c89661 100644 --- a/src/utils/cov.rs +++ b/src/utils/cov.rs @@ -1,20 +1,21 @@ -use crate::math::{Matrix, Point, Real}; +use crate::math::{Matrix, Real, Vector, VectorExt}; /// Computes the covariance matrix of a set of points. -pub fn cov(pts: &[Point]) -> Matrix { +pub fn cov(pts: &[Vector]) -> Matrix { center_cov(pts).1 } /// Computes the center and the covariance matrix of a set of points. -pub fn center_cov(pts: &[Point]) -> (Point, Matrix) { +pub fn center_cov(pts: &[Vector]) -> (Vector, Matrix) { let center = crate::utils::center(pts); - let mut cov: Matrix = na::zero(); + let mut cov = Matrix::ZERO; let normalizer: Real = 1.0 / (pts.len() as Real); for p in pts.iter() { let cp = *p - center; - // NOTE: this is more numerically stable than using cov.syger. - cov += cp * (cp * normalizer).transpose(); + let cp_scaled = cp * normalizer; + // Compute outer product: cp * cp_scaled^T + cov += cp.kronecker(cp_scaled); } (center, cov) diff --git a/src/utils/eigen2.rs b/src/utils/eigen2.rs new file mode 100644 index 00000000..3f26e31b --- /dev/null +++ b/src/utils/eigen2.rs @@ -0,0 +1,7 @@ +//! Re-export of SymmetricEigen2 from glamx. + +#[cfg(feature = "f32")] +pub use glamx::SymmetricEigen2; + +#[cfg(feature = "f64")] +pub use glamx::DSymmetricEigen2 as SymmetricEigen2; diff --git a/src/utils/eigen3.rs b/src/utils/eigen3.rs new file mode 100644 index 00000000..70202b08 --- /dev/null +++ b/src/utils/eigen3.rs @@ -0,0 +1,7 @@ +//! Re-export of SymmetricEigen3 from glamx. + +#[cfg(feature = "f32")] +pub use glamx::SymmetricEigen3; + +#[cfg(feature = "f64")] +pub use glamx::DSymmetricEigen3 as SymmetricEigen3; diff --git a/src/utils/interval.rs b/src/utils/interval.rs index 4042eac6..63abebe4 100644 --- a/src/utils/interval.rs +++ b/src/utils/interval.rs @@ -2,8 +2,9 @@ // Ulrich W. Kulisch use alloc::{vec, vec::Vec}; use core::ops::{Add, AddAssign, Div, Mul, MulAssign, Neg, Sub, SubAssign}; -use na::{RealField, SimdPartialOrd}; use num::{One, Zero}; +use num_traits::AsPrimitive; +use simba::scalar::RealField; /// A derivable valued function which can be bounded on intervals. pub trait IntervalFunction { @@ -19,7 +20,7 @@ pub trait IntervalFunction { /// /// The results are stored in `results`. The `candidate` buffer is just a workspace buffer used /// to avoid allocations. -pub fn find_root_intervals_to( +pub fn find_root_intervals_to( function: &impl IntervalFunction, init: Interval, min_interval_width: T, @@ -27,7 +28,9 @@ pub fn find_root_intervals_to( max_recursions: usize, results: &mut Vec>, candidates: &mut Vec<(Interval, usize)>, -) { +) where + f64: AsPrimitive, +{ candidates.clear(); let push_candidate = |candidate, @@ -79,7 +82,7 @@ pub fn find_root_intervals_to( let prev_width = candidate.width(); for new_candidate in new_candidates.iter().flatten() { - if new_candidate.width() > prev_width * na::convert(0.75) { + if new_candidate.width() > prev_width * 0.75.as_() { // If the new candidate range is still quite big compared to // new candidate, split it to accelerate the search. let [a, b] = new_candidate.split(); @@ -93,13 +96,16 @@ pub fn find_root_intervals_to( } /// Execute the Interval Newton Method to isolate all the roots of the given nonlinear function. -pub fn find_root_intervals( +pub fn find_root_intervals( function: &impl IntervalFunction, init: Interval, min_interval_width: T, min_image_width: T, max_recursions: usize, -) -> Vec> { +) -> Vec> +where + f64: AsPrimitive, +{ let mut results = vec![]; let mut candidates = vec![]; find_root_intervals_to( @@ -163,9 +169,9 @@ impl Interval { #[must_use] pub fn midpoint(self) -> T where - T: RealField + Copy, + T: Copy + Add + Div + One, { - let two: T = na::convert(2.0); + let two = T::one() + T::one(); (self.0 + self.1) / two } @@ -173,7 +179,7 @@ impl Interval { #[must_use] pub fn split(self) -> [Self; 2] where - T: RealField + Copy, + T: Copy + Add + Div + One, { let mid = self.midpoint(); [Interval(self.0, mid), Interval(mid, self.1)] @@ -200,9 +206,9 @@ impl Interval { #[must_use] pub fn intersect(self, rhs: Self) -> Option where - T: PartialOrd + SimdPartialOrd, // TODO: it is weird to have both. + T: PartialOrd + RealField, { - let result = Interval(self.0.simd_max(rhs.0), self.1.simd_min(rhs.1)); + let result = Interval(self.0.max(rhs.0), self.1.min(rhs.1)); if result.0 > result.1 { // The range is invalid if there is no intersection. @@ -216,7 +222,8 @@ impl Interval { #[must_use] pub fn sin_cos(self) -> (Self, Self) where - T: RealField + Copy, + T: Copy + RealField, + f64: AsPrimitive, { (self.sin(), self.cos()) } @@ -225,21 +232,27 @@ impl Interval { #[must_use] pub fn sin(self) -> Self where - T: RealField + Copy, + T: Copy + RealField, + f64: AsPrimitive, { - if self.width() >= T::two_pi() { - Interval(-T::one(), T::one()) + let two_pi = T::two_pi(); + let one = T::one(); + let pi = T::pi(); + let frac_pi_2 = T::frac_pi_2(); + + if self.width() >= two_pi { + Interval(one * (-1.0).as_(), one) } else { let sin0 = self.0.sin(); let sin1 = self.1.sin(); let mut result = Interval::sort(sin0, sin1); - let orig = (self.0 / T::two_pi()).floor() * T::two_pi(); - let crit = [orig + T::frac_pi_2(), orig + T::pi() + T::frac_pi_2()]; - let crit_vals = [T::one(), -T::one()]; + let orig = (self.0 / two_pi).floor() * two_pi; + let crit = [orig + frac_pi_2, orig + pi + frac_pi_2]; + let crit_vals = [one, one * (-1.0).as_()]; for i in 0..2 { - if self.contains(crit[i]) || self.contains(crit[i] + T::two_pi()) { + if self.contains(crit[i]) || self.contains(crit[i] + two_pi) { result = result.enclose(crit_vals[i]) } } @@ -252,21 +265,26 @@ impl Interval { #[must_use] pub fn cos(self) -> Self where - T: RealField + Copy, + T: Copy + RealField, + f64: AsPrimitive, { - if self.width() >= T::two_pi() { - Interval(-T::one(), T::one()) + let two_pi = T::two_pi(); + let one = T::one(); + let pi = T::pi(); + + if self.width() >= two_pi { + Interval(one * (-1.0).as_(), one) } else { let cos0 = self.0.cos(); let cos1 = self.1.cos(); let mut result = Interval::sort(cos0, cos1); - let orig = (self.0 / T::two_pi()).floor() * T::two_pi(); - let crit = [orig, orig + T::pi()]; - let crit_vals = [T::one(), -T::one()]; + let orig = (self.0 / two_pi).floor() * two_pi; + let crit = [orig, orig + pi]; + let crit_vals = [one, one * (-1.0).as_()]; for i in 0..2 { - if self.contains(crit[i]) || self.contains(crit[i] + T::two_pi()) { + if self.contains(crit[i]) || self.contains(crit[i] + two_pi) { result = result.enclose(crit_vals[i]) } } @@ -333,8 +351,8 @@ where impl> Mul> for Interval where - T: Copy + PartialOrd + Zero, - >::Output: SimdPartialOrd, + T: Copy + PartialOrd + Zero + RealField, + >::Output: PartialOrd + RealField, { type Output = Interval<>::Output>; @@ -354,7 +372,7 @@ where if b2 <= T::zero() { Interval(a2 * b1, a1 * b1) } else if b1 < T::zero() { - Interval((a1 * b2).simd_min(b2 * b1), (a1 * b1).simd_max(a2 * b2)) + Interval((a1 * b2).min(b2 * b1), (a1 * b1).max(a2 * b2)) } else { Interval(a1 * b2, a2 * b2) } @@ -370,8 +388,9 @@ where impl> Div> for Interval where - T: RealField + Copy, - >::Output: SimdPartialOrd, + T: Copy + One + Zero + PartialOrd + 'static, + >::Output: PartialOrd + RealField, + f64: AsPrimitive, { type Output = ( Interval<>::Output>, @@ -380,6 +399,8 @@ where fn div(self, rhs: Self) -> Self::Output { let infinity = T::one() / T::zero(); + let neg_one: T = (-1.0).as_(); + let neg_infinity = neg_one / T::zero(); let Interval(a1, a2) = self; let Interval(b1, b2) = rhs; @@ -392,19 +413,19 @@ where (Interval(a2 / b1, infinity), None) } else if b1 != T::zero() { ( - Interval(-infinity, a2 / b2), + Interval(neg_infinity, a2 / b2), Some(Interval(a2 / b1, infinity)), ) } else { - (Interval(-infinity, a2 / b2), None) + (Interval(neg_infinity, a2 / b2), None) } } else if a1 <= T::zero() { - (Interval(-infinity, infinity), None) + (Interval(neg_infinity, infinity), None) } else if b2 == T::zero() { - (Interval(-infinity, a1 / b1), None) + (Interval(neg_infinity, a1 / b1), None) } else if b1 != T::zero() { ( - Interval(-infinity, a1 / b1), + Interval(neg_infinity, a1 / b1), Some(Interval(a1 / b2, infinity)), ) } else { @@ -444,8 +465,8 @@ impl> SubAssign> for Interval { impl> MulAssign> for Interval where - T: Copy + PartialOrd + Zero, - >::Output: SimdPartialOrd, + T: Copy + PartialOrd + Zero + RealField, + >::Output: PartialOrd + RealField, { fn mul_assign(&mut self, rhs: Interval) { *self = *self * rhs; @@ -474,7 +495,6 @@ where #[cfg(test)] mod test { use super::{Interval, IntervalFunction}; - use na::RealField; #[test] fn roots_sin() { @@ -495,23 +515,20 @@ mod test { } let function = Sin; - let roots = super::find_root_intervals( - &function, - Interval(0.0, f32::two_pi()), - 1.0e-5, - 1.0e-5, - 100, - ); + let two_pi = core::f32::consts::TAU; + let roots = + super::find_root_intervals(&function, Interval(0.0, two_pi), 1.0e-5, 1.0e-5, 100); assert_eq!(roots.len(), 3); } #[test] fn interval_sin_cos() { - let a = f32::pi() / 6.0; - let b = f32::pi() / 2.0 + f32::pi() / 6.0; - let c = f32::pi() + f32::pi() / 6.0; - let d = f32::pi() + f32::pi() / 2.0 + f32::pi() / 6.0; - let shifts = [0.0, f32::two_pi() * 100.0, -f32::two_pi() * 100.0]; + let a = core::f32::consts::PI / 6.0; + let b = core::f32::consts::FRAC_PI_2 + core::f32::consts::PI / 6.0; + let c = core::f32::consts::PI + core::f32::consts::PI / 6.0; + let d = core::f32::consts::PI + core::f32::consts::FRAC_PI_2 + core::f32::consts::PI / 6.0; + let two_pi = core::f32::consts::TAU; + let shifts = [0.0, two_pi * 100.0, -two_pi * 100.0]; for shift in shifts.iter() { // Test sinus. diff --git a/src/utils/isometry_ops.rs b/src/utils/isometry_ops.rs index 7d5fe817..1a4acb76 100644 --- a/src/utils/isometry_ops.rs +++ b/src/utils/isometry_ops.rs @@ -1,60 +1,45 @@ -use crate::math::{Isometry, Point, Real, Vector}; -use na::Unit; - -#[cfg(feature = "simd-is-enabled")] -use crate::math::SimdReal; +use crate::math::{Pose, Vector}; /// Extra operations with isometries. -pub trait IsometryOps { +pub trait PoseOps { /// Transform a vector by the absolute value of the homogeneous matrix /// equivalent to `self`. - fn absolute_transform_vector(&self, v: &Vector) -> Vector; + // TODO: move this to the rotation + fn absolute_transform_vector(&self, v: Vector) -> Vector; } -impl IsometryOps for Isometry { +impl PoseOps for Pose { #[inline] - fn absolute_transform_vector(&self, v: &Vector) -> Vector { - self.rotation.to_rotation_matrix().into_inner().abs() * *v - } -} - -#[cfg(feature = "simd-is-enabled")] -impl IsometryOps for Isometry { - #[inline] - fn absolute_transform_vector(&self, v: &Vector) -> Vector { - use na::SimdComplexField; - self.rotation - .to_rotation_matrix() - .into_inner() - .map(|e| e.simd_abs()) - * *v + fn absolute_transform_vector(&self, v: Vector) -> Vector { + #[cfg(feature = "dim3")] + { + let rot_matrix = crate::math::Matrix::from_quat(self.rotation); + rot_matrix.abs() * v + } + #[cfg(feature = "dim2")] + { + let rot_matrix = self.rotation.to_mat(); + rot_matrix.abs() * v + } } } -/// Various operations usable with `Option` and `Option<&Isometry>` +/// Various operations usable with `Option` and `Option<&Pose>` /// where `None` is assumed to be equivalent to the identity. -pub trait IsometryOpt { +pub trait PoseOpt { /// Computes `self.inverse() * rhs`. - fn inv_mul(self, rhs: &Isometry) -> Isometry; + fn inv_mul(self, rhs: &Pose) -> Pose; /// Computes `rhs * self`. - fn prepend_to(self, rhs: &Isometry) -> Isometry; + fn prepend_to(self, rhs: &Pose) -> Pose; /// Computes `self * p`. - fn transform_point(self, p: &Point) -> Point; - /// Computes `self * v`. - fn transform_vector(self, v: &Vector) -> Vector; - /// Computes `self * v`. - fn transform_unit_vector(self, v: &Unit>) -> Unit>; + fn transform_point(self, p: Vector) -> Vector; /// Computes `self.inverse() * p`. - fn inverse_transform_point(self, p: &Point) -> Point; - /// Computes `self.inverse() * v`. - fn inverse_transform_vector(self, v: &Vector) -> Vector; - /// Computes `self.inverse() * v`. - fn inverse_transform_unit_vector(self, v: &Unit>) -> Unit>; + fn inverse_transform_point(self, p: Vector) -> Vector; } -impl IsometryOpt for Option<&Isometry> { +impl PoseOpt for Option<&Pose> { #[inline] - fn inv_mul(self, rhs: &Isometry) -> Isometry { + fn inv_mul(self, rhs: &Pose) -> Pose { if let Some(iso) = self { iso.inv_mul(rhs) } else { @@ -63,72 +48,36 @@ impl IsometryOpt for Option<&Isometry> { } #[inline] - fn prepend_to(self, rhs: &Isometry) -> Isometry { + fn prepend_to(self, rhs: &Pose) -> Pose { if let Some(iso) = self { - rhs * iso + *rhs * *iso } else { *rhs } } #[inline] - fn transform_point(self, p: &Point) -> Point { + fn transform_point(self, p: Vector) -> Vector { if let Some(iso) = self { iso * p } else { - *p - } - } - - #[inline] - fn transform_vector(self, v: &Vector) -> Vector { - if let Some(iso) = self { - iso * v - } else { - *v - } - } - - #[inline] - fn transform_unit_vector(self, v: &Unit>) -> Unit> { - if let Some(iso) = self { - iso * v - } else { - *v + p } } #[inline] - fn inverse_transform_point(self, p: &Point) -> Point { + fn inverse_transform_point(self, p: Vector) -> Vector { if let Some(iso) = self { iso.inverse_transform_point(p) } else { - *p - } - } - - #[inline] - fn inverse_transform_vector(self, v: &Vector) -> Vector { - if let Some(iso) = self { - iso.inverse_transform_vector(v) - } else { - *v - } - } - - #[inline] - fn inverse_transform_unit_vector(self, v: &Unit>) -> Unit> { - if let Some(iso) = self { - iso.inverse_transform_unit_vector(v) - } else { - *v + p } } } -impl IsometryOpt for Option> { +impl PoseOpt for Option { #[inline] - fn inv_mul(self, rhs: &Isometry) -> Isometry { + fn inv_mul(self, rhs: &Pose) -> Pose { if let Some(iso) = self { iso.inv_mul(rhs) } else { @@ -137,65 +86,29 @@ impl IsometryOpt for Option> { } #[inline] - fn prepend_to(self, rhs: &Isometry) -> Isometry { + fn prepend_to(self, rhs: &Pose) -> Pose { if let Some(iso) = self { - rhs * iso + *rhs * iso } else { *rhs } } #[inline] - fn transform_point(self, p: &Point) -> Point { + fn transform_point(self, p: Vector) -> Vector { if let Some(iso) = self { iso * p } else { - *p - } - } - - #[inline] - fn transform_vector(self, v: &Vector) -> Vector { - if let Some(iso) = self { - iso * v - } else { - *v + p } } #[inline] - fn transform_unit_vector(self, v: &Unit>) -> Unit> { - if let Some(iso) = self { - iso * v - } else { - *v - } - } - - #[inline] - fn inverse_transform_point(self, p: &Point) -> Point { + fn inverse_transform_point(self, p: Vector) -> Vector { if let Some(iso) = self { iso.inverse_transform_point(p) } else { - *p - } - } - - #[inline] - fn inverse_transform_vector(self, v: &Vector) -> Vector { - if let Some(iso) = self { - iso.inverse_transform_vector(v) - } else { - *v - } - } - - #[inline] - fn inverse_transform_unit_vector(self, v: &Unit>) -> Unit> { - if let Some(iso) = self { - iso.inverse_transform_unit_vector(v) - } else { - *v + p } } } diff --git a/src/utils/median.rs b/src/utils/median.rs index 96976bac..4ffbb117 100644 --- a/src/utils/median.rs +++ b/src/utils/median.rs @@ -1,5 +1,4 @@ use crate::math::Real; -use na; /// Computes the median of a set of values. /// @@ -93,7 +92,7 @@ pub fn median(vals: &mut [Real]) -> Real { let n = vals.len(); if n.is_multiple_of(2) { - (vals[n / 2 - 1] + vals[n / 2]) / na::convert::(2.0) + (vals[n / 2 - 1] + vals[n / 2]) / 2.0 } else { vals[n / 2] } diff --git a/src/utils/mod.rs b/src/utils/mod.rs index 165a20eb..b1d05f65 100644 --- a/src/utils/mod.rs +++ b/src/utils/mod.rs @@ -3,11 +3,15 @@ pub use self::ccw_face_normal::ccw_face_normal; pub use self::center::center; +#[cfg(feature = "alloc")] +pub use self::array2::Array2; #[cfg(feature = "dim3")] #[cfg(feature = "alloc")] pub use self::cleanup::remove_unused_points; +pub use self::eigen2::SymmetricEigen2; +pub use self::eigen3::SymmetricEigen3; pub(crate) use self::inv::inv; -pub use self::isometry_ops::{IsometryOps, IsometryOpt}; +pub use self::isometry_ops::{PoseOps, PoseOpt}; pub use self::median::median; pub use self::point_cloud_support_point::{ point_cloud_support_point, point_cloud_support_point_id, @@ -19,6 +23,7 @@ pub use self::sdp_matrix::{SdpMatrix2, SdpMatrix3}; pub use self::vec_map::VecMap; pub use self::as_bytes::AsBytes; +#[cfg(any(feature = "alloc", feature = "std"))] pub(crate) use self::consts::*; pub use self::cov::{center_cov, cov}; pub use self::hashable_partial_eq::HashablePartialEq; @@ -35,14 +40,18 @@ pub(crate) use self::spade::sanitize_spade_point; pub(crate) use self::wops::{WBasis, WCross, WSign}; #[cfg(feature = "simd-is-enabled")] +#[allow(unused_imports)] pub(crate) use self::wops::simd_swap; +#[cfg(feature = "alloc")] +mod array2; mod as_bytes; mod ccw_face_normal; mod center; #[cfg(feature = "dim3")] #[cfg(feature = "alloc")] mod cleanup; +#[cfg(any(feature = "alloc", feature = "std"))] mod consts; mod cov; #[cfg(feature = "enhanced-determinism")] @@ -71,5 +80,7 @@ mod sorted_pair; mod spade; mod wops; +mod eigen2; +mod eigen3; #[cfg(feature = "alloc")] mod vec_map; diff --git a/src/utils/morton.rs b/src/utils/morton.rs index 52991f95..49a27864 100644 --- a/src/utils/morton.rs +++ b/src/utils/morton.rs @@ -3,12 +3,12 @@ // From https://github.com/DGriffin91/obvhs/tree/main/src/ploc/morton.rs // MIT/Apache 2 license. -use crate::math::Vector; - //--------------------------------------------------- // --- 21 bit resolution per channel morton curve --- //--------------------------------------------------- +use crate::math::Vector; + #[inline] fn split_by_3_u64(a: u32) -> u64 { let mut x = a as u64 & 0x1fffff; // we only look at the first 21 bits @@ -30,8 +30,13 @@ fn morton_encode_u64(x: u32, y: u32, z: u32) -> u64 { #[inline] /// Encode a 3D position into a u64 morton value. /// Input should be 0.0..=1.0 -pub fn morton_encode_u64_unorm(p: Vector) -> u64 { - let p = p * (1 << 21) as f64; +pub fn morton_encode_u64_unorm(p: Vector) -> u64 { + #[cfg(feature = "dim2")] + #[cfg_attr(feature = "f64", expect(clippy::unnecessary_cast))] + let p = glamx::DVec2::new(p.x as f64, p.y as f64) * (1 << 21) as f64; + #[cfg(feature = "dim3")] + #[cfg_attr(feature = "f64", expect(clippy::unnecessary_cast))] + let p = glamx::DVec3::new(p.x as f64, p.y as f64, p.z as f64) * (1 << 21) as f64; #[cfg(feature = "dim2")] return morton_encode_u64(p.x as u32, p.y as u32, 0); diff --git a/src/utils/obb.rs b/src/utils/obb.rs index 5e6a816b..bb4db543 100644 --- a/src/utils/obb.rs +++ b/src/utils/obb.rs @@ -1,11 +1,11 @@ -use crate::math::{Isometry, Point, Real, Rotation, Translation, Vector, DIM}; +use crate::math::{MatExt, Pose, Real, Rotation, Vector, DIM}; use crate::shape::Cuboid; /// Computes an oriented bounding box for the given set of points. /// /// The returned OBB is not guaranteed to be the smallest enclosing OBB. /// Though it should be a pretty good on for most purposes. -pub fn obb(pts: &[Point]) -> (Isometry, Cuboid) { +pub fn obb(pts: &[Vector]) -> (Pose, Cuboid) { let cov = crate::utils::cov(pts); let mut eigv = cov.symmetric_eigen().eigenvectors; @@ -13,24 +13,31 @@ pub fn obb(pts: &[Point]) -> (Isometry, Cuboid) { eigv = -eigv; } - let mut mins = Vector::repeat(Real::MAX); - let mut maxs = Vector::repeat(-Real::MAX); + let mut mins = Vector::splat(Real::MAX); + let mut maxs = Vector::splat(-Real::MAX); for pt in pts { for i in 0..DIM { - let dot = eigv.column(i).dot(&pt.coords); + let dot = eigv.col(i).dot(*pt); mins[i] = mins[i].min(dot); maxs[i] = maxs[i].max(dot); } } #[cfg(feature = "dim2")] - let rot = Rotation::from_rotation_matrix(&na::Rotation2::from_matrix_unchecked(eigv)); + let rot = { + // Extract rotation from 2x2 matrix + // Matrix rows are [cos θ, -sin θ; sin θ, cos θ] + Rotation::from_matrix_unchecked(eigv) + }; #[cfg(feature = "dim3")] - let rot = Rotation::from_rotation_matrix(&na::Rotation3::from_matrix_unchecked(eigv)); + let rot = Rotation::from_mat3(&eigv); + // Create the isometry with rotation and the rotated center translation + let center = (maxs + mins) / 2.0; + let rotated_center = rot * center; ( - rot * Translation::from((maxs + mins) / 2.0), + Pose::from_parts(rotated_center, rot), Cuboid::new((maxs - mins) / 2.0), ) } diff --git a/src/utils/point_cloud_support_point.rs b/src/utils/point_cloud_support_point.rs index e4be18aa..1e5142b3 100644 --- a/src/utils/point_cloud_support_point.rs +++ b/src/utils/point_cloud_support_point.rs @@ -1,13 +1,13 @@ -use crate::math::{Point, Real, Vector}; +use crate::math::Vector; /// Computes the index of the support point of a cloud of points. #[inline] -pub fn point_cloud_support_point_id(dir: &Vector, points: &[Point]) -> usize { +pub fn point_cloud_support_point_id(dir: Vector, points: &[Vector]) -> usize { let mut best_pt = 0; - let mut best_dot = points[0].coords.dot(dir); + let mut best_dot = points[0].dot(dir); for (i, p) in points.iter().enumerate().skip(1) { - let dot = p.coords.dot(dir); + let dot = p.dot(dir); if dot > best_dot { best_dot = dot; @@ -20,6 +20,6 @@ pub fn point_cloud_support_point_id(dir: &Vector, points: &[Point]) /// Computes the support point of a cloud of points. #[inline] -pub fn point_cloud_support_point(dir: &Vector, points: &[Point]) -> Point { +pub fn point_cloud_support_point(dir: Vector, points: &[Vector]) -> Vector { points[point_cloud_support_point_id(dir, points)] } diff --git a/src/utils/point_in_poly2d.rs b/src/utils/point_in_poly2d.rs index 1eaec4b4..a452c619 100644 --- a/src/utils/point_in_poly2d.rs +++ b/src/utils/point_in_poly2d.rs @@ -1,5 +1,4 @@ -use crate::math::Real; -use na::Point2; +use crate::math::Vector2; use num::Zero; /// Tests if the given point is inside a convex polygon with arbitrary orientation. @@ -21,43 +20,43 @@ use num::Zero; /// /// # Examples /// -/// ## Point Inside a Square +/// ## Vector Inside a Square /// /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_convex_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// /// let square = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(2.0, 2.0), -/// Point2::new(0.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// -/// let inside = Point2::new(1.0, 1.0); -/// let outside = Point2::new(3.0, 1.0); +/// let inside = Vector::new(1.0, 1.0); +/// let outside = Vector::new(3.0, 1.0); /// -/// assert!(point_in_convex_poly2d(&inside, &square)); -/// assert!(!point_in_convex_poly2d(&outside, &square)); +/// assert!(point_in_convex_poly2d(inside, &square)); +/// assert!(!point_in_convex_poly2d(outside, &square)); /// # } /// ``` /// -/// ## Point on the Boundary +/// ## Vector on the Boundary /// /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_convex_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// /// let triangle = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(1.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(1.0, 2.0), /// ]; /// -/// let on_edge = Point2::new(1.0, 0.0); -/// assert!(point_in_convex_poly2d(&on_edge, &triangle)); +/// let on_edge = Vector::new(1.0, 0.0); +/// assert!(point_in_convex_poly2d(on_edge, &triangle)); /// # } /// ``` /// @@ -66,16 +65,16 @@ use num::Zero; /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_convex_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// -/// let empty: Vec> = vec![]; -/// let point = Point2::new(1.0, 1.0); +/// let empty: Vec = vec![]; +/// let point = Vector::new(1.0, 1.0); /// /// // An empty polygon contains no points -/// assert!(!point_in_convex_poly2d(&point, &empty)); +/// assert!(!point_in_convex_poly2d(point, &empty)); /// # } /// ``` -pub fn point_in_convex_poly2d(pt: &Point2, poly: &[Point2]) -> bool { +pub fn point_in_convex_poly2d(pt: Vector2, poly: &[Vector2]) -> bool { if poly.is_empty() { false } else { @@ -85,7 +84,7 @@ pub fn point_in_convex_poly2d(pt: &Point2, poly: &[Point2]) -> bool let i2 = (i1 + 1) % poly.len(); let seg_dir = poly[i2] - poly[i1]; let dpt = pt - poly[i1]; - let perp = dpt.perp(&seg_dir); + let perp = dpt.perp_dot(seg_dir); if sign.is_zero() { sign = perp; @@ -125,20 +124,20 @@ pub fn point_in_convex_poly2d(pt: &Point2, poly: &[Point2]) -> bool /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// /// let square = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(2.0, 2.0), -/// Point2::new(0.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// -/// let inside = Point2::new(1.0, 1.0); -/// let outside = Point2::new(3.0, 1.0); +/// let inside = Vector::new(1.0, 1.0); +/// let outside = Vector::new(3.0, 1.0); /// -/// assert!(point_in_poly2d(&inside, &square)); -/// assert!(!point_in_poly2d(&outside, &square)); +/// assert!(point_in_poly2d(inside, &square)); +/// assert!(!point_in_poly2d(outside, &square)); /// # } /// ``` /// @@ -147,23 +146,23 @@ pub fn point_in_convex_poly2d(pt: &Point2, poly: &[Point2]) -> bool /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// /// // L-shaped polygon (concave) /// let l_shape = vec![ -/// Point2::origin(), -/// Point2::new(2.0, 0.0), -/// Point2::new(2.0, 1.0), -/// Point2::new(1.0, 1.0), -/// Point2::new(1.0, 2.0), -/// Point2::new(0.0, 2.0), +/// Vector::ZERO, +/// Vector::new(2.0, 0.0), +/// Vector::new(2.0, 1.0), +/// Vector::new(1.0, 1.0), +/// Vector::new(1.0, 2.0), +/// Vector::new(0.0, 2.0), /// ]; /// -/// let inside_corner = Point2::new(0.5, 0.5); -/// let outside_corner = Point2::new(1.5, 1.5); +/// let inside_corner = Vector::new(0.5, 0.5); +/// let outside_corner = Vector::new(1.5, 1.5); /// -/// assert!(point_in_poly2d(&inside_corner, &l_shape)); -/// assert!(!point_in_poly2d(&outside_corner, &l_shape)); +/// assert!(point_in_poly2d(inside_corner, &l_shape)); +/// assert!(!point_in_poly2d(outside_corner, &l_shape)); /// # } /// ``` /// @@ -172,24 +171,24 @@ pub fn point_in_convex_poly2d(pt: &Point2, poly: &[Point2]) -> bool /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// /// // A star shape (self-intersecting creates a complex winding pattern) /// let star = vec![ -/// Point2::new(0.0, 1.0), -/// Point2::new(0.5, 0.5), -/// Point2::new(1.0, 1.0), -/// Point2::new(0.7, 0.3), -/// Point2::new(1.0, -0.5), -/// Point2::origin(), -/// Point2::new(-1.0, -0.5), -/// Point2::new(-0.7, 0.3), -/// Point2::new(-1.0, 1.0), -/// Point2::new(-0.5, 0.5), +/// Vector::new(0.0, 1.0), +/// Vector::new(0.5, 0.5), +/// Vector::new(1.0, 1.0), +/// Vector::new(0.7, 0.3), +/// Vector::new(1.0, -0.5), +/// Vector::ZERO, +/// Vector::new(-1.0, -0.5), +/// Vector::new(-0.7, 0.3), +/// Vector::new(-1.0, 1.0), +/// Vector::new(-0.5, 0.5), /// ]; /// -/// let center = Point2::new(0.0, 0.5); -/// assert!(point_in_poly2d(¢er, &star)); +/// let center = Vector::new(0.0, 0.5); +/// assert!(point_in_poly2d(center, &star)); /// # } /// ``` /// @@ -198,16 +197,16 @@ pub fn point_in_convex_poly2d(pt: &Point2, poly: &[Point2]) -> bool /// ``` /// # #[cfg(all(feature = "dim2", feature = "f32"))] { /// use parry2d::utils::point_in_poly2d; -/// use parry2d::na::Point2; +/// use parry2d::math::Vector; /// -/// let empty: Vec> = vec![]; -/// let point = Point2::new(1.0, 1.0); +/// let empty: Vec = vec![]; +/// let point = Vector::new(1.0, 1.0); /// /// // An empty polygon contains no points -/// assert!(!point_in_poly2d(&point, &empty)); +/// assert!(!point_in_poly2d(point, &empty)); /// # } /// ``` -pub fn point_in_poly2d(pt: &Point2, poly: &[Point2]) -> bool { +pub fn point_in_poly2d(pt: Vector2, poly: &[Vector2]) -> bool { if poly.is_empty() { return false; } @@ -218,7 +217,7 @@ pub fn point_in_poly2d(pt: &Point2, poly: &[Point2]) -> bool { let b = poly[(i + 1) % poly.len()]; let seg_dir = b - a; let dpt = pt - a; - let perp = dpt.perp(&seg_dir); + let perp = dpt.perp_dot(seg_dir); winding += match (dpt.y >= 0.0, b.y > pt.y) { (true, true) if perp < 0.0 => 1, (false, false) if perp > 0.0 => 1, @@ -245,9 +244,9 @@ mod tests { [1.0, 2.0], [-1.0, 2.0], ] - .map(Point2::from); - assert!(!point_in_poly2d(&[-0.5, -0.5].into(), &poly)); - assert!(point_in_poly2d(&[0.5, -0.5].into(), &poly)); + .map(|[x, y]| Vector2::new(x, y)); + assert!(!point_in_poly2d(Vector2::new(-0.5, -0.5), &poly)); + assert!(point_in_poly2d(Vector2::new(0.5, -0.5), &poly)); } #[test] @@ -316,23 +315,23 @@ mod tests { [615.4741821289063, 280.65472412109375], [615.4741821289063, 279.4120788574219], ] - .map(Point2::from); - let pt = Point2::from([596.0181884765625, 427.9162902832031]); - assert!(point_in_poly2d(&pt, &poly)); + .map(|[x, y]| Vector2::new(x, y)); + let pt = Vector2::new(596.0181884765625, 427.9162902832031); + assert!(point_in_poly2d(pt, &poly)); } #[test] #[cfg(all(feature = "dim2", feature = "alloc"))] fn point_in_poly2d_concave_exact_vertex_bug() { let poly = crate::shape::Ball::new(1.0).to_polyline(10); - assert!(point_in_poly2d(&Point2::origin(), &poly)); - assert!(point_in_poly2d(&Point2::new(-0.25, 0.0), &poly)); - assert!(point_in_poly2d(&Point2::new(0.25, 0.0), &poly)); - assert!(point_in_poly2d(&Point2::new(0.0, -0.25), &poly)); - assert!(point_in_poly2d(&Point2::new(0.0, 0.25), &poly)); - assert!(!point_in_poly2d(&Point2::new(-2.0, 0.0), &poly)); - assert!(!point_in_poly2d(&Point2::new(2.0, 0.0), &poly)); - assert!(!point_in_poly2d(&Point2::new(0.0, -2.0), &poly)); - assert!(!point_in_poly2d(&Point2::new(0.0, 2.0), &poly)); + assert!(point_in_poly2d(Vector2::ZERO, &poly)); + assert!(point_in_poly2d(Vector2::new(-0.25, 0.0), &poly)); + assert!(point_in_poly2d(Vector2::new(0.25, 0.0), &poly)); + assert!(point_in_poly2d(Vector2::new(0.0, -0.25), &poly)); + assert!(point_in_poly2d(Vector2::new(0.0, 0.25), &poly)); + assert!(!point_in_poly2d(Vector2::new(-2.0, 0.0), &poly)); + assert!(!point_in_poly2d(Vector2::new(2.0, 0.0), &poly)); + assert!(!point_in_poly2d(Vector2::new(0.0, -2.0), &poly)); + assert!(!point_in_poly2d(Vector2::new(0.0, 2.0), &poly)); } } diff --git a/src/utils/point_in_triangle.rs b/src/utils/point_in_triangle.rs index adb07f08..cc873643 100644 --- a/src/utils/point_in_triangle.rs +++ b/src/utils/point_in_triangle.rs @@ -1,6 +1,6 @@ //! Function to check if a point is inside a triangle and related functions. -use crate::math::{Point, Real}; +use crate::math::{Real, Vector}; #[derive(Eq, PartialEq, Debug, Copy, Clone)] /// The orientation or winding direction of a corner or polygon. @@ -28,10 +28,10 @@ pub enum Orientation { /// . . /// . o p3 /// o p1 -pub fn corner_direction(p1: &Point, p2: &Point, p3: &Point) -> Orientation { +pub fn corner_direction(p1: Vector, p2: Vector, p3: Vector) -> Orientation { let v1 = p1 - p2; let v2 = p3 - p2; - let cross: Real = v1.perp(&v2); + let cross: Real = v1.perp_dot(v2); match cross .partial_cmp(&0.0) @@ -45,12 +45,7 @@ pub fn corner_direction(p1: &Point, p2: &Point, p3: &Point) -> /// Returns `true` if point `p` is in triangle with corners `v1`, `v2` and `v3`. /// Returns `None` if the triangle is invalid i.e. all points are the same or on a straight line. -pub fn is_point_in_triangle( - p: &Point, - v1: &Point, - v2: &Point, - v3: &Point, -) -> Option { +pub fn is_point_in_triangle(p: Vector, v1: Vector, v2: Vector, v3: Vector) -> Option { let d1 = corner_direction(p, v1, v2); let d2 = corner_direction(p, v2, v3); let d3 = corner_direction(p, v3, v1); diff --git a/src/utils/sdp_matrix.rs b/src/utils/sdp_matrix.rs index 4cc5edbb..31e8b1a6 100644 --- a/src/utils/sdp_matrix.rs +++ b/src/utils/sdp_matrix.rs @@ -1,18 +1,13 @@ -use crate::math::Real; -use core::ops::{Add, Mul}; -use na::{Matrix2, Matrix3, Matrix3x2, SimdRealField, Vector2, Vector3}; - -#[cfg(feature = "rkyv")] -#[cfg(feature = "rkyv")] -use rkyv::{bytecheck, Archive, CheckBytes}; +use crate::math::{Matrix2, Matrix3, Real, Vector2, Vector3}; +use core::ops::{Add, Div, Mul, Neg, Sub}; +use num_traits::{One, Zero}; /// A 2x2 symmetric-definite-positive matrix. -#[derive(Copy, Clone, Debug, PartialEq)] +#[derive(Copy, Clone, Debug, PartialEq, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self", bound(archive = "N: Archive")) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct SdpMatrix2 { /// The component at the first row and first column of this matrix. @@ -23,7 +18,17 @@ pub struct SdpMatrix2 { pub m22: N, } -impl SdpMatrix2 { +impl< + N: Copy + + Zero + + One + + Add + + Sub + + Mul + + Div + + Neg, + > SdpMatrix2 +{ /// A new SDP 2x2 matrix with the given components. /// /// Because the matrix is symmetric, only the lower off-diagonal component is required. @@ -31,17 +36,6 @@ impl SdpMatrix2 { Self { m11, m12, m22 } } - /// Build an `SdpMatrix2` structure from a plain matrix, assuming it is SDP. - /// - /// No check is performed to ensure `mat` is actually SDP. - pub fn from_sdp_matrix(mat: Matrix2) -> Self { - Self { - m11: mat.m11, - m12: mat.m12, - m22: mat.m22, - } - } - /// Create a new SDP matrix filled with zeros. pub fn zero() -> Self { Self { @@ -83,29 +77,53 @@ impl SdpMatrix2 { (Self { m11, m12, m22 }, determinant) } +} + +impl SdpMatrix2 { + /// Build an `SdpMatrix2` structure from a plain matrix, assuming it is SDP. + pub fn from_sdp_matrix(mat: Matrix2) -> Self { + let cols = mat.to_cols_array_2d(); + Self { + m11: cols[0][0], + m12: cols[1][0], + m22: cols[1][1], + } + } /// Convert this SDP matrix to a regular matrix representation. - pub fn into_matrix(self) -> Matrix2 { - Matrix2::new(self.m11, self.m12, self.m12, self.m22) + pub fn into_matrix(self) -> Matrix2 { + Matrix2::from_cols( + Vector2::new(self.m11, self.m12), + Vector2::new(self.m12, self.m22), + ) + } + + /// Multiply this matrix by a vector. + pub fn mul_vec(&self, rhs: Vector2) -> Vector2 { + Vector2::new( + self.m11 * rhs.x + self.m12 * rhs.y, + self.m12 * rhs.x + self.m22 * rhs.y, + ) } } -impl Add> for SdpMatrix2 { +impl> Add> for SdpMatrix2 { type Output = Self; fn add(self, rhs: SdpMatrix2) -> Self { - Self::new(self.m11 + rhs.m11, self.m12 + rhs.m12, self.m22 + rhs.m22) + Self { + m11: self.m11 + rhs.m11, + m12: self.m12 + rhs.m12, + m22: self.m22 + rhs.m22, + } } } -impl Mul> for SdpMatrix2 { - type Output = Vector2; +impl Mul for SdpMatrix2 { + type Output = Vector2; - fn mul(self, rhs: Vector2) -> Self::Output { - Vector2::new( - self.m11 * rhs.x + self.m12 * rhs.y, - self.m12 * rhs.x + self.m22 * rhs.y, - ) + fn mul(self, rhs: Vector2) -> Self::Output { + self.mul_vec(rhs) } } @@ -113,7 +131,11 @@ impl Mul for SdpMatrix2 { type Output = SdpMatrix2; fn mul(self, rhs: Real) -> Self::Output { - SdpMatrix2::new(self.m11 * rhs, self.m12 * rhs, self.m22 * rhs) + SdpMatrix2 { + m11: self.m11 * rhs, + m12: self.m12 * rhs, + m22: self.m22 * rhs, + } } } @@ -122,8 +144,7 @@ impl Mul for SdpMatrix2 { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self", bound(archive = "N: Archive")) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct SdpMatrix3 { /// The component at the first row and first column of this matrix. @@ -140,7 +161,18 @@ pub struct SdpMatrix3 { pub m33: N, } -impl SdpMatrix3 { +impl< + N: Copy + + Zero + + One + + Add + + Sub + + Mul + + Div + + Neg + + PartialEq, + > SdpMatrix3 +{ /// A new SDP 3x3 matrix with the given components. /// /// Because the matrix is symmetric, only the lower off-diagonal components is required. @@ -155,20 +187,6 @@ impl SdpMatrix3 { } } - /// Build an `SdpMatrix3` structure from a plain matrix, assuming it is SDP. - /// - /// No check is performed to ensure `mat` is actually SDP. - pub fn from_sdp_matrix(mat: Matrix3) -> Self { - Self { - m11: mat.m11, - m12: mat.m12, - m13: mat.m13, - m22: mat.m22, - m23: mat.m23, - m33: mat.m33, - } - } - /// Create a new SDP matrix filled with zeros. pub fn zero() -> Self { Self { @@ -195,12 +213,12 @@ impl SdpMatrix3 { /// Are all components of this matrix equal to zero? pub fn is_zero(&self) -> bool { - self.m11.is_zero() - && self.m12.is_zero() - && self.m13.is_zero() - && self.m22.is_zero() - && self.m23.is_zero() - && self.m33.is_zero() + self.m11 == N::zero() + && self.m12 == N::zero() + && self.m13 == N::zero() + && self.m22 == N::zero() + && self.m23 == N::zero() + && self.m33 == N::zero() } /// Compute the inverse of this SDP matrix without performing any inversibility check. @@ -223,55 +241,6 @@ impl SdpMatrix3 { } } - /// Compute the quadratic form `m.transpose() * self * m`. - pub fn quadform3x2(&self, m: &Matrix3x2) -> SdpMatrix2 { - let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; - let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; - let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; - - let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; - let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; - let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; - - let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0; - let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1; - let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1; - - SdpMatrix2 { m11, m12, m22 } - } - - /// Compute the quadratic form `m.transpose() * self * m`. - pub fn quadform(&self, m: &Matrix3) -> Self { - let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; - let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; - let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; - - let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; - let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; - let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; - - let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33; - let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33; - let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33; - - let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0; - let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1; - let m13 = m.m11 * x2 + m.m21 * y2 + m.m31 * z2; - - let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1; - let m23 = m.m12 * x2 + m.m22 * y2 + m.m32 * z2; - let m33 = m.m13 * x2 + m.m23 * y2 + m.m33 * z2; - - Self { - m11, - m12, - m13, - m22, - m23, - m33, - } - } - /// Adds `elt` to the diagonal components of `self`. pub fn add_diagonal(&self, elt: N) -> Self { Self { @@ -285,8 +254,76 @@ impl SdpMatrix3 { } } -impl> Add> for SdpMatrix3 { - type Output = SdpMatrix3; +impl SdpMatrix3 { + /// Build an `SdpMatrix3` structure from a plain matrix, assuming it is SDP. + pub fn from_sdp_matrix(mat: Matrix3) -> Self { + let cols = mat.to_cols_array_2d(); + Self { + m11: cols[0][0], + m12: cols[1][0], + m13: cols[2][0], + m22: cols[1][1], + m23: cols[2][1], + m33: cols[2][2], + } + } + + /// Multiply this matrix by a vector. + pub fn mul_vec(&self, rhs: Vector3) -> Vector3 { + let x = self.m11 * rhs.x + self.m12 * rhs.y + self.m13 * rhs.z; + let y = self.m12 * rhs.x + self.m22 * rhs.y + self.m23 * rhs.z; + let z = self.m13 * rhs.x + self.m23 * rhs.y + self.m33 * rhs.z; + Vector3::new(x, y, z) + } + + /// Multiply this matrix by a 3x3 matrix. + pub fn mul_mat(&self, rhs: Matrix3) -> Matrix3 { + let cols = rhs.to_cols_array_2d(); + let col0 = self.mul_vec(Vector3::new(cols[0][0], cols[0][1], cols[0][2])); + let col1 = self.mul_vec(Vector3::new(cols[1][0], cols[1][1], cols[1][2])); + let col2 = self.mul_vec(Vector3::new(cols[2][0], cols[2][1], cols[2][2])); + Matrix3::from_cols(col0, col1, col2) + } + + /// Compute the quadratic form `m.transpose() * self * m`. + pub fn quadform(&self, m: &Matrix3) -> Self { + let sm = self.mul_mat(*m); + let result = m.transpose() * sm; + Self::from_sdp_matrix(result) + } + + /// Compute the quadratic form `m.transpose() * self * m` for a 3x2 matrix. + pub fn quadform3x2( + &self, + m11: Real, + m12: Real, + m21: Real, + m22: Real, + m31: Real, + m32: Real, + ) -> SdpMatrix2 { + let x0 = self.m11 * m11 + self.m12 * m21 + self.m13 * m31; + let y0 = self.m12 * m11 + self.m22 * m21 + self.m23 * m31; + let z0 = self.m13 * m11 + self.m23 * m21 + self.m33 * m31; + + let x1 = self.m11 * m12 + self.m12 * m22 + self.m13 * m32; + let y1 = self.m12 * m12 + self.m22 * m22 + self.m23 * m32; + let z1 = self.m13 * m12 + self.m23 * m22 + self.m33 * m32; + + let r11 = m11 * x0 + m21 * y0 + m31 * z0; + let r12 = m11 * x1 + m21 * y1 + m31 * z1; + let r22 = m12 * x1 + m22 * y1 + m32 * z1; + + SdpMatrix2 { + m11: r11, + m12: r12, + m22: r22, + } + } +} + +impl> Add> for SdpMatrix3 { + type Output = SdpMatrix3; fn add(self, rhs: SdpMatrix3) -> Self::Output { SdpMatrix3 { @@ -315,50 +352,19 @@ impl Mul for SdpMatrix3 { } } -impl Mul> for SdpMatrix3 { - type Output = Vector3; - - fn mul(self, rhs: Vector3) -> Self::Output { - let x = self.m11 * rhs.x + self.m12 * rhs.y + self.m13 * rhs.z; - let y = self.m12 * rhs.x + self.m22 * rhs.y + self.m23 * rhs.z; - let z = self.m13 * rhs.x + self.m23 * rhs.y + self.m33 * rhs.z; - Vector3::new(x, y, z) - } -} - -impl Mul> for SdpMatrix3 { - type Output = Matrix3; - - fn mul(self, rhs: Matrix3) -> Self::Output { - let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31; - let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31; - let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31; - - let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32; - let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32; - let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32; +impl Mul for SdpMatrix3 { + type Output = Vector3; - let x2 = self.m11 * rhs.m13 + self.m12 * rhs.m23 + self.m13 * rhs.m33; - let y2 = self.m12 * rhs.m13 + self.m22 * rhs.m23 + self.m23 * rhs.m33; - let z2 = self.m13 * rhs.m13 + self.m23 * rhs.m23 + self.m33 * rhs.m33; - - Matrix3::new(x0, x1, x2, y0, y1, y2, z0, z1, z2) + fn mul(self, rhs: Vector3) -> Self::Output { + self.mul_vec(rhs) } } -impl Mul> for SdpMatrix3 { - type Output = Matrix3x2; - - fn mul(self, rhs: Matrix3x2) -> Self::Output { - let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31; - let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31; - let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31; +impl Mul for SdpMatrix3 { + type Output = Matrix3; - let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32; - let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32; - let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32; - - Matrix3x2::new(x0, x1, y0, y1, z0, z1) + fn mul(self, rhs: Matrix3) -> Self::Output { + self.mul_mat(rhs) } } @@ -377,187 +383,3 @@ where } } } - -#[cfg(feature = "simd-nightly")] -impl From<[SdpMatrix3; 8]> for SdpMatrix3 { - fn from(data: [SdpMatrix3; 8]) -> Self { - SdpMatrix3 { - m11: simba::simd::f32x8::from([ - data[0].m11, - data[1].m11, - data[2].m11, - data[3].m11, - data[4].m11, - data[5].m11, - data[6].m11, - data[7].m11, - ]), - m12: simba::simd::f32x8::from([ - data[0].m12, - data[1].m12, - data[2].m12, - data[3].m12, - data[4].m12, - data[5].m12, - data[6].m12, - data[7].m12, - ]), - m13: simba::simd::f32x8::from([ - data[0].m13, - data[1].m13, - data[2].m13, - data[3].m13, - data[4].m13, - data[5].m13, - data[6].m13, - data[7].m13, - ]), - m22: simba::simd::f32x8::from([ - data[0].m22, - data[1].m22, - data[2].m22, - data[3].m22, - data[4].m22, - data[5].m22, - data[6].m22, - data[7].m22, - ]), - m23: simba::simd::f32x8::from([ - data[0].m23, - data[1].m23, - data[2].m23, - data[3].m23, - data[4].m23, - data[5].m23, - data[6].m23, - data[7].m23, - ]), - m33: simba::simd::f32x8::from([ - data[0].m33, - data[1].m33, - data[2].m33, - data[3].m33, - data[4].m33, - data[5].m33, - data[6].m33, - data[7].m33, - ]), - } - } -} - -#[cfg(feature = "simd-nightly")] -impl From<[SdpMatrix3; 16]> for SdpMatrix3 { - fn from(data: [SdpMatrix3; 16]) -> Self { - SdpMatrix3 { - m11: simba::simd::f32x16::from([ - data[0].m11, - data[1].m11, - data[2].m11, - data[3].m11, - data[4].m11, - data[5].m11, - data[6].m11, - data[7].m11, - data[8].m11, - data[9].m11, - data[10].m11, - data[11].m11, - data[12].m11, - data[13].m11, - data[14].m11, - data[15].m11, - ]), - m12: simba::simd::f32x16::from([ - data[0].m12, - data[1].m12, - data[2].m12, - data[3].m12, - data[4].m12, - data[5].m12, - data[6].m12, - data[7].m12, - data[8].m12, - data[9].m12, - data[10].m12, - data[11].m12, - data[12].m12, - data[13].m12, - data[14].m12, - data[15].m12, - ]), - m13: simba::simd::f32x16::from([ - data[0].m13, - data[1].m13, - data[2].m13, - data[3].m13, - data[4].m13, - data[5].m13, - data[6].m13, - data[7].m13, - data[8].m13, - data[9].m13, - data[10].m13, - data[11].m13, - data[12].m13, - data[13].m13, - data[14].m13, - data[15].m13, - ]), - m22: simba::simd::f32x16::from([ - data[0].m22, - data[1].m22, - data[2].m22, - data[3].m22, - data[4].m22, - data[5].m22, - data[6].m22, - data[7].m22, - data[8].m22, - data[9].m22, - data[10].m22, - data[11].m22, - data[12].m22, - data[13].m22, - data[14].m22, - data[15].m22, - ]), - m23: simba::simd::f32x16::from([ - data[0].m23, - data[1].m23, - data[2].m23, - data[3].m23, - data[4].m23, - data[5].m23, - data[6].m23, - data[7].m23, - data[8].m23, - data[9].m23, - data[10].m23, - data[11].m23, - data[12].m23, - data[13].m23, - data[14].m23, - data[15].m23, - ]), - m33: simba::simd::f32x16::from([ - data[0].m33, - data[1].m33, - data[2].m33, - data[3].m33, - data[4].m33, - data[5].m33, - data[6].m33, - data[7].m33, - data[8].m33, - data[9].m33, - data[10].m33, - data[11].m33, - data[12].m33, - data[13].m33, - data[14].m33, - data[15].m33, - ]), - } - } -} diff --git a/src/utils/segments_intersection.rs b/src/utils/segments_intersection.rs index 60159580..d74558c2 100644 --- a/src/utils/segments_intersection.rs +++ b/src/utils/segments_intersection.rs @@ -1,11 +1,8 @@ -use na::Point2; +use crate::math::Vector2; use crate::math::Real; use crate::shape::{SegmentPointLocation, Triangle, TriangleOrientation}; -#[cfg(not(feature = "alloc"))] -use na::ComplexField; - /// Intersection between two segments. pub enum SegmentsIntersection { /// Single point of intersection. @@ -30,10 +27,10 @@ pub enum SegmentsIntersection { /// Computes the intersection between two segments. pub fn segments_intersection2d( - a: &Point2, - b: &Point2, - c: &Point2, - d: &Point2, + a: Vector2, + b: Vector2, + c: Vector2, + d: Vector2, epsilon: Real, ) -> Option { let denom = a.x * (d.y - c.y) + b.x * (c.y - d.y) + d.x * (b.y - a.y) + c.x * (a.y - b.y); @@ -73,10 +70,10 @@ pub fn segments_intersection2d( } fn parallel_intersection( - a: &Point2, - b: &Point2, - c: &Point2, - d: &Point2, + a: Vector2, + b: Vector2, + c: Vector2, + d: Vector2, epsilon: Real, ) -> Option { if Triangle::orientation2d(a, b, c, epsilon) != TriangleOrientation::Degenerate { @@ -146,7 +143,7 @@ fn parallel_intersection( // Checks that `c` is in-between `a` and `b`. // Assumes the three points are collinear. -fn between(a: &Point2, b: &Point2, c: &Point2) -> Option { +fn between(a: Vector2, b: Vector2, c: Vector2) -> Option { // If ab not vertical, check betweenness on x; else on y. // TODO: handle cases where we actually are on a vertex (to return OnEdge instead of OnVertex)? if a.x != b.x { diff --git a/src/utils/sorted_pair.rs b/src/utils/sorted_pair.rs index 844edfe4..dda63796 100644 --- a/src/utils/sorted_pair.rs +++ b/src/utils/sorted_pair.rs @@ -100,8 +100,7 @@ use core::ops::Deref; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct SortedPair([T; 2]); diff --git a/src/utils/spade.rs b/src/utils/spade.rs index f9efa214..e6874226 100644 --- a/src/utils/spade.rs +++ b/src/utils/spade.rs @@ -21,7 +21,7 @@ pub fn sanitize_spade_coord(coord: Real) -> Real { coord } -/// Ensures the coordinates of the given point don’t go out of the bounds of spade’s acceptable values. +/// Ensures the coordinates of the given point don't go out of the bounds of spade's acceptable values. pub fn sanitize_spade_point(point: spade::Point2) -> spade::Point2 { spade::Point2::new(sanitize_spade_coord(point.x), sanitize_spade_coord(point.y)) } diff --git a/src/utils/vec_map.rs b/src/utils/vec_map.rs index c7330e12..daa32461 100644 --- a/src/utils/vec_map.rs +++ b/src/utils/vec_map.rs @@ -79,8 +79,7 @@ use core::slice; )] #[cfg_attr( feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) )] pub struct VecMap { n: usize, diff --git a/src/utils/wops.rs b/src/utils/wops.rs index 65574bea..d6fe4744 100644 --- a/src/utils/wops.rs +++ b/src/utils/wops.rs @@ -1,15 +1,15 @@ //! Miscellaneous utilities. -use crate::math::Real; -use na::{Scalar, SimdRealField, Vector2, Vector3}; +use crate::math::{Real, Vector2, Vector3}; #[cfg(feature = "simd-is-enabled")] use { - crate::simd::{SimdBool, SimdReal}, + crate::math::{SimdBool, SimdReal}, simba::simd::SimdValue, }; #[cfg(feature = "simd-is-enabled")] +#[allow(dead_code)] /// Conditionally swaps each lanes of `a` with those of `b`. /// /// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then @@ -35,14 +35,14 @@ impl WSign for Real { } } -impl> WSign> for N { - fn copy_sign_to(self, to: Vector2) -> Vector2 { +impl WSign for Real { + fn copy_sign_to(self, to: Vector2) -> Vector2 { Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y)) } } -impl> WSign> for N { - fn copy_sign_to(self, to: Vector3) -> Vector3 { +impl WSign for Real { + fn copy_sign_to(self, to: Vector3) -> Vector3 { Vector3::new( self.copy_sign_to(to.x), self.copy_sign_to(to.y), @@ -51,14 +51,14 @@ impl> WSign> for N { } } -impl> WSign> for Vector2 { - fn copy_sign_to(self, to: Vector2) -> Vector2 { +impl WSign for Vector2 { + fn copy_sign_to(self, to: Vector2) -> Vector2 { Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y)) } } -impl> WSign> for Vector3 { - fn copy_sign_to(self, to: Vector3) -> Vector3 { +impl WSign for Vector3 { + fn copy_sign_to(self, to: Vector3) -> Vector3 { Vector3::new( self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y), @@ -70,6 +70,7 @@ impl> WSign> for Vector3 { #[cfg(feature = "simd-is-enabled")] impl WSign for SimdReal { fn copy_sign_to(self, to: SimdReal) -> SimdReal { + use simba::simd::SimdRealField; to.simd_copysign(self) } } @@ -82,28 +83,24 @@ pub trait WBasis: Sized { fn orthonormal_basis(self) -> Self::Basis; } -impl WBasis for Vector2 { - type Basis = [Vector2; 1]; - fn orthonormal_basis(self) -> [Vector2; 1] { +impl WBasis for Vector2 { + type Basis = [Vector2; 1]; + fn orthonormal_basis(self) -> [Vector2; 1] { [Vector2::new(-self.y, self.x)] } } -impl> WBasis for Vector3 { - type Basis = [Vector3; 2]; +impl WBasis for Vector3 { + type Basis = [Vector3; 2]; // Robust and branchless implementation from Pixar: // https://graphics.pixar.com/library/OrthonormalB/paper.pdf - fn orthonormal_basis(self) -> [Vector3; 2] { - let sign = self.z.copy_sign_to(N::one()); - let a = -N::one() / (sign + self.z); + fn orthonormal_basis(self) -> [Vector3; 2] { + let sign = self.z.copy_sign_to(1.0); + let a = -1.0 / (sign + self.z); let b = self.x * self.y * a; [ - Vector3::new( - N::one() + sign * self.x * self.x * a, - sign * b, - -sign * self.x, - ), + Vector3::new(1.0 + sign * self.x * self.x * a, sign * b, -sign * self.x), Vector3::new(b, sign + self.y * self.y * a, -self.y), ] } @@ -114,55 +111,26 @@ pub(crate) trait WCross: Sized { fn gcross(&self, rhs: Rhs) -> Self::Result; } -impl WCross> for Vector3 { +impl WCross for Vector3 { type Result = Self; - fn gcross(&self, rhs: Vector3) -> Self::Result { - self.cross(&rhs) + fn gcross(&self, rhs: Vector3) -> Self::Result { + self.cross(rhs) } } -impl WCross> for Vector2 { +impl WCross for Vector2 { type Result = Real; - fn gcross(&self, rhs: Vector2) -> Self::Result { + fn gcross(&self, rhs: Vector2) -> Self::Result { self.x * rhs.y - self.y * rhs.x } } -impl WCross> for Real { - type Result = Vector2; +impl WCross for Real { + type Result = Vector2; - fn gcross(&self, rhs: Vector2) -> Self::Result { + fn gcross(&self, rhs: Vector2) -> Self::Result { Vector2::new(-rhs.y * *self, rhs.x * *self) } } - -#[cfg(feature = "simd-is-enabled")] -impl WCross> for Vector3 { - type Result = Vector3; - - fn gcross(&self, rhs: Self) -> Self::Result { - self.cross(&rhs) - } -} - -#[cfg(feature = "simd-is-enabled")] -impl WCross> for SimdReal { - type Result = Vector2; - - fn gcross(&self, rhs: Vector2) -> Self::Result { - Vector2::new(-rhs.y * *self, rhs.x * *self) - } -} - -#[cfg(feature = "simd-is-enabled")] -impl WCross> for Vector2 { - type Result = SimdReal; - - fn gcross(&self, rhs: Self) -> Self::Result { - let yx = Vector2::new(rhs.y, rhs.x); - let prod = self.component_mul(&yx); - prod.x - prod.y - } -}