geop_core_math/vector/
linalg.rs

1use crate::{
2    geop_error::{GeopError, GeopResult},
3    scalars::Number,
4    vector::{Vector, Vector2, Vector3, Vector4},
5};
6
7impl<T: Number, const N: usize> Vector<N, T> {
8    pub fn add(&self, other: &Self) -> Self {
9        let mut out = Self::new();
10        for i in 0..self.size() {
11            out[i] = self[i].add(other[i]);
12        }
13        out
14    }
15
16    pub fn sub(&self, other: &Self) -> Self {
17        let mut out = Self::new();
18        for i in 0..self.size() {
19            out[i] = self[i].sub(other[i]);
20        }
21        out
22    }
23
24    pub fn neg(&self) -> Self {
25        let mut out = Self::new();
26        for i in 0..self.size() {
27            out[i] = self[i].neg();
28        }
29        out
30    }
31
32    pub fn prod_scalar(&self, s: T) -> Self {
33        let mut out = Self::new();
34        for i in 0..self.size() {
35            out[i] = self[i].mul(s);
36        }
37        out
38    }
39
40    pub fn prod_dot(&self, other: &Self) -> T {
41        let mut acc = T::ZERO;
42        for i in 0..self.size() {
43            acc = acc.add(self[i].mul(other[i]));
44        }
45        acc
46    }
47
48    pub fn norm_sq(&self) -> T {
49        self.prod_dot(self)
50    }
51
52    pub fn norm(&self) -> T {
53        self.norm_sq().sqrt().expect("Norm cannot be negative")
54    }
55
56    /// `|self|`, failing where [`Scalar::sqrt`] refuses the squared length
57    /// instead of panicking like [`Self::norm`].
58    pub fn try_norm(&self) -> GeopResult<T> {
59        self.norm_sq().sqrt()
60    }
61
62    pub fn normalize(&self) -> GeopResult<Self> {
63        let n = self.try_norm()?;
64        if n.could_be_equal(T::ZERO) {
65            return Err(GeopError::new("Cannot normalize zero-length vector"));
66        }
67        Ok(self.prod_scalar(T::ONE.div(n)?))
68    }
69
70    /// `N - 1` orthonormal vectors perpendicular to `self`, or an error for
71    /// a zero `self`. They are the columns other than the first of the
72    /// Householder reflection `H = I - 2 w wᵀ / (wᵀ w)`, `w = v̂ + σ e_0`,
73    /// which swaps `v̂ = self / |self|` and `σ e_0`: `H` is orthogonal, so its
74    /// columns are orthonormal, and all but the first are perpendicular to
75    /// `v̂`. The sign `σ` of `v̂_0` keeps `wᵀ w = 2 (1 + |v̂_0|) >= 2` away
76    /// from cancellation, so the result is well conditioned for every `v̂`.
77    pub fn orthonormal_complement(&self) -> GeopResult<Vec<Self>> {
78        let v = self.normalize()?;
79        let sigma = if v[0].definitely_less(T::ZERO) {
80            T::ONE.neg()
81        } else {
82            T::ONE
83        };
84        let w = v.add(&Self::axis(0).prod_scalar(sigma));
85        let scale = T::from_i64(2).div(w.norm_sq())?;
86        Ok((1..N)
87            .map(|j| Self::axis(j).sub(&w.prod_scalar(scale.mul(w[j]))))
88            .collect())
89    }
90
91    /// How far `self` is from the segment `a..b`.
92    pub fn distance_to_segment(&self, a: &Self, b: &Self) -> T {
93        let ab = b.sub(a);
94        let along = self
95            .sub(a)
96            .prod_dot(&ab)
97            .div(ab.norm_sq())
98            .unwrap_or(T::ZERO);
99        let along = if along.definitely_less(T::ZERO) {
100            T::ZERO
101        } else if along.definitely_greater(T::ONE) {
102            T::ONE
103        } else {
104            along
105        };
106        self.sub(&a.add(&ab.prod_scalar(along))).norm()
107    }
108
109    /// True if every component `could_be_equal` `other`'s.
110    pub fn could_be_equal(&self, other: &Self) -> bool {
111        (0..self.size()).all(|i| self[i].could_be_equal(other[i]))
112    }
113
114    /// The componentwise [`Scalar::union`] of `self` and `other`.
115    pub fn union(&self, other: &Self) -> Self {
116        let mut out = Self::new();
117        for i in 0..self.size() {
118            out[i] = self[i].union(other[i]);
119        }
120        out
121    }
122}
123
124impl<const N: usize> Vector<N> {
125    /// Componentwise [`Scalar::sharpen`] — see that method's own doc
126    /// comment for when collapsing a value to a single representative
127    /// point is a safe, free choice (you only ever needed *some* point
128    /// within the interval) versus a silent loss of real uncertainty.
129    pub fn sharpen(&self) -> Self {
130        let mut out = Self::new();
131        for i in 0..self.size() {
132            out[i] = self[i].sharpen();
133        }
134        out
135    }
136}
137
138impl<T: Number> Vector3<T> {
139    /// Two unit directions square to the unit vector `self` and to each
140    /// other: the first from whichever of `x`, `y`, `z` runs least along it
141    /// — the first no other definitely runs less along, so that an axis a
142    /// hair off one of them does not pick between two by noise. A free
143    /// choice, made the same way wherever a direction across an axis is
144    /// measured from: a joint's turns, a frame's `x`, a sketch's parallel.
145    pub fn across(&self) -> GeopResult<[Self; 2]> {
146        let size = |k: usize| self[k].abs();
147        let least = (0..3)
148            .find(|&k| !(0..3).any(|j| size(j).definitely_less(size(k))))
149            .unwrap_or(0);
150        let reference = Self::axis(least);
151        let first = (reference.sub(&self.prod_scalar(self.prod_dot(&reference))))
152            .normalize()
153            .map_err(|e| e.with_context(format!("a direction square to {self:?}")))?;
154        Ok([first, self.prod_cross(&first)])
155    }
156
157    pub fn prod_cross(&self, other: &Self) -> Self {
158        let mut out = Self::new();
159        out[0] = self[1].mul(other[2]).sub(self[2].mul(other[1]));
160        out[1] = self[2].mul(other[0]).sub(self[0].mul(other[2]));
161        out[2] = self[0].mul(other[1]).sub(self[1].mul(other[0]));
162        out
163    }
164
165    /// Whether `self` and `other` could be parallel, in either sense: their
166    /// cross product could be zero.
167    pub fn could_be_parallel(&self, other: &Self) -> bool {
168        self.prod_cross(other).norm_sq().could_be_equal(T::ZERO)
169    }
170
171    /// `self` turned about the unit vector `axis` by the angle with cosine
172    /// `cos` and sine `sin` (Rodrigues' formula).
173    pub fn rotated_about(&self, axis: &Self, cos: T, sin: T) -> Self {
174        let along = axis.prod_scalar(axis.prod_dot(self).mul(T::ONE.sub(cos)));
175        self.prod_scalar(cos)
176            .add(&axis.prod_cross(self).prod_scalar(sin))
177            .add(&along)
178    }
179
180    /// The point `self` with weight `w`, homogeneous: `(w x, w y, w z, w)`.
181    /// Taken as it is where the weight is exactly one: multiplying by it
182    /// would only widen it by rounding.
183    pub fn homogeneous(&self, w: T) -> Vector4<T> {
184        if w.is_exactly(T::ONE) {
185            Vector4::from_array([self[0], self[1], self[2], w])
186        } else {
187            Vector4::from_array([self[0].mul(w), self[1].mul(w), self[2].mul(w), w])
188        }
189    }
190
191    /// The point the homogeneous 2-D point `(w x, w y, w)` stands for,
192    /// `(x, y)`: each coordinate divided by the weight (not multiplied by
193    /// its inverse, which rounds twice).
194    pub fn dehomogenized(&self) -> GeopResult<Vector2<T>> {
195        Ok(Vector2::from_array([
196            self[0].div(self[2])?,
197            self[1].div(self[2])?,
198        ]))
199    }
200}
201
202impl<T: Number> Vector4<T> {
203    /// The point the homogeneous 3-D point `(w x, w y, w z, w)` stands for,
204    /// `(x, y, z)`: each coordinate divided by the weight (not multiplied by
205    /// its inverse, which rounds twice).
206    pub fn dehomogenized(&self) -> GeopResult<Vector3<T>> {
207        Ok(Vector3::from_array([
208            self[0].div(self[3])?,
209            self[1].div(self[3])?,
210            self[2].div(self[3])?,
211        ]))
212    }
213}
214
215impl<T: Number> Vector2<T> {
216    /// The 2-D cross product `self.x * other.y - self.y * other.x`: the z
217    /// component of the 3-D one, positive when `other` lies counter-clockwise
218    /// of `self`.
219    pub fn prod_cross(&self, other: &Self) -> T {
220        self[0].mul(other[1]).sub(self[1].mul(other[0]))
221    }
222
223    /// The point `self` with weight `w`, homogeneous: `(w x, w y, w)` —
224    /// taken as it is where the weight is exactly one (see
225    /// [`Vector3::homogeneous`]).
226    pub fn homogeneous(&self, w: T) -> Vector3<T> {
227        if w.is_exactly(T::ONE) {
228            Vector3::from_array([self[0], self[1], w])
229        } else {
230            Vector3::from_array([self[0].mul(w), self[1].mul(w), w])
231        }
232    }
233
234    /// Rotated 90 degrees counter-clockwise — by `neg`, which is exact,
235    /// where subtracting from zero would round.
236    pub fn perp(&self) -> Self {
237        Self::from_array([self[1].neg(), self[0]])
238    }
239
240    /// Rotated counter-clockwise by the angle with cosine `c` and sine `s`.
241    pub fn rotate(&self, c: T, s: T) -> Self {
242        Self::from_array([
243            self[0].mul(c).sub(self[1].mul(s)),
244            self[0].mul(s).add(self[1].mul(c)),
245        ])
246    }
247}
geop_core_math/vector/linalg.rs

geop_core_math/vector/
linalg.rs
