nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-10-26 15:58:30 +01:00
parent 3da333f11c
commit 2b628f9580
16 changed files with 150 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples3d/heightfield3.rs
View File

@@ -54,13 +54,14 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
let collider = match j % 4 {
let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
1 => ColliderBuilder::ball(rad).build(),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
_ => ColliderBuilder::cone(rad, rad).build(),
3 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::capsule_y(rad, rad).build(),
};
colliders.insert(collider, handle, &mut bodies);

5
examples3d/primitives3.rs
View File

@@ -50,13 +50,14 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
let collider = match j % 4 {
let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
1 => ColliderBuilder::ball(rad).build(),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
_ => ColliderBuilder::cone(rad, rad).build(),
3 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::capsule_y(rad, rad).build(),
};
colliders.insert(collider, handle, &mut bodies);

5
examples3d/trimesh3.rs
View File

@@ -64,13 +64,14 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
let collider = match j % 4 {
let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
1 => ColliderBuilder::ball(rad).build(),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
_ => ColliderBuilder::cone(rad, rad).build(),
3 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::capsule_y(rad, rad).build(),
};
colliders.insert(collider, handle, &mut bodies);

13
src/dynamics/mass_properties_capsule.rs
View File

@@ -1,22 +1,24 @@
use crate::dynamics::MassProperties;
use crate::math::Point;
#[cfg(feature = "dim3")]
use crate::math::Rotation;
use crate::{geometry::Capsule, math::Rotation};
impl MassProperties {
pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self {
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);
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_unit_i = cyl_unit_i + ball_unit_i;
let local_com = na::center(&a, &b);
#[cfg(feature = "dim2")]
{
let h = half_height * 2.0;
let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
cap_unit_i += extra;
Self::new(Point::origin(), cap_mass, cap_unit_i * cap_mass)
Self::new(local_com, cap_mass, cap_unit_i * cap_mass)
}
#[cfg(feature = "dim3")]
@@ -25,11 +27,12 @@ impl MassProperties {
let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
cap_unit_i.x += extra;
cap_unit_i.z += extra;
let local_frame = Capsule::new(a, b, radius).rotation_wrt_y();
Self::with_principal_inertia_frame(
Point::origin(),
local_com,
cap_mass,
cap_unit_i * cap_mass,
Rotation::identity(),
local_frame,
)
}
}

86
src/geometry/capsule.rs
View File

@@ -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;

35
src/geometry/collider.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity,
Shape, ShapeType, Triangle, Trimesh,
Segment, Shape, ShapeType, Triangle, Trimesh,
};
#[cfg(feature = "dim3")]
use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, Rounded};
@@ -58,9 +58,14 @@ impl ColliderShape {
ColliderShape(Arc::new(Cuboid::new(half_extents)))
}
/// Initialize a capsule shape aligned with the `y` axis.
pub fn capsule(half_height: f32, radius: f32) -> Self {
ColliderShape(Arc::new(Capsule::new(half_height, radius)))
/// 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)))
}
/// 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.
@@ -157,6 +162,7 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
Some(ShapeType::Cuboid) => deser::<A, Cuboid>(&mut seq)?,
Some(ShapeType::Capsule) => deser::<A, Capsule>(&mut seq)?,
Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
Some(ShapeType::Segment) => deser::<A, Segment>(&mut seq)?,
Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
#[cfg(feature = "dim3")]
@@ -349,25 +355,21 @@ impl ColliderBuilder {
/// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
pub fn capsule_x(half_height: f32, radius: f32) -> Self {
#[cfg(feature = "dim2")]
let rot = -std::f32::consts::FRAC_PI_2;
#[cfg(feature = "dim3")]
let rot = Vector::z() * -std::f32::consts::FRAC_PI_2;
Self::new(ColliderShape::capsule(half_height, radius))
.position(Isometry::new(na::zero(), rot))
let p = Point::from(Vector::x() * half_height);
Self::new(ColliderShape::capsule(-p, p, radius))
}
/// Initialize a new collider builder with a capsule shape aligned with the `y` axis.
pub fn capsule_y(half_height: f32, radius: f32) -> Self {
Self::new(ColliderShape::capsule(half_height, radius))
let p = Point::from(Vector::y() * half_height);
Self::new(ColliderShape::capsule(-p, p, radius))
}
/// Initialize a new collider builder with a capsule shape aligned with the `z` axis.
#[cfg(feature = "dim3")]
pub fn capsule_z(half_height: f32, radius: f32) -> Self {
let rot = Vector::x() * std::f32::consts::FRAC_PI_2;
Self::new(ColliderShape::capsule(half_height, radius))
.position(Isometry::new(na::zero(), rot))
let p = Point::from(Vector::z() * half_height);
Self::new(ColliderShape::capsule(-p, p, radius))
}
/// Initialize a new collider builder with a cuboid shape defined by its half-extents.
@@ -377,11 +379,8 @@ impl ColliderBuilder {
}
/// Initializes a collider builder with a segment shape.
///
/// A segment shape is modeled by a capsule with a 0 radius.
pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
let (pos, half_height) = crate::utils::segment_to_capsule(&a, &b);
Self::new(ColliderShape::capsule(half_height, 0.0)).position(pos)
Self::new(ColliderShape::segment(a, b))
}
/// Initializes a collider builder with a triangle shape.

4
src/geometry/contact_generator/capsule_capsule_contact_generator.rs
View File

@@ -154,8 +154,8 @@ pub fn generate_contacts<'a>(
let pos12 = pos1.inverse() * pos2;
let pos21 = pos12.inverse();
let seg1 = capsule1.segment();
let seg2_1 = capsule2.segment().transformed(&pos12);
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),

