use crate::MathError;
use crate::vec::{Point2, Vec2};
#[derive(Debug, Clone)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Line2D {
origin: Point2,
direction: Vec2,
}
impl Line2D {
pub fn new(origin: Point2, direction: Vec2) -> Result<Self, MathError> {
let len = direction.length();
if len < 1e-15 {
return Err(MathError::ZeroVector);
}
Ok(Self {
origin,
direction: Vec2::new(direction.x() / len, direction.y() / len),
})
}
#[must_use]
pub fn evaluate(&self, t: f64) -> Point2 {
self.origin + self.direction * t
}
#[must_use]
pub const fn tangent(&self, _t: f64) -> Vec2 {
self.direction
}
#[must_use]
pub fn project(&self, point: Point2) -> f64 {
let v = point - self.origin;
v.dot(self.direction) / self.direction.length_squared()
}
#[must_use]
pub fn distance_to_point(&self, point: Point2) -> f64 {
let t = self.project(point);
let closest = self.evaluate(t);
let diff = point - closest;
diff.length()
}
#[must_use]
pub const fn origin(&self) -> Point2 {
self.origin
}
#[must_use]
pub const fn direction(&self) -> Vec2 {
self.direction
}
}
#[derive(Debug, Clone)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Circle2D {
center: Point2,
radius: f64,
}
impl Circle2D {
pub fn new(center: Point2, radius: f64) -> Result<Self, MathError> {
if radius <= 0.0 {
return Err(MathError::ParameterOutOfRange {
value: radius,
min: f64::EPSILON,
max: f64::MAX,
});
}
Ok(Self { center, radius })
}
#[must_use]
pub fn evaluate(&self, t: f64) -> Point2 {
self.center + Vec2::new(self.radius * t.cos(), self.radius * t.sin())
}
#[must_use]
pub fn tangent(&self, t: f64) -> Vec2 {
Vec2::new(-self.radius * t.sin(), self.radius * t.cos())
}
#[must_use]
pub fn circumference(&self) -> f64 {
2.0 * std::f64::consts::PI * self.radius
}
#[must_use]
pub fn project(&self, point: Point2) -> f64 {
let v = point - self.center;
v.y().atan2(v.x()).rem_euclid(2.0 * std::f64::consts::PI)
}
#[must_use]
pub const fn center(&self) -> Point2 {
self.center
}
#[must_use]
pub const fn radius(&self) -> f64 {
self.radius
}
}
#[derive(Debug, Clone)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Ellipse2D {
center: Point2,
semi_major: f64,
semi_minor: f64,
rotation: f64,
}
impl Ellipse2D {
pub fn new(
center: Point2,
semi_major: f64,
semi_minor: f64,
rotation: f64,
) -> Result<Self, MathError> {
if semi_major <= 0.0 {
return Err(MathError::ParameterOutOfRange {
value: semi_major,
min: f64::EPSILON,
max: f64::MAX,
});
}
if semi_minor <= 0.0 {
return Err(MathError::ParameterOutOfRange {
value: semi_minor,
min: f64::EPSILON,
max: f64::MAX,
});
}
if semi_minor > semi_major {
return Err(MathError::ParameterOutOfRange {
value: semi_minor,
min: 0.0,
max: semi_major,
});
}
Ok(Self {
center,
semi_major,
semi_minor,
rotation,
})
}
#[must_use]
pub fn evaluate(&self, t: f64) -> Point2 {
let (sin_r, cos_r) = self.rotation.sin_cos();
let x = self.semi_major * t.cos();
let y = self.semi_minor * t.sin();
self.center + Vec2::new(x.mul_add(cos_r, -(y * sin_r)), x.mul_add(sin_r, y * cos_r))
}
#[must_use]
pub fn tangent(&self, t: f64) -> Vec2 {
let (sin_r, cos_r) = self.rotation.sin_cos();
let dx = -self.semi_major * t.sin();
let dy = self.semi_minor * t.cos();
Vec2::new(
dx.mul_add(cos_r, -(dy * sin_r)),
dx.mul_add(sin_r, dy * cos_r),
)
}
#[must_use]
pub const fn center(&self) -> Point2 {
self.center
}
#[must_use]
pub const fn semi_major(&self) -> f64 {
self.semi_major
}
#[must_use]
pub const fn semi_minor(&self) -> f64 {
self.semi_minor
}
#[must_use]
pub const fn rotation(&self) -> f64 {
self.rotation
}
#[must_use]
pub fn approximate_circumference(&self) -> f64 {
let a = self.semi_major;
let b = self.semi_minor;
let h = ((a - b) * (a - b)) / ((a + b) * (a + b));
std::f64::consts::PI * (a + b) * (1.0 + 3.0 * h / (10.0 + 3.0f64.mul_add(-h, 4.0).sqrt()))
}
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct NurbsCurve2D {
degree: usize,
knots: Vec<f64>,
control_points: Vec<Point2>,
weights: Vec<f64>,
}
impl NurbsCurve2D {
pub fn new(
degree: usize,
knots: Vec<f64>,
control_points: Vec<Point2>,
weights: Vec<f64>,
) -> Result<Self, MathError> {
let n = control_points.len();
let expected_knots = n + degree + 1;
if knots.len() != expected_knots {
return Err(MathError::InvalidKnotVector {
expected: expected_knots,
got: knots.len(),
});
}
if weights.len() != n {
return Err(MathError::InvalidWeights {
expected: n,
got: weights.len(),
});
}
if n == 0 {
return Err(MathError::EmptyInput);
}
Ok(Self {
degree,
knots,
control_points,
weights,
})
}
#[must_use]
pub fn evaluate(&self, u: f64) -> Point2 {
let n = self.control_points.len();
let p = self.degree;
let span = crate::nurbs::basis::find_span(n, p, u, &self.knots);
let mut basis = [0.0f64; crate::nurbs::basis::MAX_STACK_OUTPUT + 1];
crate::nurbs::basis::basis_funs_into(span, u, p, &self.knots, &mut basis[..=p]);
let mut wx = 0.0;
let mut wy = 0.0;
let mut ww = 0.0;
for (j, &basis_val) in basis.iter().enumerate().take(p + 1) {
let idx = span - p + j;
let cp = &self.control_points[idx];
let w = self.weights[idx];
let bw = basis_val * w;
wx += bw * cp.x();
wy += bw * cp.y();
ww += bw;
}
if ww.abs() < f64::EPSILON {
return self.control_points[0];
}
Point2::new(wx / ww, wy / ww)
}
#[must_use]
#[allow(clippy::many_single_char_names)]
pub fn tangent(&self, param: f64) -> Vec2 {
let num_pts = self.control_points.len();
let deg = self.degree;
let span = crate::nurbs::basis::find_span(num_pts, deg, param, &self.knots);
let stride = deg + 1;
let mut ders_buf = [0.0f64; 2 * (crate::nurbs::basis::MAX_STACK_OUTPUT + 1)];
crate::nurbs::basis::ders_basis_funs_into(
span,
param,
deg,
1,
&self.knots,
&mut ders_buf[..2 * stride],
);
let mut curve_pt = Vec2::new(0.0, 0.0);
let mut curve_deriv = Vec2::new(0.0, 0.0);
let mut weight_sum = 0.0;
let mut weight_deriv = 0.0;
for j in 0..=deg {
let basis_val = ders_buf[j];
let basis_deriv = ders_buf[stride + j];
let idx = span - deg + j;
let wi = self.weights[idx];
let cp = &self.control_points[idx];
let cp_vec = Vec2::new(cp.x(), cp.y());
curve_pt += cp_vec * (wi * basis_val);
curve_deriv += cp_vec * (wi * basis_deriv);
weight_sum += wi * basis_val;
weight_deriv += wi * basis_deriv;
}
if weight_sum.abs() < f64::EPSILON {
return Vec2::new(0.0, 0.0);
}
(curve_deriv * weight_sum - curve_pt * weight_deriv) * (1.0 / (weight_sum * weight_sum))
}
#[must_use]
pub fn domain(&self) -> (f64, f64) {
(
self.knots[self.degree],
self.knots[self.knots.len() - self.degree - 1],
)
}
#[must_use]
pub const fn degree(&self) -> usize {
self.degree
}
#[must_use]
pub fn knots(&self) -> &[f64] {
&self.knots
}
#[must_use]
pub fn control_points(&self) -> &[Point2] {
&self.control_points
}
#[must_use]
pub fn weights(&self) -> &[f64] {
&self.weights
}
#[must_use]
pub fn is_rational(&self) -> bool {
self.weights.iter().any(|w| (*w - 1.0).abs() > f64::EPSILON)
}
pub fn from_line(start: Point2, end: Point2) -> Result<Self, MathError> {
Self::new(
1,
vec![0.0, 0.0, 1.0, 1.0],
vec![start, end],
vec![1.0, 1.0],
)
}
}
#[derive(Debug, Clone)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum Curve2D {
Line(Line2D),
Circle(Circle2D),
Ellipse(Ellipse2D),
Nurbs(NurbsCurve2D),
}
impl Curve2D {
#[must_use]
pub fn evaluate(&self, t: f64) -> Point2 {
match self {
Self::Line(c) => c.evaluate(t),
Self::Circle(c) => c.evaluate(t),
Self::Ellipse(c) => c.evaluate(t),
Self::Nurbs(c) => c.evaluate(t),
}
}
#[must_use]
pub fn tangent(&self, t: f64) -> Vec2 {
match self {
Self::Line(c) => c.tangent(t),
Self::Circle(c) => c.tangent(t),
Self::Ellipse(c) => c.tangent(t),
Self::Nurbs(c) => c.tangent(t),
}
}
}
#[cfg(test)]
#[allow(clippy::unwrap_used, clippy::expect_used)]
mod tests {
use super::*;
use std::f64::consts::PI;
const TOL: f64 = 1e-10;
fn approx_eq(a: f64, b: f64) -> bool {
(a - b).abs() < TOL
}
fn point2_approx_eq(a: Point2, b: Point2) -> bool {
approx_eq(a.x(), b.x()) && approx_eq(a.y(), b.y())
}
#[test]
fn line2d_evaluate() {
let line = Line2D::new(Point2::new(1.0, 2.0), Vec2::new(3.0, 4.0)).expect("valid line");
let p = line.evaluate(0.0);
assert!(point2_approx_eq(p, Point2::new(1.0, 2.0)));
let p = line.evaluate(5.0);
assert!(point2_approx_eq(p, Point2::new(4.0, 6.0)));
let p = line.evaluate(2.5);
assert!(point2_approx_eq(p, Point2::new(2.5, 4.0)));
}
#[test]
fn line2d_project() {
let line = Line2D::new(Point2::new(0.0, 0.0), Vec2::new(1.0, 0.0)).expect("valid line");
assert!(approx_eq(line.project(Point2::new(5.0, 0.0)), 5.0));
assert!(approx_eq(line.project(Point2::new(3.0, 7.0)), 3.0));
}
#[test]
fn line2d_distance() {
let line = Line2D::new(Point2::new(0.0, 0.0), Vec2::new(1.0, 0.0)).expect("valid line");
assert!(approx_eq(
line.distance_to_point(Point2::new(3.0, 4.0)),
4.0
));
}
#[test]
fn line2d_zero_direction_error() {
let result = Line2D::new(Point2::new(0.0, 0.0), Vec2::new(0.0, 0.0));
assert!(result.is_err());
}
#[test]
fn circle2d_evaluate_at_zero() {
let circle = Circle2D::new(Point2::new(0.0, 0.0), 1.0).expect("valid circle");
let p = circle.evaluate(0.0);
assert!(point2_approx_eq(p, Point2::new(1.0, 0.0)));
}
#[test]
fn circle2d_evaluate_quarter() {
let circle = Circle2D::new(Point2::new(0.0, 0.0), 2.0).expect("valid circle");
let p = circle.evaluate(PI / 2.0);
assert!(point2_approx_eq(p, Point2::new(0.0, 2.0)));
}
#[test]
fn circle2d_circumference() {
let circle = Circle2D::new(Point2::new(0.0, 0.0), 3.0).expect("valid circle");
assert!(approx_eq(circle.circumference(), 6.0 * PI));
}
#[test]
fn circle2d_project_roundtrip() {
let circle = Circle2D::new(Point2::new(1.0, 1.0), 5.0).expect("valid circle");
let t = 1.234;
let point = circle.evaluate(t);
let t_proj = circle.project(point);
assert!(approx_eq(t, t_proj));
}
#[test]
fn circle2d_zero_radius_error() {
let result = Circle2D::new(Point2::new(0.0, 0.0), 0.0);
assert!(result.is_err());
}
#[test]
fn ellipse2d_evaluate_no_rotation() {
let ellipse = Ellipse2D::new(Point2::new(0.0, 0.0), 3.0, 2.0, 0.0).expect("valid ellipse");
let p = ellipse.evaluate(0.0);
assert!(point2_approx_eq(p, Point2::new(3.0, 0.0)));
let p = ellipse.evaluate(PI / 2.0);
assert!(point2_approx_eq(p, Point2::new(0.0, 2.0)));
}
#[test]
fn ellipse2d_evaluate_with_rotation() {
let ellipse =
Ellipse2D::new(Point2::new(0.0, 0.0), 3.0, 2.0, PI / 2.0).expect("valid ellipse");
let p = ellipse.evaluate(0.0);
assert!(point2_approx_eq(p, Point2::new(0.0, 3.0)));
}
#[test]
fn ellipse2d_circle_circumference() {
let ellipse = Ellipse2D::new(Point2::new(0.0, 0.0), 5.0, 5.0, 0.0).expect("valid ellipse");
assert!(approx_eq(
ellipse.approximate_circumference(),
2.0 * PI * 5.0
));
}
#[test]
fn ellipse2d_zero_axis_error() {
assert!(Ellipse2D::new(Point2::new(0.0, 0.0), 0.0, 1.0, 0.0).is_err());
assert!(Ellipse2D::new(Point2::new(0.0, 0.0), 1.0, 0.0, 0.0).is_err());
}
#[test]
fn nurbs2d_line_segment() {
let curve =
NurbsCurve2D::from_line(Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)).expect("valid");
assert!(point2_approx_eq(curve.evaluate(0.0), Point2::new(0.0, 0.0)));
assert!(point2_approx_eq(curve.evaluate(1.0), Point2::new(1.0, 1.0)));
assert!(point2_approx_eq(curve.evaluate(0.5), Point2::new(0.5, 0.5)));
}
#[test]
fn nurbs2d_quadratic_bezier() {
let curve = NurbsCurve2D::new(
2,
vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0],
vec![
Point2::new(0.0, 0.0),
Point2::new(0.5, 1.0),
Point2::new(1.0, 0.0),
],
vec![1.0, 1.0, 1.0],
)
.expect("valid curve");
assert!(point2_approx_eq(curve.evaluate(0.0), Point2::new(0.0, 0.0)));
assert!(point2_approx_eq(curve.evaluate(1.0), Point2::new(1.0, 0.0)));
assert!(point2_approx_eq(curve.evaluate(0.5), Point2::new(0.5, 0.5)));
}
#[test]
fn nurbs2d_tangent() {
let curve =
NurbsCurve2D::from_line(Point2::new(0.0, 0.0), Point2::new(2.0, 3.0)).expect("valid");
let tangent = curve.tangent(0.5);
assert!(approx_eq(tangent.x(), 2.0));
assert!(approx_eq(tangent.y(), 3.0));
}
#[test]
fn nurbs2d_domain() {
let curve = NurbsCurve2D::new(
2,
vec![0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0],
vec![
Point2::new(0.0, 0.0),
Point2::new(0.25, 1.0),
Point2::new(0.75, 1.0),
Point2::new(1.0, 0.0),
],
vec![1.0, 1.0, 1.0, 1.0],
)
.expect("valid curve");
let (u_min, u_max) = curve.domain();
assert!(approx_eq(u_min, 0.0));
assert!(approx_eq(u_max, 1.0));
}
#[test]
fn nurbs2d_invalid_knots() {
let result = NurbsCurve2D::new(
1,
vec![0.0, 1.0],
vec![Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)],
vec![1.0, 1.0],
);
assert!(result.is_err());
}
#[test]
fn nurbs2d_invalid_weights() {
let result = NurbsCurve2D::new(
1,
vec![0.0, 0.0, 1.0, 1.0],
vec![Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)],
vec![1.0],
);
assert!(result.is_err());
}
#[test]
fn nurbs2d_is_rational() {
let non_rational =
NurbsCurve2D::from_line(Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)).expect("valid");
assert!(!non_rational.is_rational());
let rational = NurbsCurve2D::new(
1,
vec![0.0, 0.0, 1.0, 1.0],
vec![Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)],
vec![1.0, 2.0],
)
.expect("valid");
assert!(rational.is_rational());
}
#[test]
fn curve2d_evaluate_dispatch() {
let line =
Curve2D::Line(Line2D::new(Point2::new(0.0, 0.0), Vec2::new(1.0, 0.0)).expect("valid"));
assert!(point2_approx_eq(line.evaluate(5.0), Point2::new(5.0, 0.0)));
let circle = Curve2D::Circle(Circle2D::new(Point2::new(0.0, 0.0), 1.0).expect("valid"));
assert!(point2_approx_eq(
circle.evaluate(0.0),
Point2::new(1.0, 0.0)
));
}
#[test]
fn ellipse2d_tangent_no_rotation() {
let ellipse = Ellipse2D::new(Point2::new(0.0, 0.0), 3.0, 2.0, 0.0).expect("valid ellipse");
let tang = ellipse.tangent(0.0);
assert!(approx_eq(tang.x(), 0.0));
assert!(approx_eq(tang.y(), 2.0));
}
#[test]
fn ellipse2d_tangent_at_quarter() {
let ellipse = Ellipse2D::new(Point2::new(0.0, 0.0), 3.0, 2.0, 0.0).expect("valid ellipse");
let tang = ellipse.tangent(PI / 2.0);
assert!(approx_eq(tang.x(), -3.0));
assert!(approx_eq(tang.y(), 0.0));
}
#[test]
fn ellipse2d_minor_exceeds_major_error() {
assert!(Ellipse2D::new(Point2::new(0.0, 0.0), 2.0, 5.0, 0.0).is_err());
}
#[test]
fn circle2d_tangent_at_zero() {
let circle = Circle2D::new(Point2::new(0.0, 0.0), 3.0).expect("valid circle");
let tang = circle.tangent(0.0);
assert!(approx_eq(tang.x(), 0.0));
assert!(approx_eq(tang.y(), 3.0));
}
#[test]
fn circle2d_tangent_at_quarter() {
let circle = Circle2D::new(Point2::new(0.0, 0.0), 2.0).expect("valid circle");
let tang = circle.tangent(PI / 2.0);
assert!(approx_eq(tang.x(), -2.0));
assert!(approx_eq(tang.y(), 0.0));
}
#[test]
fn circle2d_tangent_perpendicular_to_radius() {
let circle = Circle2D::new(Point2::new(1.0, 2.0), 4.0).expect("valid circle");
for i in 0..8 {
let t = f64::from(i) * PI / 4.0;
let pt = circle.evaluate(t);
let tang = circle.tangent(t);
let radius_vec_x = pt.x() - circle.center().x();
let radius_vec_y = pt.y() - circle.center().y();
let dot = radius_vec_x * tang.x() + radius_vec_y * tang.y();
assert!(
approx_eq(dot, 0.0),
"tangent not perpendicular to radius at t={t}: dot={dot}"
);
}
}
#[test]
fn nurbs2d_tangent_quadratic_bezier() {
let curve = NurbsCurve2D::new(
2,
vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0],
vec![
Point2::new(0.0, 0.0),
Point2::new(0.5, 1.0),
Point2::new(1.0, 0.0),
],
vec![1.0, 1.0, 1.0],
)
.expect("valid curve");
let tang_start = curve.tangent(0.0);
assert!(
(tang_start.x() - 1.0).abs() < 1e-8,
"start tangent x: {}",
tang_start.x()
);
assert!(
(tang_start.y() - 2.0).abs() < 1e-8,
"start tangent y: {}",
tang_start.y()
);
let tang_end = curve.tangent(1.0);
assert!(
(tang_end.x() - 1.0).abs() < 1e-8,
"end tangent x: {}",
tang_end.x()
);
assert!(
(tang_end.y() + 2.0).abs() < 1e-8,
"end tangent y: {}",
tang_end.y()
);
}
#[test]
fn nurbs2d_tangent_midpoint_is_horizontal() {
let curve = NurbsCurve2D::new(
2,
vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0],
vec![
Point2::new(0.0, 0.0),
Point2::new(0.5, 1.0),
Point2::new(1.0, 0.0),
],
vec![1.0, 1.0, 1.0],
)
.expect("valid curve");
let tang = curve.tangent(0.5);
assert!(
tang.y().abs() < 1e-10,
"midpoint tangent y should be zero, got {}",
tang.y()
);
assert!(tang.x() > 0.0, "midpoint tangent x should be positive");
}
#[test]
fn nurbs2d_empty_input_error() {
let result = NurbsCurve2D::new(0, vec![0.0], vec![], vec![]);
assert!(
matches!(result, Err(MathError::EmptyInput)),
"expected EmptyInput, got {result:?}"
);
}
#[test]
fn curve2d_tangent_dispatch_all_variants() {
let line =
Curve2D::Line(Line2D::new(Point2::new(0.0, 0.0), Vec2::new(1.0, 0.0)).expect("valid"));
let tang = line.tangent(7.0);
assert!(approx_eq(tang.x(), 1.0));
assert!(approx_eq(tang.y(), 0.0));
let circle = Curve2D::Circle(Circle2D::new(Point2::new(0.0, 0.0), 1.0).expect("valid"));
let tang = circle.tangent(0.0);
assert!(approx_eq(tang.x(), 0.0));
assert!(approx_eq(tang.y(), 1.0));
let ellipse =
Curve2D::Ellipse(Ellipse2D::new(Point2::new(0.0, 0.0), 4.0, 2.0, 0.0).expect("valid"));
let tang = ellipse.tangent(0.0);
assert!(approx_eq(tang.x(), 0.0));
assert!(approx_eq(tang.y(), 2.0));
let nurbs = Curve2D::Nurbs(
NurbsCurve2D::from_line(Point2::new(0.0, 0.0), Point2::new(2.0, 3.0)).expect("valid"),
);
let tang = nurbs.tangent(0.5);
assert!(approx_eq(tang.x(), 2.0));
assert!(approx_eq(tang.y(), 3.0));
}
}