use crate::common::PCoords;
use crate::geom3::circle3::Circle3;
use crate::geom3::line3::Line3;
use crate::geom3::plane3::Plane3;
use crate::na::Matrix3;
use crate::{Iso3, Point3, Result, SurfacePoint3, UnitVec3, Vector3};
use std::ops;
mod fitting;
#[derive(Debug, Clone, PartialEq)]
pub struct Sphere3 {
pub center: Point3,
pub radius: f64,
}
impl Sphere3 {
pub fn new(center: &impl PCoords<3>, radius: f64) -> Self {
let center = Point3::from(center.coords());
Self { center, radius }
}
pub fn r(&self) -> f64 {
self.radius
}
pub fn from_4_points(
p0: &impl PCoords<3>,
p1: &impl PCoords<3>,
p2: &impl PCoords<3>,
p3: &impl PCoords<3>,
) -> Result<Self> {
let a = Point3::from(p0.coords());
let b = Point3::from(p1.coords());
let c = Point3::from(p2.coords());
let d = Point3::from(p3.coords());
let m = Matrix3::from_rows(&[
(b - a).transpose(),
(c - a).transpose(),
(d - a).transpose(),
]);
let rhs = 0.5
* Vector3::new(
b.coords.norm_squared() - a.coords.norm_squared(),
c.coords.norm_squared() - a.coords.norm_squared(),
d.coords.norm_squared() - a.coords.norm_squared(),
);
let center = m
.lu()
.solve(&rhs)
.map(Point3::from)
.ok_or("Points are coplanar")?;
let radius = (center - a).norm();
Ok(Self::new(¢er, radius))
}
pub fn from_min_enclosing(points: &[impl PCoords<3>]) -> Result<Self> {
let (center, radius) = crate::common::min_ball::compute_min_ball(points)?;
Ok(Self { center, radius })
}
pub fn transformed_by(&self, iso: &Iso3) -> Self {
Self::new(&(iso * self.center), self.radius)
}
pub fn closest_point(&self, test_point: &impl PCoords<3>) -> Option<SurfacePoint3> {
let v = Point3::from(test_point.coords()) - self.center;
let dist = v.norm();
if dist < 1e-10 {
return None;
}
let normal = UnitVec3::new_normalize(v);
let point = self.center + normal.into_inner() * self.radius;
Some(SurfacePoint3::new(point, normal))
}
pub fn intersect_plane(&self, plane: &Plane3) -> Option<Circle3> {
let dist = plane.signed_distance_to_point(&self.center);
if dist.abs() > self.radius {
return None;
}
let circle_radius = (self.radius * self.radius - dist * dist).sqrt();
let circle_center = self.center - plane.normal.into_inner() * dist;
Some(Circle3::new(circle_center, plane.normal, circle_radius))
}
pub fn intersect_sphere(&self, other: &Sphere3) -> Option<Circle3> {
let axis = other.center - self.center;
let d = axis.norm();
if d < 1e-10 || d > self.radius + other.radius || d < (self.radius - other.radius).abs() {
return None;
}
let h = (d * d + self.radius * self.radius - other.radius * other.radius) / (2.0 * d);
let circle_radius = (self.radius * self.radius - h * h).max(0.0).sqrt();
let normal = UnitVec3::new_normalize(axis);
let circle_center = self.center + normal.into_inner() * h;
Some(Circle3::new(circle_center, normal, circle_radius))
}
pub fn intersect_ray(&self, line: &Line3) -> Option<SurfacePoint3> {
let roots =
solve_sphere_quadratic(&line.origin, &line.direction, &self.center, self.radius);
let t = roots
.into_iter()
.filter(|&t| t >= 0.0)
.fold(f64::MAX, f64::min);
if t == f64::MAX {
return None;
}
let point = line.at(t);
let normal = UnitVec3::new_normalize(point - self.center);
Some(SurfacePoint3::new(point, normal))
}
}
pub(crate) fn solve_sphere_quadratic(
origin: &Point3,
dir: &Vector3,
center: &Point3,
r: f64,
) -> Vec<f64> {
let d = origin - center;
let a = dir.norm_squared();
let b = 2.0 * d.dot(dir);
let c = d.norm_squared() - r * r;
let discriminant = b * b - 4.0 * a * c;
if discriminant < -1e-10 {
vec![]
} else if discriminant.abs() <= 1e-10 {
vec![-b / (2.0 * a)]
} else {
let sq = discriminant.sqrt();
vec![(-b - sq) / (2.0 * a), (-b + sq) / (2.0 * a)]
}
}
impl ops::Mul<Sphere3> for Iso3 {
type Output = Sphere3;
fn mul(self, rhs: Sphere3) -> Sphere3 {
rhs.transformed_by(&self)
}
}
impl ops::Mul<&Sphere3> for Iso3 {
type Output = Sphere3;
fn mul(self, rhs: &Sphere3) -> Sphere3 {
rhs.transformed_by(&self)
}
}
impl ops::Mul<Sphere3> for &Iso3 {
type Output = Sphere3;
fn mul(self, rhs: Sphere3) -> Sphere3 {
rhs.transformed_by(self)
}
}
impl ops::Mul<&Sphere3> for &Iso3 {
type Output = Sphere3;
fn mul(self, rhs: &Sphere3) -> Sphere3 {
rhs.transformed_by(self)
}
}
#[cfg(test)]
mod tests {
use super::*;
use approx::assert_relative_eq;
fn unit_sphere() -> Sphere3 {
Sphere3::new(&Point3::origin(), 1.0)
}
fn offset_sphere() -> Sphere3 {
Sphere3::new(&Point3::new(1.0, 2.0, 3.0), 2.0)
}
#[test]
fn min_enclosing_mixed_boundary_and_interior() {
let points = [
Point3::new(-2.0, 1.0, 1.0),
Point3::new(4.0, 1.0, 1.0),
Point3::new(1.0, 2.0, 1.0),
Point3::new(1.0, 1.0, 0.0),
Point3::new(2.0, 1.5, 1.5),
];
let sphere = Sphere3::from_min_enclosing(&points).unwrap();
assert_relative_eq!(sphere.center, Point3::new(1.0, 1.0, 1.0), epsilon = 1e-12);
assert_relative_eq!(sphere.radius, 3.0, epsilon = 1e-12);
}
#[test]
fn transform_by_moves_center() {
let sphere = Sphere3::new(&Point3::origin(), 2.0);
let iso = Iso3::translation(1.0, 2.0, 3.0);
let moved = sphere.transformed_by(&iso);
assert_relative_eq!(moved.center, Point3::new(1.0, 2.0, 3.0), epsilon = 1e-12);
assert_relative_eq!(moved.r(), 2.0, epsilon = 1e-12);
}
#[test]
fn closest_point_outside_is_on_surface() {
let sphere = unit_sphere();
let test = Point3::new(3.0, 0.0, 0.0);
let sp = sphere.closest_point(&test).unwrap();
assert_relative_eq!(sp.point, Point3::new(1.0, 0.0, 0.0), epsilon = 1e-12);
assert_relative_eq!(sp.normal.into_inner(), crate::Vector3::x(), epsilon = 1e-12);
}
#[test]
fn closest_point_inside_is_on_surface() {
let sphere = unit_sphere();
let test = Point3::new(0.5, 0.0, 0.0);
let sp = sphere.closest_point(&test).unwrap();
assert_relative_eq!(
(sp.point - sphere.center).norm(),
sphere.r(),
epsilon = 1e-12
);
assert_relative_eq!(sp.point, Point3::new(1.0, 0.0, 0.0), epsilon = 1e-12);
}
#[test]
fn closest_point_offset_sphere() {
let sphere = offset_sphere();
let test = Point3::new(1.0, 2.0, 10.0);
let sp = sphere.closest_point(&test).unwrap();
assert_relative_eq!(sp.point, Point3::new(1.0, 2.0, 5.0), epsilon = 1e-12);
assert_relative_eq!(sp.normal.into_inner(), crate::Vector3::z(), epsilon = 1e-12);
}
#[test]
fn closest_point_at_center_returns_none() {
let sphere = unit_sphere();
assert!(sphere.closest_point(&Point3::origin()).is_none());
}
#[test]
fn plane_through_center_gives_great_circle() {
let sphere = unit_sphere();
let plane = Plane3::xy(); let circle = sphere.intersect_plane(&plane).unwrap();
assert_relative_eq!(circle.r(), 1.0, epsilon = 1e-12);
assert_relative_eq!(circle.center, Point3::origin(), epsilon = 1e-12);
assert_relative_eq!(circle.normal.into_inner(), Vector3::z(), epsilon = 1e-12);
}
#[test]
fn plane_tangent_gives_point_circle() {
let sphere = unit_sphere();
let plane = Plane3::new(Vector3::z_axis(), 1.0);
let circle = sphere.intersect_plane(&plane).unwrap();
assert_relative_eq!(circle.r(), 0.0, epsilon = 1e-12);
assert_relative_eq!(circle.center, Point3::new(0.0, 0.0, 1.0), epsilon = 1e-12);
}
#[test]
fn plane_misses_sphere_returns_none() {
let sphere = unit_sphere();
let plane = Plane3::new(Vector3::z_axis(), 2.0); let result = sphere.intersect_plane(&plane);
assert!(result.is_none());
}
#[test]
fn plane_intersection_circle_radius_correct() {
let sphere = Sphere3::new(&Point3::origin(), 5.0);
let plane = Plane3::new(Vector3::z_axis(), 3.0);
let circle = sphere.intersect_plane(&plane).unwrap();
assert_relative_eq!(circle.r(), 4.0, epsilon = 1e-12);
}
#[test]
fn plane_intersection_circle_center_on_plane() {
let sphere = offset_sphere(); let plane = Plane3::new(Vector3::z_axis(), 3.0); let circle = sphere.intersect_plane(&plane).unwrap();
assert_relative_eq!(circle.center, Point3::new(1.0, 2.0, 3.0), epsilon = 1e-12);
assert_relative_eq!(circle.r(), 2.0, epsilon = 1e-12);
}
#[test]
fn plane_intersection_circle_normal_matches_plane() {
let sphere = unit_sphere();
let normal = UnitVec3::new_normalize(Vector3::new(1.0, 1.0, 0.0));
let plane = Plane3::new(normal, 0.0);
let circle = sphere.intersect_plane(&plane).unwrap();
assert_relative_eq!(
circle.normal.into_inner(),
normal.into_inner(),
epsilon = 1e-12
);
}
#[test]
fn two_unit_spheres_adjacent_gives_great_circle() {
let s1 = Sphere3::new(&Point3::new(-1.0, 0.0, 0.0), 1.0);
let s2 = Sphere3::new(&Point3::new(1.0, 0.0, 0.0), 1.0);
let circle = s1.intersect_sphere(&s2).unwrap();
assert_relative_eq!(circle.r(), 0.0, epsilon = 1e-12);
assert_relative_eq!(circle.center, Point3::origin(), epsilon = 1e-12);
}
#[test]
fn equal_spheres_offset_along_x() {
let s1 = Sphere3::new(&Point3::new(-3.0, 0.0, 0.0), 5.0);
let s2 = Sphere3::new(&Point3::new(3.0, 0.0, 0.0), 5.0);
let circle = s1.intersect_sphere(&s2).unwrap();
assert_relative_eq!(circle.r(), 4.0, epsilon = 1e-12);
assert_relative_eq!(circle.center, Point3::origin(), epsilon = 1e-12);
assert_relative_eq!(circle.normal.into_inner().x.abs(), 1.0, epsilon = 1e-12);
}
#[test]
fn unequal_spheres_circle_center_correct() {
let s1 = Sphere3::new(&Point3::origin(), 3.0);
let s2 = Sphere3::new(&Point3::new(5.0, 0.0, 0.0), 4.0);
let circle = s1.intersect_sphere(&s2).unwrap();
assert_relative_eq!(circle.r(), 2.4, epsilon = 1e-10);
assert_relative_eq!(circle.center, Point3::new(1.8, 0.0, 0.0), epsilon = 1e-10);
}
#[test]
fn spheres_too_far_apart_returns_none() {
let s1 = Sphere3::new(&Point3::origin(), 1.0);
let s2 = Sphere3::new(&Point3::new(10.0, 0.0, 0.0), 1.0);
assert!(s1.intersect_sphere(&s2).is_none());
}
#[test]
fn sphere_inside_other_returns_none() {
let s1 = Sphere3::new(&Point3::origin(), 5.0);
let s2 = Sphere3::new(&Point3::new(1.0, 0.0, 0.0), 1.0);
assert!(s1.intersect_sphere(&s2).is_none());
}
#[test]
fn concentric_spheres_returns_none() {
let s1 = Sphere3::new(&Point3::origin(), 1.0);
let s2 = Sphere3::new(&Point3::origin(), 2.0);
assert!(s1.intersect_sphere(&s2).is_none());
}
#[test]
fn intersection_circle_lies_on_both_spheres() {
let s1 = Sphere3::new(&Point3::new(0.0, 0.0, 0.0), 5.0);
let s2 = Sphere3::new(&Point3::new(4.0, 0.0, 0.0), 4.0);
let circle = s1.intersect_sphere(&s2).unwrap();
let n = circle.normal.into_inner();
let reference = if n.z.abs() < 0.9 {
Vector3::z()
} else {
Vector3::x()
};
let x_axis = reference.cross(&n).normalize();
let y_axis = n.cross(&x_axis);
for t in [0.0_f64, 1.0, 2.0, 3.0, 4.0, 5.0] {
let pt =
circle.center + x_axis * (circle.r() * t.cos()) + y_axis * (circle.r() * t.sin());
assert_relative_eq!((pt - s1.center).norm(), s1.r(), epsilon = 1e-10);
assert_relative_eq!((pt - s2.center).norm(), s2.r(), epsilon = 1e-10);
}
}
#[test]
fn ray_hits_front_of_unit_sphere() {
let sphere = unit_sphere();
let line = Line3::new(Point3::new(5.0, 0.0, 0.0), (-Vector3::x()).normalize());
let sp = sphere.intersect_ray(&line).unwrap();
assert_relative_eq!(sp.point, Point3::new(1.0, 0.0, 0.0), epsilon = 1e-12);
assert_relative_eq!(sp.normal.into_inner(), Vector3::x(), epsilon = 1e-12);
}
#[test]
fn ray_misses_returns_none() {
let sphere = unit_sphere();
let line = Line3::new(Point3::new(5.0, 5.0, 0.0), (-Vector3::x()).normalize());
assert!(sphere.intersect_ray(&line).is_none());
}
#[test]
fn intersect_ray_point_is_on_surface() {
let sphere = offset_sphere();
let origin = Point3::new(10.0, 2.0, 3.0);
let dir = (sphere.center - origin).normalize();
let line = Line3::new(origin, dir);
let sp = sphere.intersect_ray(&line).unwrap();
let dist = (sp.point - sphere.center).norm();
assert_relative_eq!(dist, sphere.r(), epsilon = 1e-12);
}
#[test]
fn intersect_ray_normal_points_outward() {
let sphere = unit_sphere();
let line = Line3::new(Point3::new(0.0, 5.0, 0.0), (-Vector3::y()).normalize());
let sp = sphere.intersect_ray(&line).unwrap();
assert_relative_eq!(sp.normal.into_inner(), Vector3::y(), epsilon = 1e-12);
}
#[test]
fn from_4_points_matches_known_sphere() {
let sphere = offset_sphere(); let p0 = sphere.center + Vector3::new(2.0, 0.0, 0.0);
let p1 = sphere.center + Vector3::new(-2.0, 0.0, 0.0);
let p2 = sphere.center + Vector3::new(0.0, 2.0, 0.0);
let p3 = sphere.center + Vector3::new(0.0, 0.0, 2.0);
let result = Sphere3::from_4_points(&p0, &p1, &p2, &p3).unwrap();
assert_relative_eq!(result.center, sphere.center, epsilon = 1e-10);
assert_relative_eq!(result.r(), sphere.r(), epsilon = 1e-10);
}
#[test]
fn from_4_points_general_position() {
let center = Point3::new(0.3, -1.1, 2.4);
let radius = 3.7;
let dirs = [
Vector3::new(1.0, 0.3, -0.2).normalize(),
Vector3::new(-0.5, 1.0, 0.1).normalize(),
Vector3::new(0.2, -0.7, 1.0).normalize(),
Vector3::new(-1.0, -0.4, -0.6).normalize(),
];
let pts: Vec<Point3> = dirs.iter().map(|d| center + d * radius).collect();
let result = Sphere3::from_4_points(&pts[0], &pts[1], &pts[2], &pts[3]).unwrap();
assert_relative_eq!(result.center, center, epsilon = 1e-8);
assert_relative_eq!(result.r(), radius, epsilon = 1e-8);
}
#[test]
fn from_4_coplanar_points_returns_error() {
let p0 = Point3::new(0.0, 0.0, 0.0);
let p1 = Point3::new(1.0, 0.0, 0.0);
let p2 = Point3::new(0.0, 1.0, 0.0);
let p3 = Point3::new(1.0, 1.0, 0.0);
assert!(Sphere3::from_4_points(&p0, &p1, &p2, &p3).is_err());
}
#[test]
fn ray_from_inside_hits_back_surface() {
let sphere = unit_sphere();
let line = Line3::new(Point3::new(0.0, 0.0, 0.0), Vector3::x());
let sp = sphere.intersect_ray(&line).unwrap();
assert_relative_eq!(sp.point, Point3::new(1.0, 0.0, 0.0), epsilon = 1e-12);
}
}