ogeom_math/
vector.rs

1//! Free vectors in 2D and 3D.
2//!
3//! A vector has magnitude and direction and is unaffected by translation. See
4//! [`Direction`](crate::Direction) for the unit-length variant, whose invariant
5//! the type system enforces, and [`Point`](crate::Point) for positions.
6
7use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
8
9use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
10
11/// A free vector in space.
12#[derive(Debug, Clone, Copy, PartialEq, Default)]
13pub struct Vector {
14    /// X component.
15    pub x: f64,
16    /// Y component.
17    pub y: f64,
18    /// Z component.
19    pub z: f64,
20}
21
22/// A free vector in the plane.
23#[derive(Debug, Clone, Copy, PartialEq, Default)]
24pub struct Vector2 {
25    /// X component.
26    pub x: f64,
27    /// Y component.
28    pub y: f64,
29}
30
31impl Vector {
32    /// The zero vector.
33    pub const ZERO: Self = Self::new(0.0, 0.0, 0.0);
34    /// Unit vector along +X.
35    pub const X: Self = Self::new(1.0, 0.0, 0.0);
36    /// Unit vector along +Y.
37    pub const Y: Self = Self::new(0.0, 1.0, 0.0);
38    /// Unit vector along +Z.
39    pub const Z: Self = Self::new(0.0, 0.0, 1.0);
40
41    /// A vector from components.
42    #[must_use]
43    pub const fn new(x: f64, y: f64, z: f64) -> Self {
44        Self { x, y, z }
45    }
46
47    /// A vector with all components equal.
48    #[must_use]
49    pub const fn splat(v: f64) -> Self {
50        Self::new(v, v, v)
51    }
52
53    /// Components as an array.
54    #[must_use]
55    pub const fn to_array(self) -> [f64; 3] {
56        [self.x, self.y, self.z]
57    }
58
59    /// A vector from an array.
60    #[must_use]
61    pub const fn from_array([x, y, z]: [f64; 3]) -> Self {
62        Self::new(x, y, z)
63    }
64
65    /// Component by index, `0..3`.
66    ///
67    /// # Errors
68    ///
69    /// [`OgeomError::Range`](ogeom_core::OgeomError::Range) if `index >= 3`.
70    pub fn coord(self, index: usize) -> OgeomResult<f64> {
71        match index {
72            0 => Ok(self.x),
73            1 => Ok(self.y),
74            2 => Ok(self.z),
75            _ => ogeom_bail!(Range, "vector component {index} of 3"),
76        }
77    }
78
79    /// Dot product.
80    #[must_use]
81    pub fn dot(self, other: Self) -> f64 {
82        self.x
83            .mul_add(other.x, self.y.mul_add(other.y, self.z * other.z))
84    }
85
86    /// Cross product. Right-handed: `X.cross(Y) == Z`.
87    ///
88    /// Each component is a two-term difference `ab - cd`, evaluated without a
89    /// fused multiply-add on purpose. An FMA rounds one product and not the
90    /// other, which destroys the exact cancellation that makes the cross
91    /// product of collinear vectors come out at exactly zero.
92    #[must_use]
93    pub fn cross(self, other: Self) -> Self {
94        Self::new(
95            self.y * other.z - self.z * other.y,
96            self.z * other.x - self.x * other.z,
97            self.x * other.y - self.y * other.x,
98        )
99    }
100
101    /// Scalar triple product `self · (a × b)`: the signed volume of the
102    /// parallelepiped the three vectors span.
103    #[must_use]
104    pub fn triple(self, a: Self, b: Self) -> f64 {
105        self.dot(a.cross(b))
106    }
107
108    /// Squared magnitude. Prefer this to [`Vector::magnitude`] when comparing
109    /// lengths: it avoids a square root and the rounding that comes with it.
110    #[must_use]
111    pub fn square_magnitude(self) -> f64 {
112        self.dot(self)
113    }
114
115    /// Magnitude.
116    #[must_use]
117    pub fn magnitude(self) -> f64 {
118        self.square_magnitude().sqrt()
119    }
120
121    /// This vector scaled to unit length.
122    ///
123    /// # Errors
124    ///
125    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if the
126    /// magnitude is below `tol.confusion()`, or if any component is non-finite.
127    /// Normalizing a near-zero vector amplifies whatever noise it holds into an
128    /// arbitrary direction, so it is refused rather than approximated.
129    pub fn normalized(self, tol: Tolerances) -> OgeomResult<Self> {
130        if !self.is_finite() {
131            ogeom_bail!(Construction, "cannot normalize a non-finite vector");
132        }
133        let m = self.magnitude();
134        if m <= tol.confusion() {
135            ogeom_bail!(Construction, "cannot normalize a vector of magnitude {m}");
136        }
137        Ok(self / m)
138    }
139
140    /// Whether every component is finite.
141    #[must_use]
142    pub fn is_finite(self) -> bool {
143        self.x.is_finite() && self.y.is_finite() && self.z.is_finite()
144    }
145
146    /// Whether the magnitude is within `tol.confusion()` of zero.
147    #[must_use]
148    pub fn is_zero(self, tol: Tolerances) -> bool {
149        self.magnitude() <= tol.confusion()
150    }
151
152    /// Whether two vectors agree component-wise within `tol.confusion()`.
153    #[must_use]
154    pub fn is_equal(self, other: Self, tol: Tolerances) -> bool {
155        (self - other).magnitude() <= tol.confusion()
156    }
157
158    /// Angle to `other`, in `[0, π]`.
159    ///
160    /// Uses `atan2` of the cross and dot products rather than `acos` of the
161    /// normalized dot product: `acos` loses most of its precision for nearly
162    /// parallel or nearly antiparallel vectors, which is exactly where angle
163    /// tests matter.
164    ///
165    /// # Errors
166    ///
167    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if either
168    /// vector is degenerate.
169    pub fn angle(self, other: Self, tol: Tolerances) -> OgeomResult<f64> {
170        if self.is_zero(tol) || other.is_zero(tol) {
171            ogeom_bail!(Construction, "angle is undefined for a null vector");
172        }
173        Ok(self.cross(other).magnitude().atan2(self.dot(other)))
174    }
175
176    /// Angle to `other` measured about `reference`, in `(-π, π]`.
177    ///
178    /// Positive when `self` turns towards `other` counter-clockwise as seen from
179    /// the tip of `reference`.
180    ///
181    /// # Errors
182    ///
183    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if any vector
184    /// is degenerate, or if `reference` is not perpendicular to both within
185    /// `tol.angular()`.
186    pub fn signed_angle(self, other: Self, reference: Self, tol: Tolerances) -> OgeomResult<f64> {
187        let unsigned = self.angle(other, tol)?;
188        let normal = self.cross(other);
189        if normal.is_zero(tol) {
190            // Parallel or antiparallel: the sign is meaningless, and `unsigned`
191            // is already exactly 0 or π.
192            return Ok(unsigned);
193        }
194        if reference.is_zero(tol) {
195            ogeom_bail!(Construction, "reference vector is null");
196        }
197        Ok(if normal.dot(reference) < 0.0 {
198            -unsigned
199        } else {
200            unsigned
201        })
202    }
203
204    /// Whether `self` and `other` point the same way, within `tol.angular()`.
205    #[must_use]
206    pub fn is_parallel(self, other: Self, tol: Tolerances) -> bool {
207        self.angle(other, tol).is_ok_and(|a| a <= tol.angular())
208    }
209
210    /// Whether `self` and `other` are parallel, ignoring sense.
211    #[must_use]
212    pub fn is_collinear(self, other: Self, tol: Tolerances) -> bool {
213        self.angle(other, tol)
214            .is_ok_and(|a| a <= tol.angular() || (core::f64::consts::PI - a) <= tol.angular())
215    }
216
217    /// Whether `self` and `other` are perpendicular, within `tol.angular()`.
218    #[must_use]
219    pub fn is_normal(self, other: Self, tol: Tolerances) -> bool {
220        self.angle(other, tol)
221            .is_ok_and(|a| (core::f64::consts::FRAC_PI_2 - a).abs() <= tol.angular())
222    }
223
224    /// Component of `self` along `other`.
225    ///
226    /// # Errors
227    ///
228    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if `other` is
229    /// degenerate.
230    pub fn projected_onto(self, other: Self, tol: Tolerances) -> OgeomResult<Self> {
231        let square = other.square_magnitude();
232        if square <= tol.confusion() * tol.confusion() {
233            ogeom_bail!(Construction, "cannot project onto a null vector");
234        }
235        Ok(other * (self.dot(other) / square))
236    }
237
238    /// Linear interpolation, `t = 0` giving `self`.
239    #[must_use]
240    pub fn lerp(self, other: Self, t: f64) -> Self {
241        self + (other - self) * t
242    }
243
244    /// Component-wise minimum.
245    #[must_use]
246    pub fn min(self, other: Self) -> Self {
247        Self::new(
248            self.x.min(other.x),
249            self.y.min(other.y),
250            self.z.min(other.z),
251        )
252    }
253
254    /// Component-wise maximum.
255    #[must_use]
256    pub fn max(self, other: Self) -> Self {
257        Self::new(
258            self.x.max(other.x),
259            self.y.max(other.y),
260            self.z.max(other.z),
261        )
262    }
263
264    /// The 2D vector formed by dropping the Z component.
265    #[must_use]
266    pub const fn xy(self) -> Vector2 {
267        Vector2::new(self.x, self.y)
268    }
269}
270
271impl Vector2 {
272    /// The zero vector.
273    pub const ZERO: Self = Self::new(0.0, 0.0);
274    /// Unit vector along +X.
275    pub const X: Self = Self::new(1.0, 0.0);
276    /// Unit vector along +Y.
277    pub const Y: Self = Self::new(0.0, 1.0);
278
279    /// A vector from components.
280    #[must_use]
281    pub const fn new(x: f64, y: f64) -> Self {
282        Self { x, y }
283    }
284
285    /// Components as an array.
286    #[must_use]
287    pub const fn to_array(self) -> [f64; 2] {
288        [self.x, self.y]
289    }
290
291    /// A vector from an array.
292    #[must_use]
293    pub const fn from_array([x, y]: [f64; 2]) -> Self {
294        Self::new(x, y)
295    }
296
297    /// Dot product.
298    ///
299    /// Two terms, so no fused multiply-add; see [`Vector::cross`] for why.
300    #[must_use]
301    pub fn dot(self, other: Self) -> f64 {
302        self.x * other.x + self.y * other.y
303    }
304
305    /// The scalar cross product: the signed area of the parallelogram the two
306    /// vectors span. Positive when `other` is counter-clockwise from `self`.
307    #[must_use]
308    pub fn cross(self, other: Self) -> f64 {
309        self.x * other.y - self.y * other.x
310    }
311
312    /// Squared magnitude.
313    #[must_use]
314    pub fn square_magnitude(self) -> f64 {
315        self.dot(self)
316    }
317
318    /// Magnitude.
319    #[must_use]
320    pub fn magnitude(self) -> f64 {
321        self.square_magnitude().sqrt()
322    }
323
324    /// This vector rotated a quarter turn counter-clockwise. Exact: no
325    /// trigonometry involved.
326    #[must_use]
327    pub const fn perpendicular(self) -> Self {
328        Self::new(-self.y, self.x)
329    }
330
331    /// This vector scaled to unit length.
332    ///
333    /// # Errors
334    ///
335    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if the
336    /// magnitude is below `tol.confusion()`, or if any component is non-finite.
337    pub fn normalized(self, tol: Tolerances) -> OgeomResult<Self> {
338        if !self.is_finite() {
339            ogeom_bail!(Construction, "cannot normalize a non-finite vector");
340        }
341        let m = self.magnitude();
342        if m <= tol.confusion() {
343            ogeom_bail!(Construction, "cannot normalize a vector of magnitude {m}");
344        }
345        Ok(self / m)
346    }
347
348    /// Whether both components are finite.
349    #[must_use]
350    pub fn is_finite(self) -> bool {
351        self.x.is_finite() && self.y.is_finite()
352    }
353
354    /// Whether the magnitude is within `tol.confusion()` of zero.
355    #[must_use]
356    pub fn is_zero(self, tol: Tolerances) -> bool {
357        self.magnitude() <= tol.confusion()
358    }
359
360    /// Whether two vectors agree component-wise within `tol.confusion()`.
361    #[must_use]
362    pub fn is_equal(self, other: Self, tol: Tolerances) -> bool {
363        (self - other).magnitude() <= tol.confusion()
364    }
365
366    /// Angle to `other`, in `(-π, π]`, positive counter-clockwise.
367    ///
368    /// # Errors
369    ///
370    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if either
371    /// vector is degenerate.
372    pub fn angle(self, other: Self, tol: Tolerances) -> OgeomResult<f64> {
373        if self.is_zero(tol) || other.is_zero(tol) {
374            ogeom_bail!(Construction, "angle is undefined for a null vector");
375        }
376        Ok(self.cross(other).atan2(self.dot(other)))
377    }
378
379    /// Linear interpolation, `t = 0` giving `self`.
380    #[must_use]
381    pub fn lerp(self, other: Self, t: f64) -> Self {
382        self + (other - self) * t
383    }
384
385    /// This vector with a Z component of zero.
386    #[must_use]
387    pub const fn to_3d(self) -> Vector {
388        Vector::new(self.x, self.y, 0.0)
389    }
390}
391
392macro_rules! impl_vector_ops {
393    ($t:ty, $($f:ident),+) => {
394        impl Add for $t {
395            type Output = Self;
396            fn add(self, o: Self) -> Self { Self { $($f: self.$f + o.$f),+ } }
397        }
398        impl Sub for $t {
399            type Output = Self;
400            fn sub(self, o: Self) -> Self { Self { $($f: self.$f - o.$f),+ } }
401        }
402        impl Neg for $t {
403            type Output = Self;
404            fn neg(self) -> Self { Self { $($f: -self.$f),+ } }
405        }
406        impl Mul<f64> for $t {
407            type Output = Self;
408            fn mul(self, s: f64) -> Self { Self { $($f: self.$f * s),+ } }
409        }
410        impl Mul<$t> for f64 {
411            type Output = $t;
412            fn mul(self, v: $t) -> $t { v * self }
413        }
414        impl Div<f64> for $t {
415            type Output = Self;
416            fn div(self, s: f64) -> Self { Self { $($f: self.$f / s),+ } }
417        }
418        impl AddAssign for $t {
419            fn add_assign(&mut self, o: Self) { *self = *self + o; }
420        }
421        impl SubAssign for $t {
422            fn sub_assign(&mut self, o: Self) { *self = *self - o; }
423        }
424        impl MulAssign<f64> for $t {
425            fn mul_assign(&mut self, s: f64) { *self = *self * s; }
426        }
427        impl DivAssign<f64> for $t {
428            fn div_assign(&mut self, s: f64) { *self = *self / s; }
429        }
430    };
431}
432
433impl_vector_ops!(Vector, x, y, z);
434impl_vector_ops!(Vector2, x, y);
435
436#[cfg(test)]
437#[allow(clippy::unwrap_used)]
438mod tests {
439    use super::*;
440    use approx::assert_relative_eq;
441
442    const T: Tolerances = Tolerances::millimetres();
443
444    #[test]
445    fn cross_product_is_right_handed() {
446        assert_eq!(Vector::X.cross(Vector::Y), Vector::Z);
447        assert_eq!(Vector::Y.cross(Vector::Z), Vector::X);
448        assert_eq!(Vector::Z.cross(Vector::X), Vector::Y);
449        assert_eq!(Vector::Y.cross(Vector::X), -Vector::Z);
450    }
451
452    #[test]
453    fn normalizing_a_null_vector_is_refused_not_approximated() {
454        assert!(Vector::ZERO.normalized(T).is_err());
455        assert!(Vector::new(1e-12, 0.0, 0.0).normalized(T).is_err());
456        assert!(Vector::new(f64::NAN, 0.0, 0.0).normalized(T).is_err());
457        assert!(Vector::new(f64::INFINITY, 0.0, 0.0).normalized(T).is_err());
458        assert!(Vector::new(3.0, 4.0, 0.0).normalized(T).is_ok());
459    }
460
461    #[test]
462    fn normalized_has_unit_magnitude() {
463        let v = Vector::new(3.0, 4.0, 12.0).normalized(T).unwrap();
464        assert_relative_eq!(v.magnitude(), 1.0, epsilon = 1e-15);
465    }
466
467    #[test]
468    fn angle_is_accurate_for_nearly_parallel_vectors() {
469        // The case acos-based formulations get wrong: for a tiny angle, the dot
470        // product is 1 - O(angle^2), so acos loses half the mantissa. atan2 of
471        // cross over dot keeps full precision.
472        let tiny: f64 = 1e-9;
473        let a = Vector::X;
474        let b = Vector::new(tiny.cos(), tiny.sin(), 0.0);
475        assert_relative_eq!(a.angle(b, T).unwrap(), tiny, max_relative = 1e-9);
476    }
477
478    #[test]
479    fn angle_endpoints() {
480        assert_relative_eq!(Vector::X.angle(Vector::X, T).unwrap(), 0.0);
481        assert_relative_eq!(
482            Vector::X.angle(-Vector::X, T).unwrap(),
483            core::f64::consts::PI
484        );
485        assert_relative_eq!(
486            Vector::X.angle(Vector::Y, T).unwrap(),
487            core::f64::consts::FRAC_PI_2
488        );
489        assert!(Vector::X.angle(Vector::ZERO, T).is_err());
490    }
491
492    #[test]
493    fn signed_angle_respects_the_reference_direction() {
494        let a = Vector::X;
495        let b = Vector::Y;
496        let quarter = core::f64::consts::FRAC_PI_2;
497        assert_relative_eq!(a.signed_angle(b, Vector::Z, T).unwrap(), quarter);
498        assert_relative_eq!(a.signed_angle(b, -Vector::Z, T).unwrap(), -quarter);
499        // Antiparallel: no meaningful sign, and pi either way.
500        assert_relative_eq!(
501            a.signed_angle(-a, Vector::Z, T).unwrap(),
502            core::f64::consts::PI
503        );
504    }
505
506    #[test]
507    fn parallel_collinear_and_normal() {
508        let a = Vector::new(1.0, 2.0, 3.0);
509        assert!(a.is_parallel(a * 5.0, T));
510        assert!(!a.is_parallel(a * -5.0, T), "antiparallel is not parallel");
511        assert!(a.is_collinear(a * -5.0, T), "but it is collinear");
512        assert!(Vector::X.is_normal(Vector::Y, T));
513        assert!(!Vector::X.is_normal(Vector::X, T));
514    }
515
516    #[test]
517    fn projection_onto_an_axis() {
518        let v = Vector::new(3.0, 4.0, 5.0);
519        let p = v.projected_onto(Vector::X, T).unwrap();
520        assert_eq!(p, Vector::new(3.0, 0.0, 0.0));
521        // The residual is perpendicular to the axis, by construction.
522        assert_relative_eq!((v - p).dot(Vector::X), 0.0, epsilon = 1e-15);
523        assert!(v.projected_onto(Vector::ZERO, T).is_err());
524    }
525
526    #[test]
527    fn triple_product_is_the_signed_volume() {
528        assert_relative_eq!(Vector::X.triple(Vector::Y, Vector::Z), 1.0);
529        assert_relative_eq!(Vector::X.triple(Vector::Z, Vector::Y), -1.0);
530        // Coplanar vectors span no volume.
531        assert_relative_eq!(Vector::X.triple(Vector::Y, Vector::new(1.0, 1.0, 0.0)), 0.0);
532    }
533
534    #[test]
535    fn component_access_is_bounds_checked() {
536        let v = Vector::new(1.0, 2.0, 3.0);
537        assert_eq!(v.coord(0).unwrap(), 1.0);
538        assert_eq!(v.coord(2).unwrap(), 3.0);
539        assert!(v.coord(3).is_err());
540    }
541
542    #[test]
543    fn vector2_cross_is_the_signed_area() {
544        assert_relative_eq!(Vector2::X.cross(Vector2::Y), 1.0);
545        assert_relative_eq!(Vector2::Y.cross(Vector2::X), -1.0);
546        assert_relative_eq!(Vector2::X.cross(Vector2::X), 0.0);
547    }
548
549    #[test]
550    fn vector2_perpendicular_is_an_exact_quarter_turn() {
551        let v = Vector2::new(0.1, 0.7);
552        let p = v.perpendicular();
553        assert_eq!(p, Vector2::new(-0.7, 0.1));
554        assert_eq!(p.dot(v), 0.0, "exactly zero, not merely small");
555        assert_eq!(p.perpendicular().perpendicular().perpendicular(), v);
556    }
557
558    #[test]
559    fn vector2_angle_is_signed() {
560        let quarter = core::f64::consts::FRAC_PI_2;
561        assert_relative_eq!(Vector2::X.angle(Vector2::Y, T).unwrap(), quarter);
562        assert_relative_eq!(Vector2::Y.angle(Vector2::X, T).unwrap(), -quarter);
563    }
564
565    #[test]
566    fn arithmetic_operators() {
567        let a = Vector::new(1.0, 2.0, 3.0);
568        let b = Vector::new(4.0, 5.0, 6.0);
569        assert_eq!(a + b, Vector::new(5.0, 7.0, 9.0));
570        assert_eq!(b - a, Vector::splat(3.0));
571        assert_eq!(a * 2.0, Vector::new(2.0, 4.0, 6.0));
572        assert_eq!(2.0 * a, a * 2.0);
573        assert_eq!(a / 2.0, Vector::new(0.5, 1.0, 1.5));
574        let mut c = a;
575        c += b;
576        c -= b;
577        assert_eq!(c, a);
578    }
579
580    #[test]
581    fn lerp_hits_both_endpoints() {
582        let a = Vector::new(1.0, 0.0, 0.0);
583        let b = Vector::new(3.0, 4.0, 0.0);
584        assert_eq!(a.lerp(b, 0.0), a);
585        assert_eq!(a.lerp(b, 1.0), b);
586        assert_eq!(a.lerp(b, 0.5), Vector::new(2.0, 2.0, 0.0));
587    }
588}
ogeom_math/vector.rs

ogeom_math/
vector.rs
