From 35d16bb90bd4fb447b70d34d97caa8ba38247361 Mon Sep 17 00:00:00 2001 From: Zhixing Zhang Date: Mon, 1 Jul 2024 22:12:08 -0700 Subject: [PATCH] Add `clone_scaled` to Shape traits --- src/shape/shape.rs | 195 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 187 insertions(+), 8 deletions(-) diff --git a/src/shape/shape.rs b/src/shape/shape.rs index 2c9aa64d..aad33ebf 100644 --- a/src/shape/shape.rs +++ b/src/shape/shape.rs @@ -21,6 +21,7 @@ use crate::shape::{ConvexPolyhedron, RoundConvexPolyhedron}; #[cfg(feature = "std")] use crate::shape::{ConvexPolygon, RoundConvexPolygon}; use downcast_rs::{impl_downcast, DowncastSync}; +use either::Either::{Left, Right}; use na::{RealField, Unit}; use num::Zero; use num_derive::FromPrimitive; @@ -282,6 +283,9 @@ pub trait Shape: RayCast + PointQuery + DowncastSync { #[cfg(feature = "std")] fn clone_box(&self) -> Box; + #[cfg(feature = "std")] + fn clone_scaled(&self, scale: &Vector, num_subdivisions: u32) -> Option>; + /// Computes the [`Aabb`] of this shape with the given position. fn compute_aabb(&self, position: &Isometry) -> Aabb { self.compute_local_aabb().transform_by(position) @@ -585,6 +589,12 @@ impl Shape for Ball { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&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))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -642,6 +652,11 @@ impl Shape for Cuboid { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.scaled(scale))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -701,6 +716,12 @@ impl Shape for Capsule { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&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))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -752,6 +773,11 @@ impl Shape for Triangle { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.scaled(scale))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -815,6 +841,11 @@ impl Shape for Segment { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.scaled(scale))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -874,6 +905,26 @@ impl Shape for Compound { Box::new(self.clone()) } + fn clone_scaled(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + use super::SharedShape; + + let scaled: Vec<_> = self + .shapes() + .iter() + .map(|(pos, shape)| { + let scaled_shape = shape.clone_scaled(scale, num_subdivisions)?; + Some(( + Isometry::from_parts( + (pos.translation.vector.component_mul(scale)).into(), + pos.rotation, + ), + SharedShape(scaled_shape.into()), + )) + }) + .collect::>>()?; + Some(Box::new(Compound::new(scaled))) + } + fn compute_local_aabb(&self) -> Aabb { *self.local_aabb() } @@ -922,6 +973,10 @@ impl Shape for Polyline { Box::new(self.clone()) } + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.clone().scaled(scale))) + } + fn compute_local_aabb(&self) -> Aabb { *self.local_aabb() } @@ -968,6 +1023,10 @@ impl Shape for TriMesh { Box::new(self.clone()) } + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.clone().scaled(scale))) + } + fn compute_local_aabb(&self) -> Aabb { *self.local_aabb() } @@ -1015,6 +1074,10 @@ impl Shape for HeightField { Box::new(self.clone()) } + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.clone().scaled(scale))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -1057,6 +1120,10 @@ impl Shape for ConvexPolygon { Box::new(self.clone()) } + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.clone().scaled(scale)?)) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -1120,6 +1187,10 @@ impl Shape for ConvexPolyhedron { Box::new(self.clone()) } + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.clone().scaled(scale)?)) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -1184,6 +1255,12 @@ impl Shape for Cylinder { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&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))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -1236,6 +1313,12 @@ impl Shape for Cone { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&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))) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -1290,6 +1373,11 @@ impl Shape for HalfSpace { Box::new(*self) } + #[cfg(feature = "std")] + fn clone_scaled(&self, scale: &Vector, _num_subdivisions: u32) -> Option> { + Some(Box::new(self.clone().scaled(scale)?)) + } + fn compute_local_aabb(&self) -> Aabb { self.local_aabb() } @@ -1328,13 +1416,18 @@ impl Shape for HalfSpace { } macro_rules! impl_shape_for_round_shape( - ($($S: ty, $Tag: ident);*) => {$( + ($S: ty, $Tag: ident, $t: tt) => { impl Shape for RoundShape<$S> { #[cfg(feature = "std")] fn clone_box(&self) -> Box { Box::new(self.clone()) } + #[cfg(feature = "std")] + fn clone_scaled(&self, scale: &Vector, num_subdivisions: u32) -> Option> { + $t(self, scale, num_subdivisions) + } + fn compute_local_aabb(&self) -> Aabb { self.inner_shape.local_aabb().loosened(self.border_radius) } @@ -1381,22 +1474,108 @@ macro_rules! impl_shape_for_round_shape( Some((&self.inner_shape as &dyn PolygonalFeatureMap, self.border_radius)) } } - )*} + } +); + +impl_shape_for_round_shape!( + Cuboid, + RoundCuboid, + (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + let shape = RoundShape { + border_radius: this.border_radius, + inner_shape: this.inner_shape.scaled(scale), + }; + Some(Box::new(shape) as Box) + }) ); impl_shape_for_round_shape!( - Cuboid, RoundCuboid; - Triangle, RoundTriangle + Triangle, + RoundTriangle, + (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + let shape = RoundShape { + border_radius: this.border_radius, + inner_shape: this.inner_shape.scaled(scale), + }; + Some(Box::new(shape) as Box) + }) ); + #[cfg(feature = "dim2")] #[cfg(feature = "std")] -impl_shape_for_round_shape!(ConvexPolygon, RoundConvexPolygon); +impl_shape_for_round_shape!( + ConvexPolygon, + RoundConvexPolygon, + (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + let shape = RoundShape { + border_radius: this.border_radius, + inner_shape: this.inner_shape.clone().scaled(scale)?, + }; + Some(Box::new(shape) as Box) + }) +); + #[cfg(feature = "dim3")] impl_shape_for_round_shape!( - Cylinder, RoundCylinder; - Cone, RoundCone + Cylinder, + RoundCylinder, + (|this: &Self, scale: &Vector, num_subdivisions: u32| { + Some( + this.inner_shape + .scaled(scale, num_subdivisions)? + .either::<_, _, Box>( + |inner_shape| { + Box::new(RoundShape { + border_radius: this.border_radius, + inner_shape, + }) + }, + |inner_shape| { + Box::new(RoundShape { + border_radius: this.border_radius, + inner_shape, + }) + }, + ), + ) + }) +); +#[cfg(feature = "dim3")] +impl_shape_for_round_shape!( + Cone, + RoundCone, + (|this: &Self, scale: &Vector, num_subdivisions: u32| { + Some( + this.inner_shape + .scaled(scale, num_subdivisions)? + .either::<_, _, Box>( + |inner_shape| { + Box::new(RoundShape { + border_radius: this.border_radius, + inner_shape, + }) + }, + |inner_shape| { + Box::new(RoundShape { + border_radius: this.border_radius, + inner_shape, + }) + }, + ), + ) + }) ); #[cfg(feature = "dim3")] #[cfg(feature = "std")] -impl_shape_for_round_shape!(ConvexPolyhedron, RoundConvexPolyhedron); +impl_shape_for_round_shape!( + ConvexPolyhedron, + RoundConvexPolyhedron, + (|this: &Self, scale: &Vector, _num_subdivisions: u32| { + let shape = RoundShape { + border_radius: this.border_radius, + inner_shape: this.inner_shape.clone().scaled(scale)?, + }; + Some(Box::new(shape) as Box) + }) +);