diff options
Diffstat (limited to 'src/geometry/capsule.rs')
| -rw-r--r-- | src/geometry/capsule.rs | 192 |
1 files changed, 0 insertions, 192 deletions
diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs deleted file mode 100644 index 54736cc..0000000 --- a/src/geometry/capsule.rs +++ /dev/null @@ -1,192 +0,0 @@ -use crate::geometry::{Ray, RayIntersection, AABB}; -use crate::math::{Isometry, Point, Rotation, Vector}; -use approx::AbsDiffEq; -use na::Unit; -use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; -use ncollide::shape::{FeatureId, Segment, SupportMap}; - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a round segment. -pub struct Capsule { - /// The axis and endpoint of the capsule. - pub segment: Segment<f32>, - /// The radius of the capsule. - pub radius: f32, -} - -impl Capsule { - /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius. - pub fn new_x(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::x() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius. - pub fn new_y(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::y() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius. - #[cfg(feature = "dim3")] - pub fn new_z(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::z() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. - pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self { - let segment = Segment::new(a, b); - Self { segment, radius } - } - - /// The axis-aligned bounding box of this capsule. - pub fn aabb(&self, pos: &Isometry<f32>) -> AABB { - let a = pos * self.segment.a; - let b = pos * 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()) - } - - /// The height of this capsule. - pub fn height(&self) -> f32 { - (self.segment.b - self.segment.a).norm() - } - - /// The half-height of this capsule. - pub fn half_height(&self) -> f32 { - self.height() / 2.0 - } - - /// The center of this capsule. - pub fn center(&self) -> Point<f32> { - na::center(&self.segment.a, &self.segment.b) - } - - /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. - pub fn transform_by(&self, pos: &Isometry<f32>) -> Self { - Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) - } - - /// The rotation `r` such that `r * Y` is collinear with `b - a`. - pub fn rotation_wrt_y(&self) -> Rotation<f32> { - 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()) - } - } - - /// 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<f32> { - let rot = self.rotation_wrt_y(); - Isometry::from_parts(self.center().coords.into(), rot) - } -} - -impl SupportMap<f32> for Capsule { - fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> { - let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); - self.local_support_point_toward(&dir) - } - - fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> { - if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { - self.segment.a + **dir * self.radius - } else { - self.segment.b + **dir * self.radius - } - } -} - -impl RayCast<f32> for Capsule { - fn toi_and_normal_with_ray( - &self, - m: &Isometry<f32>, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option<RayIntersection> { - let ls_ray = ray.inverse_transform_by(m); - - ncollide::query::ray_intersection_with_support_map_with_params( - &Isometry::identity(), - self, - &mut VoronoiSimplex::new(), - &ls_ray, - max_toi, - solid, - ) - .map(|mut res| { - res.normal = m * res.normal; - res - }) - } -} - -// TODO: this code has been extracted from ncollide and added here -// so we can modify it to fit with our new definition of capsule. -// We should find a way to avoid this code duplication. -impl PointQuery<f32> for Capsule { - #[inline] - fn project_point( - &self, - m: &Isometry<f32>, - pt: &Point<f32>, - solid: bool, - ) -> PointProjection<f32> { - let seg = Segment::new(self.segment.a, self.segment.b); - let proj = seg.project_point(m, pt, solid); - let dproj = *pt - proj.point; - - if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) { - let inside = dist <= self.radius; - if solid && inside { - return PointProjection::new(true, *pt); - } else { - return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); - } - } else if solid { - return PointProjection::new(true, *pt); - } - - #[cfg(feature = "dim2")] - if let Some(dir) = seg.normal() { - let dir = m * *dir; - 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)) - } - - #[cfg(feature = "dim3")] - if let Some(dir) = seg.direction() { - use crate::utils::WBasis; - let dir = m * dir.orthonormal_basis()[0]; - 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)) - } - } - - #[inline] - fn project_point_with_feature( - &self, - m: &Isometry<f32>, - pt: &Point<f32>, - ) -> (PointProjection<f32>, FeatureId) { - (self.project_point(m, pt, false), FeatureId::Face(0)) - } -} |