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

Start adding cylinders.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-10-12 18:33:58 +02:00
parent f8acf6a5e9
commit faec3d5d46
14 changed files with 375 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/dynamics/mass_properties_capsule.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::geometry::Capsule;
use crate::math::{Point, PrincipalAngularInertia, Vector};
use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
impl MassProperties {
fn cylinder_y_volume_unit_inertia(
@@ -57,4 +57,16 @@ impl MassProperties {
)
}
}
#[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);
Self::with_principal_inertia_frame(
Point::origin(),
cyl_vol * density,
cyl_unit_i * density,
Rotation::identity(),
)
}
}

48
src/geometry/collider.rs
View File

@@ -1,7 +1,9 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
#[cfg(feature = "dim3")]
use crate::geometry::PolygonalFeatureMap;
use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
Proximity, Ray, RayIntersection, Triangle, Trimesh,
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph,
Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh,
};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use na::Point3;
@@ -27,6 +29,9 @@ pub enum Shape {
Trimesh(Trimesh),
/// A heightfield shape.
HeightField(HeightField),
#[cfg(feature = "dim3")]
/// A cylindrical shape.
Cylinder(Cylinder),
}
impl Shape {
@@ -86,6 +91,25 @@ impl Shape {
}
}
/// Gets a reference to the underlying cylindrical shape, if `self` is one.
pub fn as_cylinder(&self) -> Option<&Cylinder> {
match self {
Shape::Cylinder(c) => Some(c),
_ => None,
}
}
/// gets a reference to this shape seen as a PolygonalFeatureMap.
#[cfg(feature = "dim3")]
pub fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
match self {
Shape::Triangle(t) => Some(t),
Shape::Cuboid(c) => Some(c),
Shape::Cylinder(c) => Some(c),
_ => None,
}
}
/// Computes the axis-aligned bounding box of this shape.
pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
match self {
@@ -96,6 +120,7 @@ impl Shape {
Shape::Triangle(triangle) => triangle.bounding_volume(position),
Shape::Trimesh(trimesh) => trimesh.aabb(position),
Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
Shape::Cylinder(cylinder) => cylinder.bounding_volume(position),
}
}
@@ -139,6 +164,10 @@ impl Shape {
Shape::HeightField(heightfield) => {
heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
}
#[cfg(feature = "dim3")]
Shape::Cylinder(cylinder) => {
cylinder.toi_and_normal_with_ray(position, ray, max_toi, true)
}
}
}
}
@@ -242,9 +271,12 @@ impl Collider {
Shape::Capsule(caps) => {
MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius)
}
Shape::Triangle(_) => MassProperties::zero(),
Shape::Trimesh(_) => MassProperties::zero(),
Shape::HeightField(_) => MassProperties::zero(),
Shape::Triangle(_) | Shape::Trimesh(_) | Shape::HeightField(_) => {
MassProperties::zero()
}
Shape::Cylinder(c) => {
MassProperties::from_cylinder(self.density, c.half_height, c.radius)
}
}
}
}
@@ -291,6 +323,12 @@ impl ColliderBuilder {
Self::new(Shape::Ball(Ball::new(radius)))
}
/// Initialize a new collider builder with a cylindrical shape defined by its half-height
/// (along along the y axis) and its radius.
pub fn cylinder(half_height: f32, radius: f32) -> Self {
Self::new(Shape::Cylinder(Cylinder::new(half_height, radius)))
}
/// Initialize a new collider builder with a cuboid shape defined by its half-extents.
#[cfg(feature = "dim2")]
pub fn cuboid(hx: f32, hy: f32) -> Self {

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

@@ -12,6 +12,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true),
Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true),
Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true),
Shape::Cylinder(cylinder2) => do_generate_contacts(cylinder2, ball1, ctxt, true),
_ => unimplemented!(),
}
} else if let Shape::Ball(ball2) = ctxt.shape2 {
@@ -19,6 +20,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false),
Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false),
Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false),
Shape::Cylinder(cylinder1) => do_generate_contacts(cylinder1, ball2, ctxt, false),
_ => unimplemented!(),
}
}

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

