use crate::math::{Point, Real, Vector, DIM};
use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
use crate::shape::{FeatureId, Triangle, TrianglePointLocation};
#[inline]
fn compute_result(pt: &Point<Real>, proj: Point<Real>) -> PointProjection {
#[cfg(feature = "dim2")]
{
PointProjection::new(*pt == proj, proj)
}
#[cfg(feature = "dim3")]
{
PointProjection::new(relative_eq!(proj, *pt), proj)
}
}
impl PointQuery for Triangle {
#[inline]
fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
self.project_local_point_and_get_location(pt, solid).0
}
#[inline]
fn project_local_point_and_get_feature(
&self,
pt: &Point<Real>,
) -> (PointProjection, FeatureId) {
let (proj, loc) = if DIM == 2 {
self.project_local_point_and_get_location(pt, false)
} else {
self.project_local_point_and_get_location(pt, true)
};
let feature = match loc {
TrianglePointLocation::OnVertex(i) => FeatureId::Vertex(i),
#[cfg(feature = "dim3")]
TrianglePointLocation::OnEdge(i, _) => FeatureId::Edge(i),
#[cfg(feature = "dim2")]
TrianglePointLocation::OnEdge(i, _) => FeatureId::Face(i),
TrianglePointLocation::OnFace(i, _) => FeatureId::Face(i),
TrianglePointLocation::OnSolid => FeatureId::Face(0),
};
(proj, feature)
}
}
impl PointQueryWithLocation for Triangle {
type Location = TrianglePointLocation;
#[inline]
fn project_local_point_and_get_location(
&self,
pt: &Point<Real>,
solid: bool,
) -> (PointProjection, Self::Location) {
let a = self.a;
let b = self.b;
let c = self.c;
let ab = b - a;
let ac = c - a;
let ap = pt - a;
let ab_ap = ab.dot(&ap);
let ac_ap = ac.dot(&ap);
if ab_ap <= 0.0 && ac_ap <= 0.0 {
return (compute_result(pt, a), TrianglePointLocation::OnVertex(0));
}
let bp = pt - b;
let ab_bp = ab.dot(&bp);
let ac_bp = ac.dot(&bp);
if ab_bp >= 0.0 && ac_bp <= ab_bp {
return (compute_result(pt, b), TrianglePointLocation::OnVertex(1));
}
let cp = pt - c;
let ab_cp = ab.dot(&cp);
let ac_cp = ac.dot(&cp);
if ac_cp >= 0.0 && ab_cp <= ac_cp {
return (compute_result(pt, c), TrianglePointLocation::OnVertex(2));
}
enum ProjectionInfo {
OnAB,
OnAC,
OnBC,
OnFace(usize, Real, Real, Real),
}
fn stable_check_edges_voronoi(
ab: &Vector<Real>,
ac: &Vector<Real>,
bc: &Vector<Real>,
ap: &Vector<Real>,
bp: &Vector<Real>,
cp: &Vector<Real>,
ab_ap: Real,
ab_bp: Real,
ac_ap: Real,
ac_cp: Real,
ac_bp: Real,
ab_cp: Real,
) -> ProjectionInfo {
#[cfg(feature = "dim2")]
{
let n = ab.perp(ac);
let vc = n * ab.perp(ap);
if vc < 0.0 && ab_ap >= 0.0 && ab_bp <= 0.0 {
return ProjectionInfo::OnAB;
}
let vb = -n * ac.perp(cp);
if vb < 0.0 && ac_ap >= 0.0 && ac_cp <= 0.0 {
return ProjectionInfo::OnAC;
}
let va = n * bc.perp(bp);
if va < 0.0 && ac_bp - ab_bp >= 0.0 && ab_cp - ac_cp >= 0.0 {
return ProjectionInfo::OnBC;
}
ProjectionInfo::OnFace(0, va, vb, vc)
}
#[cfg(feature = "dim3")]
{
let n;
#[cfg(feature = "improved_fixed_point_support")]
{
let scaled_n = ab.cross(&ac);
n = scaled_n.try_normalize(0.0).unwrap_or(scaled_n);
}
#[cfg(not(feature = "improved_fixed_point_support"))]
{
n = ab.cross(ac);
}
let vc = n.dot(&ab.cross(ap));
if vc < 0.0 && ab_ap >= 0.0 && ab_bp <= 0.0 {
return ProjectionInfo::OnAB;
}
let vb = -n.dot(&ac.cross(cp));
if vb < 0.0 && ac_ap >= 0.0 && ac_cp <= 0.0 {
return ProjectionInfo::OnAC;
}
let va = n.dot(&bc.cross(bp));
if va < 0.0 && ac_bp - ab_bp >= 0.0 && ab_cp - ac_cp >= 0.0 {
return ProjectionInfo::OnBC;
}
let clockwise = if n.dot(ap) >= 0.0 { 0 } else { 1 };
ProjectionInfo::OnFace(clockwise, va, vb, vc)
}
}
let bc = c - b;
match stable_check_edges_voronoi(
&ab, &ac, &bc, &ap, &bp, &cp, ab_ap, ab_bp, ac_ap, ac_cp, ac_bp, ab_cp,
) {
ProjectionInfo::OnAB => {
let v = ab_ap / ab.norm_squared();
let bcoords = [1.0 - v, v];
let res = a + ab * v;
return (
compute_result(pt, res),
TrianglePointLocation::OnEdge(0, bcoords),
);
}
ProjectionInfo::OnAC => {
let w = ac_ap / ac.norm_squared();
let bcoords = [1.0 - w, w];
let res = a + ac * w;
return (
compute_result(pt, res),
TrianglePointLocation::OnEdge(2, bcoords),
);
}
ProjectionInfo::OnBC => {
let w = bc.dot(&bp) / bc.norm_squared();
let bcoords = [1.0 - w, w];
let res = b + bc * w;
return (
compute_result(pt, res),
TrianglePointLocation::OnEdge(1, bcoords),
);
}
ProjectionInfo::OnFace(face_side, va, vb, vc) => {
if DIM != 2 {
if va + vb + vc != 0.0 {
let denom = 1.0 / (va + vb + vc);
let v = vb * denom;
let w = vc * denom;
let bcoords = [1.0 - v - w, v, w];
let res = a + ab * v + ac * w;
return (
compute_result(pt, res),
TrianglePointLocation::OnFace(face_side as u32, bcoords),
);
}
}
}
}
if solid {
(
PointProjection::new(true, *pt),
TrianglePointLocation::OnSolid,
)
} else {
let v = ab_ap / (ab_ap - ab_bp); let w = ac_ap / (ac_ap - ac_cp); let u = (ac_bp - ab_bp) / (ac_bp - ab_bp + ab_cp - ac_cp); let bc = c - b;
let d_ab = ap.norm_squared() - (ab.norm_squared() * v * v);
let d_ac = ap.norm_squared() - (ac.norm_squared() * w * w);
let d_bc = bp.norm_squared() - (bc.norm_squared() * u * u);
let proj;
let loc;
if d_ab < d_ac {
if d_ab < d_bc {
let bcoords = [1.0 - v, v];
proj = a + ab * v;
loc = TrianglePointLocation::OnEdge(0, bcoords);
} else {
let bcoords = [1.0 - u, u];
proj = b + bc * u;
loc = TrianglePointLocation::OnEdge(1, bcoords);
}
} else if d_ac < d_bc {
let bcoords = [1.0 - w, w];
proj = a + ac * w;
loc = TrianglePointLocation::OnEdge(2, bcoords);
} else {
let bcoords = [1.0 - u, u];
proj = b + bc * u;
loc = TrianglePointLocation::OnEdge(1, bcoords);
}
(PointProjection::new(true, proj), loc)
}
}
}