yuvxyb_math/
matrix.rs

1// These types are intended for floats, which are not `Eq`
2#![allow(clippy::derive_partial_eq_without_eq)]
3
4use std::ops::{Div, Mul, Neg};
5
6use crate::mul_add::FastMulAdd;
7
8#[derive(Debug, Clone, PartialEq)]
9#[must_use]
10pub struct RowVector<T>(T, T, T);
11
12impl<T: Copy> RowVector<T> {
13    pub const fn new(x: T, y: T, z: T) -> Self {
14        Self(x, y, z)
15    }
16
17    #[must_use]
18    pub const fn x(&self) -> T {
19        self.0
20    }
21    #[must_use]
22    pub const fn y(&self) -> T {
23        self.1
24    }
25    #[must_use]
26    pub const fn z(&self) -> T {
27        self.2
28    }
29
30    pub const fn values(self) -> [T; 3] {
31        let Self(x, y, z) = self;
32        [x, y, z]
33    }
34}
35
36impl<T> RowVector<T>
37where
38    T: Copy + FastMulAdd + Mul<T, Output = T> + Div<T, Output = T> + Neg<Output = T>,
39{
40    pub fn cross(&self, other: &Self) -> Self {
41        let Self(sx, sy, sz) = *self;
42        let Self(ox, oy, oz) = *other;
43
44        Self::new(
45            sy.fast_mul_add(oz, -(sz * oy)),
46            sz.fast_mul_add(ox, -(sx * oz)),
47            sx.fast_mul_add(oy, -(sy * ox)),
48        )
49    }
50
51    #[must_use]
52    pub fn dot(&self, other: &Self) -> T {
53        self.0
54            .fast_mul_add(other.0, self.1.fast_mul_add(other.1, self.2 * other.2))
55    }
56
57    pub fn scalar_div(&self, x: T) -> Self {
58        Self(self.0 / x, self.1 / x, self.2 / x)
59    }
60
61    pub fn component_mul(&self, other: &Self) -> Self {
62        Self(self.0 * other.0, self.1 * other.1, self.2 * other.2)
63    }
64}
65
66impl<T: Copy> From<[T; 3]> for RowVector<T> {
67    fn from(value: [T; 3]) -> Self {
68        let [x, y, z] = value;
69        Self::new(x, y, z)
70    }
71}
72
73#[derive(Debug, Clone, PartialEq)]
74#[must_use]
75pub struct ColVector<T>(T, T, T);
76
77impl<T: Copy> ColVector<T> {
78    pub const fn new(r: T, g: T, b: T) -> Self {
79        Self(r, g, b)
80    }
81
82    #[must_use]
83    pub const fn r(&self) -> T {
84        self.0
85    }
86    #[must_use]
87    pub const fn g(&self) -> T {
88        self.1
89    }
90    #[must_use]
91    pub const fn b(&self) -> T {
92        self.2
93    }
94
95    pub const fn transpose(self) -> RowVector<T> {
96        RowVector::new(self.0, self.1, self.2)
97    }
98
99    pub const fn values(self) -> [T; 3] {
100        let Self(r, g, b) = self;
101        [r, g, b]
102    }
103}
104
105impl<T: Copy> From<[T; 3]> for ColVector<T> {
106    fn from(value: [T; 3]) -> Self {
107        let [r, g, b] = value;
108        Self::new(r, g, b)
109    }
110}
111
112#[derive(Debug, Clone, PartialEq)]
113#[must_use]
114pub struct Matrix<T>(RowVector<T>, RowVector<T>, RowVector<T>);
115
116impl<T: Copy> Matrix<T> {
117    pub const fn new(r1: RowVector<T>, r2: RowVector<T>, r3: RowVector<T>) -> Self {
118        Self(r1, r2, r3)
119    }
120
121    pub const fn r1(&self) -> &RowVector<T> {
122        &self.0
123    }
124    pub const fn r2(&self) -> &RowVector<T> {
125        &self.1
126    }
127    pub const fn r3(&self) -> &RowVector<T> {
128        &self.2
129    }
130
131    pub const fn transpose(self) -> Self {
132        let Self(r1, r2, r3) = self;
133
134        let RowVector(s11, s12, s13) = r1;
135        let RowVector(s21, s22, s23) = r2;
136        let RowVector(s31, s32, s33) = r3;
137
138        Self::new(
139            RowVector::new(s11, s21, s31),
140            RowVector::new(s12, s22, s32),
141            RowVector::new(s13, s23, s33),
142        )
143    }
144}
145
146impl Matrix<f32> {
147    pub const fn identity() -> Self {
148        Self::new(
149            RowVector::new(1.0, 0.0, 0.0),
150            RowVector::new(0.0, 1.0, 0.0),
151            RowVector::new(0.0, 0.0, 1.0),
152        )
153    }
154}
155
156impl Matrix<f64> {
157    pub const fn identity() -> Self {
158        Self::new(
159            RowVector::new(1.0, 0.0, 0.0),
160            RowVector::new(0.0, 1.0, 0.0),
161            RowVector::new(0.0, 0.0, 1.0),
162        )
163    }
164}
165
166impl<T> Matrix<T>
167where
168    T: Copy + FastMulAdd + Mul<T, Output = T> + Div<T, Output = T> + Neg<Output = T>,
169{
170    pub fn scalar_div(&self, x: T) -> Self {
171        Self(
172            self.0.scalar_div(x),
173            self.1.scalar_div(x),
174            self.2.scalar_div(x),
175        )
176    }
177
178    /// Will panic if the matrix is not invertible
179    pub fn invert(&self) -> Self {
180        // Cramer's rule
181        let Self(ref r1, ref r2, ref r3) = *self;
182
183        let RowVector(s11, s12, s13) = *r1;
184        let RowVector(s21, s22, s23) = *r2;
185        let RowVector(s31, s32, s33) = *r3;
186
187        let minor_11 = s22.fast_mul_add(s33, -(s32 * s23));
188        let minor_12 = s21.fast_mul_add(s33, -(s31 * s23));
189        let minor_13 = s21.fast_mul_add(s32, -(s31 * s22));
190
191        let minor_21 = s12.fast_mul_add(s33, -(s32 * s13));
192        let minor_22 = s11.fast_mul_add(s33, -(s31 * s13));
193        let minor_23 = s11.fast_mul_add(s32, -(s31 * s12));
194
195        let minor_31 = s12.fast_mul_add(s23, -(s22 * s13));
196        let minor_32 = s11.fast_mul_add(s23, -(s21 * s13));
197        let minor_33 = s11.fast_mul_add(s22, -(s21 * s12));
198
199        let determinant =
200            s11.fast_mul_add(minor_11, -s12.fast_mul_add(minor_12, -(s13 * minor_13)));
201
202        Self::new(
203            RowVector::new(minor_11, -minor_12, minor_13),
204            RowVector::new(-minor_21, minor_22, -minor_23),
205            RowVector::new(minor_31, -minor_32, minor_33),
206        )
207        .transpose()
208        .scalar_div(determinant)
209    }
210
211    pub fn mul_vec(&self, rhs: &ColVector<T>) -> ColVector<T> {
212        let Self(ref r1, ref r2, ref r3) = *self;
213
214        ColVector::new(
215            r1.0.fast_mul_add(rhs.0, r1.1.fast_mul_add(rhs.1, r1.2 * rhs.2)),
216            r2.0.fast_mul_add(rhs.0, r2.1.fast_mul_add(rhs.1, r2.2 * rhs.2)),
217            r3.0.fast_mul_add(rhs.0, r3.1.fast_mul_add(rhs.1, r3.2 * rhs.2)),
218        )
219    }
220
221    pub fn mul_mat(&self, rhs: Self) -> Self {
222        let Self(ref r1, ref r2, ref r3) = *self;
223        let Self(o1, o2, o3) = rhs;
224
225        Self::new(
226            RowVector::new(
227                r1.0.fast_mul_add(o1.0, r1.1.fast_mul_add(o2.0, r1.2 * o3.0)),
228                r1.0.fast_mul_add(o1.1, r1.1.fast_mul_add(o2.1, r1.2 * o3.1)),
229                r1.0.fast_mul_add(o1.2, r1.1.fast_mul_add(o2.2, r1.2 * o3.2)),
230            ),
231            RowVector::new(
232                r2.0.fast_mul_add(o1.0, r2.1.fast_mul_add(o2.0, r2.2 * o3.0)),
233                r2.0.fast_mul_add(o1.1, r2.1.fast_mul_add(o2.1, r2.2 * o3.1)),
234                r2.0.fast_mul_add(o1.2, r2.1.fast_mul_add(o2.2, r2.2 * o3.2)),
235            ),
236            RowVector::new(
237                r3.0.fast_mul_add(o1.0, r3.1.fast_mul_add(o2.0, r3.2 * o3.0)),
238                r3.0.fast_mul_add(o1.1, r3.1.fast_mul_add(o2.1, r3.2 * o3.1)),
239                r3.0.fast_mul_add(o1.2, r3.1.fast_mul_add(o2.2, r3.2 * o3.2)),
240            ),
241        )
242    }
243
244    #[must_use]
245    pub fn mul_arr(&self, rhs: [T; 3]) -> [T; 3] {
246        let Self(ref r1, ref r2, ref r3) = *self;
247
248        [
249            r1.0.fast_mul_add(rhs[0], r1.1.fast_mul_add(rhs[1], r1.2 * rhs[2])),
250            r2.0.fast_mul_add(rhs[0], r2.1.fast_mul_add(rhs[1], r2.2 * rhs[2])),
251            r3.0.fast_mul_add(rhs[0], r3.1.fast_mul_add(rhs[1], r3.2 * rhs[2])),
252        ]
253    }
254
255    pub const fn values(self) -> [[T; 3]; 3] {
256        [self.0.values(), self.1.values(), self.2.values()]
257    }
258}
yuvxyb_math/matrix.rs

yuvxyb_math/
matrix.rs
