Spline length

core/src/math/angle.rs

@@ -7,10 +7,10 @@ use core::{
     ops::{Add, Div, Mul, Neg, Rem, Sub},
 };
 
-use crate::math::{vary::ZDiv, Affine, ApproxEq, Linear, Vector};
+use super::{vary::ZDiv, Affine, ApproxEq, Linear, Vector};
 
 #[cfg(feature = "fp")]
-use crate::math::{float::f32, vec2, vec3, Vec2, Vec3};
+use super::{float::f32, vec2, vec3, Vec2, Vec3};
 
 //
 // Types
@@ -469,6 +469,10 @@ impl Linear for Angle {
     fn mul(&self, scalar: f32) -> Self {
         *self * scalar
     }
+
+    fn dot(&self, other: &Self) -> f32 {
+        self.0 * other.0
+    }
 }
 
 impl ZDiv for Angle {}

core/src/math/color.rs

@@ -3,11 +3,12 @@
 use core::{
     array,
     fmt::{self, Debug, Formatter},
+    iter::zip,
     marker::PhantomData,
     ops::Index,
 };
 
-use crate::math::{float::f32, vary::ZDiv, Affine, Linear, Vector};
+use super::{float::f32, vary::ZDiv, Affine, Linear, Vector};
 
 //
 // Types
@@ -529,6 +530,10 @@ impl<Sp, const DIM: usize> Linear for Color<[f32; DIM], Sp> {
     fn mul(&self, scalar: Self::Scalar) -> Self {
         array::from_fn(|i| self.0[i] * scalar).into()
     }
+
+    fn dot(&self, other: &Self) -> Self::Scalar {
+        zip(self.0, other.0).map(|(a, b)| a * b).sum()
+    }
 }
 
 impl<Sc, Sp, const N: usize> ZDiv for Color<[Sc; N], Sp> where Sc: ZDiv + Copy {}

core/src/math/mat.rs

