pub(crate) fn dt_min(a: f32, b: f32) -> f32 {
if a < b { a } else { b }
}
pub(crate) fn dt_max(a: f32, b: f32) -> f32 {
if a > b { a } else { b }
}
pub(crate) fn dt_clamp(v: f32, mn: f32, mx: f32) -> f32 {
if v < mn {
mn
} else if v > mx {
mx
} else {
v
}
}
pub(crate) fn vsub(a: &[f32; 3], b: &[f32; 3]) -> [f32; 3] {
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
pub(crate) fn vadd(a: &[f32; 3], b: &[f32; 3]) -> [f32; 3] {
[a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn vmad(a: &[f32; 3], b: &[f32; 3], s: f32) -> [f32; 3] {
[a[0] + b[0] * s, a[1] + b[1] * s, a[2] + b[2] * s]
}
pub(crate) fn vlerp(a: &[f32; 3], b: &[f32; 3], t: f32) -> [f32; 3] {
[
a[0] + (b[0] - a[0]) * t,
a[1] + (b[1] - a[1]) * t,
a[2] + (b[2] - a[2]) * t,
]
}
pub(crate) fn vdist_sqr(a: &[f32; 3], b: &[f32; 3]) -> f32 {
let dx = b[0] - a[0];
let dy = b[1] - a[1];
let dz = b[2] - a[2];
dx * dx + dy * dy + dz * dz
}
pub(crate) fn vdist(a: &[f32; 3], b: &[f32; 3]) -> f32 {
vdist_sqr(a, b).sqrt()
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn vdist_2d_sqr(a: &[f32; 3], b: &[f32; 3]) -> f32 {
let dx = b[0] - a[0];
let dz = b[2] - a[2];
dx * dx + dz * dz
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn vdist_2d(a: &[f32; 3], b: &[f32; 3]) -> f32 {
vdist_2d_sqr(a, b).sqrt()
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn vdot_2d(a: &[f32; 3], b: &[f32; 3]) -> f32 {
a[0] * b[0] + a[2] * b[2]
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn vperp_2d(a: &[f32; 3], b: &[f32; 3]) -> f32 {
a[2] * b[0] - a[0] * b[2]
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn tri_area_2d(a: &[f32; 3], b: &[f32; 3], c: &[f32; 3]) -> f32 {
let abx = b[0] - a[0];
let abz = b[2] - a[2];
let acx = c[0] - a[0];
let acz = c[2] - a[2];
acx * abz - abx * acz
}
pub(crate) fn overlap_quant_bounds(
amin: [u16; 3],
amax: [u16; 3],
bmin: [u16; 3],
bmax: [u16; 3],
) -> bool {
!(amin[0] > bmax[0]
|| amax[0] < bmin[0]
|| amin[1] > bmax[1]
|| amax[1] < bmin[1]
|| amin[2] > bmax[2]
|| amax[2] < bmin[2])
}
pub(crate) fn overlap_bounds(
amin: &[f32; 3],
amax: &[f32; 3],
bmin: &[f32; 3],
bmax: &[f32; 3],
) -> bool {
!(amin[0] > bmax[0]
|| amax[0] < bmin[0]
|| amin[1] > bmax[1]
|| amax[1] < bmin[1]
|| amin[2] > bmax[2]
|| amax[2] < bmin[2])
}
pub(crate) fn opposite_tile(side: i32) -> i32 {
(side + 4) & 7
}
pub(crate) fn distance_pt_seg_sqr_2d(pt: &[f32; 3], p: &[f32; 3], q: &[f32; 3]) -> (f32, f32) {
let pqx = q[0] - p[0];
let pqz = q[2] - p[2];
let mut dx = pt[0] - p[0];
let mut dz = pt[2] - p[2];
let d = pqx * pqx + pqz * pqz;
let mut t = pqx * dx + pqz * dz;
if d > 0.0 {
t /= d;
}
t = t.clamp(0.0, 1.0);
dx = p[0] + t * pqx - pt[0];
dz = p[2] + t * pqz - pt[2];
(dx * dx + dz * dz, t)
}
fn crossing(pt: &[f32; 3], vi: &[f32; 3], vj: &[f32; 3]) -> bool {
((vi[2] > pt[2]) != (vj[2] > pt[2]))
&& (pt[0] < (vj[0] - vi[0]) * (pt[2] - vi[2]) / (vj[2] - vi[2]) + vi[0])
}
pub(crate) fn point_in_polygon(pt: &[f32; 3], verts: &[[f32; 3]]) -> bool {
let mut c = false;
let mut prev = verts.last();
for vi in verts {
if let Some(vj) = prev
&& crossing(pt, vi, vj)
{
c = !c;
}
prev = Some(vi);
}
c
}
#[expect(
clippy::many_single_char_names,
reason = "the names of the upstream function are kept for comparison"
)]
pub(crate) fn distance_pt_poly_edges_sqr(
pt: &[f32; 3],
verts: &[[f32; 3]],
ed: &mut [f32],
et: &mut [f32],
) -> Option<bool> {
let n = verts.len();
if ed.len() < n || et.len() < n {
return None;
}
let mut c = false;
for (i, vi) in verts.iter().enumerate() {
let j = if i == 0 { n.saturating_sub(1) } else { i - 1 };
let vj = verts.get(j)?;
if crossing(pt, vi, vj) {
c = !c;
}
let (d, t) = distance_pt_seg_sqr_2d(pt, vj, vi);
*ed.get_mut(j)? = d;
*et.get_mut(j)? = t;
}
Some(c)
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) struct SegPolyHit {
pub tmin: f32,
pub tmax: f32,
pub seg_min: i32,
pub seg_max: i32,
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn intersect_segment_poly_2d(
p0: &[f32; 3],
p1: &[f32; 3],
verts: &[[f32; 3]],
) -> Option<SegPolyHit> {
const EPS: f32 = 0.000_001;
let mut tmin = 0.0_f32;
let mut tmax = 1.0_f32;
let mut seg_min = -1_i32;
let mut seg_max = -1_i32;
let dir = vsub(p1, p0);
let n = verts.len();
for (i, vi) in verts.iter().enumerate() {
let j = if i == 0 { n.saturating_sub(1) } else { i - 1 };
let vj = verts.get(j)?;
let edge = vsub(vi, vj);
let diff = vsub(p0, vj);
let nn = vperp_2d(&edge, &diff);
let d = vperp_2d(&dir, &edge);
if d.abs() < EPS {
if nn < 0.0 {
return None;
}
continue;
}
let t = nn / d;
let ji = i32::try_from(j).ok()?;
if d < 0.0 {
if t > tmin {
tmin = t;
seg_min = ji;
if tmin > tmax {
return None;
}
}
} else if t < tmax {
tmax = t;
seg_max = ji;
if tmax < tmin {
return None;
}
}
}
Some(SegPolyHit {
tmin,
tmax,
seg_min,
seg_max,
})
}
#[expect(
clippy::many_single_char_names,
reason = "the names of the upstream function are kept for comparison"
)]
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn intersect_seg_seg_2d(
ap: &[f32; 3],
aq: &[f32; 3],
bp: &[f32; 3],
bq: &[f32; 3],
) -> Option<(f32, f32)> {
let u = vsub(aq, ap);
let v = vsub(bq, bp);
let w = vsub(ap, bp);
let d = u[0] * v[2] - u[2] * v[0];
if d.abs() < 1e-6 {
return None;
}
let s = (v[0] * w[2] - v[2] * w[0]) / d;
let t = (u[0] * w[2] - u[2] * w[0]) / d;
Some((s, t))
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
fn project_poly(axis: &[f32; 3], poly: &[[f32; 3]]) -> (f32, f32) {
let mut it = poly.iter();
let Some(first) = it.next() else {
return (0.0, 0.0);
};
let mut rmin = vdot_2d(axis, first);
let mut rmax = rmin;
for v in it {
let d = vdot_2d(axis, v);
rmin = if rmin < d { rmin } else { d };
rmax = if rmax > d { rmax } else { d };
}
(rmin, rmax)
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
fn overlap_range(amin: f32, amax: f32, bmin: f32, bmax: f32, eps: f32) -> bool {
!((amin + eps) > bmax || (amax - eps) < bmin)
}
#[expect(
clippy::similar_names,
reason = "the names of the upstream function are kept for comparison"
)]
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn overlap_poly_poly_2d(polya: &[[f32; 3]], polyb: &[[f32; 3]]) -> bool {
const EPS: f32 = 1e-4;
for poly in [polya, polyb] {
let n = poly.len();
for (i, vb) in poly.iter().enumerate() {
let j = if i == 0 { n.saturating_sub(1) } else { i - 1 };
let Some(va) = poly.get(j) else { continue };
let axis = [vb[2] - va[2], 0.0, -(vb[0] - va[0])];
let (amin, amax) = project_poly(&axis, polya);
let (bmin, bmax) = project_poly(&axis, polyb);
if !overlap_range(amin, amax, bmin, bmax, EPS) {
return false;
}
}
}
true
}
#[cfg_attr(
not(test),
expect(
dead_code,
reason = "used by the polygon query and navmesh generation tasks that follow"
)
)]
pub(crate) fn calc_poly_center(verts: &[[f32; 3]]) -> [f32; 3] {
let mut tc = [0.0_f32; 3];
for v in verts {
tc[0] += v[0];
tc[1] += v[1];
tc[2] += v[2];
}
let s = 1.0 / crate::navmesh::num::i32_to_f32(i32::try_from(verts.len()).unwrap_or(i32::MAX));
[tc[0] * s, tc[1] * s, tc[2] * s]
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn segment_distance_clamps() {
let (d, t) = distance_pt_seg_sqr_2d(&[5.0, 0.0, 3.0], &[0.0; 3], &[10.0, 0.0, 0.0]);
assert!((d - 9.0).abs() < 1e-6 && (t - 0.5).abs() < 1e-6);
let (d, t) = distance_pt_seg_sqr_2d(&[-3.0, 0.0, 4.0], &[0.0; 3], &[10.0, 0.0, 0.0]);
assert!((d - 25.0).abs() < 1e-6 && t.abs() < 1e-6);
}
const SQUARE: [[f32; 3]; 4] = [
[0.0, 0.0, 0.0],
[0.0, 0.0, 10.0],
[10.0, 0.0, 10.0],
[10.0, 0.0, 0.0],
];
#[test]
fn point_in_polygon_and_edges() {
assert!(point_in_polygon(&[5.0, 0.0, 5.0], &SQUARE));
assert!(!point_in_polygon(&[15.0, 0.0, 5.0], &SQUARE));
let mut ed = [0.0; 4];
let mut et = [0.0; 4];
assert_eq!(
distance_pt_poly_edges_sqr(&[15.0, 0.0, 5.0], &SQUARE, &mut ed, &mut et),
Some(false)
);
assert_eq!(
distance_pt_poly_edges_sqr(&[5.0, 0.0, 5.0], &SQUARE, &mut ed, &mut et),
Some(true)
);
assert_eq!(
distance_pt_poly_edges_sqr(&[5.0, 0.0, 5.0], &SQUARE, &mut ed[..3], &mut et),
None
);
distance_pt_poly_edges_sqr(&[15.0, 0.0, 5.0], &SQUARE, &mut ed, &mut et);
assert!((ed[2] - 25.0).abs() < 1e-6);
assert!((et[2] - 0.5).abs() < 1e-6);
}
#[test]
fn segment_through_polygon() {
let hit = intersect_segment_poly_2d(&[-5.0, 0.0, 5.0], &[15.0, 0.0, 5.0], &SQUARE)
.expect("crosses");
assert!((hit.tmin - 0.25).abs() < 1e-6 && (hit.tmax - 0.75).abs() < 1e-6);
assert!(
intersect_segment_poly_2d(&[-5.0, 0.0, 20.0], &[15.0, 0.0, 20.0], &SQUARE).is_none()
);
}
#[test]
fn seg_seg_and_overlap() {
let (s, t) = intersect_seg_seg_2d(
&[0.0; 3],
&[10.0, 0.0, 10.0],
&[0.0, 0.0, 10.0],
&[10.0, 0.0, 0.0],
)
.expect("cross");
assert!((s - 0.5).abs() < 1e-6 && (t - 0.5).abs() < 1e-6);
assert!(
intersect_seg_seg_2d(
&[0.0; 3],
&[1.0, 0.0, 0.0],
&[0.0, 0.0, 1.0],
&[1.0, 0.0, 1.0]
)
.is_none()
);
let shifted: Vec<[f32; 3]> = SQUARE.iter().map(|v| [v[0] + 5.0, v[1], v[2]]).collect();
let far: Vec<[f32; 3]> = SQUARE.iter().map(|v| [v[0] + 30.0, v[1], v[2]]).collect();
assert!(overlap_poly_poly_2d(&SQUARE, &shifted));
assert!(!overlap_poly_poly_2d(&SQUARE, &far));
}
#[test]
fn bounds_and_vectors() {
assert!(overlap_quant_bounds([0; 3], [5; 3], [5; 3], [9; 3]));
assert!(!overlap_quant_bounds([0; 3], [4; 3], [5; 3], [9; 3]));
assert!(overlap_bounds(&[0.0; 3], &[1.0; 3], &[1.0; 3], &[2.0; 3]));
assert_eq!(opposite_tile(1), 5);
assert_eq!(opposite_tile(6), 2);
assert_eq!(vlerp(&[0.0; 3], &[2.0, 4.0, 6.0], 0.5), [1.0, 2.0, 3.0]);
assert_eq!(vmad(&[1.0; 3], &[1.0, 2.0, 3.0], 2.0), [3.0, 5.0, 7.0]);
assert_eq!(vadd(&[1.0; 3], &[1.0; 3]), [2.0; 3]);
assert!((vdist(&[0.0; 3], &[3.0, 4.0, 0.0]) - 5.0).abs() < 1e-6);
assert!((vdist_2d(&[0.0; 3], &[3.0, 9.0, 4.0]) - 5.0).abs() < 1e-6);
assert!(tri_area_2d(&[0.0; 3], &[1.0, 0.0, 0.0], &[0.0, 0.0, 1.0]) < 0.0);
let c = calc_poly_center(&SQUARE);
assert_eq!(c, [5.0, 0.0, 5.0]);
}
}