use super::{OpeningFrame, NORMALIZE_EPSILON};
use crate::{Error, Mesh, Point3, Result, Vector3};
use nalgebra::{Matrix3, Matrix4};
use rustc_hash::FxHashMap;
pub(super) fn extract_rotation_columns(m: &Matrix4<f64>) -> (Vector3<f64>, Vector3<f64>, Vector3<f64>) {
(
Vector3::new(m[(0, 0)], m[(1, 0)], m[(2, 0)]),
Vector3::new(m[(0, 1)], m[(1, 1)], m[(2, 1)]),
Vector3::new(m[(0, 2)], m[(1, 2)], m[(2, 2)]),
)
}
pub(super) fn rotate_and_normalize(
rot: &(Vector3<f64>, Vector3<f64>, Vector3<f64>),
dir: &Vector3<f64>,
) -> Result<Vector3<f64>> {
(rot.0 * dir.x + rot.1 * dir.y + rot.2 * dir.z)
.try_normalize(NORMALIZE_EPSILON)
.ok_or_else(|| Error::geometry("Zero-length direction vector".to_string()))
}
#[inline]
pub(super) fn wall_thinnest_axis_dir(wall_min: &Point3<f64>, wall_max: &Point3<f64>) -> Vector3<f64> {
let ext = [
(wall_max.x - wall_min.x).abs(),
(wall_max.y - wall_min.y).abs(),
(wall_max.z - wall_min.z).abs(),
];
let mut axis = 0;
for i in 1..3 {
if ext[i] < ext[axis] {
axis = i;
}
}
match axis {
0 => Vector3::new(1.0, 0.0, 0.0),
1 => Vector3::new(0.0, 1.0, 0.0),
_ => Vector3::new(0.0, 0.0, 1.0),
}
}
pub(super) fn infer_box_penetration_dir(
open_min: &Point3<f64>,
open_max: &Point3<f64>,
host_min: &Point3<f64>,
host_max: &Point3<f64>,
) -> Vector3<f64> {
let o = [
(open_min.x, open_max.x),
(open_min.y, open_max.y),
(open_min.z, open_max.z),
];
let h = [
(host_min.x, host_max.x),
(host_min.y, host_max.y),
(host_min.z, host_max.z),
];
let mut best_axis = usize::MAX;
let mut best_past = 1.0e-6;
for a in 0..3 {
let past = (h[a].0 - o[a].0).max(0.0) + (o[a].1 - h[a].1).max(0.0);
if past > best_past {
best_past = past;
best_axis = a;
}
}
match best_axis {
0 => Vector3::new(1.0, 0.0, 0.0),
1 => Vector3::new(0.0, 1.0, 0.0),
2 => Vector3::new(0.0, 0.0, 1.0),
_ => wall_thinnest_axis_dir(open_min, open_max),
}
}
pub(super) fn opening_mesh_thinnest_axis_dir(opening_mesh: &Mesh) -> Vector3<f64> {
let (mn, mx) = opening_mesh.bounds();
wall_thinnest_axis_dir(
&Point3::new(mn.x as f64, mn.y as f64, mn.z as f64),
&Point3::new(mx.x as f64, mx.y as f64, mx.z as f64),
)
}
pub(super) fn spatial_cluster_count(
bounds: &[(Point3<f64>, Point3<f64>, Option<Vector3<f64>>)],
) -> usize {
const TOUCH_EPS: f64 = 1.0e-3; let n = bounds.len();
if n <= 1 {
return n;
}
let mut parent: Vec<usize> = (0..n).collect();
fn find(p: &mut [usize], x: usize) -> usize {
let mut r = x;
while p[r] != r {
r = p[r];
}
let mut c = x;
while p[c] != r {
let nxt = p[c];
p[c] = r;
c = nxt;
}
r
}
let overlaps = |a: &(Point3<f64>, Point3<f64>, Option<Vector3<f64>>),
b: &(Point3<f64>, Point3<f64>, Option<Vector3<f64>>)| {
a.0.x <= b.1.x + TOUCH_EPS
&& b.0.x <= a.1.x + TOUCH_EPS
&& a.0.y <= b.1.y + TOUCH_EPS
&& b.0.y <= a.1.y + TOUCH_EPS
&& a.0.z <= b.1.z + TOUCH_EPS
&& b.0.z <= a.1.z + TOUCH_EPS
};
for i in 0..n {
for j in (i + 1)..n {
if overlaps(&bounds[i], &bounds[j]) {
let (ri, rj) = (find(&mut parent, i), find(&mut parent, j));
parent[ri] = rj;
}
}
}
(0..n).filter(|&i| find(&mut parent, i) == i).count()
}
pub(super) fn mesh_is_closed_exact(m: &Mesh) -> bool {
use std::collections::HashMap;
let key = |i: u32| {
let b = i as usize * 3;
(
m.positions[b].to_bits(),
m.positions[b + 1].to_bits(),
m.positions[b + 2].to_bits(),
)
};
let mut edges: HashMap<_, i64> = HashMap::new();
for t in m.indices.chunks_exact(3) {
let k = [key(t[0]), key(t[1]), key(t[2])];
for (u, v) in [(0usize, 1usize), (1, 2), (2, 0)] {
if k[u] == k[v] {
return false; }
*edges.entry((k[u], k[v])).or_insert(0) += 1;
*edges.entry((k[v], k[u])).or_insert(0) -= 1;
}
}
!m.indices.is_empty() && edges.values().all(|&c| c == 0)
}
#[inline]
pub(super) fn is_axis_aligned_direction(dir: &Vector3<f64>) -> bool {
const AXIS_THRESHOLD: f64 = 0.999_847_695;
dir.x.abs().max(dir.y.abs()).max(dir.z.abs()) > AXIS_THRESHOLD
}
#[inline]
pub(super) fn rotate_point(r: &Matrix3<f64>, x: f64, y: f64, z: f64) -> [f64; 3] {
[
r[(0, 0)] * x + r[(0, 1)] * y + r[(0, 2)] * z,
r[(1, 0)] * x + r[(1, 1)] * y + r[(1, 2)] * z,
r[(2, 0)] * x + r[(2, 1)] * y + r[(2, 2)] * z,
]
}
pub(super) fn thin_axis(half: &[f64; 3]) -> usize {
(0..3)
.min_by(|&a, &b| half[a].partial_cmp(&half[b]).unwrap())
.unwrap()
}
pub(super) fn signed_permutation_map(m: &Matrix3<f64>, tol: f64) -> Option<[(usize, f64); 3]> {
let mut out = [(0usize, 1.0f64); 3];
let mut used = [false; 3];
for i in 0..3 {
let (mut best, mut best_abs, mut second) = (0usize, 0.0, 0.0);
for j in 0..3 {
let a = m[(i, j)].abs();
if a > best_abs {
second = best_abs;
best_abs = a;
best = j;
} else if a > second {
second = a;
}
}
if best_abs < 1.0 - tol || second > tol || used[best] {
return None;
}
used[best] = true;
out[i] = (best, m[(i, best)].signum());
}
Some(out)
}
pub(super) fn rotate_mesh_into_frame(mesh: &Mesh, rt: &Matrix3<f64>, center: &Point3<f64>) -> Mesh {
let mut positions = Vec::with_capacity(mesh.positions.len());
for c in mesh.positions.chunks_exact(3) {
let p = rotate_point(
rt,
c[0] as f64 - center.x,
c[1] as f64 - center.y,
c[2] as f64 - center.z,
);
positions.push(p[0] as f32);
positions.push(p[1] as f32);
positions.push(p[2] as f32);
}
Mesh {
normals: vec![0.0; positions.len()],
indices: mesh.indices.clone(),
rtc_applied: mesh.rtc_applied,
origin: [0.0; 3],
positions,
instance_meta: None, local_bounds: None, local_to_world: None }
}
pub(super) fn rotate_mesh_from_frame(mesh: &Mesh, r: &Matrix3<f64>, center: &Point3<f64>) -> Mesh {
let mut positions = Vec::with_capacity(mesh.positions.len());
for c in mesh.positions.chunks_exact(3) {
let p = rotate_point(r, c[0] as f64, c[1] as f64, c[2] as f64);
positions.push(p[0] as f32);
positions.push(p[1] as f32);
positions.push(p[2] as f32);
}
let mut normals = Vec::with_capacity(mesh.normals.len());
for c in mesh.normals.chunks_exact(3) {
let p = rotate_point(r, c[0] as f64, c[1] as f64, c[2] as f64);
normals.push(p[0] as f32);
normals.push(p[1] as f32);
normals.push(p[2] as f32);
}
Mesh {
positions,
normals,
indices: mesh.indices.clone(),
rtc_applied: mesh.rtc_applied,
origin: [center.x, center.y, center.z],
instance_meta: None, local_bounds: None, local_to_world: None }
}
pub(super) fn mesh_signed_volume(mesh: &Mesh) -> f64 {
let v = |i: u32| {
let b = i as usize * 3;
[
mesh.positions[b] as f64,
mesh.positions[b + 1] as f64,
mesh.positions[b + 2] as f64,
]
};
mesh.indices
.chunks_exact(3)
.map(|t| {
let (a, b, c) = (v(t[0]), v(t[1]), v(t[2]));
a[0] * (b[1] * c[2] - b[2] * c[1]) + a[1] * (b[2] * c[0] - b[0] * c[2])
+ a[2] * (b[0] * c[1] - b[1] * c[0])
})
.sum::<f64>()
/ 6.0
}
pub(super) fn param_cut_watertight(mesh: &Mesh) -> bool {
let key = |i: u32| -> (i64, i64, i64) {
let b = i as usize * 3;
let q = |v: f32| (v as f64 / 1.0e-4).round() as i64;
(
q(mesh.positions[b]),
q(mesh.positions[b + 1]),
q(mesh.positions[b + 2]),
)
};
let mut edges: FxHashMap<((i64, i64, i64), (i64, i64, i64)), i32> = FxHashMap::default();
for tri in mesh.indices.chunks_exact(3) {
let (ka, kb, kc) = (key(tri[0]), key(tri[1]), key(tri[2]));
if ka == kb || kb == kc || kc == ka {
continue;
}
for (x, y) in [(ka, kb), (kb, kc), (kc, ka)] {
let e = if x < y { (x, y) } else { (y, x) };
*edges.entry(e).or_insert(0) += 1;
}
}
!edges.is_empty() && edges.values().all(|&c| c == 2)
}
#[inline]
pub(super) fn mesh_point(mesh: &Mesh, index: u32) -> Option<Point3<f64>> {
let base = index as usize * 3;
Some(Point3::new(
*mesh.positions.get(base)? as f64,
*mesh.positions.get(base + 1)? as f64,
*mesh.positions.get(base + 2)? as f64,
))
}
pub(super) fn ray_triangle_param(
origin: Point3<f64>,
dir: &Vector3<f64>,
a: Point3<f64>,
b: Point3<f64>,
c: Point3<f64>,
) -> Option<f64> {
const EPS: f64 = 1e-9;
let e1 = b - a;
let e2 = c - a;
let pvec = dir.cross(&e2);
let det = e1.dot(&pvec);
if det.abs() < EPS {
return None; }
let inv_det = 1.0 / det;
let tvec = origin - a;
let u = tvec.dot(&pvec) * inv_det;
if !(-EPS..=1.0 + EPS).contains(&u) {
return None;
}
let qvec = tvec.cross(&e1);
let v = dir.dot(&qvec) * inv_det;
if v < -EPS || u + v > 1.0 + EPS {
return None;
}
Some(e2.dot(&qvec) * inv_det)
}
pub(super) fn axis_line_crosses_mesh(mesh: &Mesh, point: Point3<f64>, axis: &Vector3<f64>) -> bool {
for tri in mesh.indices.chunks_exact(3) {
let (Some(a), Some(b), Some(c)) = (
mesh_point(mesh, tri[0]),
mesh_point(mesh, tri[1]),
mesh_point(mesh, tri[2]),
) else {
continue;
};
if ray_triangle_param(point, axis, a, b, c).is_some() {
return true;
}
}
false
}
pub(super) fn opening_redundant_with_host(host: &Mesh, opening: &Mesh, axis: &Vector3<f64>) -> bool {
let Some(axis) = axis.try_normalize(NORMALIZE_EPSILON) else {
return false;
};
let Some(centroid) = mesh_vertex_centroid(opening) else {
return false;
};
const PULL_TO_CENTROID: f64 = 0.1;
if axis_line_crosses_mesh(host, centroid, &axis) {
return false;
}
for v in opening.positions.chunks_exact(3) {
let vertex = Point3::new(v[0] as f64, v[1] as f64, v[2] as f64);
let sample = vertex + (centroid - vertex) * PULL_TO_CENTROID;
if axis_line_crosses_mesh(host, sample, &axis) {
return false;
}
}
true
}
pub(super) fn point_inside_mesh(mesh: &Mesh, point: Point3<f64>) -> bool {
let dir = Vector3::new(0.573_257_1, 0.665_412_3, 0.477_889_5);
let mut crossings = 0usize;
for tri in mesh.indices.chunks_exact(3) {
let (Some(a), Some(b), Some(c)) = (
mesh_point(mesh, tri[0]),
mesh_point(mesh, tri[1]),
mesh_point(mesh, tri[2]),
) else {
continue;
};
if let Some(t) = ray_triangle_param(point, &dir, a, b, c) {
if t > 1e-9 {
crossings += 1;
}
}
}
crossings % 2 == 1
}
pub(super) fn opening_engulfs_host_solid(host: &Mesh, opening: &Mesh) -> bool {
if host.indices.is_empty() || opening.indices.is_empty() {
return false;
}
let Some(host_centroid) = mesh_vertex_centroid(host) else {
return false;
};
if !point_inside_mesh(opening, host_centroid) {
return false;
}
let inset = {
let mut mn = [f32::INFINITY; 3];
let mut mx = [f32::NEG_INFINITY; 3];
for v in host.positions.chunks_exact(3) {
mn[0] = mn[0].min(v[0]);
mx[0] = mx[0].max(v[0]);
mn[1] = mn[1].min(v[1]);
mx[1] = mx[1].max(v[1]);
mn[2] = mn[2].min(v[2]);
mx[2] = mx[2].max(v[2]);
}
let min_extent = (0..3)
.map(|i| (mx[i] - mn[i]) as f64)
.fold(f64::INFINITY, f64::min);
(min_extent * 0.001).clamp(1e-5, 1e-3)
};
for v in host.positions.chunks_exact(3) {
let vertex = Point3::new(v[0] as f64, v[1] as f64, v[2] as f64);
let to_centroid = host_centroid - vertex;
let dist = to_centroid.norm();
let sample = if dist > inset {
vertex + to_centroid * (inset / dist)
} else {
host_centroid
};
if !point_inside_mesh(opening, sample) {
return false;
}
}
true
}
pub(super) fn mesh_vertex_centroid(mesh: &Mesh) -> Option<Point3<f64>> {
let n = mesh.positions.len() / 3;
if n == 0 {
return None;
}
let (mut sx, mut sy, mut sz) = (0.0f64, 0.0f64, 0.0f64);
for chunk in mesh.positions.chunks_exact(3) {
sx += chunk[0] as f64;
sy += chunk[1] as f64;
sz += chunk[2] as f64;
}
let inv = 1.0 / n as f64;
Some(Point3::new(sx * inv, sy * inv, sz * inv))
}
pub(super) fn extent_along_axis(mesh: &Mesh, axis: &Vector3<f64>) -> Option<f64> {
let mut min = f64::INFINITY;
let mut max = f64::NEG_INFINITY;
for chunk in mesh.positions.chunks_exact(3) {
let p = Vector3::new(chunk[0] as f64, chunk[1] as f64, chunk[2] as f64);
let projection = p.dot(axis);
min = min.min(projection);
max = max.max(projection);
}
min.is_finite().then_some(max - min)
}
pub(super) fn is_rectangular_box_mesh(mesh: &Mesh) -> bool {
let mut axes: Vec<Vector3<f64>> = Vec::with_capacity(4);
let mut tri_axes: Vec<(usize, f64)> = Vec::with_capacity(mesh.indices.len() / 3);
for tri in mesh.indices.chunks_exact(3) {
let (Some(p0), Some(p1), Some(p2)) = (
mesh_point(mesh, tri[0]),
mesh_point(mesh, tri[1]),
mesh_point(mesh, tri[2]),
) else {
continue;
};
let Some(normal) = (p1 - p0).cross(&(p2 - p0)).try_normalize(NORMALIZE_EPSILON) else {
continue;
};
let axis_index = match axes
.iter()
.position(|axis| normal.dot(axis).abs() > 0.98)
{
Some(idx) => idx,
None => {
if axes.len() >= 3 {
return false;
}
axes.push(normal);
axes.len() - 1
}
};
let offset = p0.coords.dot(&axes[axis_index]);
tri_axes.push((axis_index, offset));
}
if axes.len() != 3 {
return false;
}
const ORTHOGONAL_DOT_TOL: f64 = 0.02;
for i in 0..3 {
for j in (i + 1)..3 {
if axes[i].dot(&axes[j]).abs() > ORTHOGONAL_DOT_TOL {
return false;
}
}
}
const PLANE_TOL: f64 = 1e-3;
for axis_index in 0..3 {
let mut planes: Vec<f64> = Vec::with_capacity(3);
for (idx, offset) in &tri_axes {
if *idx != axis_index {
continue;
}
if !planes.iter().any(|p| (p - offset).abs() < PLANE_TOL) {
planes.push(*offset);
if planes.len() > 2 {
return false;
}
}
}
if planes.len() != 2 {
return false;
}
}
true
}
pub(super) fn infer_opening_frame(mesh: &Mesh, extrusion_dir: Option<&Vector3<f64>>) -> Option<OpeningFrame> {
let mut axes: Vec<(Vector3<f64>, f64)> = Vec::new();
for tri in mesh.indices.chunks_exact(3) {
let (Some(p0), Some(p1), Some(p2)) = (
mesh_point(mesh, tri[0]),
mesh_point(mesh, tri[1]),
mesh_point(mesh, tri[2]),
) else {
continue;
};
let normal_raw = (p1 - p0).cross(&(p2 - p0));
let weight = normal_raw.norm();
let Some(mut normal) = normal_raw.try_normalize(NORMALIZE_EPSILON) else {
continue;
};
if let Some((axis, axis_weight)) = axes
.iter_mut()
.find(|(axis, _)| normal.dot(axis).abs() > 0.98)
{
if normal.dot(axis) < 0.0 {
normal = -normal;
}
if let Some(merged) =
(*axis * *axis_weight + normal * weight).try_normalize(NORMALIZE_EPSILON)
{
*axis = merged;
*axis_weight += weight;
}
} else {
axes.push((normal, weight));
}
}
if axes.len() < 3 {
return extrusion_dir.and_then(|dir| OpeningFrame::from_depth(*dir));
}
let depth_index =
if let Some(dir) = extrusion_dir.and_then(|d| d.try_normalize(NORMALIZE_EPSILON)) {
axes.iter()
.enumerate()
.max_by(|(_, (a, _)), (_, (b, _))| a.dot(&dir).abs().total_cmp(&b.dot(&dir).abs()))
.map(|(index, _)| index)?
} else {
axes.iter()
.enumerate()
.filter_map(|(index, (axis, _))| extent_along_axis(mesh, axis).map(|e| (index, e)))
.min_by(|(_, a), (_, b)| a.total_cmp(b))
.map(|(index, _)| index)?
};
let mut depth = axes[depth_index].0;
if let Some(dir) = extrusion_dir {
if depth.dot(dir) < 0.0 {
depth = -depth;
}
}
let mut cross_candidates: Vec<Vector3<f64>> = axes
.iter()
.enumerate()
.filter_map(|(index, (axis, _))| {
(index != depth_index && axis.dot(&depth).abs() < 0.25).then_some(*axis)
})
.collect();
if cross_candidates.len() < 2 {
return OpeningFrame::from_depth(depth);
}
let mut cross_a = cross_candidates.remove(0);
cross_a = (cross_a - depth * cross_a.dot(&depth)).try_normalize(NORMALIZE_EPSILON)?;
let mut cross_b = depth.cross(&cross_a).try_normalize(NORMALIZE_EPSILON)?;
if cross_b.dot(&cross_candidates[0]) < 0.0 {
cross_b = -cross_b;
}
Some(OpeningFrame {
depth,
cross_a,
cross_b,
})
}
pub(super) fn wall_frame_from_depth(depth: Vector3<f64>) -> Option<[Vector3<f64>; 3]> {
let d = depth.try_normalize(NORMALIZE_EPSILON)?;
if d.z.abs() > 0.2 {
return None; }
let up = Vector3::new(0.0, 0.0, 1.0);
let len = up.cross(&d).try_normalize(NORMALIZE_EPSILON)?;
let up = d.cross(&len).try_normalize(NORMALIZE_EPSILON)?; Some([len, up, d])
}
pub(super) fn mesh_to_frame(mesh: &Mesh, axes: &[Vector3<f64>; 3], center: Vector3<f64>) -> Mesh {
let mut positions = Vec::with_capacity(mesh.positions.len());
for ch in mesh.positions.chunks_exact(3) {
let p = Vector3::new(ch[0] as f64, ch[1] as f64, ch[2] as f64) - center;
positions.push(p.dot(&axes[0]) as f32);
positions.push(p.dot(&axes[1]) as f32);
positions.push(p.dot(&axes[2]) as f32);
}
let mut normals = Vec::with_capacity(mesh.normals.len());
for ch in mesh.normals.chunks_exact(3) {
let n = Vector3::new(ch[0] as f64, ch[1] as f64, ch[2] as f64);
normals.push(n.dot(&axes[0]) as f32);
normals.push(n.dot(&axes[1]) as f32);
normals.push(n.dot(&axes[2]) as f32);
}
Mesh {
positions,
normals,
indices: mesh.indices.clone(),
rtc_applied: mesh.rtc_applied,
origin: mesh.origin,
instance_meta: None,
local_bounds: None,
local_to_world: None,
}
}
pub(super) fn mesh_from_frame(mesh: &Mesh, axes: &[Vector3<f64>; 3], center: Vector3<f64>) -> Mesh {
let mut positions = Vec::with_capacity(mesh.positions.len());
for ch in mesh.positions.chunks_exact(3) {
let q = center + axes[0] * ch[0] as f64 + axes[1] * ch[1] as f64 + axes[2] * ch[2] as f64;
positions.push(q.x as f32);
positions.push(q.y as f32);
positions.push(q.z as f32);
}
let mut normals = Vec::with_capacity(mesh.normals.len());
for ch in mesh.normals.chunks_exact(3) {
let m = axes[0] * ch[0] as f64 + axes[1] * ch[1] as f64 + axes[2] * ch[2] as f64;
normals.push(m.x as f32);
normals.push(m.y as f32);
normals.push(m.z as f32);
}
Mesh {
positions,
normals,
indices: mesh.indices.clone(),
rtc_applied: mesh.rtc_applied,
origin: mesh.origin,
instance_meta: None,
local_bounds: None,
local_to_world: None,
}
}
pub(super) fn project_aabb_in_frame(
mesh: &Mesh,
axes: &[Vector3<f64>; 3],
center: Vector3<f64>,
) -> Option<(Point3<f64>, Point3<f64>)> {
let mut lo = [f64::INFINITY; 3];
let mut hi = [f64::NEG_INFINITY; 3];
for ch in mesh.positions.chunks_exact(3) {
let p = Vector3::new(ch[0] as f64, ch[1] as f64, ch[2] as f64) - center;
for k in 0..3 {
let v = p.dot(&axes[k]);
lo[k] = lo[k].min(v);
hi[k] = hi[k].max(v);
}
}
(lo.iter().all(|v| v.is_finite()) && hi.iter().all(|v| v.is_finite()))
.then(|| (Point3::new(lo[0], lo[1], lo[2]), Point3::new(hi[0], hi[1], hi[2])))
}