@@ -148,7 +148,7 @@ impl<const N: usize, Map> Matrix<[[f32; N]; N], Map> {
 impl Mat4x4 {
     /// Constructs a matrix from a set of basis vectors.
     ///
-    /// The vector do not need to be linearly independent.
+    /// The vectors do not have to be orthogonal or linearly independent.
     pub const fn from_basis<S, D>(
         i: Vec3<D>,
         j: Vec3<D>,
@@ -159,7 +159,24 @@ impl Mat4x4 {
             i[0], j[0], k[0], 0.0;
             i[1], j[1], k[1], 0.0;
             i[2], j[2], k[2], 0.0;
-            0.0, 0.0, 0.0, 1.0
+            0.0,  0.0,  0.0,  1.0
+        ]
+    }
+    /// Constructs a matrix from an origin point and a set of basis vectors.
+    ///
+    /// The vectors do not have to be orthogonal or linearly independent.
+    pub const fn from_affine_basis<S, D>(
+        o: Point3<D>,
+        i: Vec3<D>,
+        j: Vec3<D>,
+        k: Vec3<D>,
+    ) -> Mat4x4<RealToReal<3, S, D>> {
+        let (o, i, j, k) = (o.0, i.0, j.0, k.0);
+        mat![
+            i[0], j[0], k[0], o[0];
+            i[1], j[1], k[1], o[1];
+            i[2], j[2], k[2], o[2];
+            0.0,  0.0,  0.0,  1.0
         ]
     }
 }

core/src/math/point.rs

@@ -5,7 +5,7 @@ use core::{
     ops::{Add, AddAssign, Index, Sub, SubAssign},
 };
 
-use crate::math::{space::Real, vary::ZDiv, Affine, ApproxEq, Linear, Vector};
+use super::{space::Real, vary::ZDiv, Affine, ApproxEq, Linear, Vector};
 
 #[repr(transparent)]
 pub struct Point<Repr, Space = ()>(pub Repr, Pd<Space>);

core/src/math/space.rs

@@ -5,7 +5,7 @@
 use core::fmt::{Debug, Formatter};
 use core::marker::PhantomData;
 
-use crate::math::vary::{Iter, Vary, ZDiv};
+use super::vary::{Iter, Vary, ZDiv};
 
 /// Trait for types representing elements of an affine space.
 ///
@@ -29,6 +29,11 @@ pub trait Affine: Sized {
     ///
     /// `sub` is anti-commutative: `v.sub(w) == w.sub(v).neg()`.
     fn sub(&self, other: &Self) -> Self::Diff;
+
+    /// Returns the squared distance between `self` and `other`.
+    fn dist_sqr(&self, other: &Self) -> <Self::Diff as Linear>::Scalar {
+        self.sub(other).len_sqr()
+    }
 }
 
 /// Trait for types representing elements of a linear space (vector space).
@@ -64,6 +69,14 @@ pub trait Linear: Affine<Diff = Self> {
     /// v.mul(a).sub(&w.mul(a)) == v.add(&w).sub(&a);
     /// ```
     fn mul(&self, scalar: Self::Scalar) -> Self;
+
+    /// Returns the dot product, or inner product, of `self` and `other`.
+    fn dot(&self, other: &Self) -> Self::Scalar;
+
+    /// Returns the squared length (magnitude) of `self`.
+    fn len_sqr(&self) -> Self::Scalar {
+        self.dot(self)
+    }
 }
 
 /// Tag type for real vector spaces and Euclidean spaces.
@@ -104,6 +117,10 @@ impl Linear for f32 {
     fn mul(&self, rhs: f32) -> f32 {
         self * rhs
     }
+
+    fn dot(&self, other: &Self) -> f32 {
+        self * other
+    }
 }
 
 impl Affine for i32 {
@@ -131,6 +148,10 @@ impl Linear for i32 {
     fn mul(&self, rhs: i32) -> i32 {
         self * rhs
     }
+
+    fn dot(&self, other: &Self) -> i32 {
+        self * other
+    }
 }
 
 impl Affine for u32 {

core/src/math/spline.rs

@@ -1,10 +1,11 @@
 //! Bézier curves and splines.
 
 use alloc::{vec, vec::Vec};
-use core::{array, fmt::Debug};
+use core::{array, fmt::Debug, iter::from_fn};
 
 use crate::geom::Ray;
-use crate::math::{Affine, Lerp, Linear};
+
+use super::{mat::RealToReal, Affine, Lerp, Linear, Mat4x4, Point3, Vec3};
 
 /// A cubic Bézier curve, defined by four control points.
 ///
@@ -185,6 +186,15 @@ where
         Self(pts.to_vec())
     }
 
+    /// Creates a Bézier spline from a sequence of [`Ray`]s.
+    ///
+    /// Every pair of consecutive rays (p₀, d₀), (p₁, d₁) makes up one cubic
+    /// Bézier component with control points (p₀, p₀ + d₀, p₁ - d₁, p₁).
+    /// It follows that the returned composite curve is *C*² continuous:
+    /// it has continuous first and second derivatives.
+    ///
+    /// # Panics
+    /// If the number of items in `rays` is less than 2.
     pub fn from_rays<I>(rays: I) -> Self
     where
         I: IntoIterator<Item = Ray<T, T::Diff>>,
@@ -202,7 +212,8 @@ where
                 pts.push(p.add(&v));
             }
         }
-        Self::new(&pts)
+        assert!(pts.len() >= 4);
+        Self(pts)
     }
 
     /// Evaluates `self` at position `t`.
@@ -301,6 +312,50 @@ where
     }
 }
 
