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>
(axisangle: Vec3<N>) -> Rotmat<Mat3<N>>
{
let mut axis = axisangle;
let angle = axis.normalize();
let _1 = One::one::<N>();
let ux = copy axis.at[0];
let uy = copy axis.at[1];
let uz = copy axis.at[2];
let sqx = ux * ux;
let sqy = uy * uy;
let sqz = uz * uz;
let cos = angle.cos();
let one_m_cos = _1 - cos;
let sin = angle.sin();
if axisangle.sqnorm().is_zero()
{ One::one() }
else
{
let mut axis = axisangle;
let angle = axis.normalize();
let _1 = One::one::<N>();
let ux = copy axis.at[0];
let uy = copy axis.at[1];
let uz = copy axis.at[2];
let sqx = ux * ux;
let sqy = uy * uy;
let sqz = uz * uz;
let cos = angle.cos();
let one_m_cos = _1 - cos;
let sin = angle.sin();
Rotmat {
submat: Mat3::new( [
(sqx + (_1 - sqx) * cos),
(ux * uy * one_m_cos - uz * sin),
(ux * uz * one_m_cos + uy * sin),
Rotmat {
submat: Mat3::new( [
(sqx + (_1 - sqx) * cos),
(ux * uy * one_m_cos - uz * sin),
(ux * uz * one_m_cos + uy * sin),
(ux * uy * one_m_cos + uz * sin),
(sqy + (_1 - sqy) * cos),
(uy * uz * one_m_cos - ux * sin),
(ux * uy * one_m_cos + uz * sin),
(sqy + (_1 - sqy) * cos),
(uy * uz * one_m_cos - ux * sin),
(ux * uz * one_m_cos - uy * sin),
(uy * uz * one_m_cos + ux * sin),
(sqz + (_1 - sqz) * cos) ] )
(ux * uz * one_m_cos - uy * sin),
(uy * uz * one_m_cos + ux * sin),
(sqz + (_1 - sqz) * cos) ] )
}
}
}

17
src/tests/mat.rs
View File

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