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

generalized powf rotation test

Browse Source
This commit is contained in:
Joshua Smith 2022-03-24 14:49:09 -05:00
parent e52057b8a5
commit d4cba8f76f
2 changed files with 68 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/geometry/rotation_interpolation.rs
View File

@ -118,7 +118,7 @@ impl<T:RealField, const D: usize> Rotation<T,D> where
pub fn general_pow(self, t:T) -> Self {
if D<=1 { return self; }
println!("r:{}", self);
// println!("r:{}", self);
//taking the (real) schur form is guaranteed to produce a block-diagonal matrix
//where each block is either a 1 (if there's no rotation in that axis) or a 2x2
@ -126,7 +126,7 @@ impl<T:RealField, const D: usize> Rotation<T,D> where
let schur = self.into_inner().schur();
let (q, mut d) = schur.unpack();
println!("q:{}d:{:.3}", q, d);
// println!("q:{}d:{:.3}", q, d);
//go down the diagonal and pow every block
for i in 0..(D-1) {
@ -135,13 +135,13 @@ impl<T:RealField, const D: usize> Rotation<T,D> where
//NOTE: the impl of the schur decomposition always sets the inferior diagonal to 0
if !d[(i+1,i)].is_zero() {
println!("{}", i);
// println!("{}", i);
//convert to a complex num and take the arg()
let (c, s) = (d[(i,i)].clone(), d[(i+1,i)].clone());
let angle = s.atan2(c);
println!("{}", angle);
// println!("{}", angle);
//scale the arg and exponentiate back
let angle2 = angle * t.clone();
@ -156,7 +156,7 @@ impl<T:RealField, const D: usize> Rotation<T,D> where
}
}
println!("d:{:.3}", d);
// println!("d:{:.3}", d);
let qt = q.transpose(); //avoids an extra clone

136
tests/geometry/rotation.rs
View File

@ -39,6 +39,69 @@ mod proptest_tests {
use crate::proptest::*;
use proptest::{prop_assert, prop_assert_eq, proptest};
macro_rules! gen_powf_rotation_test {
($(
fn $powf_rot_n:ident($($v1:ident in $vec1:ident(), $v2:ident in $vec2:ident()),*);
)*) => {
proptest!{$(
#[test]
fn $powf_rot_n(
$($v1 in $vec1(), $v2 in $vec2(),)*
pow in PROPTEST_F64
) {
use nalgebra::*;
use num_traits::Zero;
//make an orthonormal basis
let mut basis = [$($v1, $v2),*];
Vector::orthonormalize(&mut basis);
let [$($v1, $v2),*] = basis;
//"wedge" the vectors to make an arrary 2-blades representing rotation planes.
let mut bivectors = [
//Since we start with vector pairs, each bivector is guaranteed to be simple
$($v1.transpose().kronecker(&$v2) - $v2.transpose().kronecker(&$v1)),*
];
//condition the bivectors
for b in &mut bivectors {
if let Some((unit, norm)) = Unit::try_new_and_get(*b, 0.0) {
//every component is duplicated once, so there's an extra factor of
//sqrt(2) in the norm
let mut angle = norm / 2.0f64.sqrt();
angle = na::wrap(angle, -f64::pi(), f64::pi());
*b = unit.into_inner() * angle * 2.0f64.sqrt();
}
}
let mut bivector = bivectors[0].clone();
for i in 1..bivectors.len() {
bivector += bivectors[i];
}
let r1 = Rotation::from_matrix_unchecked(bivector.exp()).general_pow(pow);
let r2 = Rotation::from_matrix_unchecked((bivector * pow).exp());
prop_assert!(relative_eq!(r1, r2, epsilon=1e-7));
}
)*}
}
}
gen_powf_rotation_test!(
fn powf_rotation_4(v1 in vector4(), v2 in vector4(), v3 in vector4(), v4 in vector4());
fn powf_rotation_5(v1 in vector5(), v2 in vector5(), v3 in vector5(), v4 in vector5());
fn powf_rotation_6(
v1 in vector6(), v2 in vector6(),
v3 in vector6(), v4 in vector6(),
v5 in vector6(), v6 in vector6()
);
);
proptest! {
/*
*
@ -230,78 +293,5 @@ mod proptest_tests {
}
}
// macro_rules! gen_pof_rotation_test {
// ($(
// fn $powf_rot_n:ident($($v1:ident in $vec1:ident(), $v2:ident in $vec2:ident()),*);
// )*) => {$
//
// #[test]
// fn $powf_rot_n(
// $($v1 in $vec1(), $v2 in $vec2(),)*
// pow in PROPTEST_F64
// ) {
//
// }
//
// )*}
// }
#[test]
fn powf_rotation_4(
v1 in vector4(), v2 in vector4(),
v3 in vector4(), v4 in vector4(),
pow in PROPTEST_F64
) {
use nalgebra::*;
use num_traits::Zero;
type Rotation4<T> = Rotation<T,4>;
//make an orthonormal basis
let mut basis = [v1,v2,v3,v4];
Vector::orthonormalize(&mut basis);
let [v1,v2,v3,v4] = basis;
//"wedge" the vectors to make two 2-blades representing two rotation planes
//since we start with vector pairs, each bivector is guaranteed to be simple
let mut b1 = v1.transpose().kronecker(&v2) - v2.transpose().kronecker(&v1);
let mut b2 = v3.transpose().kronecker(&v4) - v4.transpose().kronecker(&v3);
//condition b1
if let Some((unit, norm)) = Unit::try_new_and_get(b1, 0.0) {
//every component is duplicated once, so there's an extra factor or sqrt(2) in the norm
//and wrap angle into the correct range
let mut angle = norm / 2.0f64.sqrt();
angle = na::wrap(angle, -f64::pi(), f64::pi());
b1 = unit.into_inner() * angle * 2.0f64.sqrt();
}
//condition b2
if let Some((unit, norm)) = Unit::try_new_and_get(b2, 0.0) {
let mut angle = norm / 2.0f64.sqrt();
angle = na::wrap(angle, -f64::pi(), f64::pi());
b2 = unit.into_inner() * angle * 2.0f64.sqrt();
}
let bivector = b1+b2;
println!("b:{:.3}", bivector);
let r1 = Rotation4::from_matrix_unchecked(bivector.exp());
let r2 = Rotation4::from_matrix_unchecked((bivector * pow).exp());
// println!("{}{}", r1, r2);
// println!("{}", r1.general_pow(pow));
prop_assert!(
relative_eq!(r1.general_pow(pow), r2, epsilon=1e-7)
);
}
}
}