+impl<B: Debug + Default> BezierSpline<Point3<B>> {
+    /// Returns a sequence of coordinate frames at the given `t` values.
+    ///
+    ///
+    pub fn frames<'a, C>(
+        &'a self,
+        ts: impl IntoIterator<Item = f32> + 'a,
+    ) -> impl Iterator<Item = Mat4x4<RealToReal<3, C, B>>> + 'a {
+        let mut ts = ts.into_iter();
+
+        let mut fwd = Vec3::zero();
+        let mut right = Vec3::zero();
+        let mut up = Vec3::zero();
+
+        let mut t = ts.next();
+
+        if let Some(t) = t {
+            fwd = self.tangent(t).normalize();
+            let [x, y, z] = fwd.0.map(f32::abs);
+            let up0 = if y <= x && y <= z {
+                Vec3::Y
+            } else if z <= x && z <= y {
+                Vec3::Z
+            } else {
+                Vec3::X
+            };
+            right = up0.to().cross(&fwd).normalize();
+            up = fwd.cross(&right);
+        }
+
+        from_fn(move || {
+            let pos = self.eval(t?);
+            let res = Mat4x4::from_affine_basis(pos, right, up, fwd);
+            t = ts.next();
+            if let Some(t) = t {
+                fwd = self.tangent(t).normalize();
+                right = up.cross(&fwd).normalize();
+                up = fwd.cross(&right);
+            }
+            Some(res)
+        })
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use alloc::vec;
@@ -434,4 +489,20 @@ mod tests {
         assert_eq!(c.eval(1.0), 0.5);
         assert_eq!(c.eval(2.0), 0.5);
     }
+
+    #[test]
+    fn bezier_from_rays() {
+        let rays = [
+            Ray(pt2::<_, ()>(0.0, 0.0), vec2::<_, ()>(0.0, 1.0)),
+            Ray(pt2(1.0, 1.0), vec2(1.0, 0.0)),
+            Ray(pt2(2.0, 0.0), vec2(0.0, -1.0)),
+        ];
+        let b = BezierSpline::from_rays(rays);
+
+        assert_eq!(b.eval(0.0), pt2(0.0, 0.0));
+        assert_eq!(b.eval(0.25), pt2(0.125, 0.875));
+        assert_eq!(b.eval(0.5), pt2(1.0, 1.0));
+        assert_eq!(b.eval(0.75), pt2(1.875, 0.875));
+        assert_eq!(b.eval(1.0), pt2(2.0, 0.0));
+    }
 }

core/src/math/vec.rs

@@ -5,13 +5,13 @@
 use core::{
     array,
     fmt::{Debug, Formatter},
-    iter::Sum,
+    iter::{zip, Sum},
     marker::PhantomData as Pd,
     ops::{Add, Div, Index, IndexMut, Mul, Neg, Sub},
     ops::{AddAssign, DivAssign, MulAssign, SubAssign},
 };
 
-use crate::math::{
+use super::{
     float::f32,
     space::{Proj4, Real},
     vary::ZDiv,
@@ -146,7 +146,7 @@ impl<Sp, const N: usize> Vector<[f32; N], Sp> {
         use super::float::RecipSqrt;
         let len_sqr = self.len_sqr();
         assert!(
-            len_sqr.is_finite() && !len_sqr.approx_eq(&0.0),
+            len_sqr.is_finite() && !len_sqr.approx_eq_eps(&0.0, &1e-12),
             "cannot normalize a near-zero or non-finite vector: {self:?}"
         );
         *self * f32::recip_sqrt(len_sqr)
@@ -200,10 +200,8 @@ where
     /// Returns the dot product of `self` and `other`.
     #[inline]
     pub fn dot(&self, other: &Self) -> Sc {
-        self.0
-            .iter()
-            .zip(&other.0)
-            .map(|(a, b)| a.mul(*b))
+        zip(self.0, other.0)
+            .map(|(a, b)| a.mul(b))
             .fold(Sc::zero(), |acc, x| acc.add(&x))
     }
 
@@ -412,6 +410,10 @@ where
     fn mul(&self, scalar: Sc) -> Self {
         self.map(|c| c.mul(scalar))
     }
+
+    fn dot(&self, other: &Self) -> Self::Scalar {
+        self.dot(&other)
+    }
 }
 
 impl<Sc, Sp, const N: usize> ZDiv for Vector<[Sc; N], Sp>