use nalgebra::Vector3;
use crate::geometry::primitives::Point;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct HomogeneousPoint {
coords: Vector3<f64>,
}
impl HomogeneousPoint {
pub fn new(x: f64, y: f64, w: f64) -> Self {
Self {
coords: Vector3::new(x, y, w),
}
}
pub fn from_euclidean(point: Point) -> Self {
Self::new(point.x(), point.y(), 1.0)
}
pub fn to_euclidean(&self) -> Option<Point> {
if self.is_at_infinity() {
None
} else {
Some(Point::new(
self.coords[0] / self.coords[2],
self.coords[1] / self.coords[2],
))
}
}
pub fn is_at_infinity(&self) -> bool {
self.coords[2].abs() < 1e-10
}
pub fn normalize(&self) -> Self {
if self.is_at_infinity() {
*self
} else {
Self::new(
self.coords[0] / self.coords[2],
self.coords[1] / self.coords[2],
1.0,
)
}
}
pub fn x(&self) -> f64 {
self.coords[0]
}
pub fn y(&self) -> f64 {
self.coords[1]
}
pub fn w(&self) -> f64 {
self.coords[2]
}
pub fn coords(&self) -> &Vector3<f64> {
&self.coords
}
}
#[cfg(test)]
mod tests {
use super::*;
fn approx_eq(a: f64, b: f64) -> bool {
(a - b).abs() < 1e-10
}
#[test]
fn test_new() {
let p = HomogeneousPoint::new(2.0, 3.0, 1.0);
assert_eq!(p.x(), 2.0);
assert_eq!(p.y(), 3.0);
assert_eq!(p.w(), 1.0);
}
#[test]
fn test_from_euclidean() {
let euclidean = Point::new(5.0, 7.0);
let homogeneous = HomogeneousPoint::from_euclidean(euclidean);
assert_eq!(homogeneous.x(), 5.0);
assert_eq!(homogeneous.y(), 7.0);
assert_eq!(homogeneous.w(), 1.0);
}
#[test]
fn test_to_euclidean() {
let h = HomogeneousPoint::new(6.0, 9.0, 3.0);
let euclidean = h.to_euclidean().unwrap();
assert!(approx_eq(euclidean.x(), 2.0));
assert!(approx_eq(euclidean.y(), 3.0));
}
#[test]
fn test_to_euclidean_normalized() {
let h = HomogeneousPoint::new(2.0, 3.0, 1.0);
let euclidean = h.to_euclidean().unwrap();
assert_eq!(euclidean.x(), 2.0);
assert_eq!(euclidean.y(), 3.0);
}
#[test]
fn test_is_at_infinity() {
let finite = HomogeneousPoint::new(1.0, 2.0, 1.0);
assert!(!finite.is_at_infinity());
let infinite = HomogeneousPoint::new(1.0, 0.0, 0.0);
assert!(infinite.is_at_infinity());
let nearly_infinite = HomogeneousPoint::new(1.0, 2.0, 1e-12);
assert!(nearly_infinite.is_at_infinity());
}
#[test]
fn test_to_euclidean_infinity() {
let inf = HomogeneousPoint::new(1.0, 2.0, 0.0);
assert_eq!(inf.to_euclidean(), None);
}
#[test]
fn test_normalize() {
let p = HomogeneousPoint::new(4.0, 6.0, 2.0);
let normalized = p.normalize();
assert!(approx_eq(normalized.x(), 2.0));
assert!(approx_eq(normalized.y(), 3.0));
assert!(approx_eq(normalized.w(), 1.0));
}
#[test]
fn test_normalize_infinity() {
let inf = HomogeneousPoint::new(1.0, 2.0, 0.0);
let normalized = inf.normalize();
assert_eq!(normalized.x(), 1.0);
assert_eq!(normalized.y(), 2.0);
assert_eq!(normalized.w(), 0.0);
}
#[test]
fn test_roundtrip_conversion() {
let original = Point::new(3.5, -2.5);
let homogeneous = HomogeneousPoint::from_euclidean(original);
let recovered = homogeneous.to_euclidean().unwrap();
assert!(approx_eq(recovered.x(), original.x()));
assert!(approx_eq(recovered.y(), original.y()));
}
#[test]
fn test_equivalence_under_scaling() {
let p1 = HomogeneousPoint::new(2.0, 3.0, 1.0);
let p2 = HomogeneousPoint::new(4.0, 6.0, 2.0);
let p3 = HomogeneousPoint::new(6.0, 9.0, 3.0);
let e1 = p1.to_euclidean().unwrap();
let e2 = p2.to_euclidean().unwrap();
let e3 = p3.to_euclidean().unwrap();
assert!(approx_eq(e1.x(), e2.x()));
assert!(approx_eq(e1.y(), e2.y()));
assert!(approx_eq(e2.x(), e3.x()));
assert!(approx_eq(e2.y(), e3.y()));
}
}