First public release of Rapier.
This commit is contained in:
Sébastien Crozet
284
src/geometry/polyhedron_feature3d.rs
Normal file
284
src/geometry/polyhedron_feature3d.rs
Normal file
@@ -0,0 +1,284 @@
|
||||
use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle};
|
||||
use crate::math::{Isometry, Point, Vector};
|
||||
use crate::utils::WBasis;
|
||||
use na::Point2;
|
||||
use ncollide::shape::Segment;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct PolyhedronFace {
|
||||
pub vertices: [Point<f32>; 4],
|
||||
pub vids: [u8; 4], // Feature ID of the vertices.
|
||||
pub eids: [u8; 4], // Feature ID of the edges.
|
||||
pub fid: u8, // Feature ID of the face.
|
||||
pub num_vertices: usize,
|
||||
}
|
||||
|
||||
impl From<CuboidFeatureFace> for PolyhedronFace {
|
||||
fn from(face: CuboidFeatureFace) -> Self {
|
||||
Self {
|
||||
vertices: face.vertices,
|
||||
vids: face.vids,
|
||||
eids: face.eids,
|
||||
fid: face.fid,
|
||||
num_vertices: 4,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<Triangle> for PolyhedronFace {
|
||||
fn from(tri: Triangle) -> Self {
|
||||
Self {
|
||||
vertices: [tri.a, tri.b, tri.c, tri.c],
|
||||
vids: [0, 2, 4, 4],
|
||||
eids: [1, 3, 5, 5],
|
||||
fid: 0,
|
||||
num_vertices: 3,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<Segment<f32>> for PolyhedronFace {
|
||||
fn from(seg: Segment<f32>) -> Self {
|
||||
Self {
|
||||
vertices: [seg.a, seg.b, seg.b, seg.b],
|
||||
vids: [0, 2, 2, 2],
|
||||
eids: [1, 1, 1, 1],
|
||||
fid: 0,
|
||||
num_vertices: 2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl PolyhedronFace {
|
||||
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
|
||||
for v in &mut self.vertices[0..self.num_vertices] {
|
||||
*v = iso * *v;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn contacts(
|
||||
prediction_distance: f32,
|
||||
face1: &PolyhedronFace,
|
||||
sep_axis1: &Vector<f32>,
|
||||
face2: &PolyhedronFace,
|
||||
pos21: &Isometry<f32>,
|
||||
manifold: &mut ContactManifold,
|
||||
) {
|
||||
// Project the faces to a 2D plane for contact clipping.
|
||||
// The plane they are projected onto has normal sep_axis1
|
||||
// and contains the origin (this is numerically OK because
|
||||
// we are not working in world-space here).
|
||||
let basis = sep_axis1.orthonormal_basis();
|
||||
let projected_face1 = [
|
||||
Point2::new(
|
||||
face1.vertices[0].coords.dot(&basis[0]),
|
||||
face1.vertices[0].coords.dot(&basis[1]),
|
||||
),
|
||||
Point2::new(
|
||||
face1.vertices[1].coords.dot(&basis[0]),
|
||||
face1.vertices[1].coords.dot(&basis[1]),
|
||||
),
|
||||
Point2::new(
|
||||
face1.vertices[2].coords.dot(&basis[0]),
|
||||
face1.vertices[2].coords.dot(&basis[1]),
|
||||
),
|
||||
Point2::new(
|
||||
face1.vertices[3].coords.dot(&basis[0]),
|
||||
face1.vertices[3].coords.dot(&basis[1]),
|
||||
),
|
||||
];
|
||||
let projected_face2 = [
|
||||
Point2::new(
|
||||
face2.vertices[0].coords.dot(&basis[0]),
|
||||
face2.vertices[0].coords.dot(&basis[1]),
|
||||
),
|
||||
Point2::new(
|
||||
face2.vertices[1].coords.dot(&basis[0]),
|
||||
face2.vertices[1].coords.dot(&basis[1]),
|
||||
),
|
||||
Point2::new(
|
||||
face2.vertices[2].coords.dot(&basis[0]),
|
||||
face2.vertices[2].coords.dot(&basis[1]),
|
||||
),
|
||||
Point2::new(
|
||||
face2.vertices[3].coords.dot(&basis[0]),
|
||||
face2.vertices[3].coords.dot(&basis[1]),
|
||||
),
|
||||
];
|
||||
|
||||
// Also find all the vertices located inside of the other projected face.
|
||||
if face2.num_vertices > 2 {
|
||||
let normal2 = (face2.vertices[2] - face2.vertices[1])
|
||||
.cross(&(face2.vertices[0] - face2.vertices[1]));
|
||||
|
||||
let last_index2 = face2.num_vertices as usize - 1;
|
||||
'point_loop1: for i in 0..face1.num_vertices as usize {
|
||||
let p1 = projected_face1[i];
|
||||
|
||||
let sign = (projected_face2[0] - projected_face2[last_index2])
|
||||
.perp(&(p1 - projected_face2[last_index2]));
|
||||
for j in 0..last_index2 {
|
||||
let new_sign = (projected_face2[j + 1] - projected_face2[j])
|
||||
.perp(&(p1 - projected_face2[j]));
|
||||
if new_sign * sign < 0.0 {
|
||||
// The point lies outside.
|
||||
continue 'point_loop1;
|
||||
}
|
||||
}
|
||||
|
||||
// All the perp had the same sign: the point is inside of the other shapes projection.
|
||||
// Output the contact.
|
||||
let denom = normal2.dot(&sep_axis1);
|
||||
let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
|
||||
let local_p1 = face1.vertices[i];
|
||||
let local_p2 = face1.vertices[i] + dist * sep_axis1;
|
||||
|
||||
if dist <= prediction_distance {
|
||||
manifold.points.push(Contact {
|
||||
local_p1,
|
||||
local_p2: pos21 * local_p2,
|
||||
impulse: 0.0,
|
||||
tangent_impulse: Contact::zero_tangent_impulse(),
|
||||
fid1: face1.vids[i],
|
||||
fid2: face2.fid,
|
||||
dist,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if face1.num_vertices > 2 {
|
||||
let normal1 = (face1.vertices[2] - face1.vertices[1])
|
||||
.cross(&(face1.vertices[0] - face1.vertices[1]));
|
||||
|
||||
let last_index1 = face1.num_vertices as usize - 1;
|
||||
'point_loop2: for i in 0..face2.num_vertices as usize {
|
||||
let p2 = projected_face2[i];
|
||||
|
||||
let sign = (projected_face1[0] - projected_face1[last_index1])
|
||||
.perp(&(p2 - projected_face1[last_index1]));
|
||||
for j in 0..last_index1 {
|
||||
let new_sign = (projected_face1[j + 1] - projected_face1[j])
|
||||
.perp(&(p2 - projected_face1[j]));
|
||||
|
||||
if new_sign * sign < 0.0 {
|
||||
// The point lies outside.
|
||||
continue 'point_loop2;
|
||||
}
|
||||
}
|
||||
|
||||
// All the perp had the same sign: the point is inside of the other shapes projection.
|
||||
// Output the contact.
|
||||
let denom = -normal1.dot(&sep_axis1);
|
||||
let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
|
||||
let local_p2 = face2.vertices[i];
|
||||
let local_p1 = face2.vertices[i] - dist * sep_axis1;
|
||||
|
||||
if true {
|
||||
// dist <= prediction_distance {
|
||||
manifold.points.push(Contact {
|
||||
local_p1,
|
||||
local_p2: pos21 * local_p2,
|
||||
impulse: 0.0,
|
||||
tangent_impulse: Contact::zero_tangent_impulse(),
|
||||
fid1: face1.fid,
|
||||
fid2: face2.vids[i],
|
||||
dist,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Now we have to compute the intersection between all pairs of
|
||||
// edges from the face 1 and from the face2.
|
||||
for j in 0..face2.num_vertices {
|
||||
let projected_edge2 = [
|
||||
projected_face2[j],
|
||||
projected_face2[(j + 1) % face2.num_vertices],
|
||||
];
|
||||
|
||||
for i in 0..face1.num_vertices {
|
||||
let projected_edge1 = [
|
||||
projected_face1[i],
|
||||
projected_face1[(i + 1) % face1.num_vertices],
|
||||
];
|
||||
if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
|
||||
if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
|
||||
// Found a contact between the two edges.
|
||||
let edge1 = (
|
||||
face1.vertices[i],
|
||||
face1.vertices[(i + 1) % face1.num_vertices],
|
||||
);
|
||||
let edge2 = (
|
||||
face2.vertices[j],
|
||||
face2.vertices[(j + 1) % face2.num_vertices],
|
||||
);
|
||||
let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
|
||||
let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
|
||||
let dist = (local_p2 - local_p1).dot(&sep_axis1);
|
||||
|
||||
if dist <= prediction_distance {
|
||||
manifold.points.push(Contact {
|
||||
local_p1,
|
||||
local_p2: pos21 * local_p2,
|
||||
impulse: 0.0,
|
||||
tangent_impulse: Contact::zero_tangent_impulse(),
|
||||
fid1: face1.eids[i],
|
||||
fid2: face2.eids[j],
|
||||
dist,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute the barycentric coordinates of the intersection between the two given lines.
|
||||
/// Returns `None` if the lines are parallel.
|
||||
fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
|
||||
use approx::AbsDiffEq;
|
||||
|
||||
// Inspired by Real-time collision detection by Christer Ericson.
|
||||
let dir1 = edge1[1] - edge1[0];
|
||||
let dir2 = edge2[1] - edge2[0];
|
||||
let r = edge1[0] - edge2[0];
|
||||
|
||||
let a = dir1.norm_squared();
|
||||
let e = dir2.norm_squared();
|
||||
let f = dir2.dot(&r);
|
||||
|
||||
let eps = f32::default_epsilon();
|
||||
|
||||
if a <= eps && e <= eps {
|
||||
Some((0.0, 0.0))
|
||||
} else if a <= eps {
|
||||
Some((0.0, f / e))
|
||||
} else {
|
||||
let c = dir1.dot(&r);
|
||||
if e <= eps {
|
||||
Some((-c / a, 0.0))
|
||||
} else {
|
||||
let b = dir1.dot(&dir2);
|
||||
let ae = a * e;
|
||||
let bb = b * b;
|
||||
let denom = ae - bb;
|
||||
|
||||
// Use absolute and ulps error to test collinearity.
|
||||
let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
|
||||
|
||||
let s = if !parallel {
|
||||
(b * f - c * e) / denom
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
if parallel {
|
||||
None
|
||||
} else {
|
||||
Some((s, (b * s + f) / e))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user