formatting
This commit is contained in:
parent
686ed10624
commit
063140d533
|
@ -1,5 +1,7 @@
|
||||||
use crate::{RealField, Rotation, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion};
|
use crate::{Allocator, ArrayStorage, Const, DefaultAllocator, DimDiff, DimSub, Storage, U1};
|
||||||
use crate::{Const, U1, DimSub, DimDiff, Storage, ArrayStorage, Allocator, DefaultAllocator};
|
use crate::{
|
||||||
|
RealField, Rotation, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion,
|
||||||
|
};
|
||||||
|
|
||||||
/// # Interpolation
|
/// # Interpolation
|
||||||
impl<T: SimdRealField> Rotation2<T> {
|
impl<T: SimdRealField> Rotation2<T> {
|
||||||
|
@ -81,14 +83,15 @@ impl<T: SimdRealField> Rotation3<T> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<T:RealField, const D: usize> Rotation<T,D> where
|
impl<T: RealField, const D: usize> Rotation<T, D>
|
||||||
|
where
|
||||||
Const<D>: DimSub<U1>,
|
Const<D>: DimSub<U1>,
|
||||||
ArrayStorage<T,D,D>: Storage<T,Const<D>,Const<D>>,
|
ArrayStorage<T, D, D>: Storage<T, Const<D>, Const<D>>,
|
||||||
DefaultAllocator: Allocator<T,Const<D>,Const<D>,Buffer=ArrayStorage<T,D,D>> + Allocator<T,Const<D>> +
|
DefaultAllocator: Allocator<T, Const<D>, Const<D>, Buffer = ArrayStorage<T, D, D>>
|
||||||
Allocator<T,Const<D>,DimDiff<Const<D>,U1>> +
|
+ Allocator<T, Const<D>>
|
||||||
Allocator<T,DimDiff<Const<D>,U1>>
|
+ Allocator<T, Const<D>, DimDiff<Const<D>, U1>>
|
||||||
|
+ Allocator<T, DimDiff<Const<D>, U1>>,
|
||||||
{
|
{
|
||||||
|
|
||||||
///
|
///
|
||||||
/// Computes the spherical linear interpolation between two general rotations.
|
/// Computes the spherical linear interpolation between two general rotations.
|
||||||
///
|
///
|
||||||
|
@ -109,15 +112,13 @@ impl<T:RealField, const D: usize> Rotation<T,D> where
|
||||||
//from SimdRealField to RealField
|
//from SimdRealField to RealField
|
||||||
#[inline]
|
#[inline]
|
||||||
#[must_use]
|
#[must_use]
|
||||||
pub fn slerp(&self, other: &Self, t:T) -> Self {
|
pub fn slerp(&self, other: &Self, t: T) -> Self {
|
||||||
|
|
||||||
use std::mem::transmute;
|
use std::mem::transmute;
|
||||||
|
|
||||||
//The best option here would be to use #[feature(specialization)], but until
|
//The best option here would be to use #[feature(specialization)], but until
|
||||||
//that's stabilized, this is the best we can do. Theoretically, the compiler should
|
//that's stabilized, this is the best we can do. Theoretically, the compiler should
|
||||||
//pretty thoroughly optimize away all the excess checks and conversions
|
//pretty thoroughly optimize away all the excess checks and conversions
|
||||||
match D {
|
match D {
|
||||||
|
|
||||||
0 => self.clone(),
|
0 => self.clone(),
|
||||||
|
|
||||||
//FIXME: this doesn't really work in 1D since we can't interp between -1 and 1
|
//FIXME: this doesn't really work in 1D since we can't interp between -1 and 1
|
||||||
|
@ -127,23 +128,21 @@ impl<T:RealField, const D: usize> Rotation<T,D> where
|
||||||
//NOTE: This is safe because we directly check the dimension first
|
//NOTE: This is safe because we directly check the dimension first
|
||||||
2 => unsafe {
|
2 => unsafe {
|
||||||
let (self2d, other2d) = (
|
let (self2d, other2d) = (
|
||||||
transmute::<&Self,&Rotation2<T>>(self),
|
transmute::<&Self, &Rotation2<T>>(self),
|
||||||
transmute::<&Self,&Rotation2<T>>(other),
|
transmute::<&Self, &Rotation2<T>>(other),
|
||||||
);
|
);
|
||||||
transmute::<&Rotation2<T>,&Self>(&self2d.slerp_2d(other2d, t)).clone()
|
transmute::<&Rotation2<T>, &Self>(&self2d.slerp_2d(other2d, t)).clone()
|
||||||
},
|
},
|
||||||
3 => unsafe {
|
3 => unsafe {
|
||||||
let (self3d, other3d) = (
|
let (self3d, other3d) = (
|
||||||
transmute::<&Self,&Rotation3<T>>(self),
|
transmute::<&Self, &Rotation3<T>>(self),
|
||||||
transmute::<&Self,&Rotation3<T>>(other),
|
transmute::<&Self, &Rotation3<T>>(other),
|
||||||
);
|
);
|
||||||
transmute::<&Rotation3<T>,&Self>(&self3d.slerp_3d(other3d, t)).clone()
|
transmute::<&Rotation3<T>, &Self>(&self3d.slerp_3d(other3d, t)).clone()
|
||||||
},
|
},
|
||||||
|
|
||||||
//the multiplication order matters here
|
//the multiplication order matters here
|
||||||
_ => (other/self).powf(t) * self
|
_ => (other / self).powf(t) * self,
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -15,11 +15,11 @@ use simba::scalar::RealField;
|
||||||
use simba::simd::{SimdBool, SimdRealField};
|
use simba::simd::{SimdBool, SimdRealField};
|
||||||
use std::ops::Neg;
|
use std::ops::Neg;
|
||||||
|
|
||||||
use crate::base::dimension::{Const, U1, U2, U3, DimSub, DimDiff};
|
|
||||||
use crate::base::allocator::Allocator;
|
use crate::base::allocator::Allocator;
|
||||||
|
use crate::base::dimension::{Const, DimDiff, DimSub, U1, U2, U3};
|
||||||
use crate::base::storage::Storage;
|
use crate::base::storage::Storage;
|
||||||
use crate::base::{ Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3};
|
|
||||||
use crate::base::{ArrayStorage, DefaultAllocator};
|
use crate::base::{ArrayStorage, DefaultAllocator};
|
||||||
|
use crate::base::{Matrix2, Matrix3, SMatrix, SVector, Unit, Vector, Vector1, Vector2, Vector3};
|
||||||
|
|
||||||
use crate::geometry::{Rotation, Rotation2, Rotation3, UnitComplex, UnitQuaternion};
|
use crate::geometry::{Rotation, Rotation2, Rotation3, UnitComplex, UnitQuaternion};
|
||||||
|
|
||||||
|
@ -1007,9 +1007,7 @@ where
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<T:RealField, const D: usize> Rotation<T,D>
|
impl<T: RealField, const D: usize> Rotation<T, D> {
|
||||||
{
|
|
||||||
|
|
||||||
///
|
///
|
||||||
/// Raise the rotation to a given floating power, i.e., returns the rotation with the same
|
/// Raise the rotation to a given floating power, i.e., returns the rotation with the same
|
||||||
/// axis as `self` and an angle equal to `self.angle()` multiplied by `n`.
|
/// axis as `self` and an angle equal to `self.angle()` multiplied by `n`.
|
||||||
|
@ -1028,12 +1026,14 @@ impl<T:RealField, const D: usize> Rotation<T,D>
|
||||||
/// assert_eq!(pow.angle(), 2.4);
|
/// assert_eq!(pow.angle(), 2.4);
|
||||||
/// ```
|
/// ```
|
||||||
//FIXME: merging powf for Rotation2 into this raises the trait bounds from SimdRealField to RealField
|
//FIXME: merging powf for Rotation2 into this raises the trait bounds from SimdRealField to RealField
|
||||||
pub fn powf(&self, t: T) -> Self where
|
pub fn powf(&self, t: T) -> Self
|
||||||
|
where
|
||||||
Const<D>: DimSub<U1>,
|
Const<D>: DimSub<U1>,
|
||||||
ArrayStorage<T,D,D>: Storage<T,Const<D>,Const<D>>,
|
ArrayStorage<T, D, D>: Storage<T, Const<D>, Const<D>>,
|
||||||
DefaultAllocator: Allocator<T,Const<D>,Const<D>,Buffer=ArrayStorage<T,D,D>> + Allocator<T,Const<D>> +
|
DefaultAllocator: Allocator<T, Const<D>, Const<D>, Buffer = ArrayStorage<T, D, D>>
|
||||||
Allocator<T,Const<D>,DimDiff<Const<D>,U1>> +
|
+ Allocator<T, Const<D>>
|
||||||
Allocator<T,DimDiff<Const<D>,U1>>
|
+ Allocator<T, Const<D>, DimDiff<Const<D>, U1>>
|
||||||
|
+ Allocator<T, DimDiff<Const<D>, U1>>,
|
||||||
{
|
{
|
||||||
use std::mem::*;
|
use std::mem::*;
|
||||||
|
|
||||||
|
@ -1041,33 +1041,36 @@ impl<T:RealField, const D: usize> Rotation<T,D>
|
||||||
//that's stabilized, this is the best we can do. Theoretically, the compiler should
|
//that's stabilized, this is the best we can do. Theoretically, the compiler should
|
||||||
//pretty thoroughly optimize away all the excess checks and conversions
|
//pretty thoroughly optimize away all the excess checks and conversions
|
||||||
match D {
|
match D {
|
||||||
|
|
||||||
0 => self.clone(),
|
0 => self.clone(),
|
||||||
1 => self.clone(),
|
1 => self.clone(),
|
||||||
|
|
||||||
//NOTE: Not pretty, but without refactoring the API, this is the best we can do
|
//NOTE: Not pretty, but without refactoring the API, this is the best we can do
|
||||||
//NOTE: This is safe because we directly check the dimension first
|
//NOTE: This is safe because we directly check the dimension first
|
||||||
2 => unsafe {
|
2 => unsafe {
|
||||||
let r2d = transmute::<&Self,&Rotation2<T>>(self).powf_2d(t);
|
let r2d = transmute::<&Self, &Rotation2<T>>(self).powf_2d(t);
|
||||||
transmute::<&Rotation2<T>,&Self>(&r2d).clone()
|
transmute::<&Rotation2<T>, &Self>(&r2d).clone()
|
||||||
},
|
},
|
||||||
3 => unsafe {
|
3 => unsafe {
|
||||||
let r3d = transmute::<&Self,&Rotation3<T>>(self).powf_3d(t);
|
let r3d = transmute::<&Self, &Rotation3<T>>(self).powf_3d(t);
|
||||||
transmute::<&Rotation3<T>,&Self>(&r3d).clone()
|
transmute::<&Rotation3<T>, &Self>(&r3d).clone()
|
||||||
},
|
},
|
||||||
|
|
||||||
_ => self.clone().general_pow(t)
|
_ => self.clone().general_pow(t),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fn general_pow(self, t:T) -> Self where
|
fn general_pow(self, t: T) -> Self
|
||||||
|
where
|
||||||
Const<D>: DimSub<U1>,
|
Const<D>: DimSub<U1>,
|
||||||
ArrayStorage<T,D,D>: Storage<T,Const<D>,Const<D>>,
|
ArrayStorage<T, D, D>: Storage<T, Const<D>, Const<D>>,
|
||||||
DefaultAllocator: Allocator<T,Const<D>,Const<D>,Buffer=ArrayStorage<T,D,D>> + Allocator<T,Const<D>> +
|
DefaultAllocator: Allocator<T, Const<D>, Const<D>, Buffer = ArrayStorage<T, D, D>>
|
||||||
Allocator<T,Const<D>,DimDiff<Const<D>,U1>> +
|
+ Allocator<T, Const<D>>
|
||||||
Allocator<T,DimDiff<Const<D>,U1>>
|
+ Allocator<T, Const<D>, DimDiff<Const<D>, U1>>
|
||||||
|
+ Allocator<T, DimDiff<Const<D>, U1>>,
|
||||||
{
|
{
|
||||||
if D<=1 { return self; }
|
if D <= 1 {
|
||||||
|
return self;
|
||||||
|
}
|
||||||
|
|
||||||
// println!("r:{}", self);
|
// println!("r:{}", self);
|
||||||
// println!("{}", self.clone().into_inner().hessenberg().unpack_h());
|
// println!("{}", self.clone().into_inner().hessenberg().unpack_h());
|
||||||
|
@ -1082,8 +1085,7 @@ impl<T:RealField, const D: usize> Rotation<T,D>
|
||||||
|
|
||||||
//go down the diagonal and pow every block
|
//go down the diagonal and pow every block
|
||||||
let mut i = 0;
|
let mut i = 0;
|
||||||
while i < D-1 {
|
while i < D - 1 {
|
||||||
|
|
||||||
if
|
if
|
||||||
//For most 2x2 blocks
|
//For most 2x2 blocks
|
||||||
//NOTE: we use strict equality since `nalgebra`'s schur decomp sets the infradiagonal to zero
|
//NOTE: we use strict equality since `nalgebra`'s schur decomp sets the infradiagonal to zero
|
||||||
|
@ -1092,9 +1094,8 @@ impl<T:RealField, const D: usize> Rotation<T,D>
|
||||||
//for +-180 deg rotations
|
//for +-180 deg rotations
|
||||||
d[(i,i)]<T::zero() && d[(i+1,i+1)]<T::zero()
|
d[(i,i)]<T::zero() && d[(i+1,i+1)]<T::zero()
|
||||||
{
|
{
|
||||||
|
|
||||||
//convert to a complex num and find the arg()
|
//convert to a complex num and find the arg()
|
||||||
let (c, s) = (d[(i,i)].clone(), d[(i+1,i)].clone());
|
let (c, s) = (d[(i, i)].clone(), d[(i + 1, i)].clone());
|
||||||
let angle = s.atan2(c); //for +-180deg rots, this implicitely takes the +180 branch
|
let angle = s.atan2(c); //for +-180deg rots, this implicitely takes the +180 branch
|
||||||
|
|
||||||
//scale the arg and exponentiate back
|
//scale the arg and exponentiate back
|
||||||
|
@ -1102,26 +1103,22 @@ impl<T:RealField, const D: usize> Rotation<T,D>
|
||||||
let (s2, c2) = angle2.sin_cos();
|
let (s2, c2) = angle2.sin_cos();
|
||||||
|
|
||||||
//convert back into a rot block
|
//convert back into a rot block
|
||||||
d[(i, i )] = c2.clone();
|
d[(i, i)] = c2.clone();
|
||||||
d[(i, i+1)] = -s2.clone();
|
d[(i, i + 1)] = -s2.clone();
|
||||||
d[(i+1,i )] = s2;
|
d[(i + 1, i)] = s2;
|
||||||
d[(i+1,i+1)] = c2;
|
d[(i + 1, i + 1)] = c2;
|
||||||
|
|
||||||
//increase by 2 so we don't accidentally misinterpret the
|
//increase by 2 so we don't accidentally misinterpret the
|
||||||
//next line as a 180deg rotation
|
//next line as a 180deg rotation
|
||||||
i += 2;
|
i += 2;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
i += 1;
|
i += 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
// println!("d:{:.3}", d);
|
// println!("d:{:.3}", d);
|
||||||
|
|
||||||
let qt = q.transpose(); //avoids an extra clone
|
let qt = q.transpose(); //avoids an extra clone
|
||||||
|
|
||||||
Self::from_matrix_unchecked(q * d * qt)
|
Self::from_matrix_unchecked(q * d * qt)
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -32,9 +32,9 @@ fn quaternion_euler_angles_issue_494() {
|
||||||
|
|
||||||
#[cfg(feature = "proptest-support")]
|
#[cfg(feature = "proptest-support")]
|
||||||
mod proptest_tests {
|
mod proptest_tests {
|
||||||
use na::{self, Rotation, Rotation2, Rotation3, Unit, Vector, Matrix, SMatrix};
|
use na::{self, Matrix, Rotation, Rotation2, Rotation3, SMatrix, Unit, Vector};
|
||||||
use simba::scalar::RealField;
|
|
||||||
use num_traits::Zero;
|
use num_traits::Zero;
|
||||||
|
use simba::scalar::RealField;
|
||||||
use std::f64;
|
use std::f64;
|
||||||
|
|
||||||
use crate::proptest::*;
|
use crate::proptest::*;
|
||||||
|
@ -338,7 +338,6 @@ mod proptest_tests {
|
||||||
)*}
|
)*}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
gen_powf_rotation_test!(
|
gen_powf_rotation_test!(
|
||||||
fn powf_rotation_4(v1 in vector4(), v2 in vector4(), v3 in vector4(), v4 in vector4());
|
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_5(v1 in vector5(), v2 in vector5(), v3 in vector5(), v4 in vector5());
|
||||||
|
|
Loading…
Reference in New Issue