1#![no_std]
2#![forbid(unsafe_code)]
3#![allow(clippy::needless_range_loop)]
6extern crate alloc;
41
42use core::fmt;
43use core::ops::{Add, Div, Index, Mul, Neg, Sub};
44
45use tpt_math_numeric::Scalar;
46
47#[cfg(feature = "alloc")]
48use alloc::vec::Vec;
49
50#[cfg(feature = "alloc")]
51use alloc::{format, string::String, vec};
52
53#[derive(Clone)]
55pub struct DVector<T = f64> {
56 data: Vec<T>,
57}
58
59#[derive(Clone)]
62pub struct DMatrix<T = f64> {
63 nrows: usize,
64 ncols: usize,
65 data: Vec<T>,
66}
67
68#[cfg(feature = "alloc")]
73impl<T> DVector<T> {
74 pub fn zeros(n: usize) -> Self
76 where
77 T: Scalar,
78 {
79 DVector::from_fn(n, |_| T::zero())
80 }
81
82 pub fn from_vec(data: Vec<T>) -> Self {
84 DVector { data }
85 }
86
87 pub fn from_row_slice(data: &[T]) -> Self
89 where
90 T: Clone,
91 {
92 DVector {
93 data: data.to_vec(),
94 }
95 }
96
97 pub fn from_fn(n: usize, f: impl FnMut(usize) -> T) -> Self {
99 DVector {
100 data: (0..n).map(f).collect(),
101 }
102 }
103}
104
105#[cfg(feature = "alloc")]
106impl<T> DMatrix<T> {
107 pub fn zeros(nrows: usize, ncols: usize) -> Self
109 where
110 T: Scalar,
111 {
112 DMatrix::from_fn(nrows, ncols, |_, _| T::zero())
113 }
114
115 pub fn from_vec(nrows: usize, ncols: usize, data: Vec<T>) -> Self {
118 DMatrix { nrows, ncols, data }
119 }
120
121 pub fn from_row_slice(nrows: usize, ncols: usize, data: &[T]) -> Self
123 where
124 T: Clone,
125 {
126 DMatrix {
127 nrows,
128 ncols,
129 data: (0..ncols)
130 .flat_map(|j| (0..nrows).map(move |i| data[i * ncols + j].clone()))
131 .collect(),
132 }
133 }
134
135 pub fn from_fn(nrows: usize, ncols: usize, mut f: impl FnMut(usize, usize) -> T) -> Self {
137 let mut data = Vec::with_capacity(nrows * ncols);
138 for j in 0..ncols {
139 for i in 0..nrows {
140 data.push(f(i, j));
141 }
142 }
143 DMatrix { nrows, ncols, data }
144 }
145
146 pub fn from_diagonal(v: &DVector<T>) -> Self
148 where
149 T: Scalar + Clone,
150 {
151 let n = v.len();
152 DMatrix::from_fn(n, n, |i, j| if i == j { v[i] } else { T::zero() })
153 }
154}
155
156impl<T> DVector<T> {
161 pub fn len(&self) -> usize {
163 self.data.len()
164 }
165
166 pub fn is_empty(&self) -> bool {
168 self.data.is_empty()
169 }
170
171 pub fn iter(&self) -> impl Iterator<Item = &T> {
173 self.data.iter()
174 }
175}
176
177impl<T> DMatrix<T> {
178 pub fn nrows(&self) -> usize {
180 self.nrows
181 }
182
183 pub fn ncols(&self) -> usize {
185 self.ncols
186 }
187
188 pub fn is_empty(&self) -> bool {
190 self.nrows == 0 || self.ncols == 0
191 }
192
193 pub fn iter(&self) -> impl Iterator<Item = &T> {
195 self.data.iter()
196 }
197
198 fn offset(&self, i: usize, j: usize) -> usize {
200 i + j * self.nrows
201 }
202}
203
204impl<T> Index<usize> for DVector<T> {
209 type Output = T;
210 fn index(&self, i: usize) -> &T {
214 &self.data[i]
215 }
216}
217
218impl<T> Index<(usize, usize)> for DMatrix<T> {
219 type Output = T;
220 fn index(&self, (i, j): (usize, usize)) -> &T {
224 &self.data[self.offset(i, j)]
225 }
226}
227
228impl<T: Scalar> DVector<T> {
233 pub fn norm(&self) -> T {
235 let s = self.iter().fold(T::zero(), |acc, x| acc + (*x) * (*x));
236 s.sqrt()
237 }
238
239 pub fn dot(&self, other: &DVector<T>) -> T {
241 self.iter()
242 .zip(other.iter())
243 .fold(T::zero(), |acc, (a, b)| acc + (*a) * (*b))
244 }
245}
246
247#[cfg(feature = "alloc")]
252impl<T: Scalar + Clone> DVector<T> {
253 pub fn transpose(&self) -> DMatrix<T> {
255 let n = self.len();
256 DMatrix::from_fn(1, n, |_, j| self[j])
257 }
258}
259
260#[cfg(feature = "alloc")]
261impl<T: Scalar + Clone> DMatrix<T> {
262 pub fn transpose(&self) -> DMatrix<T> {
264 let (m, n) = (self.nrows, self.ncols);
265 DMatrix::from_fn(n, m, |i, j| self[(j, i)])
266 }
267}
268
269#[cfg(feature = "alloc")]
274impl<T: Scalar> Add for DVector<T> {
275 type Output = DVector<T>;
276 fn add(self, rhs: DVector<T>) -> DVector<T> {
277 let n = self.len();
278 DVector::from_fn(n, |i| self[i] + rhs[i])
279 }
280}
281
282#[cfg(feature = "alloc")]
283impl<T: Scalar> Sub for DVector<T> {
284 type Output = DVector<T>;
285 fn sub(self, rhs: DVector<T>) -> DVector<T> {
286 let n = self.len();
287 DVector::from_fn(n, |i| self[i] - rhs[i])
288 }
289}
290
291#[cfg(feature = "alloc")]
292impl<T: Scalar> Neg for DVector<T> {
293 type Output = DVector<T>;
294 fn neg(self) -> DVector<T> {
295 let n = self.len();
296 DVector::from_fn(n, |i| -self[i])
297 }
298}
299
300#[cfg(feature = "alloc")]
301impl<T: Scalar> Mul<T> for DVector<T> {
302 type Output = DVector<T>;
303 fn mul(self, rhs: T) -> DVector<T> {
304 let n = self.len();
305 DVector::from_fn(n, |i| self[i] * rhs)
306 }
307}
308
309#[cfg(feature = "alloc")]
310impl<T: Scalar> Div<T> for DVector<T> {
311 type Output = DVector<T>;
312 fn div(self, rhs: T) -> DVector<T> {
313 let n = self.len();
314 DVector::from_fn(n, |i| self[i] / rhs)
315 }
316}
317
318#[cfg(feature = "alloc")]
319impl<T: Scalar> Add<T> for DVector<T> {
320 type Output = DVector<T>;
321 fn add(self, rhs: T) -> DVector<T> {
322 let n = self.len();
323 DVector::from_fn(n, |i| self[i] + rhs)
324 }
325}
326
327#[cfg(feature = "alloc")]
328impl<T: Scalar> Sub<T> for DVector<T> {
329 type Output = DVector<T>;
330 fn sub(self, rhs: T) -> DVector<T> {
331 let n = self.len();
332 DVector::from_fn(n, |i| self[i] - rhs)
333 }
334}
335
336#[cfg(feature = "alloc")]
337impl<T: Scalar> Add for DMatrix<T> {
338 type Output = DMatrix<T>;
339 fn add(self, rhs: DMatrix<T>) -> DMatrix<T> {
340 let (m, n) = (self.nrows, self.ncols);
341 DMatrix::from_fn(m, n, |i, j| self[(i, j)] + rhs[(i, j)])
342 }
343}
344
345#[cfg(feature = "alloc")]
346impl<T: Scalar> Sub for DMatrix<T> {
347 type Output = DMatrix<T>;
348 fn sub(self, rhs: DMatrix<T>) -> DMatrix<T> {
349 let (m, n) = (self.nrows, self.ncols);
350 DMatrix::from_fn(m, n, |i, j| self[(i, j)] - rhs[(i, j)])
351 }
352}
353
354#[cfg(feature = "alloc")]
355impl<T: Scalar> Neg for DMatrix<T> {
356 type Output = DMatrix<T>;
357 fn neg(self) -> DMatrix<T> {
358 let (m, n) = (self.nrows, self.ncols);
359 DMatrix::from_fn(m, n, |i, j| -self[(i, j)])
360 }
361}
362
363#[cfg(feature = "alloc")]
364impl<T: Scalar> Mul<T> for DMatrix<T> {
365 type Output = DMatrix<T>;
366 fn mul(self, rhs: T) -> DMatrix<T> {
367 let (m, n) = (self.nrows, self.ncols);
368 DMatrix::from_fn(m, n, |i, j| self[(i, j)] * rhs)
369 }
370}
371
372#[cfg(feature = "alloc")]
373impl<T: Scalar> Div<T> for DMatrix<T> {
374 type Output = DMatrix<T>;
375 fn div(self, rhs: T) -> DMatrix<T> {
376 let (m, n) = (self.nrows, self.ncols);
377 DMatrix::from_fn(m, n, |i, j| self[(i, j)] / rhs)
378 }
379}
380
381#[cfg(feature = "alloc")]
382impl<T: Scalar> Add<T> for DMatrix<T> {
383 type Output = DMatrix<T>;
384 fn add(self, rhs: T) -> DMatrix<T> {
385 let (m, n) = (self.nrows, self.ncols);
386 DMatrix::from_fn(m, n, |i, j| self[(i, j)] + rhs)
387 }
388}
389
390#[cfg(feature = "alloc")]
391impl<T: Scalar> Sub<T> for DMatrix<T> {
392 type Output = DMatrix<T>;
393 fn sub(self, rhs: T) -> DMatrix<T> {
394 let (m, n) = (self.nrows, self.ncols);
395 DMatrix::from_fn(m, n, |i, j| self[(i, j)] - rhs)
396 }
397}
398
399#[cfg(feature = "alloc")]
404impl<T: Scalar> Mul<DMatrix<T>> for DMatrix<T> {
405 type Output = DMatrix<T>;
406 fn mul(self, rhs: DMatrix<T>) -> DMatrix<T> {
410 let m = self.nrows;
411 let k = self.ncols;
412 let n = rhs.ncols;
413 DMatrix::from_fn(m, n, |i, j| {
414 let mut s = T::zero();
415 for kk in 0..k {
416 s = s + self[(i, kk)] * rhs[(kk, j)];
417 }
418 s
419 })
420 }
421}
422
423#[cfg(feature = "alloc")]
424impl<T: Scalar> Mul<DVector<T>> for DMatrix<T> {
425 type Output = DVector<T>;
426 fn mul(self, rhs: DVector<T>) -> DVector<T> {
430 let m = self.nrows;
431 let k = self.ncols;
432 DVector::from_fn(m, |i| {
433 let mut s = T::zero();
434 for kk in 0..k {
435 s = s + self[(i, kk)] * rhs[kk];
436 }
437 s
438 })
439 }
440}
441
442impl<T: PartialEq> PartialEq for DVector<T> {
447 fn eq(&self, other: &Self) -> bool {
448 self.data == other.data
449 }
450}
451
452impl<T: PartialEq> PartialEq for DMatrix<T> {
453 fn eq(&self, other: &Self) -> bool {
454 self.nrows == other.nrows && self.ncols == other.ncols && self.data == other.data
455 }
456}
457
458impl<T: fmt::Debug> fmt::Debug for DVector<T> {
459 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
460 f.debug_list().entries(self.iter()).finish()
461 }
462}
463
464impl<T: fmt::Debug> fmt::Debug for DMatrix<T> {
465 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
466 f.debug_struct("DMatrix")
467 .field("nrows", &self.nrows)
468 .field("ncols", &self.ncols)
469 .finish()
470 }
471}
472
473#[cfg(feature = "alloc")]
479#[derive(Debug, Clone, PartialEq)]
480pub enum DenseError {
481 Singular {
483 what: &'static str,
485 },
486 DimensionMismatch {
488 what: String,
490 },
491}
492
493#[cfg(feature = "alloc")]
494impl fmt::Display for DenseError {
495 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
496 match self {
497 DenseError::Singular { what } => write!(f, "singular matrix in {what}"),
498 DenseError::DimensionMismatch { what } => write!(f, "dimension mismatch: {what}"),
499 }
500 }
501}
502
503#[cfg(feature = "alloc")]
504impl core::error::Error for DenseError {}
505
506#[cfg(feature = "alloc")]
507impl DMatrix<f64> {
508 pub fn solve(&self, b: &DVector<f64>) -> Result<DVector<f64>, DenseError> {
511 let n = self.nrows;
512 if self.ncols != n {
513 return Err(DenseError::DimensionMismatch {
514 what: format!("matrix is {}x{}, expected square", n, self.ncols),
515 });
516 }
517 if b.len() != n {
518 return Err(DenseError::DimensionMismatch {
519 what: format!("rhs has length {}, expected {n}", b.len()),
520 });
521 }
522
523 let (lu, piv) = partial_pivot_lu(self)?;
524 let x = solve_with_lu(&lu, &piv, b);
525 Ok(DVector::from_vec(x))
526 }
527
528 pub fn inverse(&self) -> Result<DMatrix<f64>, DenseError> {
530 let n = self.nrows;
531 if self.ncols != n {
532 return Err(DenseError::DimensionMismatch {
533 what: format!("matrix is {}x{}, expected square", n, self.ncols),
534 });
535 }
536 let (lu, piv) = partial_pivot_lu(self)?;
537 let mut out = Vec::with_capacity(n * n);
538 for col in 0..n {
539 let mut e = vec![0.0_f64; n];
540 e[col] = 1.0;
541 let x = solve_with_lu(&lu, &piv, &DVector::from_vec(e));
542 out.extend(x);
543 }
544 Ok(DMatrix::from_vec(n, n, out))
545 }
546}
547
548#[cfg(feature = "alloc")]
550fn partial_pivot_lu(m: &DMatrix<f64>) -> Result<(Vec<Vec<f64>>, Vec<usize>), DenseError> {
551 let n = m.nrows;
552 let mut a: Vec<Vec<f64>> = (0..n)
553 .map(|i| (0..n).map(|j| m[(i, j)]).collect())
554 .collect();
555 let mut piv: Vec<usize> = (0..n).collect();
556
557 for k in 0..n {
558 let mut p = k;
559 let mut max = a[k][k].abs();
560 for i in (k + 1)..n {
561 let v = a[i][k].abs();
562 if v > max {
563 max = v;
564 p = i;
565 }
566 }
567 if !max.is_finite() || max < 1e-12 {
568 return Err(DenseError::Singular {
569 what: "partial_pivot_lu",
570 });
571 }
572 a.swap(k, p);
573 piv.swap(k, p);
574 for i in (k + 1)..n {
575 a[i][k] /= a[k][k];
576 for j in (k + 1)..n {
577 a[i][j] -= a[i][k] * a[k][j];
578 }
579 }
580 }
581 Ok((a, piv))
582}
583
584#[cfg(feature = "alloc")]
586fn solve_with_lu(lu: &[Vec<f64>], piv: &[usize], b: &DVector<f64>) -> Vec<f64> {
587 let n = lu.len();
588 let mut y = vec![0.0_f64; n];
590 for i in 0..n {
591 let mut s = b[piv[i]];
592 for j in 0..i {
593 s -= lu[i][j] * y[j];
594 }
595 y[i] = s;
596 }
597 let mut x = vec![0.0_f64; n];
599 for i in (0..n).rev() {
600 let mut s = y[i];
601 for j in (i + 1)..n {
602 s -= lu[i][j] * x[j];
603 }
604 x[i] = s / lu[i][i];
605 }
606 x
607}
608
609#[cfg(all(test, feature = "alloc"))]
610mod tests {
611 use super::*;
612
613 #[test]
614 fn vector_construction_and_index() {
615 let v = DVector::from_vec(vec![3.0_f64, 1.0, 4.0]);
616 assert_eq!(v.len(), 3);
617 assert_eq!(v[0], 3.0);
618 assert_eq!(v[2], 4.0);
619 }
620
621 #[test]
622 fn vector_add_sub_neg_scalar() {
623 let a = DVector::from_vec(vec![1.0_f64, 2.0, 3.0]);
624 let b = DVector::from_vec(vec![4.0_f64, 5.0, 6.0]);
625 let s = a.clone() + b.clone();
626 assert_eq!(s, DVector::from_vec(vec![5.0, 7.0, 9.0]));
627 let d = b.clone() - a.clone();
628 assert_eq!(d, DVector::from_vec(vec![3.0, 3.0, 3.0]));
629 let n = -a.clone();
630 assert_eq!(n, DVector::from_vec(vec![-1.0, -2.0, -3.0]));
631 let sc = a * 2.0;
632 assert_eq!(sc, DVector::from_vec(vec![2.0, 4.0, 6.0]));
633 }
634
635 #[test]
636 fn vector_dot_and_norm() {
637 let a = DVector::from_vec(vec![1.0_f64, 2.0, 3.0]);
638 let b = DVector::from_vec(vec![4.0_f64, 5.0, 6.0]);
639 assert!((a.dot(&b) - 32.0).abs() < 1e-12);
640 assert!((a.norm() - 14.0_f64.sqrt()).abs() < 1e-12);
641 }
642
643 #[test]
644 fn matrix_construction_and_arithmetic() {
645 let m = DMatrix::from_row_slice(2, 2, &[1.0_f64, 2.0, 3.0, 4.0]);
646 assert_eq!(m.nrows(), 2);
647 assert_eq!(m.ncols(), 2);
648 assert_eq!(m[(0, 0)], 1.0);
649 assert_eq!(m[(1, 1)], 4.0);
650 let z = DMatrix::zeros(2, 2);
651 assert_eq!(m.clone() - m.clone(), z);
652 }
653
654 #[test]
655 fn matrix_vector_and_matrix_matrix() {
656 let m = DMatrix::from_row_slice(2, 2, &[1.0_f64, 2.0, 3.0, 4.0]);
657 let v = DVector::from_vec(vec![1.0_f64, 1.0]);
658 let mv = m.clone() * v;
659 assert_eq!(mv, DVector::from_vec(vec![3.0, 7.0]));
660
661 let n = DMatrix::from_row_slice(2, 2, &[0.0_f64, 1.0, 1.0, 0.0]);
662 let mm = m.clone() * n;
663 assert_eq!(mm, DMatrix::from_row_slice(2, 2, &[2.0_f64, 1.0, 4.0, 3.0]));
664 }
665
666 #[test]
667 fn transpose_swaps_dimensions() {
668 let m = DMatrix::from_row_slice(2, 3, &[1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]);
669 let t = m.transpose();
670 assert_eq!(t.nrows(), 3);
671 assert_eq!(t.ncols(), 2);
672 assert_eq!(t[(0, 1)], 4.0);
673 }
674
675 #[test]
676 fn solve_and_inverse() {
677 let a = DMatrix::from_row_slice(2, 2, &[2.0_f64, 0.0, 0.0, 2.0]);
679 let inv = a.inverse().unwrap();
680 assert!((inv[(0, 0)] - 0.5).abs() < 1e-12);
681 assert!((inv[(1, 1)] - 0.5).abs() < 1e-12);
682
683 let b = DVector::from_vec(vec![4.0_f64, 6.0]);
684 let x = a.solve(&b).unwrap();
685 assert_eq!(x, DVector::from_vec(vec![2.0, 3.0]));
686 }
687
688 #[test]
689 fn singular_matrix_is_rejected() {
690 let a = DMatrix::from_row_slice(2, 2, &[1.0_f64, 1.0, 1.0, 1.0]);
691 assert!(a.inverse().is_err());
692 assert!(a.solve(&DVector::from_vec(vec![1.0, 1.0])).is_err());
693 }
694
695 #[test]
696 fn from_diagonal() {
697 let d = DMatrix::from_diagonal(&DVector::from_vec(vec![2.0_f64, 3.0, 4.0]));
698 assert_eq!(d[(0, 0)], 2.0);
699 assert_eq!(d[(1, 1)], 3.0);
700 assert_eq!(d[(2, 2)], 4.0);
701 assert_eq!(d[(0, 1)], 0.0);
702 }
703}