1#![no_std]
2#![allow(clippy::wrong_self_convention)]
3pub use tpt_math_linalg_dense;
49pub use tpt_math_units;
50
51use core::marker::PhantomData;
52use core::ops::{Add, Div, Mul, Neg, Sub};
53
54use tpt_math_linalg_dense::{DMatrix, DVector};
55use tpt_math_numeric::Scalar;
56
57#[derive(Clone, Debug, PartialEq)]
62pub struct Vec<U, T = f64> {
63 raw: DVector<T>,
64 _unit: PhantomData<U>,
65}
66
67#[derive(Clone, Debug, PartialEq)]
72pub struct Mat<U, V, T = f64> {
73 raw: DMatrix<T>,
74 _unit: PhantomData<(U, V)>,
75}
76
77impl<U, T> Vec<U, T> {
78 pub fn from_raw(raw: DVector<T>) -> Self {
80 Vec {
81 raw,
82 _unit: PhantomData,
83 }
84 }
85
86 pub fn raw(&self) -> &DVector<T> {
88 &self.raw
89 }
90
91 pub fn len(&self) -> usize {
93 self.raw.len()
94 }
95
96 pub fn is_empty(&self) -> bool {
98 self.raw.is_empty()
99 }
100}
101
102impl<U, V, T> Mat<U, V, T> {
103 pub fn from_raw(raw: DMatrix<T>) -> Self {
106 Mat {
107 raw,
108 _unit: PhantomData,
109 }
110 }
111
112 pub fn raw(&self) -> &DMatrix<T> {
114 &self.raw
115 }
116
117 pub fn nrows(&self) -> usize {
119 self.raw.nrows()
120 }
121
122 pub fn ncols(&self) -> usize {
124 self.raw.ncols()
125 }
126}
127
128#[cfg(feature = "alloc")]
129impl<U, T: Clone> Vec<U, T> {
130 pub fn zeros(n: usize) -> Self
132 where
133 T: Scalar,
134 {
135 Vec::from_raw(DVector::zeros(n))
136 }
137}
138
139#[cfg(feature = "alloc")]
140impl<U, V, T: Clone> Mat<U, V, T> {
141 pub fn zeros(rows: usize, cols: usize) -> Self
143 where
144 T: Scalar,
145 {
146 Mat::from_raw(DMatrix::zeros(rows, cols))
147 }
148}
149
150impl<U, T> core::ops::Index<usize> for Vec<U, T> {
151 type Output = T;
152 fn index(&self, i: usize) -> &T {
156 &self.raw[i]
157 }
158}
159
160macro_rules! impl_vec_scalar {
161 ($trait:ident, $fn:ident, $op:tt) => {
162 impl<U, T> $trait<T> for Vec<U, T>
163 where
164 T: Scalar + Clone,
165 DVector<T>: $trait<T, Output = DVector<T>>,
166 {
167 type Output = Vec<U, T>;
168 fn $fn(self, rhs: T) -> Vec<U, T> {
174 Vec::from_raw(self.raw $op rhs)
175 }
176 }
177 };
178}
179impl_vec_scalar!(Mul, mul, *);
180impl_vec_scalar!(Div, div, /);
181
182macro_rules! impl_same_unit_binop {
183 ($type:ident, $trait:ident, $fn:ident, $op:tt) => {
184 impl<U, T> $trait for $type<U, T>
185 where
186 T: Scalar + Clone,
187 DVector<T>: $trait<Output = DVector<T>>,
188 {
189 type Output = $type<U, T>;
190 fn $fn(self, rhs: $type<U, T>) -> $type<U, T> {
195 $type::from_raw(self.raw $op rhs.raw)
196 }
197 }
198 };
199}
200
201impl_same_unit_binop!(Vec, Add, add, +);
202impl_same_unit_binop!(Vec, Sub, sub, -);
203
204impl<U, T> Neg for Vec<U, T>
205where
206 T: Scalar + Clone,
207 DVector<T>: Neg<Output = DVector<T>>,
208{
209 type Output = Vec<U, T>;
210 fn neg(self) -> Vec<U, T> {
211 Vec::from_raw(-self.raw)
212 }
213}
214
215impl<U, V, T> Add for Mat<U, V, T>
216where
217 T: Scalar + Clone,
218 DMatrix<T>: Add<Output = DMatrix<T>>,
219{
220 type Output = Mat<U, V, T>;
221 fn add(self, rhs: Mat<U, V, T>) -> Mat<U, V, T> {
225 Mat::from_raw(self.raw + rhs.raw)
226 }
227}
228
229impl<U, V, T> Sub for Mat<U, V, T>
230where
231 T: Scalar + Clone,
232 DMatrix<T>: Sub<Output = DMatrix<T>>,
233{
234 type Output = Mat<U, V, T>;
235 fn sub(self, rhs: Mat<U, V, T>) -> Mat<U, V, T> {
239 Mat::from_raw(self.raw - rhs.raw)
240 }
241}
242
243impl<U, V, T> Mat<U, V, T>
244where
245 T: Scalar + Clone,
246 DMatrix<T>: Neg<Output = DMatrix<T>>,
247{
248 pub fn negate(self) -> Mat<U, V, T> {
250 Mat::from_raw(-self.raw)
251 }
252}
253
254impl<U, V, W, T> Mul<Mat<V, W, T>> for Mat<U, V, T>
255where
256 T: Scalar + Clone,
257 DMatrix<T>: Mul<DMatrix<T>, Output = DMatrix<T>>,
258{
259 type Output = Mat<U, W, T>;
260 fn mul(self, rhs: Mat<V, W, T>) -> Mat<U, W, T> {
265 Mat::from_raw(self.raw * rhs.raw)
266 }
267}
268
269impl<U, V, T> Mul<Vec<V, T>> for Mat<U, V, T>
270where
271 T: Scalar + Clone,
272 DMatrix<T>: Mul<DVector<T>, Output = DVector<T>>,
273{
274 type Output = Vec<U, T>;
275 fn mul(self, rhs: Vec<V, T>) -> Vec<U, T> {
279 Vec::from_raw(self.raw * rhs.raw)
280 }
281}
282
283#[cfg(feature = "alloc")]
284impl<U, V, T> Mat<U, V, T>
285where
286 T: Scalar + Clone,
287{
288 pub fn transpose(self) -> Mat<V, U, T> {
290 Mat::from_raw(self.raw.transpose())
291 }
292}
293
294macro_rules! impl_mat_scalar {
295 ($trait:ident, $fn:ident, $op:tt) => {
296 impl<U, V, T> $trait<T> for Mat<U, V, T>
297 where
298 T: Scalar + Clone,
299 DMatrix<T>: $trait<T, Output = DMatrix<T>>,
300 {
301 type Output = Mat<U, V, T>;
302 fn $fn(self, rhs: T) -> Mat<U, V, T> {
303 Mat::from_raw(self.raw $op rhs)
304 }
305 }
306 };
307}
308impl_mat_scalar!(Mul, mul, *);
309impl_mat_scalar!(Div, div, /);
310
311macro_rules! impl_ref_binop {
315 ($type:ident, $trait:ident, $fn:ident, $op:tt) => {
316 impl<'a, U, T> $trait<&'a $type<U, T>> for &'a $type<U, T>
317 where
318 T: Scalar + Clone,
319 DVector<T>: $trait<Output = DVector<T>>,
320 {
321 type Output = $type<U, T>;
322 fn $fn(self, rhs: &'a $type<U, T>) -> $type<U, T> {
323 $type::from_raw(self.raw.clone() $op rhs.raw.clone())
324 }
325 }
326 };
327}
328impl_ref_binop!(Vec, Add, add, +);
329impl_ref_binop!(Vec, Sub, sub, -);
330
331impl<'a, U, V, T> Add<&'a Mat<U, V, T>> for &'a Mat<U, V, T>
332where
333 T: Scalar + Clone,
334 DMatrix<T>: Add<Output = DMatrix<T>>,
335{
336 type Output = Mat<U, V, T>;
337 fn add(self, rhs: &'a Mat<U, V, T>) -> Mat<U, V, T> {
338 Mat::from_raw(self.raw.clone() + rhs.raw.clone())
339 }
340}
341
342impl<'a, U, V, T> Sub<&'a Mat<U, V, T>> for &'a Mat<U, V, T>
343where
344 T: Scalar + Clone,
345 DMatrix<T>: Sub<Output = DMatrix<T>>,
346{
347 type Output = Mat<U, V, T>;
348 fn sub(self, rhs: &'a Mat<U, V, T>) -> Mat<U, V, T> {
349 Mat::from_raw(self.raw.clone() - rhs.raw.clone())
350 }
351}
352
353impl<'a, U, V, W, T> Mul<&'a Mat<V, W, T>> for &'a Mat<U, V, T>
354where
355 T: Scalar + Clone,
356 DMatrix<T>: Mul<DMatrix<T>, Output = DMatrix<T>>,
357{
358 type Output = Mat<U, W, T>;
359 fn mul(self, rhs: &'a Mat<V, W, T>) -> Mat<U, W, T> {
360 Mat::from_raw(self.raw.clone() * rhs.raw.clone())
361 }
362}
363
364impl<'a, U, V, T> Mul<&'a Vec<V, T>> for &'a Mat<U, V, T>
365where
366 T: Scalar + Clone,
367 DMatrix<T>: Mul<DVector<T>, Output = DVector<T>>,
368{
369 type Output = Vec<U, T>;
370 fn mul(self, rhs: &'a Vec<V, T>) -> Vec<U, T> {
371 Vec::from_raw(self.raw.clone() * rhs.raw.clone())
372 }
373}
374
375impl<U, T> Vec<U, T>
376where
377 T: Scalar + Clone + 'static,
378{
379 pub fn norm(&self) -> T {
385 self.raw.norm()
386 }
387
388 pub fn dot(&self, other: &Vec<U, T>) -> T {
397 self.raw.dot(&other.raw)
398 }
399}
400
401#[cfg(feature = "alloc")]
402impl<U, V> Mat<U, V, f64> {
403 pub fn solve(&self, b: &Vec<U, f64>) -> Result<Vec<V, f64>, tpt_math_linalg_dense::DenseError> {
414 self.raw.solve(&b.raw).map(Vec::from_raw)
415 }
416
417 pub fn inverse(&self) -> Result<Mat<V, U, f64>, tpt_math_linalg_dense::DenseError> {
424 self.raw.inverse().map(Mat::from_raw)
425 }
426}
427
428#[cfg(test)]
429mod tests {
430 use super::*;
431 use tpt_math_linalg_dense::{DMatrix, DVector};
432 use tpt_math_units::prelude::{Length, Time, Velocity};
433
434 #[test]
435 fn vector_addition_keeps_unit() {
436 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[1.0, 2.0]));
437 let b = Vec::<Length>::from_raw(DVector::from_row_slice(&[3.0, 4.0]));
438 let c = a + b;
439 assert_eq!(c.raw(), &DVector::from_row_slice(&[4.0, 6.0]));
440 }
441
442 #[test]
443 fn matrix_mul_propagates_units() {
444 let m =
445 Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[1.0_f64, 2.0, 3.0, 4.0]));
446 let n = Mat::<Time, Velocity>::from_raw(DMatrix::from_row_slice(
447 2,
448 2,
449 &[1.0_f64, 0.0, 0.0, 1.0],
450 ));
451 let p: Mat<Length, Velocity> = m * n;
452 assert_eq!(
453 p.raw(),
454 &DMatrix::from_row_slice(2, 2, &[1.0_f64, 2.0, 3.0, 4.0])
455 );
456 }
457
458 #[test]
459 fn matrix_times_vector() {
460 let m =
461 Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[2.0_f64, 0.0, 0.0, 2.0]));
462 let v = Vec::<Time>::from_raw(DVector::from_row_slice(&[1.0, 2.0]));
463 let r: Vec<Length> = m * v;
464 assert_eq!(r.raw(), &DVector::from_row_slice(&[2.0, 4.0]));
465 }
466
467 #[test]
468 fn transpose_swaps_unit_tags() {
469 let m = Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(1, 2, &[1.0_f64, 2.0]));
470 let t: Mat<Time, Length> = m.transpose();
471 assert_eq!(t.nrows(), 2);
472 assert_eq!(t.ncols(), 1);
473 }
474
475 #[test]
476 fn mat_add_and_sub_keep_units() {
477 let a = Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[1.0, 2.0, 3.0, 4.0]));
478 let b = Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[4.0, 3.0, 2.0, 1.0]));
479 let sum: Mat<Length, Time> = a.clone() + b.clone();
480 assert_eq!(
481 sum.raw(),
482 &DMatrix::from_row_slice(2, 2, &[5.0, 5.0, 5.0, 5.0])
483 );
484 let diff: Mat<Length, Time> = a - b;
485 assert_eq!(
486 diff.raw(),
487 &DMatrix::from_row_slice(2, 2, &[-3.0, -1.0, 1.0, 3.0])
488 );
489 }
490
491 #[test]
492 fn vec_sub_keeps_unit() {
493 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[5.0, 6.0]));
494 let b = Vec::<Length>::from_raw(DVector::from_row_slice(&[1.0, 2.0]));
495 let c = a - b;
496 assert_eq!(c.raw(), &DVector::from_row_slice(&[4.0, 4.0]));
497 }
498
499 #[test]
500 fn vec_scalar_mul_div_keep_unit() {
501 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[2.0, 4.0]));
502 let doubled: Vec<Length> = a.clone() * 3.0;
503 assert_eq!(doubled.raw(), &DVector::from_row_slice(&[6.0, 12.0]));
504 let halved: Vec<Length> = a / 2.0;
505 assert_eq!(halved.raw(), &DVector::from_row_slice(&[1.0, 2.0]));
506 }
507
508 #[test]
509 fn vec_neg_keeps_unit() {
510 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[1.0, -2.0]));
511 let b: Vec<Length> = -a;
512 assert_eq!(b.raw(), &DVector::from_row_slice(&[-1.0, 2.0]));
513 }
514
515 #[test]
516 fn vec_index_and_len() {
517 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[3.0, 1.0, 4.0]));
518 assert_eq!(a.len(), 3);
519 assert!(!a.is_empty());
520 assert_eq!(a[0], 3.0);
521 assert_eq!(a[2], 4.0);
522 let empty = Vec::<Length>::from_raw(DVector::from_row_slice(&[]));
523 assert!(empty.is_empty());
524 }
525
526 #[test]
527 fn mat_scalar_mul_div_keep_units() {
528 let m = Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(1, 2, &[2.0_f64, 4.0]));
529 let doubled: Mat<Length, Time> = m.clone() * 2.0;
530 assert_eq!(doubled.raw(), &DMatrix::from_row_slice(1, 2, &[4.0, 8.0]));
531 let halved: Mat<Length, Time> = m / 2.0;
532 assert_eq!(halved.raw(), &DMatrix::from_row_slice(1, 2, &[1.0, 2.0]));
533 }
534
535 #[test]
536 fn by_reference_ops_match_owned_ops() {
537 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[1.0, 2.0]));
538 let b = Vec::<Length>::from_raw(DVector::from_row_slice(&[3.0, 5.0]));
539 assert_eq!(&a + &b, a.clone() + b.clone());
540 assert_eq!(&a - &b, a.clone() - b.clone());
541
542 let m = Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[1.0, 2.0, 3.0, 4.0]));
543 let n = Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[0.0, 1.0, 1.0, 0.0]));
544 assert_eq!(&m + &n, m.clone() + n.clone());
545 assert_eq!(&m - &n, m.clone() - n.clone());
546
547 let p = Mat::<Time, Length>::from_raw(DMatrix::from_row_slice(2, 2, &[1.0, 0.0, 0.0, 1.0]));
548 let prod: Mat<Length, Length> = &m * &p;
549 assert_eq!(prod, m.clone() * p.clone());
550
551 let v = Vec::<Time>::from_raw(DVector::from_row_slice(&[1.0, 1.0]));
552 let mv: Vec<Length> = &m * &v;
553 assert_eq!(mv, m * v);
554 }
555
556 #[test]
557 fn norm_and_dot() {
558 let a = Vec::<Length>::from_raw(DVector::from_row_slice(&[3.0_f64, 4.0]));
559 assert_eq!(a.norm(), 5.0);
560 let b = Vec::<Length>::from_raw(DVector::from_row_slice(&[1.0_f64, 2.0]));
561 assert_eq!(a.dot(&b), 11.0);
562 }
563
564 #[test]
565 fn solve_carries_row_and_column_units() {
566 let a =
568 Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[2.0_f64, 0.0, 0.0, 4.0]));
569 let b = Vec::<Length>::from_raw(DVector::from_row_slice(&[2.0, 8.0]));
570 let x: Vec<Time> = a.solve(&b).unwrap();
571 assert_eq!(x.raw(), &DVector::from_row_slice(&[1.0, 2.0]));
572 }
573
574 #[test]
575 fn solve_rejects_singular_matrix() {
576 let a =
577 Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[1.0_f64, 2.0, 2.0, 4.0]));
578 let b = Vec::<Length>::from_raw(DVector::from_row_slice(&[1.0, 1.0]));
579 assert!(a.solve(&b).is_err());
580 }
581
582 #[test]
583 fn inverse_swaps_unit_tags() {
584 let a =
585 Mat::<Length, Time>::from_raw(DMatrix::from_row_slice(2, 2, &[2.0_f64, 0.0, 0.0, 4.0]));
586 let inv: Mat<Time, Length> = a.inverse().unwrap();
587 assert_eq!(
588 inv.raw(),
589 &DMatrix::from_row_slice(2, 2, &[0.5, 0.0, 0.0, 0.25])
590 );
591 }
592
593 #[test]
594 fn zeros_construct_zero_filled() {
595 let v = Vec::<Length>::zeros(3);
596 assert_eq!(v.raw(), &DVector::from_row_slice(&[0.0, 0.0, 0.0]));
597 let m = Mat::<Length, Time>::zeros(2, 3);
598 assert_eq!(m.nrows(), 2);
599 assert_eq!(m.ncols(), 3);
600 assert_eq!(m.raw(), &DMatrix::from_row_slice(2, 3, &[0.0; 6]));
601 }
602}