4
src/geometry/contact_generator/contact_dispatcher.rs
View File

@@ -99,7 +99,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
| (ShapeType::Cone, _)
| (_, ShapeType::Cone)
| (ShapeType::RoundCylinder, _)
| (_, ShapeType::RoundCylinder) => (
| (_, ShapeType::RoundCylinder)
| (ShapeType::Capsule, _)
| (_, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_pfm_pfm,
..PrimitiveContactGenerator::default()

2
src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
View File

@@ -54,7 +54,7 @@ pub fn generate_contacts<'a>(
return;
}
let segment2 = capsule2.segment();
let segment2 = capsule2.segment;
/*
*

6
src/geometry/mod.rs
View File

@@ -1,6 +1,7 @@
//! Structures related to geometry: colliders, shapes, etc.
pub use self::broad_phase_multi_sap::BroadPhase;
pub use self::capsule::Capsule;
pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
pub use self::collider_set::{ColliderHandle, ColliderSet};
pub use self::contact::{
@@ -22,8 +23,8 @@ pub use self::rounded::{Roundable, Rounded};
pub use self::trimesh::Trimesh;
pub use ncollide::query::Proximity;
/// A capsule shape.
pub type Capsule = ncollide::shape::Capsule<f32>;
/// A segment shape.
pub type Segment = ncollide::shape::Segment<f32>;
/// A cuboid shape.
pub type Cuboid = ncollide::shape::Cuboid<f32>;
/// A triangle shape.
@@ -94,6 +95,7 @@ mod trimesh;
mod waabb;
mod wquadtree;
//mod z_order;
mod capsule;
#[cfg(feature = "dim3")]
mod polygonal_feature_map;
mod rounded;

5
src/geometry/polygonal_feature_map.rs
View File

@@ -1,9 +1,8 @@
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle};
use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Segment, Triangle};
use crate::math::{Point, Vector};
use approx::AbsDiffEq;
use na::{Unit, Vector2};
use ncollide::shape::Segment;
use ncollide::shape::SupportMap;
/// Trait implemented by convex shapes with features with polyhedral approximations.
@@ -11,7 +10,7 @@ pub trait PolygonalFeatureMap: SupportMap<f32> {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace);
}
impl PolygonalFeatureMap for Segment<f32> {
impl PolygonalFeatureMap for Segment {
fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
*out_feature = PolyhedronFace::from(*self);
}

2
src/geometry/polyhedron_feature3d.rs
View File

@@ -40,6 +40,8 @@ impl From<Triangle> for PolyhedronFace {
impl From<Segment<f32>> for PolyhedronFace {
fn from(seg: Segment<f32>) -> Self {
// Vertices have feature ids 0 and 2.
// The segment interior has feature id 1.
Self {
vertices: [seg.a, seg.b, seg.b, seg.b],
vids: [0, 2, 2, 2],

38
src/geometry/shape.rs
View File

@@ -1,5 +1,7 @@
use crate::dynamics::MassProperties;
use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Triangle, Trimesh};
use crate::geometry::{
Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Segment, Triangle, Trimesh,
};
use crate::math::Isometry;
use downcast_rs::{impl_downcast, DowncastSync};
use erased_serde::Serialize;
@@ -24,6 +26,8 @@ pub enum ShapeType {
Cuboid,
/// A capsule shape.
Capsule,
/// A segment shape.
Segment,
/// A triangle shape.
Triangle,
/// A triangle mesh shape.
@@ -205,16 +209,20 @@ impl Shape for Capsule {
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_capsule(density, self.half_height, self.radius)
MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Capsule
}
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((&self.segment as &dyn PolygonalFeatureMap, self.radius))
}
}
impl Shape for Triangle {
@@ -241,6 +249,30 @@ impl Shape for Triangle {
}
}
impl Shape for Segment {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::Segment
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((self as &dyn PolygonalFeatureMap, 0.0))
}
}
impl Shape for Trimesh {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {

3
src_testbed/nphysics_backend.rs
View File

@@ -184,7 +184,8 @@ fn nphysics_collider_from_rapier_collider(
} else if let Some(ball) = shape.as_ball() {
ShapeHandle::new(Ball::new(ball.radius - margin))
} else if let Some(capsule) = shape.as_capsule() {
ShapeHandle::new(Capsule::new(capsule.half_height, capsule.radius))
pos *= capsule.transform_wrt_y();
ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
} else if let Some(heightfield) = shape.as_heightfield() {
ShapeHandle::new(heightfield.clone())
} else {

2
src_testbed/objects/capsule.rs
View File

@@ -19,7 +19,7 @@ impl Capsule {
window: &mut window::Window,
) -> Capsule {
let r = capsule.radius;
let h = capsule.half_height * 2.0;
let h = capsule.half_height() * 2.0;
#[cfg(feature = "dim2")]
let node = window.add_planar_capsule(r, h);
#[cfg(feature = "dim3")]

11
src_testbed/physx_backend.rs
View File

@@ -433,8 +433,15 @@ fn physx_collider_from_rapier_collider(
} else if let Some(ball) = shape.as_ball() {
ColliderDesc::Sphere(ball.radius)
} else if let Some(capsule) = shape.as_capsule() {
let rot = UnitQuaternion::rotation_between(&Vector3::x(), &Vector3::y());
local_pose *= rot.unwrap_or(UnitQuaternion::identity());
let center = capsule.center();
let mut dir = capsule.segment.b - capsule.segment.a;
if dir.x < 0.0 {
dir = -dir;
}
let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
ColliderDesc::Capsule(capsule.radius, capsule.height())
} else if let Some(trimesh) = shape.as_trimesh() {
ColliderDesc::TriMesh {