diff --git a/Cargo.toml b/Cargo.toml index fb4ae21..a2ac820 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,20 +10,23 @@ readme = "README.md" all-features = true [features] -parry = ["parry3d-f64", "lru-slab"] +parry = ["parry3d-f64", "lru-slab", "glam"] fearless_simd = ["dep:fearless_simd"] [dependencies] na = { package = "nalgebra", version = "0.34.1" } +glam = { version = "0.33", optional = true, default-features = false, features = ["float-types", "integer-types"] } slab = "0.4.2" hashbrown = "0.17" -parry3d-f64 = { version = "0.25.0", optional = true } +parry3d-f64 = { version = "0.30.2", optional = true } lru-slab = { version = "0.1.1", optional = true } fearless_simd = { version = "0.7.0", optional = true } [dev-dependencies] criterion = "0.8.0" approx = "0.5" +# Enable glam's approx impls for use with the `approx` crate in tests +glam = { version = "0.33", features = ["approx"] } simba = { version = "0.9", features = ["wide"] } fearless_simd = { version = "0.7", features = ["force_support_fallback"] } diff --git a/benches/bench.rs b/benches/bench.rs index b8edc25..9c0b73c 100644 --- a/benches/bench.rs +++ b/benches/bench.rs @@ -1,6 +1,7 @@ use criterion::{criterion_group, criterion_main, Criterion}; #[cfg(feature = "parry")] use parry3d_f64::{ + math::{Pose, Vector}, query::{PointQuery, QueryDispatcher, Ray, RayCast}, shape::Ball, }; @@ -75,11 +76,7 @@ fn collision(c: &mut Criterion) { c.bench_function("intersect", |b| { b.iter(|| { assert!(PlanetDispatcher - .intersection_test( - &na::Isometry3::translation(PLANET_RADIUS, 0.0, 0.0), - &planet, - &ball, - ) + .intersection_test(&Pose::translation(PLANET_RADIUS, 0.0, 0.0), &planet, &ball,) .unwrap()); }); }); @@ -88,8 +85,8 @@ fn collision(c: &mut Criterion) { b.iter(|| { planet.cast_local_ray( &Ray { - origin: na::Point3::new(PLANET_RADIUS + 1.0, 0.0, 0.0), - dir: na::Vector3::y(), + origin: Vector::new(PLANET_RADIUS + 1.0, 0.0, 0.0), + dir: Vector::Y, }, 1e1, true, @@ -101,8 +98,8 @@ fn collision(c: &mut Criterion) { b.iter(|| { planet.cast_local_ray( &Ray { - origin: na::Point3::new(PLANET_RADIUS + 1.0, 0.0, 0.0), - dir: na::Vector3::y(), + origin: Vector::new(PLANET_RADIUS + 1.0, 0.0, 0.0), + dir: Vector::Y, }, 1e3, true, @@ -113,8 +110,8 @@ fn collision(c: &mut Criterion) { c.bench_function("project point", |b| { b.iter(|| { planet.project_point( - &na::Isometry3::identity(), - &na::Point3::new(PLANET_RADIUS + 1.0, 0.0, 0.0), + &Pose::identity(), + Vector::new(PLANET_RADIUS + 1.0, 0.0, 0.0), true, ) }); diff --git a/src/parry.rs b/src/parry.rs index fe0d7f0..1b2918e 100644 --- a/src/parry.rs +++ b/src/parry.rs @@ -6,13 +6,14 @@ use std::sync::{Arc, Mutex}; +use glam::UVec2; use hashbrown::hash_map; use hashbrown::HashMap; use lru_slab::LruSlab; use parry3d_f64::{ bounding_volume::{Aabb, BoundingSphere, BoundingVolume}, mass_properties::MassProperties, - math::{Isometry, Point, Real, Vector}, + math::{Matrix, Pose, Real, Vec2, Vector}, query::{ details::NormalConstraints, ClosestPoints, Contact, ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, NonlinearRigidMotion, @@ -21,11 +22,16 @@ use parry3d_f64::{ WorkspaceData, }, shape::{CompositeShape, FeatureId, HalfSpace, Shape, ShapeType, Triangle, TypedShape}, - utils::IsometryOpt, + utils::PoseOpt, }; use crate::cubemap::{Coords, Edge}; +/// Convert a `parry` vector to the nalgebra type consumed by the `cubemap` module +fn na3f32(v: Vector) -> na::Vector3 { + na::Vector3::new(v.x as f32, v.y as f32, v.z as f32) +} + /// Height data source for `Planet` pub trait Terrain: Send + Sync + 'static { /// Generate a `resolution * resolution` grid of heights wrt. sea level @@ -133,13 +139,13 @@ impl Planet { aabb: &Aabb, mut f: impl FnMut(&Coords, u32, u32, &Triangle) -> bool, ) { - let dir = bounds.center().coords; - let distance = dir.norm(); + let dir = bounds.center(); + let distance = dir.length(); let cache = &mut *self.cache.lock().unwrap(); // Iterate over each overlapping chunk 'outer: for chunk_coords in Coords::neighborhood( self.terrain.face_resolution(), - dir.cast(), + na3f32(dir), bounds.radius().atan2(distance) as f32, ) { let (slot, data) = cache.get(self, &chunk_coords); @@ -181,8 +187,7 @@ impl RayCast for Planet { solid: bool, ) -> Option { // Find the chunk containing the ray origin - let mut chunk = - Coords::from_vector(self.terrain.face_resolution(), &ray.origin.coords.cast()); + let mut chunk = Coords::from_vector(self.terrain.face_resolution(), &na3f32(ray.origin)); let mut patch = Patch::new(&chunk, self.terrain.face_resolution()); let mut ray = *ray; @@ -233,26 +238,22 @@ impl RayCast for Planet { } /// `quad` is row-major vectors from the origin -fn raycast_quad_edges( - ray: &Ray, - [a, b, c, d]: &[na::Vector3; 4], - max_toi: f64, -) -> Option<(Edge, f64)> { - let edges: [(Edge, [&na::Vector3; 2]); 4] = [ - (Edge::Nx, [c, a]), - (Edge::Ny, [a, b]), - (Edge::Px, [b, d]), - (Edge::Py, [d, c]), +fn raycast_quad_edges(ray: &Ray, [a, b, c, d]: &[Vector; 4], max_toi: f64) -> Option<(Edge, f64)> { + let edges: [(Edge, [Vector; 2]); 4] = [ + (Edge::Nx, [*c, *a]), + (Edge::Ny, [*a, *b]), + (Edge::Px, [*b, *d]), + (Edge::Py, [*d, *c]), ]; let mut closest = None; - for &(edge, [v1, v2]) in edges.iter() { + for (edge, [v1, v2]) in edges { // Construct inward-facing edge planes let plane = HalfSpace { - normal: na::Unit::new_normalize(v1.cross(v2)), + normal: v1.cross(v2).normalize(), }; // Eliminate planes behind the ray - if plane.normal.as_ref().dot(&ray.dir) >= 0.0 { + if plane.normal.dot(ray.dir) >= 0.0 { continue; } if let Some(hit) = plane.cast_local_ray(ray, max_toi, true) { @@ -267,37 +268,30 @@ fn raycast_quad_edges( } impl PointQuery for Planet { - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - if solid && pt.coords.norm_squared() < self.min_radius() * self.min_radius() { + fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection { + if solid && pt.length_squared() < self.min_radius() * self.min_radius() { return PointProjection { is_inside: true, - point: *pt, + point: pt, }; } // TODO: Handle `solid` near the surface self.project_local_point_and_get_feature(pt).0 } - 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: Optimize/fix this by projecting `pt` onto the cubemap, then scanning *outward* from // the quad containing the projected point until all remaining triangles must be further // than the closest triangle found so far, regardless of height - let coords = Coords::from_vector(self.terrain.face_resolution(), &pt.coords.cast()); - let distance2 = |x: &na::Point3| na::distance_squared(x, pt); + let coords = Coords::from_vector(self.terrain.face_resolution(), &na3f32(pt)); + let distance2 = |x: Vector| x.distance_squared(pt); let cache = &mut *self.cache.lock().unwrap(); let (slot, data) = cache.get(self, &coords); let patch = Patch::new(&coords, self.terrain.face_resolution()); let (idx, nearest) = patch .triangles(self.radius, self.chunk_resolution, &data.samples) .map(|(i, tri)| (i, tri.project_local_point(pt, false))) - .min_by(|(_, x), (_, y)| { - distance2(&x.point) - .partial_cmp(&distance2(&y.point)) - .unwrap() - }) + .min_by(|(_, x), (_, y)| distance2(x.point).partial_cmp(&distance2(y.point)).unwrap()) .unwrap(); // TODO: Check neighborhood, so we don't miss as many cliff faces (nearest, FeatureId::Face(self.feature_id(slot, idx))) @@ -306,11 +300,11 @@ impl PointQuery for Planet { impl Shape for Planet { fn compute_local_aabb(&self) -> Aabb { - Aabb::from_half_extents(Point::origin(), Vector::repeat(self.max_radius())) + Aabb::from_half_extents(Vector::ZERO, Vector::splat(self.max_radius())) } fn compute_local_bounding_sphere(&self) -> BoundingSphere { - BoundingSphere::new(Point::origin(), self.max_radius()) + BoundingSphere::new(Vector::ZERO, self.max_radius()) } fn mass_properties(&self, density: Real) -> MassProperties { @@ -340,7 +334,7 @@ impl Shape for Planet { Box::new(self.clone()) } - fn scale_dyn(&self, _scale: &Vector, _num_subdivisions: u32) -> Option> { + fn scale_dyn(&self, _scale: Vector, _num_subdivisions: u32) -> Option> { // Non-uniform scale not supported None } @@ -421,14 +415,14 @@ pub struct PlanetDispatcher; impl QueryDispatcher for PlanetDispatcher { fn intersection_test( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, ) -> Result { - if let Some(p1) = g1.downcast_ref::() { + if let Some(p1) = g1.as_shape::() { return Ok(intersects(pos12, p1, g2)); } - if let Some(p2) = g2.downcast_ref::() { + if let Some(p2) = g2.as_shape::() { return Ok(intersects(&pos12.inverse(), p2, g1)); } Err(Unsupported) @@ -436,7 +430,7 @@ impl QueryDispatcher for PlanetDispatcher { fn distance( &self, - _pos12: &Isometry, + _pos12: &Pose, _g1: &dyn Shape, _g2: &dyn Shape, ) -> Result { @@ -445,7 +439,7 @@ impl QueryDispatcher for PlanetDispatcher { fn contact( &self, - _pos12: &Isometry, + _pos12: &Pose, _g1: &dyn Shape, _g2: &dyn Shape, _prediction: Real, @@ -455,7 +449,7 @@ impl QueryDispatcher for PlanetDispatcher { fn closest_points( &self, - _pos12: &Isometry, + _pos12: &Pose, _g1: &dyn Shape, _g2: &dyn Shape, _max_dist: Real, @@ -465,24 +459,17 @@ impl QueryDispatcher for PlanetDispatcher { fn cast_shapes( &self, - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, g1: &dyn Shape, g2: &dyn Shape, options: ShapeCastOptions, ) -> Result, Unsupported> { - if let Some(p1) = g1.downcast_ref::() { + if let Some(p1) = g1.as_shape::() { return Ok(compute_toi(pos12, vel12, p1, g2, options, false)); } - if let Some(p2) = g2.downcast_ref::() { - return Ok(compute_toi( - &pos12.inverse(), - &-vel12, - p2, - g1, - options, - true, - )); + if let Some(p2) = g2.as_shape::() { + return Ok(compute_toi(&pos12.inverse(), -vel12, p2, g1, options, true)); } Err(Unsupported) } @@ -497,7 +484,7 @@ impl QueryDispatcher for PlanetDispatcher { end_time: Real, stop_at_penetration: bool, ) -> Result, Unsupported> { - if let Some(p1) = g1.downcast_ref::() { + if let Some(p1) = g1.as_shape::() { return Ok(compute_nonlinear_toi( motion1, p1, @@ -509,7 +496,7 @@ impl QueryDispatcher for PlanetDispatcher { false, )); } - if let Some(p2) = g2.downcast_ref::() { + if let Some(p2) = g2.as_shape::() { return Ok(compute_nonlinear_toi( motion2, p2, @@ -525,7 +512,7 @@ impl QueryDispatcher for PlanetDispatcher { } } -fn intersects(pos12: &Isometry, planet: &Planet, other: &dyn Shape) -> bool { +fn intersects(pos12: &Pose, planet: &Planet, other: &dyn Shape) -> bool { // TODO after https://github.com/dimforge/parry/issues/8 let dispatcher = DefaultQueryDispatcher; let bounds = other.compute_bounding_sphere(pos12); @@ -541,8 +528,8 @@ fn intersects(pos12: &Isometry, planet: &Planet, other: &dyn Shape) -> boo } fn compute_toi( - pos12: &Isometry, - vel12: &Vector, + pos12: &Pose, + vel12: Vector, planet: &Planet, other: &dyn Shape, options: ShapeCastOptions, @@ -553,16 +540,13 @@ fn compute_toi( // TODO: Raycast vs. minkowski sum of chunk bounds and bounding sphere? let aabb = { let start = other.compute_aabb(pos12); - let end = start.transform_by(&Isometry::from_parts( - (options.max_time_of_impact * vel12).into(), - na::one(), - )); + let end = start.transform_by(&Pose::from_translation(options.max_time_of_impact * vel12)); start.merged(&end) }; let mut closest = None::; planet.map_elements_in_local_sphere(&aabb.bounding_sphere(), &aabb, |_, _, _, triangle| { let impact = if flipped { - dispatcher.cast_shapes(&pos12.inverse(), &-vel12, other, triangle, options) + dispatcher.cast_shapes(&pos12.inverse(), -vel12, other, triangle, options) } else { dispatcher.cast_shapes(pos12, vel12, triangle, other, options) }; @@ -644,14 +628,14 @@ where { fn contact_manifolds( &self, - pos12: &Isometry, + pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, prediction: Real, manifolds: &mut Vec>, workspace: &mut Option, ) -> Result<(), Unsupported> { - if let Some(p1) = g1.downcast_ref::() { + if let Some(p1) = g1.as_shape::() { if let Some(composite) = g2.as_composite_shape() { compute_manifolds_vs_composite( pos12, @@ -668,7 +652,7 @@ where } return Ok(()); } - if let Some(p2) = g2.downcast_ref::() { + if let Some(p2) = g2.as_shape::() { if let Some(composite) = g2.as_composite_shape() { compute_manifolds_vs_composite( &pos12.inverse(), @@ -698,7 +682,7 @@ where fn contact_manifold_convex_convex( &self, - _pos12: &Isometry, + _pos12: &Pose, _g1: &dyn Shape, _g2: &dyn Shape, _normal_constraints1: Option<&dyn NormalConstraints>, @@ -712,7 +696,7 @@ where } fn compute_manifolds( - pos12: &Isometry, + pos12: &Pose, planet: &Planet, other: &dyn Shape, prediction: Real, @@ -815,8 +799,8 @@ struct TriangleState { #[allow(clippy::too_many_arguments)] // that's just what it takes fn compute_manifolds_vs_composite( - pos12: &Isometry, - pos21: &Isometry, + pos12: &Pose, + pos21: &Pose, planet: &Planet, other: &dyn CompositeShape, prediction: Real, @@ -876,11 +860,11 @@ fn compute_manifolds_vs_composite( if flipped { manifold.subshape1 = composite_subshape; manifold.subshape2 = id; - manifold.subshape_pos1 = composite_part_pos.copied(); + manifold.set_subshape_pos1(composite_part_pos.copied()); } else { manifold.subshape1 = id; manifold.subshape2 = composite_subshape; - manifold.subshape_pos2 = composite_part_pos.copied(); + manifold.set_subshape_pos2(composite_part_pos.copied()); }; let tri_state = TriangleState { @@ -949,6 +933,14 @@ impl WorkspaceData for WorkspaceVsComposite { } } +/// Direction on a face at `[0..1]^2` coordinates within a chunk +fn corner(coords: &Coords, face_resolution: u32, uv: [f64; 2]) -> Vector { + let dir = coords + .direction(face_resolution, &na::Point2::from(uv)) + .into_inner(); + Vector::new(dir.x, dir.y, dir.z) +} + /// Quad defined by a chunk pre-displacement /// /// Generally neither flat nor square. @@ -958,32 +950,24 @@ struct Patch { // |\ | // | \| // c--d (1,1) - a: na::Vector3, - b: na::Vector3, - c: na::Vector3, - d: na::Vector3, + a: Vector, + b: Vector, + c: Vector, + d: Vector, } impl Patch { pub fn new(coords: &Coords, face_resolution: u32) -> Self { Self { - a: coords - .direction(face_resolution, &[0.0, 0.0].into()) - .into_inner(), - b: coords - .direction(face_resolution, &[1.0, 0.0].into()) - .into_inner(), - c: coords - .direction(face_resolution, &[0.0, 1.0].into()) - .into_inner(), - d: coords - .direction(face_resolution, &[1.0, 1.0].into()) - .into_inner(), + a: corner(coords, face_resolution, [0.0, 0.0]), + b: corner(coords, face_resolution, [1.0, 0.0]), + c: corner(coords, face_resolution, [0.0, 1.0]), + d: corner(coords, face_resolution, [1.0, 1.0]), } } /// Map a point from patch space to a direction in sphere space - fn get(&self, p: &na::Point2) -> na::Vector3 { + fn get(&self, p: &Vec2) -> Vector { // Extend the triangle into a parallelogram, then bilinearly interpolate. This guarantees a // numerically exact result at each vertex, because in that case every vertex's contribution // is multiplied by 0 or 1 exactly and then summed. This precision ensures that there won't @@ -1000,36 +984,31 @@ impl Patch { /// Map a direction in sphere space to a point in patch space #[inline(always)] - fn project(&self, dir: &na::Vector3) -> na::Point2 { + fn project(&self, dir: &Vector) -> Vec2 { // Project onto each triangle, then select the in-bounds result #[inline(always)] - fn project( - p: &na::Vector3, - x: na::Vector3, - y: na::Vector3, - dir: &na::Vector3, - ) -> na::Point2 { + fn project(p: Vector, x: Vector, y: Vector, dir: Vector) -> Vec2 { // t * dir = p + u * x + v * y // -p = x * u + y * v - t * dir // = [x y dir] [u v -t]^T // [u v -t]^T = [x y dir]^-1 . -p - let m = na::Matrix3::from_columns(&[x, y, *dir]); - (-(m.try_inverse().unwrap().fixed_view::<2, 3>(0, 0) * p)).into() + let m = Matrix::from_cols(x, y, dir); + -(m.try_inverse().unwrap() * p).truncate() } - let left = project(&self.a, self.d - self.c, self.c - self.a, dir); + let left = project(self.a, self.d - self.c, self.c - self.a, *dir); let result = if left.x <= left.y { left } else { - project(&self.a, self.b - self.a, self.d - self.b, dir) + project(self.a, self.b - self.a, self.d - self.b, *dir) }; - result.map(|x| x.clamp(0.0, 1.0)) + result.clamp(Vec2::ZERO, Vec2::ONE) } fn quads(&self, chunk_resolution: u32) -> impl Iterator + '_ { let quad_resolution = chunk_resolution - 1; (0..quad_resolution).flat_map(move |y| { - (0..quad_resolution).map(move |x| Quad::new(self, quad_resolution, [x, y].into())) + (0..quad_resolution).map(move |x| Quad::new(self, quad_resolution, UVec2::new(x, y))) }) } @@ -1049,23 +1028,23 @@ impl Patch { chunk_resolution: u32, ) -> Option + 'a> { let verts = aabb.vertices(); - let v0 = self.project(&verts[0].coords).coords; + let v0 = self.project(&verts[0]); let (lower, upper) = verts[1..] .iter() - .map(|v| self.project(&v.coords).coords) - .fold((v0, v0), |(lower, upper), p| { - (lower.zip_map(&p, f64::min), upper.zip_map(&p, f64::max)) - }); - if lower.iter().any(|&v| v == 1.0) || upper.iter().any(|&v| v == 0.0) { + .map(|v| self.project(v)) + .fold((v0, v0), |(lower, upper), p| (lower.min(p), upper.max(p))); + // Components are clamped to [0, 1], so any component equals 1 iff the largest does, and + // likewise for 0. + if lower.max_element() == 1.0 || upper.min_element() == 0.0 { return None; } let quad_resolution = chunk_resolution - 1; let discretize = |x: f64| ((x * quad_resolution as f64) as u32).min(quad_resolution - 1); // FIXME: wrong units! Reuse bounding - let lower = lower.map(discretize); - let upper = upper.map(discretize); + let lower = UVec2::new(discretize(lower.x), discretize(lower.y)); + let upper = UVec2::new(discretize(upper.x), discretize(upper.y)); Some((lower.y..=upper.y).flat_map(move |y| { - (lower.x..=upper.x).map(move |x| Quad::new(self, quad_resolution, [x, y].into())) + (lower.x..=upper.x).map(move |x| Quad::new(self, quad_resolution, UVec2::new(x, y))) })) } @@ -1089,18 +1068,17 @@ impl Patch { /// Identifies a pair of triangles within a patch struct Quad { /// Row-major order - corners: [na::Vector3; 4], - position: na::Point2, + corners: [Vector; 4], + position: UVec2, } impl Quad { - fn new(patch: &Patch, resolution: u32, position: na::Point2) -> Self { + fn new(patch: &Patch, resolution: u32, position: UVec2) -> Self { let offsets = [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]]; + let position_f = position.as_dvec2(); Self { - corners: offsets.map(|x| { - patch - .get(&((position.cast::() + na::Vector2::from(x)) / f64::from(resolution))) - }), + corners: offsets + .map(|x| patch.get(&((position_f + Vec2::from(x)) / f64::from(resolution)))), position, } } @@ -1113,15 +1091,13 @@ impl Quad { let offsets = [[0, 0], [1, 0], [0, 1], [1, 1]]; let mut result = self.corners; for (v, offset) in result.iter_mut().zip(offsets) { - let sample = self.position + na::Vector2::from(offset); + let sample = self.position + UVec2::from(offset); let displacement = chunk_samples[(sample.y * chunk_resolution + sample.x) as usize]; // We deliberately don't normalize `v` in `v * radius` because we're displacing a // subdivided patch, not the surface of the sphere directly. *v = *v * radius + v.normalize() * f64::from(displacement); } - DisplacedQuad { - corners: result.map(na::Point3::from), - } + DisplacedQuad { corners: result } } fn triangles<'a>( @@ -1141,7 +1117,7 @@ impl Quad { struct DisplacedQuad { /// Row-major order - corners: [na::Point3; 4], + corners: [Vector; 4], } impl DisplacedQuad { @@ -1165,20 +1141,19 @@ fn walk_patch( mut f: impl FnMut(&Quad) -> bool, ) -> Option<(Edge, f64)> { let quad_resolution_f = quad_resolution as f64; - let start = patch.project(&ray.origin.coords); - let mut quad = start.map(|x| { - (x * quad_resolution_f) - .trunc() - .clamp(0.0, quad_resolution_f - 1.0) - }); + let start = patch.project(&ray.origin); + let mut quad = (start * quad_resolution_f) + .trunc() + .clamp(Vec2::ZERO, Vec2::splat(quad_resolution_f - 1.0)); loop { - let candidate = Quad::new(patch, quad_resolution, quad.map(|x| x as u32)); + let candidate = Quad::new(patch, quad_resolution, quad.as_uvec2()); if !f(&candidate) { return None; } // Find the next quad along the ray let (edge, toi) = raycast_quad_edges(ray, &candidate.corners, max_toi)?; - quad += edge.direction().into_inner(); + let step: na::Vector2 = edge.direction().into_inner(); + quad += Vec2::new(step.x, step.y); if quad.x >= quad_resolution_f || quad.y >= quad_resolution_f || quad.x < 0.0 @@ -1193,7 +1168,7 @@ fn walk_patch( #[cfg(test)] mod tests { use approx::{assert_abs_diff_eq, assert_relative_eq}; - use parry3d_f64::{query::ShapeCastStatus, shape::Ball}; + use parry3d_f64::{math::Rotation, query::ShapeCastStatus, shape::Ball}; use crate::cubemap::Face; @@ -1220,22 +1195,22 @@ mod tests { assert!(tri.normal().unwrap().z > 0.0); for vert in &[tri.a, tri.b, tri.c] { assert!(vert.z > 0.0); - for coord in &vert.coords { + for coord in vert.to_array() { assert_eq!(coord.abs(), expected); } - assert_relative_eq!(vert.coords.norm(), 1.0); + assert_relative_eq!(vert.length(), 1.0); } } } - fn ball_contacts(planet: &Planet, pos: Point, radius: Real) -> usize { + fn ball_contacts(planet: &Planet, pos: Vector, radius: Real) -> usize { let dispatcher = PlanetDispatcher.chain(DefaultQueryDispatcher); let ball = Ball { radius }; let mut manifolds = Vec::>::new(); let mut workspace = None; dispatcher .contact_manifolds( - &Isometry::translation(pos.x, pos.y, pos.z), + &Pose::translation(pos.x, pos.y, pos.z), planet, &ball, 0.0, @@ -1258,21 +1233,19 @@ mod tests { // We add 0.1 to PLANET_RADIUS in positive tests below to hack around the issue in // https://github.com/dimforge/parry/pull/148. Can be removed once fix is released. - assert!(ball_contacts(&planet, Point::new(2.0, PLANET_RADIUS + 0.1, 0.0), 1.0) >= 2, + assert!(ball_contacts(&planet, Vector::new(2.0, PLANET_RADIUS + 0.1, 0.0), 1.0) >= 2, "a ball lying on an axis of a planet with an even number of chunks per face overlaps with at least four triangles"); assert_eq!( - ball_contacts(&planet, Point::new(0.0, PLANET_RADIUS + 2.0, 0.0), 1.0), + ball_contacts(&planet, Vector::new(0.0, PLANET_RADIUS + 2.0, 0.0), 1.0), 0 ); - assert!(ball_contacts(&planet, Point::new(-1.0, PLANET_RADIUS + 0.1, 0.0), 1.0) > 0); + assert!(ball_contacts(&planet, Vector::new(-1.0, PLANET_RADIUS + 0.1, 0.0), 1.0) > 0); for i in 0..10 { use std::f64; - let rot = na::UnitQuaternion::from_axis_angle( - &na::Vector3::z_axis(), - (i as f64 / 1000.0) * f64::consts::PI * 1e-4, - ); - let pos = Point::from(rot * na::Vector3::new(0.0, PLANET_RADIUS + 0.1, 0.0)); + let rot = + Rotation::from_axis_angle(Vector::Z, (i as f64 / 1000.0) * f64::consts::PI * 1e-4); + let pos = rot * Vector::new(0.0, PLANET_RADIUS + 0.1, 0.0); assert!(ball_contacts(&planet, dbg!(pos), 1.0) > 0); } } @@ -1289,10 +1262,8 @@ mod tests { PLANET_RADIUS, ); - let pos = Point::from( - Vector::::new(-5_195_083.148, 3_582_099.812, -877_091.267).normalize() - * PLANET_RADIUS, - ); + let pos = + Vector::new(-5_195_083.148, 3_582_099.812, -877_091.267).normalize() * PLANET_RADIUS; assert!(ball_contacts(&planet, pos, BALL_RADIUS) > 0); } @@ -1310,8 +1281,8 @@ mod tests { let impact = PlanetDispatcher .cast_shapes( - &Isometry::translation(PLANET_RADIUS + DISTANCE, 0.0, 0.0), - &Vector::new(-1.0, 0.0, 0.0), + &Pose::translation(PLANET_RADIUS + DISTANCE, 0.0, 0.0), + Vector::new(-1.0, 0.0, 0.0), &planet, &ball, ShapeCastOptions { @@ -1324,10 +1295,10 @@ mod tests { .expect("toi not found"); assert_eq!(impact.status, ShapeCastStatus::Converged); assert_relative_eq!(impact.time_of_impact, DISTANCE - ball.radius); - assert_relative_eq!(impact.witness1, Point::new(PLANET_RADIUS, 0.0, 0.0)); - assert_relative_eq!(impact.witness2, Point::new(-ball.radius, 0.0, 0.0)); - assert_relative_eq!(impact.normal1, Vector::x_axis()); - assert_relative_eq!(impact.normal2, -Vector::x_axis()); + assert_relative_eq!(impact.witness1, Vector::new(PLANET_RADIUS, 0.0, 0.0)); + assert_relative_eq!(impact.witness2, Vector::new(-ball.radius, 0.0, 0.0)); + assert_relative_eq!(impact.normal1, Vector::X); + assert_relative_eq!(impact.normal2, -Vector::X); } #[test] @@ -1342,20 +1313,20 @@ mod tests { let hit = planet .cast_local_ray_and_get_normal( &Ray { - origin: Point::new(PLANET_RADIUS + DISTANCE, 1.0, 1.0), - dir: -Vector::x(), + origin: Vector::new(PLANET_RADIUS + DISTANCE, 1.0, 1.0), + dir: -Vector::X, }, 100.0, true, ) .expect("hit not found"); assert_relative_eq!(hit.time_of_impact, DISTANCE, epsilon = 1e-3); - assert_relative_eq!(hit.normal, Vector::x_axis(), epsilon = 1e-3); + assert_relative_eq!(hit.normal, Vector::X, epsilon = 1e-3); let hit = planet.cast_local_ray_and_get_normal( &Ray { - origin: Point::new(PLANET_RADIUS + DISTANCE, 1.0, 1.0), - dir: Vector::x(), + origin: Vector::new(PLANET_RADIUS + DISTANCE, 1.0, 1.0), + dir: Vector::X, }, 100.0, true, @@ -1373,10 +1344,10 @@ mod tests { PLANET_RADIUS, ); - for &dir in [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()].iter() { + for &dir in [Vector::X, Vector::Y, -Vector::X, -Vector::Y].iter() { let hit = planet.cast_local_ray_and_get_normal( &Ray { - origin: Point::new(1.0, 1.0, PLANET_RADIUS + DISTANCE), + origin: Vector::new(1.0, 1.0, PLANET_RADIUS + DISTANCE), dir, }, 10000.0, @@ -1398,8 +1369,8 @@ mod tests { planet .cast_local_ray_and_get_normal( &Ray { - origin: Point::new(1.0, 1.0, PLANET_RADIUS + DISTANCE), - dir: na::Vector3::new(1.5, 1.5, -1.0).normalize(), + origin: Vector::new(1.0, 1.0, PLANET_RADIUS + DISTANCE), + dir: Vector::new(1.5, 1.5, -1.0).normalize(), }, 1e5, true, @@ -1418,15 +1389,11 @@ mod tests { ); assert!(PlanetDispatcher - .intersection_test( - &Isometry::translation(PLANET_RADIUS, 0.0, 0.0), - &planet, - &ball, - ) + .intersection_test(&Pose::translation(PLANET_RADIUS, 0.0, 0.0), &planet, &ball,) .unwrap()); assert!(!PlanetDispatcher .intersection_test( - &Isometry::translation(PLANET_RADIUS + ball.radius * 2.0, 0.0, 0.0), + &Pose::translation(PLANET_RADIUS + ball.radius * 2.0, 0.0, 0.0), &planet, &ball, ) @@ -1445,13 +1412,13 @@ mod tests { let toi = PlanetDispatcher .cast_shapes_nonlinear( - &NonlinearRigidMotion::constant_position(na::one()), + &NonlinearRigidMotion::constant_position(Pose::identity()), &planet, &NonlinearRigidMotion { - start: Isometry::translation(PLANET_RADIUS + ball.radius + 0.5, 0.0, 0.0), - local_center: na::Point3::origin(), - linvel: -na::Vector3::x(), - angvel: na::zero(), + start: Pose::translation(PLANET_RADIUS + ball.radius + 0.5, 0.0, 0.0), + local_center: Vector::ZERO, + linvel: -Vector::X, + angvel: Vector::ZERO, }, &ball, 0.0, @@ -1462,21 +1429,21 @@ mod tests { .expect("no hit"); assert_eq!(toi.status, ShapeCastStatus::Converged); assert_relative_eq!(toi.time_of_impact, 0.5); - assert_relative_eq!(toi.witness1, na::Point3::new(PLANET_RADIUS, 0.0, 0.0)); - assert_relative_eq!(toi.witness2, na::Point3::new(-ball.radius, 0.0, 0.0)); - assert_relative_eq!(toi.normal1, na::Vector3::x_axis()); - assert_relative_eq!(toi.normal2, -na::Vector3::x_axis()); + assert_relative_eq!(toi.witness1, Vector::new(PLANET_RADIUS, 0.0, 0.0)); + assert_relative_eq!(toi.witness2, Vector::new(-ball.radius, 0.0, 0.0)); + assert_relative_eq!(toi.normal1, Vector::X); + assert_relative_eq!(toi.normal2, -Vector::X); // Same configuration as above, but too far to hit within the allotted time let toi = PlanetDispatcher .cast_shapes_nonlinear( - &NonlinearRigidMotion::constant_position(na::one()), + &NonlinearRigidMotion::constant_position(Pose::identity()), &planet, &NonlinearRigidMotion { - start: Isometry::translation(PLANET_RADIUS + ball.radius + 1.5, 0.0, 0.0), - local_center: na::Point3::origin(), - linvel: -na::Vector3::x(), - angvel: na::zero(), + start: Pose::translation(PLANET_RADIUS + ball.radius + 1.5, 0.0, 0.0), + local_center: Vector::ZERO, + linvel: -Vector::X, + angvel: Vector::ZERO, }, &ball, 0.0, @@ -1502,10 +1469,7 @@ mod tests { for coords in [[0.0, 0.0], [0.0, 1.0], [1.0, 0.0], [1.0, 1.0]] { // Exact equality is intended here, as a prerequisite for neighboring patches to be // seamless. - assert_eq!( - patch.get(&coords.into()), - chunk.direction(res, &coords.into()).into_inner() - ); + assert_eq!(patch.get(&Vec2::from(coords)), corner(&chunk, res, coords)); } } @@ -1519,11 +1483,19 @@ mod tests { }; let patch = Patch::new(&chunk, res); - let coords = [0.1, 0.4].into(); - assert_abs_diff_eq!(patch.project(&patch.get(&coords)), coords, epsilon = 1e-4); + let coords = [0.1, 0.4]; + assert_abs_diff_eq!( + patch.project(&patch.get(&Vec2::from(coords))), + Vec2::from(coords), + epsilon = 1e-4 + ); - let coords = [0.9, 0.7].into(); - assert_abs_diff_eq!(patch.project(&patch.get(&coords)), coords, epsilon = 1e-4); + let coords = [0.9, 0.7]; + assert_abs_diff_eq!( + patch.project(&patch.get(&Vec2::from(coords))), + Vec2::from(coords), + epsilon = 1e-4 + ); } #[test] @@ -1536,7 +1508,7 @@ mod tests { d: [1.0, 0.0, 1.0].into(), }; - let p = patch.project(&na::Vector3::new(1.0, 0.1, 0.1)); + let p = patch.project(&Vector::new(1.0, 0.1, 0.1)); assert!(p.x >= 0.0 && p.x <= 1.0); assert!(p.y >= 0.0 && p.y <= 1.0); } @@ -1552,13 +1524,10 @@ mod tests { }; assert_abs_diff_eq!( - patch.project(&na::Vector3::new(1.0, 1.1, 1.1)), - na::Point2::new(0.1, 0.9) - ); - assert_abs_diff_eq!( - patch.get(&na::Point2::new(0.1, 0.9)), - na::Vector3::new(1.0, 1.1, 1.1) + patch.project(&Vector::new(1.0, 1.1, 1.1)), + Vec2::new(0.1, 0.9) ); + assert_abs_diff_eq!(patch.get(&Vec2::new(0.1, 0.9)), Vector::new(1.0, 1.1, 1.1)); } #[test] @@ -1573,63 +1542,64 @@ mod tests { d: [RESOLUTION as f64, RESOLUTION as f64, Z].into(), }; - let check_ray = |origin, direction, expected_quads: &[[u32; 2]], expected_edge| { - let mut i = 0; - let result = walk_patch( - RESOLUTION, - &patch, - &Ray::new(origin, direction), - 100.0, - |quad| { - let expected = [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]].map(|offset| { - (na::Vector2::from(expected_quads[i]).cast::() - + na::Vector2::from(offset)) - .push(Z) - }); - i += 1; - for (actual, expected) in quad.corners.into_iter().zip(&expected) { - assert_abs_diff_eq!(actual, expected); - } - true - }, - ); - assert_eq!(result.map(|x| x.0), expected_edge); - }; + let check_ray = + |origin: Vector, direction: Vector, expected_quads: &[UVec2], expected_edge| { + let mut i = 0; + let result = walk_patch( + RESOLUTION, + &patch, + &Ray::new(origin, direction), + 100.0, + |quad| { + let expected = + [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]].map(|offset| { + let q = expected_quads[i].as_dvec2() + Vec2::from(offset); + Vector::new(q.x, q.y, Z) + }); + i += 1; + for (actual, expected) in quad.corners.into_iter().zip(&expected) { + assert_abs_diff_eq!(actual, *expected); + } + true + }, + ); + assert_eq!(result.map(|x| x.0), expected_edge); + }; check_ray( - [0.5, 0.5, Z].into(), - [1.0, 0.0, 0.0].into(), - &[[0, 0], [1, 0]], + Vector::new(0.5, 0.5, Z), + Vector::new(1.0, 0.0, 0.0), + &[UVec2::new(0, 0), UVec2::new(1, 0)], Some(Edge::Px), ); check_ray( - [0.5, 0.5, Z].into(), - [-1.0, 0.0, 0.0].into(), - &[[0, 0]], + Vector::new(0.5, 0.5, Z), + Vector::new(-1.0, 0.0, 0.0), + &[UVec2::new(0, 0)], Some(Edge::Nx), ); check_ray( - [1.5, 0.5, Z].into(), - [-1.0, 0.0, 0.0].into(), - &[[1, 0], [0, 0]], + Vector::new(1.5, 0.5, Z), + Vector::new(-1.0, 0.0, 0.0), + &[UVec2::new(1, 0), UVec2::new(0, 0)], Some(Edge::Nx), ); check_ray( - [1.5, 1.5, Z].into(), - [-1.0, 0.0, 0.0].into(), - &[[1, 1], [0, 1]], + Vector::new(1.5, 1.5, Z), + Vector::new(-1.0, 0.0, 0.0), + &[UVec2::new(1, 1), UVec2::new(0, 1)], Some(Edge::Nx), ); check_ray( - [1.5, 1.5, Z].into(), - [0.0, 1.0, 0.0].into(), - &[[1, 1]], + Vector::new(1.5, 1.5, Z), + Vector::new(0.0, 1.0, 0.0), + &[UVec2::new(1, 1)], Some(Edge::Py), ); check_ray( - [1.5, 1.5, Z].into(), - [0.0, -1.0, 0.0].into(), - &[[1, 1], [1, 0]], + Vector::new(1.5, 1.5, Z), + Vector::new(0.0, -1.0, 0.0), + &[UVec2::new(1, 1), UVec2::new(1, 0)], Some(Edge::Ny), ); }