cessen/psychopath
1
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

RMath: implement vector-matrix multiplication.

Browse Source
This commit is contained in:
Nathan Vegdahl 2022-07-14 19:02:08 -07:00
parent c398387b55
commit d8e1437db1
2 changed files with 93 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
sub_crates/rmath/src/wide4.rs
View File

@ -1,5 +1,7 @@
use std::ops::{AddAssign, DivAssign, MulAssign, SubAssign};
use approx::relative_eq;
use crate::{difference_of_products, two_prod, two_sum};
pub use fallback::Float4;
@ -351,16 +353,6 @@ impl Float4 {
/// Returns `None` if not invertible.
#[inline]
pub fn invert_3x3(m: [Self; 3]) -> Option<[Self; 3]> {
// let a = difference_of_products(m[1].b(), m[2].c(), m[1].c(), m[2].b());
// let b = difference_of_products(m[1].c(), m[2].a(), m[1].a(), m[2].c());
// let c = difference_of_products(m[1].a(), m[2].b(), m[1].b(), m[2].a());
// let d = difference_of_products(m[2].b(), m[0].c(), m[2].c(), m[0].b());
// let e = difference_of_products(m[2].c(), m[0].a(), m[2].a(), m[0].c());
// let f = difference_of_products(m[2].a(), m[0].b(), m[2].b(), m[0].a());
// let g = difference_of_products(m[0].b(), m[1].c(), m[0].c(), m[1].b());
// let h = difference_of_products(m[0].c(), m[1].a(), m[0].a(), m[1].c());
// let i = difference_of_products(m[0].a(), m[1].b(), m[0].b(), m[1].a());
let m0_bca = m[0].bcad();
let m1_bca = m[1].bcad();
let m2_bca = m[2].bcad();
@ -371,7 +363,6 @@ impl Float4 {
let def = difference_of_products(m2_bca, m0_cab, m2_cab, m0_bca);
let ghi = difference_of_products(m0_bca, m1_cab, m0_cab, m1_bca);
// TODO: use precise inner product.
let det = Self::dot_3(
Self::new(abc.a(), def.a(), ghi.a(), 0.0),
Self::new(m[0].a(), m[1].a(), m[2].a(), 0.0),
@ -389,15 +380,6 @@ impl Float4 {
/// Returns `None` if not invertible.
#[inline]
pub fn invert_3x3_fast(m: [Self; 3]) -> Option<[Self; 3]> {
// let a = (m[1].b() * m[2].c()) - (m[1].c() * m[2].b());
// let b = (m[1].c() * m[2].a()) - (m[1].a() * m[2].c());
// let c = (m[1].a() * m[2].b()) - (m[1].b() * m[2].a());
// let e = (m[2].c() * m[0].a()) - (m[2].a() * m[0].c());
// let f = (m[2].a() * m[0].b()) - (m[2].b() * m[0].a());
// let g = (m[0].b() * m[1].c()) - (m[0].c() * m[1].b());
// let h = (m[0].c() * m[1].a()) - (m[0].a() * m[1].c());
// let i = (m[0].a() * m[1].b()) - (m[0].b() * m[1].a());
let m0_bca = m[0].bcad();
let m1_bca = m[1].bcad();
let m2_bca = m[2].bcad();
@ -408,7 +390,7 @@ impl Float4 {
let def = (m2_bca * m0_cab) - (m2_cab * m0_bca);
let ghi = (m0_bca * m1_cab) - (m0_cab * m1_bca);
let det = Self::dot_3(
let det = Self::dot_3_fast(
Self::new(abc.a(), def.a(), ghi.a(), 0.0),
Self::new(m[0].a(), m[1].a(), m[2].a(), 0.0),
);
@ -422,24 +404,63 @@ impl Float4 {
/// Multiplies a 3D vector with a 3x3 matrix.
#[inline]
pub fn vec3_mul_3x3(_v: Self, _m: &[Self; 3]) -> Self {
todo!()
pub fn vec_mul_3x3(self, m: &[Self; 3]) -> Self {
let x = self.aaaa();
let y = self.bbbb();
let z = self.cccc();
// Products.
let (a, a_err) = two_prod(x, m[0]);
let (b, b_err) = two_prod(y, m[1]);
let (c, c_err) = two_prod(z, m[2]);
// Sums.
let (s1, s1_err) = two_sum(a, b);
let err1 = a_err + (b_err + s1_err);
let (s2, s2_err) = two_sum(c, s1);
let err2 = c_err + (err1 + s2_err);
s2 + err2
}
/// Multiplies a 3D vector with a 3x3 matrix.
///
/// Faster but less precise version.
#[inline]
pub fn vec3_mul_3x3_fast(_v: Self, _m: &[Self; 3]) -> Self {
todo!()
pub fn vec_mul_3x3_fast(self, m: &[Self; 3]) -> Self {
let x = self.aaaa();
let y = self.bbbb();
let z = self.cccc();
(x * m[0]) + (y * m[1]) + (z * m[2])
}
/// Transforms a 3d point by an affine transform.
///
/// `m` is the 3x3 part of the affine transform, `t` is the translation part.
#[inline]
pub fn pnt3_mul_affine(_p: Self, _m: &[Self; 3], _t: Self) -> Self {
todo!()
pub fn vec_mul_affine(self, m: &[Self; 3], t: Self) -> Self {
let x = self.aaaa();
let y = self.bbbb();
let z = self.cccc();
// Products.
let (a, a_err) = two_prod(x, m[0]);
let (b, b_err) = two_prod(y, m[1]);
let (c, c_err) = two_prod(z, m[2]);
// Sums.
let (s1, s1_err) = two_sum(a, b);
let err1 = a_err + (b_err + s1_err);
let (s2, s2_err) = two_sum(c, s1);
let err2 = c_err + (err1 + s2_err);
let (s3, s3_err) = two_sum(t, s2);
let err3 = err2 + s3_err;
s3 + err3
}
/// Transforms a 3d point by an affine transform, except it applies
@ -450,8 +471,46 @@ impl Float4 {
///
/// `m` is the 3x3 part of the affine transform, `t` is the translation part.
#[inline]
pub fn pnt3_mul_affine_rev(_p: Self, _m: &[Self; 3], _t: Self) -> Self {
todo!()
pub fn vec_mul_affine_rev(self, m: &[Self; 3], t: Self) -> Self {
let (v, v_err) = two_sum(self, t);
let (x, x_err) = (v.aaaa(), v_err.aaaa());
let (y, y_err) = (v.bbbb(), v_err.bbbb());
let (z, z_err) = (v.cccc(), v_err.cccc());
// Products.
let ((a, a_err1), a_err2) = (two_prod(x, m[0]), x_err * m[0]);
let ((b, b_err1), b_err2) = (two_prod(y, m[1]), y_err * m[1]);
let ((c, c_err1), c_err2) = (two_prod(z, m[2]), z_err * m[2]);
let a_err = a_err1 + a_err2;
let b_err = b_err1 + b_err2;
let c_err = c_err1 + c_err2;
// Sums.
let (s1, s1_err) = two_sum(a, b);
let err1 = a_err + (b_err + s1_err);
let (s2, s2_err) = two_sum(c, s1);
let err2 = c_err + (err1 + s2_err);
let (s3, s3_err) = two_sum(t, s2);
let err3 = err2 + s3_err;
s3 + err3
}
/// Returns whether the `Float4`s are approximately equal to each
/// other.
///
/// Each corresponding element cannot have a relative error exceeding
/// `epsilon`.
pub(crate) fn aprx_eq(a: Self, b: Self, epsilon: f32) -> bool {
let mut eq = true;
eq &= relative_eq!(a.a(), b.a(), epsilon = epsilon);
eq &= relative_eq!(a.b(), b.b(), epsilon = epsilon);
eq &= relative_eq!(a.c(), b.c(), epsilon = epsilon);
eq &= relative_eq!(a.d(), b.d(), epsilon = epsilon);
eq
}
}

19
sub_crates/rmath/src/xform.rs
View File

@ -2,8 +2,6 @@
use std::ops::{Add, Mul};
use approx::relative_eq;
use crate::point::Point;
use crate::wide4::Float4;
@ -80,19 +78,12 @@ impl Xform {
/// Returns whether the matrices are approximately equal to each other.
/// Each corresponding element in the matrices cannot have a relative
/// error exceeding epsilon.
#[inline]
pub fn aprx_eq(&self, other: Xform, epsilon: f32) -> bool {
pub(crate) fn aprx_eq(&self, other: Xform, epsilon: f32) -> bool {
let mut eq = true;
for (t1, t2) in self
.m
.iter()
.chain(&[self.t])
.zip(other.m.iter().chain(&[other.t]))
{
eq &= relative_eq!(t1.a(), t2.a(), epsilon = epsilon);
eq &= relative_eq!(t1.b(), t2.b(), epsilon = epsilon);
eq &= relative_eq!(t1.c(), t2.c(), epsilon = epsilon);
}
eq &= Float4::aprx_eq(self.m[0], other.m[0], epsilon);
eq &= Float4::aprx_eq(self.m[1], other.m[1], epsilon);
eq &= Float4::aprx_eq(self.m[2], other.m[2], epsilon);
eq &= Float4::aprx_eq(self.t, other.t, epsilon);
eq
}