From 865ce8a8e5301b23ca474adaaffe8b43e725803e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 11:55:33 +0200 Subject: Collider shape: use a trait-object instead of an enum. --- src/geometry/sat.rs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/geometry/sat.rs') diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 0666c04..e2548dd 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -58,8 +58,8 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( let y2 = pos12 * Vector::y(); let z2 = pos12 * Vector::z(); - // We have 3 * 3 = 9 axii to test. - let axii = [ + // We have 3 * 3 = 9 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -74,7 +74,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - for axis1 in &axii { + for axis1 in &axes { let norm1 = axis1.norm(); if norm1 > f32::default_epsilon() { let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( @@ -149,7 +149,7 @@ pub fn cube_support_map_compute_separation_wrt_local_line>( pub fn cube_support_map_find_local_separating_edge_twoway( cube1: &Cuboid, shape2: &impl SupportMap, - axii: &[Vector], + axes: &[Vector], pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { @@ -157,7 +157,7 @@ pub fn cube_support_map_find_local_separating_edge_twoway( let mut best_separation = -std::f32::MAX; let mut best_dir = Vector::zeros(); - for axis1 in axii { + for axis1 in axes { if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) { let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line( cube1, shape2, pos12, pos21, &axis1, @@ -184,8 +184,8 @@ pub fn cube_triangle_find_local_separating_edge_twoway( let y2 = pos12 * (triangle2.c - triangle2.b); let z2 = pos12 * (triangle2.a - triangle2.c); - // We have 3 * 3 = 3 axii to test. - let axii = [ + // We have 3 * 3 = 3 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -200,7 +200,7 @@ pub fn cube_triangle_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21) } #[cfg(feature = "dim3")] @@ -212,14 +212,14 @@ pub fn cube_segment_find_local_separating_edge_twoway( ) -> (f32, Vector) { let x2 = pos12 * (segment2.b - segment2.a); - let axii = [ + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), Vector::new(-x2.y, x2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21) } pub fn cube_support_map_find_local_separating_normal_oneway>( -- cgit From 08930b1238c90bec16db84c50ac4ea7c9a1e0a5b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Oct 2020 16:36:07 +0100 Subject: Fix multiple warnings. --- src/geometry/sat.rs | 1 + 1 file changed, 1 insertion(+) (limited to 'src/geometry/sat.rs') diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index e2548dd..452b380 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -4,6 +4,7 @@ use crate::utils::WSign; use na::Unit; use ncollide::shape::{Segment, SupportMap}; +#[allow(dead_code)] pub fn polygon_polygon_compute_separation_features( p1: &Polygon, p2: &Polygon, -- cgit From dbdd797d5934ee76b0358b6cf845575ce0ef29af Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 27 Oct 2020 09:08:23 +0100 Subject: Add some segment/triangle SAT functions. --- src/geometry/sat.rs | 82 ++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 78 insertions(+), 4 deletions(-) (limited to 'src/geometry/sat.rs') diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 452b380..44446af 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -1,8 +1,20 @@ -use crate::geometry::{cuboid, Cuboid, Polygon, Triangle}; +use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle}; use crate::math::{Isometry, Point, Vector, DIM}; use crate::utils::WSign; use na::Unit; -use ncollide::shape::{Segment, SupportMap}; +use ncollide::shape::SupportMap; + +#[allow(dead_code)] +pub fn support_map_support_map_compute_separation( + sm1: &impl SupportMap, + sm2: &impl SupportMap, + m12: &Isometry, + dir1: &Unit>, +) -> f32 { + let p1 = sm1.local_support_point_toward(dir1); + let p2 = sm2.support_point_toward(m12, &-*dir1); + (p2 - p1).dot(dir1) +} #[allow(dead_code)] pub fn polygon_polygon_compute_separation_features( @@ -128,7 +140,6 @@ pub fn cuboid_cuboid_find_local_separating_normal_oneway( * * */ - #[cfg(feature = "dim3")] pub fn cube_support_map_compute_separation_wrt_local_line>( cube1: &Cuboid, @@ -207,7 +218,7 @@ pub fn cube_triangle_find_local_separating_edge_twoway( #[cfg(feature = "dim3")] pub fn cube_segment_find_local_separating_edge_twoway( cube1: &Cuboid, - segment2: &Segment, + segment2: &Segment, pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { @@ -293,3 +304,66 @@ pub fn segment_cuboid_find_local_separating_normal_oneway( ) -> (f32, Vector) { point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12) } + +/* + * Capsules + */ +#[cfg(feature = "dim3")] +pub fn triangle_segment_find_local_separating_normal_oneway( + triangle1: &Triangle, + segment2: &Segment, + m12: &Isometry, +) -> (f32, Vector) { + if let Some(dir) = triangle1.normal() { + let p2a = segment2.support_point_toward(m12, &-dir); + let p2b = segment2.support_point_toward(m12, &dir); + let sep_a = (p2a - triangle1.a).dot(&dir); + let sep_b = -(p2b - triangle1.a).dot(&dir); + + if sep_a >= sep_b { + (sep_a, *dir) + } else { + (sep_b, -*dir) + } + } else { + (-f32::MAX, Vector::zeros()) + } +} + +#[cfg(feature = "dim3")] +pub fn segment_triangle_find_local_separating_edge( + segment1: &Segment, + triangle2: &Triangle, + pos12: &Isometry, +) -> (f32, Vector) { + let x2 = pos12 * (triangle2.b - triangle2.a); + let y2 = pos12 * (triangle2.c - triangle2.b); + let z2 = pos12 * (triangle2.a - triangle2.c); + let dir1 = segment1.scaled_direction(); + + let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)]; + let axes1 = [ + crosses1[0], + crosses1[1], + crosses1[2], + -crosses1[0], + -crosses1[1], + -crosses1[2], + ]; + let mut max_separation = -f32::MAX; + let mut sep_dir = axes1[0]; + + for axis1 in &axes1 { + if let Some(axis1) = Unit::try_new(*axis1, 0.0) { + let sep = + support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1); + + if sep > max_separation { + max_separation = sep; + sep_dir = *axis1; + } + } + } + + (max_separation, sep_dir) +} -- cgit From ffbc3c02c7d328d5c48a3efb84d35f5911f1880b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 27 Oct 2020 09:25:58 +0100 Subject: Fix 2D compilation. --- src/geometry/sat.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/geometry/sat.rs') diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 44446af..3cd66b4 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -298,7 +298,7 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway( #[cfg(feature = "dim2")] pub fn segment_cuboid_find_local_separating_normal_oneway( - segment1: &Segment, + segment1: &Segment, shape2: &Cuboid, pos12: &Isometry, ) -> (f32, Vector) { -- cgit