use super::coo3d::*;
use crate::proj::SMALLER_EDGE2OPEDGE_DIST;
static R_MAX: f64 = SMALLER_EDGE2OPEDGE_DIST[0];
#[derive(Debug)]
pub struct Cone {
center: UnitVect3,
radius: f64, }
impl Cone {
pub fn new(center: UnitVect3, radius: f64) -> Cone {
Cone { center, radius }
}
#[allow(dead_code)]
pub fn mec<T: Vec3 + UnitVec3>(points: &[T]) -> Option<Cone> {
match points.len() {
0 | 1 => None,
2 => {
let cone = mec_2(&points[0], &points[1]);
if cone.radius > R_MAX {
None
} else {
Some(cone)
}
}
3 => {
let cone = mec_3(&points[0], &points[1], &points[2]);
if cone.radius > R_MAX {
None
} else {
Some(cone)
}
}
_ => mec_n(points),
}
}
pub fn bounding_cone<T: UnitVec3>(points: &[T]) -> Cone {
let (x, y, z) = points.iter().fold((0.0, 0.0, 0.0), |(x, y, z), p| {
(x + p.x(), y + p.y(), z + p.z())
});
let norm = (pow2(x) + pow2(y) + pow2(z)).sqrt();
let center = UnitVect3::new(x / norm, y / norm, z / norm);
let d2_max = points
.iter()
.map(|p| center.squared_euclidean_dist(p))
.fold(0.0, f64::max);
Cone::new(center, 2.0 * (0.5 * d2_max.sqrt()).asin())
}
pub fn radius(&self) -> f64 {
self.radius
}
pub fn center(&self) -> &UnitVect3 {
&self.center
}
pub fn contains<T: Vec3 + UnitVec3>(&self, point: &T) -> bool {
self.center.ang_dist(point) <= self.radius
}
}
#[allow(dead_code)]
fn mec_2<T: Vec3 + UnitVec3>(a: &T, b: &T) -> Cone {
let radius = 0.5 * a.ang_dist(b);
let center = a.arc_center(b);
Cone::new(center, radius)
}
pub(crate) fn mec_3<T: Vec3 + UnitVec3>(a: &T, b: &T, c: &T) -> Cone {
let da = b.ang_dist(c);
let db = a.ang_dist(c);
let dc = a.ang_dist(b);
let (mut center, mut radius) = if da > db && da > dc {
(b.arc_center(c), 0.5 * da)
} else if db > dc {
(a.arc_center(c), 0.5 * db)
} else {
(a.arc_center(b), 0.5 * dc)
};
if c.ang_dist(¢er) > radius {
radius = circum_radius(da, db, dc);
center = circum_center(a, b, c, radius);
}
Cone::new(center, radius)
}
#[allow(dead_code)]
fn mec_n<T: Vec3 + UnitVec3>(points: &[T]) -> Option<Cone> {
let points: Vec<&T> = points.iter().collect();
let mut cone = mec_2(points[0], points[1]);
if cone.radius > R_MAX {
return None;
}
for i in 2..points.len() {
if !cone.contains(points[i]) {
cone = mec_2(points[0], points[i]);
if cone.radius > R_MAX {
return None;
}
for j in 1..i {
if !cone.contains(points[j]) {
cone = mec_2(points[j], points[i]);
if cone.radius > R_MAX {
return None;
}
for k in 0..j {
if !cone.contains(points[k]) {
cone = mec_3(points[k], points[j], points[i]);
if cone.radius > R_MAX {
return None;
}
}
}
}
}
}
}
Some(cone)
}
#[inline]
fn circum_radius(a: f64, b: f64, c: f64) -> f64 {
let sin_half_a = (0.5 * a).sin();
let sin_half_b = (0.5 * b).sin();
let sin_half_c = (0.5 * c).sin();
let n = pow2(sin_half_a * sin_half_b * sin_half_c);
let d = 0.25
* (sin_half_a + sin_half_b + sin_half_c)
* (sin_half_a + sin_half_b - sin_half_c)
* (sin_half_a - sin_half_b + sin_half_c)
* (sin_half_b + sin_half_c - sin_half_a);
(n / d).sqrt().asin()
}
fn circum_center<T: Vec3 + UnitVec3>(a: &T, b: &T, c: &T, radius: f64) -> UnitVect3 {
let e = 1.0 - 0.5 * pow2(radius);
let d = a.x() * (b.y() * c.z() - c.y() * b.z()) - b.x() * (a.y() * c.z() - c.y() * a.z())
+ c.x() * (a.y() * b.z() - b.y() * a.z());
let x = b.y() * c.z() - c.y() * b.z() - a.y() * c.z() + c.y() * a.z() + a.y() * b.z()
- b.y() * a.z();
let y = a.x() * (c.z() - b.z()) - b.x() * (c.z() - a.z()) + c.x() * (b.z() - a.z());
let z = a.x() * (b.y() - c.y()) - b.x() * (a.y() - c.y()) + c.x() * (a.y() - b.y());
UnitVect3::new((e * x) / d, (e * y) / d, (e * z) / d)
}
#[inline]
fn pow2(x: f64) -> f64 {
x * x
}