use crate::approx::AbsDiffEq;
use crate::math::{Isometry, Point, Real, Vector};
#[cfg(feature = "std")]
use crate::query::{ContactManifold, TrackedContact};
use crate::shape::{PackedFeatureId, Segment, Triangle};
use crate::utils::WBasis;
use na::Point2;
#[derive(Debug, Clone)]
pub struct PolygonalFeature {
pub vertices: [Point<Real>; 4],
pub vids: [PackedFeatureId; 4],
pub eids: [PackedFeatureId; 4],
pub fid: PackedFeatureId,
pub num_vertices: usize,
}
impl Default for PolygonalFeature {
fn default() -> Self {
Self {
vertices: [Point::origin(); 4],
vids: [PackedFeatureId::UNKNOWN; 4],
eids: [PackedFeatureId::UNKNOWN; 4],
fid: PackedFeatureId::UNKNOWN,
num_vertices: 0,
}
}
}
impl From<Triangle> for PolygonalFeature {
fn from(tri: Triangle) -> Self {
Self {
vertices: [tri.a, tri.b, tri.c, tri.c],
vids: PackedFeatureId::vertices([0, 1, 2, 2]),
eids: PackedFeatureId::edges([0, 1, 2, 2]),
fid: PackedFeatureId::face(0),
num_vertices: 3,
}
}
}
impl From<Segment> for PolygonalFeature {
fn from(seg: Segment) -> Self {
Self {
vertices: [seg.a, seg.b, seg.b, seg.b],
vids: PackedFeatureId::vertices([0, 1, 1, 1]),
eids: PackedFeatureId::edges([0, 0, 0, 0]),
fid: PackedFeatureId::face(0),
num_vertices: 2,
}
}
}
impl PolygonalFeature {
pub fn new() -> Self {
Self::default()
}
pub fn transform_by(&mut self, pos: &Isometry<Real>) {
for p in &mut self.vertices[0..self.num_vertices] {
*p = pos * *p;
}
}
#[cfg(feature = "std")]
pub fn contacts<ManifoldData, ContactData: Default + Copy>(
pos12: &Isometry<Real>,
_pos21: &Isometry<Real>,
sep_axis1: &Vector<Real>,
_sep_axis2: &Vector<Real>,
feature1: &Self,
feature2: &Self,
manifold: &mut ContactManifold<ManifoldData, ContactData>,
flipped: bool,
) {
match (feature1.num_vertices, feature2.num_vertices) {
(2, 2) => {
Self::contacts_edge_edge(pos12, feature1, sep_axis1, feature2, manifold, flipped)
}
_ => Self::contacts_face_face(pos12, feature1, sep_axis1, feature2, manifold, flipped),
}
}
#[cfg(feature = "std")]
fn contacts_edge_edge<ManifoldData, ContactData: Default + Copy>(
pos12: &Isometry<Real>,
face1: &PolygonalFeature,
sep_axis1: &Vector<Real>,
face2: &PolygonalFeature,
manifold: &mut ContactManifold<ManifoldData, ContactData>,
flipped: bool,
) {
let basis = sep_axis1.orthonormal_basis();
let projected_edge1 = [
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]),
),
];
let vertices2_1 = [pos12 * face2.vertices[0], pos12 * face2.vertices[1]];
let projected_edge2 = [
Point2::new(
vertices2_1[0].coords.dot(&basis[0]),
vertices2_1[0].coords.dot(&basis[1]),
),
Point2::new(
vertices2_1[1].coords.dot(&basis[0]),
vertices2_1[1].coords.dot(&basis[1]),
),
];
let tangent1 =
(projected_edge1[1] - projected_edge1[0]).try_normalize(Real::default_epsilon());
let tangent2 =
(projected_edge2[1] - projected_edge2[0]).try_normalize(Real::default_epsilon());
if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) {
let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8;
if !parallel {
let seg1 = (&projected_edge1[0], &projected_edge1[1]);
let seg2 = (&projected_edge2[0], &projected_edge2[1]);
let (loc1, loc2) =
crate::query::details::closest_points_segment_segment_with_locations_nD(
seg1, seg2,
);
let bcoords1 = loc1.barycentric_coordinates();
let bcoords2 = loc2.barycentric_coordinates();
let edge1 = (face1.vertices[0], face1.vertices[1]);
let edge2 = (vertices2_1[0], vertices2_1[1]);
let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1];
let local_p2_1 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1];
let dist = (local_p2_1 - local_p1).dot(sep_axis1);
manifold.points.push(TrackedContact::flipped(
local_p1,
pos12.inverse_transform_point(&local_p2_1),
face1.eids[0],
face2.eids[0],
dist,
flipped,
));
return;
}
}
if let Some(clips) = crate::query::details::clip_segment_segment(
(face1.vertices[0], face1.vertices[1]),
(vertices2_1[0], vertices2_1[1]),
) {
let feature_at = |face: &PolygonalFeature, id| match id {
0 => face.vids[0],
1 => face.eids[0],
2 => face.vids[1],
_ => unreachable!(),
};
manifold.points.push(TrackedContact::flipped(
(clips.0).0,
pos12.inverse_transform_point(&(clips.0).1),
feature_at(face1, (clips.0).2),
feature_at(face2, (clips.0).3),
((clips.0).1 - (clips.0).0).dot(sep_axis1),
flipped,
));
manifold.points.push(TrackedContact::flipped(
(clips.1).0,
pos12.inverse_transform_point(&(clips.1).1),
feature_at(face1, (clips.1).2),
feature_at(face2, (clips.1).3),
((clips.1).1 - (clips.1).0).dot(sep_axis1),
flipped,
));
}
}
#[cfg(feature = "std")]
fn contacts_face_face<ManifoldData, ContactData: Default + Copy>(
pos12: &Isometry<Real>,
face1: &PolygonalFeature,
sep_axis1: &Vector<Real>,
face2: &PolygonalFeature,
manifold: &mut ContactManifold<ManifoldData, ContactData>,
flipped: bool,
) {
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 vertices2_1 = [
pos12 * face2.vertices[0],
pos12 * face2.vertices[1],
pos12 * face2.vertices[2],
pos12 * face2.vertices[3],
];
let projected_face2 = [
Point2::new(
vertices2_1[0].coords.dot(&basis[0]),
vertices2_1[0].coords.dot(&basis[1]),
),
Point2::new(
vertices2_1[1].coords.dot(&basis[0]),
vertices2_1[1].coords.dot(&basis[1]),
),
Point2::new(
vertices2_1[2].coords.dot(&basis[0]),
vertices2_1[2].coords.dot(&basis[1]),
),
Point2::new(
vertices2_1[3].coords.dot(&basis[0]),
vertices2_1[3].coords.dot(&basis[1]),
),
];
if face2.num_vertices > 2 {
let normal2_1 =
(vertices2_1[2] - vertices2_1[1]).cross(&(vertices2_1[0] - vertices2_1[1]));
let denom = normal2_1.dot(sep_axis1);
if !relative_eq!(denom, 0.0) {
let last_index2 = face2.num_vertices - 1;
#[allow(clippy::needless_range_loop)] 'point_loop1: for i in 0..face1.num_vertices {
let p1 = projected_face1[i];
let mut 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 sign == 0.0 {
sign = new_sign;
} else if sign * new_sign < 0.0 {
continue 'point_loop1;
}
}
let dist = (vertices2_1[0] - face1.vertices[i]).dot(&normal2_1) / denom;
let local_p1 = face1.vertices[i];
let local_p2_1 = face1.vertices[i] + dist * sep_axis1;
manifold.points.push(TrackedContact::flipped(
local_p1,
pos12.inverse_transform_point(&local_p2_1),
face1.vids[i],
face2.fid,
dist,
flipped,
));
}
}
}
if face1.num_vertices > 2 {
let normal1 = (face1.vertices[2] - face1.vertices[1])
.cross(&(face1.vertices[0] - face1.vertices[1]));
let denom = -normal1.dot(sep_axis1);
if !relative_eq!(denom, 0.0) {
let last_index1 = face1.num_vertices - 1;
'point_loop2: for i in 0..face2.num_vertices {
let p2 = projected_face2[i];
let mut 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 sign == 0.0 {
sign = new_sign;
} else if sign * new_sign < 0.0 {
continue 'point_loop2;
}
}
let dist = (face1.vertices[0] - vertices2_1[i]).dot(&normal1) / denom;
let local_p2_1 = vertices2_1[i];
let local_p1 = vertices2_1[i] - dist * sep_axis1;
manifold.points.push(TrackedContact::flipped(
local_p1,
pos12.inverse_transform_point(&local_p2_1),
face1.fid,
face2.vids[i],
dist,
flipped,
));
}
}
}
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 {
let edge1 = (
face1.vertices[i],
face1.vertices[(i + 1) % face1.num_vertices],
);
let edge2 = (vertices2_1[j], vertices2_1[(j + 1) % face2.num_vertices]);
let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
let local_p2_1 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
let dist = (local_p2_1 - local_p1).dot(sep_axis1);
manifold.points.push(TrackedContact::flipped(
local_p1,
pos12.inverse_transform_point(&local_p2_1),
face1.eids[i],
face2.eids[j],
dist,
flipped,
));
}
}
}
}
}
}
fn closest_points_line2d(
edge1: [Point2<Real>; 2],
edge2: [Point2<Real>; 2],
) -> Option<(Real, Real)> {
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 = Real::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;
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))
}
}
}
}