use axiolid_contracts::Sign;
use axiolid_core::{Point2, Point3};
use crate::orient2d;
#[must_use]
pub fn dominant_axis(normal: Point3) -> usize {
let (x, y, z) = (normal.x.abs(), normal.y.abs(), normal.z.abs());
if x >= y && x >= z {
0
} else if y >= z {
1
} else {
2
}
}
#[must_use]
pub fn project(point: Point3, axis: usize) -> Point2 {
match axis {
0 => Point2::new(point.y, point.z),
1 => Point2::new(point.x, point.z),
_ => Point2::new(point.x, point.y),
}
}
fn lift(flat: Point2, axis: usize, reference: [Point3; 3]) -> Point3 {
let normal = (reference[1] - reference[0]).cross(reference[2] - reference[0]);
let origin = reference[0];
match axis {
0 => {
let (y, z) = (flat.x, flat.y);
let x = origin.x - (normal.y * (y - origin.y) + normal.z * (z - origin.z)) / normal.x;
Point3::new(x, y, z)
}
1 => {
let (x, z) = (flat.x, flat.y);
let y = origin.y - (normal.x * (x - origin.x) + normal.z * (z - origin.z)) / normal.y;
Point3::new(x, y, z)
}
_ => {
let (x, y) = (flat.x, flat.y);
let z = origin.z - (normal.x * (x - origin.x) + normal.y * (y - origin.y)) / normal.z;
Point3::new(x, y, z)
}
}
}
#[must_use]
pub fn coplanar_overlap(subject: [Point3; 3], clip: [Point3; 3]) -> Vec<Point3> {
let normal = (clip[1] - clip[0]).cross(clip[2] - clip[0]);
let axis = dominant_axis(normal);
let flat_clip = clip.map(|p| project(p, axis));
let clip_ring = match orientation(flat_clip) {
Sign::Negative => [flat_clip[0], flat_clip[2], flat_clip[1]],
_ => flat_clip,
};
let mut polygon: Vec<Point2> = subject.map(|p| project(p, axis)).to_vec();
for corner in 0..3 {
let edge_start = clip_ring[corner];
let edge_end = clip_ring[(corner + 1) % 3];
polygon = clip_to_halfplane(&polygon, edge_start, edge_end);
if polygon.is_empty() {
return Vec::new();
}
}
polygon.dedup();
if polygon.len() > 1 && polygon[0] == polygon[polygon.len() - 1] {
polygon.pop();
}
if polygon.len() < 3 {
return Vec::new();
}
polygon.into_iter().map(|p| lift(p, axis, clip)).collect()
}
fn clip_to_halfplane(polygon: &[Point2], a: Point2, b: Point2) -> Vec<Point2> {
let mut out = Vec::with_capacity(polygon.len() + 1);
for index in 0..polygon.len() {
let current = polygon[index];
let next = polygon[(index + 1) % polygon.len()];
let current_side = side_of(a, b, current);
let next_side = side_of(a, b, next);
if current_side != Sign::Negative {
out.push(current);
}
if (current_side == Sign::Positive && next_side == Sign::Negative)
|| (current_side == Sign::Negative && next_side == Sign::Positive)
{
out.push(line_crossing(current, next, a, b));
}
}
out
}
fn side_of(a: Point2, b: Point2, point: Point2) -> Sign {
orient2d(a, b, point)
.sign()
.expect("certified predicates are total")
}
fn orientation([a, b, c]: [Point2; 3]) -> Sign {
orient2d(a, b, c)
.sign()
.expect("certified predicates are total")
}
fn line_crossing(start: Point2, end: Point2, a: Point2, b: Point2) -> Point2 {
let edge = b - a;
let start_height = edge.x * (start.y - a.y) - edge.y * (start.x - a.x);
let end_height = edge.x * (end.y - a.y) - edge.y * (end.x - a.x);
let span = start_height - end_height;
if span == 0.0 {
return start;
}
let t = start_height / span;
Point2::new(
start.x + (end.x - start.x) * t,
start.y + (end.y - start.y) * t,
)
}