@@ -1,6 +1,7 @@
use crate::geometry::contact_generator::{
ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator,
TrimeshShapeContactGeneratorWorkspace,
};
use crate::geometry::Shape;
use std::any::Any;
@@ -73,7 +74,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
| (Shape::Triangle(_), Shape::Ball(_))
| (Shape::Ball(_), Shape::Triangle(_))
| (Shape::Capsule(_), Shape::Ball(_))
| (Shape::Ball(_), Shape::Capsule(_)) => (
| (Shape::Ball(_), Shape::Capsule(_))
| (Shape::Cylinder(_), Shape::Ball(_))
| (Shape::Ball(_), Shape::Cylinder(_)) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_ball_convex,
..PrimitiveContactGenerator::default()
@@ -94,6 +97,13 @@ impl ContactDispatcher for DefaultContactDispatcher {
},
None,
),
(Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_pfm_pfm,
..PrimitiveContactGenerator::default()
},
Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
),
_ => (PrimitiveContactGenerator::default(), None),
}
}

6
src/geometry/contact_generator/mod.rs
View File

@@ -18,6 +18,10 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang
pub use self::heightfield_shape_contact_generator::{
generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
};
#[cfg(feature = "dim3")]
pub use self::pfm_pfm_contact_generator::{
generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace,
};
pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
pub use self::trimesh_shape_contact_generator::{
generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
@@ -39,6 +43,8 @@ mod cuboid_cuboid_contact_generator;
mod cuboid_polygon_contact_generator;
mod cuboid_triangle_contact_generator;
mod heightfield_shape_contact_generator;
#[cfg(feature = "dim3")]
mod pfm_pfm_contact_generator;
mod polygon_polygon_contact_generator;
mod trimesh_shape_contact_generator;

123
src/geometry/contact_generator/pfm_pfm_contact_generator.rs Normal file
View File

@@ -0,0 +1,123 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
use crate::math::{Isometry, Vector};
use na::Unit;
use ncollide::query;
use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
pub struct PfmPfmContactManifoldGeneratorWorkspace {
simplex: VoronoiSimplex<f32>,
last_gjk_dir: Option<Unit<Vector<f32>>>,
last_optimal_dir: Option<Unit<Vector<f32>>>,
feature1: PolyhedronFace,
feature2: PolyhedronFace,
}
impl Default for PfmPfmContactManifoldGeneratorWorkspace {
fn default() -> Self {
Self {
simplex: VoronoiSimplex::new(),
last_gjk_dir: None,
last_optimal_dir: None,
feature1: PolyhedronFace::new(),
feature2: PolyhedronFace::new(),
}
}
}
pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some(pfm1), Some(pfm2)) = (
ctxt.collider1.shape().as_polygonal_feature_map(),
ctxt.collider2.shape().as_polygonal_feature_map(),
) {
do_generate_contacts(pfm1, pfm2, ctxt);
ctxt.manifold.update_warmstart_multiplier();
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
}
fn do_generate_contacts(
pfm1: &dyn PolygonalFeatureMap,
pfm2: &dyn PolygonalFeatureMap,
ctxt: &mut PrimitiveContactGenerationContext,
) {
let pos12 = ctxt.position1.inverse() * ctxt.position2;
let pos21 = pos12.inverse();
// if ctxt.manifold.try_update_contacts(&pos12) {
// return;
// }
let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
.workspace
.as_mut()
.expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
.downcast_mut()
.expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
let contact = query::contact_support_map_support_map_with_params(
&Isometry::identity(),
pfm1,
&pos12,
pfm2,
ctxt.prediction_distance,
&mut workspace.simplex,
workspace.last_gjk_dir,
);
let old_manifold_points = ctxt.manifold.points.clone();
ctxt.manifold.points.clear();
match contact {
GJKResult::ClosestPoints(local_p1, local_p2, dir) => {
workspace.last_gjk_dir = Some(dir);
let normal1 = dir;
let normal2 = pos21 * -dir;
pfm1.local_support_feature(&normal1, &mut workspace.feature1);
pfm2.local_support_feature(&normal2, &mut workspace.feature2);
workspace.feature2.transform_by(&pos12);
// PolyhedronFace::contacts(
// ctxt.prediction_distance,
// &workspace.feature1,
// &normal1,
// &workspace.feature2,
// &pos21,
// ctxt.manifold,
// );
println!(
"Contact patatrac: {:?}, {:?}, {}, {}",
ctxt.manifold.points.len(),
ctxt.position1 * dir,
workspace.feature1.num_vertices,
workspace.feature2.num_vertices
);
if ctxt.manifold.all_contacts().is_empty() {
// Add at least the deepest contact.
let dist = (local_p2 - local_p1).dot(&dir);
ctxt.manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: 0, // FIXME
fid2: 0, // FIXME
dist,
});
}
// Adjust points to take the radius into account.
ctxt.manifold.local_n1 = *normal1;
ctxt.manifold.local_n2 = *normal2;
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = 0.0;
}
GJKResult::NoIntersection(dir) => {
workspace.last_gjk_dir = Some(dir);
}
_ => {}
}
}

