diff options
Diffstat (limited to 'src')
42 files changed, 1628 insertions, 477 deletions
diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 22e7da5..d64839c 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -91,7 +91,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + /// 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<f32> { let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); self.principal_inertia_local_frame.to_rotation_matrix() @@ -103,7 +103,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> { let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); self.principal_inertia_local_frame.to_rotation_matrix() diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index 5f08958..3b1b214 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,30 +1,9 @@ use crate::dynamics::MassProperties; #[cfg(feature = "dim3")] use crate::geometry::Capsule; -use crate::math::{Point, PrincipalAngularInertia, Vector}; +use crate::math::Point; impl MassProperties { - fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia<f32>) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self { let half_height = (b - a).norm() / 2.0; let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs new file mode 100644 index 0000000..12f831f --- /dev/null +++ b/src/dynamics/mass_properties_cone.rs @@ -0,0 +1,29 @@ +use crate::dynamics::MassProperties; +use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; + +impl MassProperties { + pub(crate) fn cone_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia<f32>) { + let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; + let principal = sq_radius * 3.0 / 10.0; + + (volume, Vector::new(off_principal, principal, off_principal)) + } + + pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius); + let cyl_mass = cyl_vol * density; + + Self::with_principal_inertia_frame( + Point::new(0.0, -half_height / 2.0, 0.0), + cyl_mass, + cyl_unit_i * cyl_mass, + Rotation::identity(), + ) + } +} diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs new file mode 100644 index 0000000..7c8054a --- /dev/null +++ b/src/dynamics/mass_properties_cylinder.rs @@ -0,0 +1,40 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::math::{Point, Rotation}; +use crate::math::{PrincipalAngularInertia, Vector}; + +impl MassProperties { + pub(crate) fn cylinder_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia<f32>) { + #[cfg(feature = "dim2")] + { + Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) + } + + #[cfg(feature = "dim3")] + { + let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; + + let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); + (volume, inertia) + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); + let cyl_mass = cyl_vol * density; + + Self::with_principal_inertia_frame( + Point::origin(), + cyl_mass, + cyl_unit_i * cyl_mass, + Rotation::identity(), + ) + } +} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs index c87e888..8b0b811 100644 --- a/src/dynamics/mass_properties_polygon.rs +++ b/src/dynamics/mass_properties_polygon.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] // TODO: remove this + use crate::dynamics::MassProperties; use crate::math::Point; diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 4499d95..10cdd29 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -22,7 +22,10 @@ mod joint; mod mass_properties; mod mass_properties_ball; mod mass_properties_capsule; +#[cfg(feature = "dim3")] +mod mass_properties_cone; mod mass_properties_cuboid; +mod mass_properties_cylinder; #[cfg(feature = "dim2")] mod mass_properties_polygon; mod rigid_body; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index af1fb4a..9fa5a8e 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -218,6 +218,7 @@ impl RigidBody { let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } + pub(crate) fn integrate(&mut self, dt: f32) { self.position = self.integrate_velocity(dt) * self.position; } @@ -334,7 +335,7 @@ impl RigidBody { } } -/// A builder for rigid-bodies. +/// A builder for rigid-bodies. pub struct RigidBodyBuilder { position: Isometry<f32>, linvel: Vector<f32>, diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 8e69d24..3562c2e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -335,7 +335,7 @@ impl SAPAxis { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct SAPRegion { - axii: [SAPAxis; DIM], + axes: [SAPAxis; DIM], existing_proxies: BitVec, #[cfg_attr(feature = "serde-serialize", serde(skip))] to_insert: Vec<usize>, // Workspace @@ -344,14 +344,14 @@ struct SAPRegion { impl SAPRegion { pub fn new(bounds: AABB<f32>) -> Self { - let axii = [ + let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), #[cfg(feature = "dim3")] SAPAxis::new(bounds.mins.z, bounds.maxs.z), ]; SAPRegion { - axii, + axes, existing_proxies: BitVec::new(), to_insert: Vec::new(), need_update: false, @@ -386,15 +386,15 @@ impl SAPRegion { // Update endpoints. let mut deleted_any = false; for dim in 0..DIM { - self.axii[dim].update_endpoints(dim, proxies, reporting); - deleted_any = self.axii[dim] + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted_any = self.axes[dim] .delete_out_of_bounds_proxies(&mut self.existing_proxies) || deleted_any; } if deleted_any { for dim in 0..DIM { - self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } } @@ -404,9 +404,9 @@ impl SAPRegion { if !self.to_insert.is_empty() { // Insert new proxies. for dim in 1..DIM { - self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None); } - self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); self.to_insert.clear(); } } diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs index 0d754af..54736cc 100644 --- a/src/geometry/capsule.rs +++ b/src/geometry/capsule.rs @@ -1,18 +1,16 @@ -use crate::geometry::AABB; +use crate::geometry::{Ray, RayIntersection, AABB}; use crate::math::{Isometry, Point, Rotation, Vector}; use approx::AbsDiffEq; use na::Unit; -use ncollide::query::{PointProjection, PointQuery}; -use ncollide::shape::{FeatureId, Segment}; +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 segment with a radius. +/// A capsule shape defined as a round segment. pub struct Capsule { - /// The first endpoint of the capsule. - pub a: Point<f32>, - /// The second enpdoint of the capsule. - pub b: Point<f32>, + /// The axis and endpoint of the capsule. + pub segment: Segment<f32>, /// The radius of the capsule. pub radius: f32, } @@ -39,13 +37,14 @@ impl Capsule { /// 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 { - Self { a, b, radius } + 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.a; - let b = pos * self.b; + 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()) @@ -53,7 +52,7 @@ impl Capsule { /// The height of this capsule. pub fn height(&self) -> f32 { - (self.b - self.a).norm() + (self.segment.b - self.segment.a).norm() } /// The half-height of this capsule. @@ -63,17 +62,17 @@ impl Capsule { /// The center of this capsule. pub fn center(&self) -> Point<f32> { - na::center(&self.a, &self.b) + 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.a, pos * self.b, self.radius) + 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.b - self.a; + let mut dir = self.segment.b - self.segment.a; if dir.y < 0.0 { dir = -dir; } @@ -96,24 +95,49 @@ impl Capsule { } } -// impl SupportMap<f32> 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_toward(&self, dir: &Unit<Vector>) -> Point { -// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) { -// self.a + **dir * self.radius -// } else { -// self.b + **dir * self.radius -// } -// } -// } +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. -// Wa should find a way to avoid this code duplication. +// We should find a way to avoid this code duplication. impl PointQuery<f32> for Capsule { #[inline] fn project_point( @@ -122,7 +146,7 @@ impl PointQuery<f32> for Capsule { pt: &Point<f32>, solid: bool, ) -> PointProjection<f32> { - let seg = Segment::new(self.a, self.b); + let seg = Segment::new(self.segment.a, self.segment.b); let proj = seg.project_point(m, pt, solid); let dproj = *pt - proj.point; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 7c293b6..522f002 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,145 +1,189 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity, + Segment, Shape, ShapeType, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; -use ncollide::query::RayCast; -use num::Zero; +use ncollide::bounding_volume::AABB; +use std::ops::Deref; +use std::sync::Arc; +/// The shape of a collider. #[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// An enum grouping all the possible shape of a collider. -pub enum Shape { - /// A ball shape. - Ball(Ball), - /// A convex polygon shape. - Polygon(Polygon), - /// A cuboid shape. - Cuboid(Cuboid), - /// A capsule shape. - Capsule(Capsule), - /// A triangle shape. - Triangle(Triangle), - /// A triangle mesh shape. - Trimesh(Trimesh), - /// A heightfield shape. - HeightField(HeightField), +pub struct ColliderShape(pub Arc<dyn Shape>); + +impl Deref for ColliderShape { + type Target = dyn Shape; + fn deref(&self) -> &dyn Shape { + &*self.0 + } } -impl Shape { - /// Gets a reference to the underlying ball shape, if `self` is one. - pub fn as_ball(&self) -> Option<&Ball> { - match self { - Shape::Ball(b) => Some(b), - _ => None, - } +impl ColliderShape { + /// Initialize a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + ColliderShape(Arc::new(Ball::new(radius))) } - /// Gets a reference to the underlying polygon shape, if `self` is one. - pub fn as_polygon(&self) -> Option<&Polygon> { - match self { - Shape::Polygon(p) => Some(p), - _ => None, - } + /// Initialize a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] + pub fn cylinder(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// Gets a reference to the underlying cuboid shape, if `self` is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - match self { - Shape::Cuboid(c) => Some(c), - _ => None, - } + /// Initialize a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self { + ColliderShape(Arc::new(RoundCylinder::new( + half_height, + radius, + border_radius, + ))) } - /// Gets a reference to the underlying capsule shape, if `self` is one. - pub fn as_capsule(&self) -> Option<&Capsule> { - match self { - Shape::Capsule(c) => Some(c), - _ => None, - } + /// Initialize a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cone::new(half_height, radius))) } - /// Gets a reference to the underlying triangle mesh shape, if `self` is one. - pub fn as_trimesh(&self) -> Option<&Trimesh> { - match self { - Shape::Trimesh(c) => Some(c), - _ => None, - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector<f32>) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// Gets a reference to the underlying heightfield shape, if `self` is one. - pub fn as_heightfield(&self) -> Option<&HeightField> { - match self { - Shape::HeightField(h) => Some(h), - _ => None, - } + /// Initialize a capsule shape from its endpoints and radius. + pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(a, b, radius))) } - /// Gets a reference to the underlying triangle shape, if `self` is one. - pub fn as_triangle(&self) -> Option<&Triangle> { - match self { - Shape::Triangle(c) => Some(c), - _ => None, - } + /// Initialize a segment shape from its endpoints. + pub fn segment(a: Point<f32>, b: Point<f32>) -> Self { + ColliderShape(Arc::new(Segment::new(a, b))) + } + + /// Initializes a triangle shape. + pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self { + ColliderShape(Arc::new(Triangle::new(a, b, c))) + } + + /// Initializes a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self { + ColliderShape(Arc::new(Trimesh::new(vertices, indices))) } - /// Computes the axis-aligned bounding box of this shape. - pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> { - match self { - Shape::Ball(ball) => ball.bounding_volume(position), - Shape::Polygon(poly) => poly.aabb(position), - Shape::Capsule(caps) => caps.aabb(position), - Shape::Cuboid(cuboid) => cuboid.bounding_volume(position), - Shape::Triangle(triangle) => triangle.bounding_volume(position), - Shape::Trimesh(trimesh) => trimesh.aabb(position), - Shape::HeightField(heightfield) => heightfield.bounding_volume(position), + /// Initializes an 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<f32>, scale: Vector<f32>) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } + + /// Initializes an 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<f32>, scale: Vector<f32>) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } +} + +#[cfg(feature = "serde-serialize")] +impl serde::Serialize for ColliderShape { + fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error> + where + S: serde::Serializer, + { + use crate::serde::ser::SerializeStruct; + + if let Some(ser) = self.0.as_serialize() { + let typ = self.0.shape_type(); + let mut state = serializer.serialize_struct("ColliderShape", 2)?; + state.serialize_field("tag", &(typ as i32))?; + state.serialize_field("inner", ser)?; + state.end() + } else { + Err(serde::ser::Error::custom( + "Found a non-serializable custom shape.", + )) } } +} - /// Computes the first intersection point between a ray in this collider. - /// - /// Some shapes are not supported yet and will always return `None`. - /// - /// # Parameters - /// - `position`: the position of this shape. - /// - `ray`: the ray to cast. - /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. - pub fn cast_ray( - &self, - position: &Isometry<f32>, - ray: &Ray, - max_toi: f32, - ) -> Option<RayIntersection> { - match self { - Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), - Shape::Polygon(_poly) => None, - Shape::Capsule(caps) => { - let pos = position * caps.transform_wrt_y(); - let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); - caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) - } - Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), - #[cfg(feature = "dim2")] - Shape::Triangle(_) | Shape::Trimesh(_) => { - // This is not implemented yet in 2D. - None +#[cfg(feature = "serde-serialize")] +impl<'de> serde::Deserialize<'de> for ColliderShape { + fn deserialize<D>(deserializer: D) -> Result<Self, D::Error> + where + D: serde::Deserializer<'de>, + { + struct Visitor {}; + impl<'de> serde::de::Visitor<'de> for Visitor { + type Value = ColliderShape; + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(formatter, "one shape type tag and the inner shape data") } - #[cfg(feature = "dim3")] - Shape::Triangle(triangle) => { - triangle.toi_and_normal_with_ray(position, ray, max_toi, true) - } - #[cfg(feature = "dim3")] - Shape::Trimesh(trimesh) => { - trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) - } - Shape::HeightField(heightfield) => { - heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) + + fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error> + |