constgebra/
comptime.rs

1#![allow(clippy::needless_range_loop)]
2#![allow(clippy::erasing_op)]
3
4use const_soft_float::soft_f64::SoftF64 as Sf64;
5
6/// Used with `CMatrix::apply_each` to specify an operation, as `const` function
7/// pointers and closures are not yet stable
8pub enum Operation {
9    Mul(f64),
10    Div(f64),
11    Sqrt,
12}
13
14/// A helper type to construct row vectors. `CMatrix` is used
15/// internally, but makes working with 1-dimensional data less verbose.
16/// ```
17/// # use constgebra::CVector;
18/// const ARRAY: [f64; 2] = [4.0, 7.0];
19///
20/// const ROW_VECTOR: CVector::<2> = CVector::new_vector(ARRAY);
21/// const RESULT: [f64; 2] = ROW_VECTOR.finish_vector();
22///
23/// assert_eq!(ARRAY, RESULT)
24/// ```
25pub type CVector<const N: usize> = CMatrix<1, N>;
26
27/// A `const` matrix type, with dimensions checked at compile time
28/// for all operations.
29#[derive(Clone, Copy, PartialEq, PartialOrd)]
30pub struct CMatrix<const R: usize, const C: usize>([[Sf64; C]; R]);
31
32impl<const R: usize, const C: usize> Default for CMatrix<R, C> {
33    fn default() -> Self {
34        Self::zero()
35    }
36}
37
38impl<const R: usize, const C: usize> CMatrix<R, C> {
39    /// Create a `CMatrix` from a 2D array of `f64`.
40    /// ```rust
41    /// # use constgebra::CMatrix;
42    /// const ARRAY: [[f64; 2]; 2] = [
43    ///     [4.0, 7.0],
44    ///     [2.0, 6.0]
45    /// ];
46    ///
47    /// const CMATRIX: CMatrix::<2, 2> = CMatrix::new(ARRAY);
48    /// ```
49    pub const fn new(vals: [[f64; C]; R]) -> Self {
50        let mut ret = [[Sf64(0.0); C]; R];
51
52        let mut i = 0;
53        while i < R {
54            let mut j = 0;
55            while j < C {
56                ret[i][j] = Sf64(vals[i][j]);
57                j += 1;
58            }
59            i += 1
60        }
61        CMatrix(ret)
62    }
63
64    /// Equivalent to `CMatrix::new` using `const_soft_float::SoftF64`
65    /// instead of `f64.`
66    pub const fn new_from_soft(vals: [[Sf64; C]; R]) -> Self {
67        CMatrix(vals)
68    }
69
70    /// Create a `CMatrix` filled with zeroes.
71    pub const fn zero() -> Self {
72        // TODO: Replace this with `::default()` once this PR is merged and released:
73        // https://github.com/823984418/const_soft_float/pull/7
74        Self([[Sf64::from_f64(0.0); C]; R])
75    }
76
77    /// Create an identity `CMatrix`.
78    ///```rust
79    /// # use constgebra::CMatrix;
80    /// const LEFT: CMatrix<4, 3> = CMatrix::new([
81    ///     [1.0, 0.0, 1.0],
82    ///     [2.0, 1.0, 1.0],
83    ///     [0.0, 1.0, 1.0],
84    ///     [1.0, 1.0, 2.0],
85    /// ]);
86    ///
87    /// const RIGHT: CMatrix<3, 3> = CMatrix::identity();
88    ///
89    /// const EXPECTED: [[f64; 3]; 4] = LEFT.finish();
90    ///
91    /// const RESULT: [[f64; 3]; 4] = LEFT.mul(RIGHT).finish();
92    ///
93    /// assert_eq!(EXPECTED, RESULT);
94    /// ```
95    pub const fn identity() -> Self {
96        let mut ret = Self::zero();
97        let diag_max = match R > C {
98            true => C,
99            false => R,
100        };
101
102        let mut idx = 0;
103        while idx < diag_max {
104            ret.0[idx][idx] = Sf64::from_f64(1.0);
105            idx += 1;
106        }
107
108        ret
109    }
110
111    /// Converts a `CMatrix` back into a two-dimensional array.
112    /// ```rust
113    /// # use constgebra::CMatrix;
114    /// const ARRAY: [[f64; 2]; 2] = [
115    ///     [4.0, 7.0],
116    ///     [2.0, 6.0]
117    /// ];
118    ///
119    /// const CMATRIX: CMatrix::<2, 2> = CMatrix::new(ARRAY);
120    ///
121    /// const RESULT: [[f64; 2]; 2] = CMATRIX.finish();
122    ///
123    /// assert_eq!(ARRAY, RESULT)
124    /// ```
125    pub const fn finish(self) -> [[f64; C]; R] {
126        let mut ret = [[0.0; C]; R];
127
128        let mut i = 0;
129        while i < R {
130            let mut j = 0;
131            while j < C {
132                ret[i][j] = self.0[i][j].0;
133                j += 1;
134            }
135            i += 1
136        }
137        ret
138    }
139
140    /// `CMatrix::finish`, but returns `const_soft_float::SoftF64`
141    pub const fn finish_soft(self) -> [[Sf64; C]; R] {
142        self.0
143    }
144
145    const fn rows(&self) -> usize {
146        R
147    }
148
149    const fn columns(&self) -> usize {
150        C
151    }
152
153    const fn get_dims(&self) -> [usize; 2] {
154        if R == C {
155            return [R, C];
156        }
157
158        if R > C {
159            [R, C]
160        } else {
161            [C, R]
162        }
163    }
164
165    /// Multiply two `CMatrix` and return the result. Columns
166    /// of self and rows of multiplier must agree in number.
167    ///```rust
168    /// # use constgebra::CMatrix;
169    /// const LEFT: CMatrix<4, 3> = CMatrix::new([
170    ///     [1.0, 0.0, 1.0],
171    ///     [2.0, 1.0, 1.0],
172    ///     [0.0, 1.0, 1.0],
173    ///     [1.0, 1.0, 2.0],
174    /// ]);
175    ///
176    /// const RIGHT: CMatrix<3, 3> = CMatrix::new([
177    ///     [1.0, 2.0, 1.0],
178    ///     [2.0, 3.0, 1.0],
179    ///     [4.0, 2.0, 2.0]
180    /// ]);
181    ///
182    /// const EXPECTED: [[f64; 3]; 4] = [
183    ///     [5.0, 4.0, 3.0],
184    ///     [8.0, 9.0, 5.0],
185    ///     [6.0, 5.0, 3.0],
186    ///     [11.0, 9.0, 6.0],
187    /// ];
188    ///
189    /// const RESULT: [[f64; 3]; 4] = LEFT.mul(RIGHT).finish();
190    ///
191    /// assert_eq!(EXPECTED, RESULT);
192    /// ```
193    pub const fn mul<const OC: usize>(self, rhs: CMatrix<C, OC>) -> CMatrix<R, OC> {
194        let mut ret = [[Sf64(0.0); OC]; R];
195
196        let mut i = 0;
197        while i < R {
198            let mut j = 0;
199            while j < OC {
200                let mut k = 0;
201                let mut acc: Sf64 = Sf64(0.0_f64);
202                while k < C {
203                    acc = acc.add(self.0[i][k].mul(rhs.0[k][j]));
204                    k += 1
205                }
206                ret[i][j] = acc;
207                j += 1;
208            }
209            i += 1;
210        }
211        CMatrix(ret)
212    }
213
214    /// Add two `CMatrix` and return the result.
215    /// ```rust
216    /// # use constgebra::CMatrix;
217    /// const LEFT: CMatrix<3, 3> = CMatrix::new([
218    ///     [1.0, 0.0, 1.0],
219    ///     [2.0, 1.0, 1.0],
220    ///     [0.0, 1.0, 1.0]]
221    /// );
222    ///
223    /// const RIGHT: CMatrix<3, 3> = CMatrix::new([
224    ///     [1.0, 2.0, 1.0],
225    ///     [2.0, 3.0, 1.0],
226    ///     [4.0, 2.0, 2.0]]
227    /// );
228    ///
229    /// const EXPECTED: [[f64; 3]; 3] = [
230    ///     [2.0, 2.0, 2.0],
231    ///     [4.0, 4.0, 2.0],
232    ///     [4.0, 3.0, 3.0]
233    /// ];
234    ///
235    /// const RESULT: [[f64; 3]; 3] = LEFT.add(RIGHT).finish();
236    ///
237    /// assert_eq!(EXPECTED, RESULT);
238    ///```
239    pub const fn add(self, rhs: Self) -> Self {
240        let mut ret = [[Sf64(0.0); C]; R];
241
242        let mut i = 0;
243        while i < R {
244            let mut j = 0;
245            while j < C {
246                ret[i][j] = self.0[i][j].add(rhs.0[i][j]);
247                j += 1;
248            }
249            i += 1;
250        }
251        CMatrix(ret)
252    }
253
254    /// Subtract two `CMatrix` and return the result.
255    /// ```rust
256    /// # use constgebra::CMatrix;
257    /// const LEFT: CMatrix<3, 3> = CMatrix::new([
258    ///     [1.0, 2.0, 1.0],
259    ///     [2.0, 3.0, 1.0],
260    ///     [4.0, 2.0, 2.0]]
261    /// );
262    ///
263    /// const RIGHT: CMatrix<3, 3> = CMatrix::new([
264    ///     [1.0, 0.0, 1.0],
265    ///     [2.0, 1.0, 1.0],
266    ///     [0.0, 1.0, 1.0]]
267    /// );
268    ///
269    /// const EXPECTED: [[f64; 3]; 3] = [
270    ///     [0.0, 2.0, 0.0],
271    ///     [0.0, 2.0, 0.0],
272    ///     [4.0, 1.0, 1.0]
273    /// ];
274    ///
275    /// const RESULT: [[f64; 3]; 3] = LEFT.sub(RIGHT).finish();
276    ///
277    /// assert_eq!(EXPECTED, RESULT);
278    ///```
279    pub const fn sub(self, rhs: Self) -> Self {
280        let mut ret = [[Sf64(0.0); C]; R];
281
282        let mut i = 0;
283        while i < R {
284            let mut j = 0;
285            while j < C {
286                ret[i][j] = self.0[i][j].sub(rhs.0[i][j]);
287                j += 1;
288            }
289            i += 1;
290        }
291        CMatrix(ret)
292    }
293
294    /// Apply an operation to each member of the matrix separately. Especially
295    /// useful for scaling vectors
296    /// ```
297    /// # use constgebra::{CMatrix, Operation};
298    /// const BASE: CMatrix<1, 3> = CMatrix::new([[1.0, 2.0, 3.0]]);
299    /// const MUL: CMatrix<1, 3> = BASE.apply_each(Operation::Mul(3.0));
300    /// assert_eq!([[3.0, 6.0, 9.0]], MUL.finish());
301    pub const fn apply_each(mut self, op: Operation) -> Self {
302        let mut i = 0;
303        while i < R {
304            let mut j = 0;
305            while j < C {
306                self.0[i][j] = match op {
307                    Operation::Mul(val) => self.0[i][j].mul(Sf64(val)),
308                    Operation::Div(val) => self.0[i][j].div(Sf64(val)),
309                    Operation::Sqrt => self.0[i][j].sqrt(),
310                };
311                j += 1;
312            }
313            i += 1;
314        }
315        self
316    }
317
318    const fn get(&self, row: usize, column: usize) -> Sf64 {
319        self.0[row][column]
320    }
321
322    #[must_use]
323    // TODO Replace once const_mut_refs are stabilized
324    const fn set(self, row: usize, column: usize, value: Sf64) -> Self {
325        let mut ret = self.0;
326        ret[row][column] = value;
327        Self(ret)
328    }
329
330    /// Return the transpose of a `CMatrix`.
331    /// ```rust
332    /// # use constgebra::CMatrix;
333    /// const START: [[f64; 2]; 2] = [
334    ///     [4.0, 7.0],
335    ///     [2.0, 6.0]
336    /// ];
337    ///
338    /// const EXPECTED: [[f64; 2]; 2] = [
339    ///     [4.0, 2.0],
340    ///     [7.0, 6.0]
341    /// ];
342    ///
343    /// const RESULT: [[f64; 2]; 2] =
344    ///     CMatrix::new(START).transpose().finish();
345    ///
346    /// assert_eq!(EXPECTED, RESULT)
347    /// ```
348    pub const fn transpose(self) -> CMatrix<C, R> {
349        let mut i = 0;
350        let mut ret = [[Sf64(0.0); R]; C];
351        while i < R {
352            let mut j = 0;
353            while j < C {
354                ret[j][i] = self.0[i][j];
355                j += 1;
356            }
357            i += 1;
358        }
359        CMatrix(ret)
360    }
361
362    #[must_use]
363    pub const fn givens_l(self, m: usize, a: Sf64, b: Sf64) -> Self {
364        let r = (a.powi(2).add(b.powi(2))).sqrt();
365        if eq(r, Sf64(0.0)) {
366            return self;
367        }
368
369        let mut mut_self = self;
370        let c = a.div(r);
371        let s = b.neg().div(r);
372        let mut i = 0;
373        while i < C {
374            let s0 = mut_self.get(m, i);
375            let s1 = mut_self.get(m + 1, i);
376
377            let val = mut_self.get(m, i).add(s0.mul(c.sub(Sf64(1.0))));
378            mut_self = mut_self.set(m, i, val);
379
380            let val = mut_self.get(m, i).add(s1.mul(s.neg()));
381            mut_self = mut_self.set(m, i, val);
382
383            let val = mut_self.get(m + 1, i).add(s0.mul(s));
384            mut_self = mut_self.set(m + 1, i, val);
385
386            let val = mut_self.get(m + 1, i).add(s1.mul(c.sub(Sf64(1.0))));
387            mut_self = mut_self.set(m + 1, i, val);
388            i += 1;
389        }
390        mut_self
391    }
392
393    #[must_use]
394    pub const fn givens_r(self, m: usize, a: Sf64, b: Sf64) -> Self {
395        let r = (a.powi(2).add(b.powi(2))).sqrt();
396        if eq(r, Sf64(0.0)) {
397            return self;
398        }
399
400        let mut mut_self = self;
401        let c = a.div(r);
402        let s = b.neg().div(r);
403        let mut i = 0;
404        while i < R {
405            let s0 = mut_self.get(i, m);
406            let s1 = mut_self.get(i, m + 1);
407
408            let val = mut_self.get(i, m).add(s0.mul(c.sub(Sf64(1.0))));
409            mut_self = mut_self.set(i, m, val);
410
411            let val = mut_self.get(i, m).add(s1.mul(s.neg()));
412            mut_self = mut_self.set(i, m, val);
413
414            let val = mut_self.get(i, m + 1).add(s0.mul(s));
415            mut_self = mut_self.set(i, m + 1, val);
416
417            let val = mut_self.get(i, m + 1).add(s1.mul(c.sub(Sf64(1.0))));
418            mut_self = mut_self.set(i, m + 1, val);
419            i += 1;
420        }
421        mut_self
422    }
423
424    const fn gemm<const M: usize>(
425        mut self,
426        a: &CMatrix<R, M>,
427        b: &CMatrix<M, C>,
428        alpha: f64,
429        beta: f64,
430    ) -> Self {
431        let alpha = Sf64(alpha);
432        let beta = Sf64(beta);
433        const fn beta_term(x: Sf64, beta: Sf64) -> Sf64 {
434            match is_zero(beta) {
435                true => Sf64(0.0),
436                false => beta.mul(x),
437            }
438        }
439        let mut n = 0;
440
441        while n < C {
442            let mut m = 0;
443            while m < R {
444                let mut axb = Sf64(0.0);
445                let mut k = 0;
446                while k < M {
447                    axb = axb.add(a.0[m][k].mul(b.0[k][n]));
448                    k += 1;
449                }
450                let b_term = beta_term(self.0[m][n], beta);
451                self.0[m][n] = alpha.mul(axb.add(b_term));
452                m += 1;
453            }
454            n += 1;
455        }
456        self
457    }
458
459    /// Singular Value Decomposition
460    pub const fn svd(self, epsilon: f64) -> CMatrix<C, R> {
461        const fn less_zero_sign(x: Sf64) -> Sf64 {
462            let Some(cmp) = x.cmp(Sf64(0.0)) else {
463                panic!("failed to get sign of value")
464            };
465            match cmp {
466                core::cmp::Ordering::Less => Sf64(-1.0),
467                _ => Sf64(1.0),
468            }
469        }
470
471        let dim = self.get_dims();
472
473        if self.rows() == self.columns() {
474            let mut s_working = CMatrix([[Sf64(0.0); R]; R]);
475            let mut u_working = CMatrix([[Sf64(0.0); R]; R]);
476            let mut v_working = CMatrix([[Sf64(0.0); R]; R]);
477
478            let mut u_out = CMatrix::new([[0.0; C]; C]);
479            let mut s_out = CMatrix::new([[0.0; R]; 1]);
480            let mut vt_out = CMatrix::new([[0.0; C]; R]);
481            {
482                let mut i = 0;
483                while i < R {
484                    let mut j = 0;
485                    while j < R {
486                        s_working.0[i][j] = self.0[i][j];
487                        j += 1;
488                    }
489                    i += 1;
490                }
491            }
492
493            {
494                let mut i = 0;
495                while i < R {
496                    u_working.0[i][i] = Sf64(1.0);
497                    i += 1;
498                }
499            }
500
501            {
502                let mut i = 0;
503                while i < R {
504                    v_working.0[i][i] = Sf64(1.0);
505                    i += 1;
506                }
507            }
508
509            (u_working, s_working, v_working) = Self::svd_inner(
510                self.get_dims(),
511                u_working,
512                s_working,
513                v_working,
514                Sf64(epsilon),
515            );
516
517            {
518                let mut i = 0;
519                while i < R {
520                    // Set S
521                    s_out.0[0][i] = s_working.get(i, i);
522                    i += 1;
523                }
524            }
525
526            let mut i = 0;
527            while i < C {
528                let mut j = 0;
529                while j < C {
530                    u_out.0[i][j] = v_working.get(i, j).mul(less_zero_sign(s_out.get(0, i)));
531                    j += 1;
532                }
533                i += 1;
534            }
535
536            // Set V
537            let mut i = 0;
538            while i < R {
539                let mut j = 0;
540                while j < dim[1] {
541                    vt_out.0[i][j] = u_working.0[i][j];
542                    j += 1
543                }
544                i += 1;
545            }
546            {
547                let mut i = 0;
548                while i < R {
549                    s_out.0[0][i] = s_out.get(0, i).mul(less_zero_sign(s_out.get(0, i)));
550                    i += 1;
551                }
552            }
553
554            // set all below epsilon to zero
555            let eps_ = Sf64(epsilon); //s_out[0] * epsilon;
556            let mut i = 0;
557            while i < R {
558                if lt(s_out.0[0][i], eps_) {
559                    s_out.0[0][i] = Sf64(0.0);
560                } else {
561                    s_out.0[0][i] = Sf64(1.0).div(s_out.0[0][i]);
562                }
563                i += 1;
564            }
565
566            let mut i = 0;
567            while i < C {
568                let mut j = 0;
569                while j < C {
570                    u_out.0[j][i] = u_out.0[j][i].mul(s_out.0[0][j]);
571                    j += 1;
572                }
573                i += 1;
574            }
575
576            self.gemm(&vt_out, &u_out, 1.0, 0.0).transpose()
577        } else {
578            panic!("Non-square matrices not yet supported");
579        }
580    }
581
582    #[must_use]
583    const fn svd_inner<const L: usize, const S: usize>(
584        dim: [usize; 2],
585        mut u_working: CMatrix<L, L>,
586        mut s_working: CMatrix<L, S>,
587        mut v_working: CMatrix<S, S>,
588        eps: Sf64,
589    ) -> (CMatrix<L, L>, CMatrix<L, S>, CMatrix<S, S>) {
590        let n = S;
591
592        let mut house_vec = [Sf64(0.0); L];
593        let mut i = 0;
594        while i < S {
595            // Column Householder
596            {
597                let x1 = abs(s_working.get(i, i));
598
599                let x_inv_norm = {
600                    let mut x_inv_norm = Sf64(0.0);
601                    let mut j = i;
602                    while j < dim[0] {
603                        x_inv_norm = x_inv_norm.add(s_working.get(j, i).powi(2));
604                        j += 1;
605                    }
606                    if gt(x_inv_norm, Sf64(0.0)) {
607                        x_inv_norm = Sf64(1.0).div(x_inv_norm.sqrt());
608                    }
609                    x_inv_norm
610                };
611
612                let (alpha, beta) = {
613                    let mut alpha = (Sf64(1.0).add(x1.mul(x_inv_norm))).sqrt();
614                    let beta = x_inv_norm.div(alpha);
615                    if eq(x_inv_norm, Sf64(0.0)) {
616                        alpha = Sf64(0.0);
617                    } // nothing to do
618                    (alpha, beta)
619                };
620
621                house_vec[i] = alpha.neg();
622                let mut j = i + 1;
623                while j < L {
624                    house_vec[j] = beta.neg().mul(s_working.get(j, i));
625                    j += 1;
626                }
627
628                if lt(s_working.get(i, i), Sf64(0.0)) {
629                    let mut j = i + 1;
630                    while j < dim[0] {
631                        house_vec[j] = house_vec[j].neg();
632                        j += 1;
633                    }
634                }
635            }
636
637            let mut k = i;
638            while k < dim[1] {
639                let mut dot_prod = Sf64(0.0);
640                let mut j = i;
641                while j < dim[0] {
642                    dot_prod = dot_prod.add(s_working.get(j, k).mul(house_vec[j]));
643                    j += 1;
644                }
645                let mut j = i;
646                while j < dim[0] {
647                    let val = s_working.get(j, k).sub(dot_prod.mul(house_vec[j]));
648                    s_working = s_working.set(j, k, val);
649                    j += 1;
650                }
651                k += 1;
652            }
653            let mut k = 0;
654            while k < dim[0] {
655                let mut dot_prod = Sf64(0.0);
656                let mut j = i;
657                while j < dim[0] {
658                    dot_prod = dot_prod.add(u_working.get(k, j).mul(house_vec[j]));
659                    j += 1;
660                }
661
662                let mut j = i;
663                while j < dim[0] {
664                    let val = u_working.get(k, j).sub(dot_prod.mul(house_vec[j]));
665                    u_working = u_working.set(k, j, val);
666                    j += 1;
667                }
668                k += 1;
669            }
670
671            if i >= n - 1 {
672                i += 1;
673                continue;
674            }
675
676            {
677                let x1 = abs(s_working.get(i, i + 1));
678
679                let x_inv_norm = {
680                    let mut x_inv_norm = Sf64(0.0);
681                    let mut j = i + 1;
682                    while j < dim[1] {
683                        x_inv_norm = x_inv_norm.add(s_working.get(i, j).powi(2));
684                        j += 1;
685                    }
686                    if gt(x_inv_norm, Sf64(0.0)) {
687                        x_inv_norm = Sf64(1.0).div(x_inv_norm.sqrt());
688                    }
689                    x_inv_norm
690                };
691
692                let (alpha, beta) = {
693                    let mut alpha = Sf64(1.0).add(x1.mul(x_inv_norm)).sqrt();
694                    let beta = x_inv_norm.div(alpha);
695                    if eq(x_inv_norm, Sf64(0.0)) {
696                        alpha = Sf64(0.0); // nothing to do
697                    }
698                    (alpha, beta)
699                };
700
701                house_vec[i + 1] = alpha.neg();
702                let mut j = i + 2;
703                while j < dim[1] {
704                    house_vec[j] = beta.neg().mul(s_working.get(i, j));
705                    j += 1;
706                }
707                if lt(s_working.get(i, i + 1), Sf64(0.0)) {
708                    let mut j = i + 2;
709                    while j < dim[1] {
710                        house_vec[j] = house_vec[j].neg();
711                        j += 1;
712                    }
713                }
714            }
715
716            let mut k = i;
717            while k < dim[0] {
718                let mut dot_prod = Sf64(0.0);
719                let mut j = i + 1;
720                while j < dim[1] {
721                    dot_prod = dot_prod.add(s_working.get(k, j).mul(house_vec[j]));
722                    j += 1;
723                }
724                let mut j = i + 1;
725                while j < dim[1] {
726                    let val = s_working.get(k, j).sub(dot_prod.mul(house_vec[j]));
727                    s_working = s_working.set(k, j, val);
728                    j += 1;
729                }
730                k += 1;
731            }
732
733            let mut k = 0;
734            while k < dim[1] {
735                let mut dot_prod = Sf64(0.0);
736                let mut j = i + 1;
737                while j < dim[1] {
738                    dot_prod = dot_prod.add(v_working.get(j, k).mul(house_vec[j]));
739                    j += 1;
740                }
741                let mut j = i + 1;
742                while j < dim[1] {
743                    let val = v_working.get(j, k).sub(dot_prod.mul(house_vec[j]));
744                    v_working = v_working.set(j, k, val);
745                    j += 1;
746                }
747                k += 1;
748            }
749
750            i += 1;
751        }
752
753        let eps = if lt(eps, Sf64(0.0)) {
754            let mut eps = Sf64(1.0);
755            while gt(eps.add(Sf64(1.0)), Sf64(1.0)) {
756                eps = eps.mul(Sf64(0.5));
757            }
758
759            eps = eps.mul(Sf64(64.0));
760            eps
761        } else {
762            eps
763        };
764
765        let mut k0 = 0;
766        while k0 < dim[1] - 1 {
767            // Diagonalization
768            let s_max = {
769                let mut s_max = Sf64(0.0);
770                let mut i = 0;
771                while i < dim[1] {
772                    let tmp = abs(s_working.get(i, i));
773
774                    if gt(tmp, s_max) {
775                        s_max = tmp;
776                    }
777                    i += 1;
778                }
779
780                let mut i = 0;
781                while i < (dim[1] - 1) {
782                    let tmp = abs(s_working.get(i, i + 1));
783                    if gt(tmp, s_max) {
784                        s_max = tmp
785                    }
786                    i += 1;
787                }
788                s_max
789            };
790
791            while (k0 < dim[1] - 1) && le(abs(s_working.get(k0, k0 + 1)), eps.mul(s_max)) {
792                k0 += 1;
793            }
794            if k0 == dim[1] - 1 {
795                continue;
796            }
797
798            let n = {
799                let mut n = k0 + 2;
800                while n < dim[1] && gt(abs(s_working.get(n - 1, n)), eps.mul(s_max)) {
801                    n += 1;
802                }
803                n
804            };
805
806            let (alpha, beta) = {
807                if n - k0 == 2
808                    && lt(abs(s_working.get(k0, k0)), eps.mul(s_max))
809                    && lt(abs(s_working.get(k0 + 1, k0 + 1)), eps.mul(s_max))
810                {
811                    // Compute mu
812                    (Sf64(0.0), Sf64(1.0))
813                } else {
814                    let mut c_vec = [Sf64(0.0); 4];
815                    c_vec[0 * 2] = s_working.get(n - 2, n - 2).mul(s_working.get(n - 2, n - 2));
816                    if n - k0 > 2 {
817                        c_vec[0 * 2] = c_vec[0 * 2]
818                            .add(s_working.get(n - 3, n - 2).mul(s_working.get(n - 3, n - 2)));
819                    }
820                    c_vec[1] = s_working.get(n - 2, n - 2).mul(s_working.get(n - 2, n - 1));
821                    c_vec[2] = s_working.get(n - 2, n - 2).mul(s_working.get(n - 2, n - 1));
822                    c_vec[2 + 1] = s_working
823                        .get(n - 1, n - 1)
824                        .mul(s_working.get(n - 1, n - 1))
825                        .add(s_working.get(n - 2, n - 1).mul(s_working.get(n - 2, n - 1)));
826
827                    let (b, d) = {
828                        let mut b = (c_vec[0 * 2].add(c_vec[2 + 1])).neg().div(Sf64(2.0));
829                        let mut c = c_vec[0 * 2].mul(c_vec[2 + 1]).sub(c_vec[1].mul(c_vec[2]));
830                        let mut d = Sf64(0.0);
831                        if gt(abs(b.powi(2).sub(c)), eps.mul(b.powi(2))) {
832                            d = (b.powi(2).sub(c)).sqrt();
833                        } else {
834                            b = c_vec[0 * 2].sub((c_vec[2 + 1]).div(Sf64(2.0)));
835                            c = c_vec[1].neg().mul(c_vec[2]);
836                            if gt(b.mul(b).sub(c), Sf64(0.0)) {
837                                d = (b.mul(b).sub(c)).sqrt();
838                            }
839                        }
840
841                        (b, d)
842                    };
843
844                    let lambda1 = b.neg().add(d);
845                    let lambda2 = b.neg().sub(d);
846
847                    let d1 = abs(lambda1.sub(c_vec[2 + 1]));
848                    let d2 = abs(lambda2.sub(c_vec[2 + 1]));
849                    let mu = if lt(d1, d2) { lambda1 } else { lambda2 };
850
851                    let alpha = s_working.get(k0, k0).powi(2).sub(mu);
852                    let beta = s_working.get(k0, k0).mul(s_working.get(k0, k0 + 1));
853
854                    (alpha, beta)
855                }
856            };
857            {
858                let mut alpha = alpha;
859                let mut beta = beta;
860                let mut k = k0;
861                while k < (n - 1) {
862                    s_working = s_working.givens_r(k, alpha, beta);
863                    v_working = v_working.givens_l(k, alpha, beta);
864
865                    alpha = s_working.get(k, k);
866                    beta = s_working.get(k + 1, k);
867                    s_working = s_working.givens_l(k, alpha, beta);
868                    u_working = u_working.givens_r(k, alpha, beta);
869
870                    alpha = s_working.get(k, k + 1);
871
872                    if k != n - 2 {
873                        beta = s_working.get(k, k + 2);
874                    }
875                    k += 1;
876                }
877            }
878
879            {
880                // Make S bi-diagonal again
881                let mut i0 = k0;
882                while i0 < (n - 1) {
883                    let mut i1 = 0;
884                    while i1 < dim[1] {
885                        if i0 > i1 || i0 + 1 < i1 {
886                            s_working = s_working.set(i0, i1, Sf64(0.0));
887                        }
888                        i1 += 1;
889                    }
890                    i0 += 1;
891                }
892                let mut i0 = 0;
893                while i0 < dim[0] {
894                    let mut i1 = k0;
895                    while i1 < (n - 1) {
896                        if i0 > i1 || i0 + 1 < i1 {
897                            s_working = s_working.set(i0, i1, Sf64(0.0));
898                        }
899                        i1 += 1;
900                    }
901                    i0 += 1;
902                }
903                let mut i = 0;
904                while i < (dim[1] - 1) {
905                    if le(abs(s_working.get(i, i + 1)), eps.mul(s_max)) {
906                        s_working = s_working.set(i, i + 1, Sf64(0.0));
907                    }
908                    i += 1;
909                }
910            }
911        }
912        (u_working, s_working, v_working)
913    }
914
915    pub const fn pinv(self, epsilon: f64) -> CMatrix<C, R> {
916        if self.rows() * self.columns() == 0 {
917            return self.transpose();
918        }
919
920        self.svd(epsilon)
921    }
922}
923
924impl<const N: usize> CMatrix<1, N> {
925    /// Special case of `CMatrix::new` for constructing a CVector
926    /// Always returns a row vector, follow with `transpose` to build
927    /// a column vector
928    /// ```
929    /// # use constgebra::CVector;
930    /// const ARRAY: [f64; 2] = [4.0, 7.0];
931    ///
932    /// const ROWVECTOR: CVector::<2> = CVector::new_vector(ARRAY);
933    pub const fn new_vector(vals: [f64; N]) -> CVector<N> {
934        CMatrix::new([vals])
935    }
936
937    pub const fn new_vector_from_soft(vals: [Sf64; N]) -> Self {
938        CMatrix::new_from_soft([vals])
939    }
940
941    /// Dot product of two `CVector` of the same size.
942    /// ```rust
943    /// # use constgebra::{CVector, const_soft_float::soft_f64::SoftF64 as Sf64};
944    /// const LEFT: CVector<3> = CVector::new_vector([1.0, 3.0, -5.0]);
945    /// const RIGHT: CVector<3> = CVector::new_vector([4.0, -2.0, -1.0]);
946    ///
947    /// const EXPECTED: f64 = 3.0;
948    /// const RESULT: f64 = LEFT.dot(RIGHT);
949    ///
950    /// assert_eq!(EXPECTED, RESULT)
951    /// ```
952    pub const fn dot(self, other: Self) -> f64 {
953        self.mul(other.transpose()).get(0, 0).to_f64()
954    }
955
956    /// Special case of `CMatrix::finish` for use with a CVector,
957    /// returns `[f64 ; N]` instead of `[[f64 ; N]; 1]`
958    /// ```rust
959    /// # use constgebra::CVector;
960    /// const ARRAY: [f64; 2] = [4.0, 7.0];
961    ///
962    /// const CVECTOR: CVector::<2> = CVector::new_vector(ARRAY);
963    ///
964    /// const RESULT: [f64; 2] = CVECTOR.finish_vector();
965    ///
966    /// assert_eq!(ARRAY, RESULT)
967    /// ```
968    pub const fn finish_vector(self) -> [f64; N] {
969        self.finish()[0]
970    }
971
972    /// `CVector::finish_vector`, but returns soft floats
973    pub const fn finish_vector_soft(self) -> [Sf64; N] {
974        self.finish_soft()[0]
975    }
976}
977
978#[cfg(unused)]
979const fn panic_if_ne(val: Sf64, test: f64) {
980    if gt(abs(val.sub(Sf64(test))), Sf64(0.00001)) {
981        panic!();
982    }
983}
984
985const fn is_zero(mut arg: Sf64) -> bool {
986    let mut i = 0;
987    while i < 64 {
988        if arg.0 as u64 != 0 {
989            return false;
990        }
991        arg = arg.mul(Sf64(2.0));
992        i += 1;
993    }
994    true
995}
996
997const fn abs(x: Sf64) -> Sf64 {
998    let Some(cmp) = x.cmp(Sf64(0.0)) else {
999        panic!("failed to get sign of value")
1000    };
1001    match cmp {
1002        core::cmp::Ordering::Less => x.neg(),
1003        _ => x,
1004    }
1005}
1006
1007const fn gt(x: Sf64, y: Sf64) -> bool {
1008    let Some(cmp) = x.cmp(y) else {
1009        panic!("failed to compare values")
1010    };
1011    matches!(cmp, core::cmp::Ordering::Greater)
1012}
1013
1014const fn lt(x: Sf64, y: Sf64) -> bool {
1015    let Some(cmp) = x.cmp(y) else {
1016        panic!("failed to compare values")
1017    };
1018    matches!(cmp, core::cmp::Ordering::Less)
1019}
1020
1021const fn le(x: Sf64, y: Sf64) -> bool {
1022    let Some(cmp) = x.cmp(y) else {
1023        panic!("failed to compare values")
1024    };
1025    matches!(cmp, core::cmp::Ordering::Less | core::cmp::Ordering::Equal)
1026}
1027
1028const fn eq(x: Sf64, y: Sf64) -> bool {
1029    let Some(cmp) = x.cmp(y) else {
1030        panic!("failed to compare values")
1031    };
1032    matches!(cmp, core::cmp::Ordering::Equal)
1033}
1034
1035#[cfg(test)]
1036mod const_tests {
1037    use super::*;
1038
1039    fn float_equal(one: f64, two: f64, eps: f64) -> bool {
1040        match (one, two) {
1041            (a, b) if (a - b).abs() < eps => true,
1042            (a, b) if a == -0.0 || b == -0.0 => a + b == 0.0 || -(a + b) == 0.0,
1043            _ => false,
1044        }
1045    }
1046
1047    #[test]
1048    fn test_cvec() {
1049        const VEC_BASE: CVector<3> = CVector::new_vector([1.0, 2.0, 3.0]);
1050        const MAT_BASE: CMatrix<1, 3> = CMatrix::new([[1.0, 2.0, 3.0]]);
1051
1052        assert_eq!(VEC_BASE.finish(), MAT_BASE.finish());
1053
1054        const ADDED: CVector<3> = VEC_BASE.add(MAT_BASE);
1055        const DIVIDED: CVector<3> = ADDED.apply_each(Operation::Div(3.0));
1056        assert_eq!(DIVIDED.finish_vector()[2], 2.0)
1057    }
1058
1059    #[test]
1060    fn test_apply_each() {
1061        const VEC_BASE: CVector<3> = CVector::new_vector([1.0, 2.0, 3.0]);
1062        const MAT_BASE: CMatrix<1, 3> = CMatrix::new([[1.0, 2.0, 3.0]]);
1063
1064        assert_eq!(VEC_BASE.finish(), MAT_BASE.finish());
1065
1066        const ADDED: CVector<3> = VEC_BASE.add(MAT_BASE);
1067        const DIVIDED: CVector<3> = ADDED.apply_each(Operation::Div(3.0));
1068        assert_eq!(DIVIDED.finish_vector()[2], 2.0)
1069    }
1070
1071    #[test]
1072    fn test_dot_product_normal() {
1073        const VEC1: CVector<3> = CVector::new_vector([2.0, 3.0, 4.0]);
1074        const VEC2: CVector<3> = CVector::new_vector([1.0, 5.0, 7.0]);
1075        const EXPECTED: f64 = 2.0 * 1.0 + 3.0 * 5.0 + 4.0 * 7.0;
1076        const RESULT: f64 = VEC1.dot(VEC2);
1077        assert_eq!(EXPECTED, RESULT);
1078    }
1079
1080    #[test]
1081    fn test_dot_product_zero_vector() {
1082        const VEC1: CVector<3> = CVector::new_vector([0.0, 0.0, 0.0]);
1083        const VEC2: CVector<3> = CVector::new_vector([1.0, 5.0, 7.0]);
1084        const EXPECTED: f64 = 0.0;
1085        const RESULT: f64 = VEC1.dot(VEC2);
1086        assert_eq!(EXPECTED, RESULT);
1087    }
1088
1089    #[test]
1090    fn test_dot_product_with_negatives() {
1091        const VEC1: CVector<3> = CVector::new_vector([-1.0, -3.0, 5.0]);
1092        const VEC2: CVector<3> = CVector::new_vector([4.0, -2.0, -1.0]);
1093        const EXPECTED: f64 = (-1.0) * 4.0 + (-3.0) * (-2.0) + 5.0 * (-1.0);
1094        const RESULT: f64 = VEC1.dot(VEC2);
1095        assert_eq!(EXPECTED, RESULT);
1096    }
1097
1098    #[test]
1099    fn test_dot_product_large_values() {
1100        const VEC1: CVector<3> = CVector::new_vector([1000000.0, 3000000.0, -5000000.0]);
1101        const VEC2: CVector<3> = CVector::new_vector([4000000.0, -2000000.0, -1000000.0]);
1102        const EXPECTED: f64 =
1103            1000000.0 * 4000000.0 + 3000000.0 * (-2000000.0) + (-5000000.0) * (-1000000.0);
1104        const RESULT: f64 = VEC1.dot(VEC2);
1105        assert_eq!(EXPECTED, RESULT);
1106    }
1107
1108    #[test]
1109    fn test_dot_product_identity() {
1110        const VEC1: CVector<3> = CVector::new_vector([1.0, 0.0, 0.0]);
1111        const VEC2: CVector<3> = CVector::new_vector([0.0, 1.0, 0.0]);
1112        const EXPECTED: f64 = 0.0;
1113        const RESULT: f64 = VEC1.dot(VEC2);
1114        assert_eq!(EXPECTED, RESULT);
1115    }
1116
1117    #[test]
1118    fn test_2_x_2_example() {
1119        const START: CMatrix<2, 2> = CMatrix::new([[4.0, 1.0], [2.0, 3.0]]);
1120        const ADD: CMatrix<2, 2> = CMatrix::new([[0.0, 6.0], [0.0, 3.0]]);
1121        const EXPECTED: [[f64; 2]; 2] = [[0.6, -0.7], [-0.2, 0.4]];
1122
1123        const RESULT: [[f64; 2]; 2] = START.add(ADD).pinv(f64::EPSILON).finish();
1124        for i in 0..2 {
1125            for j in 0..2 {
1126                assert!(float_equal(RESULT[i][j], EXPECTED[i][j], 1e-5));
1127            }
1128        }
1129    }
1130
1131    #[test]
1132    fn test_2_x_2_invert() {
1133        const START: [[f64; 2]; 2] = [[4.0, 7.0], [2.0, 6.0]];
1134        const EXPECTED: [[f64; 2]; 2] = [[0.6, -0.7], [-0.2, 0.4]];
1135
1136        const RESULT: [[f64; 2]; 2] = CMatrix::new(START).pinv(f64::EPSILON).finish();
1137        for i in 0..2 {
1138            for j in 0..2 {
1139                assert!(float_equal(RESULT[i][j], EXPECTED[i][j], 1e-5));
1140            }
1141        }
1142    }
1143
1144    #[test]
1145    fn check_4_x_4() {
1146        const START: [[f64; 4]; 4] = [
1147            [13.0, 17.0, 25.0, 12.0],
1148            [19.0, 24.0, 16.0, 21.0],
1149            [29.0, 9.0, 3.0, 14.0],
1150            [23.0, 27.0, 20.0, 15.0],
1151        ];
1152        const EXPECTED: [[f64; 4]; 4] = [
1153            [
1154                0.005_304_652_520_926_611,
1155                -0.053_014_080_851_339_955,
1156                0.043_653_883_589_643_76,
1157                0.029_232_366_491_467_134,
1158            ],
1159            [
1160                -0.072_318_473_817_403_15,
1161                0.004_087_989_098_695_737,
1162                -0.044_675_880_864_317_695,
1163                0.093_829_083_122_445,
1164            ],
1165            [
1166                0.077_331_127_116_994_36,
1167                -0.029_719_031_860_359_485,
1168                0.003_357_991_045_357_212,
1169                -0.023_392_382_064_758_938,
1170            ],
1171            [
1172                0.018_931_282_849_912_4,
1173                0.113_555_252_741_548_25,
1174                0.009_003_309_324_508_468,
1175                -0.115_858_802_154_305_37,
1176            ],
1177        ];
1178        const INVERSE: [[f64; 4]; 4] = CMatrix::new(START).pinv(f64::EPSILON).finish();
1179        for i in 0..4 {
1180            for j in 0..4 {
1181                assert!(float_equal(INVERSE[i][j], EXPECTED[i][j], 1e-5))
1182            }
1183        }
1184    }
1185
1186    #[test]
1187    fn check_8_x_8() {
1188        #[rustfmt::skip]
1189        const START: [[f64;8];8] = [
1190            [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
1191            [0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
1192            [0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0.0],
1193            [0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 0.0],
1194            [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
1195            [0.0, 0.0, 0.0, 0.0, 0.0, 400000000.0, 0.0, 0.0],
1196            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 400000000.0, 0.0],
1197            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 400000000.0],
1198        ];
1199
1200        #[rustfmt::skip]
1201        const EXPECTED: [[f64;8];8] = [
1202            [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
1203            [0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
1204            [0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
1205            [0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0],
1206            [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
1207            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0000000025, 0.0, 0.0],
1208            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0000000025, 0.0],
1209            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0000000025],
1210        ];
1211
1212        const INVERSE: [[f64; 8]; 8] = CMatrix::new(START).pinv(f64::EPSILON).finish();
1213        for i in 0..8 {
1214            for j in 0..8 {
1215                assert!(float_equal(INVERSE[i][j], EXPECTED[i][j], 1e-5))
1216            }
1217        }
1218    }
1219}
constgebra/comptime.rs

constgebra/
comptime.rs