7
src/geometry/mod.rs
View File

@@ -30,6 +30,9 @@ pub type Triangle = ncollide::shape::Triangle<f32>;
pub type Ball = ncollide::shape::Ball<f32>;
/// A heightfield shape.
pub type HeightField = ncollide::shape::HeightField<f32>;
/// A cylindrical shape.
#[cfg(feature = "dim3")]
pub type Cylinder = ncollide::shape::Cylinder<f32>;
/// An axis-aligned bounding box.
pub type AABB = ncollide::bounding_volume::AABB<f32>;
/// Event triggered when two non-sensor colliders start or stop being in contact.
@@ -51,6 +54,8 @@ pub(crate) use self::contact::WContact;
pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal};
pub(crate) use self::narrow_phase::ContactManifoldIndex;
#[cfg(feature = "dim3")]
pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap;
#[cfg(feature = "dim3")]
pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
pub(crate) use self::waabb::{WRay, WAABB};
pub(crate) use self::wquadtree::WQuadtree;
@@ -81,3 +86,5 @@ mod trimesh;
mod waabb;
mod wquadtree;
//mod z_order;
#[cfg(feature = "dim3")]
mod polygonal_feature_map;

65
src/geometry/polygonal_feature_map.rs Normal file
View File

@@ -0,0 +1,65 @@
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle};
use crate::math::{Point, Vector};
use approx::AbsDiffEq;
use na::{Unit, Vector2, Vector3};
use ncollide::shape::Segment;
use ncollide::shape::SupportMap;
/// Trait implemented by convex shapes with features with polyhedral approximations.
pub trait PolygonalFeatureMap: SupportMap<f32> {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace);
}
impl PolygonalFeatureMap for Segment<f32> {
fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
*out_feature = PolyhedronFace::from(*self);
}
}
impl PolygonalFeatureMap for Triangle {
fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
*out_feature = PolyhedronFace::from(*self);
}
}
impl PolygonalFeatureMap for Cuboid {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
let face = cuboid::support_face(self, **dir);
*out_feature = PolyhedronFace::from(face);
}
}
impl PolygonalFeatureMap for Cylinder {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
let dir2 = Vector2::new(dir.x, dir.z)
.try_normalize(f32::default_epsilon())
.unwrap_or(Vector2::x());
if dir.y.abs() < 0.5 {
// We return a segment lying on the cylinder's curved part.
out_features.vertices[0] = Point::new(
dir2.x * self.radius,
-self.half_height,
dir2.y * self.radius,
);
out_features.vertices[1] =
Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
out_features.eids = [0, 0, 0, 0]; // FIXME
out_features.fid = 1;
out_features.num_vertices = 2;
out_features.vids = [0, 1, 1, 1]; // FIXME
} else {
// We return a square approximation of the cylinder cap.
let y = self.half_height.copysign(dir.y);
out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
out_features.eids = [0, 1, 2, 3]; // FIXME
out_features.fid = if dir.y < 0.0 { 0 } else { 2 };
out_features.num_vertices = 4;
out_features.vids = [0, 1, 2, 3]; // FIXME
}
}
}

10
src/geometry/polyhedron_feature3d.rs
View File

@@ -50,6 +50,16 @@ impl From<Segment<f32>> for PolyhedronFace {
}
impl PolyhedronFace {
pub fn new() -> Self {
Self {
vertices: [Point::origin(); 4],
vids: [0; 4],
eids: [0; 4],
fid: 0,
num_vertices: 0,
}
}
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
for v in &mut self.vertices[0..self.num_vertices] {
*v = iso * *v;

2
src/utils.rs
View File

@@ -91,7 +91,7 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
impl WSign<SimdFloat> for SimdFloat {
fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
self.simd_copysign(to)
to.simd_copysign(self)
}
}