#[cfg(test)]
mod math_test {
use maliput_sys::math::ffi::Vector3_cross;
use maliput_sys::math::ffi::Vector3_dot;
use maliput_sys::math::ffi::Vector3_equals;
use maliput_sys::math::ffi::Vector3_new;
use maliput_sys::math::ffi::Vector4_dot;
use maliput_sys::math::ffi::Vector4_equals;
use maliput_sys::math::ffi::Vector4_new;
use maliput_sys::math::ffi::Matrix3_adjoint;
use maliput_sys::math::ffi::Matrix3_cofactor_matrix;
use maliput_sys::math::ffi::Matrix3_col;
use maliput_sys::math::ffi::Matrix3_equals;
use maliput_sys::math::ffi::Matrix3_inverse;
use maliput_sys::math::ffi::Matrix3_new;
use maliput_sys::math::ffi::Matrix3_operator_divide_by_scalar;
use maliput_sys::math::ffi::Matrix3_operator_mul;
use maliput_sys::math::ffi::Matrix3_operator_sub;
use maliput_sys::math::ffi::Matrix3_operator_sum;
use maliput_sys::math::ffi::Matrix3_row;
use maliput_sys::math::ffi::Matrix3_to_str;
use maliput_sys::math::ffi::Matrix3_transpose;
use maliput_sys::math::ffi::RollPitchYaw_CalcRotationMatrixDt;
use maliput_sys::math::ffi::RollPitchYaw_SetFromQuaternion;
use maliput_sys::math::ffi::RollPitchYaw_ToMatrix;
use maliput_sys::math::ffi::RollPitchYaw_ToQuaternion;
use maliput_sys::math::ffi::RollPitchYaw_new;
use maliput_sys::math::ffi::RollPitchYaw_set;
use maliput_sys::math::ffi::Quaternion_Inverse;
use maliput_sys::math::ffi::Quaternion_IsApprox;
use maliput_sys::math::ffi::Quaternion_ToRotationMatrix;
use maliput_sys::math::ffi::Quaternion_TransformVector;
use maliput_sys::math::ffi::Quaternion_coeffs;
use maliput_sys::math::ffi::Quaternion_conjugate;
use maliput_sys::math::ffi::Quaternion_equals;
use maliput_sys::math::ffi::Quaternion_new;
use maliput_sys::math::ffi::Quaternion_to_str;
use maliput_sys::math::ffi::Quaternion_vec;
use maliput_sys::math::ffi::BoundingBox_get_vertices;
use maliput_sys::math::ffi::BoundingBox_new;
#[test]
fn vector3_new() {
let v = Vector3_new(1.0, 2.0, 3.0);
assert_eq!(v.x(), 1.0);
assert_eq!(v.y(), 2.0);
assert_eq!(v.z(), 3.0);
}
#[test]
fn vector3_norm() {
let v = Vector3_new(1.0, 2.0, 3.0);
assert_eq!(v.norm(), (v.x() * v.x() + v.y() * v.y() + v.z() * v.z()).sqrt());
}
#[test]
fn vector3_normalize() {
let mut v = Vector3_new(1.0, 2.0, 3.0);
let norm = v.norm();
v.as_mut().expect("").normalize();
assert_eq!(v.x(), 1.0 / norm);
assert_eq!(v.y(), 2.0 / norm);
assert_eq!(v.z(), 3.0 / norm);
}
#[test]
fn vector3_dot() {
let v = Vector3_new(1.0, 2.0, 3.0);
let w = Vector3_new(4.0, 5.0, 6.0);
assert_eq!(Vector3_dot(&v, &w), v.x() * w.x() + v.y() * w.y() + v.z() * w.z());
}
#[test]
fn vector3_cross() {
let v = Vector3_new(1.0, 2.0, 3.0);
let w = Vector3_new(4.0, 5.0, 6.0);
let cross = Vector3_cross(&v, &w);
assert_eq!(cross.x(), v.y() * w.z() - v.z() * w.y());
assert_eq!(cross.y(), v.z() * w.x() - v.x() * w.z());
assert_eq!(cross.z(), v.x() * w.y() - v.y() * w.x());
}
#[test]
fn vector3_equals() {
let v = Vector3_new(1.0, 2.0, 3.0);
let w = Vector3_new(1.0, 2.0, 3.0);
assert!(Vector3_equals(&v, &w));
}
#[test]
fn vector4_new() {
let v = Vector4_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(v.x(), 1.0);
assert_eq!(v.y(), 2.0);
assert_eq!(v.z(), 3.0);
assert_eq!(v.w(), 4.0);
}
#[test]
fn vector4_norm() {
let v = Vector4_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(
v.norm(),
(v.x() * v.x() + v.y() * v.y() + v.z() * v.z() + v.w() * v.w()).sqrt()
);
}
#[test]
fn vector4_normalize() {
let mut v = Vector4_new(1.0, 2.0, 3.0, 4.0);
let norm = v.norm();
v.as_mut().expect("").normalize();
assert_eq!(v.x(), 1.0 / norm);
assert_eq!(v.y(), 2.0 / norm);
assert_eq!(v.z(), 3.0 / norm);
assert_eq!(v.w(), 4.0 / norm);
}
#[test]
fn vector4_dot() {
let v = Vector4_new(1.0, 2.0, 3.0, 4.0);
let w = Vector4_new(5.0, 6.0, 7.0, 8.0);
assert_eq!(
Vector4_dot(&v, &w),
v.x() * w.x() + v.y() * w.y() + v.z() * w.z() + v.w() * w.w()
);
}
#[test]
fn vector4_equals() {
let v = Vector4_new(1.0, 2.0, 3.0, 4.0);
let w = Vector4_new(1.0, 2.0, 3.0, 4.0);
assert!(Vector4_equals(&v, &w));
}
#[test]
fn matrix3_new() {
let m = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
let row1 = Matrix3_row(&m, 0);
let row2 = Matrix3_row(&m, 1);
let row3 = Matrix3_row(&m, 2);
assert_eq!(row1.x(), 1.0);
assert_eq!(row1.y(), 2.0);
assert_eq!(row1.z(), 3.0);
assert_eq!(row2.x(), 4.0);
assert_eq!(row2.y(), 5.0);
assert_eq!(row2.z(), 6.0);
assert_eq!(row3.x(), 7.0);
assert_eq!(row3.y(), 8.0);
assert_eq!(row3.z(), 9.0);
}
#[test]
fn matrix3_col() {
let m = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
let col1 = Matrix3_col(&m, 0);
let col2 = Matrix3_col(&m, 1);
let col3 = Matrix3_col(&m, 2);
assert_eq!(col1.x(), 1.0);
assert_eq!(col1.y(), 4.0);
assert_eq!(col1.z(), 7.0);
assert_eq!(col2.x(), 2.0);
assert_eq!(col2.y(), 5.0);
assert_eq!(col2.z(), 8.0);
assert_eq!(col3.x(), 3.0);
assert_eq!(col3.y(), 6.0);
assert_eq!(col3.z(), 9.0);
}
#[test]
fn matrix3_cofactor() {
let m = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
assert_eq!(m.cofactor(0, 0), -3.0);
assert_eq!(m.cofactor(0, 1), 6.0);
assert_eq!(m.cofactor(0, 2), -3.0);
assert_eq!(m.cofactor(1, 0), 6.0);
assert_eq!(m.cofactor(1, 1), -12.0);
assert_eq!(m.cofactor(1, 2), 6.0);
assert_eq!(m.cofactor(2, 0), -3.0);
assert_eq!(m.cofactor(2, 1), 6.0);
assert_eq!(m.cofactor(2, 2), -3.0);
}
#[test]
fn matrix3_cofactor_matrix() {
let m = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
let cofactor_matrix = Matrix3_cofactor_matrix(&m);
assert_eq!(Matrix3_row(&cofactor_matrix, 0).x(), m.cofactor(0, 0));
}
#[test]
fn matrix3_determinant() {
let m = Matrix3_new(6.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
assert_eq!(m.determinant(), -15.);
}
#[test]
fn matrix3_is_singular() {
let m = Matrix3_new(6.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
assert_eq!(m.is_singular(), m.determinant() == 0.0);
}
#[test]
fn matrix3_transpose() {
let m = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
let t = Matrix3_transpose(&m);
let n = Matrix3_new(1.0, 4.0, 7.0, 2.0, 5.0, 8.0, 3.0, 6.0, 9.0);
assert!(Matrix3_equals(&t, &n));
}
#[test]
fn matrix3_equals() {
let m = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
let n = Matrix3_new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
assert!(Matrix3_equals(&m, &n));
}
#[test]
fn matrix3_adjoint() {
let m = Matrix3_new(6.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
let adj = Matrix3_adjoint(&m);
assert!(Matrix3_equals(&Matrix3_transpose(&Matrix3_cofactor_matrix(&m)), &adj));
}
#[test]
fn matrix3_operator_mul() {
let m = Matrix3_new(1., 2., 3., 4., 5., 6., 7., 8., 9.);
let n = Matrix3_new(9., 8., 7., 6., 5., 4., 3., 2., 1.);
let p = Matrix3_new(30., 24., 18., 84., 69., 54., 138., 114., 90.);
assert!(Matrix3_equals(&p, &Matrix3_operator_mul(&m, &n)));
}
#[test]
fn matrix3_inverse() {
let k_tolerance = 1e-10;
let m = Matrix3_new(1., 12., 3., 8., 5., 3., 6., 14., 9.);
let inv = Matrix3_inverse(&m);
let identity = Matrix3_new(1., 0., 0., 0., 1., 0., 0., 0., 1.);
let product = Matrix3_operator_mul(&m, &inv);
assert!((Matrix3_row(&product, 0).x() - Matrix3_row(&identity, 0).x()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 0).y() - Matrix3_row(&identity, 0).y()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 0).z() - Matrix3_row(&identity, 0).z()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 1).x() - Matrix3_row(&identity, 1).x()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 1).y() - Matrix3_row(&identity, 1).y()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 1).z() - Matrix3_row(&identity, 1).z()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 2).x() - Matrix3_row(&identity, 2).x()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 2).y() - Matrix3_row(&identity, 2).y()).abs() < k_tolerance);
assert!((Matrix3_row(&product, 2).z() - Matrix3_row(&identity, 2).z()).abs() < k_tolerance);
}
#[test]
fn matrix3_operator_sum() {
let m = Matrix3_new(1., 2., 3., 4., 5., 6., 7., 8., 9.);
let n = Matrix3_new(9., 8., 7., 6., 5., 4., 3., 2., 1.);
let p = Matrix3_new(10., 10., 10., 10., 10., 10., 10., 10., 10.);
assert!(Matrix3_equals(&p, &Matrix3_operator_sum(&m, &n)));
}
#[test]
fn matrix3_operator_sub() {
let m = Matrix3_new(1., 2., 3., 4., 5., 6., 7., 8., 9.);
let n = Matrix3_new(9., 8., 7., 6., 5., 4., 3., 2., 1.);
let p = Matrix3_new(-8., -6., -4., -2., 0., 2., 4., 6., 8.);
assert!(Matrix3_equals(&p, &Matrix3_operator_sub(&m, &n)));
}
#[test]
fn matrix3_operator_divide_by_scalar() {
let m = Matrix3_new(1., 2., 3., 4., 5., 6., 7., 8., 9.);
let n = 2.0;
let p = Matrix3_new(0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5);
assert!(Matrix3_equals(&p, &Matrix3_operator_divide_by_scalar(&m, n)));
}
#[test]
fn matrix3_to_str() {
let m = Matrix3_new(1., 2., 3., 4., 5., 6., 7., 8., 9.);
assert_eq!(Matrix3_to_str(&m), "{{1, 2, 3},\n{4, 5, 6},\n{7, 8, 9}}");
}
#[test]
fn quaternion_new() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(q.w(), 1.0);
assert_eq!(q.x(), 2.0);
assert_eq!(q.y(), 3.0);
assert_eq!(q.z(), 4.0);
}
#[test]
fn quaternion_dot() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let r = Quaternion_new(5.0, 6.0, 7.0, 8.0);
assert_eq!(q.dot(&r), q.w() * r.w() + q.x() * r.x() + q.y() * r.y() + q.z() * r.z());
}
#[test]
fn quaternion_angular_distance() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let r = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(q.AngularDistance(&r), 0.);
}
#[test]
fn quaternion_norm() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(
q.norm(),
(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()).sqrt()
);
}
#[test]
fn quaternion_normalize() {
let mut q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let norm = q.norm();
q.as_mut().expect("").normalize();
assert_eq!(q.w(), 1.0 / norm);
assert_eq!(q.x(), 2.0 / norm);
assert_eq!(q.y(), 3.0 / norm);
assert_eq!(q.z(), 4.0 / norm);
}
#[test]
fn quaternion_squared_norm() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(
q.squared_norm(),
q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()
);
}
#[test]
fn quaternion_vec() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let v = Quaternion_vec(&q);
assert_eq!(v.x(), q.x());
assert_eq!(v.y(), q.y());
assert_eq!(v.z(), q.z());
}
#[test]
fn quaternion_coeffs() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let v = Quaternion_coeffs(&q);
assert_eq!(v.x(), q.w());
assert_eq!(v.y(), q.x());
assert_eq!(v.z(), q.y());
assert_eq!(v.w(), q.z());
}
#[test]
fn quaternion_inverse() {
let q = Quaternion_new(1.0, 0.0, 0.0, 0.0);
let inv = Quaternion_Inverse(&q);
assert!(Quaternion_equals(&inv, &Quaternion_Inverse(&q)));
}
#[test]
fn quaternion_equals() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let r = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert!(Quaternion_equals(&q, &r));
}
#[test]
fn quaternion_conjugate() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let conj = Quaternion_conjugate(&q);
assert_eq!(conj.w(), q.w());
assert_eq!(conj.x(), -q.x());
assert_eq!(conj.y(), -q.y());
assert_eq!(conj.z(), -q.z());
}
#[test]
fn quaternion_to_rotation_matrix() {
let q = Quaternion_new(1.0, 0.0, 0.0, 0.0);
let r = Quaternion_ToRotationMatrix(&q);
assert_eq!(Matrix3_row(&r, 0).x(), 1.0);
assert_eq!(Matrix3_row(&r, 1).y(), 1.0);
assert_eq!(Matrix3_row(&r, 2).z(), 1.0);
}
#[test]
fn quaternion_transform_vector() {
let q = Quaternion_new(1.0, 0.0, 0.0, 0.0);
let v = Vector3_new(1.0, 2.0, 3.0);
let w = Quaternion_TransformVector(&q, &v);
assert!(Vector3_equals(&w, &v));
}
#[test]
fn quaternion_is_approx() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
let r = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert!(Quaternion_IsApprox(&q, &r, 1e-10));
}
#[test]
fn quaternion_to_str() {
let q = Quaternion_new(1.0, 2.0, 3.0, 4.0);
assert_eq!(Quaternion_to_str(&q), "(w: 1, x: 2, y: 3, z: 4)");
}
#[test]
fn roll_pitch_yaw_new() {
let rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
assert_eq!(rpy.roll_angle(), 1.0);
assert_eq!(rpy.pitch_angle(), 2.0);
assert_eq!(rpy.yaw_angle(), 3.0);
}
#[test]
fn roll_pitch_yaw_vector() {
let rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
let v = rpy.vector();
assert_eq!(v.x(), 1.0);
assert_eq!(v.y(), 2.0);
assert_eq!(v.z(), 3.0);
}
#[test]
fn roll_pitch_yaw_set() {
let mut rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
RollPitchYaw_set(rpy.as_mut().expect(""), 4.0, 5.0, 6.0);
assert_eq!(rpy.roll_angle(), 4.0);
assert_eq!(rpy.pitch_angle(), 5.0);
assert_eq!(rpy.yaw_angle(), 6.0);
}
#[test]
fn roll_pitch_yaw_set_from_quaternion() {
let q = Quaternion_new(1.0, 0.0, 0.0, 0.0);
let mut rpy = RollPitchYaw_new(1.0, 2.0, 3.0);
RollPitchYaw_SetFromQuaternion(rpy.as_mut().expect(""), &q);
assert_eq!(rpy.roll_angle(), 0.0);
assert_eq!(rpy.pitch_angle(), 0.0);
assert_eq!(rpy.yaw_angle(), 0.0);
}
#[test]
fn roll_pitch_yaw_to_quaternion() {
let rpy = RollPitchYaw_new(0.0, 0.0, 0.0);
let q = RollPitchYaw_ToQuaternion(&rpy);
assert_eq!(q.w(), 1.);
assert_eq!(q.x(), 0.);
assert_eq!(q.y(), 0.);
assert_eq!(q.z(), 0.);
}
#[test]
fn roll_pitch_yaw_to_matrix() {
let rpy = RollPitchYaw_new(0.0, 0.0, 0.0);
let m = RollPitchYaw_ToMatrix(&rpy);
assert_eq!(Matrix3_row(&m, 0).x(), 1.0);
assert_eq!(Matrix3_row(&m, 1).y(), 1.0);
assert_eq!(Matrix3_row(&m, 2).z(), 1.0);
}
#[test]
fn roll_pitch_yaw_calc_rotation_matrix_dt() {
let rpy = RollPitchYaw_new(0.0, 0.0, 0.0);
let v = Vector3_new(1.0, 1.0, 1.0);
let m = RollPitchYaw_CalcRotationMatrixDt(&rpy, &v);
assert_eq!(Matrix3_row(&m, 0).x(), 0.0);
assert_eq!(Matrix3_row(&m, 1).y(), 0.0);
assert_eq!(Matrix3_row(&m, 2).z(), 0.0);
}
#[test]
fn bounding_box_new() {
let position = Vector3_new(1.0, 2.0, 3.0);
let box_size = Vector3_new(4.0, 5.0, 6.0);
let orientation = RollPitchYaw_new(0.1, 0.2, 0.3);
let bb = BoundingBox_new(&position, &box_size, &orientation, 1e-3);
assert_eq!(bb.position().x(), 1.0);
assert_eq!(bb.position().y(), 2.0);
assert_eq!(bb.position().z(), 3.0);
assert_eq!(bb.box_size().x(), 4.0);
assert_eq!(bb.box_size().y(), 5.0);
assert_eq!(bb.box_size().z(), 6.0);
assert_eq!(bb.get_orientation().roll_angle(), 0.1);
assert_eq!(bb.get_orientation().pitch_angle(), 0.2);
assert_eq!(bb.get_orientation().yaw_angle(), 0.3);
}
#[test]
fn bounding_box_get_vertices() {
let position = Vector3_new(0.0, 0.0, 0.0);
let box_size = Vector3_new(2.0, 2.0, 2.0);
let orientation = RollPitchYaw_new(0.0, 0.0, 0.0);
let bb = BoundingBox_new(&position, &box_size, &orientation, 1e-3);
let vertices = BoundingBox_get_vertices(&bb);
assert_eq!(vertices.len(), 8);
}
#[test]
fn bounding_box_is_box_contained() {
let origin = Vector3_new(0.0, 0.0, 0.0);
let identity = RollPitchYaw_new(0.0, 0.0, 0.0);
let big_box = BoundingBox_new(&origin, &Vector3_new(10.0, 10.0, 10.0), &identity, 1e-3);
let small_box = BoundingBox_new(&origin, &Vector3_new(1.0, 1.0, 1.0), &identity, 1e-3);
assert!(big_box.IsBoxContained(&small_box));
assert!(!small_box.IsBoxContained(&big_box));
}
#[test]
fn bounding_box_is_box_intersected() {
let identity = RollPitchYaw_new(0.0, 0.0, 0.0);
let box_a = BoundingBox_new(
&Vector3_new(0.0, 0.0, 0.0),
&Vector3_new(2.0, 2.0, 2.0),
&identity,
1e-3,
);
let box_b = BoundingBox_new(
&Vector3_new(1.0, 0.0, 0.0),
&Vector3_new(2.0, 2.0, 2.0),
&identity,
1e-3,
);
let box_c = BoundingBox_new(
&Vector3_new(100.0, 0.0, 0.0),
&Vector3_new(2.0, 2.0, 2.0),
&identity,
1e-3,
);
assert!(box_a.IsBoxIntersected(&box_b));
assert!(!box_a.IsBoxIntersected(&box_c));
}
}