From faec3d5d46c88e2949179dd2789899e5cf26ed48 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 12 Oct 2020 18:33:58 +0200 Subject: Start adding cylinders. --- src/geometry/polygonal_feature_map.rs | 65 +++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 src/geometry/polygonal_feature_map.rs (limited to 'src/geometry/polygonal_feature_map.rs') diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs new file mode 100644 index 0000000..8b047fc --- /dev/null +++ b/src/geometry/polygonal_feature_map.rs @@ -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 { + fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace); +} + +impl PolygonalFeatureMap for Segment { + fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { + *out_feature = PolyhedronFace::from(*self); + } +} + +impl PolygonalFeatureMap for Triangle { + fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { + *out_feature = PolyhedronFace::from(*self); + } +} + +impl PolygonalFeatureMap for Cuboid { + fn local_support_feature(&self, dir: &Unit>, 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>, 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 + } + } +} -- cgit From 947c4813c9666fd8215743de298fe17780fa3ef2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 19 Oct 2020 16:51:40 +0200 Subject: Complete the pfm/pfm contact generator. --- src/geometry/polygonal_feature_map.rs | 36 ++++++++++++++++++++++++++++------- 1 file changed, 29 insertions(+), 7 deletions(-) (limited to 'src/geometry/polygonal_feature_map.rs') diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 8b047fc..70be5d0 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -32,6 +32,20 @@ impl PolygonalFeatureMap for Cuboid { impl PolygonalFeatureMap for Cylinder { fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + // About feature ids. + // At all times, we consider our cylinder to be approximated as follows: + // - The curved part is approximated by a single segment. + // - Each flat cap of the cylinder is approximated by a square. + // - The curved-part segment has a feature ID of 0, and its endpoint with negative + // `y` coordinate has an ID of 1. + // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order + // when looking at the cap with an eye looking towards +y). + // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order. + // - The bottom cap has its face feature ID of 9. + // - The feature IDs of the top cap are the same as the bottom cap to which we add 10. + // So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19. + // - Note that at all times, one of each cap's vertices are the same as the curved-part + // segment endpoints. let dir2 = Vector2::new(dir.x, dir.z) .try_normalize(f32::default_epsilon()) .unwrap_or(Vector2::x()); @@ -45,10 +59,10 @@ impl PolygonalFeatureMap for Cylinder { ); 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.eids = [0, 0, 0, 0]; + out_features.fid = 0; out_features.num_vertices = 2; - out_features.vids = [0, 1, 1, 1]; // FIXME + out_features.vids = [1, 11, 11, 11]; } else { // We return a square approximation of the cylinder cap. let y = self.half_height.copysign(dir.y); @@ -56,10 +70,18 @@ impl PolygonalFeatureMap for Cylinder { 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 + + if dir.y < 0.0 { + out_features.eids = [2, 4, 6, 8]; + out_features.fid = 9; + out_features.num_vertices = 4; + out_features.vids = [1, 3, 5, 7]; + } else { + out_features.eids = [12, 14, 16, 18]; + out_features.fid = 19; + out_features.num_vertices = 4; + out_features.vids = [11, 13, 15, 17]; + } } } } -- cgit From d513c22d33ab44b0048355bcfd1db4173b3f7ece Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 14:16:01 +0200 Subject: Add cone support. --- src/geometry/polygonal_feature_map.rs | 48 ++++++++++++++++++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) (limited to 'src/geometry/polygonal_feature_map.rs') diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 70be5d0..fc5e066 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -1,5 +1,5 @@ use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle}; +use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle}; use crate::math::{Point, Vector}; use approx::AbsDiffEq; use na::{Unit, Vector2, Vector3}; @@ -85,3 +85,49 @@ impl PolygonalFeatureMap for Cylinder { } } } + +impl PolygonalFeatureMap for Cone { + fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + // About feature ids. It is very similar to the feature ids of cylinders. + // At all times, we consider our cone to be approximated as follows: + // - The curved part is approximated by a single segment. + // - The flat cap of the cone is approximated by a square. + // - The curved-part segment has a feature ID of 0, and its endpoint with negative + // `y` coordinate has an ID of 1. + // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order + // when looking at the cap with an eye looking towards +y). + // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order. + // - The bottom cap has its face feature ID of 9. + // - Note that at all times, one of the cap's vertices are the same as the curved-part + // segment endpoints. + let dir2 = Vector2::new(dir.x, dir.z) + .try_normalize(f32::default_epsilon()) + .unwrap_or(Vector2::x()); + + if dir.y > 0.0 { + // We return a segment lying on the cone'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(0.0, self.half_height, 0.0); + out_features.eids = [0, 0, 0, 0]; + out_features.fid = 0; + out_features.num_vertices = 2; + out_features.vids = [1, 11, 11, 11]; + } else { + // We return a square approximation of the cone cap. + let y = -self.half_height; + 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 = [2, 4, 6, 8]; + out_features.fid = 9; + out_features.num_vertices = 4; + out_features.vids = [1, 3, 5, 7]; + } + } +} -- cgit From 949e3f5384a366c3bff5415c5db4635e811a580e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 16:22:53 +0200 Subject: Fix many warnings. --- src/geometry/polygonal_feature_map.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/geometry/polygonal_feature_map.rs') diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index fc5e066..2586826 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -2,7 +2,7 @@ use crate::geometry::PolyhedronFace; use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle}; use crate::math::{Point, Vector}; use approx::AbsDiffEq; -use na::{Unit, Vector2, Vector3}; +use na::{Unit, Vector2}; use ncollide::shape::Segment; use ncollide::shape::SupportMap; -- cgit From 2b628f9580a826722346983ef42672d4e8dd8053 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Oct 2020 15:58:30 +0100 Subject: Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it. --- src/geometry/polygonal_feature_map.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/geometry/polygonal_feature_map.rs') diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 2586826..2a8fc8d 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -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 { fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace); } -impl PolygonalFeatureMap for Segment { +impl PolygonalFeatureMap for Segment { fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { *out_feature = PolyhedronFace::from(*self); } -- cgit