use crate::errors::linear_error::LinearError;
use deep_causality_algebra::{CommutativeRing, Field, RealField};
use deep_causality_num::FromPrimitive;
pub fn dot<T>(a: &[T], b: &[T]) -> Result<T, LinearError>
where
T: CommutativeRing + Copy,
{
if a.len() != b.len() {
return Err(LinearError::LengthMismatch(a.len(), b.len()));
}
Ok(a.iter()
.zip(b.iter())
.fold(T::zero(), |acc, (&x, &y)| acc + x * y))
}
pub fn dot_n<T, const N: usize>(a: &[T; N], b: &[T; N]) -> T
where
T: CommutativeRing + Copy,
{
a.iter()
.zip(b.iter())
.fold(T::zero(), |acc, (&x, &y)| acc + x * y)
}
pub fn determinant_3x3<T>(m: &[[T; 3]; 3]) -> T
where
T: CommutativeRing + Copy,
{
m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
- m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
+ m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])
}
pub fn determinant_4x4<T>(m: &[[T; 4]; 4]) -> T
where
T: CommutativeRing + Copy,
{
let (s, c) = minors_4x4(m);
s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] - s[4] * c[1] + s[5] * c[0]
}
pub fn trace_of_square_3x3<T>(a: &[[T; 3]; 3]) -> T
where
T: CommutativeRing + Copy,
{
a[0][0] * a[0][0]
+ a[0][1] * a[1][0]
+ a[0][2] * a[2][0]
+ a[1][0] * a[0][1]
+ a[1][1] * a[1][1]
+ a[1][2] * a[2][1]
+ a[2][0] * a[0][2]
+ a[2][1] * a[1][2]
+ a[2][2] * a[2][2]
}
pub fn double_dot_3x3<T>(a: &[[T; 3]; 3], b: &[[T; 3]; 3]) -> T
where
T: CommutativeRing + Copy,
{
a[0][0] * b[0][0]
+ a[0][1] * b[0][1]
+ a[0][2] * b[0][2]
+ a[1][0] * b[1][0]
+ a[1][1] * b[1][1]
+ a[1][2] * b[1][2]
+ a[2][0] * b[2][0]
+ a[2][1] * b[2][1]
+ a[2][2] * b[2][2]
}
pub fn mat3_vec<T>(m: &[[T; 3]; 3], v: &[T; 3]) -> [T; 3]
where
T: CommutativeRing + Copy,
{
core::array::from_fn(|i| m[i][0] * v[0] + m[i][1] * v[1] + m[i][2] * v[2])
}
pub fn inverse_3x3<T>(m: &[[T; 3]; 3]) -> Result<[[T; 3]; 3], LinearError>
where
T: Field + Copy + PartialEq,
{
let det = determinant_3x3(m);
if det == T::zero() {
return Err(LinearError::Singular(0));
}
let inv = T::one() / det;
Ok([
[
inv * (m[1][1] * m[2][2] - m[1][2] * m[2][1]),
inv * (m[0][2] * m[2][1] - m[0][1] * m[2][2]),
inv * (m[0][1] * m[1][2] - m[0][2] * m[1][1]),
],
[
inv * (m[1][2] * m[2][0] - m[1][0] * m[2][2]),
inv * (m[0][0] * m[2][2] - m[0][2] * m[2][0]),
inv * (m[0][2] * m[1][0] - m[0][0] * m[1][2]),
],
[
inv * (m[1][0] * m[2][1] - m[1][1] * m[2][0]),
inv * (m[0][1] * m[2][0] - m[0][0] * m[2][1]),
inv * (m[0][0] * m[1][1] - m[0][1] * m[1][0]),
],
])
}
pub fn inverse_4x4<T>(m: &[[T; 4]; 4]) -> Result<[[T; 4]; 4], LinearError>
where
T: Field + Copy + PartialEq,
{
let (s, c) = minors_4x4(m);
let det = s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] - s[4] * c[1] + s[5] * c[0];
if det == T::zero() {
return Err(LinearError::Singular(0));
}
let d = T::one() / det;
let z = T::zero();
Ok([
[
d * (m[1][1] * c[5] - m[1][2] * c[4] + m[1][3] * c[3]),
d * (z - (m[0][1] * c[5] - m[0][2] * c[4] + m[0][3] * c[3])),
d * (m[3][1] * s[5] - m[3][2] * s[4] + m[3][3] * s[3]),
d * (z - (m[2][1] * s[5] - m[2][2] * s[4] + m[2][3] * s[3])),
],
[
d * (z - (m[1][0] * c[5] - m[1][2] * c[2] + m[1][3] * c[1])),
d * (m[0][0] * c[5] - m[0][2] * c[2] + m[0][3] * c[1]),
d * (z - (m[3][0] * s[5] - m[3][2] * s[2] + m[3][3] * s[1])),
d * (m[2][0] * s[5] - m[2][2] * s[2] + m[2][3] * s[1]),
],
[
d * (m[1][0] * c[4] - m[1][1] * c[2] + m[1][3] * c[0]),
d * (z - (m[0][0] * c[4] - m[0][1] * c[2] + m[0][3] * c[0])),
d * (m[3][0] * s[4] - m[3][1] * s[2] + m[3][3] * s[0]),
d * (z - (m[2][0] * s[4] - m[2][1] * s[2] + m[2][3] * s[0])),
],
[
d * (z - (m[1][0] * c[3] - m[1][1] * c[1] + m[1][2] * c[0])),
d * (m[0][0] * c[3] - m[0][1] * c[1] + m[0][2] * c[0]),
d * (z - (m[3][0] * s[3] - m[3][1] * s[1] + m[3][2] * s[0])),
d * (m[2][0] * s[3] - m[2][1] * s[1] + m[2][2] * s[0]),
],
])
}
pub fn eigen_symmetric_3x3<T>(m: &[[T; 3]; 3]) -> Result<[T; 3], LinearError>
where
T: RealField + FromPrimitive,
{
let third: T = constant(1.0 / 3.0)?;
let half: T = constant(0.5)?;
let two: T = constant(2.0)?;
let three: T = constant(3.0)?;
let six: T = constant(6.0)?;
let two_pi_over_3: T = constant(2.0 * core::f64::consts::PI / 3.0)?;
let scale = max_magnitude_6(m);
if scale == T::zero() {
return Ok([T::zero(); 3]);
}
let a00 = m[0][0] / scale;
let a11 = m[1][1] / scale;
let a22 = m[2][2] / scale;
let a01 = m[0][1] / scale;
let a02 = m[0][2] / scale;
let a12 = m[1][2] / scale;
let p1 = a01 * a01 + a02 * a02 + a12 * a12;
if p1 == T::zero() {
let mut e = [m[0][0], m[1][1], m[2][2]];
sort_desc_3(&mut e);
return Ok(e);
}
let q = (a00 + a11 + a22) * third; let d00 = a00 - q;
let d11 = a11 - q;
let d22 = a22 - q;
let p2 = d00 * d00 + d11 * d11 + d22 * d22 + two * p1;
let p = (p2 / six).sqrt();
let inv_p = T::one() / p;
let b00 = d00 * inv_p;
let b11 = d11 * inv_p;
let b22 = d22 * inv_p;
let b01 = a01 * inv_p;
let b02 = a02 * inv_p;
let b12 = a12 * inv_p;
let det_b = b00 * (b11 * b22 - b12 * b12) - b01 * (b01 * b22 - b12 * b02)
+ b02 * (b01 * b12 - b11 * b02);
let mut r = det_b * half;
if r < -T::one() {
r = -T::one();
}
if r > T::one() {
r = T::one();
}
let phi = r.acos() * third;
let eig1 = q + two * p * phi.cos();
let eig3 = q + two * p * (phi + two_pi_over_3).cos();
let eig2 = three * q - eig1 - eig3;
let mut e = [eig1 * scale, eig2 * scale, eig3 * scale];
sort_desc_3(&mut e);
Ok(e)
}
fn max_magnitude_6<T: RealField>(m: &[[T; 3]; 3]) -> T {
let mut max = T::zero();
for x in [m[0][0], m[1][1], m[2][2], m[0][1], m[0][2], m[1][2]] {
let a = x.abs();
if a > max {
max = a;
}
}
max
}
fn sort_desc_3<T: RealField>(a: &mut [T; 3]) {
if a[0] < a[1] {
a.swap(0, 1);
}
if a[1] < a[2] {
a.swap(1, 2);
}
if a[0] < a[1] {
a.swap(0, 1);
}
}
fn constant<T: RealField + FromPrimitive>(v: f64) -> Result<T, LinearError> {
T::from_f64(v).ok_or_else(|| LinearError::Overflow("eigen_symmetric_3x3 constant"))
}
fn minors_4x4<T>(m: &[[T; 4]; 4]) -> ([T; 6], [T; 6])
where
T: CommutativeRing + Copy,
{
let s = [
m[0][0] * m[1][1] - m[0][1] * m[1][0],
m[0][0] * m[1][2] - m[0][2] * m[1][0],
m[0][0] * m[1][3] - m[0][3] * m[1][0],
m[0][1] * m[1][2] - m[0][2] * m[1][1],
m[0][1] * m[1][3] - m[0][3] * m[1][1],
m[0][2] * m[1][3] - m[0][3] * m[1][2],
];
let c = [
m[2][0] * m[3][1] - m[2][1] * m[3][0],
m[2][0] * m[3][2] - m[2][2] * m[3][0],
m[2][0] * m[3][3] - m[2][3] * m[3][0],
m[2][1] * m[3][2] - m[2][2] * m[3][1],
m[2][1] * m[3][3] - m[2][3] * m[3][1],
m[2][2] * m[3][3] - m[2][3] * m[3][2],
];
(s, c)
}