M-Labs
/
nalgebra
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Avoid matrix multiplication.

This commit is contained in:
Rouven Spreckels 2024-03-11 17:14:35 +01:00
parent 5e10563612
commit 27198398fa
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/base/cg.rs
View File

@ -89,16 +89,14 @@ where
SC: Storage<T, D> + Clone, SC: Storage<T, D> + Clone,
{ {
let (a, b, ab) = (from.as_ref(), to.as_ref(), from.dot(to)); let (a, b, ab) = (from.as_ref(), to.as_ref(), from.dot(to));
let d = T::one() + ab; let d = T::one() + ab.clone();
(d > T::default_epsilon().sqrt()).then(|| { (d > T::default_epsilon().sqrt()).then(|| {
let k = &(b * a.transpose() - a * b.transpose()); let [at, bt] = &[a.transpose(), b.transpose()];
let [b_at, a_bt, a_at, b_bt] = &[b * at, a * bt, a * at, b * bt];
let [k1, k2] = [b_at - a_bt, (b_at + a_bt) * ab - (a_at + b_bt)];
// Codesido's Rotation Formula // Codesido's Rotation Formula
// <https://doi.org/10.14232/ejqtde.2018.1.13> // <https://doi.org/10.14232/ejqtde.2018.1.13>
// Self::identity() + k1 + k2 / d
// Equals `Self::identity() + k + k * k / d`:
let mut r = Self::identity() + k;
r.gemm(d.recip(), k, k, T::one());
r
}) })
} }
/// The n-dimensional rotation matrix described by an oriented minor arc and a signed angle. /// The n-dimensional rotation matrix described by an oriented minor arc and a signed angle.
@ -120,7 +118,7 @@ where
.try_normalize(T::default_epsilon().sqrt()) .try_normalize(T::default_epsilon().sqrt())
.map(|ref b| { .map(|ref b| {
let (sin, cos) = angle.sin_cos(); let (sin, cos) = angle.sin_cos();
let [at, bt] = [&a.transpose(), &b.transpose()]; let [at, bt] = &[a.transpose(), b.transpose()];
let [k1, k2] = [b * at - a * bt, -(a * at + b * bt)]; let [k1, k2] = [b * at - a * bt, -(a * at + b * bt)];
// Simple rotations / Rotation in a two–plane // Simple rotations / Rotation in a two–plane
// <https://doi.org/10.48550/arXiv.1103.5263> // <https://doi.org/10.48550/arXiv.1103.5263>