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

Fix bug when trying to create a rotation matrix with a zero angle.

This commit is contained in:
Sébastien Crozet 2013-06-30 21:19:36 +00:00
parent 02e22717a4
commit 6fd9696253
2 changed files with 40 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
src/adaptors/rotmat.rs
View File

@ -37,32 +37,37 @@ pub fn rotmat2<N: Copy + Trigonometric + Neg<N>>(angle: N) -> Rotmat<Mat2<N>>
pub fn rotmat3<N: Copy + Trigonometric + DivisionRing + Algebraic> pub fn rotmat3<N: Copy + Trigonometric + DivisionRing + Algebraic>
(axisangle: Vec3<N>) -> Rotmat<Mat3<N>> (axisangle: Vec3<N>) -> Rotmat<Mat3<N>>
{ {
let mut axis = axisangle; if axisangle.sqnorm().is_zero()
let angle = axis.normalize(); { One::one() }
let _1 = One::one::<N>(); else
let ux = copy axis.at[0]; {
let uy = copy axis.at[1]; let mut axis = axisangle;
let uz = copy axis.at[2]; let angle = axis.normalize();
let sqx = ux * ux; let _1 = One::one::<N>();
let sqy = uy * uy; let ux = copy axis.at[0];
let sqz = uz * uz; let uy = copy axis.at[1];
let cos = angle.cos(); let uz = copy axis.at[2];
let one_m_cos = _1 - cos; let sqx = ux * ux;
let sin = angle.sin(); let sqy = uy * uy;
let sqz = uz * uz;
let cos = angle.cos();
let one_m_cos = _1 - cos;
let sin = angle.sin();
Rotmat { Rotmat {
submat: Mat3::new( [ submat: Mat3::new( [
(sqx + (_1 - sqx) * cos), (sqx + (_1 - sqx) * cos),
(ux * uy * one_m_cos - uz * sin), (ux * uy * one_m_cos - uz * sin),
(ux * uz * one_m_cos + uy * sin), (ux * uz * one_m_cos + uy * sin),
(ux * uy * one_m_cos + uz * sin), (ux * uy * one_m_cos + uz * sin),
(sqy + (_1 - sqy) * cos), (sqy + (_1 - sqy) * cos),
(uy * uz * one_m_cos - ux * sin), (uy * uz * one_m_cos - ux * sin),
(ux * uz * one_m_cos - uy * sin), (ux * uz * one_m_cos - uy * sin),
(uy * uz * one_m_cos + ux * sin), (uy * uz * one_m_cos + ux * sin),
(sqz + (_1 - sqz) * cos) ] ) (sqz + (_1 - sqz) * cos) ] )
}
} }
} }

17
src/tests/mat.rs
View File

@ -15,7 +15,7 @@ use traits::transpose::Transpose;
#[test] #[test]
use vec::Vec1; use vec::Vec1;
#[test] #[test]
use mat::{Mat1, Mat2, Mat3}; use mat::{Mat1, Mat2, Mat3, Mat4, Mat5, Mat6};
#[test] #[test]
use adaptors::rotmat::Rotmat; use adaptors::rotmat::Rotmat;
@ -42,10 +42,17 @@ fn test_inv_mat2()
fn test_inv_mat3() fn test_inv_mat3()
{ test_inv_mat_impl!(Mat3<f64>); } { test_inv_mat_impl!(Mat3<f64>); }
// FIXME: ICE #[test]
// #[test] fn test_inv_mat4()
// fn test_inv_nmat() { test_inv_mat_impl!(Mat4<f64>); }
// { test_inv_mat_impl!(NMat<d7, f64>); }
#[test]
fn test_inv_mat5()
{ test_inv_mat_impl!(Mat5<f64>); }
#[test]
fn test_inv_mat6()
{ test_inv_mat_impl!(Mat6<f64>); }
#[test] #[test]
fn test_rotation2() fn test_rotation2()