aboutsummaryrefslogtreecommitdiff
path: root/src/geometry/cuboid_feature3d.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/geometry/cuboid_feature3d.rs')
-rw-r--r--src/geometry/cuboid_feature3d.rs516
1 files changed, 516 insertions, 0 deletions
diff --git a/src/geometry/cuboid_feature3d.rs b/src/geometry/cuboid_feature3d.rs
new file mode 100644
index 0000000..3a4b675
--- /dev/null
+++ b/src/geometry/cuboid_feature3d.rs
@@ -0,0 +1,516 @@
+use crate::geometry::{Contact, ContactManifold};
+use crate::math::{Isometry, Point, Vector};
+use crate::utils::WBasis;
+use na::Point2;
+
+#[derive(Debug)]
+#[allow(dead_code)]
+pub(crate) enum CuboidFeature {
+ Face(CuboidFeatureFace),
+ Edge(CuboidFeatureEdge),
+ Vertex(CuboidFeatureVertex),
+}
+
+#[derive(Debug)]
+pub(crate) struct CuboidFeatureVertex {
+ pub vertex: Point<f32>,
+ pub vid: u8,
+}
+
+impl CuboidFeatureVertex {
+ pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+ self.vertex = iso * self.vertex;
+ }
+}
+
+#[derive(Debug)]
+pub(crate) struct CuboidFeatureEdge {
+ pub vertices: [Point<f32>; 2],
+ pub vids: [u8; 2],
+ pub eid: u8,
+}
+
+impl CuboidFeatureEdge {
+ pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+ self.vertices[0] = iso * self.vertices[0];
+ self.vertices[1] = iso * self.vertices[1];
+ }
+}
+
+#[derive(Debug)]
+pub(crate) struct CuboidFeatureFace {
+ 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.
+}
+
+impl CuboidFeatureFace {
+ pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+ self.vertices[0] = iso * self.vertices[0];
+ self.vertices[1] = iso * self.vertices[1];
+ self.vertices[2] = iso * self.vertices[2];
+ self.vertices[3] = iso * self.vertices[3];
+ }
+}
+
+impl CuboidFeature {
+ pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+ match self {
+ CuboidFeature::Face(face) => face.transform_by(iso),
+ CuboidFeature::Edge(edge) => edge.transform_by(iso),
+ CuboidFeature::Vertex(vertex) => vertex.transform_by(iso),
+ }
+ }
+
+ /// Compute contacts points between a face and a vertex.
+ ///
+ /// This method assume we already know that at least one contact exists.
+ pub fn face_vertex_contacts(
+ face1: &CuboidFeatureFace,
+ sep_axis1: &Vector<f32>,
+ vertex2: &CuboidFeatureVertex,
+ pos21: &Isometry<f32>,
+ manifold: &mut ContactManifold,
+ ) {
+ let normal1 =
+ (face1.vertices[0] - face1.vertices[1]).cross(&(face1.vertices[2] - face1.vertices[1]));
+ let denom = -normal1.dot(&sep_axis1);
+ let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom;
+ let local_p2 = vertex2.vertex;
+ let local_p1 = vertex2.vertex - dist * sep_axis1;
+
+ manifold.points.push(Contact {
+ local_p1,
+ local_p2: pos21 * local_p2,
+ impulse: 0.0,
+ tangent_impulse: Contact::zero_tangent_impulse(),
+ fid1: face1.fid,
+ fid2: vertex2.vid,
+ dist,
+ });
+ }
+
+ /// Compute contacts points between a face and an edge.
+ ///
+ /// This method assume we already know that at least one contact exists.
+ pub fn face_edge_contacts(
+ prediction_distance: f32,
+ face1: &CuboidFeatureFace,
+ sep_axis1: &Vector<f32>,
+ edge2: &CuboidFeatureEdge,
+ pos21: &Isometry<f32>,
+ manifold: &mut ContactManifold,
+ flipped: bool,
+ ) {
+ // 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_edge2 = [
+ Point2::new(
+ edge2.vertices[0].coords.dot(&basis[0]),
+ edge2.vertices[0].coords.dot(&basis[1]),
+ ),
+ Point2::new(
+ edge2.vertices[1].coords.dot(&basis[0]),
+ edge2.vertices[1].coords.dot(&basis[1]),
+ ),
+ ];
+
+ // Now we have to compute the intersection between all pairs of
+ // edges from the face 1 with the edge 2
+ for i in 0..4 {
+ let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]];
+
+ 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) % 4]];
+ let local_p1 = edge1[0] * (1.0 - bcoords.0) + edge1[1].coords * bcoords.0;
+ let local_p2 = edge2.vertices[0] * (1.0 - bcoords.1)
+ + edge2.vertices[1].coords * bcoords.1;
+ let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+ if dist < prediction_distance {
+ if flipped {
+ manifold.points.push(Contact {
+ local_p1: local_p2,
+ // All points are expressed in the locale space of the first shape
+ // (even if there was a flip). So the point we need to transform by
+ // pos21 is the one that will go into local_p2.
+ local_p2: pos21 * local_p1,
+ impulse: 0.0,
+ tangent_impulse: Contact::zero_tangent_impulse(),
+ fid1: edge2.eid,
+ fid2: face1.eids[i],
+ dist,
+ });
+ } else {
+ 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: edge2.eid,
+ dist,
+ });
+ }
+ }
+ }
+ }
+ }
+
+ // Project the two points from the edge into the face.
+ let normal1 =
+ (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1]));
+
+ 'point_loop2: for i in 0..2 {
+ let p2 = projected_edge2[i];
+
+ let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3]));
+ for j in 0..3 {
+ 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] - edge2.vertices[i]).dot(&normal1) / denom;
+ let local_p2 = edge2.vertices[i];
+ let local_p1 = edge2.vertices[i] - dist * sep_axis1;
+
+ if dist < prediction_distance {
+ if flipped {
+ manifold.points.push(Contact {
+ local_p1: local_p2,
+ // All points are expressed in the locale space of the first shape
+ // (even if there was a flip). So the point we need to transform by
+ // pos21 is the one that will go into local_p2.
+ local_p2: pos21 * local_p1,
+ impulse: 0.0,
+ tangent_impulse: Contact::zero_tangent_impulse(),
+ fid1: edge2.vids[i],
+ fid2: face1.fid,
+ dist,
+ });
+ } else {
+ manifold.points.push(Contact {
+ local_p1,
+ local_p2: pos21 * local_p2,
+ impulse: 0.0,
+ tangent_impulse: Contact::zero_tangent_impulse(),
+ fid1: face1.fid,
+ fid2: edge2.vids[i],
+ dist,
+ });
+ }
+ }
+ }
+ }
+
+ /// Compute contacts points between two edges.
+ ///
+ /// This method assume we already know that at least one contact exists.
+ pub fn edge_edge_contacts(
+ edge1: &CuboidFeatureEdge,
+ sep_axis1: &Vector<f32>,
+ edge2: &CuboidFeatureEdge,
+ pos21: &Isometry<f32>,
+ manifold: &mut ContactManifold,
+ ) {
+ let basis = sep_axis1.orthonormal_basis();
+ let projected_edge1 = [
+ Point2::new(
+ edge1.vertices[0].coords.dot(&basis[0]),
+ edge1.vertices[0].coords.dot(&basis[1]),
+ ),
+ Point2::new(
+ edge1.vertices[1].coords.dot(&basis[0]),
+ edge1.vertices[1].coords.dot(&basis[1]),
+ ),
+ ];
+ let projected_edge2 = [
+ Point2::new(
+ edge2.vertices[0].coords.dot(&basis[0]),
+ edge2.vertices[0].coords.dot(&basis[1]),
+ ),
+ Point2::new(
+ edge2.vertices[1].coords.dot(&basis[0]),
+ edge2.vertices[1].coords.dot(&basis[1]),
+ ),
+ ];
+
+ if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
+ let local_p1 =
+ edge1.vertices[0] * (1.0 - bcoords.0) + edge1.vertices[1].coords * bcoords.0;
+ let local_p2 =
+ edge2.vertices[0] * (1.0 - bcoords.1) + edge2.vertices[1].coords * bcoords.1;
+ let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+ manifold.points.push(Contact {
+ local_p1,
+ local_p2: pos21 * local_p2, // NOTE: local_p2 is expressed in the local space of cube1.
+ impulse: 0.0,
+ tangent_impulse: Contact::zero_tangent_impulse(),
+ fid1: edge1.eid,
+ fid2: edge2.eid,
+ dist,
+ });
+ }
+ }
+
+ pub fn face_face_contacts(
+ _prediction_distance: f32,
+ face1: &CuboidFeatureFace,
+ sep_axis1: &Vector<f32>,
+ face2: &CuboidFeatureFace,
+ 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.
+ let normal1 =
+ (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1]));
+ let normal2 =
+ (face2.vertices[2] - face2.vertices[1]).cross(&(face2.vertices[0] - face2.vertices[1]));
+
+ // NOTE: The loop iterating on all the vertices for face1 is different from
+ // the one iterating on all the vertices of face2. In the second loop, we
+ // classify every point wrt. every edge on the other face. This will give
+ // us bit masks to filter out several edge-edge intersections.
+ 'point_loop1: for i in 0..4 {
+ let p1 = projected_face1[i];
+
+ let sign = (projected_face2[0] - projected_face2[3]).perp(&(p1 - projected_face2[3]));
+ for j in 0..3 {
+ 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 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.vids[i],
+ fid2: face2.fid,
+ dist,
+ });
+ }
+ }
+
+ let is_clockwise1 = (projected_face1[0] - projected_face1[1])
+ .perp(&(projected_face1[2] - projected_face1[1]))
+ >= 0.0;
+ let mut vertex_class2 = [0u8; 4];
+
+ for i in 0..4 {
+ let p2 = projected_face2[i];
+
+ let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3]));
+ vertex_class2[i] |= ((sign >= 0.0) as u8) << 3;
+
+ for j in 0..3 {
+ let sign =
+ (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j]));
+ vertex_class2[i] |= ((sign >= 0.0) as u8) << j;
+ }
+
+ if !is_clockwise1 {
+ vertex_class2[i] = (!vertex_class2[i]) & 0b01111;
+ }
+
+ if vertex_class2[i] == 0 {
+ // 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..4 {
+ let projected_edge2 = [projected_face2[j], projected_face2[(j + 1) % 4]];
+
+ if (vertex_class2[j] & vertex_class2[(j + 1) % 4]) != 0 {
+ continue;
+ }
+
+ let edge_class2 = vertex_class2[j] | vertex_class2[(j + 1) % 4];
+
+ for i in 0..4 {
+ if (edge_class2 & (1 << i)) != 0 {
+ let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]];
+ 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) % 4]);
+ let edge2 = (face2.vertices[j], face2.vertices[(j + 1) % 4]);
+ 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 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.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))
+ }
+ }
+ }
+}
generated by cgit (git 2.34.1) at 2026-01-16 16:03:44 +0000