hodasemi/cgmath
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Merge pull request #284 from DerekBurch/fix-matrix3-to-quaternion

Browse source 
Fix conversion from Matrix3 to Quaternion
This commit is contained in:
Brendan Zabarauskas 2015-12-23 13:12:03 +11:00
commit 0875ea4444
2 changed files with 53 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

30
src/matrix.rs
View file

@ -1084,28 +1084,28 @@ impl<S: BaseFloat> From<Matrix3<S>> for Quaternion<S> {
let z = (mat[0][1] - mat[1][0]) * s;
Quaternion::new(w, x, y, z)
} else if (mat[0][0] > mat[1][1]) && (mat[0][0] > mat[2][2]) {
let s = (half + (mat[0][0] - mat[1][1] - mat[2][2])).sqrt();
let w = half * s;
let s = ((mat[0][0] - mat[1][1] - mat[2][2]) + S::one()).sqrt();
let x = half * s;
let s = half / s;
let x = (mat[0][1] - mat[1][0]) * s;
let y = (mat[2][0] - mat[0][2]) * s;
let z = (mat[1][2] - mat[2][1]) * s;
let y = (mat[1][0] + mat[0][1]) * s;
let z = (mat[0][2] + mat[2][0]) * s;
let w = (mat[1][2] - mat[2][1]) * s;
Quaternion::new(w, x, y, z)
} else if mat[1][1] > mat[2][2] {
let s = (half + (mat[1][1] - mat[0][0] - mat[2][2])).sqrt();
let w = half * s;
let s = ((mat[1][1] - mat[0][0] - mat[2][2]) + S::one()).sqrt();
let y = half * s;
let s = half / s;
let x = (mat[0][1] - mat[1][0]) * s;
let y = (mat[1][2] - mat[2][1]) * s;
let z = (mat[2][0] - mat[0][2]) * s;
let z = (mat[2][1] + mat[1][2]) * s;
let x = (mat[1][0] + mat[0][1]) * s;
let w = (mat[2][0] - mat[0][2]) * s;
Quaternion::new(w, x, y, z)
} else {
let s = (half + (mat[2][2] - mat[0][0] - mat[1][1])).sqrt();
let w = half * s;
let s = ((mat[2][2] - mat[0][0] - mat[1][1]) + S::one()).sqrt();
let z = half * s;
let s = half / s;
let x = (mat[2][0] - mat[0][2]) * s;
let y = (mat[1][2] - mat[2][1]) * s;
let z = (mat[0][1] - mat[1][0]) * s;
let x = (mat[0][2] + mat[2][0]) * s;
let y = (mat[2][1] + mat[1][2]) * s;
let w = (mat[0][1] - mat[1][0]) * s;
Quaternion::new(w, x, y, z)
}
}

38
tests/matrix.rs
View file

@ -13,6 +13,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#[macro_use]
extern crate cgmath;
use cgmath::*;
@ -365,3 +366,40 @@ fn test_from_angle() {
let rot3: Matrix2<f64> = Matrix2::from_angle(rad(f64::consts::PI));
assert!((rot3 * Vector2::new(1.0, 1.0)).approx_eq(&Vector2::new(-1.0, -1.0)));
}
fn test_matrix3_into_quaternion_for_angles(x: Rad<f32>, y: Rad<f32>, z: Rad<f32>) {
let matrix3: Matrix3<f32> = Matrix3::from_euler(x, y, z);
let quaternion: Quaternion<f32> = matrix3.into();
let quaternion_matrix3: Matrix3<f32> = quaternion.into();
assert_approx_eq!(matrix3, quaternion_matrix3);
}
// The next 4 tests trigger each of the cases in the impl for
// conversion from Matrix3 to Quaternion
#[test]
fn test_matrix3_into_quaternion_positive_trace()
{
// trigger the case: trace >= S::zero()
test_matrix3_into_quaternion_for_angles(rad(0.0f32), rad(0.0), rad(0.0f32));
}
#[test]
fn test_matrix3_into_quaternion_xx_maximum()
{
// trigger the case: (mat[0][0] > mat[1][1]) && (mat[0][0] > mat[2][2])
test_matrix3_into_quaternion_for_angles(rad(2.0f32), rad(1.0), rad(-1.2f32));
}
#[test]
fn test_matrix3_into_quaternion_yy_maximum()
{
// trigger the case: mat[1][1] > mat[2][2]
test_matrix3_into_quaternion_for_angles(rad(2.0f32), rad(1.0), rad(3.0f32));
}
#[test]
fn test_matrix3_into_quaternion_zz_maximum()
{
// trigger the else case
test_matrix3_into_quaternion_for_angles(rad(1.0f32), rad(1.0), rad(3.0f32));
}