forked from M-Labs/nalgebra
parent
e0fc89ffc6
commit
99b6181b1e
31
Cargo.toml
31
Cargo.toml
@ -1,7 +1,7 @@
|
|||||||
[package]
|
[package]
|
||||||
name = "nalgebra"
|
name = "nalgebra"
|
||||||
version = "0.10.1"
|
version = "0.10.1"
|
||||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ] # FIXME: add the contributors.
|
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||||
|
|
||||||
description = "Linear algebra library for computer physics, computer graphics and general low-dimensional linear algebra for Rust."
|
description = "Linear algebra library for computer physics, computer graphics and general low-dimensional linear algebra for Rust."
|
||||||
documentation = "http://nalgebra.org/doc/nalgebra/index.html"
|
documentation = "http://nalgebra.org/doc/nalgebra/index.html"
|
||||||
@ -16,28 +16,19 @@ name = "nalgebra"
|
|||||||
path = "src/lib.rs"
|
path = "src/lib.rs"
|
||||||
|
|
||||||
[features]
|
[features]
|
||||||
# Generate arbitrary instances of nalgebra types for testing with quickcheck
|
|
||||||
arbitrary = [ "quickcheck" ]
|
arbitrary = [ "quickcheck" ]
|
||||||
generic_sizes = [ "generic-array", "typenum" ]
|
|
||||||
abstract_algebra = [ "algebra" ]
|
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
rustc-serialize = "0.3.*"
|
rustc-serialize = "0.3"
|
||||||
rand = "0.3.*"
|
typenum = "1.4"
|
||||||
num = "0.1.*"
|
generic-array = "0.2"
|
||||||
|
rand = "0.3"
|
||||||
[dependencies.generic-array]
|
num-traits = "0.1"
|
||||||
optional = true
|
num-complex = "0.1"
|
||||||
version = "0.2.*"
|
approx = "0.1"
|
||||||
|
alga = "0.4"
|
||||||
[dependencies.typenum]
|
# clippy = "*"
|
||||||
optional = true
|
|
||||||
version = "1.3.*"
|
|
||||||
|
|
||||||
[dependencies.quickcheck]
|
[dependencies.quickcheck]
|
||||||
optional = true
|
optional = true
|
||||||
version = "0.2.*"
|
version = "0.3"
|
||||||
|
|
||||||
[dependencies.algebra]
|
|
||||||
optional = true
|
|
||||||
version = "0.2.*"
|
|
||||||
|
@ -16,7 +16,30 @@ macro_rules! bench_binop(
|
|||||||
i = (i + 1) & (LEN - 1);
|
i = (i + 1) & (LEN - 1);
|
||||||
|
|
||||||
unsafe {
|
unsafe {
|
||||||
test::black_box((*elems1.get_unchecked(i)).$binop(*elems2.get_unchecked(i)))
|
test::black_box(elems1.get_unchecked(i).$binop(*elems2.get_unchecked(i)))
|
||||||
|
}
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! bench_binop_ref(
|
||||||
|
($name: ident, $t1: ty, $t2: ty, $binop: ident) => {
|
||||||
|
#[bench]
|
||||||
|
fn $name(bh: &mut Bencher) {
|
||||||
|
const LEN: usize = 1 << 13;
|
||||||
|
|
||||||
|
let mut rng = IsaacRng::new_unseeded();
|
||||||
|
|
||||||
|
let elems1: Vec<$t1> = (0usize .. LEN).map(|_| rng.gen::<$t1>()).collect();
|
||||||
|
let elems2: Vec<$t2> = (0usize .. LEN).map(|_| rng.gen::<$t2>()).collect();
|
||||||
|
let mut i = 0;
|
||||||
|
|
||||||
|
bh.iter(|| {
|
||||||
|
i = (i + 1) & (LEN - 1);
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
test::black_box(elems1.get_unchecked(i).$binop(elems2.get_unchecked(i)))
|
||||||
}
|
}
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
@ -46,7 +69,7 @@ macro_rules! bench_binop_na(
|
|||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
macro_rules! bench_unop(
|
macro_rules! bench_unop_na(
|
||||||
($name: ident, $t: ty, $unop: ident) => {
|
($name: ident, $t: ty, $unop: ident) => {
|
||||||
#[bench]
|
#[bench]
|
||||||
fn $name(bh: &mut Bencher) {
|
fn $name(bh: &mut Bencher) {
|
||||||
@ -68,7 +91,7 @@ macro_rules! bench_unop(
|
|||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
macro_rules! bench_unop_self(
|
macro_rules! bench_unop(
|
||||||
($name: ident, $t: ty, $unop: ident) => {
|
($name: ident, $t: ty, $unop: ident) => {
|
||||||
#[bench]
|
#[bench]
|
||||||
fn $name(bh: &mut Bencher) {
|
fn $name(bh: &mut Bencher) {
|
||||||
|
@ -1,19 +0,0 @@
|
|||||||
#![feature(test)]
|
|
||||||
|
|
||||||
extern crate test;
|
|
||||||
extern crate rand;
|
|
||||||
extern crate nalgebra as na;
|
|
||||||
|
|
||||||
use rand::{IsaacRng, Rng};
|
|
||||||
use test::Bencher;
|
|
||||||
use na::{UnitQuaternion, Rotation2, Rotation3, Vector1, Vector3};
|
|
||||||
|
|
||||||
#[path="common/macros.rs"]
|
|
||||||
mod macros;
|
|
||||||
|
|
||||||
bench_construction!(_bench_quaternion_from_axisangle, UnitQuaternion::from_scaled_axis, axisangle: Vector3<f32>);
|
|
||||||
bench_construction!(_bench_rot2_from_axisangle, Rotation2::new, axisangle: Vector1<f32>);
|
|
||||||
bench_construction!(_bench_rot3_from_axisangle, Rotation3::new, axisangle: Vector3<f32>);
|
|
||||||
|
|
||||||
bench_construction!(_bench_quaternion_from_euler_angles, UnitQuaternion::from_euler_angles, roll: f32, pitch: f32, yaw: f32);
|
|
||||||
bench_construction!(_bench_rot3_from_euler_angles, Rotation3::from_euler_angles, roll: f32, pitch: f32, yaw: f32);
|
|
@ -1,90 +0,0 @@
|
|||||||
#![feature(test)]
|
|
||||||
|
|
||||||
extern crate test;
|
|
||||||
extern crate nalgebra as na;
|
|
||||||
|
|
||||||
use test::Bencher;
|
|
||||||
use na::{DVector, DMatrix};
|
|
||||||
|
|
||||||
macro_rules! bench_mul_dmatrix(
|
|
||||||
($bh: expr, $nrows: expr, $ncols: expr) => {
|
|
||||||
{
|
|
||||||
$bh.iter(|| {
|
|
||||||
let a: DMatrix<f64> = DMatrix::new_random($nrows, $ncols);
|
|
||||||
let mut b: DMatrix<f64> = DMatrix::new_random($nrows, $ncols);
|
|
||||||
|
|
||||||
for _ in 0usize .. 1000 {
|
|
||||||
// XXX: the clone here is highly undesirable!
|
|
||||||
b = a.clone() * b;
|
|
||||||
}
|
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat2(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmatrix!(bh, 2, 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat3(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmatrix!(bh, 3, 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat4(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmatrix!(bh, 4, 4);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat5(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmatrix!(bh, 5, 5);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat6(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmatrix!(bh, 6, 6);
|
|
||||||
}
|
|
||||||
|
|
||||||
macro_rules! bench_mul_dmat_dvector(
|
|
||||||
($bh: expr, $nrows: expr, $ncols: expr) => {
|
|
||||||
{
|
|
||||||
|
|
||||||
$bh.iter(|| {
|
|
||||||
let m : DMatrix<f64> = DMatrix::new_random($nrows, $ncols);
|
|
||||||
let mut v : DVector<f64> = DVector::new_random($ncols);
|
|
||||||
|
|
||||||
for _ in 0usize .. 1000 {
|
|
||||||
// XXX: the clone here is highly undesirable!
|
|
||||||
v = m.clone() * v
|
|
||||||
}
|
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat_dvec2(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmat_dvector!(bh, 2, 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat_dvec3(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmat_dvector!(bh, 3, 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat_dvec4(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmat_dvector!(bh, 4, 4);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat_dvec5(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmat_dvector!(bh, 5, 5);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[bench]
|
|
||||||
fn bench_mul_dmat_dvec6(bh: &mut Bencher) {
|
|
||||||
bench_mul_dmat_dvector!(bh, 6, 6);
|
|
||||||
}
|
|
@ -16,6 +16,10 @@ bench_binop!(_bench_mat2_mul_m, Matrix2<f32>, Matrix2<f32>, mul);
|
|||||||
bench_binop!(_bench_mat3_mul_m, Matrix3<f32>, Matrix3<f32>, mul);
|
bench_binop!(_bench_mat3_mul_m, Matrix3<f32>, Matrix3<f32>, mul);
|
||||||
bench_binop!(_bench_mat4_mul_m, Matrix4<f32>, Matrix4<f32>, mul);
|
bench_binop!(_bench_mat4_mul_m, Matrix4<f32>, Matrix4<f32>, mul);
|
||||||
|
|
||||||
|
bench_binop_ref!(_bench_mat2_tr_mul_m, Matrix2<f32>, Matrix2<f32>, tr_mul);
|
||||||
|
bench_binop_ref!(_bench_mat3_tr_mul_m, Matrix3<f32>, Matrix3<f32>, tr_mul);
|
||||||
|
bench_binop_ref!(_bench_mat4_tr_mul_m, Matrix4<f32>, Matrix4<f32>, tr_mul);
|
||||||
|
|
||||||
bench_binop!(_bench_mat2_add_m, Matrix2<f32>, Matrix2<f32>, add);
|
bench_binop!(_bench_mat2_add_m, Matrix2<f32>, Matrix2<f32>, add);
|
||||||
bench_binop!(_bench_mat3_add_m, Matrix3<f32>, Matrix3<f32>, add);
|
bench_binop!(_bench_mat3_add_m, Matrix3<f32>, Matrix3<f32>, add);
|
||||||
bench_binop!(_bench_mat4_add_m, Matrix4<f32>, Matrix4<f32>, add);
|
bench_binop!(_bench_mat4_add_m, Matrix4<f32>, Matrix4<f32>, add);
|
||||||
@ -28,6 +32,10 @@ bench_binop!(_bench_mat2_mul_v, Matrix2<f32>, Vector2<f32>, mul);
|
|||||||
bench_binop!(_bench_mat3_mul_v, Matrix3<f32>, Vector3<f32>, mul);
|
bench_binop!(_bench_mat3_mul_v, Matrix3<f32>, Vector3<f32>, mul);
|
||||||
bench_binop!(_bench_mat4_mul_v, Matrix4<f32>, Vector4<f32>, mul);
|
bench_binop!(_bench_mat4_mul_v, Matrix4<f32>, Vector4<f32>, mul);
|
||||||
|
|
||||||
|
bench_binop_ref!(_bench_mat2_tr_mul_v, Matrix2<f32>, Vector2<f32>, tr_mul);
|
||||||
|
bench_binop_ref!(_bench_mat3_tr_mul_v, Matrix3<f32>, Vector3<f32>, tr_mul);
|
||||||
|
bench_binop_ref!(_bench_mat4_tr_mul_v, Matrix4<f32>, Vector4<f32>, tr_mul);
|
||||||
|
|
||||||
bench_binop!(_bench_mat2_mul_s, Matrix2<f32>, f32, mul);
|
bench_binop!(_bench_mat2_mul_s, Matrix2<f32>, f32, mul);
|
||||||
bench_binop!(_bench_mat3_mul_s, Matrix3<f32>, f32, mul);
|
bench_binop!(_bench_mat3_mul_s, Matrix3<f32>, f32, mul);
|
||||||
bench_binop!(_bench_mat4_mul_s, Matrix4<f32>, f32, mul);
|
bench_binop!(_bench_mat4_mul_s, Matrix4<f32>, f32, mul);
|
||||||
@ -36,9 +44,9 @@ bench_binop!(_bench_mat2_div_s, Matrix2<f32>, f32, div);
|
|||||||
bench_binop!(_bench_mat3_div_s, Matrix3<f32>, f32, div);
|
bench_binop!(_bench_mat3_div_s, Matrix3<f32>, f32, div);
|
||||||
bench_binop!(_bench_mat4_div_s, Matrix4<f32>, f32, div);
|
bench_binop!(_bench_mat4_div_s, Matrix4<f32>, f32, div);
|
||||||
|
|
||||||
bench_unop!(_bench_mat2_inv, Matrix2<f32>, inverse);
|
bench_unop!(_bench_mat2_inv, Matrix2<f32>, try_inverse);
|
||||||
bench_unop!(_bench_mat3_inv, Matrix3<f32>, inverse);
|
bench_unop!(_bench_mat3_inv, Matrix3<f32>, try_inverse);
|
||||||
bench_unop!(_bench_mat4_inv, Matrix4<f32>, inverse);
|
bench_unop!(_bench_mat4_inv, Matrix4<f32>, try_inverse);
|
||||||
|
|
||||||
bench_unop!(_bench_mat2_transpose, Matrix2<f32>, transpose);
|
bench_unop!(_bench_mat2_transpose, Matrix2<f32>, transpose);
|
||||||
bench_unop!(_bench_mat3_transpose, Matrix3<f32>, transpose);
|
bench_unop!(_bench_mat3_transpose, Matrix3<f32>, transpose);
|
@ -15,10 +15,14 @@ mod macros;
|
|||||||
bench_binop!(_bench_quaternion_add_q, Quaternion<f32>, Quaternion<f32>, add);
|
bench_binop!(_bench_quaternion_add_q, Quaternion<f32>, Quaternion<f32>, add);
|
||||||
bench_binop!(_bench_quaternion_sub_q, Quaternion<f32>, Quaternion<f32>, sub);
|
bench_binop!(_bench_quaternion_sub_q, Quaternion<f32>, Quaternion<f32>, sub);
|
||||||
bench_binop!(_bench_quaternion_mul_q, Quaternion<f32>, Quaternion<f32>, mul);
|
bench_binop!(_bench_quaternion_mul_q, Quaternion<f32>, Quaternion<f32>, mul);
|
||||||
// bench_binop!(_bench_quaternion_div_q, Quaternion<f32>, Quaternion<f32>, div)
|
|
||||||
bench_binop!(_bench_quaternion_mul_v, UnitQuaternion<f32>, Vector3<f32>, mul);
|
bench_binop!(_bench_unit_quaternion_mul_v, UnitQuaternion<f32>, Vector3<f32>, mul);
|
||||||
|
|
||||||
bench_binop!(_bench_quaternion_mul_s, Quaternion<f32>, f32, mul);
|
bench_binop!(_bench_quaternion_mul_s, Quaternion<f32>, f32, mul);
|
||||||
bench_binop!(_bench_quaternion_div_s, Quaternion<f32>, f32, div);
|
bench_binop!(_bench_quaternion_div_s, Quaternion<f32>, f32, div);
|
||||||
bench_unop!(_bench_quaternion_inv, Quaternion<f32>, inverse);
|
|
||||||
bench_unop_self!(_bench_quaternion_conjugate, Quaternion<f32>, conjugate);
|
bench_unop!(_bench_quaternion_inv, Quaternion<f32>, try_inverse);
|
||||||
bench_unop!(_bench_quaternion_normalize, Quaternion<f32>, normalize);
|
bench_unop!(_bench_unit_quaternion_inv, UnitQuaternion<f32>, inverse);
|
||||||
|
|
||||||
|
// bench_unop_self!(_bench_quaternion_conjugate, Quaternion<f32>, conjugate);
|
||||||
|
// bench_unop!(_bench_quaternion_normalize, Quaternion<f32>, normalize);
|
119
benches/vec.rs
119
benches/vec.rs
@ -1,119 +0,0 @@
|
|||||||
#![feature(test)]
|
|
||||||
|
|
||||||
#[cfg(feature = "generic_sizes")]
|
|
||||||
extern crate typenum;
|
|
||||||
|
|
||||||
extern crate test;
|
|
||||||
extern crate rand;
|
|
||||||
extern crate nalgebra as na;
|
|
||||||
|
|
||||||
use rand::{IsaacRng, Rng};
|
|
||||||
use test::Bencher;
|
|
||||||
use na::{Vector2, Vector3, Vector4};
|
|
||||||
use std::ops::{Add, Sub, Mul, Div};
|
|
||||||
|
|
||||||
#[path="common/macros.rs"]
|
|
||||||
mod macros;
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_add_v, Vector2<f32>, Vector2<f32>, add);
|
|
||||||
bench_binop!(_bench_vec3_add_v, Vector3<f32>, Vector3<f32>, add);
|
|
||||||
bench_binop!(_bench_vec4_add_v, Vector4<f32>, Vector4<f32>, add);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_sub_v, Vector2<f32>, Vector2<f32>, sub);
|
|
||||||
bench_binop!(_bench_vec3_sub_v, Vector3<f32>, Vector3<f32>, sub);
|
|
||||||
bench_binop!(_bench_vec4_sub_v, Vector4<f32>, Vector4<f32>, sub);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_mul_v, Vector2<f32>, Vector2<f32>, mul);
|
|
||||||
bench_binop!(_bench_vec3_mul_v, Vector3<f32>, Vector3<f32>, mul);
|
|
||||||
bench_binop!(_bench_vec4_mul_v, Vector4<f32>, Vector4<f32>, mul);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_div_v, Vector2<f32>, Vector2<f32>, div);
|
|
||||||
bench_binop!(_bench_vec3_div_v, Vector3<f32>, Vector3<f32>, div);
|
|
||||||
bench_binop!(_bench_vec4_div_v, Vector4<f32>, Vector4<f32>, div);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_add_s, Vector2<f32>, f32, add);
|
|
||||||
bench_binop!(_bench_vec3_add_s, Vector3<f32>, f32, add);
|
|
||||||
bench_binop!(_bench_vec4_add_s, Vector4<f32>, f32, add);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_sub_s, Vector2<f32>, f32, sub);
|
|
||||||
bench_binop!(_bench_vec3_sub_s, Vector3<f32>, f32, sub);
|
|
||||||
bench_binop!(_bench_vec4_sub_s, Vector4<f32>, f32, sub);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_mul_s, Vector2<f32>, f32, mul);
|
|
||||||
bench_binop!(_bench_vec3_mul_s, Vector3<f32>, f32, mul);
|
|
||||||
bench_binop!(_bench_vec4_mul_s, Vector4<f32>, f32, mul);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vec2_div_s, Vector2<f32>, f32, div);
|
|
||||||
bench_binop!(_bench_vec3_div_s, Vector3<f32>, f32, div);
|
|
||||||
bench_binop!(_bench_vec4_div_s, Vector4<f32>, f32, div);
|
|
||||||
|
|
||||||
bench_binop_na!(_bench_vec2_dot, Vector2<f32>, Vector2<f32>, dot);
|
|
||||||
bench_binop_na!(_bench_vec3_dot, Vector3<f32>, Vector3<f32>, dot);
|
|
||||||
bench_binop_na!(_bench_vec4_dot, Vector4<f32>, Vector4<f32>, dot);
|
|
||||||
|
|
||||||
bench_binop_na!(_bench_vec3_cross, Vector3<f32>, Vector3<f32>, cross);
|
|
||||||
|
|
||||||
bench_unop!(_bench_vec2_norm, Vector2<f32>, norm);
|
|
||||||
bench_unop!(_bench_vec3_norm, Vector3<f32>, norm);
|
|
||||||
bench_unop!(_bench_vec4_norm, Vector4<f32>, norm);
|
|
||||||
|
|
||||||
bench_unop!(_bench_vec2_normalize, Vector2<f32>, normalize);
|
|
||||||
bench_unop!(_bench_vec3_normalize, Vector3<f32>, normalize);
|
|
||||||
bench_unop!(_bench_vec4_normalize, Vector4<f32>, normalize);
|
|
||||||
|
|
||||||
#[cfg(feature = "generic_sizes")]
|
|
||||||
mod bench_vecn {
|
|
||||||
extern crate test;
|
|
||||||
extern crate rand;
|
|
||||||
extern crate nalgebra as na;
|
|
||||||
|
|
||||||
use rand::{IsaacRng, Rng};
|
|
||||||
use test::Bencher;
|
|
||||||
use std::ops::{Add, Sub, Mul, Div};
|
|
||||||
use typenum::{U2, U3, U4};
|
|
||||||
use na::VectorN;
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_add_v, VectorN<f32, U2>, VectorN<f32, U2>, add);
|
|
||||||
bench_binop!(_bench_vecn3_add_v, VectorN<f32, U3>, VectorN<f32, U3>, add);
|
|
||||||
bench_binop!(_bench_vecn4_add_v, VectorN<f32, U4>, VectorN<f32, U4>, add);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_sub_v, VectorN<f32, U2>, VectorN<f32, U2>, sub);
|
|
||||||
bench_binop!(_bench_vecn3_sub_v, VectorN<f32, U3>, VectorN<f32, U3>, sub);
|
|
||||||
bench_binop!(_bench_vecn4_sub_v, VectorN<f32, U4>, VectorN<f32, U4>, sub);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_mul_v, VectorN<f32, U2>, VectorN<f32, U2>, mul);
|
|
||||||
bench_binop!(_bench_vecn3_mul_v, VectorN<f32, U3>, VectorN<f32, U3>, mul);
|
|
||||||
bench_binop!(_bench_vecn4_mul_v, VectorN<f32, U4>, VectorN<f32, U4>, mul);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_div_v, VectorN<f32, U2>, VectorN<f32, U2>, div);
|
|
||||||
bench_binop!(_bench_vecn3_div_v, VectorN<f32, U3>, VectorN<f32, U3>, div);
|
|
||||||
bench_binop!(_bench_vecn4_div_v, VectorN<f32, U4>, VectorN<f32, U4>, div);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_add_s, VectorN<f32, U2>, f32, add);
|
|
||||||
bench_binop!(_bench_vecn3_add_s, VectorN<f32, U3>, f32, add);
|
|
||||||
bench_binop!(_bench_vecn4_add_s, VectorN<f32, U4>, f32, add);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_sub_s, VectorN<f32, U2>, f32, sub);
|
|
||||||
bench_binop!(_bench_vecn3_sub_s, VectorN<f32, U3>, f32, sub);
|
|
||||||
bench_binop!(_bench_vecn4_sub_s, VectorN<f32, U4>, f32, sub);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_mul_s, VectorN<f32, U2>, f32, mul);
|
|
||||||
bench_binop!(_bench_vecn3_mul_s, VectorN<f32, U3>, f32, mul);
|
|
||||||
bench_binop!(_bench_vecn4_mul_s, VectorN<f32, U4>, f32, mul);
|
|
||||||
|
|
||||||
bench_binop!(_bench_vecn2_div_s, VectorN<f32, U2>, f32, div);
|
|
||||||
bench_binop!(_bench_vecn3_div_s, VectorN<f32, U3>, f32, div);
|
|
||||||
bench_binop!(_bench_vecn4_div_s, VectorN<f32, U4>, f32, div);
|
|
||||||
|
|
||||||
bench_binop_na!(_bench_vecn2_dot, VectorN<f32, U2>, VectorN<f32, U2>, dot);
|
|
||||||
bench_binop_na!(_bench_vecn3_dot, VectorN<f32, U3>, VectorN<f32, U3>, dot);
|
|
||||||
bench_binop_na!(_bench_vecn4_dot, VectorN<f32, U4>, VectorN<f32, U4>, dot);
|
|
||||||
|
|
||||||
bench_unop!(_bench_vecn2_norm, VectorN<f32, U2>, norm);
|
|
||||||
bench_unop!(_bench_vecn3_norm, VectorN<f32, U3>, norm);
|
|
||||||
bench_unop!(_bench_vecn4_norm, VectorN<f32, U4>, norm);
|
|
||||||
|
|
||||||
bench_unop!(_bench_vecn2_normalize, VectorN<f32, U2>, normalize);
|
|
||||||
bench_unop!(_bench_vecn3_normalize, VectorN<f32, U3>, normalize);
|
|
||||||
bench_unop!(_bench_vecn4_normalize, VectorN<f32, U4>, normalize);
|
|
||||||
}
|
|
43
benches/vector.rs
Normal file
43
benches/vector.rs
Normal file
@ -0,0 +1,43 @@
|
|||||||
|
#![feature(test)]
|
||||||
|
|
||||||
|
extern crate test;
|
||||||
|
extern crate rand;
|
||||||
|
extern crate nalgebra as na;
|
||||||
|
|
||||||
|
use rand::{IsaacRng, Rng};
|
||||||
|
use test::Bencher;
|
||||||
|
use na::{Vector2, Vector3, Vector4};
|
||||||
|
use std::ops::{Add, Sub, Mul, Div};
|
||||||
|
|
||||||
|
#[path="common/macros.rs"]
|
||||||
|
mod macros;
|
||||||
|
|
||||||
|
bench_binop!(_bench_vec2_add_v, Vector2<f32>, Vector2<f32>, add);
|
||||||
|
bench_binop!(_bench_vec3_add_v, Vector3<f32>, Vector3<f32>, add);
|
||||||
|
bench_binop!(_bench_vec4_add_v, Vector4<f32>, Vector4<f32>, add);
|
||||||
|
|
||||||
|
bench_binop!(_bench_vec2_sub_v, Vector2<f32>, Vector2<f32>, sub);
|
||||||
|
bench_binop!(_bench_vec3_sub_v, Vector3<f32>, Vector3<f32>, sub);
|
||||||
|
bench_binop!(_bench_vec4_sub_v, Vector4<f32>, Vector4<f32>, sub);
|
||||||
|
|
||||||
|
bench_binop!(_bench_vec2_mul_s, Vector2<f32>, f32, mul);
|
||||||
|
bench_binop!(_bench_vec3_mul_s, Vector3<f32>, f32, mul);
|
||||||
|
bench_binop!(_bench_vec4_mul_s, Vector4<f32>, f32, mul);
|
||||||
|
|
||||||
|
bench_binop!(_bench_vec2_div_s, Vector2<f32>, f32, div);
|
||||||
|
bench_binop!(_bench_vec3_div_s, Vector3<f32>, f32, div);
|
||||||
|
bench_binop!(_bench_vec4_div_s, Vector4<f32>, f32, div);
|
||||||
|
|
||||||
|
bench_binop_ref!(_bench_vec2_dot, Vector2<f32>, Vector2<f32>, dot);
|
||||||
|
bench_binop_ref!(_bench_vec3_dot, Vector3<f32>, Vector3<f32>, dot);
|
||||||
|
bench_binop_ref!(_bench_vec4_dot, Vector4<f32>, Vector4<f32>, dot);
|
||||||
|
|
||||||
|
bench_binop_ref!(_bench_vec3_cross, Vector3<f32>, Vector3<f32>, cross);
|
||||||
|
|
||||||
|
bench_unop!(_bench_vec2_norm, Vector2<f32>, norm);
|
||||||
|
bench_unop!(_bench_vec3_norm, Vector3<f32>, norm);
|
||||||
|
bench_unop!(_bench_vec4_norm, Vector4<f32>, norm);
|
||||||
|
|
||||||
|
bench_unop!(_bench_vec2_normalize, Vector2<f32>, normalize);
|
||||||
|
bench_unop!(_bench_vec3_normalize, Vector3<f32>, normalize);
|
||||||
|
bench_unop!(_bench_vec4_normalize, Vector4<f32>, normalize);
|
110
src/core/alias.rs
Normal file
110
src/core/alias.rs
Normal file
@ -0,0 +1,110 @@
|
|||||||
|
use core::Matrix;
|
||||||
|
use core::dimension::{Dynamic, U1, U2, U3, U4, U5, U6};
|
||||||
|
use core::matrix_array::MatrixArray;
|
||||||
|
use core::matrix_vec::MatrixVec;
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* Column-major matrices.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// A dynamically sized column-major matrix.
|
||||||
|
pub type DMatrix<N> = Matrix<N, Dynamic, Dynamic, MatrixVec<N, Dynamic, Dynamic>>;
|
||||||
|
|
||||||
|
/// A staticaly sized column-major matrix with `R` rows and `C` columns.
|
||||||
|
pub type MatrixNM<N, R, C> = Matrix<N, R, C, MatrixArray<N, R, C>>;
|
||||||
|
|
||||||
|
/// A staticaly sized column-major square matrix with `D` rows and columns.
|
||||||
|
pub type MatrixN<N, D> = MatrixNM<N, D, D>;
|
||||||
|
|
||||||
|
pub type Matrix1<N> = MatrixN<N, U1>;
|
||||||
|
pub type Matrix2<N> = MatrixN<N, U2>;
|
||||||
|
pub type Matrix3<N> = MatrixN<N, U3>;
|
||||||
|
pub type Matrix4<N> = MatrixN<N, U4>;
|
||||||
|
pub type Matrix5<N> = MatrixN<N, U5>;
|
||||||
|
pub type Matrix6<N> = MatrixN<N, U6>;
|
||||||
|
|
||||||
|
pub type Matrix1x2<N> = MatrixNM<N, U1, U2>;
|
||||||
|
pub type Matrix1x3<N> = MatrixNM<N, U1, U3>;
|
||||||
|
pub type Matrix1x4<N> = MatrixNM<N, U1, U4>;
|
||||||
|
pub type Matrix1x5<N> = MatrixNM<N, U1, U5>;
|
||||||
|
pub type Matrix1x6<N> = MatrixNM<N, U1, U6>;
|
||||||
|
|
||||||
|
pub type Matrix2x3<N> = MatrixNM<N, U2, U3>;
|
||||||
|
pub type Matrix2x4<N> = MatrixNM<N, U2, U4>;
|
||||||
|
pub type Matrix2x5<N> = MatrixNM<N, U2, U5>;
|
||||||
|
pub type Matrix2x6<N> = MatrixNM<N, U2, U6>;
|
||||||
|
|
||||||
|
pub type Matrix3x4<N> = MatrixNM<N, U3, U4>;
|
||||||
|
pub type Matrix3x5<N> = MatrixNM<N, U3, U5>;
|
||||||
|
pub type Matrix3x6<N> = MatrixNM<N, U3, U6>;
|
||||||
|
|
||||||
|
pub type Matrix4x5<N> = MatrixNM<N, U4, U5>;
|
||||||
|
pub type Matrix4x6<N> = MatrixNM<N, U4, U6>;
|
||||||
|
|
||||||
|
pub type Matrix5x6<N> = MatrixNM<N, U5, U6>;
|
||||||
|
|
||||||
|
|
||||||
|
pub type Matrix2x1<N> = MatrixNM<N, U2, U1>;
|
||||||
|
pub type Matrix3x1<N> = MatrixNM<N, U3, U1>;
|
||||||
|
pub type Matrix4x1<N> = MatrixNM<N, U4, U1>;
|
||||||
|
pub type Matrix5x1<N> = MatrixNM<N, U5, U1>;
|
||||||
|
pub type Matrix6x1<N> = MatrixNM<N, U6, U1>;
|
||||||
|
|
||||||
|
pub type Matrix3x2<N> = MatrixNM<N, U3, U2>;
|
||||||
|
pub type Matrix4x2<N> = MatrixNM<N, U4, U2>;
|
||||||
|
pub type Matrix5x2<N> = MatrixNM<N, U5, U2>;
|
||||||
|
pub type Matrix6x2<N> = MatrixNM<N, U6, U2>;
|
||||||
|
|
||||||
|
pub type Matrix4x3<N> = MatrixNM<N, U4, U3>;
|
||||||
|
pub type Matrix5x3<N> = MatrixNM<N, U5, U3>;
|
||||||
|
pub type Matrix6x3<N> = MatrixNM<N, U6, U3>;
|
||||||
|
|
||||||
|
pub type Matrix5x4<N> = MatrixNM<N, U5, U4>;
|
||||||
|
pub type Matrix6x4<N> = MatrixNM<N, U6, U4>;
|
||||||
|
|
||||||
|
pub type Matrix6x5<N> = MatrixNM<N, U6, U5>;
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* Column vectors.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// A dynamically sized column vector.
|
||||||
|
pub type DVector<N> = Matrix<N, Dynamic, U1, MatrixVec<N, Dynamic, U1>>;
|
||||||
|
|
||||||
|
/// A statically sized D-dimensional column vector.
|
||||||
|
pub type VectorN<N, D> = MatrixNM<N, D, U1>;
|
||||||
|
|
||||||
|
pub type Vector1<N> = VectorN<N, U1>;
|
||||||
|
pub type Vector2<N> = VectorN<N, U2>;
|
||||||
|
pub type Vector3<N> = VectorN<N, U3>;
|
||||||
|
pub type Vector4<N> = VectorN<N, U4>;
|
||||||
|
pub type Vector5<N> = VectorN<N, U5>;
|
||||||
|
pub type Vector6<N> = VectorN<N, U6>;
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* Row vectors.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// A dynamically sized row vector.
|
||||||
|
pub type RowDVector<N> = Matrix<N, U1, Dynamic, MatrixVec<N, U1, Dynamic>>;
|
||||||
|
|
||||||
|
/// A statically sized D-dimensional row vector.
|
||||||
|
pub type RowVectorN<N, D> = MatrixNM<N, U1, D>;
|
||||||
|
|
||||||
|
pub type RowVector1<N> = RowVectorN<N, U1>;
|
||||||
|
pub type RowVector2<N> = RowVectorN<N, U2>;
|
||||||
|
pub type RowVector3<N> = RowVectorN<N, U3>;
|
||||||
|
pub type RowVector4<N> = RowVectorN<N, U4>;
|
||||||
|
pub type RowVector5<N> = RowVectorN<N, U5>;
|
||||||
|
pub type RowVector6<N> = RowVectorN<N, U6>;
|
86
src/core/allocator.rs
Normal file
86
src/core/allocator.rs
Normal file
@ -0,0 +1,86 @@
|
|||||||
|
use std::any::Any;
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::constraint::{SameNumberOfRows, SameNumberOfColumns, ShapeConstraint};
|
||||||
|
use core::dimension::{Dim, U1};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
|
||||||
|
/// A matrix allocator of a memory buffer that may contain `R::to_usize() * C::to_usize()`
|
||||||
|
/// elements of type `N`.
|
||||||
|
///
|
||||||
|
/// An allocator is said to be:
|
||||||
|
/// − static: if `R` and `C` both implement `DimName`.
|
||||||
|
/// − dynamic: if either one (or both) of `R` or `C` is equal to `Dynamic`.
|
||||||
|
///
|
||||||
|
/// Every allocator must be both static and dynamic. Though not all implementations may share the
|
||||||
|
/// same `Buffer` type.
|
||||||
|
pub trait Allocator<N: Scalar, R: Dim, C: Dim>: Any + Sized {
|
||||||
|
type Buffer: OwnedStorage<N, R, C, Alloc = Self>;
|
||||||
|
|
||||||
|
/// Allocates a buffer with the given number of rows and columns without initializing its content.
|
||||||
|
unsafe fn allocate_uninitialized(nrows: R, ncols: C) -> Self::Buffer;
|
||||||
|
|
||||||
|
/// Allocates a buffer initialized with the content of the given iterator.
|
||||||
|
fn allocate_from_iterator<I: IntoIterator<Item = N>>(nrows: R, ncols: C, iter: I) -> Self::Buffer;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A matrix data allocator dedicated to the given owned matrix storage.
|
||||||
|
pub trait OwnedAllocator<N: Scalar, R: Dim, C: Dim, S: OwnedStorage<N, R, C, Alloc = Self>>:
|
||||||
|
Allocator<N, R, C, Buffer = S> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C, T, S> OwnedAllocator<N, R, C, S> for T
|
||||||
|
where N: Scalar, R: Dim, C: Dim,
|
||||||
|
T: Allocator<N, R, C, Buffer = S>,
|
||||||
|
S: OwnedStorage<N, R, C, Alloc = T> {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The number of rows of the result of a componentwise operation on two matrices.
|
||||||
|
pub type SameShapeR<R1, R2> = <ShapeConstraint as SameNumberOfRows<R1, R2>>::Representative;
|
||||||
|
|
||||||
|
/// The number of columns of the result of a componentwise operation on two matrices.
|
||||||
|
pub type SameShapeC<C1, C2> = <ShapeConstraint as SameNumberOfColumns<C1, C2>>::Representative;
|
||||||
|
|
||||||
|
// FIXME: Bad name.
|
||||||
|
/// Restricts the given number of rows and columns to be respectively the same. Can only be used
|
||||||
|
/// when `Self = SA::Alloc`.
|
||||||
|
pub trait SameShapeAllocator<N, R1, C1, R2, C2, SA>:
|
||||||
|
Allocator<N, R1, C1> +
|
||||||
|
Allocator<N, SameShapeR<R1, R2>, SameShapeC<C1, C2>>
|
||||||
|
where R1: Dim, R2: Dim, C1: Dim, C2: Dim,
|
||||||
|
N: Scalar,
|
||||||
|
SA: Storage<N, R1, C1, Alloc = Self>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R1, R2, C1, C2, SA> SameShapeAllocator<N, R1, C1, R2, C2, SA> for SA::Alloc
|
||||||
|
where R1: Dim, R2: Dim, C1: Dim, C2: Dim,
|
||||||
|
N: Scalar,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SA::Alloc:
|
||||||
|
Allocator<N, R1, C1> +
|
||||||
|
Allocator<N, SameShapeR<R1, R2>, SameShapeC<C1, C2>>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
}
|
||||||
|
|
||||||
|
// XXX: Bad name.
|
||||||
|
/// Restricts the given number of rows to be equal. Can only be used when `Self = SA::Alloc`.
|
||||||
|
pub trait SameShapeColumnVectorAllocator<N, R1, R2, SA>:
|
||||||
|
Allocator<N, R1, U1> +
|
||||||
|
Allocator<N, SameShapeR<R1, R2>, U1> +
|
||||||
|
SameShapeAllocator<N, R1, U1, R2, U1, SA>
|
||||||
|
where R1: Dim, R2: Dim,
|
||||||
|
N: Scalar,
|
||||||
|
SA: Storage<N, R1, U1, Alloc = Self>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R1, R2, SA> SameShapeColumnVectorAllocator<N, R1, R2, SA> for SA::Alloc
|
||||||
|
where R1: Dim, R2: Dim,
|
||||||
|
N: Scalar,
|
||||||
|
SA: Storage<N, R1, U1>,
|
||||||
|
SA::Alloc:
|
||||||
|
Allocator<N, R1, U1> +
|
||||||
|
Allocator<N, SameShapeR<R1, R2>, U1>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> {
|
||||||
|
}
|
376
src/core/cg.rs
Normal file
376
src/core/cg.rs
Normal file
@ -0,0 +1,376 @@
|
|||||||
|
/*
|
||||||
|
*
|
||||||
|
* Computer-graphics specific implementations.
|
||||||
|
* Currently, it is mostly implemented for homogeneous matrices in 2- and 3-space.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
use num::One;
|
||||||
|
|
||||||
|
use core::{Scalar, SquareMatrix, OwnedSquareMatrix, ColumnVector, Unit};
|
||||||
|
use core::dimension::{DimName, DimNameSub, DimNameDiff, U1, U2, U3, U4};
|
||||||
|
use core::storage::{Storage, StorageMut, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use geometry::{PointBase, OrthographicBase, PerspectiveBase, IsometryBase, OwnedRotation, OwnedPoint};
|
||||||
|
|
||||||
|
use alga::general::{Real, Field};
|
||||||
|
use alga::linear::Transformation;
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
/// Creates a new homogeneous matrix that applies the same scaling factor on each dimension.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_scaling(scaling: N) -> Self {
|
||||||
|
let mut res = Self::from_diagonal_element(scaling);
|
||||||
|
res[(D::dim(), D::dim())] = N::one();
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new homogeneous matrix that applies a distinct scaling factor for each dimension.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_nonuniform_scaling<SB>(scaling: &ColumnVector<N, DimNameDiff<D, U1>, SB>) -> Self
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1> {
|
||||||
|
let mut res = Self::one();
|
||||||
|
for i in 0 .. scaling.len() {
|
||||||
|
res[(i, i)] = scaling[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new homogeneous matrix that applies a pure translation.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_translation<SB>(translation: &ColumnVector<N, DimNameDiff<D, U1>, SB>) -> Self
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, U1> {
|
||||||
|
let mut res = Self::one();
|
||||||
|
res.fixed_slice_mut::<DimNameDiff<D, U1>, U1>(0, D::dim()).copy_from(translation);
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> SquareMatrix<N, U3, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U3, U3>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U3, S> {
|
||||||
|
/// Builds a 2 dimensional homogeneous rotation matrix from an angle in radian.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_rotation(angle: N) -> Self
|
||||||
|
where S::Alloc: Allocator<N, U2, U2> {
|
||||||
|
OwnedRotation::<N, U2, S::Alloc>::new(angle).to_homogeneous()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> SquareMatrix<N, U4, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U4>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
|
||||||
|
/// Builds a 3D homogeneous rotation matrix from an axis and an angle (multiplied together).
|
||||||
|
///
|
||||||
|
/// Returns the identity matrix if the given argument is zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_rotation<SB>(axisangle: ColumnVector<N, U3, SB>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> {
|
||||||
|
OwnedRotation::<N, U3, S::Alloc>::new(axisangle).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a 3D homogeneous rotation matrix from an axis and an angle (multiplied together).
|
||||||
|
///
|
||||||
|
/// Returns the identity matrix if the given argument is zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_rotation_wrt_point<SB>(axisangle: ColumnVector<N, U3, SB>, pt: OwnedPoint<N, U3, S::Alloc>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> +
|
||||||
|
Allocator<N, U3, U1> +
|
||||||
|
Allocator<N, U1, U3> {
|
||||||
|
let rot = OwnedRotation::<N, U3, S::Alloc>::from_scaled_axis(axisangle);
|
||||||
|
IsometryBase::rotation_wrt_point(rot, pt).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a 3D homogeneous rotation matrix from an axis and an angle (multiplied together).
|
||||||
|
///
|
||||||
|
/// Returns the identity matrix if the given argument is zero.
|
||||||
|
/// This is identical to `Self::new_rotation`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_scaled_axis<SB>(axisangle: ColumnVector<N, U3, SB>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> {
|
||||||
|
OwnedRotation::<N, U3, S::Alloc>::from_scaled_axis(axisangle).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new rotation from Euler angles.
|
||||||
|
///
|
||||||
|
/// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
|
||||||
|
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self
|
||||||
|
where S::Alloc: Allocator<N, U3, U3> {
|
||||||
|
OwnedRotation::<N, U3, S::Alloc>::from_euler_angles(roll, pitch, yaw).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a 3D homogeneous rotation matrix from an axis and a rotation angle.
|
||||||
|
pub fn from_axis_angle<SB>(axis: &Unit<ColumnVector<N, U3, SB>>, angle: N) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> {
|
||||||
|
OwnedRotation::<N, U3, S::Alloc>::from_axis_angle(axis, angle).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new homogeneous matrix for an orthographic projection.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_orthographic(left: N, right: N, bottom: N, top: N, znear: N, zfar: N) -> Self {
|
||||||
|
OrthographicBase::new(left, right, bottom, top, znear, zfar).unwrap()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new homogeneous matrix for a perspective projection.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_perspective(aspect: N, fovy: N, znear: N, zfar: N) -> Self {
|
||||||
|
PerspectiveBase::new(aspect, fovy, znear, zfar).unwrap()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates an isometry that corresponds to the local frame of an observer standing at the
|
||||||
|
/// point `eye` and looking toward `target`.
|
||||||
|
///
|
||||||
|
/// It maps the view direction `target - eye` to the positive `z` axis and the origin to the
|
||||||
|
/// `eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_observer_frame<SB>(eye: &PointBase<N, U3, SB>,
|
||||||
|
target: &PointBase<N, U3, SB>,
|
||||||
|
up: &ColumnVector<N, U3, SB>)
|
||||||
|
-> Self
|
||||||
|
where SB: OwnedStorage<N, U3, U1, Alloc = S::Alloc>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> +
|
||||||
|
Allocator<N, U1, U3> +
|
||||||
|
Allocator<N, U3, U3> {
|
||||||
|
IsometryBase::<N, U3, SB, OwnedRotation<N, U3, SB::Alloc>>
|
||||||
|
::new_observer_frame(eye, target, up).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a right-handed look-at view matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_rh<SB>(eye: &PointBase<N, U3, SB>,
|
||||||
|
target: &PointBase<N, U3, SB>,
|
||||||
|
up: &ColumnVector<N, U3, SB>)
|
||||||
|
-> Self
|
||||||
|
where SB: OwnedStorage<N, U3, U1, Alloc = S::Alloc>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> +
|
||||||
|
Allocator<N, U1, U3> +
|
||||||
|
Allocator<N, U3, U3> {
|
||||||
|
IsometryBase::<N, U3, SB, OwnedRotation<N, U3, SB::Alloc>>
|
||||||
|
::look_at_rh(eye, target, up).to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a left-handed look-at view matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_lh<SB>(eye: &PointBase<N, U3, SB>,
|
||||||
|
target: &PointBase<N, U3, SB>,
|
||||||
|
up: &ColumnVector<N, U3, SB>)
|
||||||
|
-> Self
|
||||||
|
where SB: OwnedStorage<N, U3, U1, Alloc = S::Alloc>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> +
|
||||||
|
Allocator<N, U1, U3> +
|
||||||
|
Allocator<N, U3, U3> {
|
||||||
|
IsometryBase::<N, U3, SB, OwnedRotation<N, U3, SB::Alloc>>
|
||||||
|
::look_at_lh(eye, target, up).to_homogeneous()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: Storage<N, D, D> {
|
||||||
|
|
||||||
|
/// Computes the transformation equal to `self` followed by an uniform scaling factor.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_scaling(&self, scaling: N) -> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, D> {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
res.append_scaling_mut(scaling);
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to an uniform scaling factor followed by `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_scaling(&self, scaling: N) -> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
S::Alloc: Allocator<N, D, DimNameDiff<D, U1>> {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
res.prepend_scaling_mut(scaling);
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to `self` followed by a non-uniform scaling factor.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_nonuniform_scaling<SB>(&self, scaling: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
-> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, U1, D> {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
res.append_nonuniform_scaling_mut(scaling);
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to a non-uniform scaling factor followed by `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_nonuniform_scaling<SB>(&self, scaling: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
-> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, D, U1> {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
res.prepend_nonuniform_scaling_mut(scaling);
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to `self` followed by a translation.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_translation<SB>(&self, shift: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
-> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, U1> {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
res.append_translation_mut(shift);
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to a translation followed by `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_translation<SB>(&self, shift: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
-> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, U1> +
|
||||||
|
Allocator<N, DimNameDiff<D, U1>, DimNameDiff<D, U1>> +
|
||||||
|
Allocator<N, U1, DimNameDiff<D, U1>> {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
res.prepend_translation_mut(shift);
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: StorageMut<N, D, D> {
|
||||||
|
|
||||||
|
/// Computes in-place the transformation equal to `self` followed by an uniform scaling factor.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_scaling_mut(&mut self, scaling: N)
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, D> {
|
||||||
|
let mut to_scale = self.fixed_rows_mut::<DimNameDiff<D, U1>>(0);
|
||||||
|
to_scale *= scaling;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes in-place the transformation equal to an uniform scaling factor followed by `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_scaling_mut(&mut self, scaling: N)
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
S::Alloc: Allocator<N, D, DimNameDiff<D, U1>> {
|
||||||
|
let mut to_scale = self.fixed_columns_mut::<DimNameDiff<D, U1>>(0);
|
||||||
|
to_scale *= scaling;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes in-place the transformation equal to `self` followed by a non-uniform scaling factor.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_nonuniform_scaling_mut<SB>(&mut self, scaling: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, U1, D> {
|
||||||
|
for i in 0 .. scaling.len() {
|
||||||
|
let mut to_scale = self.fixed_rows_mut::<U1>(i);
|
||||||
|
to_scale *= scaling[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes in-place the transformation equal to a non-uniform scaling factor followed by `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_nonuniform_scaling_mut<SB>(&mut self, scaling: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, D, U1> {
|
||||||
|
for i in 0 .. scaling.len() {
|
||||||
|
let mut to_scale = self.fixed_columns_mut::<U1>(i);
|
||||||
|
to_scale *= scaling[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to `self` followed by a translation.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_translation_mut<SB>(&mut self, shift: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, U1> {
|
||||||
|
for i in 0 .. D::dim() {
|
||||||
|
for j in 0 .. D::dim() - 1 {
|
||||||
|
self[(j, i)] += shift[i] * self[(D::dim(), j)];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the transformation equal to a translation followed by `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_translation_mut<SB>(&mut self, shift: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
where D: DimNameSub<U1>,
|
||||||
|
SB: Storage<N, DimNameDiff<D, U1>, U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameDiff<D, U1>, U1> +
|
||||||
|
Allocator<N, DimNameDiff<D, U1>, DimNameDiff<D, U1>> +
|
||||||
|
Allocator<N, U1, DimNameDiff<D, U1>> {
|
||||||
|
let scale = self.fixed_slice::<U1, DimNameDiff<D, U1>>(D::dim(), 0).tr_dot(&shift);
|
||||||
|
let post_translation = self.fixed_slice::<DimNameDiff<D, U1>, DimNameDiff<D, U1>>(0, 0) * shift;
|
||||||
|
|
||||||
|
self[(D::dim(), D::dim())] += scale;
|
||||||
|
|
||||||
|
let mut translation = self.fixed_slice_mut::<DimNameDiff<D, U1>, U1>(0, D::dim());
|
||||||
|
translation += post_translation;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D, SA, SB> Transformation<PointBase<N, DimNameDiff<D, U1>, SB>> for SquareMatrix<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
D: DimNameSub<U1>,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, DimNameDiff<D, U1>, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA> +
|
||||||
|
Allocator<N, DimNameDiff<D, U1>, DimNameDiff<D, U1>> +
|
||||||
|
Allocator<N, DimNameDiff<D, U1>, U1> +
|
||||||
|
Allocator<N, U1, DimNameDiff<D, U1>>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, DimNameDiff<D, U1>, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
-> ColumnVector<N, DimNameDiff<D, U1>, SB> {
|
||||||
|
let transform = self.fixed_slice::<DimNameDiff<D, U1>, DimNameDiff<D, U1>>(0, 0);
|
||||||
|
let normalizer = self.fixed_slice::<U1, DimNameDiff<D, U1>>(D::dim(), 0);
|
||||||
|
let n = normalizer.tr_dot(&v);
|
||||||
|
|
||||||
|
if !n.is_zero() {
|
||||||
|
return transform * (v / n);
|
||||||
|
}
|
||||||
|
|
||||||
|
transform * v
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, DimNameDiff<D, U1>, SB>)
|
||||||
|
-> PointBase<N, DimNameDiff<D, U1>, SB> {
|
||||||
|
let transform = self.fixed_slice::<DimNameDiff<D, U1>, DimNameDiff<D, U1>>(0, 0);
|
||||||
|
let translation = self.fixed_slice::<DimNameDiff<D, U1>, U1>(0, D::dim());
|
||||||
|
let normalizer = self.fixed_slice::<U1, DimNameDiff<D, U1>>(D::dim(), 0);
|
||||||
|
let n = normalizer.tr_dot(&pt.coords) + unsafe { *self.get_unchecked(D::dim(), D::dim()) };
|
||||||
|
|
||||||
|
if !n.is_zero() {
|
||||||
|
return transform * (pt / n) + translation;
|
||||||
|
}
|
||||||
|
|
||||||
|
transform * pt + translation
|
||||||
|
}
|
||||||
|
}
|
77
src/core/componentwise.rs
Normal file
77
src/core/componentwise.rs
Normal file
@ -0,0 +1,77 @@
|
|||||||
|
// Non-convensional componentwise operators.
|
||||||
|
|
||||||
|
use num::Signed;
|
||||||
|
|
||||||
|
use alga::general::{ClosedMul, ClosedDiv};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, OwnedMatrix, MatrixSum};
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
use core::allocator::SameShapeAllocator;
|
||||||
|
use core::constraint::{ShapeConstraint, SameNumberOfRows, SameNumberOfColumns};
|
||||||
|
|
||||||
|
|
||||||
|
/// The type of the result of a matrix componentwise operation.
|
||||||
|
pub type MatrixComponentOp<N, R1, C1, R2, C2, SA> = MatrixSum<N, R1, C1, R2, C2, SA>;
|
||||||
|
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
|
||||||
|
/// Computes the componentwise absolute value.
|
||||||
|
#[inline]
|
||||||
|
pub fn abs(&self) -> OwnedMatrix<N, R, C, S::Alloc>
|
||||||
|
where N: Signed {
|
||||||
|
let mut res = self.clone_owned();
|
||||||
|
|
||||||
|
for e in res.iter_mut() {
|
||||||
|
*e = e.abs();
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: add other operators like component_ln, component_pow, etc. ?
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! component_binop_impl(
|
||||||
|
($($binop: ident, $binop_mut: ident, $Trait: ident . $binop_assign: ident);* $(;)*) => {$(
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
|
||||||
|
/// Componentwise matrix multiplication.
|
||||||
|
#[inline]
|
||||||
|
pub fn $binop<R2, C2, SB>(&self, rhs: &Matrix<N, R2, C2, SB>) -> MatrixComponentOp<N, R, C, R2, C2, S>
|
||||||
|
where N: $Trait,
|
||||||
|
R2: Dim, C2: Dim,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
S::Alloc: SameShapeAllocator<N, R, C, R2, C2, S>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R, R2> + SameNumberOfColumns<C, C2> {
|
||||||
|
|
||||||
|
let mut res = self.clone_owned_sum();
|
||||||
|
|
||||||
|
for (res, rhs) in res.iter_mut().zip(rhs.iter()) {
|
||||||
|
res.$binop_assign(*rhs);
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: StorageMut<N, R, C>> Matrix<N, R, C, S> {
|
||||||
|
/// Componentwise matrix multiplication.
|
||||||
|
#[inline]
|
||||||
|
pub fn $binop_mut<R2, C2, SB>(&mut self, rhs: &Matrix<N, R2, C2, SB>)
|
||||||
|
where N: $Trait,
|
||||||
|
R2: Dim,
|
||||||
|
C2: Dim,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R, R2> + SameNumberOfColumns<C, C2> {
|
||||||
|
for (me, rhs) in self.iter_mut().zip(rhs.iter()) {
|
||||||
|
me.$binop_assign(*rhs);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
component_binop_impl!(
|
||||||
|
component_mul, component_mul_mut, ClosedMul.mul_assign;
|
||||||
|
component_div, component_div_mut, ClosedDiv.div_assign;
|
||||||
|
// FIXME: add other operators like bitshift, etc. ?
|
||||||
|
);
|
55
src/core/constraint.rs
Normal file
55
src/core/constraint.rs
Normal file
@ -0,0 +1,55 @@
|
|||||||
|
use core::dimension::{Dim, DimName, Dynamic};
|
||||||
|
|
||||||
|
/// A type for enforcing constraints.
|
||||||
|
pub struct ShapeConstraint;
|
||||||
|
|
||||||
|
/// Constraints `C1` and `R2` to be equivalent.
|
||||||
|
pub trait AreMultipliable<R1: Dim, C1: Dim,
|
||||||
|
R2: Dim, C2: Dim> {
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<R1: Dim, C1: Dim, R2: Dim, C2: Dim> AreMultipliable<R1, C1, R2, C2> for ShapeConstraint
|
||||||
|
where ShapeConstraint: DimEq<C1, R2> {
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! equality_trait_decl(
|
||||||
|
($($Trait: ident),* $(,)*) => {$(
|
||||||
|
// XXX: we can't do something like `DimEq<D1> for D2` because we would require a blancket impl…
|
||||||
|
pub trait $Trait<D1: Dim, D2: Dim> {
|
||||||
|
type Representative: Dim;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: Dim> $Trait<D, D> for ShapeConstraint {
|
||||||
|
type Representative = D;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: DimName> $Trait<D, Dynamic> for ShapeConstraint {
|
||||||
|
type Representative = D;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: DimName> $Trait<Dynamic, D> for ShapeConstraint {
|
||||||
|
type Representative = D;
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
equality_trait_decl!(DimEq, SameNumberOfRows, SameNumberOfColumns);
|
||||||
|
|
||||||
|
/// Constraints D1 and D2 to be equivalent, where the both designates dimensions of algebraic
|
||||||
|
/// entities (e.g. square matrices).
|
||||||
|
pub trait SameDimension<D1: Dim, D2: Dim>: SameNumberOfRows<D1, D2> + SameNumberOfColumns<D1, D2> {
|
||||||
|
type Representative: Dim;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: Dim> SameDimension<D, D> for ShapeConstraint {
|
||||||
|
type Representative = D;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: DimName> SameDimension<D, Dynamic> for ShapeConstraint {
|
||||||
|
type Representative = D;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: DimName> SameDimension<Dynamic, D> for ShapeConstraint {
|
||||||
|
type Representative = D;
|
||||||
|
}
|
643
src/core/construction.rs
Normal file
643
src/core/construction.rs
Normal file
@ -0,0 +1,643 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use std::iter;
|
||||||
|
use num::{Zero, One, Bounded};
|
||||||
|
use rand::{self, Rand, Rng};
|
||||||
|
use typenum::{self, Cmp, Greater};
|
||||||
|
|
||||||
|
use alga::general::{ClosedAdd, ClosedMul};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, SquareMatrix, ColumnVector, Unit};
|
||||||
|
use core::dimension::{Dim, DimName, Dynamic, U1, U2, U3, U4, U5, U6};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Generic constructors.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: OwnedStorage<N, R, C>> Matrix<N, R, C, S>
|
||||||
|
// XXX: needed because of a compiler bug. See the rust compiler issue #26026.
|
||||||
|
where S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
/// Creates a new uninitialized matrix. If the matrix has a compile-time dimension, this panics
|
||||||
|
/// if `nrows != R::to_usize()` or `ncols != C::to_usize()`.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn new_uninitialized_generic(nrows: R, ncols: C) -> Matrix<N, R, C, S> {
|
||||||
|
Matrix::from_data(S::Alloc::allocate_uninitialized(nrows, ncols))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with all its elements set to `elem`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_element_generic(nrows: R, ncols: C, elem: N) -> Matrix<N, R, C, S> {
|
||||||
|
let len = nrows.value() * ncols.value();
|
||||||
|
Matrix::from_iterator_generic(nrows, ncols, iter::repeat(elem).take(len))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with all its elements filled by an iterator.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_iterator_generic<I>(nrows: R, ncols: C, iter: I) -> Matrix<N, R, C, S>
|
||||||
|
where I: IntoIterator<Item = N> {
|
||||||
|
Matrix::from_data(S::Alloc::allocate_from_iterator(nrows, ncols, iter))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with its elements filled with the components provided by a slice in
|
||||||
|
/// row-major order.
|
||||||
|
///
|
||||||
|
/// The order of elements in the slice must follow the usual mathematic writing, i.e.,
|
||||||
|
/// row-by-row.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_row_slice_generic(nrows: R, ncols: C, slice: &[N]) -> Matrix<N, R, C, S> {
|
||||||
|
assert!(slice.len() == nrows.value() * ncols.value(),
|
||||||
|
"Matrix init. error: the slice did not contain the right number of elements.");
|
||||||
|
|
||||||
|
let mut res = unsafe { Self::new_uninitialized_generic(nrows, ncols) };
|
||||||
|
let mut iter = slice.iter();
|
||||||
|
|
||||||
|
for i in 0 .. nrows.value() {
|
||||||
|
for j in 0 .. ncols.value() {
|
||||||
|
unsafe {
|
||||||
|
*res.get_unchecked_mut(i, j) = *iter.next().unwrap()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with its elements filled with the components provided by a slice. The
|
||||||
|
/// components must have the same layout as the matrix data storage (i.e. row-major or column-major).
|
||||||
|
#[inline]
|
||||||
|
pub fn from_column_slice_generic(nrows: R, ncols: C, slice: &[N]) -> Matrix<N, R, C, S> {
|
||||||
|
Matrix::from_iterator_generic(nrows, ncols, slice.iter().cloned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix filled with the results of a function applied to each of its component
|
||||||
|
/// coordinates.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_fn_generic<F>(nrows: R, ncols: C, mut f: F) -> Matrix<N, R, C, S>
|
||||||
|
where F: FnMut(usize, usize) -> N {
|
||||||
|
let mut res = unsafe { Self::new_uninitialized_generic(nrows, ncols) };
|
||||||
|
|
||||||
|
for i in 0 .. nrows.value() {
|
||||||
|
for j in 0 .. ncols.value() {
|
||||||
|
unsafe { *res.get_unchecked_mut(i, j) = f(i, j) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new indentity matrix.
|
||||||
|
///
|
||||||
|
/// If the matrix is not square, the largest square submatrix starting at index `(0, 0)` is set
|
||||||
|
/// to the identity matrix. All other entries are set to zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity_generic(nrows: R, ncols: C) -> Matrix<N, R, C, S>
|
||||||
|
where N: Zero + One {
|
||||||
|
Self::from_diagonal_element_generic(nrows, ncols, N::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new matrix with its diagonal filled with copies of `elt`.
|
||||||
|
///
|
||||||
|
/// If the matrix is not square, the largest square submatrix starting at index `(0, 0)` is set
|
||||||
|
/// to the identity matrix. All other entries are set to zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_diagonal_element_generic(nrows: R, ncols: C, elt: N) -> Matrix<N, R, C, S>
|
||||||
|
where N: Zero + One {
|
||||||
|
let mut res = unsafe { Self::new_uninitialized_generic(nrows, ncols) };
|
||||||
|
res.fill(N::zero());
|
||||||
|
|
||||||
|
for i in 0 .. ::min(nrows.value(), ncols.value()) {
|
||||||
|
unsafe { *res.get_unchecked_mut(i, i) = elt }
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn from_rows<SB>(rows: &[Matrix<N, U1, C, SB>]) -> Matrix<N, R, C, S>
|
||||||
|
where SB: Storage<N, U1, C> {
|
||||||
|
|
||||||
|
assert!(rows.len() > 0, "At least one row must be given.");
|
||||||
|
let nrows = R::try_to_usize().unwrap_or(rows.len());
|
||||||
|
let ncols = rows[0].len();
|
||||||
|
assert!(rows.len() == nrows, "Invalid number of rows provided to build this matrix.");
|
||||||
|
|
||||||
|
if C::try_to_usize().is_none() {
|
||||||
|
assert!(rows.iter().all(|r| r.len() == ncols),
|
||||||
|
"The rows provided must all have the same dimension.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: optimize that.
|
||||||
|
Self::from_fn_generic(R::from_usize(nrows), C::from_usize(ncols), |i, j| rows[i][(0, j)])
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn from_columns<SB>(columns: &[ColumnVector<N, R, SB>]) -> Matrix<N, R, C, S>
|
||||||
|
where SB: Storage<N, R, U1> {
|
||||||
|
|
||||||
|
assert!(columns.len() > 0, "At least one column must be given.");
|
||||||
|
let ncols = C::try_to_usize().unwrap_or(columns.len());
|
||||||
|
let nrows = columns[0].len();
|
||||||
|
assert!(columns.len() == ncols, "Invalid number of columns provided to build this matrix.");
|
||||||
|
|
||||||
|
if R::try_to_usize().is_none() {
|
||||||
|
assert!(columns.iter().all(|r| r.len() == nrows),
|
||||||
|
"The columns provided must all have the same dimension.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: optimize that.
|
||||||
|
Self::from_fn_generic(R::from_usize(nrows), C::from_usize(ncols), |i, j| columns[j][i])
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Rand,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
/// Creates a matrix filled with random values.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_random_generic(nrows: R, ncols: C) -> Matrix<N, R, C, S> {
|
||||||
|
Matrix::from_fn_generic(nrows, ncols, |_, _| rand::random())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: Dim, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Zero,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
/// Creates a square matrix with its diagonal set to `diag` and all other entries set to 0.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_diagonal<SB: Storage<N, D, U1>>(diag: &ColumnVector<N, D, SB>) -> Self {
|
||||||
|
let (dim, _) = diag.data.shape();
|
||||||
|
let mut res = Self::from_element_generic(dim, dim, N::zero());
|
||||||
|
|
||||||
|
for i in 0 .. diag.len() {
|
||||||
|
unsafe { *res.get_unchecked_mut(i, i) = *diag.get_unchecked(i, 0); }
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Generate constructors with varying number of arguments, depending on the object type.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
macro_rules! impl_constructors(
|
||||||
|
($($Dims: ty),*; $(=> $DimIdent: ident: $DimBound: ident),*; $($gargs: expr),*; $($args: ident),*) => {
|
||||||
|
impl<N: Scalar, $($DimIdent: $DimBound, )* S> Matrix<N $(, $Dims)*, S>
|
||||||
|
where S: OwnedStorage<N $(, $Dims)*>,
|
||||||
|
S::Alloc: OwnedAllocator<N $(, $Dims)*, S> {
|
||||||
|
|
||||||
|
/// Creates a new uninitialized matrix.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn new_uninitialized($($args: usize),*) -> Matrix<N $(, $Dims)*, S> {
|
||||||
|
Self::new_uninitialized_generic($($gargs),*)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with all its elements set to `elem`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_element($($args: usize,)* elem: N) -> Matrix<N $(, $Dims)*, S> {
|
||||||
|
Self::from_element_generic($($gargs, )* elem)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with all its elements filled by an iterator.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_iterator<I>($($args: usize,)* iter: I) -> Matrix<N $(, $Dims)*, S>
|
||||||
|
where I: IntoIterator<Item = N> {
|
||||||
|
Self::from_iterator_generic($($gargs, )* iter)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with its elements filled with the components provided by a slice
|
||||||
|
/// in row-major order.
|
||||||
|
///
|
||||||
|
/// The order of elements in the slice must follow the usual mathematic writing, i.e.,
|
||||||
|
/// row-by-row.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_row_slice($($args: usize,)* slice: &[N]) -> Matrix<N $(, $Dims)*, S> {
|
||||||
|
Self::from_row_slice_generic($($gargs, )* slice)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix with its elements filled with the components provided by a slice
|
||||||
|
/// in column-major order.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_column_slice($($args: usize,)* slice: &[N]) -> Matrix<N $(, $Dims)*, S> {
|
||||||
|
Self::from_column_slice_generic($($gargs, )* slice)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a matrix filled with the results of a function applied to each of its
|
||||||
|
/// component coordinates.
|
||||||
|
// FIXME: don't take a dimension of the matrix is statically sized.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_fn<F>($($args: usize,)* f: F) -> Matrix<N $(, $Dims)*, S>
|
||||||
|
where F: FnMut(usize, usize) -> N {
|
||||||
|
Self::from_fn_generic($($gargs, )* f)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn identity($($args: usize,)*) -> Matrix<N $(, $Dims)*, S>
|
||||||
|
where N: Zero + One {
|
||||||
|
Self::identity_generic($($gargs),* )
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn from_diagonal_element($($args: usize,)* elt: N) -> Matrix<N $(, $Dims)*, S>
|
||||||
|
where N: Zero + One {
|
||||||
|
Self::from_diagonal_element_generic($($gargs, )* elt)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar + Rand, $($DimIdent: $DimBound, )* S> Matrix<N $(, $Dims)*, S>
|
||||||
|
where S: OwnedStorage<N $(, $Dims)*>,
|
||||||
|
S::Alloc: OwnedAllocator<N $(, $Dims)*, S> {
|
||||||
|
|
||||||
|
/// Creates a matrix filled with random values.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_random($($args: usize),*) -> Matrix<N $(, $Dims)*, S> {
|
||||||
|
Self::new_random_generic($($gargs),*)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// FIXME: this is not very pretty. We could find a better call syntax.
|
||||||
|
impl_constructors!(R, C; // Arguments for Matrix<N, ..., S>
|
||||||
|
=> R: DimName, => C: DimName; // Type parameters for impl<N, ..., S>
|
||||||
|
R::name(), C::name(); // Arguments for `_generic` constructors.
|
||||||
|
); // Arguments for non-generic constructors.
|
||||||
|
|
||||||
|
impl_constructors!(R, Dynamic;
|
||||||
|
=> R: DimName;
|
||||||
|
R::name(), Dynamic::new(ncols);
|
||||||
|
ncols);
|
||||||
|
|
||||||
|
impl_constructors!(Dynamic, C;
|
||||||
|
=> C: DimName;
|
||||||
|
Dynamic::new(nrows), C::name();
|
||||||
|
nrows);
|
||||||
|
|
||||||
|
impl_constructors!(Dynamic, Dynamic;
|
||||||
|
;
|
||||||
|
Dynamic::new(nrows), Dynamic::new(ncols);
|
||||||
|
nrows, ncols);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Zero, One, Rand traits.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, R: DimName, C: DimName, S> Zero for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Zero + ClosedAdd,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn zero() -> Self {
|
||||||
|
Self::from_element(N::zero())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_zero(&self) -> bool {
|
||||||
|
self.iter().all(|e| e.is_zero())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> One for Matrix<N, D, D, S>
|
||||||
|
where N: Scalar + Zero + One + ClosedMul + ClosedAdd,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> Bounded for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Bounded,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn max_value() -> Self {
|
||||||
|
Self::from_element(N::max_value())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn min_value() -> Self {
|
||||||
|
Self::from_element(N::min_value())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar + Rand, R: Dim, C: Dim, S> Rand for Matrix<N, R, C, S>
|
||||||
|
where S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<G: Rng>(rng: &mut G) -> Self {
|
||||||
|
let nrows = R::try_to_usize().unwrap_or(rng.gen_range(0, 10));
|
||||||
|
let ncols = C::try_to_usize().unwrap_or(rng.gen_range(0, 10));
|
||||||
|
|
||||||
|
Self::from_fn_generic(R::from_usize(nrows), C::from_usize(ncols), |_, _| rng.gen())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
impl<N, R, C, S> Arbitrary for Matrix<N, R, C, S>
|
||||||
|
where R: Dim, C: Dim,
|
||||||
|
N: Scalar + Arbitrary + Send,
|
||||||
|
S: OwnedStorage<N, R, C> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
let nrows = R::try_to_usize().unwrap_or(g.gen_range(0, 10));
|
||||||
|
let ncols = C::try_to_usize().unwrap_or(g.gen_range(0, 10));
|
||||||
|
|
||||||
|
Self::from_fn_generic(R::from_usize(nrows), C::from_usize(ncols), |_, _| N::arbitrary(g))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Constructors for small matrices and vectors.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
macro_rules! componentwise_constructors_impl(
|
||||||
|
($($R: ty, $C: ty, $($args: ident:($irow: expr,$icol: expr)),*);* $(;)*) => {$(
|
||||||
|
impl<N, S> Matrix<N, $R, $C, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: OwnedStorage<N, $R, $C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $R, $C, S> {
|
||||||
|
/// Initializes this matrix from its components.
|
||||||
|
#[inline]
|
||||||
|
pub fn new($($args: N),*) -> Matrix<N, $R, $C, S> {
|
||||||
|
unsafe {
|
||||||
|
let mut res = Self::new_uninitialized();
|
||||||
|
$( *res.get_unchecked_mut($irow, $icol) = $args; )*
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
componentwise_constructors_impl!(
|
||||||
|
/*
|
||||||
|
* Square matrices 1 .. 6.
|
||||||
|
*/
|
||||||
|
U2, U2, m11:(0,0), m12:(0,1),
|
||||||
|
m21:(1,0), m22:(1,1);
|
||||||
|
U3, U3, m11:(0,0), m12:(0,1), m13:(0,2),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2);
|
||||||
|
U4, U4, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3);
|
||||||
|
U5, U5, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3), m45:(3,4),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2), m54:(4,3), m55:(4,4);
|
||||||
|
U6, U6, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4), m16:(0,5),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4), m26:(1,5),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4), m36:(2,5),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3), m45:(3,4), m46:(3,5),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2), m54:(4,3), m55:(4,4), m56:(4,5),
|
||||||
|
m61:(5,0), m62:(5,1), m63:(5,2), m64:(5,3), m65:(5,4), m66:(5,5);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 2 rows.
|
||||||
|
*/
|
||||||
|
U2, U3, m11:(0,0), m12:(0,1), m13:(0,2),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2);
|
||||||
|
U2, U4, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3);
|
||||||
|
U2, U5, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4);
|
||||||
|
U2, U6, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4), m16:(0,5),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4), m26:(1,5);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 3 rows.
|
||||||
|
*/
|
||||||
|
U3, U2, m11:(0,0), m12:(0,1),
|
||||||
|
m21:(1,0), m22:(1,1),
|
||||||
|
m31:(2,0), m32:(2,1);
|
||||||
|
U3, U4, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3);
|
||||||
|
U3, U5, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4);
|
||||||
|
U3, U6, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4), m16:(0,5),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4), m26:(1,5),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4), m36:(2,5);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 4 rows.
|
||||||
|
*/
|
||||||
|
U4, U2, m11:(0,0), m12:(0,1),
|
||||||
|
m21:(1,0), m22:(1,1),
|
||||||
|
m31:(2,0), m32:(2,1),
|
||||||
|
m41:(3,0), m42:(3,1);
|
||||||
|
U4, U3, m11:(0,0), m12:(0,1), m13:(0,2),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2);
|
||||||
|
U4, U5, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3), m45:(3,4);
|
||||||
|
U4, U6, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4), m16:(0,5),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4), m26:(1,5),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4), m36:(2,5),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3), m45:(3,4), m46:(3,5);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 5 rows.
|
||||||
|
*/
|
||||||
|
U5, U2, m11:(0,0), m12:(0,1),
|
||||||
|
m21:(1,0), m22:(1,1),
|
||||||
|
m31:(2,0), m32:(2,1),
|
||||||
|
m41:(3,0), m42:(3,1),
|
||||||
|
m51:(4,0), m52:(4,1);
|
||||||
|
U5, U3, m11:(0,0), m12:(0,1), m13:(0,2),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2);
|
||||||
|
U5, U4, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2), m54:(4,3);
|
||||||
|
U5, U6, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4), m16:(0,5),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4), m26:(1,5),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4), m36:(2,5),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3), m45:(3,4), m46:(3,5),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2), m54:(4,3), m55:(4,4), m56:(4,5);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 6 rows.
|
||||||
|
*/
|
||||||
|
U6, U2, m11:(0,0), m12:(0,1),
|
||||||
|
m21:(1,0), m22:(1,1),
|
||||||
|
m31:(2,0), m32:(2,1),
|
||||||
|
m41:(3,0), m42:(3,1),
|
||||||
|
m51:(4,0), m52:(4,1),
|
||||||
|
m61:(5,0), m62:(5,1);
|
||||||
|
U6, U3, m11:(0,0), m12:(0,1), m13:(0,2),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2),
|
||||||
|
m61:(5,0), m62:(5,1), m63:(5,2);
|
||||||
|
U6, U4, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2), m54:(4,3),
|
||||||
|
m61:(5,0), m62:(5,1), m63:(5,2), m64:(5,3);
|
||||||
|
U6, U5, m11:(0,0), m12:(0,1), m13:(0,2), m14:(0,3), m15:(0,4),
|
||||||
|
m21:(1,0), m22:(1,1), m23:(1,2), m24:(1,3), m25:(1,4),
|
||||||
|
m31:(2,0), m32:(2,1), m33:(2,2), m34:(2,3), m35:(2,4),
|
||||||
|
m41:(3,0), m42:(3,1), m43:(3,2), m44:(3,3), m45:(3,4),
|
||||||
|
m51:(4,0), m52:(4,1), m53:(4,2), m54:(4,3), m55:(4,4),
|
||||||
|
m61:(5,0), m62:(5,1), m63:(5,2), m64:(5,3), m65:(5,4);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Row vectors 1 .. 6.
|
||||||
|
*/
|
||||||
|
U1, U1, x:(0,0);
|
||||||
|
U1, U2, x:(0,0), y:(0,1);
|
||||||
|
U1, U3, x:(0,0), y:(0,1), z:(0,2);
|
||||||
|
U1, U4, x:(0,0), y:(0,1), z:(0,2), w:(0,3);
|
||||||
|
U1, U5, x:(0,0), y:(0,1), z:(0,2), w:(0,3), a:(0,4);
|
||||||
|
U1, U6, x:(0,0), y:(0,1), z:(0,2), w:(0,3), a:(0,4), b:(0,5);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Column vectors 1 .. 6.
|
||||||
|
*/
|
||||||
|
U2, U1, x:(0,0), y:(1,0);
|
||||||
|
U3, U1, x:(0,0), y:(1,0), z:(2,0);
|
||||||
|
U4, U1, x:(0,0), y:(1,0), z:(2,0), w:(3,0);
|
||||||
|
U5, U1, x:(0,0), y:(1,0), z:(2,0), w:(3,0), a:(4,0);
|
||||||
|
U6, U1, x:(0,0), y:(1,0), z:(2,0), w:(3,0), a:(4,0), b:(5,0);
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Axis constructors.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, R: DimName, S> ColumnVector<N, R, S>
|
||||||
|
where N: Scalar + Zero + One,
|
||||||
|
S: OwnedStorage<N, R, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, U1, S> {
|
||||||
|
/// The column vector with a 1 as its first component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn x() -> Self
|
||||||
|
where R::Value: Cmp<typenum::U0, Output = Greater> {
|
||||||
|
let mut res = Self::from_element(N::zero());
|
||||||
|
unsafe { *res.get_unchecked_mut(0, 0) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The column vector with a 1 as its second component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn y() -> Self
|
||||||
|
where R::Value: Cmp<typenum::U1, Output = Greater> {
|
||||||
|
let mut res = Self::from_element(N::zero());
|
||||||
|
unsafe { *res.get_unchecked_mut(1, 0) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The column vector with a 1 as its third component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn z() -> Self
|
||||||
|
where R::Value: Cmp<typenum::U2, Output = Greater> {
|
||||||
|
let mut res = Self::from_element(N::zero());
|
||||||
|
unsafe { *res.get_unchecked_mut(2, 0) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The column vector with a 1 as its fourth component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn w() -> Self
|
||||||
|
where R::Value: Cmp<typenum::U3, Output = Greater> {
|
||||||
|
let mut res = Self::from_element(N::zero());
|
||||||
|
unsafe { *res.get_unchecked_mut(3, 0) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The column vector with a 1 as its fifth component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn a() -> Self
|
||||||
|
where R::Value: Cmp<typenum::U4, Output = Greater> {
|
||||||
|
let mut res = Self::from_element(N::zero());
|
||||||
|
unsafe { *res.get_unchecked_mut(4, 0) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The column vector with a 1 as its sixth component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn b() -> Self
|
||||||
|
where R::Value: Cmp<typenum::U5, Output = Greater> {
|
||||||
|
let mut res = Self::from_element(N::zero());
|
||||||
|
unsafe { *res.get_unchecked_mut(5, 0) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit column vector with a 1 as its first component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn x_axis() -> Unit<Self>
|
||||||
|
where R::Value: Cmp<typenum::U0, Output = Greater> {
|
||||||
|
Unit::new_unchecked(Self::x())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit column vector with a 1 as its second component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn y_axis() -> Unit<Self>
|
||||||
|
where R::Value: Cmp<typenum::U1, Output = Greater> {
|
||||||
|
Unit::new_unchecked(Self::y())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit column vector with a 1 as its third component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn z_axis() -> Unit<Self>
|
||||||
|
where R::Value: Cmp<typenum::U2, Output = Greater> {
|
||||||
|
Unit::new_unchecked(Self::z())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit column vector with a 1 as its fourth component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn w_axis() -> Unit<Self>
|
||||||
|
where R::Value: Cmp<typenum::U3, Output = Greater> {
|
||||||
|
Unit::new_unchecked(Self::w())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit column vector with a 1 as its fifth component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn a_axis() -> Unit<Self>
|
||||||
|
where R::Value: Cmp<typenum::U4, Output = Greater> {
|
||||||
|
Unit::new_unchecked(Self::a())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit column vector with a 1 as its sixth component, and zero elsewhere.
|
||||||
|
#[inline]
|
||||||
|
pub fn b_axis() -> Unit<Self>
|
||||||
|
where R::Value: Cmp<typenum::U5, Output = Greater> {
|
||||||
|
Unit::new_unchecked(Self::b())
|
||||||
|
}
|
||||||
|
}
|
59
src/core/conversion.rs
Normal file
59
src/core/conversion.rs
Normal file
@ -0,0 +1,59 @@
|
|||||||
|
use alga::general::{SubsetOf, SupersetOf};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix};
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::constraint::{ShapeConstraint, SameNumberOfRows, SameNumberOfColumns};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{OwnedAllocator, SameShapeAllocator};
|
||||||
|
|
||||||
|
|
||||||
|
// FIXME: too bad this won't work allo slice conversions.
|
||||||
|
impl<N1, N2, R1, C1, R2, C2, SA, SB> SubsetOf<Matrix<N2, R2, C2, SB>> for Matrix<N1, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N1: Scalar,
|
||||||
|
N2: Scalar + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, R1, C1>,
|
||||||
|
SB: OwnedStorage<N2, R2, C2>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, R2, C2, SB>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, R1, C1, SA> +
|
||||||
|
SameShapeAllocator<N1, R1, C1, R2, C2, SA>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> Matrix<N2, R2, C2, SB> {
|
||||||
|
let (nrows, ncols) = self.shape();
|
||||||
|
let nrows2 = R2::from_usize(nrows);
|
||||||
|
let ncols2 = C2::from_usize(ncols);
|
||||||
|
|
||||||
|
let mut res = unsafe { Matrix::<N2, R2, C2, SB>::new_uninitialized_generic(nrows2, ncols2) };
|
||||||
|
for i in 0 .. nrows {
|
||||||
|
for j in 0 .. ncols {
|
||||||
|
unsafe {
|
||||||
|
*res.get_unchecked_mut(i, j) = N2::from_subset(self.get_unchecked(i, j))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &Matrix<N2, R2, C2, SB>) -> bool {
|
||||||
|
m.iter().all(|e| e.is_in_subset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &Matrix<N2, R2, C2, SB>) -> Self {
|
||||||
|
let (nrows2, ncols2) = m.shape();
|
||||||
|
let nrows = R1::from_usize(nrows2);
|
||||||
|
let ncols = C1::from_usize(ncols2);
|
||||||
|
|
||||||
|
let mut res = Self::new_uninitialized_generic(nrows, ncols);
|
||||||
|
for i in 0 .. nrows2 {
|
||||||
|
for j in 0 .. ncols2 {
|
||||||
|
*res.get_unchecked_mut(i, j) = m.get_unchecked(i, j).to_subset_unchecked()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
230
src/core/coordinates.rs
Normal file
230
src/core/coordinates.rs
Normal file
@ -0,0 +1,230 @@
|
|||||||
|
use std::mem;
|
||||||
|
use std::ops::{Deref, DerefMut};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix};
|
||||||
|
use core::dimension::{U1, U2, U3, U4, U5, U6};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Give coordinates to owned Vector{1 .. 6} and Matrix{1 .. 6}
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
macro_rules! coords_impl(
|
||||||
|
($T: ident; $($comps: ident),*) => {
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Eq, PartialEq, RustcEncodable, RustcDecodable, Clone, Hash, Debug, Copy)]
|
||||||
|
pub struct $T<N: Scalar> {
|
||||||
|
$(pub $comps: N),*
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
macro_rules! deref_impl(
|
||||||
|
($R: ty, $C: ty; $Target: ident) => {
|
||||||
|
impl<N: Scalar, S> Deref for Matrix<N, $R, $C, S>
|
||||||
|
where S: OwnedStorage<N, $R, $C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $R, $C, S> {
|
||||||
|
type Target = $Target<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
unsafe { mem::transmute(self) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, S> DerefMut for Matrix<N, $R, $C, S>
|
||||||
|
where S: OwnedStorage<N, $R, $C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $R, $C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
unsafe { mem::transmute(self) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Vector coordinates.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
coords_impl!(X; x);
|
||||||
|
coords_impl!(XY; x, y);
|
||||||
|
coords_impl!(XYZ; x, y, z);
|
||||||
|
coords_impl!(XYZW; x, y, z, w);
|
||||||
|
coords_impl!(XYZWA; x, y, z, w, a);
|
||||||
|
coords_impl!(XYZWAB; x, y, z, w, a, b);
|
||||||
|
coords_impl!(IJKW; i, j, k, w);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 2 rows.
|
||||||
|
*/
|
||||||
|
coords_impl!(M2x2; m11, m21,
|
||||||
|
m12, m22);
|
||||||
|
coords_impl!(M2x3; m11, m21,
|
||||||
|
m12, m22,
|
||||||
|
m13, m23);
|
||||||
|
coords_impl!(M2x4; m11, m21,
|
||||||
|
m12, m22,
|
||||||
|
m13, m23,
|
||||||
|
m14, m24);
|
||||||
|
coords_impl!(M2x5; m11, m21,
|
||||||
|
m12, m22,
|
||||||
|
m13, m23,
|
||||||
|
m14, m24,
|
||||||
|
m15, m25);
|
||||||
|
coords_impl!(M2x6; m11, m21,
|
||||||
|
m12, m22,
|
||||||
|
m13, m23,
|
||||||
|
m14, m24,
|
||||||
|
m15, m25,
|
||||||
|
m16, m26);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 3 rows.
|
||||||
|
*/
|
||||||
|
coords_impl!(M3x2; m11, m21, m31,
|
||||||
|
m12, m22, m32);
|
||||||
|
coords_impl!(M3x3; m11, m21, m31,
|
||||||
|
m12, m22, m32,
|
||||||
|
m13, m23, m33);
|
||||||
|
coords_impl!(M3x4; m11, m21, m31,
|
||||||
|
m12, m22, m32,
|
||||||
|
m13, m23, m33,
|
||||||
|
m14, m24, m34);
|
||||||
|
coords_impl!(M3x5; m11, m21, m31,
|
||||||
|
m12, m22, m32,
|
||||||
|
m13, m23, m33,
|
||||||
|
m14, m24, m34,
|
||||||
|
m15, m25, m35);
|
||||||
|
coords_impl!(M3x6; m11, m21, m31,
|
||||||
|
m12, m22, m32,
|
||||||
|
m13, m23, m33,
|
||||||
|
m14, m24, m34,
|
||||||
|
m15, m25, m35,
|
||||||
|
m16, m26, m36);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 4 rows.
|
||||||
|
*/
|
||||||
|
coords_impl!(M4x2; m11, m21, m31, m41,
|
||||||
|
m12, m22, m32, m42);
|
||||||
|
coords_impl!(M4x3; m11, m21, m31, m41,
|
||||||
|
m12, m22, m32, m42,
|
||||||
|
m13, m23, m33, m43);
|
||||||
|
coords_impl!(M4x4; m11, m21, m31, m41,
|
||||||
|
m12, m22, m32, m42,
|
||||||
|
m13, m23, m33, m43,
|
||||||
|
m14, m24, m34, m44);
|
||||||
|
coords_impl!(M4x5; m11, m21, m31, m41,
|
||||||
|
m12, m22, m32, m42,
|
||||||
|
m13, m23, m33, m43,
|
||||||
|
m14, m24, m34, m44,
|
||||||
|
m15, m25, m35, m45);
|
||||||
|
coords_impl!(M4x6; m11, m21, m31, m41,
|
||||||
|
m12, m22, m32, m42,
|
||||||
|
m13, m23, m33, m43,
|
||||||
|
m14, m24, m34, m44,
|
||||||
|
m15, m25, m35, m45,
|
||||||
|
m16, m26, m36, m46);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 5 rows.
|
||||||
|
*/
|
||||||
|
coords_impl!(M5x2; m11, m21, m31, m41, m51,
|
||||||
|
m12, m22, m32, m42, m52);
|
||||||
|
coords_impl!(M5x3; m11, m21, m31, m41, m51,
|
||||||
|
m12, m22, m32, m42, m52,
|
||||||
|
m13, m23, m33, m43, m53);
|
||||||
|
coords_impl!(M5x4; m11, m21, m31, m41, m51,
|
||||||
|
m12, m22, m32, m42, m52,
|
||||||
|
m13, m23, m33, m43, m53,
|
||||||
|
m14, m24, m34, m44, m54);
|
||||||
|
coords_impl!(M5x5; m11, m21, m31, m41, m51,
|
||||||
|
m12, m22, m32, m42, m52,
|
||||||
|
m13, m23, m33, m43, m53,
|
||||||
|
m14, m24, m34, m44, m54,
|
||||||
|
m15, m25, m35, m45, m55);
|
||||||
|
coords_impl!(M5x6; m11, m21, m31, m41, m51,
|
||||||
|
m12, m22, m32, m42, m52,
|
||||||
|
m13, m23, m33, m43, m53,
|
||||||
|
m14, m24, m34, m44, m54,
|
||||||
|
m15, m25, m35, m45, m55,
|
||||||
|
m16, m26, m36, m46, m56);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Rectangular matrices with 6 rows.
|
||||||
|
*/
|
||||||
|
|
||||||
|
coords_impl!(M6x2; m11, m21, m31, m41, m51, m61,
|
||||||
|
m12, m22, m32, m42, m52, m62);
|
||||||
|
coords_impl!(M6x3; m11, m21, m31, m41, m51, m61,
|
||||||
|
m12, m22, m32, m42, m52, m62,
|
||||||
|
m13, m23, m33, m43, m53, m63);
|
||||||
|
coords_impl!(M6x4; m11, m21, m31, m41, m51, m61,
|
||||||
|
m12, m22, m32, m42, m52, m62,
|
||||||
|
m13, m23, m33, m43, m53, m63,
|
||||||
|
m14, m24, m34, m44, m54, m64);
|
||||||
|
coords_impl!(M6x5; m11, m21, m31, m41, m51, m61,
|
||||||
|
m12, m22, m32, m42, m52, m62,
|
||||||
|
m13, m23, m33, m43, m53, m63,
|
||||||
|
m14, m24, m34, m44, m54, m64,
|
||||||
|
m15, m25, m35, m45, m55, m65);
|
||||||
|
coords_impl!(M6x6; m11, m21, m31, m41, m51, m61,
|
||||||
|
m12, m22, m32, m42, m52, m62,
|
||||||
|
m13, m23, m33, m43, m53, m63,
|
||||||
|
m14, m24, m34, m44, m54, m64,
|
||||||
|
m15, m25, m35, m45, m55, m65,
|
||||||
|
m16, m26, m36, m46, m56, m66);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Attach coordinates to matrices.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
deref_impl!(U1, U1; X);
|
||||||
|
deref_impl!(U2, U1; XY);
|
||||||
|
deref_impl!(U3, U1; XYZ);
|
||||||
|
deref_impl!(U4, U1; XYZW);
|
||||||
|
deref_impl!(U5, U1; XYZWA);
|
||||||
|
deref_impl!(U6, U1; XYZWAB);
|
||||||
|
|
||||||
|
deref_impl!(U1, U2; XY);
|
||||||
|
deref_impl!(U1, U3; XYZ);
|
||||||
|
deref_impl!(U1, U4; XYZW);
|
||||||
|
deref_impl!(U1, U5; XYZWA);
|
||||||
|
deref_impl!(U1, U6; XYZWAB);
|
||||||
|
|
||||||
|
deref_impl!(U2, U2; M2x2);
|
||||||
|
deref_impl!(U2, U3; M2x3);
|
||||||
|
deref_impl!(U2, U4; M2x4);
|
||||||
|
deref_impl!(U2, U5; M2x5);
|
||||||
|
deref_impl!(U2, U6; M2x6);
|
||||||
|
|
||||||
|
deref_impl!(U3, U2; M3x2);
|
||||||
|
deref_impl!(U3, U3; M3x3);
|
||||||
|
deref_impl!(U3, U4; M3x4);
|
||||||
|
deref_impl!(U3, U5; M3x5);
|
||||||
|
deref_impl!(U3, U6; M3x6);
|
||||||
|
|
||||||
|
deref_impl!(U4, U2; M4x2);
|
||||||
|
deref_impl!(U4, U3; M4x3);
|
||||||
|
deref_impl!(U4, U4; M4x4);
|
||||||
|
deref_impl!(U4, U5; M4x5);
|
||||||
|
deref_impl!(U4, U6; M4x6);
|
||||||
|
|
||||||
|
deref_impl!(U5, U2; M5x2);
|
||||||
|
deref_impl!(U5, U3; M5x3);
|
||||||
|
deref_impl!(U5, U4; M5x4);
|
||||||
|
deref_impl!(U5, U5; M5x5);
|
||||||
|
deref_impl!(U5, U6; M5x6);
|
||||||
|
|
||||||
|
deref_impl!(U6, U2; M6x2);
|
||||||
|
deref_impl!(U6, U3; M6x3);
|
||||||
|
deref_impl!(U6, U4; M6x4);
|
||||||
|
deref_impl!(U6, U5; M6x5);
|
||||||
|
deref_impl!(U6, U6; M6x6);
|
373
src/core/decompositions.rs
Normal file
373
src/core/decompositions.rs
Normal file
@ -0,0 +1,373 @@
|
|||||||
|
use std::cmp;
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use core::{SquareMatrix, OwnedSquareMatrix, ColumnVector, OwnedColumnVector};
|
||||||
|
use dimension::{Dim, Dynamic, U1};
|
||||||
|
use storage::{Storage, OwnedStorage};
|
||||||
|
use allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: Dim, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
/// Get the householder matrix corresponding to a reflexion to the hyperplane
|
||||||
|
/// defined by `vector`. It can be a reflexion contained in a subspace.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * `dimension` - the dimension of the space the resulting matrix operates in
|
||||||
|
/// * `start` - the starting dimension of the subspace of the reflexion
|
||||||
|
/// * `vector` - the vector defining the reflection.
|
||||||
|
pub fn new_householder_generic<SB, D2>(dimension: D, start: usize, vector: &ColumnVector<N, D2, SB>)
|
||||||
|
-> OwnedSquareMatrix<N, D, S::Alloc>
|
||||||
|
where D2: Dim,
|
||||||
|
SB: Storage<N, D2, U1> {
|
||||||
|
let mut qk = Self::identity_generic(dimension, dimension);
|
||||||
|
let subdim = vector.shape().0;
|
||||||
|
|
||||||
|
let stop = subdim + start;
|
||||||
|
|
||||||
|
assert!(dimension.value() >= stop, "Householder matrix creation: subspace dimension index out of bounds.");
|
||||||
|
|
||||||
|
for j in start .. stop {
|
||||||
|
for i in start .. stop {
|
||||||
|
unsafe {
|
||||||
|
let vv = *vector.get_unchecked(i - start, 0) * *vector.get_unchecked(j - start, 0);
|
||||||
|
let qkij = *qk.get_unchecked(i, j);
|
||||||
|
*qk.get_unchecked_mut(i, j) = qkij - vv - vv;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
qk
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N: Real, D: Dim, S: Storage<N, D, D>> SquareMatrix<N, D, S> {
|
||||||
|
/// QR decomposition using Householder reflections.
|
||||||
|
pub fn qr(self) -> (OwnedSquareMatrix<N, D, S::Alloc>, OwnedSquareMatrix<N, D, S::Alloc>)
|
||||||
|
where S::Alloc: Allocator<N, Dynamic, U1> +
|
||||||
|
Allocator<N, D, U1> {
|
||||||
|
|
||||||
|
let (nrows, ncols) = self.data.shape();
|
||||||
|
|
||||||
|
// XXX: too restrictive.
|
||||||
|
assert!(nrows.value() >= ncols.value(), "");
|
||||||
|
|
||||||
|
let mut q = OwnedSquareMatrix::<N, D, S::Alloc>::identity_generic(nrows, ncols);
|
||||||
|
let mut r = self.into_owned();
|
||||||
|
|
||||||
|
// Temporary buffer that contains a column.
|
||||||
|
let mut col = unsafe {
|
||||||
|
OwnedColumnVector::<N, D, S::Alloc>::new_uninitialized_generic(nrows, U1)
|
||||||
|
};
|
||||||
|
|
||||||
|
for ite in 0 .. cmp::min(nrows.value() - 1, ncols.value()) {
|
||||||
|
let subdim = Dynamic::new(nrows.value() - ite);
|
||||||
|
let mut v = col.rows_mut(0, subdim.value());
|
||||||
|
v.copy_from(&r.generic_slice((ite, ite), (subdim, U1)));
|
||||||
|
|
||||||
|
let alpha =
|
||||||
|
if unsafe { *v.get_unchecked(ite, 0) } >= ::zero() {
|
||||||
|
-v.norm()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
v.norm()
|
||||||
|
};
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let x = *v.get_unchecked(0, 0);
|
||||||
|
*v.get_unchecked_mut(0, 0) = x - alpha;
|
||||||
|
}
|
||||||
|
|
||||||
|
if !v.normalize_mut().is_zero() {
|
||||||
|
let mut qk = OwnedSquareMatrix::<N, D, S::Alloc>::new_householder_generic(nrows, ite, &v);
|
||||||
|
r = &qk * r;
|
||||||
|
|
||||||
|
// FIXME: add a method `q.mul_tr(qk) := q * qk.transpose` ?
|
||||||
|
qk.transpose_mut();
|
||||||
|
q = q * qk;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
(q, r)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Eigendecomposition of a square symmetric matrix.
|
||||||
|
pub fn eig(&self, eps: N, niter: usize)
|
||||||
|
-> (OwnedSquareMatrix<N, D, S::Alloc>, OwnedColumnVector<N, D, S::Alloc>)
|
||||||
|
where S::Alloc: Allocator<N, D, U1> +
|
||||||
|
Allocator<N, Dynamic, U1> {
|
||||||
|
|
||||||
|
assert!(self.is_square(),
|
||||||
|
"Unable to compute the eigenvectors and eigenvalues of a non-square matrix.");
|
||||||
|
|
||||||
|
let dim = self.data.shape().0;
|
||||||
|
|
||||||
|
let (mut eigenvectors, mut eigenvalues) = self.hessenberg();
|
||||||
|
|
||||||
|
if dim.value() == 1 {
|
||||||
|
return (eigenvectors, eigenvalues.diagonal());
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate arrays for Givens rotation components
|
||||||
|
let mut c = unsafe { OwnedColumnVector::<N, D, S::Alloc>::new_uninitialized_generic(dim, U1) };
|
||||||
|
let mut s = unsafe { OwnedColumnVector::<N, D, S::Alloc>::new_uninitialized_generic(dim, U1) };
|
||||||
|
|
||||||
|
let mut iter = 0;
|
||||||
|
let mut curdim = dim.value() - 1;
|
||||||
|
|
||||||
|
for _ in 0 .. dim.value() {
|
||||||
|
|
||||||
|
let mut stop = false;
|
||||||
|
|
||||||
|
while !stop && iter < niter {
|
||||||
|
|
||||||
|
let lambda;
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let a = *eigenvalues.get_unchecked(curdim - 1, curdim - 1);
|
||||||
|
let b = *eigenvalues.get_unchecked(curdim - 1, curdim);
|
||||||
|
let c = *eigenvalues.get_unchecked(curdim, curdim - 1);
|
||||||
|
let d = *eigenvalues.get_unchecked(curdim, curdim);
|
||||||
|
|
||||||
|
let trace = a + d;
|
||||||
|
let determinant = a * d - b * c;
|
||||||
|
|
||||||
|
let constquarter: N = ::convert(0.25f64);
|
||||||
|
let consthalf: N = ::convert(0.5f64);
|
||||||
|
|
||||||
|
let e = (constquarter * trace * trace - determinant).sqrt();
|
||||||
|
|
||||||
|
let lambda1 = consthalf * trace + e;
|
||||||
|
let lambda2 = consthalf * trace - e;
|
||||||
|
|
||||||
|
if (lambda1 - d).abs() < (lambda2 - d).abs() {
|
||||||
|
lambda = lambda1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
lambda = lambda2;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Shift matrix
|
||||||
|
for k in 0 .. curdim + 1 {
|
||||||
|
unsafe {
|
||||||
|
let a = *eigenvalues.get_unchecked(k, k);
|
||||||
|
*eigenvalues.get_unchecked_mut(k, k) = a - lambda;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Givens rotation from left
|
||||||
|
for k in 0 .. curdim {
|
||||||
|
let x_i = unsafe { *eigenvalues.get_unchecked(k, k) };
|
||||||
|
let x_j = unsafe { *eigenvalues.get_unchecked(k + 1, k) };
|
||||||
|
|
||||||
|
let ctmp;
|
||||||
|
let stmp;
|
||||||
|
|
||||||
|
if x_j.abs() < eps {
|
||||||
|
ctmp = N::one();
|
||||||
|
stmp = N::zero();
|
||||||
|
}
|
||||||
|
else if x_i.abs() < eps {
|
||||||
|
ctmp = N::zero();
|
||||||
|
stmp = -N::one();
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
let r = x_i.hypot(x_j);
|
||||||
|
ctmp = x_i / r;
|
||||||
|
stmp = -x_j / r;
|
||||||
|
}
|
||||||
|
|
||||||
|
c[k] = ctmp;
|
||||||
|
s[k] = stmp;
|
||||||
|
|
||||||
|
for j in k .. (curdim + 1) {
|
||||||
|
unsafe {
|
||||||
|
let a = *eigenvalues.get_unchecked(k, j);
|
||||||
|
let b = *eigenvalues.get_unchecked(k + 1, j);
|
||||||
|
|
||||||
|
*eigenvalues.get_unchecked_mut(k, j) = ctmp * a - stmp * b;
|
||||||
|
*eigenvalues.get_unchecked_mut(k + 1, j) = stmp * a + ctmp * b;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Givens rotation from right applied to eigenvalues
|
||||||
|
for k in 0 .. curdim {
|
||||||
|
for i in 0 .. (k + 2) {
|
||||||
|
unsafe {
|
||||||
|
let a = *eigenvalues.get_unchecked(i, k);
|
||||||
|
let b = *eigenvalues.get_unchecked(i, k + 1);
|
||||||
|
|
||||||
|
*eigenvalues.get_unchecked_mut(i, k) = c[k] * a - s[k] * b;
|
||||||
|
*eigenvalues.get_unchecked_mut(i, k + 1) = s[k] * a + c[k] * b;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Shift back
|
||||||
|
for k in 0 .. curdim + 1 {
|
||||||
|
unsafe {
|
||||||
|
let a = *eigenvalues.get_unchecked(k, k);
|
||||||
|
*eigenvalues.get_unchecked_mut(k, k) = a + lambda;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Givens rotation from right applied to eigenvectors
|
||||||
|
for k in 0 .. curdim {
|
||||||
|
for i in 0 .. dim.value() {
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let a = *eigenvectors.get_unchecked(i, k);
|
||||||
|
let b = *eigenvectors.get_unchecked(i, k + 1);
|
||||||
|
|
||||||
|
*eigenvectors.get_unchecked_mut(i, k) = c[k] * a - s[k] * b;
|
||||||
|
*eigenvectors.get_unchecked_mut(i, k + 1) = s[k] * a + c[k] * b;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
iter = iter + 1;
|
||||||
|
stop = true;
|
||||||
|
|
||||||
|
for j in 0 .. curdim {
|
||||||
|
// Check last row.
|
||||||
|
if unsafe { *eigenvalues.get_unchecked(curdim, j) }.abs() >= eps {
|
||||||
|
stop = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check last column.
|
||||||
|
if unsafe { *eigenvalues.get_unchecked(j, curdim) }.abs() >= eps {
|
||||||
|
stop = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if stop {
|
||||||
|
if curdim > 1 {
|
||||||
|
curdim = curdim - 1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
(eigenvectors, eigenvalues.diagonal())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Cholesky decomposition G of a square symmetric positive definite matrix A, such that A = G * G^T
|
||||||
|
///
|
||||||
|
/// Matrix symmetricness is not checked. Returns `None` if `self` is not definite positive.
|
||||||
|
#[inline]
|
||||||
|
pub fn cholesky(&self) -> Option<OwnedSquareMatrix<N, D, S::Alloc>> {
|
||||||
|
let out = self.transpose();
|
||||||
|
self.do_cholesky(out).ok()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Cholesky decomposition G of a square symmetric positive definite matrix A, such that A = G * G^T
|
||||||
|
#[inline]
|
||||||
|
pub fn cholesky_unchecked(&self) -> Result<OwnedSquareMatrix<N, D, S::Alloc>, &'static str> {
|
||||||
|
let out = self.transpose();
|
||||||
|
|
||||||
|
if !out.relative_eq(self, N::default_epsilon(), N::default_max_relative()) {
|
||||||
|
return Err("Cholesky: Input matrix is not symmetric");
|
||||||
|
}
|
||||||
|
|
||||||
|
self.do_cholesky(out)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline(always)]
|
||||||
|
fn do_cholesky(&self, mut out: OwnedSquareMatrix<N, D, S::Alloc>)
|
||||||
|
-> Result<OwnedSquareMatrix<N, D, S::Alloc>, &'static str> {
|
||||||
|
assert!(self.is_square(), "The input matrix must be square.");
|
||||||
|
|
||||||
|
for i in 0 .. out.nrows() {
|
||||||
|
for j in 0 .. (i + 1) {
|
||||||
|
|
||||||
|
let mut sum = out[(i, j)];
|
||||||
|
|
||||||
|
for k in 0 .. j {
|
||||||
|
sum = sum - out[(i, k)] * out[(j, k)];
|
||||||
|
}
|
||||||
|
|
||||||
|
if i > j {
|
||||||
|
out[(i, j)] = sum / out[(j, j)];
|
||||||
|
}
|
||||||
|
else if sum > N::zero() {
|
||||||
|
out[(i, i)] = sum.sqrt();
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return Err("Cholesky: Input matrix is not positive definite to machine precision.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for i in 0 .. out.nrows() {
|
||||||
|
for j in i + 1 .. out.ncols() {
|
||||||
|
out[(i, j)] = N::zero();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Ok(out)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Hessenberg
|
||||||
|
/// Returns the matrix `self` in Hessenberg form and the corresponding similarity transformation
|
||||||
|
///
|
||||||
|
/// # Returns
|
||||||
|
/// The tuple (`q`, `h`) that `q * h * q^T = self`
|
||||||
|
pub fn hessenberg(&self) -> (OwnedSquareMatrix<N, D, S::Alloc>, OwnedSquareMatrix<N, D, S::Alloc>)
|
||||||
|
where S::Alloc: Allocator<N, D, U1> + Allocator<N, Dynamic, U1> {
|
||||||
|
|
||||||
|
let (nrows, ncols) = self.data.shape();
|
||||||
|
let mut h = self.clone_owned();
|
||||||
|
|
||||||
|
let mut q = OwnedSquareMatrix::<N, D, S::Alloc>::identity_generic(nrows, ncols);
|
||||||
|
|
||||||
|
if ncols.value() <= 2 {
|
||||||
|
return (q, h);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Temporary buffer that contains a column.
|
||||||
|
let mut col = unsafe {
|
||||||
|
OwnedColumnVector::<N, D, S::Alloc>::new_uninitialized_generic(nrows, U1)
|
||||||
|
};
|
||||||
|
|
||||||
|
for ite in 0 .. (ncols.value() - 2) {
|
||||||
|
let subdim = Dynamic::new(nrows.value() - (ite + 1));
|
||||||
|
let mut v = col.rows_mut(0, subdim.value());
|
||||||
|
v.copy_from(&h.generic_slice((ite + 1, ite), (subdim, U1)));
|
||||||
|
|
||||||
|
let alpha = v.norm();
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let x = *v.get_unchecked(0, 0);
|
||||||
|
*v.get_unchecked_mut(0, 0) = x - alpha;
|
||||||
|
}
|
||||||
|
|
||||||
|
if !v.normalize_mut().is_zero() {
|
||||||
|
// XXX: we output the householder matrix to a pre-allocated matrix instead of
|
||||||
|
// return a value to `p`. This would avoid allocation at each iteration.
|
||||||
|
let p = OwnedSquareMatrix::<N, D, S::Alloc>::new_householder_generic(nrows, ite + 1, &v);
|
||||||
|
|
||||||
|
q = q * &p;
|
||||||
|
h = &p * h * p;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
(q, h)
|
||||||
|
}
|
||||||
|
}
|
104
src/core/default_allocator.rs
Normal file
104
src/core/default_allocator.rs
Normal file
@ -0,0 +1,104 @@
|
|||||||
|
use std::mem;
|
||||||
|
use std::ops::Mul;
|
||||||
|
|
||||||
|
use typenum::Prod;
|
||||||
|
use generic_array::ArrayLength;
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::dimension::{Dim, DimName, Dynamic};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
use core::matrix_array::MatrixArray;
|
||||||
|
use core::matrix_vec::MatrixVec;
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Allocator.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// An allocator based on `GenericArray` and `MatrixVec` for statically-sized and dynamically-sized
|
||||||
|
/// matrices respectively.
|
||||||
|
pub struct DefaultAllocator;
|
||||||
|
|
||||||
|
// Static - Static
|
||||||
|
impl<N, R, C> Allocator<N, R, C> for DefaultAllocator
|
||||||
|
where N: Scalar,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
type Buffer = MatrixArray<N, R, C>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn allocate_uninitialized(_: R, _: C) -> Self::Buffer {
|
||||||
|
mem::uninitialized()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn allocate_from_iterator<I: IntoIterator<Item = N>>(nrows: R, ncols: C, iter: I) -> Self::Buffer {
|
||||||
|
let mut res = unsafe { Self::allocate_uninitialized(nrows, ncols) };
|
||||||
|
let mut count = 0;
|
||||||
|
|
||||||
|
for (res, e) in res.iter_mut().zip(iter.into_iter()) {
|
||||||
|
*res = e;
|
||||||
|
count += 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
assert!(count == nrows.value() * ncols.value(),
|
||||||
|
"Matrix init. from iterator: iterator not long enough.");
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Dynamic - Static
|
||||||
|
// Dynamic - Dynamic
|
||||||
|
impl<N: Scalar, C: Dim> Allocator<N, Dynamic, C> for DefaultAllocator {
|
||||||
|
type Buffer = MatrixVec<N, Dynamic, C>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn allocate_uninitialized(nrows: Dynamic, ncols: C) -> Self::Buffer {
|
||||||
|
let mut res = Vec::new();
|
||||||
|
let length = nrows.value() * ncols.value();
|
||||||
|
res.reserve_exact(length);
|
||||||
|
res.set_len(length);
|
||||||
|
|
||||||
|
MatrixVec::new(nrows, ncols, res)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn allocate_from_iterator<I: IntoIterator<Item = N>>(nrows: Dynamic, ncols: C, iter: I) -> Self::Buffer {
|
||||||
|
let it = iter.into_iter();
|
||||||
|
let res: Vec<N> = it.collect();
|
||||||
|
assert!(res.len() == nrows.value() * ncols.value(),
|
||||||
|
"Allocation from iterator error: the iterator did not yield the correct number of elements.");
|
||||||
|
|
||||||
|
MatrixVec::new(nrows, ncols, res)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Static - Dynamic
|
||||||
|
impl<N: Scalar, R: DimName> Allocator<N, R, Dynamic> for DefaultAllocator {
|
||||||
|
type Buffer = MatrixVec<N, R, Dynamic>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn allocate_uninitialized(nrows: R, ncols: Dynamic) -> Self::Buffer {
|
||||||
|
let mut res = Vec::new();
|
||||||
|
let length = nrows.value() * ncols.value();
|
||||||
|
res.reserve_exact(length);
|
||||||
|
res.set_len(length);
|
||||||
|
|
||||||
|
MatrixVec::new(nrows, ncols, res)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn allocate_from_iterator<I: IntoIterator<Item = N>>(nrows: R, ncols: Dynamic, iter: I) -> Self::Buffer {
|
||||||
|
let it = iter.into_iter();
|
||||||
|
let res: Vec<N> = it.collect();
|
||||||
|
assert!(res.len() == nrows.value() * ncols.value(),
|
||||||
|
"Allocation from iterator error: the iterator did not yield the correct number of elements.");
|
||||||
|
|
||||||
|
MatrixVec::new(nrows, ncols, res)
|
||||||
|
}
|
||||||
|
}
|
56
src/core/determinant.rs
Normal file
56
src/core/determinant.rs
Normal file
@ -0,0 +1,56 @@
|
|||||||
|
use num::One;
|
||||||
|
|
||||||
|
use alga::general::{ClosedMul, ClosedSub, ClosedAdd};
|
||||||
|
|
||||||
|
use core::{Scalar, SquareMatrix};
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::storage::Storage;
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: Dim, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + One + ClosedMul + ClosedAdd + ClosedSub,
|
||||||
|
S: Storage<N, D, D> {
|
||||||
|
/// This matrix determinant.
|
||||||
|
#[inline]
|
||||||
|
pub fn determinant(&self) -> N {
|
||||||
|
assert!(self.is_square(), "Unable to invert a non-square matrix.");
|
||||||
|
let dim = self.shape().0;
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
match dim {
|
||||||
|
0 => N::one(),
|
||||||
|
1 => {
|
||||||
|
*self.get_unchecked(0, 0)
|
||||||
|
},
|
||||||
|
2 => {
|
||||||
|
let m11 = *self.get_unchecked(0, 0); let m12 = *self.get_unchecked(0, 1);
|
||||||
|
let m21 = *self.get_unchecked(1, 0); let m22 = *self.get_unchecked(1, 1);
|
||||||
|
|
||||||
|
m11 * m22 - m21 * m12
|
||||||
|
},
|
||||||
|
3 => {
|
||||||
|
let m11 = *self.get_unchecked(0, 0);
|
||||||
|
let m12 = *self.get_unchecked(0, 1);
|
||||||
|
let m13 = *self.get_unchecked(0, 2);
|
||||||
|
|
||||||
|
let m21 = *self.get_unchecked(1, 0);
|
||||||
|
let m22 = *self.get_unchecked(1, 1);
|
||||||
|
let m23 = *self.get_unchecked(1, 2);
|
||||||
|
|
||||||
|
let m31 = *self.get_unchecked(2, 0);
|
||||||
|
let m32 = *self.get_unchecked(2, 1);
|
||||||
|
let m33 = *self.get_unchecked(2, 2);
|
||||||
|
|
||||||
|
let minor_m12_m23 = m22 * m33 - m32 * m23;
|
||||||
|
let minor_m11_m23 = m21 * m33 - m31 * m23;
|
||||||
|
let minor_m11_m22 = m21 * m32 - m31 * m22;
|
||||||
|
|
||||||
|
m11 * minor_m12_m23 - m12 * minor_m11_m23 + m13 * minor_m11_m22
|
||||||
|
},
|
||||||
|
_ => {
|
||||||
|
unimplemented!()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
342
src/core/dimension.rs
Normal file
342
src/core/dimension.rs
Normal file
@ -0,0 +1,342 @@
|
|||||||
|
use std::fmt::Debug;
|
||||||
|
use std::any::Any;
|
||||||
|
use std::ops::{Add, Sub, Mul, Div};
|
||||||
|
use typenum::{self, Unsigned, UInt, B1, Bit, UTerm, Sum, Prod, Diff, Quot};
|
||||||
|
|
||||||
|
/// Dim of dynamically-sized algebraic entities.
|
||||||
|
#[derive(Clone, Copy, Eq, PartialEq, Debug)]
|
||||||
|
pub struct Dynamic {
|
||||||
|
value: usize
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Dynamic {
|
||||||
|
/// A dynamic size equal to `value`.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(value: usize) -> Dynamic {
|
||||||
|
Dynamic {
|
||||||
|
value: value
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Trait implemented by dynamic dimensions.
|
||||||
|
pub trait IsDynamic { }
|
||||||
|
/// Trait implemented by dimensions that are not equal to U1.
|
||||||
|
pub trait IsNotStaticOne { }
|
||||||
|
|
||||||
|
impl IsDynamic for Dynamic { }
|
||||||
|
impl IsNotStaticOne for Dynamic { }
|
||||||
|
|
||||||
|
pub trait Dim: Any + Debug + Copy + PartialEq + Send {
|
||||||
|
fn try_to_usize() -> Option<usize>;
|
||||||
|
fn value(&self) -> usize;
|
||||||
|
fn from_usize(dim: usize) -> Self;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Dim for Dynamic {
|
||||||
|
#[inline]
|
||||||
|
fn try_to_usize() -> Option<usize> {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_usize(dim: usize) -> Self {
|
||||||
|
Dynamic::new(dim)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn value(&self) -> usize {
|
||||||
|
self.value
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Operations.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
macro_rules! dim_ops(
|
||||||
|
($($DimOp: ident, $DimNameOp: ident,
|
||||||
|
$Op: ident, $op: ident,
|
||||||
|
$DimResOp: ident, $DimNameResOp: ident,
|
||||||
|
$ResOp: ident);* $(;)*) => {$(
|
||||||
|
pub type $DimResOp<D1, D2> = <D1 as $DimOp<D2>>::Output;
|
||||||
|
|
||||||
|
pub trait $DimOp<D: Dim>: Dim {
|
||||||
|
type Output: Dim;
|
||||||
|
|
||||||
|
fn $op(self, other: D) -> Self::Output;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D1: DimName, D2: DimName> $DimOp<D2> for D1
|
||||||
|
where D1::Value: $Op<D2::Value>,
|
||||||
|
$ResOp<D1::Value, D2::Value>: NamedDim {
|
||||||
|
type Output = <$ResOp<D1::Value, D2::Value> as NamedDim>::Name;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op(self, _: D2) -> Self::Output {
|
||||||
|
Self::Output::name()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: Dim> $DimOp<D> for Dynamic {
|
||||||
|
type Output = Dynamic;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op(self, other: D) -> Dynamic {
|
||||||
|
Dynamic::new(self.value.$op(other.value()))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: DimName> $DimOp<Dynamic> for D {
|
||||||
|
type Output = Dynamic;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op(self, other: Dynamic) -> Dynamic {
|
||||||
|
Dynamic::new(self.value().$op(other.value))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub type $DimNameResOp<D1, D2> = <D1 as $DimNameOp<D2>>::Output;
|
||||||
|
|
||||||
|
pub trait $DimNameOp<D: DimName>: DimName {
|
||||||
|
type Output: DimName;
|
||||||
|
|
||||||
|
fn $op(self, other: D) -> Self::Output;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D1: DimName, D2: DimName> $DimNameOp<D2> for D1
|
||||||
|
where D1::Value: $Op<D2::Value>,
|
||||||
|
$ResOp<D1::Value, D2::Value>: NamedDim {
|
||||||
|
type Output = <$ResOp<D1::Value, D2::Value> as NamedDim>::Name;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op(self, _: D2) -> Self::Output {
|
||||||
|
Self::Output::name()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
dim_ops!(
|
||||||
|
DimAdd, DimNameAdd, Add, add, DimSum, DimNameSum, Sum;
|
||||||
|
DimMul, DimNameMul, Mul, mul, DimProd, DimNameProd, Prod;
|
||||||
|
DimSub, DimNameSub, Sub, sub, DimDiff, DimNameDiff, Diff;
|
||||||
|
DimDiv, DimNameDiv, Div, div, DimQuot, DimNameQuot, Quot;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
pub trait DimName: Dim {
|
||||||
|
type Value: NamedDim<Name = Self>;
|
||||||
|
|
||||||
|
/// The name of this dimension, i.e., the singleton `Self`.
|
||||||
|
#[inline]
|
||||||
|
fn name() -> Self;
|
||||||
|
|
||||||
|
// FIXME: this is not a very idiomatic name.
|
||||||
|
/// The value of this dimension.
|
||||||
|
#[inline]
|
||||||
|
fn dim() -> usize {
|
||||||
|
Self::Value::to_usize()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub trait NamedDim: Sized + Any + Unsigned {
|
||||||
|
type Name: DimName<Value = Self>;
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Debug, Copy, Clone, Hash, PartialEq, Eq)]
|
||||||
|
pub struct U1;
|
||||||
|
|
||||||
|
impl Dim for U1 {
|
||||||
|
#[inline]
|
||||||
|
fn try_to_usize() -> Option<usize> {
|
||||||
|
Some(1)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_usize(dim: usize) -> Self {
|
||||||
|
assert!(dim == 1, "Mismatched dimension.");
|
||||||
|
U1
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn value(&self) -> usize {
|
||||||
|
1
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl DimName for U1 {
|
||||||
|
type Value = typenum::U1;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn name() -> Self {
|
||||||
|
U1
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl NamedDim for typenum::U1{
|
||||||
|
type Name = U1;
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! named_dimension(
|
||||||
|
($($D: ident),* $(,)*) => {$(
|
||||||
|
#[derive(Debug, Copy, Clone, Hash, PartialEq, Eq)]
|
||||||
|
pub struct $D;
|
||||||
|
|
||||||
|
impl Dim for $D {
|
||||||
|
#[inline]
|
||||||
|
fn try_to_usize() -> Option<usize> {
|
||||||
|
Some(typenum::$D::to_usize())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_usize(dim: usize) -> Self {
|
||||||
|
assert!(dim == typenum::$D::to_usize(), "Mismatched dimension.");
|
||||||
|
$D
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn value(&self) -> usize {
|
||||||
|
typenum::$D::to_usize()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl DimName for $D {
|
||||||
|
type Value = typenum::$D;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn name() -> Self {
|
||||||
|
$D
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl NamedDim for typenum::$D {
|
||||||
|
type Name = $D;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl IsNotStaticOne for $D { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// We give explicit names to all Unsigned in [0, 128[
|
||||||
|
named_dimension!(
|
||||||
|
U0, /*U1,*/ U2, U3, U4, U5, U6, U7, U8, U9,
|
||||||
|
U10, U11, U12, U13, U14, U15, U16, U17, U18, U19,
|
||||||
|
U20, U21, U22, U23, U24, U25, U26, U27, U28, U29,
|
||||||
|
U30, U31, U32, U33, U34, U35, U36, U37, U38, U39,
|
||||||
|
U40, U41, U42, U43, U44, U45, U46, U47, U48, U49,
|
||||||
|
U50, U51, U52, U53, U54, U55, U56, U57, U58, U59,
|
||||||
|
U60, U61, U62, U63, U64, U65, U66, U67, U68, U69,
|
||||||
|
U70, U71, U72, U73, U74, U75, U76, U77, U78, U79,
|
||||||
|
U80, U81, U82, U83, U84, U85, U86, U87, U88, U89,
|
||||||
|
U90, U91, U92, U93, U94, U95, U96, U97, U98, U99,
|
||||||
|
U100, U101, U102, U103, U104, U105, U106, U107, U108, U109,
|
||||||
|
U110, U111, U112, U113, U114, U115, U116, U117, U118, U119,
|
||||||
|
U120, U121, U122, U123, U124, U125, U126, U127
|
||||||
|
);
|
||||||
|
|
||||||
|
// For values greater than U1023, just use the typenum binary representation directly.
|
||||||
|
impl<A: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
B: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
C: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
D: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
E: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
F: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
G: Bit + Any + Debug + Copy + PartialEq + Send>
|
||||||
|
NamedDim
|
||||||
|
for UInt<UInt<UInt<UInt<UInt<UInt<UInt<UInt<UTerm, B1>, A>, B>, C>, D>, E>, F>, G> {
|
||||||
|
type Name = Self;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
B: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
C: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
D: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
E: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
F: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
G: Bit + Any + Debug + Copy + PartialEq + Send>
|
||||||
|
Dim
|
||||||
|
for UInt<UInt<UInt<UInt<UInt<UInt<UInt<UInt<UTerm, B1>, A>, B>, C>, D>, E>, F>, G> {
|
||||||
|
#[inline]
|
||||||
|
fn try_to_usize() -> Option<usize> {
|
||||||
|
Some(Self::to_usize())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_usize(dim: usize) -> Self {
|
||||||
|
assert!(dim == Self::to_usize(), "Mismatched dimension.");
|
||||||
|
Self::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn value(&self) -> usize {
|
||||||
|
Self::to_usize()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
B: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
C: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
D: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
E: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
F: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
G: Bit + Any + Debug + Copy + PartialEq + Send>
|
||||||
|
DimName
|
||||||
|
for UInt<UInt<UInt<UInt<UInt<UInt<UInt<UInt<UTerm, B1>, A>, B>, C>, D>, E>, F>, G> {
|
||||||
|
type Value = Self;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn name() -> Self {
|
||||||
|
Self::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
B: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
C: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
D: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
E: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
F: Bit + Any + Debug + Copy + PartialEq + Send,
|
||||||
|
G: Bit + Any + Debug + Copy + PartialEq + Send>
|
||||||
|
IsNotStaticOne
|
||||||
|
for UInt<UInt<UInt<UInt<UInt<UInt<UInt<UInt<UTerm, B1>, A>, B>, C>, D>, E>, F>, G> {
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
impl<U: Unsigned + DimName, B: Bit + Any + Debug + Copy + PartialEq + Send> NamedDim for UInt<U, B> {
|
||||||
|
type Name = UInt<U, B>;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<U: Unsigned + DimName, B: Bit + Any + Debug + Copy + PartialEq + Send> Dim for UInt<U, B> {
|
||||||
|
#[inline]
|
||||||
|
fn try_to_usize() -> Option<usize> {
|
||||||
|
Some(Self::to_usize())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_usize(dim: usize) -> Self {
|
||||||
|
assert!(dim == Self::to_usize(), "Mismatched dimension.");
|
||||||
|
Self::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn value(&self) -> usize {
|
||||||
|
Self::to_usize()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<U: Unsigned + DimName, B: Bit + Any + Debug + Copy + PartialEq + Send> DimName for UInt<U, B> {
|
||||||
|
type Value = UInt<U, B>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn name() -> Self {
|
||||||
|
Self::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<U: Unsigned + DimName, B: Bit + Any + Debug + Copy + PartialEq + Send> IsNotStaticOne for UInt<U, B> {
|
||||||
|
}
|
19
src/core/helper.rs
Normal file
19
src/core/helper.rs
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
|
||||||
|
/// Simple helper function for rejection sampling
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
#[doc(hidden)]
|
||||||
|
#[inline]
|
||||||
|
pub fn reject<G: Gen, F: FnMut(&T) -> bool, T: Arbitrary>(g: &mut G, f: F) -> T {
|
||||||
|
use std::iter;
|
||||||
|
iter::repeat(()).map(|_| Arbitrary::arbitrary(g)).find(f).unwrap()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[doc(hidden)]
|
||||||
|
#[inline]
|
||||||
|
pub fn reject_rand<G: Rng, F: FnMut(&T) -> bool, T: Rand>(g: &mut G, f: F) -> T {
|
||||||
|
use std::iter;
|
||||||
|
iter::repeat(()).map(|_| Rand::rand(g)).find(f).unwrap()
|
||||||
|
}
|
202
src/core/inverse.rs
Normal file
202
src/core/inverse.rs
Normal file
@ -0,0 +1,202 @@
|
|||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::Field;
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, SquareMatrix, OwnedSquareMatrix};
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: Dim, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
S: Storage<N, D, D> {
|
||||||
|
/// Attempts to invert this matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_inverse(self) -> Option<OwnedSquareMatrix<N, D, S::Alloc>> {
|
||||||
|
let mut res = self.into_owned();
|
||||||
|
|
||||||
|
if res.shape().0 <= 3 {
|
||||||
|
if res.try_inverse_mut() {
|
||||||
|
Some(res)
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
gauss_jordan_inverse(res)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: Dim, S> SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
S: StorageMut<N, D, D> {
|
||||||
|
/// Attempts to invert this matrix in-place. Returns `false` and leaves `self` untouched if
|
||||||
|
/// inversion fails.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
assert!(self.is_square(), "Unable to invert a non-square matrix.");
|
||||||
|
|
||||||
|
let dim = self.shape().0;
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
match dim {
|
||||||
|
0 => true,
|
||||||
|
1 => {
|
||||||
|
if relative_eq!(self.get_unchecked(0, 0), &N::zero()) {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
*self.get_unchecked_mut(0, 0) = N::one() / self.determinant();
|
||||||
|
true
|
||||||
|
}
|
||||||
|
},
|
||||||
|
2 => {
|
||||||
|
let determinant = self.determinant();
|
||||||
|
|
||||||
|
if relative_eq!(&determinant, &N::zero()) {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
let m11 = *self.get_unchecked(0, 0); let m12 = *self.get_unchecked(0, 1);
|
||||||
|
let m21 = *self.get_unchecked(1, 0); let m22 = *self.get_unchecked(1, 1);
|
||||||
|
|
||||||
|
*self.get_unchecked_mut(0, 0) = m22 / determinant;
|
||||||
|
*self.get_unchecked_mut(0, 1) = -m12 / determinant;
|
||||||
|
|
||||||
|
*self.get_unchecked_mut(1, 0) = -m21 / determinant;
|
||||||
|
*self.get_unchecked_mut(1, 1) = m11 / determinant;
|
||||||
|
|
||||||
|
true
|
||||||
|
}
|
||||||
|
},
|
||||||
|
3 => {
|
||||||
|
let m11 = *self.get_unchecked(0, 0);
|
||||||
|
let m12 = *self.get_unchecked(0, 1);
|
||||||
|
let m13 = *self.get_unchecked(0, 2);
|
||||||
|
|
||||||
|
let m21 = *self.get_unchecked(1, 0);
|
||||||
|
let m22 = *self.get_unchecked(1, 1);
|
||||||
|
let m23 = *self.get_unchecked(1, 2);
|
||||||
|
|
||||||
|
let m31 = *self.get_unchecked(2, 0);
|
||||||
|
let m32 = *self.get_unchecked(2, 1);
|
||||||
|
let m33 = *self.get_unchecked(2, 2);
|
||||||
|
|
||||||
|
|
||||||
|
let minor_m12_m23 = m22 * m33 - m32 * m23;
|
||||||
|
let minor_m11_m23 = m21 * m33 - m31 * m23;
|
||||||
|
let minor_m11_m22 = m21 * m32 - m31 * m22;
|
||||||
|
|
||||||
|
let determinant = m11 * minor_m12_m23 -
|
||||||
|
m12 * minor_m11_m23 +
|
||||||
|
m13 * minor_m11_m22;
|
||||||
|
|
||||||
|
if relative_eq!(&determinant, &N::zero()) {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
*self.get_unchecked_mut(0, 0) = minor_m12_m23 / determinant;
|
||||||
|
*self.get_unchecked_mut(0, 1) = (m13 * m32 - m33 * m12) / determinant;
|
||||||
|
*self.get_unchecked_mut(0, 2) = (m12 * m23 - m22 * m13) / determinant;
|
||||||
|
|
||||||
|
*self.get_unchecked_mut(1, 0) = -minor_m11_m23 / determinant;
|
||||||
|
*self.get_unchecked_mut(1, 1) = (m11 * m33 - m31 * m13) / determinant;
|
||||||
|
*self.get_unchecked_mut(1, 2) = (m13 * m21 - m23 * m11) / determinant;
|
||||||
|
|
||||||
|
*self.get_unchecked_mut(2, 0) = minor_m11_m22 / determinant;
|
||||||
|
*self.get_unchecked_mut(2, 1) = (m12 * m31 - m32 * m11) / determinant;
|
||||||
|
*self.get_unchecked_mut(2, 2) = (m11 * m22 - m21 * m12) / determinant;
|
||||||
|
|
||||||
|
true
|
||||||
|
}
|
||||||
|
},
|
||||||
|
_ => {
|
||||||
|
let oself = self.clone_owned();
|
||||||
|
if let Some(res) = gauss_jordan_inverse(oself) {
|
||||||
|
self.copy_from(&res);
|
||||||
|
true
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// Inverts the given matrix using Gauss-Jordan Ellimitation.
|
||||||
|
fn gauss_jordan_inverse<N, D, S>(mut matrix: SquareMatrix<N, D, S>) -> Option<OwnedSquareMatrix<N, D, S::Alloc>>
|
||||||
|
where D: Dim,
|
||||||
|
N: Scalar + Field + ApproxEq,
|
||||||
|
S: StorageMut<N, D, D> {
|
||||||
|
|
||||||
|
assert!(matrix.is_square(), "Unable to invert a non-square matrix.");
|
||||||
|
let dim = matrix.data.shape().0;
|
||||||
|
let mut res: OwnedSquareMatrix<N, D, S::Alloc> = Matrix::identity_generic(dim, dim);
|
||||||
|
let dim = dim.value();
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
for k in 0 .. dim {
|
||||||
|
// Search a non-zero value on the k-th column.
|
||||||
|
// FIXME: would it be worth it to spend some more time searching for the
|
||||||
|
// max instead?
|
||||||
|
|
||||||
|
let mut n0 = k; // index of a non-zero entry.
|
||||||
|
|
||||||
|
while n0 != dim {
|
||||||
|
if !matrix.get_unchecked(n0, k).is_zero() {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
n0 += 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if n0 == dim {
|
||||||
|
return None
|
||||||
|
}
|
||||||
|
|
||||||
|
// Swap pivot line.
|
||||||
|
if n0 != k {
|
||||||
|
for j in 0 .. dim {
|
||||||
|
matrix.swap_unchecked((n0, j), (k, j));
|
||||||
|
res.swap_unchecked((n0, j), (k, j));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
let pivot = *matrix.get_unchecked(k, k);
|
||||||
|
|
||||||
|
for j in k .. dim {
|
||||||
|
let selfval = *matrix.get_unchecked(k, j) / pivot;
|
||||||
|
*matrix.get_unchecked_mut(k, j) = selfval;
|
||||||
|
}
|
||||||
|
|
||||||
|
for j in 0 .. dim {
|
||||||
|
let resval = *res.get_unchecked(k, j) / pivot;
|
||||||
|
*res.get_unchecked_mut(k, j) = resval;
|
||||||
|
}
|
||||||
|
|
||||||
|
for l in 0 .. dim {
|
||||||
|
if l != k {
|
||||||
|
let normalizer = *matrix.get_unchecked(l, k);
|
||||||
|
|
||||||
|
for j in k .. dim {
|
||||||
|
let selfval = *matrix.get_unchecked(l, j) - *matrix.get_unchecked(k, j) * normalizer;
|
||||||
|
*matrix.get_unchecked_mut(l, j) = selfval;
|
||||||
|
}
|
||||||
|
|
||||||
|
for j in 0 .. dim {
|
||||||
|
let resval = *res.get_unchecked(l, j) - *res.get_unchecked(k, j) * normalizer;
|
||||||
|
*res.get_unchecked_mut(l, j) = resval;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Some(res)
|
||||||
|
}
|
||||||
|
}
|
86
src/core/iter.rs
Normal file
86
src/core/iter.rs
Normal file
@ -0,0 +1,86 @@
|
|||||||
|
use std::marker::PhantomData;
|
||||||
|
use std::mem;
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
|
||||||
|
macro_rules! iterator {
|
||||||
|
(struct $Name:ident for $Storage:ident.$ptr: ident -> $Ptr:ty, $Ref:ty, $SRef: ty) => {
|
||||||
|
|
||||||
|
/// An iterator through a dense matrix with arbitrary strides matrix.
|
||||||
|
pub struct $Name<'a, N: Scalar, R: Dim, C: Dim, S: 'a + $Storage<N, R, C>> {
|
||||||
|
ptr: $Ptr,
|
||||||
|
inner_ptr: $Ptr,
|
||||||
|
inner_end: $Ptr,
|
||||||
|
size: usize, // We can't use an end pointer here because a stride might be zero.
|
||||||
|
strides: (S::RStride, S::CStride),
|
||||||
|
_phantoms: PhantomData<($Ref, R, C, S)>
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: we need to specialize for the case where the matrix storage is owned (in which
|
||||||
|
// case the iterator is trivial because it does not have any stride).
|
||||||
|
impl<'a, N: Scalar, R: Dim, C: Dim, S: 'a + $Storage<N, R, C>> $Name<'a, N, R, C, S> {
|
||||||
|
/// Creates a new iterator for the given matrix storage.
|
||||||
|
pub fn new(storage: $SRef) -> $Name<'a, N, R, C, S> {
|
||||||
|
let shape = storage.shape();
|
||||||
|
let strides = storage.strides();
|
||||||
|
let inner_offset = shape.0.value() * strides.0.value();
|
||||||
|
let ptr = storage.$ptr();
|
||||||
|
|
||||||
|
$Name {
|
||||||
|
ptr: ptr,
|
||||||
|
inner_ptr: ptr,
|
||||||
|
inner_end: unsafe { ptr.offset(inner_offset as isize) },
|
||||||
|
size: shape.0.value() * shape.1.value(),
|
||||||
|
strides: strides,
|
||||||
|
_phantoms: PhantomData
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N: Scalar, R: Dim, C: Dim, S: 'a + $Storage<N, R, C>> Iterator for $Name<'a, N, R, C, S> {
|
||||||
|
type Item = $Ref;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn next(&mut self) -> Option<$Ref> {
|
||||||
|
unsafe {
|
||||||
|
if self.size == 0 {
|
||||||
|
None
|
||||||
|
} else {
|
||||||
|
self.size -= 1;
|
||||||
|
|
||||||
|
// Jump to the next outer dimension if needed.
|
||||||
|
if self.ptr == self.inner_end {
|
||||||
|
let stride = self.strides.1.value() as isize;
|
||||||
|
self.inner_end = self.ptr.offset(stride);
|
||||||
|
self.ptr = self.inner_ptr.offset(stride);
|
||||||
|
self.inner_ptr = self.ptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Go to the next element.
|
||||||
|
let old = self.ptr;
|
||||||
|
|
||||||
|
let stride = self.strides.0.value() as isize;
|
||||||
|
self.ptr = self.ptr.offset(stride);
|
||||||
|
|
||||||
|
Some(mem::transmute(old))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn size_hint(&self) -> (usize, Option<usize>) {
|
||||||
|
(self.size, Some(self.size))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn count(self) -> usize {
|
||||||
|
self.size_hint().0
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
iterator!(struct MatrixIter for Storage.ptr -> *const N, &'a N, &'a S);
|
||||||
|
iterator!(struct MatrixIterMut for StorageMut.ptr_mut -> *mut N, &'a mut N, &'a mut S);
|
1013
src/core/matrix.rs
Normal file
1013
src/core/matrix.rs
Normal file
File diff suppressed because it is too large
Load Diff
788
src/core/matrix_alga.rs
Normal file
788
src/core/matrix_alga.rs
Normal file
@ -0,0 +1,788 @@
|
|||||||
|
use num::{Zero, One};
|
||||||
|
|
||||||
|
use alga::general::{AbstractMagma, AbstractGroupAbelian, AbstractGroup, AbstractLoop,
|
||||||
|
AbstractMonoid, AbstractQuasigroup, AbstractSemigroup, AbstractModule,
|
||||||
|
Module, Field, RingCommutative, Real, Inverse, Additive, Multiplicative,
|
||||||
|
MeetSemilattice, JoinSemilattice, Lattice, Identity,
|
||||||
|
ClosedAdd, ClosedNeg, ClosedMul};
|
||||||
|
use alga::linear::{VectorSpace, NormedSpace, InnerSpace, FiniteDimVectorSpace, FiniteDimInnerSpace};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, SquareMatrix};
|
||||||
|
use core::dimension::{Dim, DimName};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Additive structures.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, R: DimName, C: DimName, S> Identity<Additive> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Zero,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::from_element(N::zero())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> AbstractMagma<Additive> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + ClosedAdd,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, other: &Self) -> Self {
|
||||||
|
self + other
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> Inverse<Additive> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Matrix<N, R, C, S> {
|
||||||
|
-self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
*self = -self.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! inherit_additive_structure(
|
||||||
|
($($marker: ident<$operator: ident> $(+ $bounds: ident)*),* $(,)*) => {$(
|
||||||
|
impl<N, R: DimName, C: DimName, S> $marker<$operator> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + $marker<$operator> $(+ $bounds)*,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
inherit_additive_structure!(
|
||||||
|
AbstractSemigroup<Additive> + ClosedAdd,
|
||||||
|
AbstractMonoid<Additive> + Zero + ClosedAdd,
|
||||||
|
AbstractQuasigroup<Additive> + ClosedAdd + ClosedNeg,
|
||||||
|
AbstractLoop<Additive> + Zero + ClosedAdd + ClosedNeg,
|
||||||
|
AbstractGroup<Additive> + Zero + ClosedAdd + ClosedNeg,
|
||||||
|
AbstractGroupAbelian<Additive> + Zero + ClosedAdd + ClosedNeg
|
||||||
|
);
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> AbstractModule for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + RingCommutative,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
type AbstractRing = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn multiply_by(&self, n: N) -> Self {
|
||||||
|
self * n
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> Module for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + RingCommutative,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
type Ring = N;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> VectorSpace for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
type Field = N;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> FiniteDimVectorSpace for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn dimension() -> usize {
|
||||||
|
R::dim() * C::dim()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn canonical_basis_element(i: usize) -> Self {
|
||||||
|
assert!(i < Self::dimension(), "Index out of bound.");
|
||||||
|
|
||||||
|
let mut res = Self::zero();
|
||||||
|
unsafe { *res.data.get_unchecked_linear_mut(i) = N::one(); }
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn dot(&self, other: &Self) -> N {
|
||||||
|
self.dot(other)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn component_unchecked(&self, i: usize) -> &N {
|
||||||
|
self.data.get_unchecked_linear(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn component_unchecked_mut(&mut self, i: usize) -> &mut N {
|
||||||
|
self.data.get_unchecked_linear_mut(i)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> NormedSpace for Matrix<N, R, C, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn norm_squared(&self) -> N {
|
||||||
|
self.norm_squared()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn norm(&self) -> N {
|
||||||
|
self.norm()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn normalize(&self) -> Self {
|
||||||
|
self.normalize()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn normalize_mut(&mut self) -> N {
|
||||||
|
self.normalize_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn try_normalize(&self, min_norm: N) -> Option<Self> {
|
||||||
|
self.try_normalize(min_norm)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
|
||||||
|
self.try_normalize_mut(min_norm)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: DimName, C: DimName, S> InnerSpace for Matrix<N, R, C, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
type Real = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn angle(&self, other: &Self) -> N {
|
||||||
|
self.angle(other)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inner_product(&self, other: &Self) -> N {
|
||||||
|
self.dot(other)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: specialization will greatly simplify this implementation in the future.
|
||||||
|
// In particular:
|
||||||
|
// − use `x()` instead of `::canonical_basis_element`
|
||||||
|
// − use `::new(x, y, z)` instead of `::from_slice`
|
||||||
|
impl<N, R: DimName, C: DimName, S> FiniteDimInnerSpace for Matrix<N, R, C, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn orthonormalize(vs: &mut [Matrix<N, R, C, S>]) -> usize {
|
||||||
|
let mut nbasis_elements = 0;
|
||||||
|
|
||||||
|
for i in 0 .. vs.len() {
|
||||||
|
{
|
||||||
|
let (elt, basis) = vs[.. i + 1].split_last_mut().unwrap();
|
||||||
|
|
||||||
|
for basis_element in &basis[.. nbasis_elements] {
|
||||||
|
*elt -= &*basis_element * elt.dot(basis_element)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if vs[i].try_normalize_mut(N::zero()).is_some() {
|
||||||
|
// FIXME: this will be efficient on dynamically-allocated vectors but for
|
||||||
|
// statically-allocated ones, `.clone_from` would be better.
|
||||||
|
vs.swap(nbasis_elements, i);
|
||||||
|
nbasis_elements += 1;
|
||||||
|
|
||||||
|
// All the other vectors will be dependent.
|
||||||
|
if nbasis_elements == Self::dimension() {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
nbasis_elements
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn orthonormal_subspace_basis<F>(vs: &[Self], mut f: F)
|
||||||
|
where F: FnMut(&Self) -> bool {
|
||||||
|
// FIXME: is this necessary?
|
||||||
|
assert!(vs.len() <= Self::dimension(), "The given set of vectors has no chance of being a free family.");
|
||||||
|
|
||||||
|
match Self::dimension() {
|
||||||
|
1 => {
|
||||||
|
if vs.len() == 0 {
|
||||||
|
f(&Self::canonical_basis_element(0));
|
||||||
|
}
|
||||||
|
},
|
||||||
|
2 => {
|
||||||
|
if vs.len() == 0 {
|
||||||
|
let _ = f(&Self::canonical_basis_element(0)) &&
|
||||||
|
f(&Self::canonical_basis_element(1));
|
||||||
|
}
|
||||||
|
else if vs.len() == 1 {
|
||||||
|
let v = &vs[0];
|
||||||
|
let res = Self::from_column_slice(&[-v[1], v[0]]);
|
||||||
|
|
||||||
|
f(&res.normalize());
|
||||||
|
}
|
||||||
|
|
||||||
|
// Otherwise, nothing.
|
||||||
|
},
|
||||||
|
3 => {
|
||||||
|
if vs.len() == 0 {
|
||||||
|
let _ = f(&Self::canonical_basis_element(0)) &&
|
||||||
|
f(&Self::canonical_basis_element(1)) &&
|
||||||
|
f(&Self::canonical_basis_element(2));
|
||||||
|
}
|
||||||
|
else if vs.len() == 1 {
|
||||||
|
let v = &vs[0];
|
||||||
|
let mut a;
|
||||||
|
|
||||||
|
if v[0].abs() > v[1].abs() {
|
||||||
|
a = Self::from_column_slice(&[v[2], N::zero(), -v[0]]);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
a = Self::from_column_slice(&[N::zero(), -v[2], v[1]]);
|
||||||
|
};
|
||||||
|
|
||||||
|
a.normalize_mut();
|
||||||
|
|
||||||
|
if f(&a.cross(v)) {
|
||||||
|
f(&a);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if vs.len() == 2 {
|
||||||
|
f(&vs[0].cross(&vs[1]).normalize());
|
||||||
|
}
|
||||||
|
},
|
||||||
|
_ => {
|
||||||
|
// XXX: use a GenericArray instead.
|
||||||
|
let mut known_basis = Vec::new();
|
||||||
|
|
||||||
|
for v in vs.iter() {
|
||||||
|
known_basis.push(v.normalize())
|
||||||
|
}
|
||||||
|
|
||||||
|
for i in 0 .. Self::dimension() - vs.len() {
|
||||||
|
let mut elt = Self::canonical_basis_element(i);
|
||||||
|
|
||||||
|
for v in &known_basis {
|
||||||
|
elt -= v * elt.dot(v)
|
||||||
|
};
|
||||||
|
|
||||||
|
if let Some(subsp_elt) = elt.try_normalize(N::zero()) {
|
||||||
|
if !f(&subsp_elt) { return };
|
||||||
|
|
||||||
|
known_basis.push(subsp_elt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* Multiplicative structures.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Identity<Multiplicative> for SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Zero + One,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> AbstractMagma<Multiplicative> for SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, other: &Self) -> Self {
|
||||||
|
self * other
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_multiplicative_structure(
|
||||||
|
($($marker: ident<$operator: ident> $(+ $bounds: ident)*),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimName, S> $marker<$operator> for SquareMatrix<N, D, S>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul + $marker<$operator> $(+ $bounds)*,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_multiplicative_structure!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative> + One
|
||||||
|
);
|
||||||
|
|
||||||
|
// // FIXME: Field too strong?
|
||||||
|
// impl<N, S> Matrix for Matrix<N, S>
|
||||||
|
// where N: Scalar + Field,
|
||||||
|
// S: Storage<N> {
|
||||||
|
// type Field = N;
|
||||||
|
// type Row = OwnedMatrix<N, Static<U1>, S::C, S::Alloc>;
|
||||||
|
// type Column = OwnedMatrix<N, S::R, Static<U1>, S::Alloc>;
|
||||||
|
// type Transpose = OwnedMatrix<N, S::C, S::R, S::Alloc>;
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn nrows(&self) -> usize {
|
||||||
|
// self.shape().0
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn ncolumns(&self) -> usize {
|
||||||
|
// self.shape().1
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn row(&self, row: usize) -> Self::Row {
|
||||||
|
// let mut res: Self::Row = ::zero();
|
||||||
|
|
||||||
|
// for (column, e) in res.iter_mut().enumerate() {
|
||||||
|
// *e = self[(row, column)];
|
||||||
|
// }
|
||||||
|
|
||||||
|
// res
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn column(&self, column: usize) -> Self::Column {
|
||||||
|
// let mut res: Self::Column = ::zero();
|
||||||
|
|
||||||
|
// for (row, e) in res.iter_mut().enumerate() {
|
||||||
|
// *e = self[(row, column)];
|
||||||
|
// }
|
||||||
|
|
||||||
|
// res
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// unsafe fn get_unchecked(&self, i: usize, j: usize) -> Self::Field {
|
||||||
|
// self.get_unchecked(i, j)
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn transpose(&self) -> Self::Transpose {
|
||||||
|
// self.transpose()
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// impl<N, S> MatrixMut for Matrix<N, S>
|
||||||
|
// where N: Scalar + Field,
|
||||||
|
// S: StorageMut<N> {
|
||||||
|
// #[inline]
|
||||||
|
// fn set_row_mut(&mut self, irow: usize, row: &Self::Row) {
|
||||||
|
// assert!(irow < self.shape().0, "Row index out of bounds.");
|
||||||
|
|
||||||
|
// for (icol, e) in row.iter().enumerate() {
|
||||||
|
// unsafe { self.set_unchecked(irow, icol, *e) }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn set_column_mut(&mut self, icol: usize, col: &Self::Column) {
|
||||||
|
// assert!(icol < self.shape().1, "Column index out of bounds.");
|
||||||
|
// for (irow, e) in col.iter().enumerate() {
|
||||||
|
// unsafe { self.set_unchecked(irow, icol, *e) }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// unsafe fn set_unchecked(&mut self, i: usize, j: usize, val: Self::Field) {
|
||||||
|
// *self.get_unchecked_mut(i, j) = val
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // FIXME: Real is needed here only for invertibility...
|
||||||
|
// impl<N: Real> SquareMatrixMut for $t<N> {
|
||||||
|
// #[inline]
|
||||||
|
// fn from_diagonal(diag: &Self::Coordinates) -> Self {
|
||||||
|
// let mut res: $t<N> = ::zero();
|
||||||
|
// res.set_diagonal_mut(diag);
|
||||||
|
// res
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn set_diagonal_mut(&mut self, diag: &Self::Coordinates) {
|
||||||
|
// for (i, e) in diag.iter().enumerate() {
|
||||||
|
// unsafe { self.set_unchecked(i, i, *e) }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Specializations depending on the dimension.
|
||||||
|
// matrix_group_approx_impl!(common: $t, 1, $vector, $($compN),+);
|
||||||
|
|
||||||
|
// // FIXME: Real is needed here only for invertibility...
|
||||||
|
// impl<N: Real> SquareMatrix for $t<N> {
|
||||||
|
// type Vector = $vector<N>;
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn diagonal(&self) -> Self::Coordinates {
|
||||||
|
// $vector::new(self.m11)
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn determinant(&self) -> Self::Field {
|
||||||
|
// self.m11
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse(&self) -> Option<Self> {
|
||||||
|
// let mut res = *self;
|
||||||
|
// if res.try_inverse_mut() {
|
||||||
|
// Some(res)
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// None
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
// if relative_eq!(&self.m11, &::zero()) {
|
||||||
|
// false
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// self.m11 = ::one::<N>() / ::determinant(self);
|
||||||
|
|
||||||
|
// true
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn transpose_mut(&mut self) {
|
||||||
|
// // no-op
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// ident, 2, $vector: ident, $($compN: ident),+) => {
|
||||||
|
// matrix_group_approx_impl!(common: $t, 2, $vector, $($compN),+);
|
||||||
|
|
||||||
|
// // FIXME: Real is needed only for inversion here.
|
||||||
|
// impl<N: Real> SquareMatrix for $t<N> {
|
||||||
|
// type Vector = $vector<N>;
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn diagonal(&self) -> Self::Coordinates {
|
||||||
|
// $vector::new(self.m11, self.m22)
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn determinant(&self) -> Self::Field {
|
||||||
|
// self.m11 * self.m22 - self.m21 * self.m12
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse(&self) -> Option<Self> {
|
||||||
|
// let mut res = *self;
|
||||||
|
// if res.try_inverse_mut() {
|
||||||
|
// Some(res)
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// None
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
// let determinant = ::determinant(self);
|
||||||
|
|
||||||
|
// if relative_eq!(&determinant, &::zero()) {
|
||||||
|
// false
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// *self = Matrix2::new(
|
||||||
|
// self.m22 / determinant , -self.m12 / determinant,
|
||||||
|
// -self.m21 / determinant, self.m11 / determinant);
|
||||||
|
|
||||||
|
// true
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn transpose_mut(&mut self) {
|
||||||
|
// mem::swap(&mut self.m12, &mut self.m21)
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// ident, 3, $vector: ident, $($compN: ident),+) => {
|
||||||
|
// matrix_group_approx_impl!(common: $t, 3, $vector, $($compN),+);
|
||||||
|
|
||||||
|
// // FIXME: Real is needed only for inversion here.
|
||||||
|
// impl<N: Real> SquareMatrix for $t<N> {
|
||||||
|
// type Vector = $vector<N>;
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn diagonal(&self) -> Self::Coordinates {
|
||||||
|
// $vector::new(self.m11, self.m22, self.m33)
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn determinant(&self) -> Self::Field {
|
||||||
|
// let minor_m12_m23 = self.m22 * self.m33 - self.m32 * self.m23;
|
||||||
|
// let minor_m11_m23 = self.m21 * self.m33 - self.m31 * self.m23;
|
||||||
|
// let minor_m11_m22 = self.m21 * self.m32 - self.m31 * self.m22;
|
||||||
|
|
||||||
|
// self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse(&self) -> Option<Self> {
|
||||||
|
// let mut res = *self;
|
||||||
|
// if res.try_inverse_mut() {
|
||||||
|
// Some(res)
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// None
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
// let minor_m12_m23 = self.m22 * self.m33 - self.m32 * self.m23;
|
||||||
|
// let minor_m11_m23 = self.m21 * self.m33 - self.m31 * self.m23;
|
||||||
|
// let minor_m11_m22 = self.m21 * self.m32 - self.m31 * self.m22;
|
||||||
|
|
||||||
|
// let determinant = self.m11 * minor_m12_m23 -
|
||||||
|
// self.m12 * minor_m11_m23 +
|
||||||
|
// self.m13 * minor_m11_m22;
|
||||||
|
|
||||||
|
// if relative_eq!(&determinant, &::zero()) {
|
||||||
|
// false
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// *self = Matrix3::new(
|
||||||
|
// (minor_m12_m23 / determinant),
|
||||||
|
// ((self.m13 * self.m32 - self.m33 * self.m12) / determinant),
|
||||||
|
// ((self.m12 * self.m23 - self.m22 * self.m13) / determinant),
|
||||||
|
|
||||||
|
// (-minor_m11_m23 / determinant),
|
||||||
|
// ((self.m11 * self.m33 - self.m31 * self.m13) / determinant),
|
||||||
|
// ((self.m13 * self.m21 - self.m23 * self.m11) / determinant),
|
||||||
|
|
||||||
|
// (minor_m11_m22 / determinant),
|
||||||
|
// ((self.m12 * self.m31 - self.m32 * self.m11) / determinant),
|
||||||
|
// ((self.m11 * self.m22 - self.m21 * self.m12) / determinant)
|
||||||
|
// );
|
||||||
|
|
||||||
|
// true
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn transpose_mut(&mut self) {
|
||||||
|
// mem::swap(&mut self.m12, &mut self.m21);
|
||||||
|
// mem::swap(&mut self.m13, &mut self.m31);
|
||||||
|
// mem::swap(&mut self.m23, &mut self.m32);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// ident, $dimension: expr, $vector: ident, $($compN: ident),+) => {
|
||||||
|
// matrix_group_approx_impl!(common: $t, $dimension, $vector, $($compN),+);
|
||||||
|
|
||||||
|
// // FIXME: Real is needed only for inversion here.
|
||||||
|
// impl<N: Real> SquareMatrix for $t<N> {
|
||||||
|
// type Vector = $vector<N>;
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn diagonal(&self) -> Self::Coordinates {
|
||||||
|
// let mut diagonal: $vector<N> = ::zero();
|
||||||
|
|
||||||
|
// for i in 0 .. $dimension {
|
||||||
|
// unsafe { diagonal.unsafe_set(i, self.get_unchecked(i, i)) }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// diagonal
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn determinant(&self) -> Self::Field {
|
||||||
|
// // FIXME: extremely naive implementation.
|
||||||
|
// let mut det = ::zero();
|
||||||
|
|
||||||
|
// for icol in 0 .. $dimension {
|
||||||
|
// let e = unsafe { self.unsafe_at((0, icol)) };
|
||||||
|
|
||||||
|
// if e != ::zero() {
|
||||||
|
// let minor_mat = self.delete_row_column(0, icol);
|
||||||
|
// let minor = minor_mat.determinant();
|
||||||
|
|
||||||
|
// if icol % 2 == 0 {
|
||||||
|
// det += minor;
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// det -= minor;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// det
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse(&self) -> Option<Self> {
|
||||||
|
// let mut res = *self;
|
||||||
|
// if res.try_inverse_mut() {
|
||||||
|
// Some(res)
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// None
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
// let mut res: $t<N> = ::one();
|
||||||
|
|
||||||
|
// // Inversion using Gauss-Jordan elimination
|
||||||
|
// for k in 0 .. $dimension {
|
||||||
|
// // search a non-zero value on the k-th column
|
||||||
|
// // FIXME: would it be worth it to spend some more time searching for the
|
||||||
|
// // max instead?
|
||||||
|
|
||||||
|
// let mut n0 = k; // index of a non-zero entry
|
||||||
|
|
||||||
|
// while n0 != $dimension {
|
||||||
|
// if self[(n0, k)] != ::zero() {
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// n0 = n0 + 1;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if n0 == $dimension {
|
||||||
|
// return false
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // swap pivot line
|
||||||
|
// if n0 != k {
|
||||||
|
// for j in 0 .. $dimension {
|
||||||
|
// self.swap((n0, j), (k, j));
|
||||||
|
// res.swap((n0, j), (k, j));
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// let pivot = self[(k, k)];
|
||||||
|
|
||||||
|
// for j in k .. $dimension {
|
||||||
|
// let selfval = self[(k, j)] / pivot;
|
||||||
|
// self[(k, j)] = selfval;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// for j in 0 .. $dimension {
|
||||||
|
// let resval = res[(k, j)] / pivot;
|
||||||
|
// res[(k, j)] = resval;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// for l in 0 .. $dimension {
|
||||||
|
// if l != k {
|
||||||
|
// let normalizer = self[(l, k)];
|
||||||
|
|
||||||
|
// for j in k .. $dimension {
|
||||||
|
// let selfval = self[(l, j)] - self[(k, j)] * normalizer;
|
||||||
|
// self[(l, j)] = selfval;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// for j in 0 .. $dimension {
|
||||||
|
// let resval = res[(l, j)] - res[(k, j)] * normalizer;
|
||||||
|
// res[(l, j)] = resval;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// *self = res;
|
||||||
|
|
||||||
|
// true
|
||||||
|
// }
|
||||||
|
|
||||||
|
// #[inline]
|
||||||
|
// fn transpose_mut(&mut self) {
|
||||||
|
// for i in 1 .. $dimension {
|
||||||
|
// for j in 0 .. i {
|
||||||
|
// self.swap((i, j), (j, i))
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Ordering
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, R: Dim, C: Dim, S> MeetSemilattice for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + MeetSemilattice,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn meet(&self, other: &Self) -> Self {
|
||||||
|
self.zip_map(other, |a, b| a.meet(&b))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> JoinSemilattice for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + JoinSemilattice,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn join(&self, other: &Self) -> Self {
|
||||||
|
self.zip_map(other, |a, b| a.join(&b))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> Lattice for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + Lattice,
|
||||||
|
S: OwnedStorage<N, R, C>,
|
||||||
|
S::Alloc: OwnedAllocator<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn meet_join(&self, other: &Self) -> (Self, Self) {
|
||||||
|
let shape = self.data.shape();
|
||||||
|
assert!(shape == other.data.shape(), "Matrix meet/join error: mismatched dimensions.");
|
||||||
|
|
||||||
|
let mut mres = unsafe { Self::new_uninitialized_generic(shape.0, shape.1) };
|
||||||
|
let mut jres = unsafe { Self::new_uninitialized_generic(shape.0, shape.1) };
|
||||||
|
|
||||||
|
for i in 0 .. shape.0.value() * shape.1.value() {
|
||||||
|
unsafe {
|
||||||
|
let mj = self.data.get_unchecked_linear(i).meet_join(other.data.get_unchecked_linear(i));
|
||||||
|
*mres.data.get_unchecked_linear_mut(i) = mj.0;
|
||||||
|
*jres.data.get_unchecked_linear_mut(i) = mj.1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
(mres, jres)
|
||||||
|
}
|
||||||
|
}
|
187
src/core/matrix_array.rs
Normal file
187
src/core/matrix_array.rs
Normal file
@ -0,0 +1,187 @@
|
|||||||
|
use std::ops::{Deref, DerefMut, Mul};
|
||||||
|
use std::fmt::{Debug, Formatter, Result};
|
||||||
|
use std::hash::{Hash, Hasher};
|
||||||
|
|
||||||
|
use typenum::Prod;
|
||||||
|
use generic_array::{ArrayLength, GenericArray};
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::storage::{Storage, StorageMut, Owned, OwnedStorage};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
use core::default_allocator::DefaultAllocator;
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Static Storage.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// A array-based statically sized matrix data storage.
|
||||||
|
#[repr(C)]
|
||||||
|
pub struct MatrixArray<N, R, C>
|
||||||
|
where R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
|
||||||
|
data: GenericArray<N, Prod<R::Value, C::Value>>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> Hash for MatrixArray<N, R, C>
|
||||||
|
where N: Hash,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
fn hash<H: Hasher>(&self, state: &mut H) {
|
||||||
|
self.data[..].hash(state)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> Deref for MatrixArray<N, R, C>
|
||||||
|
where R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
type Target = GenericArray<N, Prod<R::Value, C::Value>>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
&self.data
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> DerefMut for MatrixArray<N, R, C>
|
||||||
|
where R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
#[inline]
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
&mut self.data
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> Debug for MatrixArray<N, R, C>
|
||||||
|
where N: Debug,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
#[inline]
|
||||||
|
fn fmt(&self, fmt: &mut Formatter) -> Result {
|
||||||
|
self.data.fmt(fmt)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> Copy for MatrixArray<N, R, C>
|
||||||
|
where N: Copy,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N>,
|
||||||
|
GenericArray<N, Prod<R::Value, C::Value>> : Copy
|
||||||
|
{ }
|
||||||
|
|
||||||
|
impl<N, R, C> Clone for MatrixArray<N, R, C>
|
||||||
|
where N: Clone,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
#[inline]
|
||||||
|
fn clone(&self) -> Self {
|
||||||
|
MatrixArray {
|
||||||
|
data: self.data.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> Eq for MatrixArray<N, R, C>
|
||||||
|
where N: Eq,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R, C> PartialEq for MatrixArray<N, R, C>
|
||||||
|
where N: PartialEq,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &Self) -> bool {
|
||||||
|
self.data == right.data
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
unsafe impl<N, R, C> Storage<N, R, C> for MatrixArray<N, R, C>
|
||||||
|
where N: Scalar,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
type RStride = U1;
|
||||||
|
type CStride = R;
|
||||||
|
type Alloc = DefaultAllocator;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn into_owned(self) -> Owned<N, R, C, Self::Alloc> {
|
||||||
|
self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn clone_owned(&self) -> Owned<N, R, C, Self::Alloc> {
|
||||||
|
let it = self.iter().cloned();
|
||||||
|
|
||||||
|
Self::Alloc::allocate_from_iterator(self.shape().0, self.shape().1, it)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ptr(&self) -> *const N {
|
||||||
|
self[..].as_ptr()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn shape(&self) -> (R, C) {
|
||||||
|
(R::name(), C::name())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn strides(&self) -> (Self::RStride, Self::CStride) {
|
||||||
|
(Self::RStride::name(), Self::CStride::name())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
unsafe impl<N, R, C> StorageMut<N, R, C> for MatrixArray<N, R, C>
|
||||||
|
where N: Scalar,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
#[inline]
|
||||||
|
fn ptr_mut(&mut self) -> *mut N {
|
||||||
|
self[..].as_mut_ptr()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
unsafe impl<N, R, C> OwnedStorage<N, R, C> for MatrixArray<N, R, C>
|
||||||
|
where N: Scalar,
|
||||||
|
R: DimName,
|
||||||
|
C: DimName,
|
||||||
|
R::Value: Mul<C::Value>,
|
||||||
|
Prod<R::Value, C::Value>: ArrayLength<N> {
|
||||||
|
#[inline]
|
||||||
|
fn as_slice(&self) -> &[N] {
|
||||||
|
&self[..]
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn as_mut_slice(&mut self) -> &mut [N] {
|
||||||
|
&mut self[..]
|
||||||
|
}
|
||||||
|
}
|
466
src/core/matrix_slice.rs
Normal file
466
src/core/matrix_slice.rs
Normal file
@ -0,0 +1,466 @@
|
|||||||
|
use std::marker::PhantomData;
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix};
|
||||||
|
use core::dimension::{Dim, DimName, Dynamic, DimMul, DimProd, U1};
|
||||||
|
use core::iter::MatrixIter;
|
||||||
|
use core::storage::{Storage, StorageMut, Owned};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
|
||||||
|
macro_rules! slice_storage_impl(
|
||||||
|
($Storage: ident as $SRef: ty; $T: ident.$get_addr: ident ($Ptr: ty as $Ref: ty)) => {
|
||||||
|
pub struct $T<'a, N: Scalar, R: Dim, C: Dim, RStride: Dim, CStride: Dim, Alloc> {
|
||||||
|
ptr: $Ptr,
|
||||||
|
shape: (R, C),
|
||||||
|
strides: (RStride, CStride),
|
||||||
|
_phantoms: PhantomData<($Ref, Alloc)>,
|
||||||
|
}
|
||||||
|
|
||||||
|
// Dynamic and () are arbitrary. It's just to be able to call the constructors with
|
||||||
|
// `Slice::`
|
||||||
|
impl<'a, N: Scalar, R: Dim, C: Dim> $T<'a, N, R, C, Dynamic, Dynamic, ()> {
|
||||||
|
/// Create a new matrix slice without bound checking.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn new_unchecked<RStor, CStor, S>(storage: $SRef, start: (usize, usize), shape: (R, C))
|
||||||
|
-> $T<'a, N, R, C, S::RStride, S::CStride, S::Alloc>
|
||||||
|
where RStor: Dim,
|
||||||
|
CStor: Dim,
|
||||||
|
S: $Storage<N, RStor, CStor> {
|
||||||
|
|
||||||
|
let strides = storage.strides();
|
||||||
|
$T::new_with_strides_unchecked(storage, start, shape, strides)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Create a new matrix slice without bound checking.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn new_with_strides_unchecked<S, RStor, CStor, RStride, CStride>(storage: $SRef,
|
||||||
|
start: (usize, usize),
|
||||||
|
shape: (R, C),
|
||||||
|
strides: (RStride, CStride))
|
||||||
|
-> $T<'a, N, R, C, RStride, CStride, S::Alloc>
|
||||||
|
where RStor: Dim,
|
||||||
|
CStor: Dim,
|
||||||
|
S: $Storage<N, RStor, CStor>,
|
||||||
|
RStride: Dim,
|
||||||
|
CStride: Dim {
|
||||||
|
|
||||||
|
$T {
|
||||||
|
ptr: storage.$get_addr(start.0, start.1),
|
||||||
|
shape: shape,
|
||||||
|
strides: (strides.0, strides.1),
|
||||||
|
_phantoms: PhantomData
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
slice_storage_impl!(Storage as &'a S; SliceStorage.get_address_unchecked(*const N as &'a N));
|
||||||
|
slice_storage_impl!(StorageMut as &'a mut S; SliceStorageMut.get_address_unchecked_mut(*mut N as &'a mut N));
|
||||||
|
|
||||||
|
|
||||||
|
impl<'a, N: Scalar, R: Dim, C: Dim, RStride: Dim, CStride: Dim, Alloc> Copy
|
||||||
|
for SliceStorage<'a, N, R, C, RStride, CStride, Alloc> { }
|
||||||
|
|
||||||
|
impl<'a, N: Scalar, R: Dim, C: Dim, RStride: Dim, CStride: Dim, Alloc> Clone
|
||||||
|
for SliceStorage<'a, N, R, C, RStride, CStride, Alloc> {
|
||||||
|
#[inline]
|
||||||
|
fn clone(&self) -> Self {
|
||||||
|
SliceStorage {
|
||||||
|
ptr: self.ptr,
|
||||||
|
shape: self.shape,
|
||||||
|
strides: self.strides,
|
||||||
|
_phantoms: PhantomData,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! storage_impl(
|
||||||
|
($($T: ident),* $(,)*) => {$(
|
||||||
|
unsafe impl<'a, N, R: Dim, C: Dim, RStride: Dim, CStride: Dim, Alloc> Storage<N, R, C>
|
||||||
|
for $T<'a, N, R, C, RStride, CStride, Alloc>
|
||||||
|
where N: Scalar,
|
||||||
|
Alloc: Allocator<N, R, C> {
|
||||||
|
|
||||||
|
type RStride = RStride;
|
||||||
|
type CStride = CStride;
|
||||||
|
type Alloc = Alloc;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn into_owned(self) -> Owned<N, R, C, Self::Alloc> {
|
||||||
|
self.clone_owned()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn clone_owned(&self) -> Owned<N, R, C, Self::Alloc> {
|
||||||
|
let (nrows, ncols) = self.shape();
|
||||||
|
let it = MatrixIter::new(self).cloned();
|
||||||
|
Alloc::allocate_from_iterator(nrows, ncols, it)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ptr(&self) -> *const N {
|
||||||
|
self.ptr
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn shape(&self) -> (R, C) {
|
||||||
|
self.shape
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn strides(&self) -> (Self::RStride, Self::CStride) {
|
||||||
|
self.strides
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
storage_impl!(SliceStorage, SliceStorageMut);
|
||||||
|
|
||||||
|
unsafe impl<'a, N, R: Dim, C: Dim, RStride: Dim, CStride: Dim, Alloc> StorageMut<N, R, C>
|
||||||
|
for SliceStorageMut<'a, N, R, C, RStride, CStride, Alloc>
|
||||||
|
where N: Scalar,
|
||||||
|
Alloc: Allocator<N, R, C> {
|
||||||
|
#[inline]
|
||||||
|
fn ptr_mut(&mut self) -> *mut N {
|
||||||
|
self.ptr
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn assert_slice_index(&self, start: (usize, usize), shape: (usize, usize), steps: (usize, usize)) {
|
||||||
|
let my_shape = self.shape();
|
||||||
|
assert!(start.0 + (shape.0 - 1) * steps.0 <= my_shape.0, "Matrix slicing out of bounds.");
|
||||||
|
assert!(start.1 + (shape.1 - 1) * steps.1 <= my_shape.1, "Matrix slicing out of bounds.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
macro_rules! matrix_slice_impl(
|
||||||
|
($me: ident: $Me: ty, $MatrixSlice: ident, $SliceStorage: ident, $Storage: ident, $data: expr;
|
||||||
|
$row: ident,
|
||||||
|
$rows: ident,
|
||||||
|
$rows_with_step: ident,
|
||||||
|
$fixed_rows: ident,
|
||||||
|
$fixed_rows_with_step: ident,
|
||||||
|
$rows_generic: ident,
|
||||||
|
$column: ident,
|
||||||
|
$columns: ident,
|
||||||
|
$columns_with_step: ident,
|
||||||
|
$fixed_columns: ident,
|
||||||
|
$fixed_columns_with_step: ident,
|
||||||
|
$columns_generic: ident,
|
||||||
|
$slice: ident,
|
||||||
|
$slice_with_steps: ident,
|
||||||
|
$fixed_slice: ident,
|
||||||
|
$fixed_slice_with_steps: ident,
|
||||||
|
$generic_slice: ident,
|
||||||
|
$generic_slice_with_steps: ident) => {
|
||||||
|
/// A matrix slice.
|
||||||
|
pub type $MatrixSlice<'a, N, R, C, RStride, CStride, Alloc>
|
||||||
|
= Matrix<N, R, C, $SliceStorage<'a, N, R, C, RStride, CStride, Alloc>>;
|
||||||
|
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: $Storage<N, R, C>> Matrix<N, R, C, S> {
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Row slicing.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// Returns a slice containing the i-th column of this matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn $row($me: $Me, i: usize) -> $MatrixSlice<N, U1, C, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
$me.$fixed_rows::<U1>(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix a set of consecutive rows.
|
||||||
|
#[inline]
|
||||||
|
pub fn $rows($me: $Me, first_row: usize, nrows: usize)
|
||||||
|
-> $MatrixSlice<N, Dynamic, C, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
|
||||||
|
let my_shape = $me.data.shape();
|
||||||
|
$me.assert_slice_index((first_row, 0), (nrows, my_shape.1.value()), (1, 1));
|
||||||
|
let shape = (Dynamic::new(nrows), my_shape.1);
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, (first_row, 0), shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix a set of consecutive rows regularly spaced by `step` rows.
|
||||||
|
#[inline]
|
||||||
|
pub fn $rows_with_step($me: $Me, first_row: usize, nrows: usize, step: usize)
|
||||||
|
-> $MatrixSlice<N, Dynamic, C, Dynamic, S::CStride, S::Alloc> {
|
||||||
|
|
||||||
|
$me.$rows_generic(first_row, Dynamic::new(nrows), Dynamic::new(step))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts a compile-time number of consecutive rows from this matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn $fixed_rows<RSlice>($me: $Me, first_row: usize)
|
||||||
|
-> $MatrixSlice<N, RSlice, C, S::RStride, S::CStride, S::Alloc>
|
||||||
|
where RSlice: DimName {
|
||||||
|
|
||||||
|
let my_shape = $me.data.shape();
|
||||||
|
$me.assert_slice_index((first_row, 0), (RSlice::dim(), my_shape.1.value()), (1, 1));
|
||||||
|
let shape = (RSlice::name(), my_shape.1);
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, (first_row, 0), shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix a compile-time number of rows regularly spaced by `step` rows.
|
||||||
|
#[inline]
|
||||||
|
pub fn $fixed_rows_with_step<RSlice>($me: $Me, first_row: usize, step: usize)
|
||||||
|
-> $MatrixSlice<N, RSlice, C, Dynamic, S::CStride, S::Alloc>
|
||||||
|
where RSlice: DimName {
|
||||||
|
|
||||||
|
$me.$rows_generic(first_row, RSlice::name(), Dynamic::new(step))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix `nrows` rows regularly spaced by `step` rows. Both argument may
|
||||||
|
/// or may not be values known at compile-time.
|
||||||
|
#[inline]
|
||||||
|
pub fn $rows_generic<RSlice, RStep>($me: $Me, row_start: usize, nrows: RSlice, step: RStep)
|
||||||
|
-> $MatrixSlice<N, RSlice, C, DimProd<RStep, S::RStride>, S::CStride, S::Alloc>
|
||||||
|
where RSlice: Dim,
|
||||||
|
RStep: DimMul<S::RStride> {
|
||||||
|
|
||||||
|
let my_shape = $me.data.shape();
|
||||||
|
let my_strides = $me.data.strides();
|
||||||
|
$me.assert_slice_index((row_start, 0), (nrows.value(), my_shape.1.value()), (step.value(), 1));
|
||||||
|
|
||||||
|
let strides = (step.mul(my_strides.0), my_strides.1);
|
||||||
|
let shape = (nrows, my_shape.1);
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_with_strides_unchecked($data, (row_start, 0), shape, strides);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Column slicing.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// Returns a slice containing the i-th column of this matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn $column($me: $Me, i: usize) -> $MatrixSlice<N, R, U1, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
$me.$fixed_columns::<U1>(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix a set of consecutive columns.
|
||||||
|
#[inline]
|
||||||
|
pub fn $columns($me: $Me, first_col: usize, ncols: usize)
|
||||||
|
-> $MatrixSlice<N, R, Dynamic, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
|
||||||
|
let my_shape = $me.data.shape();
|
||||||
|
$me.assert_slice_index((0, first_col), (my_shape.0.value(), ncols), (1, 1));
|
||||||
|
let shape = (my_shape.0, Dynamic::new(ncols));
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, (0, first_col), shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix a set of consecutive columns regularly spaced by `step` columns.
|
||||||
|
#[inline]
|
||||||
|
pub fn $columns_with_step($me: $Me, first_col: usize, ncols: usize, step: usize)
|
||||||
|
-> $MatrixSlice<N, R, Dynamic, S::RStride, Dynamic, S::Alloc> {
|
||||||
|
|
||||||
|
$me.$columns_generic(first_col, Dynamic::new(ncols), Dynamic::new(step))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts a compile-time number of consecutive columns from this matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn $fixed_columns<CSlice>($me: $Me, first_col: usize)
|
||||||
|
-> $MatrixSlice<N, R, CSlice, S::RStride, S::CStride, S::Alloc>
|
||||||
|
where CSlice: DimName {
|
||||||
|
|
||||||
|
let my_shape = $me.data.shape();
|
||||||
|
$me.assert_slice_index((0, first_col), (my_shape.0.value(), CSlice::dim()), (1, 1));
|
||||||
|
let shape = (my_shape.0, CSlice::name());
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, (0, first_col), shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix a compile-time number of columns regularly spaced by `step`
|
||||||
|
/// columns.
|
||||||
|
#[inline]
|
||||||
|
pub fn $fixed_columns_with_step<CSlice>($me: $Me, first_col: usize, step: usize)
|
||||||
|
-> $MatrixSlice<N, R, CSlice, S::RStride, Dynamic, S::Alloc>
|
||||||
|
where CSlice: DimName {
|
||||||
|
|
||||||
|
$me.$columns_generic(first_col, CSlice::name(), Dynamic::new(step))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Extracts from this matrix `ncols` columns regularly spaced by `step` columns. Both argument may
|
||||||
|
/// or may not be values known at compile-time.
|
||||||
|
#[inline]
|
||||||
|
pub fn $columns_generic<CSlice, CStep>($me: $Me, first_col: usize, ncols: CSlice, step: CStep)
|
||||||
|
-> $MatrixSlice<N, R, CSlice, S::RStride, DimProd<CStep, S::CStride>, S::Alloc>
|
||||||
|
where CSlice: Dim,
|
||||||
|
CStep: DimMul<S::CStride> {
|
||||||
|
|
||||||
|
let my_shape = $me.data.shape();
|
||||||
|
let my_strides = $me.data.strides();
|
||||||
|
|
||||||
|
$me.assert_slice_index((0, first_col), (my_shape.0.value(), ncols.value()), (1, step.value()));
|
||||||
|
|
||||||
|
let strides = (my_strides.0, step.mul(my_strides.1));
|
||||||
|
let shape = (my_shape.0, ncols);
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_with_strides_unchecked($data, (0, first_col), shape, strides);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* General slicing.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// Slices this matrix starting at its component `(irow, icol)` and with `(nrows, ncols)`
|
||||||
|
/// consecutive elements.
|
||||||
|
#[inline]
|
||||||
|
pub fn $slice($me: $Me, start: (usize, usize), shape: (usize, usize))
|
||||||
|
-> $MatrixSlice<N, Dynamic, Dynamic, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
|
||||||
|
$me.assert_slice_index(start, shape, (1, 1));
|
||||||
|
let shape = (Dynamic::new(shape.0), Dynamic::new(shape.1));
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, start, shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn $slice_with_steps($me: $Me, start: (usize, usize), shape: (usize, usize), steps: (usize, usize))
|
||||||
|
-> $MatrixSlice<N, Dynamic, Dynamic, Dynamic, Dynamic, S::Alloc> {
|
||||||
|
let shape = (Dynamic::new(shape.0), Dynamic::new(shape.1));
|
||||||
|
let steps = (Dynamic::new(steps.0), Dynamic::new(steps.1));
|
||||||
|
|
||||||
|
$me.$generic_slice_with_steps(start, shape, steps)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Slices this matrix starting at its component `(irow, icol)` and with `(R::dim(),
|
||||||
|
/// CSlice::dim())` consecutive components.
|
||||||
|
#[inline]
|
||||||
|
pub fn $fixed_slice<RSlice, CSlice>($me: $Me, irow: usize, icol: usize)
|
||||||
|
-> $MatrixSlice<N, RSlice, CSlice, S::RStride, S::CStride, S::Alloc>
|
||||||
|
where RSlice: DimName,
|
||||||
|
CSlice: DimName {
|
||||||
|
|
||||||
|
$me.assert_slice_index((irow, icol), (RSlice::dim(), CSlice::dim()), (1, 1));
|
||||||
|
let shape = (RSlice::name(), CSlice::name());
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, (irow, icol), shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn $fixed_slice_with_steps<RSlice, CSlice>($me: $Me, start: (usize, usize), steps: (usize, usize))
|
||||||
|
-> $MatrixSlice<N, RSlice, CSlice, Dynamic, Dynamic, S::Alloc>
|
||||||
|
where RSlice: DimName,
|
||||||
|
CSlice: DimName {
|
||||||
|
let shape = (RSlice::name(), CSlice::name());
|
||||||
|
let steps = (Dynamic::new(steps.0), Dynamic::new(steps.1));
|
||||||
|
$me.$generic_slice_with_steps(start, shape, steps)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a slice that may or may not have a fixed size and stride.
|
||||||
|
#[inline]
|
||||||
|
pub fn $generic_slice<RSlice, CSlice>($me: $Me, start: (usize, usize), shape: (RSlice, CSlice))
|
||||||
|
-> $MatrixSlice<N, RSlice, CSlice, S::RStride, S::CStride, S::Alloc>
|
||||||
|
where RSlice: Dim,
|
||||||
|
CSlice: Dim {
|
||||||
|
|
||||||
|
$me.assert_slice_index(start, (shape.0.value(), shape.1.value()), (1, 1));
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_unchecked($data, start, shape);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a slice that may or may not have a fixed size and stride.
|
||||||
|
#[inline]
|
||||||
|
pub fn $generic_slice_with_steps<RSlice, CSlice, RStep, CStep>($me: $Me,
|
||||||
|
start: (usize, usize),
|
||||||
|
shape: (RSlice, CSlice),
|
||||||
|
steps: (RStep, CStep))
|
||||||
|
-> $MatrixSlice<N, RSlice, CSlice, DimProd<RStep, S::RStride>, DimProd<CStep, S::CStride>, S::Alloc>
|
||||||
|
where RSlice: Dim,
|
||||||
|
CSlice: Dim,
|
||||||
|
RStep: DimMul<S::RStride>,
|
||||||
|
CStep: DimMul<S::CStride> {
|
||||||
|
|
||||||
|
$me.assert_slice_index(start, (shape.0.value(), shape.1.value()), (steps.0.value(), steps.1.value()));
|
||||||
|
|
||||||
|
let my_strides = $me.data.strides();
|
||||||
|
let strides = (steps.0.mul(my_strides.0), steps.1.mul(my_strides.1));
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
let data = $SliceStorage::new_with_strides_unchecked($data, start, shape, strides);
|
||||||
|
Matrix::from_data_statically_unchecked(data)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
matrix_slice_impl!(
|
||||||
|
self: &Self, MatrixSlice, SliceStorage, Storage, &self.data;
|
||||||
|
row,
|
||||||
|
rows,
|
||||||
|
rows_with_step,
|
||||||
|
fixed_rows,
|
||||||
|
fixed_rows_with_step,
|
||||||
|
rows_generic,
|
||||||
|
column,
|
||||||
|
columns,
|
||||||
|
columns_with_step,
|
||||||
|
fixed_columns,
|
||||||
|
fixed_columns_with_step,
|
||||||
|
columns_generic,
|
||||||
|
slice,
|
||||||
|
slice_with_steps,
|
||||||
|
fixed_slice,
|
||||||
|
fixed_slice_with_steps,
|
||||||
|
generic_slice,
|
||||||
|
generic_slice_with_steps);
|
||||||
|
|
||||||
|
|
||||||
|
matrix_slice_impl!(
|
||||||
|
self: &mut Self, MatrixSliceMut, SliceStorageMut, StorageMut, &mut self.data;
|
||||||
|
row_mut,
|
||||||
|
rows_mut,
|
||||||
|
rows_with_step_mut,
|
||||||
|
fixed_rows_mut,
|
||||||
|
fixed_rows_with_step_mut,
|
||||||
|
rows_generic_mut,
|
||||||
|
column_mut,
|
||||||
|
columns_mut,
|
||||||
|
columns_with_step_mut,
|
||||||
|
fixed_columns_mut,
|
||||||
|
fixed_columns_with_step_mut,
|
||||||
|
columns_generic_mut,
|
||||||
|
slice_mut,
|
||||||
|
slice_with_steps_mut,
|
||||||
|
fixed_slice_mut,
|
||||||
|
fixed_slice_with_steps_mut,
|
||||||
|
generic_slice_mut,
|
||||||
|
generic_slice_with_steps_mut);
|
171
src/core/matrix_vec.rs
Normal file
171
src/core/matrix_vec.rs
Normal file
@ -0,0 +1,171 @@
|
|||||||
|
use std::ops::Deref;
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::dimension::{Dim, DimName, Dynamic, U1};
|
||||||
|
use core::storage::{Storage, StorageMut, Owned, OwnedStorage};
|
||||||
|
use core::default_allocator::DefaultAllocator;
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Storage.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
/// A Vec-based matrix data storage. It may be dynamically-sized.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Eq, Debug, Clone, PartialEq)]
|
||||||
|
pub struct MatrixVec<N, R: Dim, C: Dim> {
|
||||||
|
data: Vec<N>,
|
||||||
|
nrows: R,
|
||||||
|
ncols: C
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim> MatrixVec<N, R, C> {
|
||||||
|
#[inline]
|
||||||
|
pub fn new(nrows: R, ncols: C, data: Vec<N>) -> MatrixVec<N, R, C> {
|
||||||
|
MatrixVec {
|
||||||
|
data: data,
|
||||||
|
nrows: nrows,
|
||||||
|
ncols: ncols
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The underlying data storage.
|
||||||
|
#[inline]
|
||||||
|
pub fn data(&self) -> &Vec<N> {
|
||||||
|
&self.data
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The underlying mutable data storage.
|
||||||
|
///
|
||||||
|
/// This is unsafe because this may cause UB if the vector is modified by the user.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn data_mut(&mut self) -> &mut Vec<N> {
|
||||||
|
&mut self.data
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim> Deref for MatrixVec<N, R, C> {
|
||||||
|
type Target = Vec<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
&self.data
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Dynamic − Static
|
||||||
|
* Dynamic − Dynamic
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
unsafe impl<N: Scalar, C: Dim> Storage<N, Dynamic, C> for MatrixVec<N, Dynamic, C> {
|
||||||
|
type RStride = U1;
|
||||||
|
type CStride = Dynamic;
|
||||||
|
type Alloc = DefaultAllocator;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn into_owned(self) -> Owned<N, Dynamic, C, Self::Alloc> {
|
||||||
|
self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn clone_owned(&self) -> Owned<N, Dynamic, C, Self::Alloc> {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ptr(&self) -> *const N {
|
||||||
|
self[..].as_ptr()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn shape(&self) -> (Dynamic, C) {
|
||||||
|
(self.nrows, self.ncols)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn strides(&self) -> (Self::RStride, Self::CStride) {
|
||||||
|
(Self::RStride::name(), self.nrows)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
unsafe impl<N: Scalar, R: DimName> Storage<N, R, Dynamic> for MatrixVec<N, R, Dynamic> {
|
||||||
|
type RStride = U1;
|
||||||
|
type CStride = R;
|
||||||
|
type Alloc = DefaultAllocator;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn into_owned(self) -> Owned<N, R, Dynamic, Self::Alloc> {
|
||||||
|
self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn clone_owned(&self) -> Owned<N, R, Dynamic, Self::Alloc> {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ptr(&self) -> *const N {
|
||||||
|
self[..].as_ptr()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn shape(&self) -> (R, Dynamic) {
|
||||||
|
(self.nrows, self.ncols)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn strides(&self) -> (Self::RStride, Self::CStride) {
|
||||||
|
(Self::RStride::name(), self.nrows)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* StorageMut, OwnedStorage.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
unsafe impl<N: Scalar, C: Dim> StorageMut<N, Dynamic, C> for MatrixVec<N, Dynamic, C> {
|
||||||
|
#[inline]
|
||||||
|
fn ptr_mut(&mut self) -> *mut N {
|
||||||
|
self.as_mut_slice().as_mut_ptr()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
unsafe impl<N: Scalar, C: Dim> OwnedStorage<N, Dynamic, C> for MatrixVec<N, Dynamic, C> {
|
||||||
|
#[inline]
|
||||||
|
fn as_slice(&self) -> &[N] {
|
||||||
|
&self[..]
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn as_mut_slice(&mut self) -> &mut [N] {
|
||||||
|
&mut self.data[..]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
unsafe impl<N: Scalar, R: DimName> StorageMut<N, R, Dynamic> for MatrixVec<N, R, Dynamic> {
|
||||||
|
#[inline]
|
||||||
|
fn ptr_mut(&mut self) -> *mut N {
|
||||||
|
self.as_mut_slice().as_mut_ptr()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
unsafe impl<N: Scalar, R: DimName> OwnedStorage<N, R, Dynamic> for MatrixVec<N, R, Dynamic> {
|
||||||
|
#[inline]
|
||||||
|
fn as_slice(&self) -> &[N] {
|
||||||
|
&self[..]
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn as_mut_slice(&mut self) -> &mut [N] {
|
||||||
|
&mut self.data[..]
|
||||||
|
}
|
||||||
|
}
|
41
src/core/mod.rs
Normal file
41
src/core/mod.rs
Normal file
@ -0,0 +1,41 @@
|
|||||||
|
pub mod dimension;
|
||||||
|
pub mod constraint;
|
||||||
|
pub mod allocator;
|
||||||
|
pub mod storage;
|
||||||
|
pub mod coordinates;
|
||||||
|
pub mod ops;
|
||||||
|
pub mod iter;
|
||||||
|
pub mod default_allocator;
|
||||||
|
|
||||||
|
mod scalar;
|
||||||
|
mod matrix;
|
||||||
|
mod construction;
|
||||||
|
mod properties;
|
||||||
|
mod alias;
|
||||||
|
mod matrix_alga;
|
||||||
|
mod determinant;
|
||||||
|
mod inverse;
|
||||||
|
mod conversion;
|
||||||
|
mod matrix_slice;
|
||||||
|
mod matrix_array;
|
||||||
|
mod matrix_vec;
|
||||||
|
mod cg;
|
||||||
|
mod unit;
|
||||||
|
mod componentwise;
|
||||||
|
|
||||||
|
mod decompositions;
|
||||||
|
|
||||||
|
#[doc(hidden)]
|
||||||
|
pub mod helper;
|
||||||
|
|
||||||
|
pub use self::scalar::*;
|
||||||
|
pub use self::matrix::*;
|
||||||
|
pub use self::unit::*;
|
||||||
|
|
||||||
|
pub use self::dimension::*;
|
||||||
|
pub use self::default_allocator::*;
|
||||||
|
|
||||||
|
pub use self::alias::*;
|
||||||
|
pub use self::matrix_slice::*;
|
||||||
|
pub use self::matrix_array::*;
|
||||||
|
pub use self::matrix_vec::*;
|
462
src/core/ops.rs
Normal file
462
src/core/ops.rs
Normal file
@ -0,0 +1,462 @@
|
|||||||
|
use std::ops::{Add, AddAssign, Sub, SubAssign, Mul, MulAssign, Div, DivAssign, Neg,
|
||||||
|
Index, IndexMut};
|
||||||
|
use num::Zero;
|
||||||
|
|
||||||
|
use alga::general::{ClosedMul, ClosedDiv, ClosedAdd, ClosedSub, ClosedNeg};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, OwnedMatrix, MatrixSum, MatrixMul, MatrixTrMul};
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::constraint::{ShapeConstraint, SameNumberOfRows, SameNumberOfColumns, AreMultipliable};
|
||||||
|
use core::storage::{Storage, StorageMut, OwnedStorage};
|
||||||
|
use core::allocator::{SameShapeAllocator, Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Indexing.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Index<usize> for Matrix<N, R, C, S> {
|
||||||
|
type Output = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index(&self, i: usize) -> &N {
|
||||||
|
let ij = self.vector_to_matrix_index(i);
|
||||||
|
&self[ij]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> Index<(usize, usize)> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, R, C> {
|
||||||
|
type Output = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index(&self, ij: (usize, usize)) -> &N {
|
||||||
|
assert!(ij < self.shape(), "Matrix index out of bounds.");
|
||||||
|
unsafe { self.get_unchecked(ij.0, ij.1) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Mutable versions.
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: StorageMut<N, R, C>> IndexMut<usize> for Matrix<N, R, C, S> {
|
||||||
|
#[inline]
|
||||||
|
fn index_mut(&mut self, i: usize) -> &mut N {
|
||||||
|
let ij = self.vector_to_matrix_index(i);
|
||||||
|
&mut self[ij]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> IndexMut<(usize, usize)> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: StorageMut<N, R, C> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index_mut(&mut self, ij: (usize, usize)) -> &mut N {
|
||||||
|
assert!(ij < self.shape(), "Matrix index out of bounds.");
|
||||||
|
unsafe { self.get_unchecked_mut(ij.0, ij.1) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Neg
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, R: Dim, C: Dim, S> Neg for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: Storage<N, R, C> {
|
||||||
|
type Output = OwnedMatrix<N, R, C, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
let mut res = self.into_owned();
|
||||||
|
res.neg_mut();
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, R: Dim, C: Dim, S> Neg for &'a Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: Storage<N, R, C> {
|
||||||
|
type Output = OwnedMatrix<N, R, C, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
-self.clone_owned()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: StorageMut<N, R, C> {
|
||||||
|
/// Negates `self` in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn neg_mut(&mut self) {
|
||||||
|
for e in self.iter_mut() {
|
||||||
|
*e = -*e
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Addition & Substraction
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
macro_rules! componentwise_binop_impl(
|
||||||
|
($Trait: ident, $method: ident, $bound: ident;
|
||||||
|
$TraitAssign: ident, $method_assign: ident) => {
|
||||||
|
impl<'b, N, R1, C1, R2, C2, SA, SB> $Trait<&'b Matrix<N, R2, C2, SB>> for Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N: Scalar + $bound,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA::Alloc: SameShapeAllocator<N, R1, C1, R2, C2, SA>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
type Output = MatrixSum<N, R1, C1, R2, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, right: &'b Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
assert!(self.shape() == right.shape(), "Matrix addition/subtraction dimensions mismatch.");
|
||||||
|
let mut res = self.into_owned_sum::<R2, C2>();
|
||||||
|
|
||||||
|
for (left, right) in res.iter_mut().zip(right.iter()) {
|
||||||
|
*left = left.$method(*right)
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, R1, C1, R2, C2, SA, SB> $Trait<Matrix<N, R2, C2, SB>> for &'a Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N: Scalar + $bound,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SB::Alloc: SameShapeAllocator<N, R2, C2, R1, C1, SB>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R2, R1> + SameNumberOfColumns<C2, C1> {
|
||||||
|
type Output = MatrixSum<N, R2, C2, R1, C1, SB>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, right: Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
assert!(self.shape() == right.shape(), "Matrix addition/subtraction dimensions mismatch.");
|
||||||
|
let mut res = right.into_owned_sum::<R1, C1>();
|
||||||
|
|
||||||
|
for (left, right) in self.iter().zip(res.iter_mut()) {
|
||||||
|
*right = left.$method(*right)
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R1, C1, R2, C2, SA, SB> $Trait<Matrix<N, R2, C2, SB>> for Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N: Scalar + $bound,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA::Alloc: SameShapeAllocator<N, R1, C1, R2, C2, SA>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
type Output = MatrixSum<N, R1, C1, R2, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, right: Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
self.$method(&right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, 'b, N, R1, C1, R2, C2, SA, SB> $Trait<&'b Matrix<N, R2, C2, SB>> for &'a Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N: Scalar + $bound,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA::Alloc: SameShapeAllocator<N, R1, C1, R2, C2, SA>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
type Output = MatrixSum<N, R1, C1, R2, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, right: &'b Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
self.clone_owned().$method(right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N, R1, C1, R2, C2, SA, SB> $TraitAssign<&'b Matrix<N, R2, C2, SB>> for Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N: Scalar + $bound,
|
||||||
|
SA: StorageMut<N, R1, C1>,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method_assign(&mut self, right: &'b Matrix<N, R2, C2, SB>) {
|
||||||
|
assert!(self.shape() == right.shape(), "Matrix addition/subtraction dimensions mismatch.");
|
||||||
|
for (left, right) in self.iter_mut().zip(right.iter()) {
|
||||||
|
left.$method_assign(*right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R1, C1, R2, C2, SA, SB> $TraitAssign<Matrix<N, R2, C2, SB>> for Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim, C2: Dim,
|
||||||
|
N: Scalar + $bound,
|
||||||
|
SA: StorageMut<N, R1, C1>,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<R1, R2> + SameNumberOfColumns<C1, C2> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method_assign(&mut self, right: Matrix<N, R2, C2, SB>) {
|
||||||
|
self.$method_assign(&right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
componentwise_binop_impl!(Add, add, ClosedAdd; AddAssign, add_assign);
|
||||||
|
componentwise_binop_impl!(Sub, sub, ClosedSub; SubAssign, sub_assign);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Multiplication
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Matrix × Scalar
|
||||||
|
// Matrix / Scalar
|
||||||
|
macro_rules! componentwise_scalarop_impl(
|
||||||
|
($Trait: ident, $method: ident, $bound: ident;
|
||||||
|
$TraitAssign: ident, $method_assign: ident) => {
|
||||||
|
impl<N, R: Dim, C: Dim, S> $Trait<N> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
S: Storage<N, R, C> {
|
||||||
|
type Output = OwnedMatrix<N, R, C, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, rhs: N) -> Self::Output {
|
||||||
|
let mut res = self.into_owned();
|
||||||
|
|
||||||
|
for left in res.iter_mut() {
|
||||||
|
*left = left.$method(rhs)
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, R: Dim, C: Dim, S> $Trait<N> for &'a Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
S: Storage<N, R, C> {
|
||||||
|
type Output = OwnedMatrix<N, R, C, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, rhs: N) -> Self::Output {
|
||||||
|
self.clone_owned().$method(rhs)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R: Dim, C: Dim, S> $TraitAssign<N> for Matrix<N, R, C, S>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
S: StorageMut<N, R, C> {
|
||||||
|
#[inline]
|
||||||
|
fn $method_assign(&mut self, right: N) {
|
||||||
|
for left in self.iter_mut() {
|
||||||
|
left.$method_assign(right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
componentwise_scalarop_impl!(Mul, mul, ClosedMul; MulAssign, mul_assign);
|
||||||
|
componentwise_scalarop_impl!(Div, div, ClosedDiv; DivAssign, div_assign);
|
||||||
|
|
||||||
|
macro_rules! left_scalar_mul_impl(
|
||||||
|
($($T: ty),* $(,)*) => {$(
|
||||||
|
impl<R: Dim, C: Dim, S> Mul<Matrix<$T, R, C, S>> for $T
|
||||||
|
where S: Storage<$T, R, C> {
|
||||||
|
type Output = OwnedMatrix<$T, R, C, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: Matrix<$T, R, C, S>) -> Self::Output {
|
||||||
|
let mut res = right.into_owned();
|
||||||
|
|
||||||
|
for right in res.iter_mut() {
|
||||||
|
*right = self * *right
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, R: Dim, C: Dim, S> Mul<&'b Matrix<$T, R, C, S>> for $T
|
||||||
|
where S: Storage<$T, R, C> {
|
||||||
|
type Output = OwnedMatrix<$T, R, C, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: &'b Matrix<$T, R, C, S>) -> Self::Output {
|
||||||
|
self * right.clone_owned()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
left_scalar_mul_impl!(
|
||||||
|
u8, u16, u32, u64, usize,
|
||||||
|
i8, i16, i32, i64, isize,
|
||||||
|
f32, f64
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Matrix × Matrix
|
||||||
|
impl<'a, 'b, N, R1: Dim, C1: Dim, R2: Dim, C2: Dim, SA, SB> Mul<&'b Matrix<N, R2, C2, SB>>
|
||||||
|
for &'a Matrix<N, R1, C1, SA>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SA::Alloc: Allocator<N, R1, C2>,
|
||||||
|
ShapeConstraint: AreMultipliable<R1, C1, R2, C2> {
|
||||||
|
type Output = MatrixMul<N, R1, C1, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: &'b Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
let (nrows1, ncols1) = self.shape();
|
||||||
|
let (nrows2, ncols2) = right.shape();
|
||||||
|
|
||||||
|
assert!(ncols1 == nrows2, "Matrix multiplication dimensions mismatch.");
|
||||||
|
|
||||||
|
let mut res: MatrixMul<N, R1, C1, C2, SA> = unsafe {
|
||||||
|
Matrix::new_uninitialized_generic(self.data.shape().0, right.data.shape().1)
|
||||||
|
};
|
||||||
|
|
||||||
|
for i in 0 .. nrows1 {
|
||||||
|
for j in 0 .. ncols2 {
|
||||||
|
let mut acc = N::zero();
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
for k in 0 .. ncols1 {
|
||||||
|
acc = acc + *self.get_unchecked(i, k) * *right.get_unchecked(k, j);
|
||||||
|
}
|
||||||
|
|
||||||
|
*res.get_unchecked_mut(i, j) = acc;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, R1: Dim, C1: Dim, R2: Dim, C2: Dim, SA, SB> Mul<Matrix<N, R2, C2, SB>>
|
||||||
|
for &'a Matrix<N, R1, C1, SA>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SA::Alloc: Allocator<N, R1, C2>,
|
||||||
|
ShapeConstraint: AreMultipliable<R1, C1, R2, C2> {
|
||||||
|
type Output = MatrixMul<N, R1, C1, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
self * &right
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N, R1: Dim, C1: Dim, R2: Dim, C2: Dim, SA, SB> Mul<&'b Matrix<N, R2, C2, SB>>
|
||||||
|
for Matrix<N, R1, C1, SA>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SA::Alloc: Allocator<N, R1, C2>,
|
||||||
|
ShapeConstraint: AreMultipliable<R1, C1, R2, C2> {
|
||||||
|
type Output = MatrixMul<N, R1, C1, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: &'b Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
&self * right
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, R1: Dim, C1: Dim, R2: Dim, C2: Dim, SA, SB> Mul<Matrix<N, R2, C2, SB>>
|
||||||
|
for Matrix<N, R1, C1, SA>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SB: Storage<N, R2, C2>,
|
||||||
|
SA: Storage<N, R1, C1>,
|
||||||
|
SA::Alloc: Allocator<N, R1, C2>,
|
||||||
|
ShapeConstraint: AreMultipliable<R1, C1, R2, C2> {
|
||||||
|
type Output = MatrixMul<N, R1, C1, C2, SA>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: Matrix<N, R2, C2, SB>) -> Self::Output {
|
||||||
|
&self * &right
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: this is too restrictive:
|
||||||
|
// − we can't use `a *= b` when `a` is a mutable slice.
|
||||||
|
// − we can't use `a *= b` when C2 is not equal to C1.
|
||||||
|
impl<N, R1, C1, R2, SA, SB> MulAssign<Matrix<N, R2, C1, SB>> for Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim,
|
||||||
|
N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SB: Storage<N, R2, C1>,
|
||||||
|
SA: OwnedStorage<N, R1, C1>,
|
||||||
|
ShapeConstraint: AreMultipliable<R1, C1, R2, C1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, R1, C1, SA> {
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, right: Matrix<N, R2, C1, SB>) {
|
||||||
|
*self = &*self * right
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N, R1, C1, R2, SA, SB> MulAssign<&'b Matrix<N, R2, C1, SB>> for Matrix<N, R1, C1, SA>
|
||||||
|
where R1: Dim, C1: Dim, R2: Dim,
|
||||||
|
N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SB: Storage<N, R2, C1>,
|
||||||
|
SA: OwnedStorage<N, R1, C1>,
|
||||||
|
ShapeConstraint: AreMultipliable<R1, C1, R2, C1>,
|
||||||
|
// FIXME: this is too restrictive. See comments for the non-ref version.
|
||||||
|
SA::Alloc: OwnedAllocator<N, R1, C1, SA> {
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, right: &'b Matrix<N, R2, C1, SB>) {
|
||||||
|
*self = &*self * right
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, R1: Dim, C1: Dim, SA> Matrix<N, R1, C1, SA>
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul,
|
||||||
|
SA: Storage<N, R1, C1> {
|
||||||
|
/// Equivalent to `self.transpose() * right`.
|
||||||
|
#[inline]
|
||||||
|
pub fn tr_mul<R2: Dim, C2: Dim, SB>(&self, right: &Matrix<N, R2, C2, SB>) -> MatrixTrMul<N, R1, C1, C2, SA>
|
||||||
|
where SB: Storage<N, R2, C2>,
|
||||||
|
SA::Alloc: Allocator<N, C1, C2>,
|
||||||
|
ShapeConstraint: AreMultipliable<C1, R1, R2, C2> {
|
||||||
|
let (nrows1, ncols1) = self.shape();
|
||||||
|
let (nrows2, ncols2) = right.shape();
|
||||||
|
|
||||||
|
assert!(nrows1 == nrows2, "Matrix multiplication dimensions mismatch.");
|
||||||
|
|
||||||
|
let mut res: MatrixTrMul<N, R1, C1, C2, SA> = unsafe {
|
||||||
|
Matrix::new_uninitialized_generic(self.data.shape().1, right.data.shape().1)
|
||||||
|
};
|
||||||
|
|
||||||
|
for i in 0 .. ncols1 {
|
||||||
|
for j in 0 .. ncols2 {
|
||||||
|
let mut acc = N::zero();
|
||||||
|
|
||||||
|
unsafe {
|
||||||
|
for k in 0 .. nrows1 {
|
||||||
|
acc = acc + *self.get_unchecked(k, i) * *right.get_unchecked(k, j);
|
||||||
|
}
|
||||||
|
|
||||||
|
*res.get_unchecked_mut(i, j) = acc;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
107
src/core/properties.rs
Normal file
107
src/core/properties.rs
Normal file
@ -0,0 +1,107 @@
|
|||||||
|
// Matrix properties checks.
|
||||||
|
use num::{Zero, One};
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{ClosedAdd, ClosedMul, ClosedSub, Field};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, SquareMatrix};
|
||||||
|
use core::dimension::Dim;
|
||||||
|
use core::storage::Storage;
|
||||||
|
|
||||||
|
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
|
||||||
|
/// Indicates if this is a square matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn is_square(&self) -> bool {
|
||||||
|
let shape = self.shape();
|
||||||
|
shape.0 == shape.1
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S>
|
||||||
|
// FIXME: ApproxEq prevents us from using those methods on integer matrices…
|
||||||
|
where N: ApproxEq,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
/// Indicated if this is the identity matrix within a relative error of `eps`.
|
||||||
|
///
|
||||||
|
/// If the matrix is diagonal, this checks that diagonal elements (i.e. at coordinates `(i, i)`
|
||||||
|
/// for i from `0` to `min(R, C)`) are equal one; and that all other elements are zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn is_identity(&self, eps: N::Epsilon) -> bool
|
||||||
|
where N: Zero + One {
|
||||||
|
let (nrows, ncols) = self.shape();
|
||||||
|
let d;
|
||||||
|
|
||||||
|
if nrows > ncols {
|
||||||
|
d = ncols;
|
||||||
|
|
||||||
|
for i in d .. nrows {
|
||||||
|
for j in 0 .. ncols {
|
||||||
|
if !relative_eq!(self[(i, j)], N::zero(), epsilon = eps) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else { // nrows <= ncols
|
||||||
|
d = nrows;
|
||||||
|
|
||||||
|
for i in 0 .. nrows {
|
||||||
|
for j in d .. ncols {
|
||||||
|
if !relative_eq!(self[(i, j)], N::zero(), epsilon = eps) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Off-diagonal elements of the sub-square matrix.
|
||||||
|
for i in 1 .. d {
|
||||||
|
for j in 0 .. i {
|
||||||
|
// FIXME: use unsafe indexing.
|
||||||
|
if !relative_eq!(self[(i, j)], N::zero(), epsilon = eps) ||
|
||||||
|
!relative_eq!(self[(j, i)], N::zero(), epsilon = eps) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Diagonal elements of the sub-square matrix.
|
||||||
|
for i in 0 .. d {
|
||||||
|
if !relative_eq!(self[(i, i)], N::one(), epsilon = eps) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N: Scalar + ApproxEq, D: Dim, S: Storage<N, D, D>> SquareMatrix<N, D, S>
|
||||||
|
where N: Zero + One + ClosedAdd + ClosedMul,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
/// Checks that this matrix is orthogonal, i.e., that it is square and `M × Mᵀ = Id`.
|
||||||
|
///
|
||||||
|
/// In this definition `Id` is approximately equal to the identity matrix with a relative error
|
||||||
|
/// equal to `eps`.
|
||||||
|
#[inline]
|
||||||
|
pub fn is_orthogonal(&self, eps: N::Epsilon) -> bool {
|
||||||
|
self.is_square() && (self.tr_mul(self)).is_identity(eps)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Checks that this matrix is orthogonal and has a determinant equal to 1.
|
||||||
|
#[inline]
|
||||||
|
pub fn is_special_orthogonal(&self, eps: N::Epsilon) -> bool
|
||||||
|
where N: ClosedSub + PartialOrd {
|
||||||
|
self.is_orthogonal(eps) && self.determinant() > N::zero()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Returns `true` if this matrix is invertible.
|
||||||
|
#[inline]
|
||||||
|
pub fn is_invertible(&self) -> bool
|
||||||
|
where N: Field {
|
||||||
|
// FIXME: improve this?
|
||||||
|
self.clone_owned().try_inverse().is_some()
|
||||||
|
}
|
||||||
|
}
|
9
src/core/scalar.rs
Normal file
9
src/core/scalar.rs
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
use std::fmt::Debug;
|
||||||
|
use std::any::Any;
|
||||||
|
|
||||||
|
/// The basic scarar type for all structures of `nalgebra`.
|
||||||
|
///
|
||||||
|
/// This does not make any assumption on the algebraic properties of `Self`.
|
||||||
|
pub trait Scalar: Copy + PartialEq + Debug + Any {
|
||||||
|
}
|
||||||
|
impl<T: Copy + PartialEq + Debug + Any> Scalar for T { }
|
176
src/core/storage.rs
Normal file
176
src/core/storage.rs
Normal file
@ -0,0 +1,176 @@
|
|||||||
|
use std::mem;
|
||||||
|
use std::any::Any;
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use dimension::Dim;
|
||||||
|
use allocator::{Allocator, SameShapeR, SameShapeC};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Aliases for sum storage.
|
||||||
|
*/
|
||||||
|
/// The data storage for the sum of two matrices with dimensions `(R1, C1)` and `(R2, C2)`.
|
||||||
|
pub type SumStorage<N, R1, C1, R2, C2, SA> =
|
||||||
|
<<SA as Storage<N, R1, C1>>::Alloc as Allocator<N, SameShapeR<R1, R2>, SameShapeC<C1, C2>>>::Buffer;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Aliases for multiplication storage.
|
||||||
|
*/
|
||||||
|
/// The data storage for the multiplication of two matrices with dimensions `(R1, C1)` on the left
|
||||||
|
/// hand side, and with `C2` columns on the right hand side.
|
||||||
|
pub type MulStorage<N, R1, C1, C2, SA> =
|
||||||
|
<<SA as Storage<N, R1, C1>>::Alloc as Allocator<N, R1, C2>>::Buffer;
|
||||||
|
|
||||||
|
/// The data storage for the multiplication of two matrices with dimensions `(R1, C1)` on the left
|
||||||
|
/// hand side, and with `C2` columns on the right hand side. The first matrix is implicitly
|
||||||
|
/// transposed.
|
||||||
|
pub type TrMulStorage<N, R1, C1, C2, SA> =
|
||||||
|
<<SA as Storage<N, R1, C1>>::Alloc as Allocator<N, C1, C2>>::Buffer;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Alias for allocation result.
|
||||||
|
*/
|
||||||
|
/// The owned data storage that can be allocated from `S`.
|
||||||
|
pub type Owned<N, R, C, A> =
|
||||||
|
<A as Allocator<N, R, C>>::Buffer;
|
||||||
|
|
||||||
|
|
||||||
|
/// The trait shared by all matrix data storage.
|
||||||
|
///
|
||||||
|
/// FIXME: doc
|
||||||
|
///
|
||||||
|
/// Note that `Self` must always have a number of elements compatible with the matrix length (given
|
||||||
|
/// by `R` and `C` if they are known at compile-time). For example, implementors of this trait
|
||||||
|
/// should **not** allow the user to modify the size of the underlying buffer with safe methods
|
||||||
|
/// (for example the `MatrixVec::data_mut` method is unsafe because the user could change the
|
||||||
|
/// vector's size so that it no longer contains enough elements: this will lead to UB.
|
||||||
|
pub unsafe trait Storage<N: Scalar, R: Dim, C: Dim>: Sized {
|
||||||
|
/// The static stride of this storage's rows.
|
||||||
|
type RStride: Dim;
|
||||||
|
|
||||||
|
/// The static stride of this storage's columns.
|
||||||
|
type CStride: Dim;
|
||||||
|
|
||||||
|
/// The allocator for this family of storage.
|
||||||
|
type Alloc: Allocator<N, R, C>;
|
||||||
|
|
||||||
|
/// Builds a matrix data storage that does not contain any reference.
|
||||||
|
fn into_owned(self) -> Owned<N, R, C, Self::Alloc>;
|
||||||
|
|
||||||
|
/// Clones this data storage into one that does not contain any reference.
|
||||||
|
fn clone_owned(&self) -> Owned<N, R, C, Self::Alloc>;
|
||||||
|
|
||||||
|
/// The matrix data pointer.
|
||||||
|
fn ptr(&self) -> *const N;
|
||||||
|
|
||||||
|
/// The dimension of the matrix at run-time. Arr length of zero indicates the additive identity
|
||||||
|
/// element of any dimension. Must be equal to `Self::dimension()` if it is not `None`.
|
||||||
|
fn shape(&self) -> (R, C);
|
||||||
|
|
||||||
|
/// The spacing between concecutive row elements and consecutive column elements.
|
||||||
|
///
|
||||||
|
/// For example this returns `(1, 5)` for a row-major matrix with 5 columns.
|
||||||
|
fn strides(&self) -> (Self::RStride, Self::CStride);
|
||||||
|
|
||||||
|
/// Compute the index corresponding to the irow-th row and icol-th column of this matrix. The
|
||||||
|
/// index must be such that the following holds:
|
||||||
|
///
|
||||||
|
/// ```.ignore
|
||||||
|
/// let lindex = self.linear_index(irow, icol);
|
||||||
|
/// assert!(*self.get_unchecked(irow, icol) == *self.get_unchecked_linear(lindex)
|
||||||
|
/// ```
|
||||||
|
#[inline]
|
||||||
|
fn linear_index(&self, irow: usize, icol: usize) -> usize {
|
||||||
|
let (rstride, cstride) = self.strides();
|
||||||
|
|
||||||
|
irow * rstride.value() + icol * cstride.value()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the address of the i-th matrix component without performing bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_address_unchecked_linear(&self, i: usize) -> *const N {
|
||||||
|
self.ptr().offset(i as isize)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the address of the i-th matrix component without performing bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_address_unchecked(&self, irow: usize, icol: usize) -> *const N {
|
||||||
|
self.get_address_unchecked_linear(self.linear_index(irow, icol))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves a reference to the i-th element without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_unchecked_linear(&self, i: usize) -> &N {
|
||||||
|
&*self.get_address_unchecked_linear(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves a reference to the i-th element without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_unchecked(&self, irow: usize, icol: usize) -> &N {
|
||||||
|
self.get_unchecked_linear(self.linear_index(irow, icol))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub unsafe trait StorageMut<N: Scalar, R: Dim, C: Dim>: Storage<N, R, C> {
|
||||||
|
/// The matrix mutable data pointer.
|
||||||
|
fn ptr_mut(&mut self) -> *mut N;
|
||||||
|
|
||||||
|
/// Gets the mutable address of the i-th matrix component without performing bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_address_unchecked_linear_mut(&mut self, i: usize) -> *mut N {
|
||||||
|
self.ptr_mut().offset(i as isize)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the mutable address of the i-th matrix component without performing bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_address_unchecked_mut(&mut self, irow: usize, icol: usize) -> *mut N {
|
||||||
|
let lid = self.linear_index(irow, icol);
|
||||||
|
self.get_address_unchecked_linear_mut(lid)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves a mutable reference to the i-th element without bound-checking.
|
||||||
|
unsafe fn get_unchecked_linear_mut(&mut self, i: usize) -> &mut N {
|
||||||
|
&mut *self.get_address_unchecked_linear_mut(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves a mutable reference to the element at `(irow, icol)` without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_unchecked_mut(&mut self, irow: usize, icol: usize) -> &mut N {
|
||||||
|
&mut *self.get_address_unchecked_mut(irow, icol)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Swaps two elements using their linear index without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn swap_unchecked_linear(&mut self, i1: usize, i2: usize) {
|
||||||
|
let a = self.get_address_unchecked_linear_mut(i1);
|
||||||
|
let b = self.get_address_unchecked_linear_mut(i2);
|
||||||
|
|
||||||
|
mem::swap(&mut *a, &mut *b);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Swaps two elements without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
unsafe fn swap_unchecked(&mut self, row_col1: (usize, usize), row_col2: (usize, usize)) {
|
||||||
|
let lid1 = self.linear_index(row_col1.0, row_col1.1);
|
||||||
|
let lid2 = self.linear_index(row_col2.0, row_col2.1);
|
||||||
|
|
||||||
|
self.swap_unchecked_linear(lid1, lid2)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A matrix storage that does not contain any reference and that is stored contiguously in memory.
|
||||||
|
///
|
||||||
|
/// The storage requirement means that for any value of `i` in `[0, nrows * ncols[`, the value
|
||||||
|
/// `.get_unchecked_linear` succeeds. This trait is unsafe because failing to comply to this may
|
||||||
|
/// cause Undefined Behaviors.
|
||||||
|
pub unsafe trait OwnedStorage<N: Scalar, R: Dim, C: Dim>: StorageMut<N, R, C> + Clone + Any
|
||||||
|
where Self::Alloc: Allocator<N, R, C, Buffer = Self> {
|
||||||
|
// NOTE: We could auto-impl those two methods but we don't to make sure the user is aware that
|
||||||
|
// data must be contiguous.
|
||||||
|
/// Converts this data storage to a slice.
|
||||||
|
#[inline]
|
||||||
|
fn as_slice(&self) -> &[N];
|
||||||
|
|
||||||
|
/// Converts this data storage to a mutable slice.
|
||||||
|
#[inline]
|
||||||
|
fn as_mut_slice(&mut self) -> &mut [N];
|
||||||
|
}
|
165
src/core/unit.rs
Normal file
165
src/core/unit.rs
Normal file
@ -0,0 +1,165 @@
|
|||||||
|
use std::ops::Neg;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::SubsetOf;
|
||||||
|
use alga::linear::NormedSpace;
|
||||||
|
|
||||||
|
|
||||||
|
/// A wrapper that ensures the undelying algebraic entity has a unit norm.
|
||||||
|
///
|
||||||
|
/// Use `.as_ref()` or `.unwrap()` to obtain the undelying value by-reference or by-move.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Eq, PartialEq, Clone, Hash, Debug, Copy)]
|
||||||
|
pub struct Unit<T> {
|
||||||
|
value: T
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: NormedSpace> Unit<T> {
|
||||||
|
/// Normalize the given value and return it wrapped on a `Unit` structure.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_normalize(value: T) -> Self {
|
||||||
|
Self::new_and_get(value).0
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Attempts to normalize the given value and return it wrapped on a `Unit` structure.
|
||||||
|
///
|
||||||
|
/// Returns `None` if the norm was smaller or equal to `min_norm`.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_new(value: T, min_norm: T::Field) -> Option<Self> {
|
||||||
|
Self::try_new_and_get(value, min_norm).map(|res| res.0)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Normalize the given value and return it wrapped on a `Unit` structure and its norm.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_and_get(mut value: T) -> (Self, T::Field) {
|
||||||
|
let n = value.normalize_mut();
|
||||||
|
|
||||||
|
(Unit { value: value }, n)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Normalize the given value and return it wrapped on a `Unit` structure and its norm.
|
||||||
|
///
|
||||||
|
/// Returns `None` if the norm was smaller or equal to `min_norm`.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_new_and_get(mut value: T, min_norm: T::Field) -> Option<(Self, T::Field)> {
|
||||||
|
if let Some(n) = value.try_normalize_mut(min_norm) {
|
||||||
|
Some((Unit { value: value }, n))
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Normalizes this value again. This is useful when repeated computations
|
||||||
|
/// might cause a drift in the norm because of float inaccuracies.
|
||||||
|
///
|
||||||
|
/// Returns the norm before re-normalization (should be close to `1.0`).
|
||||||
|
#[inline]
|
||||||
|
pub fn renormalize(&mut self) -> T::Field {
|
||||||
|
self.value.normalize_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> Unit<T> {
|
||||||
|
/// Wraps the given value, assuming it is already normalized.
|
||||||
|
///
|
||||||
|
/// This function is not safe because `value` is not verified to be actually normalized.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_unchecked(value: T) -> Self {
|
||||||
|
Unit { value: value }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves the underlying value.
|
||||||
|
#[inline]
|
||||||
|
pub fn unwrap(self) -> T {
|
||||||
|
self.value
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Returns a mutable reference to the underlying value. This is `_unchecked` because modifying
|
||||||
|
/// the underlying value in such a way that it no longer has unit length may lead to unexpected
|
||||||
|
/// results.
|
||||||
|
#[inline]
|
||||||
|
pub fn as_mut_unchecked(&mut self) -> &mut T {
|
||||||
|
&mut self.value
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> AsRef<T> for Unit<T> {
|
||||||
|
#[inline]
|
||||||
|
fn as_ref(&self) -> &T {
|
||||||
|
&self.value
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Conversions.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<T: NormedSpace> SubsetOf<T> for Unit<T>
|
||||||
|
where T::Field: ApproxEq {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> T {
|
||||||
|
self.clone().unwrap()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(value: &T) -> bool {
|
||||||
|
relative_eq!(value.norm_squared(), ::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(value: &T) -> Self {
|
||||||
|
Unit::new_normalize(value.clone()) // We still need to re-normalize because the condition is inexact.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: ApproxEq> ApproxEq for Unit<T> {
|
||||||
|
type Epsilon = T::Epsilon;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
T::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
T::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
T::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.value.relative_eq(&other.value, epsilon, max_relative)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.value.ulps_eq(&other.value, epsilon, max_ulps)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// FIXME:re-enable this impl when spacialization is possible.
|
||||||
|
// Currently, it is disabled so that we can have a nice output for the `UnitQuaternion` display.
|
||||||
|
/*
|
||||||
|
impl<T: fmt::Display> fmt::Display for Unit<T> {
|
||||||
|
// XXX: will not always work correctly due to rounding errors.
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
self.value.fmt(f)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
impl<T: Neg> Neg for Unit<T> {
|
||||||
|
type Output = Unit<T::Output>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
Unit::new_unchecked(-self.value)
|
||||||
|
}
|
||||||
|
}
|
195
src/geometry/isometry.rs
Normal file
195
src/geometry/isometry.rs
Normal file
@ -0,0 +1,195 @@
|
|||||||
|
use std::fmt;
|
||||||
|
use std::marker::PhantomData;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{Real, SubsetOf};
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::{Scalar, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use geometry::{TranslationBase, PointBase};
|
||||||
|
|
||||||
|
/// A direct isometry, i.e., a rotation followed by a translation.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Hash, Debug, Clone, Copy)]
|
||||||
|
pub struct IsometryBase<N: Scalar, D: DimName, S, R> {
|
||||||
|
pub rotation: R,
|
||||||
|
pub translation: TranslationBase<N, D, S>,
|
||||||
|
// One private field just to prevent explicit construction.
|
||||||
|
_noconstruct: PhantomData<N>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new isometry from its rotational and translational parts.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_parts(translation: TranslationBase<N, D, S>, rotation: R) -> IsometryBase<N, D, S, R> {
|
||||||
|
IsometryBase {
|
||||||
|
rotation: rotation,
|
||||||
|
translation: translation,
|
||||||
|
_noconstruct: PhantomData
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(&self) -> IsometryBase<N, D, S, R> {
|
||||||
|
let mut res = self.clone();
|
||||||
|
res.inverse_mut();
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self) {
|
||||||
|
self.rotation.inverse_mut();
|
||||||
|
self.translation.inverse_mut();
|
||||||
|
self.translation.vector = self.rotation.transform_vector(&self.translation.vector);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends to `self` the given translation in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_translation_mut(&mut self, t: &TranslationBase<N, D, S>) {
|
||||||
|
self.translation.vector += &t.vector
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends to `self` the given rotation in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_rotation_mut(&mut self, r: &R) {
|
||||||
|
self.rotation = self.rotation.append_rotation(&r);
|
||||||
|
self.translation.vector = r.transform_vector(&self.translation.vector);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends in-place to `self` a rotation centered at the point `p`, i.e., the rotation that
|
||||||
|
/// lets `p` invariant.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_rotation_wrt_point_mut(&mut self, r: &R, p: &PointBase<N, D, S>) {
|
||||||
|
self.translation.vector -= &p.coords;
|
||||||
|
self.append_rotation_mut(r);
|
||||||
|
self.translation.vector += &p.coords;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends in-place to `self` a rotation centered at the point with coordinates
|
||||||
|
/// `self.translation`.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_rotation_wrt_center_mut(&mut self, r: &R) {
|
||||||
|
let center = PointBase::from_coordinates(self.translation.vector.clone());
|
||||||
|
self.append_rotation_wrt_point_mut(r, ¢er)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// NOTE: we don't require `R: Rotation<...>` here because this is not useful for the implementation
|
||||||
|
// and makes it hard to use it, e.g., for Transform × Isometry implementation.
|
||||||
|
// This is OK since all constructors of the isometry enforce the Rotation bound already (and
|
||||||
|
// explicit struct construction is prevented by the dummy ZST field).
|
||||||
|
impl<N, D: DimName, S, R> IsometryBase<N, D, S, R>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
/// Converts this isometry into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc>
|
||||||
|
where D: DimNameAdd<U1>,
|
||||||
|
R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc>>,
|
||||||
|
S::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
let mut res: OwnedSquareMatrix<N, _, S::Alloc> = ::convert_ref(&self.rotation);
|
||||||
|
res.fixed_slice_mut::<D, U1>(0, D::dim()).copy_from(&self.translation.vector);
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Eq for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + Eq,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> PartialEq for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + PartialEq,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &IsometryBase<N, D, S, R>) -> bool {
|
||||||
|
self.translation == right.translation &&
|
||||||
|
self.rotation == right.rotation
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> ApproxEq for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + ApproxEq<Epsilon = N::Epsilon>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
type Epsilon = N::Epsilon;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
N::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
N::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
N::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.translation.relative_eq(&other.translation, epsilon, max_relative) &&
|
||||||
|
self.rotation.relative_eq(&other.rotation, epsilon, max_relative)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.translation.ulps_eq(&other.translation, epsilon, max_ulps) &&
|
||||||
|
self.rotation.ulps_eq(&other.rotation, epsilon, max_ulps)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S, R> fmt::Display for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real + fmt::Display,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: fmt::Display,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> + Allocator<usize, D, U1> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
let precision = f.precision().unwrap_or(3);
|
||||||
|
|
||||||
|
try!(writeln!(f, "IsometryBase {{"));
|
||||||
|
try!(write!(f, "{:.*}", precision, self.translation));
|
||||||
|
try!(write!(f, "{:.*}", precision, self.rotation));
|
||||||
|
writeln!(f, "}}")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// /*
|
||||||
|
// *
|
||||||
|
// * Absolute
|
||||||
|
// *
|
||||||
|
// */
|
||||||
|
// impl<N: Absolute> Absolute for $t<N> {
|
||||||
|
// type AbsoluteValue = $submatrix<N::AbsoluteValue>;
|
||||||
|
//
|
||||||
|
// #[inline]
|
||||||
|
// fn abs(m: &$t<N>) -> $submatrix<N::AbsoluteValue> {
|
||||||
|
// Absolute::abs(&m.submatrix)
|
||||||
|
// }
|
||||||
|
// }
|
198
src/geometry/isometry_alga.rs
Normal file
198
src/geometry/isometry_alga.rs
Normal file
@ -0,0 +1,198 @@
|
|||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractLoop, AbstractMonoid, AbstractQuasigroup,
|
||||||
|
AbstractSemigroup, Real, Inverse, Multiplicative, Identity, Id};
|
||||||
|
use alga::linear::{Transformation, Similarity, AffineTransformation, DirectIsometry, Isometry,
|
||||||
|
Rotation, ProjectiveTransformation};
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{IsometryBase, TranslationBase, PointBase};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Algebraic structures.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S, R> Identity<Multiplicative> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Inverse<Multiplicative> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.inverse_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> AbstractMagma<Multiplicative> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_multiplicative_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimName, S, R> $marker<$operator> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_multiplicative_structures!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Transformation groups.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S, R> Transformation<PointBase<N, D, S>> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
self * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, D, S>) -> ColumnVector<N, D, S> {
|
||||||
|
self * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> ProjectiveTransformation<PointBase<N, D, S>> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
self.rotation.inverse_transform_point(&(pt - &self.translation.vector))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, D, S>) -> ColumnVector<N, D, S> {
|
||||||
|
self.rotation.inverse_transform_vector(v)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> AffineTransformation<PointBase<N, D, S>> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Rotation = R;
|
||||||
|
type NonUniformScaling = Id;
|
||||||
|
type Translation = TranslationBase<N, D, S>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn decompose(&self) -> (TranslationBase<N, D, S>, R, Id, R) {
|
||||||
|
(self.translation.clone(), self.rotation.clone(), Id::new(), R::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_translation(&self, t: &Self::Translation) -> Self {
|
||||||
|
t * self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_translation(&self, t: &Self::Translation) -> Self {
|
||||||
|
self * t
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
let shift = r.transform_vector(&self.translation.vector);
|
||||||
|
IsometryBase::from_parts(TranslationBase::from_vector(shift), r.clone() * self.rotation.clone())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
self * r
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation_wrt_point(&self, r: &Self::Rotation, p: &PointBase<N, D, S>) -> Option<Self> {
|
||||||
|
let mut res = self.clone();
|
||||||
|
res.append_rotation_wrt_point_mut(r, p);
|
||||||
|
Some(res)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Similarity<PointBase<N, D, S>> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Scaling = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation(&self) -> TranslationBase<N, D, S> {
|
||||||
|
self.translation.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation(&self) -> R {
|
||||||
|
self.rotation.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaling(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! marker_impl(
|
||||||
|
($($Trait: ident),*) => {$(
|
||||||
|
impl<N, D: DimName, S, R> $Trait<PointBase<N, D, S>> for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
marker_impl!(Isometry, DirectIsometry);
|
19
src/geometry/isometry_alias.rs
Normal file
19
src/geometry/isometry_alias.rs
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U1, U2, U3};
|
||||||
|
|
||||||
|
use geometry::{Rotation, IsometryBase, UnitQuaternion, UnitComplex};
|
||||||
|
|
||||||
|
/// A D-dimensional isometry.
|
||||||
|
pub type Isometry<N, D> = IsometryBase<N, D, MatrixArray<N, D, U1>, Rotation<N, D>>;
|
||||||
|
|
||||||
|
/// A 2-dimensional isometry using a unit complex number for its rotational part.
|
||||||
|
pub type Isometry2<N> = IsometryBase<N, U2, MatrixArray<N, U2, U1>, UnitComplex<N>>;
|
||||||
|
|
||||||
|
/// A 3-dimensional isometry using a unit quaternion for its rotational part.
|
||||||
|
pub type Isometry3<N> = IsometryBase<N, U3, MatrixArray<N, U3, U1>, UnitQuaternion<N>>;
|
||||||
|
|
||||||
|
/// A 2-dimensional isometry using a rotation matrix for its rotation part.
|
||||||
|
pub type IsometryMatrix2<N> = Isometry<N, U2>;
|
||||||
|
|
||||||
|
/// A 3-dimensional isometry using a rotation matrix for its rotation part.
|
||||||
|
pub type IsometryMatrix3<N> = Isometry<N, U3>;
|
195
src/geometry/isometry_construction.rs
Normal file
195
src/geometry/isometry_construction.rs
Normal file
@ -0,0 +1,195 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use num::One;
|
||||||
|
use rand::{Rng, Rand};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use alga::linear::Rotation as AlgaRotation;
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1, U2, U3, U4};
|
||||||
|
use core::allocator::{OwnedAllocator, Allocator};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
|
||||||
|
use geometry::{PointBase, TranslationBase, RotationBase, IsometryBase, UnitQuaternionBase, UnitComplex};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new identity isometry.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> Self {
|
||||||
|
Self::from_parts(TranslationBase::identity(), R::identity())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> One for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new identity isometry.
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Rand for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>> + Rand,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<G: Rng>(rng: &mut G) -> Self {
|
||||||
|
Self::from_parts(rng.gen(), rng.gen())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> IsometryBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// The isometry that applies the rotation `r` with its axis passing through the point `p`.
|
||||||
|
/// This effectively lets `p` invariant.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_wrt_point(r: R, p: PointBase<N, D, S>) -> Self {
|
||||||
|
let shift = r.transform_vector(&-&p.coords);
|
||||||
|
Self::from_parts(TranslationBase::from_vector(shift + p.coords), r)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
impl<N, D: DimName, S, R> Arbitrary for IsometryBase<N, D, S, R>
|
||||||
|
where N: Real + Arbitrary + Send,
|
||||||
|
S: OwnedStorage<N, D, U1> + Send,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>> + Arbitrary + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(rng: &mut G) -> Self {
|
||||||
|
Self::from_parts(Arbitrary::arbitrary(rng), Arbitrary::arbitrary(rng))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Constructors for various static dimensions.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
// 2D rotation.
|
||||||
|
impl<N, S, SR> IsometryBase<N, U2, S, RotationBase<N, U2, SR>>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1, Alloc = SR::Alloc>,
|
||||||
|
SR: OwnedStorage<N, U2, U2>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S>,
|
||||||
|
SR::Alloc: OwnedAllocator<N, U2, U2, SR> {
|
||||||
|
/// Creates a new isometry from a translation and a rotation angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(translation: ColumnVector<N, U2, S>, angle: N) -> Self {
|
||||||
|
Self::from_parts(TranslationBase::from_vector(translation), RotationBase::<N, U2, SR>::new(angle))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> IsometryBase<N, U2, S, UnitComplex<N>>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> {
|
||||||
|
/// Creates a new isometry from a translation and a rotation angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(translation: ColumnVector<N, U2, S>, angle: N) -> Self {
|
||||||
|
Self::from_parts(TranslationBase::from_vector(translation), UnitComplex::from_angle(angle))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3D rotation.
|
||||||
|
macro_rules! isometry_construction_impl(
|
||||||
|
($RotId: ident < $($RotParams: ident),*>, $RRDim: ty, $RCDim: ty) => {
|
||||||
|
impl<N, S, SR> IsometryBase<N, U3, S, $RotId<$($RotParams),*>>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U3, U1, Alloc = SR::Alloc>,
|
||||||
|
SR: OwnedStorage<N, $RRDim, $RCDim>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U1, S>,
|
||||||
|
SR::Alloc: OwnedAllocator<N, $RRDim, $RCDim, SR> +
|
||||||
|
Allocator<N, U3, U3> {
|
||||||
|
/// Creates a new isometry from a translation and a rotation axis-angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(translation: ColumnVector<N, U3, S>, axisangle: ColumnVector<N, U3, S>) -> Self {
|
||||||
|
Self::from_parts(
|
||||||
|
TranslationBase::from_vector(translation),
|
||||||
|
$RotId::<$($RotParams),*>::from_scaled_axis(axisangle))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates an isometry that corresponds to the local frame of an observer standing at the
|
||||||
|
/// point `eye` and looking toward `target`.
|
||||||
|
///
|
||||||
|
/// It maps the view direction `target - eye` to the positive `z` axis and the origin to the
|
||||||
|
/// `eye`.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The observer position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - Vertical direction. The only requirement of this parameter is to not be collinear
|
||||||
|
/// to `eye - at`. Non-collinearity is not checked.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_observer_frame(eye: &PointBase<N, U3, S>,
|
||||||
|
target: &PointBase<N, U3, S>,
|
||||||
|
up: &ColumnVector<N, U3, S>)
|
||||||
|
-> Self {
|
||||||
|
Self::from_parts(
|
||||||
|
TranslationBase::from_vector(eye.coords.clone()),
|
||||||
|
$RotId::new_observer_frame(&(target - eye), up))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a right-handed look-at view matrix.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of right handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_rh(eye: &PointBase<N, U3, S>,
|
||||||
|
target: &PointBase<N, U3, S>,
|
||||||
|
up: &ColumnVector<N, U3, S>)
|
||||||
|
-> Self {
|
||||||
|
let rotation = $RotId::look_at_rh(&(target - eye), up);
|
||||||
|
let trans = &rotation * (-eye);
|
||||||
|
|
||||||
|
Self::from_parts(TranslationBase::from_vector(trans.coords), rotation)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a left-handed look-at view matrix.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of left handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_lh(eye: &PointBase<N, U3, S>,
|
||||||
|
target: &PointBase<N, U3, S>,
|
||||||
|
up: &ColumnVector<N, U3, S>)
|
||||||
|
-> Self {
|
||||||
|
let rotation = $RotId::look_at_lh(&(target - eye), up);
|
||||||
|
let trans = &rotation * (-eye);
|
||||||
|
|
||||||
|
Self::from_parts(TranslationBase::from_vector(trans.coords), rotation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
isometry_construction_impl!(RotationBase<N, U3, SR>, U3, U3);
|
||||||
|
isometry_construction_impl!(UnitQuaternionBase<N, SR>, U4, U1);
|
163
src/geometry/isometry_conversion.rs
Normal file
163
src/geometry/isometry_conversion.rs
Normal file
@ -0,0 +1,163 @@
|
|||||||
|
use alga::general::{Real, SubsetOf, SupersetOf};
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::{SquareMatrix, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, TranslationBase, IsometryBase, SimilarityBase, TransformBase, SuperTCategoryOf, TAffine};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides the following conversions:
|
||||||
|
* =============================================
|
||||||
|
*
|
||||||
|
* IsometryBase -> IsometryBase
|
||||||
|
* IsometryBase -> SimilarityBase
|
||||||
|
* IsometryBase -> TransformBase
|
||||||
|
* IsometryBase -> Matrix (homogeneous)
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R1, R2> SubsetOf<IsometryBase<N2, D, SB, R2>> for IsometryBase<N1, D, SA, R1>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
R1: Rotation<PointBase<N1, D, SA>> + SubsetOf<R2>,
|
||||||
|
R2: Rotation<PointBase<N2, D, SB>>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> IsometryBase<N2, D, SB, R2> {
|
||||||
|
IsometryBase::from_parts(
|
||||||
|
self.translation.to_superset(),
|
||||||
|
self.rotation.to_superset()
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(iso: &IsometryBase<N2, D, SB, R2>) -> bool {
|
||||||
|
::is_convertible::<_, TranslationBase<N1, D, SA>>(&iso.translation) &&
|
||||||
|
::is_convertible::<_, R1>(&iso.rotation)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(iso: &IsometryBase<N2, D, SB, R2>) -> Self {
|
||||||
|
IsometryBase::from_parts(
|
||||||
|
iso.translation.to_subset_unchecked(),
|
||||||
|
iso.rotation.to_subset_unchecked()
|
||||||
|
)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R1, R2> SubsetOf<SimilarityBase<N2, D, SB, R2>> for IsometryBase<N1, D, SA, R1>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
R1: Rotation<PointBase<N1, D, SA>> + SubsetOf<R2>,
|
||||||
|
R2: Rotation<PointBase<N2, D, SB>>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SimilarityBase<N2, D, SB, R2> {
|
||||||
|
SimilarityBase::from_isometry(
|
||||||
|
self.to_superset(),
|
||||||
|
N2::one()
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(sim: &SimilarityBase<N2, D, SB, R2>) -> bool {
|
||||||
|
::is_convertible::<_, IsometryBase<N1, D, SA, R1>>(&sim.isometry) &&
|
||||||
|
sim.scaling() == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(sim: &SimilarityBase<N2, D, SB, R2>) -> Self {
|
||||||
|
::convert_ref_unchecked(&sim.isometry)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB, R, C> SubsetOf<TransformBase<N2, D, SB, C>> for IsometryBase<N1, D, SA, R>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: SuperTCategoryOf<TAffine>,
|
||||||
|
R: Rotation<PointBase<N1, D, SA>> +
|
||||||
|
SubsetOf<OwnedSquareMatrix<N1, DimNameSum<D, U1>, SA::Alloc>> + // needed by: .to_homogeneous()
|
||||||
|
SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>>, // needed by: ::convert_unchecked(mm)
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, D, D> + // needed by R
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>> + // needed by: .to_homogeneous()
|
||||||
|
Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>, // needed by R
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, D> + // needed by: mm.fixed_slice_mut
|
||||||
|
Allocator<N2, D, U1> + // needed by: m.fixed_slice
|
||||||
|
Allocator<N2, U1, D> { // needed by: m.fixed_slice
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TransformBase<N2, D, SB, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(t: &TransformBase<N2, D, SB, C>) -> bool {
|
||||||
|
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(t: &TransformBase<N2, D, SB, C>) -> Self {
|
||||||
|
Self::from_superset_unchecked(t.matrix())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB, R> SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>> for IsometryBase<N1, D, SA, R>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
R: Rotation<PointBase<N1, D, SA>> +
|
||||||
|
SubsetOf<OwnedSquareMatrix<N1, DimNameSum<D, U1>, SA::Alloc>> + // needed by: .to_homogeneous()
|
||||||
|
SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>>, // needed by: ::convert_unchecked(mm)
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, D, D> + // needed by R
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>> + // needed by: .to_homogeneous()
|
||||||
|
Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>, // needed by R
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, D> + // needed by: mm.fixed_slice_mut
|
||||||
|
Allocator<N2, D, U1> + // needed by: m.fixed_slice
|
||||||
|
Allocator<N2, U1, D> { // needed by: m.fixed_slice
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SquareMatrix<N2, DimNameSum<D, U1>, SB> {
|
||||||
|
self.to_homogeneous().to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> bool {
|
||||||
|
let rot = m.fixed_slice::<D, D>(0, 0);
|
||||||
|
let bottom = m.fixed_slice::<U1, D>(D::dim(), 0);
|
||||||
|
|
||||||
|
// Scalar types agree.
|
||||||
|
m.iter().all(|e| SupersetOf::<N1>::is_in_subset(e)) &&
|
||||||
|
// The block part is a rotation.
|
||||||
|
rot.is_special_orthogonal(N2::default_epsilon() * ::convert(100.0)) &&
|
||||||
|
// The bottom row is (0, 0, ..., 1)
|
||||||
|
bottom.iter().all(|e| e.is_zero()) &&
|
||||||
|
m[(D::dim(), D::dim())] == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> Self {
|
||||||
|
let t = m.fixed_slice::<D, U1>(0, D::dim()).into_owned();
|
||||||
|
let t = TranslationBase::from_vector(::convert_unchecked(t));
|
||||||
|
|
||||||
|
Self::from_parts(t, ::convert_unchecked(m.clone_owned()))
|
||||||
|
}
|
||||||
|
}
|
435
src/geometry/isometry_ops.rs
Normal file
435
src/geometry/isometry_ops.rs
Normal file
@ -0,0 +1,435 @@
|
|||||||
|
use std::ops::{Mul, MulAssign, Div, DivAssign};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1, U3, U4};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{PointBase, RotationBase, IsometryBase, TranslationBase, UnitQuaternionBase};
|
||||||
|
|
||||||
|
// FIXME: there are several cloning of rotations that we could probably get rid of (but we didn't
|
||||||
|
// yet because that would require to add a bound like `where for<'a, 'b> &'a R: Mul<&'b R, Output = R>`
|
||||||
|
// which is quite ugly.
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* In this file, we provide:
|
||||||
|
* =========================
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Operators)
|
||||||
|
*
|
||||||
|
* IsometryBase × IsometryBase
|
||||||
|
* IsometryBase × R
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* IsometryBase ÷ IsometryBase
|
||||||
|
* IsometryBase ÷ R
|
||||||
|
*
|
||||||
|
* IsometryBase × PointBase
|
||||||
|
* IsometryBase × ColumnVector
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* IsometryBase × TranslationBase
|
||||||
|
* TranslationBase × IsometryBase
|
||||||
|
* TranslationBase × R -> IsometryBase<R>
|
||||||
|
*
|
||||||
|
* NOTE: The following are provided explicitly because we can't have R × IsometryBase.
|
||||||
|
* RotationBase × IsometryBase<RotationBase>
|
||||||
|
* UnitQuaternion × IsometryBase<UnitQuaternion>
|
||||||
|
*
|
||||||
|
* RotationBase ÷ IsometryBase<RotationBase>
|
||||||
|
* UnitQuaternion ÷ IsometryBase<UnitQuaternion>
|
||||||
|
*
|
||||||
|
* RotationBase × TranslationBase -> IsometryBase<RotationBase>
|
||||||
|
* UnitQuaternion × TranslationBase -> IsometryBase<UnitQuaternion>
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Assignment Operators)
|
||||||
|
*
|
||||||
|
* IsometryBase ×= TranslationBase
|
||||||
|
*
|
||||||
|
* IsometryBase ×= IsometryBase
|
||||||
|
* IsometryBase ×= R
|
||||||
|
*
|
||||||
|
* IsometryBase ÷= IsometryBase
|
||||||
|
* IsometryBase ÷= R
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
macro_rules! isometry_binop_impl(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N, D: DimName, S, R> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Output = $Output;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! isometry_binop_impl_all(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
[val val] => $action_val_val: expr;
|
||||||
|
[ref val] => $action_ref_val: expr;
|
||||||
|
[val ref] => $action_val_ref: expr;
|
||||||
|
[ref ref] => $action_ref_ref: expr;) => {
|
||||||
|
isometry_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_val_val; );
|
||||||
|
|
||||||
|
isometry_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_ref_val; 'a);
|
||||||
|
|
||||||
|
isometry_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_val_ref; 'b);
|
||||||
|
|
||||||
|
isometry_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_ref_ref; 'a, 'b);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! isometry_binop_assign_impl_all(
|
||||||
|
($OpAssign: ident, $op_assign: ident;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
|
||||||
|
[val] => $action_val: expr;
|
||||||
|
[ref] => $action_ref: expr;) => {
|
||||||
|
impl<N, D: DimName, S, R> $OpAssign<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn $op_assign(&mut $lhs, $rhs: $Rhs) {
|
||||||
|
$action_val
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N, D: DimName, S, R> $OpAssign<&'b $Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) {
|
||||||
|
$action_ref
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// IsometryBase × IsometryBase
|
||||||
|
// IsometryBase ÷ IsometryBase
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => {
|
||||||
|
let shift = self.rotation.transform_vector(&rhs.translation.vector);
|
||||||
|
|
||||||
|
IsometryBase::from_parts(TranslationBase::from_vector(&self.translation.vector + shift),
|
||||||
|
self.rotation.clone() * rhs.rotation.clone()) // FIXME: too bad we have to clone.
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// IsometryBase ×= TranslationBase
|
||||||
|
isometry_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: TranslationBase<N, D, S>;
|
||||||
|
[val] => *self *= &rhs;
|
||||||
|
[ref] => {
|
||||||
|
let shift = self.rotation.transform_vector(&rhs.vector);
|
||||||
|
self.translation.vector += shift;
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
// IsometryBase ×= IsometryBase
|
||||||
|
// IsometryBase ÷= IsometryBase
|
||||||
|
isometry_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>;
|
||||||
|
[val] => *self *= &rhs;
|
||||||
|
[ref] => {
|
||||||
|
let shift = self.rotation.transform_vector(&rhs.translation.vector);
|
||||||
|
self.translation.vector += shift;
|
||||||
|
self.rotation *= rhs.rotation.clone();
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
isometry_binop_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>;
|
||||||
|
[val] => *self /= &rhs;
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
// IsometryBase ×= R
|
||||||
|
// IsometryBase ÷= R
|
||||||
|
isometry_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: R;
|
||||||
|
[val] => self.rotation *= rhs;
|
||||||
|
[ref] => self.rotation *= rhs.clone();
|
||||||
|
);
|
||||||
|
|
||||||
|
isometry_binop_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: R;
|
||||||
|
// FIXME: don't invert explicitly?
|
||||||
|
[val] => *self *= rhs.inverse();
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// IsometryBase × RotationBase
|
||||||
|
// IsometryBase ÷ RotationBase
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: R, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => IsometryBase::from_parts(self.translation, self.rotation * rhs);
|
||||||
|
[ref val] => IsometryBase::from_parts(self.translation.clone(), self.rotation.clone() * rhs); // FIXME: do not clone.
|
||||||
|
[val ref] => IsometryBase::from_parts(self.translation, self.rotation * rhs.clone());
|
||||||
|
[ref ref] => IsometryBase::from_parts(self.translation.clone(), self.rotation.clone() * rhs.clone());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: R, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => IsometryBase::from_parts(self.translation, self.rotation / rhs);
|
||||||
|
[ref val] => IsometryBase::from_parts(self.translation.clone(), self.rotation.clone() / rhs);
|
||||||
|
[val ref] => IsometryBase::from_parts(self.translation, self.rotation / rhs.clone());
|
||||||
|
[ref ref] => IsometryBase::from_parts(self.translation.clone(), self.rotation.clone() / rhs.clone());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// IsometryBase × PointBase
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: IsometryBase<N, D, S, R>, right: PointBase<N, D, S>, Output = PointBase<N, D, S>;
|
||||||
|
[val val] => self.translation * self.rotation.transform_point(&right);
|
||||||
|
[ref val] => &self.translation * self.rotation.transform_point(&right);
|
||||||
|
[val ref] => self.translation * self.rotation.transform_point(right);
|
||||||
|
[ref ref] => &self.translation * self.rotation.transform_point(right);
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// IsometryBase × Vector
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: IsometryBase<N, D, S, R>, right: ColumnVector<N, D, S>, Output = ColumnVector<N, D, S>;
|
||||||
|
[val val] => self.rotation.transform_vector(&right);
|
||||||
|
[ref val] => self.rotation.transform_vector(&right);
|
||||||
|
[val ref] => self.rotation.transform_vector(right);
|
||||||
|
[ref ref] => self.rotation.transform_vector(right);
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// IsometryBase × TranslationBase
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: IsometryBase<N, D, S, R>, right: TranslationBase<N, D, S>, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => &self * &right;
|
||||||
|
[ref val] => self * &right;
|
||||||
|
[val ref] => &self * right;
|
||||||
|
[ref ref] => {
|
||||||
|
let new_tr = &self.translation.vector + self.rotation.transform_vector(&right.vector);
|
||||||
|
IsometryBase::from_parts(TranslationBase::from_vector(new_tr), self.rotation.clone())
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
// TranslationBase × IsometryBase
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: TranslationBase<N, D, S>, right: IsometryBase<N, D, S, R>, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => IsometryBase::from_parts(self * right.translation, right.rotation);
|
||||||
|
[ref val] => IsometryBase::from_parts(self * &right.translation, right.rotation);
|
||||||
|
[val ref] => IsometryBase::from_parts(self * &right.translation, right.rotation.clone());
|
||||||
|
[ref ref] => IsometryBase::from_parts(self * &right.translation, right.rotation.clone());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TranslationBase × R
|
||||||
|
isometry_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: TranslationBase<N, D, S>, right: R, Output = IsometryBase<N, D, S, R>;
|
||||||
|
[val val] => IsometryBase::from_parts(self, right);
|
||||||
|
[ref val] => IsometryBase::from_parts(self.clone(), right);
|
||||||
|
[val ref] => IsometryBase::from_parts(self, right.clone());
|
||||||
|
[ref ref] => IsometryBase::from_parts(self.clone(), right.clone());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
macro_rules! isometry_from_composition_impl(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N $(, $Dims: $DimsBound)*, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, $R1, $C1>,
|
||||||
|
SB: OwnedStorage<N, $R2, $C2, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, $R1, $C1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, $R2, $C2, SB> {
|
||||||
|
type Output = $Output;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! isometry_from_composition_impl_all(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
[val val] => $action_val_val: expr;
|
||||||
|
[ref val] => $action_ref_val: expr;
|
||||||
|
[val ref] => $action_val_ref: expr;
|
||||||
|
[ref ref] => $action_ref_ref: expr;) => {
|
||||||
|
|
||||||
|
isometry_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_val_val; );
|
||||||
|
|
||||||
|
isometry_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_ref_val; 'a);
|
||||||
|
|
||||||
|
isometry_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_val_ref; 'b);
|
||||||
|
|
||||||
|
isometry_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_ref_ref; 'a, 'b);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase × TranslationBase
|
||||||
|
isometry_from_composition_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(D, D), (D, U1) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: TranslationBase<N, D, SB>, Output = IsometryBase<N, D, SB, RotationBase<N, D, SA>>;
|
||||||
|
[val val] => IsometryBase::from_parts(TranslationBase::from_vector(&self * right.vector), self);
|
||||||
|
[ref val] => IsometryBase::from_parts(TranslationBase::from_vector(self * right.vector), self.clone());
|
||||||
|
[val ref] => IsometryBase::from_parts(TranslationBase::from_vector(&self * &right.vector), self);
|
||||||
|
[ref ref] => IsometryBase::from_parts(TranslationBase::from_vector(self * &right.vector), self.clone());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternionBase × TranslationBase
|
||||||
|
isometry_from_composition_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, right: TranslationBase<N, U3, SB>,
|
||||||
|
Output = IsometryBase<N, U3, SB, UnitQuaternionBase<N, SA>>;
|
||||||
|
[val val] => IsometryBase::from_parts(TranslationBase::from_vector(&self * right.vector), self);
|
||||||
|
[ref val] => IsometryBase::from_parts(TranslationBase::from_vector(self * right.vector), self.clone());
|
||||||
|
[val ref] => IsometryBase::from_parts(TranslationBase::from_vector(&self * &right.vector), self);
|
||||||
|
[ref ref] => IsometryBase::from_parts(TranslationBase::from_vector(self * &right.vector), self.clone());
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase × IsometryBase
|
||||||
|
isometry_from_composition_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(D, D), (D, U1) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: IsometryBase<N, D, SB, RotationBase<N, D, SA>>,
|
||||||
|
Output = IsometryBase<N, D, SB, RotationBase<N, D, SA>>;
|
||||||
|
[val val] => &self * &right;
|
||||||
|
[ref val] => self * &right;
|
||||||
|
[val ref] => &self * right;
|
||||||
|
[ref ref] => {
|
||||||
|
let shift = self * &right.translation.vector;
|
||||||
|
IsometryBase::from_parts(TranslationBase::from_vector(shift), self * &right.rotation)
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase ÷ IsometryBase
|
||||||
|
isometry_from_composition_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(D, D), (D, U1) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: IsometryBase<N, D, SB, RotationBase<N, D, SA>>,
|
||||||
|
Output = IsometryBase<N, D, SB, RotationBase<N, D, SA>>;
|
||||||
|
// FIXME: don't call iverse explicitly?
|
||||||
|
[val val] => self * right.inverse();
|
||||||
|
[ref val] => self * right.inverse();
|
||||||
|
[val ref] => self * right.inverse();
|
||||||
|
[ref ref] => self * right.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternion × IsometryBase
|
||||||
|
isometry_from_composition_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, right: IsometryBase<N, U3, SB, UnitQuaternionBase<N, SA>>,
|
||||||
|
Output = IsometryBase<N, U3, SB, UnitQuaternionBase<N, SA>>;
|
||||||
|
[val val] => &self * &right;
|
||||||
|
[ref val] => self * &right;
|
||||||
|
[val ref] => &self * right;
|
||||||
|
[ref ref] => {
|
||||||
|
let shift = self * &right.translation.vector;
|
||||||
|
IsometryBase::from_parts(TranslationBase::from_vector(shift), self * &right.rotation)
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternion ÷ IsometryBase
|
||||||
|
isometry_from_composition_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, right: IsometryBase<N, U3, SB, UnitQuaternionBase<N, SA>>,
|
||||||
|
Output = IsometryBase<N, U3, SB, UnitQuaternionBase<N, SA>>;
|
||||||
|
// FIXME: don't call inverse explicitly?
|
||||||
|
[val val] => self * right.inverse();
|
||||||
|
[ref val] => self * right.inverse();
|
||||||
|
[val ref] => self * right.inverse();
|
||||||
|
[ref ref] => self * right.inverse();
|
||||||
|
);
|
87
src/geometry/mod.rs
Normal file
87
src/geometry/mod.rs
Normal file
@ -0,0 +1,87 @@
|
|||||||
|
mod op_macros;
|
||||||
|
|
||||||
|
mod point;
|
||||||
|
mod point_construction;
|
||||||
|
mod point_alias;
|
||||||
|
mod point_ops;
|
||||||
|
mod point_alga;
|
||||||
|
mod point_conversion;
|
||||||
|
mod point_coordinates;
|
||||||
|
|
||||||
|
mod rotation;
|
||||||
|
mod rotation_construction;
|
||||||
|
mod rotation_ops;
|
||||||
|
mod rotation_alga; // FIXME: implement RotationBase methods.
|
||||||
|
mod rotation_conversion;
|
||||||
|
mod rotation_alias;
|
||||||
|
mod rotation_specialization;
|
||||||
|
|
||||||
|
mod quaternion;
|
||||||
|
mod quaternion_construction;
|
||||||
|
mod quaternion_ops;
|
||||||
|
mod quaternion_alga;
|
||||||
|
mod quaternion_alias;
|
||||||
|
mod quaternion_coordinates;
|
||||||
|
mod quaternion_conversion;
|
||||||
|
|
||||||
|
mod unit_complex;
|
||||||
|
mod unit_complex_construction;
|
||||||
|
mod unit_complex_ops;
|
||||||
|
mod unit_complex_alga;
|
||||||
|
|
||||||
|
mod translation;
|
||||||
|
mod translation_construction;
|
||||||
|
mod translation_ops;
|
||||||
|
mod translation_alga;
|
||||||
|
mod translation_conversion;
|
||||||
|
mod translation_alias;
|
||||||
|
|
||||||
|
mod isometry;
|
||||||
|
mod isometry_construction;
|
||||||
|
mod isometry_ops;
|
||||||
|
mod isometry_alga;
|
||||||
|
mod isometry_conversion;
|
||||||
|
mod isometry_alias;
|
||||||
|
|
||||||
|
mod similarity;
|
||||||
|
mod similarity_construction;
|
||||||
|
mod similarity_ops;
|
||||||
|
mod similarity_alga;
|
||||||
|
mod similarity_conversion;
|
||||||
|
mod similarity_alias;
|
||||||
|
|
||||||
|
mod transform;
|
||||||
|
mod transform_construction;
|
||||||
|
mod transform_ops;
|
||||||
|
mod transform_alga;
|
||||||
|
mod transform_conversion;
|
||||||
|
mod transform_alias;
|
||||||
|
|
||||||
|
mod orthographic;
|
||||||
|
mod perspective;
|
||||||
|
|
||||||
|
pub use self::point::*;
|
||||||
|
pub use self::point_alias::*;
|
||||||
|
|
||||||
|
pub use self::rotation::*;
|
||||||
|
pub use self::rotation_alias::*;
|
||||||
|
|
||||||
|
pub use self::quaternion::*;
|
||||||
|
pub use self::quaternion_alias::*;
|
||||||
|
|
||||||
|
pub use self::unit_complex::*;
|
||||||
|
|
||||||
|
pub use self::translation::*;
|
||||||
|
pub use self::translation_alias::*;
|
||||||
|
|
||||||
|
pub use self::isometry::*;
|
||||||
|
pub use self::isometry_alias::*;
|
||||||
|
|
||||||
|
pub use self::similarity::*;
|
||||||
|
pub use self::similarity_alias::*;
|
||||||
|
|
||||||
|
pub use self::transform::*;
|
||||||
|
pub use self::transform_alias::*;
|
||||||
|
|
||||||
|
pub use self::orthographic::{OrthographicBase, Orthographic3};
|
||||||
|
pub use self::perspective::{PerspectiveBase, Perspective3};
|
188
src/geometry/op_macros.rs
Normal file
188
src/geometry/op_macros.rs
Normal file
@ -0,0 +1,188 @@
|
|||||||
|
#![macro_use]
|
||||||
|
|
||||||
|
|
||||||
|
// FIXME: merge with `md_impl`.
|
||||||
|
/// Macro for the implementation of multiplication and division.
|
||||||
|
macro_rules! md_impl(
|
||||||
|
(
|
||||||
|
// Operator, operator method, and calar bounds.
|
||||||
|
$Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)*;
|
||||||
|
// Storage dimensions, and dimension bounds.
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$BoundParam: ty>)*),+
|
||||||
|
// [Optional] Extra allocator bounds.
|
||||||
|
$(where $ConstraintType: ty: $ConstraintBound: ident<$($ConstraintBoundParams: ty $( = $EqBound: ty )*),*> )*;
|
||||||
|
// Argument identifiers and types + output.
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
|
||||||
|
// Operator actual mplementation.
|
||||||
|
$action: expr;
|
||||||
|
// Lifetime.
|
||||||
|
$($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N $(, $Dims: $DimsBound $(<$BoundParam>)*)*, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul $($(+ $ScalarBounds)*)*,
|
||||||
|
SA: Storage<N, $R1, $C1>,
|
||||||
|
SB: Storage<N, $R2, $C2>,
|
||||||
|
$( $ConstraintType: $ConstraintBound<$( $ConstraintBoundParams $( = $EqBound )*),*> ),*
|
||||||
|
{
|
||||||
|
type Output = $Result;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
/// Macro for the implementation of multiplication and division.
|
||||||
|
/// Implements all the argument reference combinations.
|
||||||
|
macro_rules! md_impl_all(
|
||||||
|
(
|
||||||
|
// Operator, operator method, and calar bounds.
|
||||||
|
$Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)*;
|
||||||
|
// Storage dimensions, and dimension bounds.
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$BoundParam: ty>)*),+
|
||||||
|
// [Optional] Extra allocator bounds.
|
||||||
|
$(where $ConstraintType: ty: $ConstraintBound: ident<$($ConstraintBoundParams: ty $( = $EqBound: ty )*),*> )*;
|
||||||
|
// Argument identifiers and types + output.
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
|
||||||
|
// Operators actual implementations.
|
||||||
|
[val val] => $action_val_val: expr;
|
||||||
|
[ref val] => $action_ref_val: expr;
|
||||||
|
[val ref] => $action_val_ref: expr;
|
||||||
|
[ref ref] => $action_ref_ref: expr;) => {
|
||||||
|
|
||||||
|
md_impl!(
|
||||||
|
$Op, $op $(where N: $($ScalarBounds),*)*;
|
||||||
|
($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$BoundParam>)*),+
|
||||||
|
$(where $ConstraintType: $ConstraintBound<$($ConstraintBoundParams $( = $EqBound )*),*>)*;
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs, Output = $Result;
|
||||||
|
$action_val_val; );
|
||||||
|
|
||||||
|
md_impl!(
|
||||||
|
$Op, $op $(where N: $($ScalarBounds),*)*;
|
||||||
|
($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$BoundParam>)*),+
|
||||||
|
$(where $ConstraintType: $ConstraintBound<$($ConstraintBoundParams $( = $EqBound )*),*>)*;
|
||||||
|
$lhs: &'a $Lhs, $rhs: $Rhs, Output = $Result;
|
||||||
|
$action_ref_val; 'a);
|
||||||
|
|
||||||
|
md_impl!(
|
||||||
|
$Op, $op $(where N: $($ScalarBounds),*)*;
|
||||||
|
($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$BoundParam>)*),+
|
||||||
|
$(where $ConstraintType: $ConstraintBound<$($ConstraintBoundParams $( = $EqBound )*),*>)*;
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs, Output = $Result;
|
||||||
|
$action_val_ref; 'b);
|
||||||
|
|
||||||
|
md_impl!(
|
||||||
|
$Op, $op $(where N: $($ScalarBounds),*)*;
|
||||||
|
($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$BoundParam>)*),+
|
||||||
|
$(where $ConstraintType: $ConstraintBound<$($ConstraintBoundParams $( = $EqBound )*),*>)*;
|
||||||
|
$lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Result;
|
||||||
|
$action_ref_ref; 'a, 'b);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
/// Macro for the implementation of assignement-multiplication and assignement-division.
|
||||||
|
macro_rules! md_assign_impl(
|
||||||
|
(
|
||||||
|
// Operator, operator method, and scalar bounds.
|
||||||
|
$Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)*;
|
||||||
|
// Storage dimensions, and dimension bounds.
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$BoundParam: ty>)*),+
|
||||||
|
// [Optional] Extra allocator bounds.
|
||||||
|
$(where $ConstraintType: ty: $ConstraintBound: ident<$($ConstraintBoundParams: ty $( = $EqBound: ty )*),*> )*;
|
||||||
|
// Argument identifiers and types.
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
|
||||||
|
// Actual implementation and lifetimes.
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N $(, $Dims: $DimsBound $(<$BoundParam>)*)*, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Scalar + Zero + ClosedAdd + ClosedMul $($(+ $ScalarBounds)*)*,
|
||||||
|
SA: OwnedStorage<N, $R1, $C1>, // FIXME: this is too restrictive.
|
||||||
|
SB: Storage<N, $R2, $C2>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, $R1, $C1, SA>,
|
||||||
|
$( $ConstraintType: $ConstraintBound<$( $ConstraintBoundParams $( = $EqBound )*),*> ),*
|
||||||
|
{
|
||||||
|
#[inline]
|
||||||
|
fn $op(&mut $lhs, $rhs: $Rhs) {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
/// Macro for the implementation of assignement-multiplication and assignement-division with and
|
||||||
|
/// without reference to the right-hand-side.
|
||||||
|
macro_rules! md_assign_impl_all(
|
||||||
|
(
|
||||||
|
// Operator, operator method, and scalar bounds.
|
||||||
|
$Op: ident, $op: ident $(where N: $($ScalarBounds: ident),*)*;
|
||||||
|
// Storage dimensions, and dimension bounds.
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident $(<$BoundParam: ty>)*),+
|
||||||
|
// [Optional] Extra allocator bounds.
|
||||||
|
$(where $ConstraintType: ty: $ConstraintBound: ident<$($ConstraintBoundParams: ty $( = $EqBound: ty )*),*> )*;
|
||||||
|
// Argument identifiers and types.
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
|
||||||
|
// Actual implementation and lifetimes.
|
||||||
|
[val] => $action_val: expr;
|
||||||
|
[ref] => $action_ref: expr;) => {
|
||||||
|
md_assign_impl!(
|
||||||
|
$Op, $op $(where N: $($ScalarBounds),*)*;
|
||||||
|
($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$BoundParam>)*),+
|
||||||
|
$(where $ConstraintType: $ConstraintBound<$($ConstraintBoundParams $( = $EqBound )*),*>)*;
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs;
|
||||||
|
$action_val; );
|
||||||
|
|
||||||
|
md_assign_impl!(
|
||||||
|
$Op, $op $(where N: $($ScalarBounds),*)*;
|
||||||
|
($R1, $C1),($R2, $C2) for $($Dims: $DimsBound $(<$BoundParam>)*),+
|
||||||
|
$(where $ConstraintType: $ConstraintBound<$($ConstraintBoundParams $( = $EqBound )*),*>)*;
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs;
|
||||||
|
$action_ref; 'b);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// FIXME: merge with `as_impl`.
|
||||||
|
/// Macro for the implementation of addition and subtraction.
|
||||||
|
macro_rules! add_sub_impl(
|
||||||
|
($Op: ident, $op: ident, $bound: ident;
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(-> ($RRes: ty))* for $($Dims: ident: $DimsBound: ident),+;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N $(, $Dims: $DimsBound)*, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
SA: Storage<N, $R1, $C1>,
|
||||||
|
SB: Storage<N, $R2, $C2>,
|
||||||
|
SA::Alloc: SameShapeAllocator<N, $R1, $C1, $R2, $C2, SA>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<$R1, $R2 $(, Representative = $RRes)*> +
|
||||||
|
SameNumberOfColumns<$C1, $C2> {
|
||||||
|
type Output = $Result;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// FIXME: merge with `md_assign_impl`.
|
||||||
|
/// Macro for the implementation of assignement-addition and assignement-subtraction.
|
||||||
|
macro_rules! add_sub_assign_impl(
|
||||||
|
($Op: ident, $op: ident, $bound: ident;
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) for $($Dims: ident: $DimsBound: ident),+;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N $(, $Dims: $DimsBound)*, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
SA: OwnedStorage<N, $R1, $C1>, // FIXME: this is too restrictive.
|
||||||
|
SB: Storage<N, $R2, $C2>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, $R1, $C1, SA>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<$R1, $R2> + SameNumberOfColumns<$C1, $C2> {
|
||||||
|
#[inline]
|
||||||
|
fn $op(&mut $lhs, $rhs: $Rhs) {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
269
src/geometry/orthographic.rs
Normal file
269
src/geometry/orthographic.rs
Normal file
@ -0,0 +1,269 @@
|
|||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{Scalar, SquareMatrix, OwnedSquareMatrix, ColumnVector, OwnedColumnVector, MatrixArray};
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
use core::storage::{OwnedStorage, Storage, StorageMut};
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
use core::helper;
|
||||||
|
|
||||||
|
use geometry::{PointBase, OwnedPoint};
|
||||||
|
|
||||||
|
/// A 3D orthographic projection stored as an homogeneous 4x4 matrix.
|
||||||
|
#[derive(Debug, Clone, Copy)] // FIXME: Hash
|
||||||
|
pub struct OrthographicBase<N: Scalar, S: Storage<N, U4, U4>> {
|
||||||
|
matrix: SquareMatrix<N, U4, S>
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A 3D orthographic projection stored as a static homogeneous 4x4 matrix.
|
||||||
|
pub type Orthographic3<N> = OrthographicBase<N, MatrixArray<N, U4, U4>>;
|
||||||
|
|
||||||
|
impl<N, S> Eq for OrthographicBase<N, S>
|
||||||
|
where N: Scalar + Eq,
|
||||||
|
S: Storage<N, U4, U4> { }
|
||||||
|
|
||||||
|
impl<N, S> PartialEq for OrthographicBase<N, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, U4, U4> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &Self) -> bool {
|
||||||
|
self.matrix == right.matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> OrthographicBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U4>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
/// Creates a new orthographic projection matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(left: N, right: N, bottom: N, top: N, znear: N, zfar: N) -> Self {
|
||||||
|
assert!(left < right, "The left corner must be farther than the right corner.");
|
||||||
|
assert!(bottom < top, "The top corner must be higher than the bottom corner.");
|
||||||
|
assert!(znear < zfar, "The far plane must be farther than the near plane.");
|
||||||
|
|
||||||
|
let matrix = SquareMatrix::<N, U4, S>::identity();
|
||||||
|
let mut res = Self::from_matrix_unchecked(matrix);
|
||||||
|
|
||||||
|
res.set_left_and_right(left, right);
|
||||||
|
res.set_bottom_and_top(bottom, top);
|
||||||
|
res.set_znear_and_zfar(znear, zfar);
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Wraps the given matrix to interpret it as a 3D orthographic matrix.
|
||||||
|
///
|
||||||
|
/// It is not checked whether or not the given matrix actually represents an orthographic
|
||||||
|
/// projection.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_matrix_unchecked(matrix: SquareMatrix<N, U4, S>) -> Self {
|
||||||
|
OrthographicBase {
|
||||||
|
matrix: matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new orthographic projection matrix from an aspect ratio and the vertical field of view.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_fov(aspect: N, vfov: N, znear: N, zfar: N) -> Self {
|
||||||
|
assert!(znear < zfar, "The far plane must be farther than the near plane.");
|
||||||
|
assert!(!relative_eq!(aspect, N::zero()), "The apsect ratio must not be zero.");
|
||||||
|
|
||||||
|
let half: N = ::convert(0.5);
|
||||||
|
let width = zfar * (vfov * half).tan();
|
||||||
|
let height = width / aspect;
|
||||||
|
|
||||||
|
Self::new(-width * half, width * half, -height * half, height * half, znear, zfar)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> OrthographicBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U4> {
|
||||||
|
|
||||||
|
/// A reference to the underlying homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn as_matrix(&self) -> &SquareMatrix<N, U4, S> {
|
||||||
|
&self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves the underlying homogeneous matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn unwrap(self) -> SquareMatrix<N, U4, S> {
|
||||||
|
self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the corresponding homogeneous matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, U4, S::Alloc> {
|
||||||
|
self.matrix.clone_owned()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The smallest x-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn left(&self) -> N {
|
||||||
|
(-N::one() - self.matrix[(0, 3)]) / self.matrix[(0, 0)]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The largest x-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn right(&self) -> N {
|
||||||
|
(N::one() - self.matrix[(0, 3)]) / self.matrix[(0, 0)]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The smallest y-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn bottom(&self) -> N {
|
||||||
|
(-N::one() - self.matrix[(1, 3)]) / self.matrix[(1, 1)]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The largest y-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn top(&self) -> N {
|
||||||
|
(N::one() - self.matrix[(1, 3)]) / self.matrix[(1, 1)]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The near plane offset of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn znear(&self) -> N {
|
||||||
|
(N::one() + self.matrix[(2, 3)]) / self.matrix[(2, 2)]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The far plane offset of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn zfar(&self) -> N {
|
||||||
|
(-N::one() + self.matrix[(2, 3)]) / self.matrix[(2, 2)]
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: when we get specialization, specialize the Mul impl instead.
|
||||||
|
/// Projects a point. Faster than matrix multiplication.
|
||||||
|
#[inline]
|
||||||
|
pub fn project_point<SB>(&self, p: &PointBase<N, U3, SB>) -> OwnedPoint<N, U3, SB::Alloc>
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
|
||||||
|
OwnedPoint::<N, U3, SB::Alloc>::new(
|
||||||
|
self.matrix[(0, 0)] * p[0] + self.matrix[(0, 3)],
|
||||||
|
self.matrix[(1, 1)] * p[1] + self.matrix[(1, 3)],
|
||||||
|
self.matrix[(2, 2)] * p[2] + self.matrix[(2, 3)]
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: when we get specialization, specialize the Mul impl instead.
|
||||||
|
/// Projects a vector. Faster than matrix multiplication.
|
||||||
|
#[inline]
|
||||||
|
pub fn project_vector<SB>(&self, p: &ColumnVector<N, U3, SB>) -> OwnedColumnVector<N, U3, SB::Alloc>
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
|
||||||
|
OwnedColumnVector::<N, U3, SB::Alloc>::new(
|
||||||
|
self.matrix[(0, 0)] * p[0],
|
||||||
|
self.matrix[(1, 1)] * p[1],
|
||||||
|
self.matrix[(2, 2)] * p[2]
|
||||||
|
)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> OrthographicBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: StorageMut<N, U4, U4> {
|
||||||
|
/// Sets the smallest x-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_left(&mut self, left: N) {
|
||||||
|
let right = self.right();
|
||||||
|
self.set_left_and_right(left, right);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the largest x-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_right(&mut self, right: N) {
|
||||||
|
let left = self.left();
|
||||||
|
self.set_left_and_right(left, right);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the smallest y-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_bottom(&mut self, bottom: N) {
|
||||||
|
let top = self.top();
|
||||||
|
self.set_bottom_and_top(bottom, top);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the largest y-coordinate of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_top(&mut self, top: N) {
|
||||||
|
let bottom = self.bottom();
|
||||||
|
self.set_bottom_and_top(bottom, top);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the near plane offset of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_znear(&mut self, znear: N) {
|
||||||
|
let zfar = self.zfar();
|
||||||
|
self.set_znear_and_zfar(znear, zfar);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the far plane offset of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_zfar(&mut self, zfar: N) {
|
||||||
|
let znear = self.znear();
|
||||||
|
self.set_znear_and_zfar(znear, zfar);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the view cuboid coordinates along the `x` axis.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_left_and_right(&mut self, left: N, right: N) {
|
||||||
|
assert!(left < right, "The left corner must be farther than the right corner.");
|
||||||
|
self.matrix[(0, 0)] = ::convert::<_, N>(2.0) / (right - left);
|
||||||
|
self.matrix[(0, 3)] = -(right + left) / (right - left);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the view cuboid coordinates along the `y` axis.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_bottom_and_top(&mut self, bottom: N, top: N) {
|
||||||
|
assert!(bottom < top, "The top corner must be higher than the bottom corner.");
|
||||||
|
self.matrix[(1, 1)] = ::convert::<_, N>(2.0) / (top - bottom);
|
||||||
|
self.matrix[(1, 3)] = -(top + bottom) / (top - bottom);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the near and far plane offsets of the view cuboid.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_znear_and_zfar(&mut self, znear: N, zfar: N) {
|
||||||
|
assert!(!relative_eq!(zfar - znear, N::zero()), "The near-plane and far-plane must not be superimposed.");
|
||||||
|
self.matrix[(2, 2)] = -::convert::<_, N>(2.0) / (zfar - znear);
|
||||||
|
self.matrix[(2, 3)] = -(zfar + znear) / (zfar - znear);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Rand for OrthographicBase<N, S>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, U4, U4>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
fn rand<R: Rng>(r: &mut R) -> Self {
|
||||||
|
let left = Rand::rand(r);
|
||||||
|
let right = helper::reject_rand(r, |x: &N| *x > left);
|
||||||
|
let bottom = Rand::rand(r);
|
||||||
|
let top = helper::reject_rand(r, |x: &N| *x > bottom);
|
||||||
|
let znear = Rand::rand(r);
|
||||||
|
let zfar = helper::reject_rand(r, |x: &N| *x > znear);
|
||||||
|
|
||||||
|
Self::new(left, right, bottom, top, znear, zfar)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, S> Arbitrary for OrthographicBase<N, S>
|
||||||
|
where N: Real + Arbitrary,
|
||||||
|
S: OwnedStorage<N, U4, U4> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
let left = Arbitrary::arbitrary(g);
|
||||||
|
let right = helper::reject(g, |x: &N| *x > left);
|
||||||
|
let bottom = Arbitrary::arbitrary(g);
|
||||||
|
let top = helper::reject(g, |x: &N| *x > bottom);
|
||||||
|
let znear = Arbitrary::arbitrary(g);
|
||||||
|
let zfar = helper::reject(g, |x: &N| *x > znear);
|
||||||
|
|
||||||
|
Self::new(left, right, bottom, top, znear, zfar)
|
||||||
|
}
|
||||||
|
}
|
222
src/geometry/perspective.rs
Normal file
222
src/geometry/perspective.rs
Normal file
@ -0,0 +1,222 @@
|
|||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{Scalar, SquareMatrix, OwnedSquareMatrix, ColumnVector, OwnedColumnVector, MatrixArray};
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
use core::storage::{OwnedStorage, Storage, StorageMut};
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
use core::helper;
|
||||||
|
|
||||||
|
use geometry::{PointBase, OwnedPoint};
|
||||||
|
|
||||||
|
/// A 3D perspective projection stored as an homogeneous 4x4 matrix.
|
||||||
|
#[derive(Debug, Clone, Copy)] // FIXME: Hash
|
||||||
|
pub struct PerspectiveBase<N: Scalar, S: Storage<N, U4, U4>> {
|
||||||
|
matrix: SquareMatrix<N, U4, S>
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A 3D perspective projection stored as a static homogeneous 4x4 matrix.
|
||||||
|
pub type Perspective3<N> = PerspectiveBase<N, MatrixArray<N, U4, U4>>;
|
||||||
|
|
||||||
|
impl<N, S> Eq for PerspectiveBase<N, S>
|
||||||
|
where N: Scalar + Eq,
|
||||||
|
S: Storage<N, U4, U4> { }
|
||||||
|
|
||||||
|
impl<N, S> PartialEq for PerspectiveBase<N, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, U4, U4> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &Self) -> bool {
|
||||||
|
self.matrix == right.matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> PerspectiveBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U4>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
/// Creates a new perspective matrix from the aspect ratio, y field of view, and near/far planes.
|
||||||
|
pub fn new(aspect: N, fovy: N, znear: N, zfar: N) -> Self {
|
||||||
|
assert!(!relative_eq!(zfar - znear, N::zero()), "The near-plane and far-plane must not be superimposed.");
|
||||||
|
assert!(!relative_eq!(aspect, N::zero()), "The apsect ratio must not be zero.");
|
||||||
|
|
||||||
|
let matrix = SquareMatrix::<N, U4, S>::identity();
|
||||||
|
let mut res = PerspectiveBase::from_matrix_unchecked(matrix);
|
||||||
|
|
||||||
|
res.set_fovy(fovy);
|
||||||
|
res.set_aspect(aspect);
|
||||||
|
res.set_znear_and_zfar(znear, zfar);
|
||||||
|
|
||||||
|
res.matrix[(3, 3)] = N::zero();
|
||||||
|
res.matrix[(3, 2)] = -N::one();
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// Wraps the given matrix to interpret it as a 3D perspective matrix.
|
||||||
|
///
|
||||||
|
/// It is not checked whether or not the given matrix actually represents an orthographic
|
||||||
|
/// projection.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_matrix_unchecked(matrix: SquareMatrix<N, U4, S>) -> Self {
|
||||||
|
PerspectiveBase {
|
||||||
|
matrix: matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> PerspectiveBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U4> {
|
||||||
|
|
||||||
|
/// A reference to the underlying homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn as_matrix(&self) -> &SquareMatrix<N, U4, S> {
|
||||||
|
&self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Retrieves the underlying homogeneous matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn unwrap(self) -> SquareMatrix<N, U4, S> {
|
||||||
|
self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Computes the corresponding homogeneous matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, U4, S::Alloc> {
|
||||||
|
self.matrix.clone_owned()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the `width / height` aspect ratio of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn aspect(&self) -> N {
|
||||||
|
self.matrix[(1, 1)] / self.matrix[(0, 0)]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the y field of view of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn fovy(&self) -> N {
|
||||||
|
(N::one() / self.matrix[(1, 1)]).atan() * ::convert(2.0)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the near plane offset of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn znear(&self) -> N {
|
||||||
|
let ratio = (-self.matrix[(2, 2)] + N::one()) / (-self.matrix[(2, 2)] - N::one());
|
||||||
|
|
||||||
|
self.matrix[(2, 3)] / (ratio * ::convert(2.0)) - self.matrix[(2, 3)] / ::convert(2.0)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets the far plane offset of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn zfar(&self) -> N {
|
||||||
|
let ratio = (-self.matrix[(2, 2)] + N::one()) / (-self.matrix[(2, 2)] - N::one());
|
||||||
|
|
||||||
|
(self.matrix[(2, 3)] - ratio * self.matrix[(2, 3)]) / ::convert(2.0)
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: add a method to retrieve znear and zfar simultaneously?
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// FIXME: when we get specialization, specialize the Mul impl instead.
|
||||||
|
/// Projects a point. Faster than matrix multiplication.
|
||||||
|
#[inline]
|
||||||
|
pub fn project_point<SB>(&self, p: &PointBase<N, U3, SB>) -> OwnedPoint<N, U3, SB::Alloc>
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
|
||||||
|
let inverse_denom = -N::one() / p[2];
|
||||||
|
OwnedPoint::<N, U3, SB::Alloc>::new(
|
||||||
|
self.matrix[(0, 0)] * p[0] * inverse_denom,
|
||||||
|
self.matrix[(1, 1)] * p[1] * inverse_denom,
|
||||||
|
(self.matrix[(2, 2)] * p[2] + self.matrix[(2, 3)]) * inverse_denom
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: when we get specialization, specialize the Mul impl instead.
|
||||||
|
/// Projects a vector. Faster than matrix multiplication.
|
||||||
|
#[inline]
|
||||||
|
pub fn project_vector<SB>(&self, p: &ColumnVector<N, U3, SB>) -> OwnedColumnVector<N, U3, SB::Alloc>
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
|
||||||
|
let inverse_denom = -N::one() / p[2];
|
||||||
|
OwnedColumnVector::<N, U3, SB::Alloc>::new(
|
||||||
|
self.matrix[(0, 0)] * p[0] * inverse_denom,
|
||||||
|
self.matrix[(1, 1)] * p[1] * inverse_denom,
|
||||||
|
self.matrix[(2, 2)]
|
||||||
|
)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> PerspectiveBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: StorageMut<N, U4, U4> {
|
||||||
|
/// Updates this perspective matrix with a new `width / height` aspect ratio of the view
|
||||||
|
/// frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_aspect(&mut self, aspect: N) {
|
||||||
|
assert!(!relative_eq!(aspect, N::zero()), "The aspect ratio must not be zero.");
|
||||||
|
self.matrix[(0, 0)] = self.matrix[(1, 1)] / aspect;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Updates this perspective with a new y field of view of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_fovy(&mut self, fovy: N) {
|
||||||
|
let old_m22 = self.matrix[(1, 1)];
|
||||||
|
self.matrix[(1, 1)] = N::one() / (fovy / ::convert(2.0)).tan();
|
||||||
|
self.matrix[(0, 0)] = self.matrix[(0, 0)] * (self.matrix[(1, 1)] / old_m22);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Updates this perspective matrix with a new near plane offset of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_znear(&mut self, znear: N) {
|
||||||
|
let zfar = self.zfar();
|
||||||
|
self.set_znear_and_zfar(znear, zfar);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Updates this perspective matrix with a new far plane offset of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_zfar(&mut self, zfar: N) {
|
||||||
|
let znear = self.znear();
|
||||||
|
self.set_znear_and_zfar(znear, zfar);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Updates this perspective matrix with new near and far plane offsets of the view frustrum.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_znear_and_zfar(&mut self, znear: N, zfar: N) {
|
||||||
|
self.matrix[(2, 2)] = (zfar + znear) / (znear - zfar);
|
||||||
|
self.matrix[(2, 3)] = zfar * znear * ::convert(2.0) / (znear - zfar);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Rand for PerspectiveBase<N, S>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, U4, U4>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
fn rand<R: Rng>(r: &mut R) -> Self {
|
||||||
|
let znear = Rand::rand(r);
|
||||||
|
let zfar = helper::reject_rand(r, |&x: &N| !(x - znear).is_zero());
|
||||||
|
let aspect = helper::reject_rand(r, |&x: &N| !x.is_zero());
|
||||||
|
|
||||||
|
Self::new(aspect, Rand::rand(r), znear, zfar)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, S> Arbitrary for PerspectiveBase<N, S>
|
||||||
|
where N: Real + Arbitrary,
|
||||||
|
S: OwnedStorage<N, U4, U4> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U4, S> {
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
let znear = Arbitrary::arbitrary(g);
|
||||||
|
let zfar = helper::reject(g, |&x: &N| !(x - znear).is_zero());
|
||||||
|
let aspect = helper::reject(g, |&x: &N| !x.is_zero());
|
||||||
|
|
||||||
|
Self::new(aspect, Arbitrary::arbitrary(g), znear, zfar)
|
||||||
|
}
|
||||||
|
}
|
228
src/geometry/point.rs
Normal file
228
src/geometry/point.rs
Normal file
@ -0,0 +1,228 @@
|
|||||||
|
use num::One;
|
||||||
|
use std::fmt;
|
||||||
|
use std::cmp::Ordering;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use core::{Scalar, ColumnVector, OwnedColumnVector};
|
||||||
|
use core::iter::{MatrixIter, MatrixIterMut};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::{Storage, StorageMut, MulStorage};
|
||||||
|
use core::allocator::{Allocator, SameShapeR};
|
||||||
|
|
||||||
|
pub type PointSumStorage<N, R1, R2, SA> =
|
||||||
|
<<SA as Storage<N, R1, U1>>::Alloc as Allocator<N, SameShapeR<R1, R2>, U1>>::Buffer;
|
||||||
|
|
||||||
|
// XXX Bad name: we can't even add points…
|
||||||
|
/// The type of the result of the sum of a point with a vector.
|
||||||
|
pub type PointSum<N, D1, D2, SA> =
|
||||||
|
PointBase<N, SameShapeR<D1, D2>, PointSumStorage<N, D1, D2, SA>>;
|
||||||
|
|
||||||
|
/// The type of the result of the multiplication of a point by a matrix.
|
||||||
|
pub type PointMul<N, R1, C1, SA> = PointBase<N, R1, MulStorage<N, R1, C1, U1, SA>>;
|
||||||
|
|
||||||
|
/// A point with an owned storage.
|
||||||
|
pub type OwnedPoint<N, D, A> = PointBase<N, D, <A as Allocator<N, D, U1>>::Buffer>;
|
||||||
|
|
||||||
|
/// A point in n-dimensional euclidean space.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Hash, Debug)]
|
||||||
|
pub struct PointBase<N: Scalar, D: DimName, S: Storage<N, D, U1>> {
|
||||||
|
pub coords: ColumnVector<N, D, S>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D, S> Copy for PointBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimName,
|
||||||
|
S: Storage<N, D, U1> + Copy { }
|
||||||
|
|
||||||
|
impl<N, D, S> Clone for PointBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimName,
|
||||||
|
S: Storage<N, D, U1> + Clone {
|
||||||
|
#[inline]
|
||||||
|
fn clone(&self) -> Self {
|
||||||
|
PointBase::from_coordinates(self.coords.clone())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: Storage<N, D, U1>> PointBase<N, D, S> {
|
||||||
|
/// Creates a new point with the given coordinates.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_coordinates(coords: ColumnVector<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
PointBase {
|
||||||
|
coords: coords
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: Storage<N, D, U1>> PointBase<N, D, S> {
|
||||||
|
/// Moves this point into one that owns its data.
|
||||||
|
#[inline]
|
||||||
|
pub fn into_owned(self) -> OwnedPoint<N, D, S::Alloc> {
|
||||||
|
PointBase::from_coordinates(self.coords.into_owned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Clones this point into one that owns its data.
|
||||||
|
#[inline]
|
||||||
|
pub fn clone_owned(&self) -> OwnedPoint<N, D, S::Alloc> {
|
||||||
|
PointBase::from_coordinates(self.coords.clone_owned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The dimension of this point.
|
||||||
|
#[inline]
|
||||||
|
pub fn len(&self) -> usize {
|
||||||
|
self.coords.len()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The stride of this point. This is the number of buffer element separating each component of
|
||||||
|
/// this point.
|
||||||
|
#[inline]
|
||||||
|
pub fn stride(&self) -> usize {
|
||||||
|
self.coords.strides().0
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Iterates through this point coordinates.
|
||||||
|
#[inline]
|
||||||
|
pub fn iter(&self) -> MatrixIter<N, D, U1, S> {
|
||||||
|
self.coords.iter()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets a reference to i-th element of this point without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn get_unchecked(&self, i: usize) -> &N {
|
||||||
|
self.coords.get_unchecked(i, 0)
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// Converts this point into a vector in homogeneous coordinates, i.e., appends a `1` at the
|
||||||
|
/// end of it.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedColumnVector<N, DimNameSum<D, U1>, S::Alloc>
|
||||||
|
where N: One,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameSum<D, U1>, U1> {
|
||||||
|
|
||||||
|
let mut res = unsafe { OwnedColumnVector::<N, _, S::Alloc>::new_uninitialized() };
|
||||||
|
res.fixed_slice_mut::<D, U1>(0, 0).copy_from(&self.coords);
|
||||||
|
res[(D::dim(), 0)] = N::one();
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: StorageMut<N, D, U1>> PointBase<N, D, S> {
|
||||||
|
/// Mutably iterates through this point coordinates.
|
||||||
|
#[inline]
|
||||||
|
pub fn iter_mut(&mut self) -> MatrixIterMut<N, D, U1, S> {
|
||||||
|
self.coords.iter_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Gets a mutable reference to i-th element of this point without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn get_unchecked_mut(&mut self, i: usize) -> &mut N {
|
||||||
|
self.coords.get_unchecked_mut(i, 0)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Swaps two entries without bound-checking.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn swap_unchecked(&mut self, i1: usize, i2: usize) {
|
||||||
|
self.coords.swap_unchecked((i1, 0), (i2, 0))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> ApproxEq for PointBase<N, D, S>
|
||||||
|
where N: Scalar + ApproxEq,
|
||||||
|
S: Storage<N, D, U1>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
type Epsilon = N::Epsilon;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
N::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
N::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
N::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.coords.relative_eq(&other.coords, epsilon, max_relative)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.coords.ulps_eq(&other.coords, epsilon, max_ulps)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Eq for PointBase<N, D, S>
|
||||||
|
where N: Scalar + Eq,
|
||||||
|
S: Storage<N, D, U1> { }
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> PartialEq for PointBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &Self) -> bool {
|
||||||
|
self.coords == right.coords
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> PartialOrd for PointBase<N, D, S>
|
||||||
|
where N: Scalar + PartialOrd,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
|
||||||
|
self.coords.partial_cmp(&other.coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn lt(&self, right: &Self) -> bool {
|
||||||
|
self.coords.lt(&right.coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn le(&self, right: &Self) -> bool {
|
||||||
|
self.coords.le(&right.coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn gt(&self, right: &Self) -> bool {
|
||||||
|
self.coords.gt(&right.coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ge(&self, right: &Self) -> bool {
|
||||||
|
self.coords.ge(&right.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> fmt::Display for PointBase<N, D, S>
|
||||||
|
where N: Scalar + fmt::Display,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
try!(write!(f, "{{"));
|
||||||
|
|
||||||
|
let mut it = self.coords.iter();
|
||||||
|
|
||||||
|
try!(write!(f, "{}", *it.next().unwrap()));
|
||||||
|
|
||||||
|
for comp in it {
|
||||||
|
try!(write!(f, ", {}", *comp));
|
||||||
|
}
|
||||||
|
|
||||||
|
write!(f, "}}")
|
||||||
|
}
|
||||||
|
}
|
83
src/geometry/point_alga.rs
Normal file
83
src/geometry/point_alga.rs
Normal file
@ -0,0 +1,83 @@
|
|||||||
|
use alga::general::{Field, Real, MeetSemilattice, JoinSemilattice, Lattice};
|
||||||
|
use alga::linear::{AffineSpace, EuclideanSpace};
|
||||||
|
|
||||||
|
use core::{ColumnVector, Scalar};
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::PointBase;
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> AffineSpace for PointBase<N, D, S>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Translation = ColumnVector<N, D, S>;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> EuclideanSpace for PointBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Coordinates = ColumnVector<N, D, S>;
|
||||||
|
type Real = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn origin() -> Self {
|
||||||
|
Self::origin()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn coordinates(&self) -> Self::Coordinates {
|
||||||
|
self.coords.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_coordinates(coords: Self::Coordinates) -> Self {
|
||||||
|
Self::from_coordinates(coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scale_by(&self, n: N) -> Self {
|
||||||
|
self * n
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Ordering
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> MeetSemilattice for PointBase<N, D, S>
|
||||||
|
where N: Scalar + MeetSemilattice,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn meet(&self, other: &Self) -> Self {
|
||||||
|
PointBase::from_coordinates(self.coords.meet(&other.coords))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> JoinSemilattice for PointBase<N, D, S>
|
||||||
|
where N: Scalar + JoinSemilattice,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn join(&self, other: &Self) -> Self {
|
||||||
|
PointBase::from_coordinates(self.coords.join(&other.coords))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Lattice for PointBase<N, D, S>
|
||||||
|
where N: Scalar + Lattice,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn meet_join(&self, other: &Self) -> (Self, Self) {
|
||||||
|
let (meet, join) = self.coords.meet_join(&other.coords);
|
||||||
|
|
||||||
|
(PointBase::from_coordinates(meet), PointBase::from_coordinates(join))
|
||||||
|
}
|
||||||
|
}
|
20
src/geometry/point_alias.rs
Normal file
20
src/geometry/point_alias.rs
Normal file
@ -0,0 +1,20 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U1, U2, U3, U4, U5, U6};
|
||||||
|
|
||||||
|
use geometry::PointBase;
|
||||||
|
|
||||||
|
/// A statically sized D-dimensional column point.
|
||||||
|
pub type Point<N, D> = PointBase<N, D, MatrixArray<N, D, U1>>;
|
||||||
|
|
||||||
|
/// A statically sized 1-dimensional column point.
|
||||||
|
pub type Point1<N> = Point<N, U1>;
|
||||||
|
/// A statically sized 2-dimensional column point.
|
||||||
|
pub type Point2<N> = Point<N, U2>;
|
||||||
|
/// A statically sized 3-dimensional column point.
|
||||||
|
pub type Point3<N> = Point<N, U3>;
|
||||||
|
/// A statically sized 4-dimensional column point.
|
||||||
|
pub type Point4<N> = Point<N, U4>;
|
||||||
|
/// A statically sized 5-dimensional column point.
|
||||||
|
pub type Point5<N> = Point<N, U5>;
|
||||||
|
/// A statically sized 6-dimensional column point.
|
||||||
|
pub type Point6<N> = Point<N, U6>;
|
127
src/geometry/point_construction.rs
Normal file
127
src/geometry/point_construction.rs
Normal file
@ -0,0 +1,127 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
use num::{Zero, One, Bounded};
|
||||||
|
|
||||||
|
use alga::general::ClosedDiv;
|
||||||
|
use core::{Scalar, ColumnVector};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1, U2, U3, U4, U5, U6};
|
||||||
|
|
||||||
|
use geometry::PointBase;
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> PointBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new point with uninitialized coordinates.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn new_uninitialized() -> Self {
|
||||||
|
Self::from_coordinates(ColumnVector::<_, D, _>::new_uninitialized())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new point with all coordinates equal to zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn origin() -> Self
|
||||||
|
where N: Zero {
|
||||||
|
Self::from_coordinates(ColumnVector::<_, D, _>::from_element(N::zero()))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new point from its homogeneous vector representation.
|
||||||
|
///
|
||||||
|
/// In practice, this builds a D-dimensional points with the same first D component as `v`
|
||||||
|
/// divided by the last component of `v`. Returns `None` if this divisor is zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_homogeneous<SB>(v: ColumnVector<N, DimNameSum<D, U1>, SB>) -> Option<Self>
|
||||||
|
where N: Scalar + Zero + One + ClosedDiv,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SB: Storage<N, DimNameSum<D, U1>, U1, Alloc = S::Alloc>,
|
||||||
|
S::Alloc: Allocator<N, DimNameSum<D, U1>, U1> {
|
||||||
|
|
||||||
|
if !v[D::dim()].is_zero() {
|
||||||
|
let coords = v.fixed_slice::<D, U1>(0, 0) / v[D::dim()];
|
||||||
|
Some(Self::from_coordinates(coords))
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Traits that buid points.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Bounded for PointBase<N, D, S>
|
||||||
|
where N: Scalar + Bounded,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn max_value() -> Self {
|
||||||
|
Self::from_coordinates(ColumnVector::max_value())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn min_value() -> Self {
|
||||||
|
Self::from_coordinates(ColumnVector::min_value())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Rand for PointBase<N, D, S>
|
||||||
|
where N: Scalar + Rand,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<G: Rng>(rng: &mut G) -> Self {
|
||||||
|
PointBase::from_coordinates(rng.gen())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, D: DimName, S> Arbitrary for PointBase<N, D, S>
|
||||||
|
where N: Scalar + Arbitrary + Send,
|
||||||
|
S: OwnedStorage<N, D, U1> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
PointBase::from_coordinates(ColumnVector::arbitrary(g))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Small points construction from components.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
macro_rules! componentwise_constructors_impl(
|
||||||
|
($($D: ty, $($args: ident:$irow: expr),*);* $(;)*) => {$(
|
||||||
|
impl<N, S> PointBase<N, $D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: OwnedStorage<N, $D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $D, U1, S> {
|
||||||
|
/// Initializes this matrix from its components.
|
||||||
|
#[inline]
|
||||||
|
pub fn new($($args: N),*) -> PointBase<N, $D, S> {
|
||||||
|
unsafe {
|
||||||
|
let mut res = Self::new_uninitialized();
|
||||||
|
$( *res.get_unchecked_mut($irow) = $args; )*
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
componentwise_constructors_impl!(
|
||||||
|
U1, x:0;
|
||||||
|
U2, x:0, y:1;
|
||||||
|
U3, x:0, y:1, z:2;
|
||||||
|
U4, x:0, y:1, z:2, w:3;
|
||||||
|
U5, x:0, y:1, z:2, w:3, a:4;
|
||||||
|
U6, x:0, y:1, z:2, w:3, a:4, b:5;
|
||||||
|
);
|
73
src/geometry/point_conversion.rs
Normal file
73
src/geometry/point_conversion.rs
Normal file
@ -0,0 +1,73 @@
|
|||||||
|
use num::{One, Zero};
|
||||||
|
use alga::general::{SubsetOf, SupersetOf, ClosedDiv};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, ColumnVector, OwnedColumnVector};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, OwnedPoint};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides the following conversions:
|
||||||
|
* =============================================
|
||||||
|
*
|
||||||
|
* PointBase -> PointBase
|
||||||
|
* PointBase -> ColumnVector (homogeneous)
|
||||||
|
*/
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB> SubsetOf<PointBase<N2, D, SB>> for PointBase<N1, D, SA>
|
||||||
|
where D: DimName,
|
||||||
|
N1: Scalar,
|
||||||
|
N2: Scalar + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> PointBase<N2, D, SB> {
|
||||||
|
PointBase::from_coordinates(self.coords.to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &PointBase<N2, D, SB>) -> bool {
|
||||||
|
// FIXME: is there a way to reuse the `.is_in_subset` from the matrix implementation of
|
||||||
|
// SubsetOf?
|
||||||
|
m.iter().all(|e| e.is_in_subset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &PointBase<N2, D, SB>) -> Self {
|
||||||
|
PointBase::from_coordinates(Matrix::from_superset_unchecked(&m.coords))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB> SubsetOf<ColumnVector<N2, DimNameSum<D, U1>, SB>> for PointBase<N1, D, SA>
|
||||||
|
where D: DimNameAdd<U1>,
|
||||||
|
N1: Scalar,
|
||||||
|
N2: Scalar + Zero + One + ClosedDiv + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, U1>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, U1, SB> +
|
||||||
|
Allocator<N2, D, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> ColumnVector<N2, DimNameSum<D, U1>, SB> {
|
||||||
|
let p: OwnedPoint<N2, D, SB::Alloc> = self.to_superset();
|
||||||
|
p.to_homogeneous()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(v: &ColumnVector<N2, DimNameSum<D, U1>, SB>) -> bool {
|
||||||
|
::is_convertible::<_, OwnedColumnVector<N1, DimNameSum<D, U1>, SA::Alloc>>(v) &&
|
||||||
|
!v[D::dim()].is_zero()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(v: &ColumnVector<N2, DimNameSum<D, U1>, SB>) -> Self {
|
||||||
|
let coords = v.fixed_slice::<D, U1>(0, 0) / v[D::dim()];
|
||||||
|
Self::from_coordinates(::convert_unchecked(coords))
|
||||||
|
}
|
||||||
|
}
|
47
src/geometry/point_coordinates.rs
Normal file
47
src/geometry/point_coordinates.rs
Normal file
@ -0,0 +1,47 @@
|
|||||||
|
use std::mem;
|
||||||
|
use std::ops::{Deref, DerefMut};
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::dimension::{U1, U2, U3, U4, U5, U6};
|
||||||
|
use core::coordinates::{X, XY, XYZ, XYZW, XYZWA, XYZWAB};
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
|
||||||
|
use geometry::PointBase;
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Give coordinates to PointBase{1 .. 6}
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
macro_rules! deref_impl(
|
||||||
|
($D: ty, $Target: ident $(, $comps: ident)*) => {
|
||||||
|
impl<N: Scalar, S> Deref for PointBase<N, $D, S>
|
||||||
|
where S: OwnedStorage<N, $D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $D, U1, S> {
|
||||||
|
type Target = $Target<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
unsafe { mem::transmute(self) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, S> DerefMut for PointBase<N, $D, S>
|
||||||
|
where S: OwnedStorage<N, $D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
unsafe { mem::transmute(self) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
deref_impl!(U1, X, x);
|
||||||
|
deref_impl!(U2, XY, x, y);
|
||||||
|
deref_impl!(U3, XYZ, x, y, z);
|
||||||
|
deref_impl!(U4, XYZW, x, y, z, w);
|
||||||
|
deref_impl!(U5, XYZWA, x, y, z, w, a);
|
||||||
|
deref_impl!(U6, XYZWAB, x, y, z, w, a, b);
|
263
src/geometry/point_ops.rs
Normal file
263
src/geometry/point_ops.rs
Normal file
@ -0,0 +1,263 @@
|
|||||||
|
use std::ops::{Neg, Add, AddAssign, Sub, SubAssign, Mul, MulAssign, Div, DivAssign, Index, IndexMut};
|
||||||
|
use num::Zero;
|
||||||
|
|
||||||
|
use alga::general::{ClosedNeg, ClosedAdd, ClosedSub, ClosedMul, ClosedDiv};
|
||||||
|
|
||||||
|
use core::{Scalar, ColumnVector, Matrix, ColumnVectorSum};
|
||||||
|
use core::dimension::{Dim, DimName, U1};
|
||||||
|
use core::constraint::{ShapeConstraint, SameNumberOfRows, SameNumberOfColumns, AreMultipliable};
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
use core::allocator::{SameShapeAllocator, Allocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, OwnedPoint, PointMul};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Indexing.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Index<usize> for PointBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
type Output = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index(&self, i: usize) -> &Self::Output {
|
||||||
|
&self.coords[i]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> IndexMut<usize> for PointBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: StorageMut<N, D, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn index_mut(&mut self, i: usize) -> &mut Self::Output {
|
||||||
|
&mut self.coords[i]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Neg.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Neg for PointBase<N, D, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
type Output = OwnedPoint<N, D, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
PointBase::from_coordinates(-self.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, D: DimName, S> Neg for &'a PointBase<N, D, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
type Output = OwnedPoint<N, D, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
PointBase::from_coordinates(-&self.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Subtraction & Addition.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
// PointBase - PointBase
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: &'a PointBase<N, D, SA>, right: &'b PointBase<N, D, SB>, Output = ColumnVectorSum<N, D, D, SA>;
|
||||||
|
&self.coords - &right.coords; 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: &'a PointBase<N, D, SB>, right: PointBase<N, D, SA>, Output = ColumnVectorSum<N, D, D, SA>;
|
||||||
|
&self.coords - right.coords; 'a);
|
||||||
|
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: PointBase<N, D, SA>, right: &'b PointBase<N, D, SB>, Output = ColumnVectorSum<N, D, D, SA>;
|
||||||
|
self.coords - &right.coords; 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: PointBase<N, D, SA>, right: PointBase<N, D, SB>, Output = ColumnVectorSum<N, D, D, SA>;
|
||||||
|
self.coords - right.coords; );
|
||||||
|
|
||||||
|
// PointBase - Vector
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: &'a PointBase<N, D1, SA>, right: &'b ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(&self.coords - right); 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: &'a PointBase<N, D1, SA>, right: ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(&self.coords - &right); 'a); // FIXME: should not be a ref to `right`.
|
||||||
|
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: PointBase<N, D1, SA>, right: &'b ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(self.coords - right); 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Sub, sub, ClosedSub;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: PointBase<N, D1, SA>, right: ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(self.coords - right); );
|
||||||
|
|
||||||
|
|
||||||
|
// PointBase + Vector
|
||||||
|
add_sub_impl!(Add, add, ClosedAdd;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: &'a PointBase<N, D1, SA>, right: &'b ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(&self.coords + right); 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Add, add, ClosedAdd;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: &'a PointBase<N, D1, SA>, right: ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(&self.coords + &right); 'a); // FIXME: should not be a ref to `right`.
|
||||||
|
|
||||||
|
add_sub_impl!(Add, add, ClosedAdd;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: PointBase<N, D1, SA>, right: &'b ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(self.coords + right); 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Add, add, ClosedAdd;
|
||||||
|
(D1, U1), (D2, U1) -> (D1) for D1: DimName, D2: Dim;
|
||||||
|
self: PointBase<N, D1, SA>, right: ColumnVector<N, D2, SB>, Output = OwnedPoint<N, D1, SA::Alloc>;
|
||||||
|
Self::Output::from_coordinates(self.coords + right); );
|
||||||
|
|
||||||
|
|
||||||
|
// XXX: replace by the shared macro: add_sub_assign_impl
|
||||||
|
macro_rules! op_assign_impl(
|
||||||
|
($($TraitAssign: ident, $method_assign: ident, $bound: ident);* $(;)*) => {$(
|
||||||
|
impl<'b, N, D1: DimName, D2: Dim, SA, SB> $TraitAssign<&'b ColumnVector<N, D2, SB>> for PointBase<N, D1, SA>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
SA: StorageMut<N, D1, U1>,
|
||||||
|
SB: Storage<N, D2, U1>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<D1, D2> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method_assign(&mut self, right: &'b ColumnVector<N, D2, SB>) {
|
||||||
|
self.coords.$method_assign(right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D1: DimName, D2: Dim, SA, SB> $TraitAssign<ColumnVector<N, D2, SB>> for PointBase<N, D1, SA>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
SA: StorageMut<N, D1, U1>,
|
||||||
|
SB: Storage<N, D2, U1>,
|
||||||
|
ShapeConstraint: SameNumberOfRows<D1, D2> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method_assign(&mut self, right: ColumnVector<N, D2, SB>) {
|
||||||
|
self.coords.$method_assign(right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
op_assign_impl!(
|
||||||
|
AddAssign, add_assign, ClosedAdd;
|
||||||
|
SubAssign, sub_assign, ClosedSub;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Matrix × PointBase
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(R1, C1), (D2, U1) for R1: DimName, C1: Dim, D2: DimName
|
||||||
|
where SA::Alloc: Allocator<N, R1, U1>
|
||||||
|
where ShapeConstraint: AreMultipliable<R1, C1, D2, U1>;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: PointBase<N, D2, SB>, Output = PointMul<N, R1, C1, SA>;
|
||||||
|
[val val] => PointBase::from_coordinates(self * right.coords);
|
||||||
|
[ref val] => PointBase::from_coordinates(self * right.coords);
|
||||||
|
[val ref] => PointBase::from_coordinates(self * &right.coords);
|
||||||
|
[ref ref] => PointBase::from_coordinates(self * &right.coords);
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* PointBase ×/÷ Scalar
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
macro_rules! componentwise_scalarop_impl(
|
||||||
|
($Trait: ident, $method: ident, $bound: ident;
|
||||||
|
$TraitAssign: ident, $method_assign: ident) => {
|
||||||
|
impl<N, D: DimName, S> $Trait<N> for PointBase<N, D, S>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
type Output = OwnedPoint<N, D, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, right: N) -> Self::Output {
|
||||||
|
PointBase::from_coordinates(self.coords.$method(right))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, D: DimName, S> $Trait<N> for &'a PointBase<N, D, S>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
type Output = OwnedPoint<N, D, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $method(self, right: N) -> Self::Output {
|
||||||
|
PointBase::from_coordinates((&self.coords).$method(right))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> $TraitAssign<N> for PointBase<N, D, S>
|
||||||
|
where N: Scalar + $bound,
|
||||||
|
S: StorageMut<N, D, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn $method_assign(&mut self, right: N) {
|
||||||
|
self.coords.$method_assign(right)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
componentwise_scalarop_impl!(Mul, mul, ClosedMul; MulAssign, mul_assign);
|
||||||
|
componentwise_scalarop_impl!(Div, div, ClosedDiv; DivAssign, div_assign);
|
||||||
|
|
||||||
|
macro_rules! left_scalar_mul_impl(
|
||||||
|
($($T: ty),* $(,)*) => {$(
|
||||||
|
impl<D: DimName, S> Mul<PointBase<$T, D, S>> for $T
|
||||||
|
where S: Storage<$T, D, U1> {
|
||||||
|
type Output = OwnedPoint<$T, D, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: PointBase<$T, D, S>) -> Self::Output {
|
||||||
|
PointBase::from_coordinates(self * right.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, D: DimName, S> Mul<&'b PointBase<$T, D, S>> for $T
|
||||||
|
where S: Storage<$T, D, U1> {
|
||||||
|
type Output = OwnedPoint<$T, D, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: &'b PointBase<$T, D, S>) -> Self::Output {
|
||||||
|
PointBase::from_coordinates(self * &right.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
left_scalar_mul_impl!(
|
||||||
|
u8, u16, u32, u64, usize,
|
||||||
|
i8, i16, i32, i64, isize,
|
||||||
|
f32, f64
|
||||||
|
);
|
455
src/geometry/quaternion.rs
Normal file
455
src/geometry/quaternion.rs
Normal file
@ -0,0 +1,455 @@
|
|||||||
|
use std::fmt;
|
||||||
|
use num::Zero;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{Unit, ColumnVector, OwnedColumnVector, MatrixSlice, MatrixSliceMut, SquareMatrix,
|
||||||
|
OwnedSquareMatrix};
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
|
||||||
|
use geometry::{RotationBase, OwnedRotation};
|
||||||
|
|
||||||
|
/// A quaternion with an owned storage allocated by `A`.
|
||||||
|
pub type OwnedQuaternionBase<N, A> = QuaternionBase<N, <A as Allocator<N, U4, U1>>::Buffer>;
|
||||||
|
|
||||||
|
/// A unit quaternion with an owned storage allocated by `A`.
|
||||||
|
pub type OwnedUnitQuaternionBase<N, A> = UnitQuaternionBase<N, <A as Allocator<N, U4, U1>>::Buffer>;
|
||||||
|
|
||||||
|
/// A quaternion. See the type alias `UnitQuaternionBase = Unit<QuaternionBase>` for a quaternion
|
||||||
|
/// that may be used as a rotation.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Hash, Debug, Copy, Clone)]
|
||||||
|
pub struct QuaternionBase<N: Real, S: Storage<N, U4, U1>> {
|
||||||
|
pub coords: ColumnVector<N, U4, S>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Eq for QuaternionBase<N, S>
|
||||||
|
where N: Real + Eq,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> PartialEq for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
fn eq(&self, rhs: &Self) -> bool {
|
||||||
|
self.coords == rhs.coords ||
|
||||||
|
// Account for the double-covering of S², i.e. q = -q
|
||||||
|
self.as_vector().iter().zip(rhs.as_vector().iter()).all(|(a, b)| *a == -*b)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
/// Moves this quaternion into one that owns its data.
|
||||||
|
#[inline]
|
||||||
|
pub fn into_owned(self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
QuaternionBase::from_vector(self.coords.into_owned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Clones this quaternion into one that owns its data.
|
||||||
|
#[inline]
|
||||||
|
pub fn clone_owned(&self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
QuaternionBase::from_vector(self.coords.clone_owned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The vector part `(i, j, k)` of this quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn vector(&self) -> MatrixSlice<N, U3, U1, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
self.coords.fixed_rows::<U3>(0)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The scalar part `w` of this quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn scalar(&self) -> N {
|
||||||
|
self.coords[3]
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Reinterprets this quaternion as a 4D vector.
|
||||||
|
#[inline]
|
||||||
|
pub fn as_vector(&self) -> &ColumnVector<N, U4, S> {
|
||||||
|
&self.coords
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The norm of this quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn norm(&self) -> N {
|
||||||
|
self.coords.norm()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The squared norm of this quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn norm_squared(&self) -> N {
|
||||||
|
self.coords.norm_squared()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the conjugate of this quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn conjugate(&self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
let v = OwnedColumnVector::<N, U4, S::Alloc>::new(-self.coords[0],
|
||||||
|
-self.coords[1],
|
||||||
|
-self.coords[2],
|
||||||
|
self.coords[3]);
|
||||||
|
QuaternionBase::from_vector(v)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this quaternion if it is not zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_inverse(&self) -> Option<OwnedQuaternionBase<N, S::Alloc>> {
|
||||||
|
let mut res = QuaternionBase::from_vector(self.coords.clone_owned());
|
||||||
|
|
||||||
|
if res.try_inverse_mut() {
|
||||||
|
Some(res)
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U1> {
|
||||||
|
/// The polar decomposition of this quaternion.
|
||||||
|
///
|
||||||
|
/// Returns, from left to right: the quaternion norm, the half rotation angle, the rotation
|
||||||
|
/// axis. If the rotation angle is zero, the rotation axis is set to `None`.
|
||||||
|
pub fn polar_decomposition(&self) -> (N, N, Option<Unit<OwnedColumnVector<N, U3, S::Alloc>>>) {
|
||||||
|
if let Some((q, n)) = Unit::try_new_and_get(self.clone_owned(), N::zero()) {
|
||||||
|
if let Some(axis) = Unit::try_new(self.vector().clone_owned(), N::zero()) {
|
||||||
|
let angle = q.angle() / ::convert(2.0f64);
|
||||||
|
|
||||||
|
(n, angle, Some(axis))
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
(n, N::zero(), None)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
(N::zero(), N::zero(), None)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the exponential of a quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn exp(&self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
let v = self.vector();
|
||||||
|
let nn = v.norm_squared();
|
||||||
|
|
||||||
|
if relative_eq!(nn, N::zero()) {
|
||||||
|
QuaternionBase::identity()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
let w_exp = self.scalar().exp();
|
||||||
|
let n = nn.sqrt();
|
||||||
|
let nv = v * (w_exp * n.sin() / n);
|
||||||
|
|
||||||
|
QuaternionBase::from_parts(n.cos(), nv)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the natural logarithm of a quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn ln(&self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
let n = self.norm();
|
||||||
|
let v = self.vector();
|
||||||
|
let s = self.scalar();
|
||||||
|
|
||||||
|
QuaternionBase::from_parts(n.ln(), v.normalize() * (s / n).acos())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Raise the quaternion to a given floating power.
|
||||||
|
#[inline]
|
||||||
|
pub fn powf(&self, n: N) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
(self.ln() * n).exp()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: StorageMut<N, U4, U1> {
|
||||||
|
/// Transforms this quaternion into its 4D vector form (Vector part, Scalar part).
|
||||||
|
#[inline]
|
||||||
|
pub fn as_vector_mut(&mut self) -> &mut ColumnVector<N, U4, S> {
|
||||||
|
&mut self.coords
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The mutable vector part `(i, j, k)` of this quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn vector_mut(&mut self) -> MatrixSliceMut<N, U3, U1, S::RStride, S::CStride, S::Alloc> {
|
||||||
|
self.coords.fixed_rows_mut::<U3>(0)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Replaces this quaternion by its conjugate.
|
||||||
|
#[inline]
|
||||||
|
pub fn conjugate_mut(&mut self) {
|
||||||
|
self.coords[0] = -self.coords[0];
|
||||||
|
self.coords[1] = -self.coords[1];
|
||||||
|
self.coords[2] = -self.coords[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this quaternion in-place if it is not zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
let norm_squared = self.norm_squared();
|
||||||
|
|
||||||
|
if relative_eq!(&norm_squared, &N::zero()) {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
self.conjugate_mut();
|
||||||
|
self.coords /= norm_squared;
|
||||||
|
|
||||||
|
true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> ApproxEq for QuaternionBase<N, S>
|
||||||
|
where N: Real + ApproxEq<Epsilon = N>,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
type Epsilon = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
N::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
N::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
N::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.as_vector().relative_eq(other.as_vector(), epsilon, max_relative) ||
|
||||||
|
// Account for the double-covering of S², i.e. q = -q
|
||||||
|
self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.relative_eq(&-*b, epsilon, max_relative))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.as_vector().ulps_eq(other.as_vector(), epsilon, max_ulps) ||
|
||||||
|
// Account for the double-covering of S², i.e. q = -q.
|
||||||
|
self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.ulps_eq(&-*b, epsilon, max_ulps))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> fmt::Display for QuaternionBase<N, S>
|
||||||
|
where N: Real + fmt::Display,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
write!(f, "Quaternion {} − ({}, {}, {})", self[3], self[0], self[1], self[2])
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A unit quaternions. May be used to represent a rotation.
|
||||||
|
pub type UnitQuaternionBase<N, S> = Unit<QuaternionBase<N, S>>;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
/// The rotation angle in [0; pi] of this unit quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle(&self) -> N {
|
||||||
|
let w = self.quaternion().scalar().abs();
|
||||||
|
|
||||||
|
// Handle innacuracies that make break `.acos`.
|
||||||
|
if w >= N::one() {
|
||||||
|
N::zero()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
w.acos() * ::convert(2.0f64)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The underlying quaternion.
|
||||||
|
///
|
||||||
|
/// Same as `self.as_ref()`.
|
||||||
|
#[inline]
|
||||||
|
pub fn quaternion(&self) -> &QuaternionBase<N, S> {
|
||||||
|
self.as_ref()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the conjugate of this unit quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn conjugate(&self) -> OwnedUnitQuaternionBase<N, S::Alloc> {
|
||||||
|
UnitQuaternionBase::new_unchecked(self.as_ref().conjugate())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this quaternion if it is not zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(&self) -> OwnedUnitQuaternionBase<N, S::Alloc> {
|
||||||
|
self.conjugate()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation angle needed to make `self` and `other` coincide.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle_to<S2>(&self, other: &UnitQuaternionBase<N, S2>) -> N
|
||||||
|
where S2: Storage<N, U4, U1> {
|
||||||
|
let delta = self.rotation_to(other);
|
||||||
|
delta.angle()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit quaternion needed to make `self` and `other` coincide.
|
||||||
|
///
|
||||||
|
/// The result is such that: `self.rotation_to(other) * self == other`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_to<S2>(&self, other: &UnitQuaternionBase<N, S2>) -> OwnedUnitQuaternionBase<N, S2::Alloc>
|
||||||
|
where S2: Storage<N, U4, U1> {
|
||||||
|
other / self
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: StorageMut<N, U4, U1> {
|
||||||
|
/// Compute the conjugate of this unit quaternion in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn conjugate_mut(&mut self) {
|
||||||
|
self.as_mut_unchecked().conjugate_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this quaternion if it is not zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self) {
|
||||||
|
self.as_mut_unchecked().conjugate_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U1> {
|
||||||
|
/// The rotation axis of this unit quaternion or `None` if the rotation is zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn axis(&self) -> Option<Unit<OwnedColumnVector<N, U3, S::Alloc>>> {
|
||||||
|
let v =
|
||||||
|
if self.quaternion().scalar() >= N::zero() {
|
||||||
|
self.as_ref().vector().clone_owned()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
-self.as_ref().vector()
|
||||||
|
};
|
||||||
|
|
||||||
|
Unit::try_new(v, N::zero())
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// The rotation axis of this unit quaternion multiplied by the rotation agle.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_axis(&self) -> OwnedColumnVector<N, U3, S::Alloc> {
|
||||||
|
if let Some(axis) = self.axis() {
|
||||||
|
axis.unwrap() * self.angle()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
ColumnVector::zero()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the exponential of a quaternion.
|
||||||
|
///
|
||||||
|
/// Note that this function yields a `QuaternionBase<N>` because it looses the unit property.
|
||||||
|
#[inline]
|
||||||
|
pub fn exp(&self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
self.as_ref().exp()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the natural logarithm of a quaternion.
|
||||||
|
///
|
||||||
|
/// Note that this function yields a `QuaternionBase<N>` because it looses the unit property.
|
||||||
|
/// The vector part of the return value corresponds to the axis-angle representation (divided
|
||||||
|
/// by 2.0) of this unit quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn ln(&self) -> OwnedQuaternionBase<N, S::Alloc> {
|
||||||
|
if let Some(v) = self.axis() {
|
||||||
|
QuaternionBase::from_parts(N::zero(), v.unwrap() * self.angle())
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
QuaternionBase::zero()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Raise the quaternion to a given floating power.
|
||||||
|
///
|
||||||
|
/// This returns the unit quaternion that identifies a rotation with axis `self.axis()` and
|
||||||
|
/// angle `self.angle() × n`.
|
||||||
|
#[inline]
|
||||||
|
pub fn powf(&self, n: N) -> OwnedUnitQuaternionBase<N, S::Alloc> {
|
||||||
|
if let Some(v) = self.axis() {
|
||||||
|
UnitQuaternionBase::from_axis_angle(&v, self.angle() * n)
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
UnitQuaternionBase::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> {
|
||||||
|
/// Builds a rotation matrix from this unit quaternion.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_rotation_matrix(&self) -> OwnedRotation<N, U3, S::Alloc> {
|
||||||
|
let i = self.as_ref()[0];
|
||||||
|
let j = self.as_ref()[1];
|
||||||
|
let k = self.as_ref()[2];
|
||||||
|
let w = self.as_ref()[3];
|
||||||
|
|
||||||
|
let ww = w * w;
|
||||||
|
let ii = i * i;
|
||||||
|
let jj = j * j;
|
||||||
|
let kk = k * k;
|
||||||
|
let ij = i * j * ::convert(2.0f64);
|
||||||
|
let wk = w * k * ::convert(2.0f64);
|
||||||
|
let wj = w * j * ::convert(2.0f64);
|
||||||
|
let ik = i * k * ::convert(2.0f64);
|
||||||
|
let jk = j * k * ::convert(2.0f64);
|
||||||
|
let wi = w * i * ::convert(2.0f64);
|
||||||
|
|
||||||
|
RotationBase::from_matrix_unchecked(
|
||||||
|
SquareMatrix::<_, U3, _>::new(
|
||||||
|
ww + ii - jj - kk, ij - wk, wj + ik,
|
||||||
|
wk + ij, ww - ii + jj - kk, jk - wi,
|
||||||
|
ik - wj, wi + jk, ww - ii - jj + kk
|
||||||
|
)
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Converts this unit quaternion into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, U4, S::Alloc>
|
||||||
|
where S::Alloc: Allocator<N, U4, U4> {
|
||||||
|
self.to_rotation_matrix().to_homogeneous()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> fmt::Display for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real + fmt::Display,
|
||||||
|
S: Storage<N, U4, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U1> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
if let Some(axis) = self.axis() {
|
||||||
|
let axis = axis.unwrap();
|
||||||
|
write!(f, "UnitQuaternion angle: {} − axis: ({}, {}, {})", self.angle(), axis[0], axis[1], axis[2])
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
write!(f, "UnitQuaternion angle: {} − axis: (undefined)", self.angle())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
382
src/geometry/quaternion_alga.rs
Normal file
382
src/geometry/quaternion_alga.rs
Normal file
@ -0,0 +1,382 @@
|
|||||||
|
use num::Zero;
|
||||||
|
|
||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractGroupAbelian, AbstractLoop,
|
||||||
|
AbstractMonoid, AbstractQuasigroup, AbstractSemigroup, AbstractModule,
|
||||||
|
Module, Real, Inverse, Multiplicative, Additive, Identity, Id};
|
||||||
|
use alga::linear::{Transformation, AffineTransformation, Similarity, Isometry, DirectIsometry,
|
||||||
|
OrthogonalTransformation, VectorSpace, FiniteDimVectorSpace, NormedSpace,
|
||||||
|
Rotation, ProjectiveTransformation};
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
use geometry::{PointBase, QuaternionBase, UnitQuaternionBase};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> Identity<Multiplicative> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Identity<Additive> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::zero()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> AbstractMagma<Multiplicative> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> AbstractMagma<Additive> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self + rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Inverse<Additive> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
-self
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_structures(
|
||||||
|
($Quaternion: ident; $($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, S> $marker<$operator> for $Quaternion<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_structures!(
|
||||||
|
QuaternionBase;
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
|
||||||
|
AbstractSemigroup<Additive>,
|
||||||
|
AbstractQuasigroup<Additive>,
|
||||||
|
AbstractMonoid<Additive>,
|
||||||
|
AbstractLoop<Additive>,
|
||||||
|
AbstractGroup<Additive>,
|
||||||
|
AbstractGroupAbelian<Additive>
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Vector space.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, S> AbstractModule for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
type AbstractRing = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn multiply_by(&self, n: N) -> Self {
|
||||||
|
self * n
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Module for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
type Ring = N;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> VectorSpace for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
type Field = N;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> FiniteDimVectorSpace for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn dimension() -> usize {
|
||||||
|
4
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn canonical_basis_element(i: usize) -> Self {
|
||||||
|
Self::from_vector(ColumnVector::canonical_basis_element(i))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn dot(&self, other: &Self) -> N {
|
||||||
|
self.coords.dot(&other.coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn component_unchecked(&self, i: usize) -> &N {
|
||||||
|
self.coords.component_unchecked(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn component_unchecked_mut(&mut self, i: usize) -> &mut N {
|
||||||
|
self.coords.component_unchecked_mut(i)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> NormedSpace for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn norm_squared(&self) -> N {
|
||||||
|
self.coords.norm_squared()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn norm(&self) -> N {
|
||||||
|
self.as_vector().norm()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn normalize(&self) -> Self {
|
||||||
|
let v = self.coords.normalize();
|
||||||
|
Self::from_vector(v)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn normalize_mut(&mut self) -> N {
|
||||||
|
self.coords.normalize_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn try_normalize(&self, min_norm: N) -> Option<Self> {
|
||||||
|
if let Some(v) = self.coords.try_normalize(min_norm) {
|
||||||
|
Some(Self::from_vector(v))
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
|
||||||
|
self.coords.try_normalize_mut(min_norm)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Implementations for UnitQuaternionBase.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, S> Identity<Multiplicative> for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> AbstractMagma<Multiplicative> for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Inverse<Multiplicative> for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.inverse_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl_structures!(
|
||||||
|
UnitQuaternionBase;
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
impl<N, SA, SB> Transformation<PointBase<N, U3, SB>> for UnitQuaternionBase<N, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, U4, U1>,
|
||||||
|
SB: OwnedStorage<N, U3, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, U3, SB>) -> PointBase<N, U3, SB> {
|
||||||
|
self * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, U3, SB>) -> ColumnVector<N, U3, SB> {
|
||||||
|
self * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, SA, SB> ProjectiveTransformation<PointBase<N, U3, SB>> for UnitQuaternionBase<N, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, U4, U1>,
|
||||||
|
SB: OwnedStorage<N, U3, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, U3, SB>) -> PointBase<N, U3, SB> {
|
||||||
|
// FIXME: would it be useful performancewise not to call inverse explicitly (i-e. implement
|
||||||
|
// the inverse transformation explicitly here) ?
|
||||||
|
self.inverse() * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, U3, SB>) -> ColumnVector<N, U3, SB> {
|
||||||
|
self.inverse() * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, SA, SB> AffineTransformation<PointBase<N, U3, SB>> for UnitQuaternionBase<N, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, U4, U1>,
|
||||||
|
SB: OwnedStorage<N, U3, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> {
|
||||||
|
type Rotation = Self;
|
||||||
|
type NonUniformScaling = Id;
|
||||||
|
type Translation = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn decompose(&self) -> (Id, Self, Id, Self) {
|
||||||
|
(Id::new(), self.clone(), Id::new(), Self::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_translation(&self, _: &Self::Translation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_translation(&self, _: &Self::Translation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
r * self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
self * r
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, SA, SB> Similarity<PointBase<N, U3, SB>> for UnitQuaternionBase<N, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, U4, U1>,
|
||||||
|
SB: OwnedStorage<N, U3, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> {
|
||||||
|
type Scaling = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation(&self) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaling(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! marker_impl(
|
||||||
|
($($Trait: ident),*) => {$(
|
||||||
|
impl<N, SA, SB> $Trait<PointBase<N, U3, SB>> for UnitQuaternionBase<N, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, U4, U1>,
|
||||||
|
SB: OwnedStorage<N, U3, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
marker_impl!(Isometry, DirectIsometry, OrthogonalTransformation);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, SA, SB> Rotation<PointBase<N, U3, SB>> for UnitQuaternionBase<N, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, U4, U1>,
|
||||||
|
SB: OwnedStorage<N, U3, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, U4, U1, SA> + Allocator<N, U3, U1>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, U3, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn powf(&self, n: N) -> Option<Self> {
|
||||||
|
Some(self.powf(n))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation_between(a: &ColumnVector<N, U3, SB>, b: &ColumnVector<N, U3, SB>) -> Option<Self> {
|
||||||
|
Self::rotation_between(a, b)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaled_rotation_between(a: &ColumnVector<N, U3, SB>, b: &ColumnVector<N, U3, SB>, s: N) -> Option<Self> {
|
||||||
|
Self::scaled_rotation_between(a, b, s)
|
||||||
|
}
|
||||||
|
}
|
10
src/geometry/quaternion_alias.rs
Normal file
10
src/geometry/quaternion_alias.rs
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U1, U4};
|
||||||
|
|
||||||
|
use geometry::{QuaternionBase, UnitQuaternionBase};
|
||||||
|
|
||||||
|
/// A statically-allocated quaternion.
|
||||||
|
pub type Quaternion<N> = QuaternionBase<N, MatrixArray<N, U4, U1>>;
|
||||||
|
|
||||||
|
/// A statically-allocated unit quaternion.
|
||||||
|
pub type UnitQuaternion<N> = UnitQuaternionBase<N, MatrixArray<N, U4, U1>>;
|
344
src/geometry/quaternion_construction.rs
Normal file
344
src/geometry/quaternion_construction.rs
Normal file
@ -0,0 +1,344 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
use num::{Zero, One};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{Unit, ColumnVector, Vector3};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
|
||||||
|
use geometry::{QuaternionBase, UnitQuaternionBase, RotationBase, OwnedRotation};
|
||||||
|
|
||||||
|
impl<N, S> QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
/// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w`
|
||||||
|
/// vector component.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_vector(vector: ColumnVector<N, U4, S>) -> Self {
|
||||||
|
QuaternionBase {
|
||||||
|
coords: vector
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
/// Creates a new quaternion from its individual components. Note that the arguments order does
|
||||||
|
/// **not** follow the storage order.
|
||||||
|
///
|
||||||
|
/// The storage order is [ x, y, z, w ].
|
||||||
|
#[inline]
|
||||||
|
pub fn new(w: N, x: N, y: N, z: N) -> Self {
|
||||||
|
let v = ColumnVector::<N, U4, S>::new(x, y, z, w);
|
||||||
|
Self::from_vector(v)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new quaternion from its scalar and vector parts. Note that the arguments order does
|
||||||
|
/// **not** follow the storage order.
|
||||||
|
///
|
||||||
|
/// The storage order is [ vector, scalar ].
|
||||||
|
#[inline]
|
||||||
|
// FIXME: take a reference to `vector`?
|
||||||
|
pub fn from_parts<SB>(scalar: N, vector: ColumnVector<N, U3, SB>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
|
||||||
|
Self::new(scalar, vector[0], vector[1], vector[2])
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new quaternion from its polar decomposition.
|
||||||
|
///
|
||||||
|
/// Note that `axis` is assumed to be a unit vector.
|
||||||
|
// FIXME: take a reference to `axis`?
|
||||||
|
pub fn from_polar_decomposition<SB>(scale: N, theta: N, axis: Unit<ColumnVector<N, U3, SB>>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
let rot = UnitQuaternionBase::<N, S>::from_axis_angle(&axis, theta * ::convert(2.0f64));
|
||||||
|
|
||||||
|
rot.unwrap() * scale
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The quaternion multiplicative identity.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> Self {
|
||||||
|
Self::new(N::one(), N::zero(), N::zero(), N::zero())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> One for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Zero for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn zero() -> Self {
|
||||||
|
Self::new(N::zero(), N::zero(), N::zero(), N::zero())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_zero(&self) -> bool {
|
||||||
|
self.coords.is_zero()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Rand for QuaternionBase<N, S>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<R: Rng>(rng: &mut R) -> Self {
|
||||||
|
QuaternionBase::new(rng.gen(), rng.gen(), rng.gen(), rng.gen())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, S> Arbitrary for QuaternionBase<N, S>
|
||||||
|
where N: Real + Arbitrary,
|
||||||
|
S: OwnedStorage<N, U4, U1> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
QuaternionBase::new(N::arbitrary(g), N::arbitrary(g),
|
||||||
|
N::arbitrary(g), N::arbitrary(g))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
/// The quaternion multiplicative identity.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> Self {
|
||||||
|
Self::new_unchecked(QuaternionBase::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new quaternion from a unit vector (the rotation axis) and an angle
|
||||||
|
/// (the rotation angle).
|
||||||
|
#[inline]
|
||||||
|
pub fn from_axis_angle<SB>(axis: &Unit<ColumnVector<N, U3, SB>>, angle: N) -> Self
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
let (sang, cang) = (angle / ::convert(2.0f64)).sin_cos();
|
||||||
|
|
||||||
|
let q = QuaternionBase::from_parts(cang, axis.as_ref() * sang);
|
||||||
|
Self::new_unchecked(q)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new unit quaternion from a quaternion.
|
||||||
|
///
|
||||||
|
/// The input quaternion will be normalized.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_quaternion(q: QuaternionBase<N, S>) -> Self {
|
||||||
|
Self::new_normalize(q)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new unit quaternion from Euler angles.
|
||||||
|
///
|
||||||
|
/// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self {
|
||||||
|
let (sr, cr) = (roll * ::convert(0.5f64)).sin_cos();
|
||||||
|
let (sp, cp) = (pitch * ::convert(0.5f64)).sin_cos();
|
||||||
|
let (sy, cy) = (yaw * ::convert(0.5f64)).sin_cos();
|
||||||
|
|
||||||
|
let q = QuaternionBase::new(
|
||||||
|
cr * cp * cy + sr * sp * sy,
|
||||||
|
sr * cp * cy - cr * sp * sy,
|
||||||
|
cr * sp * cy + sr * cp * sy,
|
||||||
|
cr * cp * sy - sr * sp * cy);
|
||||||
|
|
||||||
|
Self::new_unchecked(q)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds an unit quaternion from a rotation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_rotation_matrix<SB>(rotmat: &RotationBase<N, U3, SB>) -> Self
|
||||||
|
where SB: Storage<N, U3, U3>,
|
||||||
|
SB::Alloc: Allocator<N, U3, U1> {
|
||||||
|
let angle = rotmat.angle();
|
||||||
|
|
||||||
|
if let Some(axis) = rotmat.axis() {
|
||||||
|
Self::from_axis_angle(&axis, angle)
|
||||||
|
}
|
||||||
|
else if angle > ::convert(1.0f64) {
|
||||||
|
// The angle is 3.14.
|
||||||
|
-Self::identity()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// The angle is 0.
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
|
||||||
|
/// direction.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_between<SB, SC>(a: &ColumnVector<N, U3, SB>, b: &ColumnVector<N, U3, SC>) -> Option<Self>
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
Self::scaled_rotation_between(a, b, N::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The smallest rotation needed to make `a` and `b` collinear and point toward the same
|
||||||
|
/// direction, raised to the power `s`.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_rotation_between<SB, SC>(a: &ColumnVector<N, U3, SB>, b: &ColumnVector<N, U3, SC>, s: N) -> Option<Self>
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
// FIXME: code duplication with RotationBase.
|
||||||
|
if let (Some(na), Some(nb)) = (a.try_normalize(N::zero()), b.try_normalize(N::zero())) {
|
||||||
|
let c = na.cross(&nb);
|
||||||
|
|
||||||
|
if let Some(axis) = Unit::try_new(c, N::default_epsilon()) {
|
||||||
|
return Some(Self::from_axis_angle(&axis, na.dot(&nb).acos() * s))
|
||||||
|
}
|
||||||
|
|
||||||
|
// Zero or PI.
|
||||||
|
if na.dot(&nb) < N::zero() {
|
||||||
|
// PI
|
||||||
|
//
|
||||||
|
// The rotation axis is undefined but the angle not zero. This is not a
|
||||||
|
// simple rotation.
|
||||||
|
return None;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Some(Self::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// Creates an unit quaternion that corresponds to the local frame of an observer standing at the
|
||||||
|
/// origin and looking toward `dir`.
|
||||||
|
///
|
||||||
|
/// It maps the view direction `dir` to the positive `z` axis.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * dir - The look direction, that is, direction the matrix `z` axis will be aligned with.
|
||||||
|
/// * up - The vertical direction. The only requirement of this parameter is to not be
|
||||||
|
/// collinear
|
||||||
|
/// to `dir`. Non-collinearity is not checked.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_observer_frame<SB, SC>(dir: &ColumnVector<N, U3, SB>, up: &ColumnVector<N, U3, SC>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
Self::from_rotation_matrix(&OwnedRotation::<N, U3, S::Alloc>::new_observer_frame(dir, up))
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// Builds a right-handed look-at view matrix without translation.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of right handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_rh<SB, SC>(dir: &ColumnVector<N, U3, SB>, up: &ColumnVector<N, U3, SC>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
Self::new_observer_frame(&-dir, up).inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a left-handed look-at view matrix without translation.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of left handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_lh<SB, SC>(dir: &ColumnVector<N, U3, SB>, up: &ColumnVector<N, U3, SC>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1>,
|
||||||
|
S::Alloc: Allocator<N, U3, U3> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
Self::new_observer_frame(dir, up).inverse()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
/// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
|
||||||
|
///
|
||||||
|
/// If `axisangle` is zero, this returns the indentity rotation.
|
||||||
|
#[inline]
|
||||||
|
pub fn new<SB>(axisangle: ColumnVector<N, U3, SB>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
let two: N = ::convert(2.0f64);
|
||||||
|
let q = QuaternionBase::<N, S>::from_parts(N::zero(), axisangle / two).exp();
|
||||||
|
Self::new_unchecked(q)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
|
||||||
|
///
|
||||||
|
/// If `axisangle` is zero, this returns the indentity rotation.
|
||||||
|
/// Same as `Self::new(axisangle)`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_scaled_axis<SB>(axisangle: ColumnVector<N, U3, SB>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
Self::new(axisangle)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> One for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Rand for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<R: Rng>(rng: &mut R) -> Self {
|
||||||
|
let axisangle = Vector3::rand(rng);
|
||||||
|
UnitQuaternionBase::from_scaled_axis(axisangle)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, S> Arbitrary for UnitQuaternionBase<N, S>
|
||||||
|
where N: Real + Arbitrary,
|
||||||
|
S: OwnedStorage<N, U4, U1> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
let axisangle = Vector3::arbitrary(g);
|
||||||
|
UnitQuaternionBase::from_scaled_axis(axisangle)
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
212
src/geometry/quaternion_conversion.rs
Normal file
212
src/geometry/quaternion_conversion.rs
Normal file
@ -0,0 +1,212 @@
|
|||||||
|
use num::Zero;
|
||||||
|
|
||||||
|
use alga::general::{SubsetOf, SupersetOf, Real};
|
||||||
|
use alga::linear::Rotation as AlgaRotation;
|
||||||
|
|
||||||
|
use core::{ColumnVector, SquareMatrix};
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use geometry::{PointBase, QuaternionBase, UnitQuaternionBase, OwnedUnitQuaternionBase, RotationBase,
|
||||||
|
OwnedRotation, IsometryBase, SimilarityBase, TransformBase, SuperTCategoryOf, TAffine, TranslationBase};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides the following conversions:
|
||||||
|
* =============================================
|
||||||
|
*
|
||||||
|
* Quaternion -> Quaternion
|
||||||
|
* UnitQuaternion -> UnitQuaternion
|
||||||
|
* UnitQuaternion -> RotationBase<U3>
|
||||||
|
* UnitQuaternion -> IsometryBase<U3>
|
||||||
|
* UnitQuaternion -> SimilarityBase<U3>
|
||||||
|
* UnitQuaternion -> TransformBase<U3>
|
||||||
|
* UnitQuaternion -> Matrix<U4> (homogeneous)
|
||||||
|
*
|
||||||
|
* NOTE:
|
||||||
|
* UnitQuaternion -> Quaternion is already provided by: Unit<T> -> T
|
||||||
|
*/
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB> SubsetOf<QuaternionBase<N2, SB>> for QuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U4, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U4, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> QuaternionBase<N2, SB> {
|
||||||
|
QuaternionBase::from_vector(self.coords.to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(q: &QuaternionBase<N2, SB>) -> bool {
|
||||||
|
::is_convertible::<_, ColumnVector<N1, U4, SA>>(&q.coords)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(q: &QuaternionBase<N2, SB>) -> Self {
|
||||||
|
Self::from_vector(q.coords.to_subset_unchecked())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB> SubsetOf<UnitQuaternionBase<N2, SB>> for UnitQuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U4, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U4, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> UnitQuaternionBase<N2, SB> {
|
||||||
|
UnitQuaternionBase::new_unchecked(self.as_ref().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(uq: &UnitQuaternionBase<N2, SB>) -> bool {
|
||||||
|
::is_convertible::<_, QuaternionBase<N1, SA>>(uq.as_ref())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(uq: &UnitQuaternionBase<N2, SB>) -> Self {
|
||||||
|
Self::new_unchecked(::convert_ref_unchecked(uq.as_ref()))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB> SubsetOf<RotationBase<N2, U3, SB>> for UnitQuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U3, U3>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA> +
|
||||||
|
Allocator<N1, U3, U3>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U3, U3, SB> +
|
||||||
|
Allocator<N2, U4, U1> +
|
||||||
|
Allocator<N2, U3, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> RotationBase<N2, U3, SB> {
|
||||||
|
let q: OwnedUnitQuaternionBase<N2, SB::Alloc> = self.to_superset();
|
||||||
|
q.to_rotation_matrix()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(rot: &RotationBase<N2, U3, SB>) -> bool {
|
||||||
|
::is_convertible::<_, OwnedRotation<N1, U3, SA::Alloc>>(rot)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(rot: &RotationBase<N2, U3, SB>) -> Self {
|
||||||
|
let q = OwnedUnitQuaternionBase::<N2, SB::Alloc>::from_rotation_matrix(rot);
|
||||||
|
::convert_unchecked(q)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB, R> SubsetOf<IsometryBase<N2, U3, SB, R>> for UnitQuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U3, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N2, U3, SB>> + SupersetOf<UnitQuaternionBase<N1, SA>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U3, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> IsometryBase<N2, U3, SB, R> {
|
||||||
|
IsometryBase::from_parts(TranslationBase::identity(), ::convert_ref(self))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(iso: &IsometryBase<N2, U3, SB, R>) -> bool {
|
||||||
|
iso.translation.vector.is_zero()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(iso: &IsometryBase<N2, U3, SB, R>) -> Self {
|
||||||
|
::convert_ref_unchecked(&iso.rotation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB, R> SubsetOf<SimilarityBase<N2, U3, SB, R>> for UnitQuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U3, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N2, U3, SB>> + SupersetOf<UnitQuaternionBase<N1, SA>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U3, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SimilarityBase<N2, U3, SB, R> {
|
||||||
|
SimilarityBase::from_isometry(::convert_ref(self), N2::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(sim: &SimilarityBase<N2, U3, SB, R>) -> bool {
|
||||||
|
sim.isometry.translation.vector.is_zero() &&
|
||||||
|
sim.scaling() == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(sim: &SimilarityBase<N2, U3, SB, R>) -> Self {
|
||||||
|
::convert_ref_unchecked(&sim.isometry)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB, C> SubsetOf<TransformBase<N2, U3, SB, C>> for UnitQuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U4, U4>,
|
||||||
|
C: SuperTCategoryOf<TAffine>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA> +
|
||||||
|
Allocator<N1, U3, U3> +
|
||||||
|
Allocator<N1, U3, U1> +
|
||||||
|
Allocator<N1, U4, U4>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U4, U4, SB> +
|
||||||
|
Allocator<N2, U3, U3> +
|
||||||
|
Allocator<N2, U1, U3> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TransformBase<N2, U3, SB, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(t: &TransformBase<N2, U3, SB, C>) -> bool {
|
||||||
|
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(t: &TransformBase<N2, U3, SB, C>) -> Self {
|
||||||
|
Self::from_superset_unchecked(t.matrix())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB> SubsetOf<SquareMatrix<N2, U4, SB>> for UnitQuaternionBase<N1, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U4, U1>,
|
||||||
|
SB: OwnedStorage<N2, U4, U4>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U4, U1, SA> +
|
||||||
|
Allocator<N1, U3, U3> +
|
||||||
|
Allocator<N1, U3, U1> +
|
||||||
|
Allocator<N1, U4, U4>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U4, U4, SB> +
|
||||||
|
Allocator<N2, U3, U3> +
|
||||||
|
Allocator<N2, U1, U3> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SquareMatrix<N2, U4, SB> {
|
||||||
|
self.to_homogeneous().to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &SquareMatrix<N2, U4, SB>) -> bool {
|
||||||
|
::is_convertible::<_, OwnedRotation<N1, U3, SA::Alloc>>(m)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &SquareMatrix<N2, U4, SB>) -> Self {
|
||||||
|
let rot: OwnedRotation<N1, U3, SA::Alloc> = ::convert_ref_unchecked(m);
|
||||||
|
Self::from_rotation_matrix(&rot)
|
||||||
|
}
|
||||||
|
}
|
34
src/geometry/quaternion_coordinates.rs
Normal file
34
src/geometry/quaternion_coordinates.rs
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
use std::ops::{Deref, DerefMut};
|
||||||
|
use std::mem;
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::coordinates::IJKW;
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
use core::dimension::{U1, U4};
|
||||||
|
|
||||||
|
use geometry::QuaternionBase;
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> Deref for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
type Target = IJKW<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
unsafe { mem::transmute(self) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> DerefMut for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U4, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U4, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
unsafe { mem::transmute(self) }
|
||||||
|
}
|
||||||
|
}
|
729
src/geometry/quaternion_ops.rs
Normal file
729
src/geometry/quaternion_ops.rs
Normal file
@ -0,0 +1,729 @@
|
|||||||
|
/*
|
||||||
|
* This file provides:
|
||||||
|
* ===================
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Quaternion)
|
||||||
|
*
|
||||||
|
* Index<usize>
|
||||||
|
* IndexMut<usize>
|
||||||
|
* Quaternion × Quaternion
|
||||||
|
* Quaternion + Quaternion
|
||||||
|
* Quaternion - Quaternion
|
||||||
|
* -Quaternion
|
||||||
|
* Quaternion × Scalar
|
||||||
|
* Quaternion ÷ Scalar
|
||||||
|
* Scalar × Quaternion
|
||||||
|
*
|
||||||
|
* (Unit Quaternion)
|
||||||
|
* UnitQuaternion × UnitQuaternion
|
||||||
|
* UnitQuaternion × RotationBase -> UnitQuaternion
|
||||||
|
* RotationBase × UnitQuaternion -> UnitQuaternion
|
||||||
|
*
|
||||||
|
* UnitQuaternion ÷ UnitQuaternion
|
||||||
|
* UnitQuaternion ÷ RotationBase -> UnitQuaternion
|
||||||
|
* RotationBase ÷ UnitQuaternion -> UnitQuaternion
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* UnitQuaternion × PointBase
|
||||||
|
* UnitQuaternion × ColumnVector
|
||||||
|
* UnitQuaternion × Unit<ColumnVector>
|
||||||
|
*
|
||||||
|
* NOTE: -UnitQuaternion is already provided by `Unit<T>`.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Assignment Operators)
|
||||||
|
*
|
||||||
|
* Quaternion ×= Scalar
|
||||||
|
* Quaternion ×= Quaternion
|
||||||
|
* Quaternion += Quaternion
|
||||||
|
* Quaternion -= Quaternion
|
||||||
|
*
|
||||||
|
* UnitQuaternion ×= UnitQuaternion
|
||||||
|
* UnitQuaternion ×= RotationBase
|
||||||
|
*
|
||||||
|
* UnitQuaternion ÷= UnitQuaternion
|
||||||
|
* UnitQuaternion ÷= RotationBase
|
||||||
|
*
|
||||||
|
* FIXME: RotationBase ×= UnitQuaternion
|
||||||
|
* FIXME: RotationBase ÷= UnitQuaternion
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
use std::ops::{Index, IndexMut, Neg, Add, AddAssign, Mul, MulAssign, Div, DivAssign, Sub, SubAssign};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{ColumnVector, OwnedColumnVector, Unit};
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
use core::dimension::{U1, U3, U4};
|
||||||
|
|
||||||
|
use geometry::{QuaternionBase, OwnedQuaternionBase, UnitQuaternionBase, OwnedUnitQuaternionBase,
|
||||||
|
PointBase, OwnedPoint, RotationBase};
|
||||||
|
|
||||||
|
impl<N, S> Index<usize> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
type Output = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index(&self, i: usize) -> &N {
|
||||||
|
&self.coords[i]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> IndexMut<usize> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: StorageMut<N, U4, U1> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index_mut(&mut self, i: usize) -> &mut N {
|
||||||
|
&mut self.coords[i]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! quaternion_op_impl(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident);
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
SA: Storage<N, $LhsRDim, $LhsCDim>,
|
||||||
|
SB: Storage<N, $RhsRDim, $RhsCDim>,
|
||||||
|
$(SA::Alloc: Allocator<N, $VDimA, U1>,
|
||||||
|
SB::Alloc: Allocator<N, $VDimB, U1>)* {
|
||||||
|
type Output = $Result;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// Quaternion + Quaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Add, add;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::from_vector(&self.coords + &rhs.coords);
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Add, add;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SB::Alloc>;
|
||||||
|
QuaternionBase::from_vector(&self.coords + rhs.coords);
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Add, add;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::from_vector(self.coords + &rhs.coords);
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Add, add;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::from_vector(self.coords + rhs.coords);
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// Quaternion - Quaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Sub, sub;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::from_vector(&self.coords - &rhs.coords);
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Sub, sub;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SB::Alloc>;
|
||||||
|
QuaternionBase::from_vector(&self.coords - rhs.coords);
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Sub, sub;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::from_vector(self.coords - &rhs.coords);
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Sub, sub;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::from_vector(self.coords - rhs.coords);
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// Quaternion × Quaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
QuaternionBase::new(
|
||||||
|
self[3] * rhs[3] - self[0] * rhs[0] - self[1] * rhs[1] - self[2] * rhs[2],
|
||||||
|
self[3] * rhs[0] + self[0] * rhs[3] + self[1] * rhs[2] - self[2] * rhs[1],
|
||||||
|
self[3] * rhs[1] - self[0] * rhs[2] + self[1] * rhs[3] + self[2] * rhs[0],
|
||||||
|
self[3] * rhs[2] + self[0] * rhs[1] - self[1] * rhs[0] + self[2] * rhs[3]);
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
self * &rhs;
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
&self * rhs;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>, Output = OwnedQuaternionBase<N, SA::Alloc>;
|
||||||
|
&self * &rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion × UnitQuaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
UnitQuaternionBase::new_unchecked(self.quaternion() * rhs.quaternion());
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
self * &rhs;
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
&self * rhs;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
&self * &rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion ÷ UnitQuaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
self * rhs.inverse();
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
self / &rhs;
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
&self / rhs;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: UnitQuaternionBase<N, SB>, Output = OwnedUnitQuaternionBase<N, SA::Alloc>;
|
||||||
|
&self / &rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion × RotationBase
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
// FIXME: can we avoid the conversion from a rotation matrix?
|
||||||
|
self * OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(rhs);
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
self * OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(&rhs);
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
self * OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(rhs);
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
self * OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(&rhs);
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion ÷ RotationBase
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
// FIXME: can we avoid the conversion to a rotation matrix?
|
||||||
|
self / OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(rhs);
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
self / OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(&rhs);
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
self / OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(rhs);
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: RotationBase<N, U3, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SA::Alloc> => U3, U3;
|
||||||
|
self / OwnedUnitQuaternionBase::<N, SA::Alloc>::from_rotation_matrix(&rhs);
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase × UnitQuaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: &'a RotationBase<N, U3, SA>, rhs: &'b UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
// FIXME: can we avoid the conversion from a rotation matrix?
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(self) * rhs;
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: &'a RotationBase<N, U3, SA>, rhs: UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(self) * rhs;
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: RotationBase<N, U3, SA>, rhs: &'b UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(&self) * rhs;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: RotationBase<N, U3, SA>, rhs: UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(&self) * rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase ÷ UnitQuaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: &'a RotationBase<N, U3, SA>, rhs: &'b UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
// FIXME: can we avoid the conversion from a rotation matrix?
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(self) / rhs;
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: &'a RotationBase<N, U3, SA>, rhs: UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(self) / rhs;
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: RotationBase<N, U3, SA>, rhs: &'b UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(&self) / rhs;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Div, div;
|
||||||
|
(U3, U3), (U4, U1);
|
||||||
|
self: RotationBase<N, U3, SA>, rhs: UnitQuaternionBase<N, SB>,
|
||||||
|
Output = OwnedUnitQuaternionBase<N, SB::Alloc> => U3, U3;
|
||||||
|
OwnedUnitQuaternionBase::<N, SB::Alloc>::from_rotation_matrix(&self) / rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion × Vector
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b ColumnVector<N, U3, SB>,
|
||||||
|
Output = OwnedColumnVector<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
{
|
||||||
|
let two: N = ::convert(2.0f64);
|
||||||
|
let t = self.as_ref().vector().cross(rhs) * two;
|
||||||
|
let cross = self.as_ref().vector().cross(&t);
|
||||||
|
|
||||||
|
t * self.as_ref().scalar() + cross + rhs
|
||||||
|
};
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: ColumnVector<N, U3, SB>,
|
||||||
|
Output = OwnedColumnVector<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
self * &rhs;
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b ColumnVector<N, U3, SB>,
|
||||||
|
Output = OwnedColumnVector<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
&self * rhs;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: ColumnVector<N, U3, SB>,
|
||||||
|
Output = OwnedColumnVector<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
&self * &rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion × PointBase
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b PointBase<N, U3, SB>,
|
||||||
|
Output = OwnedPoint<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
PointBase::from_coordinates(self * &rhs.coords);
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: PointBase<N, U3, SB>,
|
||||||
|
Output = OwnedPoint<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
PointBase::from_coordinates(self * rhs.coords);
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b PointBase<N, U3, SB>,
|
||||||
|
Output = OwnedPoint<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
PointBase::from_coordinates(self * &rhs.coords);
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: PointBase<N, U3, SB>,
|
||||||
|
Output = OwnedPoint<N, U3, SA::Alloc> => U3, U4;
|
||||||
|
PointBase::from_coordinates(self * rhs.coords);
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitQuaternion × Unit<Vector>
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: &'b Unit<ColumnVector<N, U3, SB>>,
|
||||||
|
Output = Unit<OwnedColumnVector<N, U3, SA::Alloc>> => U3, U4;
|
||||||
|
Unit::new_unchecked(self * rhs.as_ref());
|
||||||
|
'a, 'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: &'a UnitQuaternionBase<N, SA>, rhs: Unit<ColumnVector<N, U3, SB>>,
|
||||||
|
Output = Unit<OwnedColumnVector<N, U3, SA::Alloc>> => U3, U4;
|
||||||
|
Unit::new_unchecked(self * rhs.unwrap());
|
||||||
|
'a);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b Unit<ColumnVector<N, U3, SB>>,
|
||||||
|
Output = Unit<OwnedColumnVector<N, U3, SA::Alloc>> => U3, U4;
|
||||||
|
Unit::new_unchecked(self * rhs.as_ref());
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: Unit<ColumnVector<N, U3, SB>>,
|
||||||
|
Output = Unit<OwnedColumnVector<N, U3, SA::Alloc>> => U3, U4;
|
||||||
|
Unit::new_unchecked(self * rhs.unwrap());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
macro_rules! scalar_op_impl(
|
||||||
|
($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$(
|
||||||
|
impl<N, S> $Op<N> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
type Output = OwnedQuaternionBase<N, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op(self, n: N) -> Self::Output {
|
||||||
|
QuaternionBase::from_vector(self.coords.$op(n))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, S> $Op<N> for &'a QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
type Output = OwnedQuaternionBase<N, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op(self, n: N) -> Self::Output {
|
||||||
|
QuaternionBase::from_vector((&self.coords).$op(n))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> $OpAssign<N> for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: StorageMut<N, U4, U1> {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op_assign(&mut self, n: N) {
|
||||||
|
self.coords.$op_assign(n)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
scalar_op_impl!(
|
||||||
|
Mul, mul, MulAssign, mul_assign;
|
||||||
|
Div, div, DivAssign, div_assign;
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! left_scalar_mul_impl(
|
||||||
|
($($T: ty),* $(,)*) => {$(
|
||||||
|
impl<S> Mul<QuaternionBase<$T, S>> for $T
|
||||||
|
where S: Storage<$T, U4, U1> {
|
||||||
|
type Output = OwnedQuaternionBase<$T, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: QuaternionBase<$T, S>) -> Self::Output {
|
||||||
|
QuaternionBase::from_vector(self * right.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, S> Mul<&'b QuaternionBase<$T, S>> for $T
|
||||||
|
where S: Storage<$T, U4, U1> {
|
||||||
|
type Output = OwnedQuaternionBase<$T, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, right: &'b QuaternionBase<$T, S>) -> Self::Output {
|
||||||
|
QuaternionBase::from_vector(self * &right.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
left_scalar_mul_impl!(f32, f64);
|
||||||
|
|
||||||
|
impl<N, S> Neg for QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
type Output = OwnedQuaternionBase<N, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
QuaternionBase::from_vector(-self.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N, S> Neg for &'a QuaternionBase<N, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U4, U1> {
|
||||||
|
type Output = OwnedQuaternionBase<N, S::Alloc>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn neg(self) -> Self::Output {
|
||||||
|
QuaternionBase::from_vector(-&self.coords)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! quaternion_op_impl(
|
||||||
|
($OpAssign: ident, $op_assign: ident;
|
||||||
|
($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident);
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N, SA, SB> $OpAssign<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
SA: StorageMut<N, $LhsRDim, $LhsCDim>,
|
||||||
|
SB: Storage<N, $RhsRDim, $RhsCDim>,
|
||||||
|
$(SA::Alloc: Allocator<N, $VDimA, U1> + Allocator<N, U4, U1>,
|
||||||
|
// ^^^^^^^^^^^^^^^^^^^^
|
||||||
|
// XXX: For some reasons, the compiler needs
|
||||||
|
// this bound to compile UnitQuat *= RotationBase.
|
||||||
|
// Though in theory this bound is already
|
||||||
|
// inherited from `SA: StorageMut`…
|
||||||
|
SB::Alloc: Allocator<N, $VDimB, U1>)* {
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op_assign(&mut $lhs, $rhs: $Rhs) {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// Quaternion += Quaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
AddAssign, add_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>;
|
||||||
|
self.coords += &rhs.coords;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
AddAssign, add_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>;
|
||||||
|
self.coords += rhs.coords; );
|
||||||
|
|
||||||
|
|
||||||
|
// Quaternion -= Quaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
SubAssign, sub_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>;
|
||||||
|
self.coords -= &rhs.coords;
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
SubAssign, sub_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>;
|
||||||
|
self.coords -= rhs.coords; );
|
||||||
|
|
||||||
|
// Quaternion ×= Quaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: &'b QuaternionBase<N, SB>;
|
||||||
|
{
|
||||||
|
let res = &*self * rhs;
|
||||||
|
// FIXME: will this be optimized away?
|
||||||
|
self.coords.copy_from(&res.coords);
|
||||||
|
};
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: QuaternionBase<N, SA>, rhs: QuaternionBase<N, SB>;
|
||||||
|
*self *= &rhs; );
|
||||||
|
|
||||||
|
// UnitQuaternion ×= UnitQuaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b UnitQuaternionBase<N, SB>;
|
||||||
|
{
|
||||||
|
let res = &*self * rhs;
|
||||||
|
self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
|
||||||
|
};
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: UnitQuaternionBase<N, SB>;
|
||||||
|
*self *= &rhs; );
|
||||||
|
|
||||||
|
// UnitQuaternion ÷= UnitQuaternion
|
||||||
|
quaternion_op_impl!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b UnitQuaternionBase<N, SB>;
|
||||||
|
{
|
||||||
|
let res = &*self / rhs;
|
||||||
|
self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
|
||||||
|
};
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
(U4, U1), (U4, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: UnitQuaternionBase<N, SB>;
|
||||||
|
*self /= &rhs; );
|
||||||
|
|
||||||
|
// UnitQuaternion ×= RotationBase
|
||||||
|
quaternion_op_impl!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b RotationBase<N, U3, SB> => U3, U3;
|
||||||
|
{
|
||||||
|
let res = &*self * rhs;
|
||||||
|
self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
|
||||||
|
};
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: RotationBase<N, U3, SB> => U3, U3;
|
||||||
|
*self *= &rhs; );
|
||||||
|
|
||||||
|
// UnitQuaternion ÷= RotationBase
|
||||||
|
quaternion_op_impl!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: &'b RotationBase<N, U3, SB> => U3, U3;
|
||||||
|
{
|
||||||
|
let res = &*self / rhs;
|
||||||
|
self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords);
|
||||||
|
};
|
||||||
|
'b);
|
||||||
|
|
||||||
|
quaternion_op_impl!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
(U4, U1), (U3, U3);
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: RotationBase<N, U3, SB> => U3, U3;
|
||||||
|
*self /= &rhs; );
|
173
src/geometry/rotation.rs
Normal file
173
src/geometry/rotation.rs
Normal file
@ -0,0 +1,173 @@
|
|||||||
|
use num::{Zero, One};
|
||||||
|
use std::fmt;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{SquareMatrix, Scalar, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
|
||||||
|
|
||||||
|
/// A rotation matrix with an owned storage.
|
||||||
|
pub type OwnedRotation<N, D, A> = RotationBase<N, D, <A as Allocator<N, D, D>>::Buffer>;
|
||||||
|
|
||||||
|
/// A rotation matrix.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Hash, Debug, Clone, Copy)]
|
||||||
|
pub struct RotationBase<N: Scalar, D: DimName, S: Storage<N, D, D>> {
|
||||||
|
matrix: SquareMatrix<N, D, S>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: Storage<N, D, D>> RotationBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, D, D> {
|
||||||
|
/// A reference to the underlying matrix representation of this rotation.
|
||||||
|
#[inline]
|
||||||
|
pub fn matrix(&self) -> &SquareMatrix<N, D, S> {
|
||||||
|
&self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A mutable reference to the underlying matrix representation of this rotation.
|
||||||
|
///
|
||||||
|
/// This is unsafe because this allows the user to replace the matrix by another one that is
|
||||||
|
/// non-square, non-inversible, or non-orthonormal. If one of those properties is broken,
|
||||||
|
/// subsequent method calls may be UB.
|
||||||
|
#[inline]
|
||||||
|
pub unsafe fn matrix_mut(&mut self) -> &mut SquareMatrix<N, D, S> {
|
||||||
|
&mut self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Unwraps the underlying matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn unwrap(self) -> SquareMatrix<N, D, S> {
|
||||||
|
self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Converts this rotation into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc>
|
||||||
|
where N: Zero + One,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
let mut res = OwnedSquareMatrix::<N, _, S::Alloc>::identity();
|
||||||
|
res.fixed_slice_mut::<D, D>(0, 0).copy_from(&self.matrix);
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: Storage<N, D, D>> RotationBase<N, D, S> {
|
||||||
|
/// Creates a new rotation from the given square matrix.
|
||||||
|
///
|
||||||
|
/// The matrix squareness is checked but not its orthonormality.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_matrix_unchecked(matrix: SquareMatrix<N, D, S>) -> RotationBase<N, D, S> {
|
||||||
|
assert!(matrix.is_square(), "Unable to create a rotation from a non-square matrix.");
|
||||||
|
|
||||||
|
RotationBase {
|
||||||
|
matrix: matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Transposes `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn transpose(&self) -> OwnedRotation<N, D, S::Alloc> {
|
||||||
|
RotationBase::from_matrix_unchecked(self.matrix.transpose())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(&self) -> OwnedRotation<N, D, S::Alloc> {
|
||||||
|
self.transpose()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: StorageMut<N, D, D>> RotationBase<N, D, S> {
|
||||||
|
/// Transposes `self` in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn transpose_mut(&mut self) {
|
||||||
|
self.matrix.transpose_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self` in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self) {
|
||||||
|
self.transpose_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Scalar + Eq, D: DimName, S: Storage<N, D, D>> Eq for RotationBase<N, D, S> { }
|
||||||
|
|
||||||
|
impl<N: Scalar + PartialEq, D: DimName, S: Storage<N, D, D>> PartialEq for RotationBase<N, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &RotationBase<N, D, S>) -> bool {
|
||||||
|
self.matrix == right.matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> ApproxEq for RotationBase<N, D, S>
|
||||||
|
where N: Scalar + ApproxEq,
|
||||||
|
S: Storage<N, D, D>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
type Epsilon = N::Epsilon;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
N::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
N::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
N::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.matrix.relative_eq(&other.matrix, epsilon, max_relative)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.matrix.ulps_eq(&other.matrix, epsilon, max_ulps)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> fmt::Display for RotationBase<N, D, S>
|
||||||
|
where N: Real + fmt::Display,
|
||||||
|
S: Storage<N, D, D>,
|
||||||
|
S::Alloc: Allocator<usize, D, D> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
let precision = f.precision().unwrap_or(3);
|
||||||
|
|
||||||
|
try!(writeln!(f, "RotationBase matrix {{"));
|
||||||
|
try!(write!(f, "{:.*}", precision, self.matrix));
|
||||||
|
writeln!(f, "}}")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// // /*
|
||||||
|
// // *
|
||||||
|
// // * Absolute
|
||||||
|
// // *
|
||||||
|
// // */
|
||||||
|
// // impl<N: Absolute> Absolute for $t<N> {
|
||||||
|
// // type AbsoluteValue = $submatrix<N::AbsoluteValue>;
|
||||||
|
// //
|
||||||
|
// // #[inline]
|
||||||
|
// // fn abs(m: &$t<N>) -> $submatrix<N::AbsoluteValue> {
|
||||||
|
// // Absolute::abs(&m.submatrix)
|
||||||
|
// // }
|
||||||
|
// // }
|
298
src/geometry/rotation_alga.rs
Normal file
298
src/geometry/rotation_alga.rs
Normal file
@ -0,0 +1,298 @@
|
|||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractLoop, AbstractMonoid, AbstractQuasigroup,
|
||||||
|
AbstractSemigroup, Real, Inverse, Multiplicative, Identity, Id};
|
||||||
|
use alga::linear::{Transformation, Similarity, AffineTransformation, Isometry, DirectIsometry,
|
||||||
|
OrthogonalTransformation, ProjectiveTransformation, Rotation};
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{RotationBase, PointBase};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Algebraic structures.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Identity<Multiplicative> for RotationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Inverse<Multiplicative> for RotationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.transpose()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.transpose_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> AbstractMagma<Multiplicative> for RotationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_multiplicative_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimName, S> $marker<$operator> for RotationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_multiplicative_structures!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Transformation groups.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, SA, SB> Transformation<PointBase<N, D, SB>> for RotationBase<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, D, SB>) -> PointBase<N, D, SB> {
|
||||||
|
self * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, D, SB>) -> ColumnVector<N, D, SB> {
|
||||||
|
self * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, SA, SB> ProjectiveTransformation<PointBase<N, D, SB>> for RotationBase<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, D, SB>) -> PointBase<N, D, SB> {
|
||||||
|
PointBase::from_coordinates(self.inverse_transform_vector(&pt.coords))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, D, SB>) -> ColumnVector<N, D, SB> {
|
||||||
|
self.matrix().tr_mul(v)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, SA, SB> AffineTransformation<PointBase<N, D, SB>> for RotationBase<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
type Rotation = Self;
|
||||||
|
type NonUniformScaling = Id;
|
||||||
|
type Translation = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn decompose(&self) -> (Id, Self, Id, Self) {
|
||||||
|
(Id::new(), self.clone(), Id::new(), Self::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_translation(&self, _: &Self::Translation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_translation(&self, _: &Self::Translation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
r * self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
self * r
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, SA, SB> Similarity<PointBase<N, D, SB>> for RotationBase<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
type Scaling = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation(&self) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaling(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! marker_impl(
|
||||||
|
($($Trait: ident),*) => {$(
|
||||||
|
impl<N, D: DimName, SA, SB> $Trait<PointBase<N, D, SB>> for RotationBase<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
marker_impl!(Isometry, DirectIsometry, OrthogonalTransformation);
|
||||||
|
|
||||||
|
|
||||||
|
/// Subgroups of the n-dimensional rotation group `SO(n)`.
|
||||||
|
impl<N, D: DimName, SA, SB> Rotation<PointBase<N, D, SB>> for RotationBase<N, D, SA>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, D, D>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn powf(&self, n: N) -> Option<Self> {
|
||||||
|
// XXX: add the general case.
|
||||||
|
unimplemented!()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation_between(a: &ColumnVector<N, D, SB>, b: &ColumnVector<N, D, SB>) -> Option<Self> {
|
||||||
|
// XXX: add the general case.
|
||||||
|
unimplemented!()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaled_rotation_between(a: &ColumnVector<N, D, SB>, b: &ColumnVector<N, D, SB>, n: N) -> Option<Self> {
|
||||||
|
// XXX: add the general case.
|
||||||
|
unimplemented!()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
impl<N: Real> Matrix for RotationBase<N> {
|
||||||
|
type Field = N;
|
||||||
|
type Row = Matrix<N>;
|
||||||
|
type Column = Matrix<N>;
|
||||||
|
type Transpose = Self;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn nrows(&self) -> usize {
|
||||||
|
self.submatrix.nrows()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ncolumns(&self) -> usize {
|
||||||
|
self.submatrix.ncolumns()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn row(&self, i: usize) -> Self::Row {
|
||||||
|
self.submatrix.row(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn column(&self, i: usize) -> Self::Column {
|
||||||
|
self.submatrix.column(i)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn get(&self, i: usize, j: usize) -> Self::Field {
|
||||||
|
self.submatrix[(i, j)]
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn get_unchecked(&self, i: usize, j: usize) -> Self::Field {
|
||||||
|
self.submatrix.at_fast(i, j)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transpose(&self) -> Self::Transpose {
|
||||||
|
RotationBase::from_matrix_unchecked(self.submatrix.transpose())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real> SquareMatrix for RotationBase<N> {
|
||||||
|
type Vector = Matrix<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn diagonal(&self) -> Self::Coordinates {
|
||||||
|
self.submatrix.diagonal()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn determinant(&self) -> Self::Field {
|
||||||
|
::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn try_inverse(&self) -> Option<Self> {
|
||||||
|
Some(::transpose(self))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
self.transpose_mut();
|
||||||
|
true
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transpose_mut(&mut self) {
|
||||||
|
self.submatrix.transpose_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real> InversibleSquareMatrix for RotationBase<N> { }
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
13
src/geometry/rotation_alias.rs
Normal file
13
src/geometry/rotation_alias.rs
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U2, U3};
|
||||||
|
|
||||||
|
use geometry::RotationBase;
|
||||||
|
|
||||||
|
/// A D-dimensional rotation matrix.
|
||||||
|
pub type Rotation<N, D> = RotationBase<N, D, MatrixArray<N, D, D>>;
|
||||||
|
|
||||||
|
/// A 2-dimensional rotation matrix.
|
||||||
|
pub type Rotation2<N> = Rotation<N, U2>;
|
||||||
|
|
||||||
|
/// A 3-dimensional rotation matrix.
|
||||||
|
pub type Rotation3<N> = Rotation<N, U3>;
|
31
src/geometry/rotation_construction.rs
Normal file
31
src/geometry/rotation_construction.rs
Normal file
@ -0,0 +1,31 @@
|
|||||||
|
use num::{Zero, One};
|
||||||
|
|
||||||
|
use alga::general::{ClosedAdd, ClosedMul};
|
||||||
|
|
||||||
|
use core::{SquareMatrix, Scalar};
|
||||||
|
use core::dimension::DimName;
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::RotationBase;
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> RotationBase<N, D, S>
|
||||||
|
where N: Scalar + Zero + One,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
/// Creates a new square identity rotation of the given `dimension`.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> RotationBase<N, D, S> {
|
||||||
|
Self::from_matrix_unchecked(SquareMatrix::<N, D, S>::identity())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> One for RotationBase<N, D, S>
|
||||||
|
where N: Scalar + Zero + One + ClosedAdd + ClosedMul,
|
||||||
|
S: OwnedStorage<N, D, D>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, D, S> {
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
195
src/geometry/rotation_conversion.rs
Normal file
195
src/geometry/rotation_conversion.rs
Normal file
@ -0,0 +1,195 @@
|
|||||||
|
use num::Zero;
|
||||||
|
|
||||||
|
use alga::general::{Real, SubsetOf, SupersetOf};
|
||||||
|
use alga::linear::Rotation as AlgaRotation;
|
||||||
|
|
||||||
|
use core::{Matrix, SquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1, U3, U4};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{OwnedAllocator, Allocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, TranslationBase, RotationBase, UnitQuaternionBase, OwnedUnitQuaternionBase, IsometryBase,
|
||||||
|
SimilarityBase, TransformBase, SuperTCategoryOf, TAffine};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides the following conversions:
|
||||||
|
* =============================================
|
||||||
|
*
|
||||||
|
* RotationBase -> RotationBase
|
||||||
|
* Rotation3 -> UnitQuaternion
|
||||||
|
* RotationBase -> IsometryBase
|
||||||
|
* RotationBase -> SimilarityBase
|
||||||
|
* RotationBase -> TransformBase
|
||||||
|
* RotationBase -> Matrix (homogeneous)
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB> SubsetOf<RotationBase<N2, D, SB>> for RotationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, D>,
|
||||||
|
SB: OwnedStorage<N2, D, D>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, D, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> RotationBase<N2, D, SB> {
|
||||||
|
RotationBase::from_matrix_unchecked(self.matrix().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(rot: &RotationBase<N2, D, SB>) -> bool {
|
||||||
|
::is_convertible::<_, Matrix<N1, D, D, SA>>(rot.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(rot: &RotationBase<N2, D, SB>) -> Self {
|
||||||
|
RotationBase::from_matrix_unchecked(rot.matrix().to_subset_unchecked())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, SA, SB> SubsetOf<UnitQuaternionBase<N2, SB>> for RotationBase<N1, U3, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, U3, U3>,
|
||||||
|
SB: OwnedStorage<N2, U4, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, U3, U3, SA> +
|
||||||
|
Allocator<N1, U4, U1> +
|
||||||
|
Allocator<N1, U3, U1>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, U4, U1, SB> +
|
||||||
|
Allocator<N2, U3, U3> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> UnitQuaternionBase<N2, SB> {
|
||||||
|
let q = OwnedUnitQuaternionBase::<N1, SA::Alloc>::from_rotation_matrix(self);
|
||||||
|
q.to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(q: &UnitQuaternionBase<N2, SB>) -> bool {
|
||||||
|
::is_convertible::<_, OwnedUnitQuaternionBase<N1, SA::Alloc>>(q)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(q: &UnitQuaternionBase<N2, SB>) -> Self {
|
||||||
|
let q: OwnedUnitQuaternionBase<N1, SA::Alloc> = ::convert_ref_unchecked(q);
|
||||||
|
q.to_rotation_matrix()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R> SubsetOf<IsometryBase<N2, D, SB, R>> for RotationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, D>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N2, D, SB>> + SupersetOf<RotationBase<N1, D, SA>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> IsometryBase<N2, D, SB, R> {
|
||||||
|
IsometryBase::from_parts(TranslationBase::identity(), ::convert_ref(self))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(iso: &IsometryBase<N2, D, SB, R>) -> bool {
|
||||||
|
iso.translation.vector.is_zero()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(iso: &IsometryBase<N2, D, SB, R>) -> Self {
|
||||||
|
::convert_ref_unchecked(&iso.rotation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R> SubsetOf<SimilarityBase<N2, D, SB, R>> for RotationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, D>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N2, D, SB>> + SupersetOf<RotationBase<N1, D, SA>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, D, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SimilarityBase<N2, D, SB, R> {
|
||||||
|
SimilarityBase::from_parts(TranslationBase::identity(), ::convert_ref(self), N2::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(sim: &SimilarityBase<N2, D, SB, R>) -> bool {
|
||||||
|
sim.isometry.translation.vector.is_zero() &&
|
||||||
|
sim.scaling() == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(sim: &SimilarityBase<N2, D, SB, R>) -> Self {
|
||||||
|
::convert_ref_unchecked(&sim.isometry.rotation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB, C> SubsetOf<TransformBase<N2, D, SB, C>> for RotationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, D>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: SuperTCategoryOf<TAffine>,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, D, SA> +
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, D> +
|
||||||
|
Allocator<N2, U1, D> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TransformBase<N2, D, SB, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(t: &TransformBase<N2, D, SB, C>) -> bool {
|
||||||
|
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(t: &TransformBase<N2, D, SB, C>) -> Self {
|
||||||
|
Self::from_superset_unchecked(t.matrix())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB> SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>> for RotationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, D>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, D, SA> +
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, D> +
|
||||||
|
Allocator<N2, U1, D> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SquareMatrix<N2, DimNameSum<D, U1>, SB> {
|
||||||
|
self.to_homogeneous().to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> bool {
|
||||||
|
let rot = m.fixed_slice::<D, D>(0, 0);
|
||||||
|
let bottom = m.fixed_slice::<U1, D>(D::dim(), 0);
|
||||||
|
|
||||||
|
// Scalar types agree.
|
||||||
|
m.iter().all(|e| SupersetOf::<N1>::is_in_subset(e)) &&
|
||||||
|
// The block part is a rotation.
|
||||||
|
rot.is_special_orthogonal(N2::default_epsilon() * ::convert(100.0)) &&
|
||||||
|
// The bottom row is (0, 0, ..., 1)
|
||||||
|
bottom.iter().all(|e| e.is_zero()) &&
|
||||||
|
m[(D::dim(), D::dim())] == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> Self {
|
||||||
|
let r = m.fixed_slice::<D, D>(0, 0);
|
||||||
|
Self::from_matrix_unchecked(::convert_unchecked(r.into_owned()))
|
||||||
|
}
|
||||||
|
}
|
162
src/geometry/rotation_ops.rs
Normal file
162
src/geometry/rotation_ops.rs
Normal file
@ -0,0 +1,162 @@
|
|||||||
|
/*
|
||||||
|
*
|
||||||
|
* This provides the following operator overladings:
|
||||||
|
*
|
||||||
|
* Index<(usize, usize)>
|
||||||
|
*
|
||||||
|
* RotationBase × RotationBase
|
||||||
|
* RotationBase ÷ RotationBase
|
||||||
|
* RotationBase × Matrix
|
||||||
|
* Matrix × RotationBase
|
||||||
|
* Matrix ÷ RotationBase
|
||||||
|
* RotationBase × PointBase
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* RotationBase ×= RotationBase
|
||||||
|
* Matrix ×= RotationBase
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
use std::ops::{Mul, MulAssign, Div, DivAssign, Index};
|
||||||
|
use num::Zero;
|
||||||
|
|
||||||
|
use alga::general::{ClosedMul, ClosedAdd};
|
||||||
|
|
||||||
|
use core::{Scalar, Matrix, MatrixMul};
|
||||||
|
use core::dimension::{Dim, DimName, U1};
|
||||||
|
use core::constraint::{ShapeConstraint, AreMultipliable};
|
||||||
|
use core::storage::{OwnedStorage, Storage};
|
||||||
|
use core::allocator::{OwnedAllocator, Allocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, PointMul, RotationBase, OwnedRotation};
|
||||||
|
|
||||||
|
impl<N: Scalar, D: DimName, S: Storage<N, D, D>> Index<(usize, usize)> for RotationBase<N, D, S> {
|
||||||
|
type Output = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index(&self, row_col: (usize, usize)) -> &N {
|
||||||
|
self.matrix().index(row_col)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// RotationBase × RotationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(D, D), (D, D) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: RotationBase<N, D, SB>, Output = OwnedRotation<N, D, SA::Alloc>;
|
||||||
|
[val val] => RotationBase::from_matrix_unchecked(self.unwrap() * right.unwrap());
|
||||||
|
[ref val] => RotationBase::from_matrix_unchecked(self.matrix() * right.unwrap());
|
||||||
|
[val ref] => RotationBase::from_matrix_unchecked(self.unwrap() * right.matrix());
|
||||||
|
[ref ref] => RotationBase::from_matrix_unchecked(self.matrix() * right.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase ÷ RotationBase
|
||||||
|
// FIXME: instead of calling inverse explicitely, could we just add a `mul_tr` or `mul_inv` method?
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(D, D), (D, D) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: RotationBase<N, D, SB>, Output = OwnedRotation<N, D, SA::Alloc>;
|
||||||
|
[val val] => self * right.inverse();
|
||||||
|
[ref val] => self * right.inverse();
|
||||||
|
[val ref] => self * right.inverse();
|
||||||
|
[ref ref] => self * right.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase × Matrix
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(D1, D1), (R2, C2) for D1: DimName, R2: Dim, C2: Dim
|
||||||
|
where SA::Alloc: Allocator<N, D1, C2>
|
||||||
|
where ShapeConstraint: AreMultipliable<D1, D1, R2, C2>;
|
||||||
|
self: RotationBase<N, D1, SA>, right: Matrix<N, R2, C2, SB>, Output = MatrixMul<N, D1, D1, C2 , SA>;
|
||||||
|
[val val] => self.unwrap() * right;
|
||||||
|
[ref val] => self.matrix() * right;
|
||||||
|
[val ref] => self.unwrap() * right;
|
||||||
|
[ref ref] => self.matrix() * right;
|
||||||
|
);
|
||||||
|
|
||||||
|
// Matrix × RotationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(R1, C1), (D2, D2) for R1: Dim, C1: Dim, D2: DimName
|
||||||
|
where SA::Alloc: Allocator<N, R1, D2>
|
||||||
|
where ShapeConstraint: AreMultipliable<R1, C1, D2, D2>;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: RotationBase<N, D2, SB>, Output = MatrixMul<N, R1, C1, D2, SA>;
|
||||||
|
[val val] => self * right.unwrap();
|
||||||
|
[ref val] => self * right.unwrap();
|
||||||
|
[val ref] => self * right.matrix();
|
||||||
|
[ref ref] => self * right.matrix();
|
||||||
|
);
|
||||||
|
|
||||||
|
// Matrix ÷ RotationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(R1, C1), (D2, D2) for R1: Dim, C1: Dim, D2: DimName
|
||||||
|
where SA::Alloc: Allocator<N, R1, D2>
|
||||||
|
where ShapeConstraint: AreMultipliable<R1, C1, D2, D2>;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: RotationBase<N, D2, SB>, Output = MatrixMul<N, R1, C1, D2, SA>;
|
||||||
|
[val val] => self * right.inverse();
|
||||||
|
[ref val] => self * right.inverse();
|
||||||
|
[val ref] => self * right.inverse();
|
||||||
|
[ref ref] => self * right.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase × PointBase
|
||||||
|
// FIXME: we don't handle properly non-zero origins here. Do we want this to be the intended
|
||||||
|
// behavior?
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(D, D), (D, U1) for D: DimName
|
||||||
|
where SA::Alloc: Allocator<N, D, U1>
|
||||||
|
where ShapeConstraint: AreMultipliable<D, D, D, U1>;
|
||||||
|
self: RotationBase<N, D, SA>, right: PointBase<N, D, SB>, Output = PointMul<N, D, D, SA>;
|
||||||
|
[val val] => self.unwrap() * right;
|
||||||
|
[ref val] => self.matrix() * right;
|
||||||
|
[val ref] => self.unwrap() * right;
|
||||||
|
[ref ref] => self.matrix() * right;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase *= RotationBase
|
||||||
|
// FIXME: try not to call `inverse()` explicitly.
|
||||||
|
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(D, D), (D, D) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: RotationBase<N, D, SB>;
|
||||||
|
[val] => unsafe { self.matrix_mut().mul_assign(right.unwrap()) };
|
||||||
|
[ref] => unsafe { self.matrix_mut().mul_assign(right.matrix()) };
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
md_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
(D, D), (D, D) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: RotationBase<N, D, SB>;
|
||||||
|
[val] => unsafe { self.matrix_mut().mul_assign(right.inverse().unwrap()) };
|
||||||
|
[ref] => unsafe { self.matrix_mut().mul_assign(right.inverse().matrix()) };
|
||||||
|
);
|
||||||
|
|
||||||
|
// Matrix *= RotationBase
|
||||||
|
// FIXME: try not to call `inverse()` explicitly.
|
||||||
|
// FIXME: this shares the same limitations as for the current impl. of MulAssign for matrices.
|
||||||
|
// (In particular the number of matrix column must be equal to the number of rotation columns,
|
||||||
|
// i.e., equal to the rotation dimension.
|
||||||
|
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(R1, C1), (C1, C1) for R1: DimName, C1: DimName;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: RotationBase<N, C1, SB>;
|
||||||
|
[val] => self.mul_assign(right.unwrap());
|
||||||
|
[ref] => self.mul_assign(right.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
md_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
(R1, C1), (C1, C1) for R1: DimName, C1: DimName;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: RotationBase<N, C1, SB>;
|
||||||
|
[val] => self.mul_assign(right.inverse().unwrap());
|
||||||
|
[ref] => self.mul_assign(right.inverse().matrix());
|
||||||
|
);
|
377
src/geometry/rotation_specialization.rs
Normal file
377
src/geometry/rotation_specialization.rs
Normal file
@ -0,0 +1,377 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use std::ops::Neg;
|
||||||
|
use num::Zero;
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
use alga::general::Real;
|
||||||
|
|
||||||
|
use core::{Unit, ColumnVector, SquareMatrix, OwnedSquareMatrix, OwnedColumnVector, Vector3};
|
||||||
|
use core::dimension::{U1, U2, U3};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
use geometry::{RotationBase, OwnedRotation, UnitComplex};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* 2D RotationBase matrix.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, S> RotationBase<N, U2, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U2>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U2, S> {
|
||||||
|
/// Builds a 2 dimensional rotation matrix from an angle in radian.
|
||||||
|
pub fn new(angle: N) -> Self {
|
||||||
|
let (sia, coa) = angle.sin_cos();
|
||||||
|
Self::from_matrix_unchecked(SquareMatrix::<N, U2, S>::new(coa, -sia, sia, coa))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a 2 dimensional rotation matrix from an angle in radian wrapped in a 1-dimensional vector.
|
||||||
|
///
|
||||||
|
/// Equivalent to `Self::new(axisangle[0])`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_scaled_axis<SB: Storage<N, U1, U1>>(axisangle: ColumnVector<N, U1, SB>) -> Self {
|
||||||
|
Self::new(axisangle[0])
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation matrix required to align `a` and `b` but with its angl.
|
||||||
|
///
|
||||||
|
/// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_between<SB, SC>(a: &ColumnVector<N, U2, SB>, b: &ColumnVector<N, U2, SC>) -> Self
|
||||||
|
where SB: Storage<N, U2, U1>,
|
||||||
|
SC: Storage<N, U2, U1> {
|
||||||
|
UnitComplex::rotation_between(a, b).to_rotation_matrix()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The smallest rotation needed to make `a` and `b` collinear and point toward the same
|
||||||
|
/// direction, raised to the power `s`.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_rotation_between<SB, SC>(a: &ColumnVector<N, U2, SB>, b: &ColumnVector<N, U2, SC>, s: N) -> Self
|
||||||
|
where SB: Storage<N, U2, U1>,
|
||||||
|
SC: Storage<N, U2, U1> {
|
||||||
|
UnitComplex::scaled_rotation_between(a, b, s).to_rotation_matrix()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> RotationBase<N, U2, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U2, U2> {
|
||||||
|
/// The rotation angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle(&self) -> N {
|
||||||
|
self.matrix()[(1, 0)].atan2(self.matrix()[(0, 0)])
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation angle needed to make `self` and `other` coincide.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle_to<SB: Storage<N, U2, U2>>(&self, other: &RotationBase<N, U2, SB>) -> N {
|
||||||
|
self.rotation_to(other).angle()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation matrix needed to make `self` and `other` coincide.
|
||||||
|
///
|
||||||
|
/// The result is such that: `self.rotation_to(other) * self == other`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_to<SB>(&self, other: &RotationBase<N, U2, SB>) -> OwnedRotation<N, U2, SB::Alloc>
|
||||||
|
where SB: Storage<N, U2, U2> {
|
||||||
|
other * self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn powf(&self, n: N) -> OwnedRotation<N, U2, S::Alloc> {
|
||||||
|
OwnedRotation::<_, _, S::Alloc>::new(self.angle() * n)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation angle returned as a 1-dimensional vector.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_axis(&self) -> OwnedColumnVector<N, U1, S::Alloc>
|
||||||
|
where S::Alloc: Allocator<N, U1, U1> {
|
||||||
|
ColumnVector::<_, U1, _>::new(self.angle())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Rand for RotationBase<N, U2, S>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, U2, U2>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U2, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<R: Rng>(rng: &mut R) -> Self {
|
||||||
|
Self::new(rng.gen())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, S> Arbitrary for RotationBase<N, U2, S>
|
||||||
|
where N: Real + Arbitrary,
|
||||||
|
S: OwnedStorage<N, U2, U2> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U2, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
Self::new(N::arbitrary(g))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* 3D RotationBase matrix.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, S> RotationBase<N, U3, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U3, U3>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U3, S> {
|
||||||
|
|
||||||
|
/// Builds a 3 dimensional rotation matrix from an axis and an angle.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * `axisangle` - A vector representing the rotation. Its magnitude is the amount of rotation
|
||||||
|
/// in radian. Its direction is the axis of rotation.
|
||||||
|
pub fn new<SB: Storage<N, U3, U1>>(axisangle: ColumnVector<N, U3, SB>) -> Self {
|
||||||
|
let (axis, angle) = Unit::new_and_get(axisangle.into_owned());
|
||||||
|
Self::from_axis_angle(&axis, angle)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a 3D rotation matrix from an axis scaled by the rotation angle.
|
||||||
|
pub fn from_scaled_axis<SB: Storage<N, U3, U1>>(axisangle: ColumnVector<N, U3, SB>) -> Self {
|
||||||
|
Self::new(axisangle)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a 3D rotation matrix from an axis and a rotation angle.
|
||||||
|
pub fn from_axis_angle<SB>(axis: &Unit<ColumnVector<N, U3, SB>>, angle: N) -> Self
|
||||||
|
where SB: Storage<N, U3, U1> {
|
||||||
|
if angle.is_zero() {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
let ux = axis.as_ref()[0];
|
||||||
|
let uy = axis.as_ref()[1];
|
||||||
|
let uz = axis.as_ref()[2];
|
||||||
|
let sqx = ux * ux;
|
||||||
|
let sqy = uy * uy;
|
||||||
|
let sqz = uz * uz;
|
||||||
|
let (sin, cos) = angle.sin_cos();
|
||||||
|
let one_m_cos = N::one() - cos;
|
||||||
|
|
||||||
|
Self::from_matrix_unchecked(
|
||||||
|
SquareMatrix::<N, U3, S>::new(
|
||||||
|
(sqx + (N::one() - sqx) * cos),
|
||||||
|
(ux * uy * one_m_cos - uz * sin),
|
||||||
|
(ux * uz * one_m_cos + uy * sin),
|
||||||
|
|
||||||
|
(ux * uy * one_m_cos + uz * sin),
|
||||||
|
(sqy + (N::one() - sqy) * cos),
|
||||||
|
(uy * uz * one_m_cos - ux * sin),
|
||||||
|
|
||||||
|
(ux * uz * one_m_cos - uy * sin),
|
||||||
|
(uy * uz * one_m_cos + ux * sin),
|
||||||
|
(sqz + (N::one() - sqz) * cos)))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new rotation from Euler angles.
|
||||||
|
///
|
||||||
|
/// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
|
||||||
|
pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self {
|
||||||
|
let (sr, cr) = roll.sin_cos();
|
||||||
|
let (sp, cp) = pitch.sin_cos();
|
||||||
|
let (sy, cy) = yaw.sin_cos();
|
||||||
|
|
||||||
|
Self::from_matrix_unchecked(
|
||||||
|
SquareMatrix::<N, U3, S>::new(
|
||||||
|
cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
|
||||||
|
sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr,
|
||||||
|
-sp, cp * sr, cp * cr)
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a rotation that corresponds to the local frame of an observer standing at the
|
||||||
|
/// origin and looking toward `dir`.
|
||||||
|
///
|
||||||
|
/// It maps the view direction `dir` to the positive `z` axis.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * dir - The look direction, that is, direction the matrix `z` axis will be aligned with.
|
||||||
|
/// * up - The vertical direction. The only requirement of this parameter is to not be
|
||||||
|
/// collinear
|
||||||
|
/// to `dir`. Non-collinearity is not checked.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_observer_frame<SB, SC>(dir: &ColumnVector<N, U3, SB>, up: &ColumnVector<N, U3, SC>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
let zaxis = dir.normalize();
|
||||||
|
let xaxis = up.cross(&zaxis).normalize();
|
||||||
|
let yaxis = zaxis.cross(&xaxis).normalize();
|
||||||
|
|
||||||
|
Self::from_matrix_unchecked(SquareMatrix::<N, U3, S>::new(
|
||||||
|
xaxis.x, yaxis.x, zaxis.x,
|
||||||
|
xaxis.y, yaxis.y, zaxis.y,
|
||||||
|
xaxis.z, yaxis.z, zaxis.z))
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// Builds a right-handed look-at view matrix without translation.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of right handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_rh<SB, SC>(dir: &ColumnVector<N, U3, SB>, up: &ColumnVector<N, U3, SC>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
Self::new_observer_frame(&dir.neg(), up).inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a left-handed look-at view matrix without translation.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of left handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_lh<SB, SC>(dir: &ColumnVector<N, U3, SB>, up: &ColumnVector<N, U3, SC>) -> Self
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
Self::new_observer_frame(dir, up).inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation matrix required to align `a` and `b` but with its angl.
|
||||||
|
///
|
||||||
|
/// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_between<SB, SC>(a: &ColumnVector<N, U3, SB>, b: &ColumnVector<N, U3, SC>) -> Option<Self>
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
Self::scaled_rotation_between(a, b, N::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The smallest rotation needed to make `a` and `b` collinear and point toward the same
|
||||||
|
/// direction, raised to the power `s`.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_rotation_between<SB, SC>(a: &ColumnVector<N, U3, SB>, b: &ColumnVector<N, U3, SC>, n: N)
|
||||||
|
-> Option<Self>
|
||||||
|
where SB: Storage<N, U3, U1>,
|
||||||
|
SC: Storage<N, U3, U1> {
|
||||||
|
// FIXME: code duplication with RotationBase.
|
||||||
|
if let (Some(na), Some(nb)) = (a.try_normalize(N::zero()), b.try_normalize(N::zero())) {
|
||||||
|
let c = na.cross(&nb);
|
||||||
|
|
||||||
|
if let Some(axis) = Unit::try_new(c, N::default_epsilon()) {
|
||||||
|
return Some(Self::from_axis_angle(&axis, na.dot(&nb).acos() * n))
|
||||||
|
}
|
||||||
|
|
||||||
|
// Zero or PI.
|
||||||
|
if na.dot(&nb) < N::zero() {
|
||||||
|
// PI
|
||||||
|
//
|
||||||
|
// The rotation axis is undefined but the angle not zero. This is not a
|
||||||
|
// simple rotation.
|
||||||
|
return None;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Some(Self::identity())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> RotationBase<N, U3, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U3, U3> {
|
||||||
|
/// The rotation angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle(&self) -> N {
|
||||||
|
((self.matrix()[(0, 0)] + self.matrix()[(1, 1)] + self.matrix()[(2, 2)] - N::one()) / ::convert(2.0)).acos()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> RotationBase<N, U3, S>
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, U3, U3>,
|
||||||
|
S::Alloc: Allocator<N, U3, U1> {
|
||||||
|
/// The rotation axis. Returns `None` if the rotation angle is zero or PI.
|
||||||
|
#[inline]
|
||||||
|
pub fn axis(&self) -> Option<Unit<OwnedColumnVector<N, U3, S::Alloc>>> {
|
||||||
|
let axis = OwnedColumnVector::<N, U3, S::Alloc>::new(
|
||||||
|
self.matrix()[(2, 1)] - self.matrix()[(1, 2)],
|
||||||
|
self.matrix()[(0, 2)] - self.matrix()[(2, 0)],
|
||||||
|
self.matrix()[(1, 0)] - self.matrix()[(0, 1)]);
|
||||||
|
|
||||||
|
Unit::try_new(axis, N::default_epsilon())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation axis multiplied by the rotation angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_axis(&self) -> OwnedColumnVector<N, U3, S::Alloc> {
|
||||||
|
if let Some(axis) = self.axis() {
|
||||||
|
axis.unwrap() * self.angle()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
ColumnVector::zero()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation angle needed to make `self` and `other` coincide.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle_to<SB: Storage<N, U3, U3>>(&self, other: &RotationBase<N, U3, SB>) -> N {
|
||||||
|
self.rotation_to(other).angle()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation matrix needed to make `self` and `other` coincide.
|
||||||
|
///
|
||||||
|
/// The result is such that: `self.rotation_to(other) * self == other`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_to<SB>(&self, other: &RotationBase<N, U3, SB>) -> OwnedRotation<N, U3, SB::Alloc>
|
||||||
|
where SB: Storage<N, U3, U3> {
|
||||||
|
other * self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn powf(&self, n: N) -> OwnedRotation<N, U3, S::Alloc> {
|
||||||
|
if let Some(axis) = self.axis() {
|
||||||
|
OwnedRotation::<_, _, S::Alloc>::from_axis_angle(&axis, self.angle() * n)
|
||||||
|
}
|
||||||
|
else if self.matrix()[(0, 0)] < N::zero() {
|
||||||
|
let minus_id = OwnedSquareMatrix::<N, U3, S::Alloc>::from_diagonal_element(-N::one());
|
||||||
|
OwnedRotation::<_, _, S::Alloc>::from_matrix_unchecked(minus_id)
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
OwnedRotation::<_, _, S::Alloc>::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Rand for RotationBase<N, U3, S>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, U3, U3>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U3, S> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<R: Rng>(rng: &mut R) -> Self {
|
||||||
|
Self::new(Vector3::rand(rng))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N, S> Arbitrary for RotationBase<N, U3, S>
|
||||||
|
where N: Real + Arbitrary,
|
||||||
|
S: OwnedStorage<N, U3, U3> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U3, S> +
|
||||||
|
Allocator<N, U3, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
Self::new(Vector3::arbitrary(g))
|
||||||
|
}
|
||||||
|
}
|
269
src/geometry/similarity.rs
Normal file
269
src/geometry/similarity.rs
Normal file
@ -0,0 +1,269 @@
|
|||||||
|
use std::fmt;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{ClosedMul, Real, SubsetOf};
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::{Scalar, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::{Storage, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use geometry::{PointBase, TranslationBase, IsometryBase};
|
||||||
|
|
||||||
|
/// A similarity, i.e., an uniform scaling, followed by a rotation, followed by a translation.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Hash, Debug, Clone, Copy)]
|
||||||
|
pub struct SimilarityBase<N: Scalar, D: DimName, S, R> {
|
||||||
|
pub isometry: IsometryBase<N, D, S, R>,
|
||||||
|
scaling: N
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new similarity from its rotational and translational parts.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_parts(translation: TranslationBase<N, D, S>, rotation: R, scaling: N) -> SimilarityBase<N, D, S, R> {
|
||||||
|
SimilarityBase::from_isometry(IsometryBase::from_parts(translation, rotation), scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new similarity from its rotational and translational parts.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_isometry(isometry: IsometryBase<N, D, S, R>, scaling: N) -> SimilarityBase<N, D, S, R> {
|
||||||
|
assert!(!relative_eq!(scaling, N::zero()), "The scaling factor must not be zero.");
|
||||||
|
|
||||||
|
SimilarityBase {
|
||||||
|
isometry: isometry,
|
||||||
|
scaling: scaling
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new similarity that applies only a scaling factor.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_scaling(scaling: N) -> SimilarityBase<N, D, S, R> {
|
||||||
|
Self::from_isometry(IsometryBase::identity(), scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(&self) -> SimilarityBase<N, D, S, R> {
|
||||||
|
let mut res = self.clone();
|
||||||
|
res.inverse_mut();
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self` in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self) {
|
||||||
|
self.scaling = N::one() / self.scaling;
|
||||||
|
self.isometry.inverse_mut();
|
||||||
|
self.isometry.translation.vector *= self.scaling;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The scaling factor of this similarity transformation.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaling(&self) -> N {
|
||||||
|
self.scaling
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The scaling factor of this similarity transformation.
|
||||||
|
#[inline]
|
||||||
|
pub fn set_scaling(&mut self, scaling: N) {
|
||||||
|
assert!(!relative_eq!(scaling, N::zero()), "The similarity scaling factor must not be zero.");
|
||||||
|
|
||||||
|
self.scaling = scaling;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The similarity transformation that applies a scaling factor `scaling` before `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_scaling(&self, scaling: N) -> Self {
|
||||||
|
assert!(!relative_eq!(scaling, N::zero()), "The similarity scaling factor must not be zero.");
|
||||||
|
|
||||||
|
Self::from_isometry(self.isometry.clone(), self.scaling * scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The similarity transformation that applies a scaling factor `scaling` after `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_scaling(&self, scaling: N) -> Self {
|
||||||
|
assert!(!relative_eq!(scaling, N::zero()), "The similarity scaling factor must not be zero.");
|
||||||
|
|
||||||
|
Self::from_parts(
|
||||||
|
TranslationBase::from_vector(&self.isometry.translation.vector * scaling),
|
||||||
|
self.isometry.rotation.clone(),
|
||||||
|
self.scaling * scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets `self` to the similarity transformation that applies a scaling factor `scaling` before `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn prepend_scaling_mut(&mut self, scaling: N) {
|
||||||
|
assert!(!relative_eq!(scaling, N::zero()), "The similarity scaling factor must not be zero.");
|
||||||
|
|
||||||
|
self.scaling *= scaling
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets `self` to the similarity transformation that applies a scaling factor `scaling` after `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_scaling_mut(&mut self, scaling: N) {
|
||||||
|
assert!(!relative_eq!(scaling, N::zero()), "The similarity scaling factor must not be zero.");
|
||||||
|
|
||||||
|
self.isometry.translation.vector *= scaling;
|
||||||
|
self.scaling *= scaling;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends to `self` the given translation in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_translation_mut(&mut self, t: &TranslationBase<N, D, S>) {
|
||||||
|
self.isometry.append_translation_mut(t)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends to `self` the given rotation in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_rotation_mut(&mut self, r: &R) {
|
||||||
|
self.isometry.append_rotation_mut(r)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends in-place to `self` a rotation centered at the point `p`, i.e., the rotation that
|
||||||
|
/// lets `p` invariant.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_rotation_wrt_point_mut(&mut self, r: &R, p: &PointBase<N, D, S>) {
|
||||||
|
self.isometry.append_rotation_wrt_point_mut(r, p)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Appends in-place to `self` a rotation centered at the point with coordinates
|
||||||
|
/// `self.translation`.
|
||||||
|
#[inline]
|
||||||
|
pub fn append_rotation_wrt_center_mut(&mut self, r: &R) {
|
||||||
|
self.isometry.append_rotation_wrt_center_mut(r)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// NOTE: we don't require `R: Rotation<...>` here becaus this is not useful for the implementation
|
||||||
|
// and makes it harde to use it, e.g., for Transform × Isometry implementation.
|
||||||
|
// This is OK since all constructors of the isometry enforce the Rotation bound already (and
|
||||||
|
// explicit struct construction is prevented by the private scaling factor).
|
||||||
|
impl<N, D: DimName, S, R> SimilarityBase<N, D, S, R>
|
||||||
|
where N: Scalar + ClosedMul,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
/// Converts this similarity into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc>
|
||||||
|
where D: DimNameAdd<U1>,
|
||||||
|
R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc>>,
|
||||||
|
S::Alloc: Allocator<N, D, D> +
|
||||||
|
Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
let mut res = self.isometry.to_homogeneous();
|
||||||
|
|
||||||
|
for e in res.fixed_slice_mut::<D, D>(0, 0).iter_mut() {
|
||||||
|
*e *= self.scaling
|
||||||
|
}
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Eq for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + Eq,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> PartialEq for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + PartialEq,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &SimilarityBase<N, D, S, R>) -> bool {
|
||||||
|
self.isometry == right.isometry && self.scaling == right.scaling
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> ApproxEq for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + ApproxEq<Epsilon = N::Epsilon>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
type Epsilon = N::Epsilon;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
N::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
N::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
N::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.isometry.relative_eq(&other.isometry, epsilon, max_relative) &&
|
||||||
|
self.scaling.relative_eq(&other.scaling, epsilon, max_relative)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.isometry.ulps_eq(&other.isometry, epsilon, max_ulps) &&
|
||||||
|
self.scaling.ulps_eq(&other.scaling, epsilon, max_ulps)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S, R> fmt::Display for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real + fmt::Display,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>> + fmt::Display,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> + Allocator<usize, D, U1> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
let precision = f.precision().unwrap_or(3);
|
||||||
|
|
||||||
|
try!(writeln!(f, "SimilarityBase {{"));
|
||||||
|
try!(write!(f, "{:.*}", precision, self.isometry));
|
||||||
|
try!(write!(f, "Scaling: {:.*}", precision, self.scaling));
|
||||||
|
writeln!(f, "}}")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
// /*
|
||||||
|
// *
|
||||||
|
// * ToHomogeneous
|
||||||
|
// *
|
||||||
|
// */
|
||||||
|
// impl<N: Real> ToHomogeneous<$homogeneous<N>> for $t<N> {
|
||||||
|
// #[inline]
|
||||||
|
// fn to_homogeneous(&self) -> $homogeneous<N> {
|
||||||
|
// self.vector.to_homogeneous()
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
// /*
|
||||||
|
// *
|
||||||
|
// * Absolute
|
||||||
|
// *
|
||||||
|
// */
|
||||||
|
// impl<N: Absolute> Absolute for $t<N> {
|
||||||
|
// type AbsoluteValue = $submatrix<N::AbsoluteValue>;
|
||||||
|
//
|
||||||
|
// #[inline]
|
||||||
|
// fn abs(m: &$t<N>) -> $submatrix<N::AbsoluteValue> {
|
||||||
|
// Absolute::abs(&m.submatrix)
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
*/
|
184
src/geometry/similarity_alga.rs
Normal file
184
src/geometry/similarity_alga.rs
Normal file
@ -0,0 +1,184 @@
|
|||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractLoop, AbstractMonoid, AbstractQuasigroup,
|
||||||
|
AbstractSemigroup, Real, Inverse, Multiplicative, Identity};
|
||||||
|
use alga::linear::{Transformation, AffineTransformation, Rotation, Similarity, ProjectiveTransformation};
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{SimilarityBase, TranslationBase, PointBase};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Algebraic structures.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S, R> Identity<Multiplicative> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Inverse<Multiplicative> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.inverse_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> AbstractMagma<Multiplicative> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_multiplicative_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimName, S, R> $marker<$operator> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_multiplicative_structures!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Transformation groups.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S, R> Transformation<PointBase<N, D, S>> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
self * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, D, S>) -> ColumnVector<N, D, S> {
|
||||||
|
self * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> ProjectiveTransformation<PointBase<N, D, S>> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
self.isometry.inverse_transform_point(pt) / self.scaling()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, D, S>) -> ColumnVector<N, D, S> {
|
||||||
|
self.isometry.inverse_transform_vector(v) / self.scaling()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> AffineTransformation<PointBase<N, D, S>> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type NonUniformScaling = N;
|
||||||
|
type Rotation = R;
|
||||||
|
type Translation = TranslationBase<N, D, S>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn decompose(&self) -> (TranslationBase<N, D, S>, R, N, R) {
|
||||||
|
(self.isometry.translation.clone(), self.isometry.rotation.clone(), self.scaling(), R::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_translation(&self, t: &Self::Translation) -> Self {
|
||||||
|
t * self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_translation(&self, t: &Self::Translation) -> Self {
|
||||||
|
self * t
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
SimilarityBase::from_isometry(self.isometry.append_rotation(r), self.scaling())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
self * r
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_scaling(&self, s: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.append_scaling(*s)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_scaling(&self, s: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.prepend_scaling(*s)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation_wrt_point(&self, r: &Self::Rotation, p: &PointBase<N, D, S>) -> Option<Self> {
|
||||||
|
let mut res = self.clone();
|
||||||
|
res.append_rotation_wrt_point_mut(r, p);
|
||||||
|
Some(res)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Similarity<PointBase<N, D, S>> for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Scaling = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation(&self) -> TranslationBase<N, D, S> {
|
||||||
|
self.isometry.translation()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation(&self) -> R {
|
||||||
|
self.isometry.rotation()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaling(&self) -> N {
|
||||||
|
self.scaling()
|
||||||
|
}
|
||||||
|
}
|
19
src/geometry/similarity_alias.rs
Normal file
19
src/geometry/similarity_alias.rs
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U1, U2, U3};
|
||||||
|
|
||||||
|
use geometry::{Rotation, SimilarityBase, UnitQuaternion, UnitComplex};
|
||||||
|
|
||||||
|
/// A D-dimensional similarity.
|
||||||
|
pub type Similarity<N, D> = SimilarityBase<N, D, MatrixArray<N, D, U1>, Rotation<N, D>>;
|
||||||
|
|
||||||
|
/// A 2-dimensional similarity.
|
||||||
|
pub type Similarity2<N> = SimilarityBase<N, U2, MatrixArray<N, U2, U1>, UnitComplex<N>>;
|
||||||
|
|
||||||
|
/// A 3-dimensional similarity.
|
||||||
|
pub type Similarity3<N> = SimilarityBase<N, U3, MatrixArray<N, U3, U1>, UnitQuaternion<N>>;
|
||||||
|
|
||||||
|
/// A 3-dimensional similarity using a rotation matrix for its rotation part.
|
||||||
|
pub type SimilarityMatrix2<N> = Similarity<N, U2>;
|
||||||
|
|
||||||
|
/// A 3-dimensional similarity using a rotation matrix for its rotation part.
|
||||||
|
pub type SimilarityMatrix3<N> = Similarity<N, U3>;
|
188
src/geometry/similarity_construction.rs
Normal file
188
src/geometry/similarity_construction.rs
Normal file
@ -0,0 +1,188 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use num::One;
|
||||||
|
use rand::{Rng, Rand};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use alga::linear::Rotation as AlgaRotation;
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1, U2, U3, U4};
|
||||||
|
use core::allocator::{OwnedAllocator, Allocator};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
|
||||||
|
use geometry::{PointBase, TranslationBase, RotationBase, SimilarityBase, UnitQuaternionBase, IsometryBase};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new identity similarity.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> Self {
|
||||||
|
Self::from_isometry(IsometryBase::identity(), N::one())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> One for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new identity similarity.
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> Rand for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real + Rand,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>> + Rand,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<G: Rng>(rng: &mut G) -> Self {
|
||||||
|
let mut s = rng.gen();
|
||||||
|
while relative_eq!(s, N::zero()) {
|
||||||
|
s = rng.gen()
|
||||||
|
}
|
||||||
|
|
||||||
|
Self::from_isometry(rng.gen(), s)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S, R> SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// The similarity that applies tha scaling factor `scaling`, followed by the rotation `r` with
|
||||||
|
/// its axis passing through the point `p`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_wrt_point(r: R, p: PointBase<N, D, S>, scaling: N) -> Self {
|
||||||
|
let shift = r.transform_vector(&-&p.coords);
|
||||||
|
Self::from_parts(TranslationBase::from_vector(shift + p.coords), r, scaling)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
impl<N, D: DimName, S, R> Arbitrary for SimilarityBase<N, D, S, R>
|
||||||
|
where N: Real + Arbitrary + Send,
|
||||||
|
S: OwnedStorage<N, D, U1> + Send,
|
||||||
|
R: AlgaRotation<PointBase<N, D, S>> + Arbitrary + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(rng: &mut G) -> Self {
|
||||||
|
let mut s = Arbitrary::arbitrary(rng);
|
||||||
|
while relative_eq!(s, N::zero()) {
|
||||||
|
s = Arbitrary::arbitrary(rng)
|
||||||
|
}
|
||||||
|
|
||||||
|
Self::from_isometry(Arbitrary::arbitrary(rng), s)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Constructors for various static dimensions.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
// 2D rotation.
|
||||||
|
impl<N, S, SR> SimilarityBase<N, U2, S, RotationBase<N, U2, SR>>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1, Alloc = SR::Alloc>,
|
||||||
|
SR: OwnedStorage<N, U2, U2>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S>,
|
||||||
|
SR::Alloc: OwnedAllocator<N, U2, U2, SR> {
|
||||||
|
/// Creates a new similarity from a translation and a rotation angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(translation: ColumnVector<N, U2, S>, angle: N, scaling: N) -> Self {
|
||||||
|
Self::from_parts(TranslationBase::from_vector(translation), RotationBase::<N, U2, SR>::new(angle), scaling)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3D rotation.
|
||||||
|
macro_rules! similarity_construction_impl(
|
||||||
|
($Rot: ty, $RotId: ident, $RRDim: ty, $RCDim: ty) => {
|
||||||
|
impl<N, S, SR> SimilarityBase<N, U3, S, $Rot>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U3, U1, Alloc = SR::Alloc>,
|
||||||
|
SR: OwnedStorage<N, $RRDim, $RCDim>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U1, S>,
|
||||||
|
SR::Alloc: OwnedAllocator<N, $RRDim, $RCDim, SR> +
|
||||||
|
Allocator<N, U3, U3> {
|
||||||
|
/// Creates a new similarity from a translation, rotation axis-angle, and scaling
|
||||||
|
/// factor.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(translation: ColumnVector<N, U3, S>, axisangle: ColumnVector<N, U3, S>, scaling: N) -> Self {
|
||||||
|
Self::from_isometry(IsometryBase::<_, _, _, $Rot>::new(translation, axisangle), scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates an similarity that corresponds to the a scaling factor and a local frame of
|
||||||
|
/// an observer standing at the point `eye` and looking toward `target`.
|
||||||
|
///
|
||||||
|
/// It maps the view direction `target - eye` to the positive `z` axis and the origin to the
|
||||||
|
/// `eye`.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The observer position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - Vertical direction. The only requirement of this parameter is to not be collinear
|
||||||
|
/// to `eye - at`. Non-collinearity is not checked.
|
||||||
|
#[inline]
|
||||||
|
pub fn new_observer_frame(eye: &PointBase<N, U3, S>,
|
||||||
|
target: &PointBase<N, U3, S>,
|
||||||
|
up: &ColumnVector<N, U3, S>,
|
||||||
|
scaling: N)
|
||||||
|
-> Self {
|
||||||
|
Self::from_isometry(IsometryBase::<_, _, _, $Rot>::new_observer_frame(eye, target, up), scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a right-handed look-at view matrix including scaling factor.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of right handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_rh(eye: &PointBase<N, U3, S>,
|
||||||
|
target: &PointBase<N, U3, S>,
|
||||||
|
up: &ColumnVector<N, U3, S>,
|
||||||
|
scaling: N)
|
||||||
|
-> Self {
|
||||||
|
Self::from_isometry(IsometryBase::<_, _, _, $Rot>::look_at_rh(eye, target, up), scaling)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a left-handed look-at view matrix including a scaling factor.
|
||||||
|
///
|
||||||
|
/// This conforms to the common notion of left handed look-at matrix from the computer
|
||||||
|
/// graphics community.
|
||||||
|
///
|
||||||
|
/// # Arguments
|
||||||
|
/// * eye - The eye position.
|
||||||
|
/// * target - The target position.
|
||||||
|
/// * up - A vector approximately aligned with required the vertical axis. The only
|
||||||
|
/// requirement of this parameter is to not be collinear to `target - eye`.
|
||||||
|
#[inline]
|
||||||
|
pub fn look_at_lh(eye: &PointBase<N, U3, S>,
|
||||||
|
target: &PointBase<N, U3, S>,
|
||||||
|
up: &ColumnVector<N, U3, S>,
|
||||||
|
scaling: N)
|
||||||
|
-> Self {
|
||||||
|
Self::from_isometry(IsometryBase::<_, _, _, $Rot>::look_at_lh(eye, target, up), scaling)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
similarity_construction_impl!(RotationBase<N, U3, SR>, RotationBase, U3, U3);
|
||||||
|
similarity_construction_impl!(UnitQuaternionBase<N, SR>, UnitQuaternionBase, U4, U1);
|
163
src/geometry/similarity_conversion.rs
Normal file
163
src/geometry/similarity_conversion.rs
Normal file
@ -0,0 +1,163 @@
|
|||||||
|
use alga::general::{Real, SubsetOf, SupersetOf};
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::{SquareMatrix, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, TranslationBase, IsometryBase, SimilarityBase, TransformBase, SuperTCategoryOf, TAffine};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides the following conversions:
|
||||||
|
* =============================================
|
||||||
|
*
|
||||||
|
* SimilarityBase -> SimilarityBase
|
||||||
|
* SimilarityBase -> TransformBase
|
||||||
|
* SimilarityBase -> Matrix (homogeneous)
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R1, R2> SubsetOf<SimilarityBase<N2, D, SB, R2>> for SimilarityBase<N1, D, SA, R1>
|
||||||
|
where N1: Real + SubsetOf<N2>,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
R1: Rotation<PointBase<N1, D, SA>> + SubsetOf<R2>,
|
||||||
|
R2: Rotation<PointBase<N2, D, SB>>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SimilarityBase<N2, D, SB, R2> {
|
||||||
|
SimilarityBase::from_isometry(
|
||||||
|
self.isometry.to_superset(),
|
||||||
|
self.scaling().to_superset()
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(sim: &SimilarityBase<N2, D, SB, R2>) -> bool {
|
||||||
|
::is_convertible::<_, IsometryBase<N1, D, SA, R1>>(&sim.isometry) &&
|
||||||
|
::is_convertible::<_, N1>(&sim.scaling())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(sim: &SimilarityBase<N2, D, SB, R2>) -> Self {
|
||||||
|
SimilarityBase::from_isometry(
|
||||||
|
sim.isometry.to_subset_unchecked(),
|
||||||
|
sim.scaling().to_subset_unchecked()
|
||||||
|
)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB, R, C> SubsetOf<TransformBase<N2, D, SB, C>> for SimilarityBase<N1, D, SA, R>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: SuperTCategoryOf<TAffine>,
|
||||||
|
R: Rotation<PointBase<N1, D, SA>> +
|
||||||
|
SubsetOf<OwnedSquareMatrix<N1, DimNameSum<D, U1>, SA::Alloc>> + // needed by: .to_homogeneous()
|
||||||
|
SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>>, // needed by: ::convert_unchecked(mm)
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, D, D> + // needed by R
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>> + // needed by: .to_homogeneous()
|
||||||
|
Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>, // needed by R
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, D> + // needed by: mm.fixed_slice_mut
|
||||||
|
Allocator<N2, D, U1> + // needed by: m.fixed_slice
|
||||||
|
Allocator<N2, U1, D> { // needed by: m.fixed_slice
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TransformBase<N2, D, SB, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(t: &TransformBase<N2, D, SB, C>) -> bool {
|
||||||
|
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(t: &TransformBase<N2, D, SB, C>) -> Self {
|
||||||
|
Self::from_superset_unchecked(t.matrix())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB, R> SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>> for SimilarityBase<N1, D, SA, R>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
R: Rotation<PointBase<N1, D, SA>> +
|
||||||
|
SubsetOf<OwnedSquareMatrix<N1, DimNameSum<D, U1>, SA::Alloc>> + // needed by: .to_homogeneous()
|
||||||
|
SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>>, // needed by: ::convert_unchecked(mm)
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, D, D> + // needed by R
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>> + // needed by: .to_homogeneous()
|
||||||
|
Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>, // needed by R
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, D> + // needed by: mm.fixed_slice_mut
|
||||||
|
Allocator<N2, D, U1> + // needed by: m.fixed_slice
|
||||||
|
Allocator<N2, U1, D> { // needed by: m.fixed_slice
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SquareMatrix<N2, DimNameSum<D, U1>, SB> {
|
||||||
|
self.to_homogeneous().to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> bool {
|
||||||
|
let mut rot = m.fixed_slice::<D, D>(0, 0).clone_owned();
|
||||||
|
if rot.fixed_columns_mut::<U1>(0).try_normalize_mut(N2::zero()).is_some() &&
|
||||||
|
rot.fixed_columns_mut::<U1>(1).try_normalize_mut(N2::zero()).is_some() &&
|
||||||
|
rot.fixed_columns_mut::<U1>(2).try_normalize_mut(N2::zero()).is_some() {
|
||||||
|
|
||||||
|
// FIXME: could we avoid explicit the computation of the determinant?
|
||||||
|
// (its sign is needed to see if the scaling factor is negative).
|
||||||
|
if rot.determinant() < N2::zero() {
|
||||||
|
rot.fixed_columns_mut::<U1>(0).neg_mut();
|
||||||
|
rot.fixed_columns_mut::<U1>(1).neg_mut();
|
||||||
|
rot.fixed_columns_mut::<U1>(2).neg_mut();
|
||||||
|
}
|
||||||
|
|
||||||
|
let bottom = m.fixed_slice::<U1, D>(D::dim(), 0);
|
||||||
|
// Scalar types agree.
|
||||||
|
m.iter().all(|e| SupersetOf::<N1>::is_in_subset(e)) &&
|
||||||
|
// The normalized block part is a rotation.
|
||||||
|
// rot.is_special_orthogonal(N2::default_epsilon().sqrt()) &&
|
||||||
|
// The bottom row is (0, 0, ..., 1)
|
||||||
|
bottom.iter().all(|e| e.is_zero()) &&
|
||||||
|
m[(D::dim(), D::dim())] == N2::one()
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> Self {
|
||||||
|
let mut mm = m.clone_owned();
|
||||||
|
let na = mm.fixed_slice_mut::<D, U1>(0, 0).normalize_mut();
|
||||||
|
let nb = mm.fixed_slice_mut::<D, U1>(0, 1).normalize_mut();
|
||||||
|
let nc = mm.fixed_slice_mut::<D, U1>(0, 2).normalize_mut();
|
||||||
|
|
||||||
|
let mut scale = (na + nb + nc) / ::convert(3.0); // We take the mean, for robustness.
|
||||||
|
|
||||||
|
// FIXME: could we avoid the explicit computation of the determinant?
|
||||||
|
// (its sign is needed to see if the scaling factor is negative).
|
||||||
|
if mm.fixed_slice::<D, D>(0, 0).determinant() < N2::zero() {
|
||||||
|
mm.fixed_slice_mut::<D, U1>(0, 0).neg_mut();
|
||||||
|
mm.fixed_slice_mut::<D, U1>(0, 1).neg_mut();
|
||||||
|
mm.fixed_slice_mut::<D, U1>(0, 2).neg_mut();
|
||||||
|
scale = -scale;
|
||||||
|
}
|
||||||
|
|
||||||
|
let t = m.fixed_slice::<D, U1>(0, D::dim()).into_owned();
|
||||||
|
let t = TranslationBase::from_vector(::convert_unchecked(t));
|
||||||
|
|
||||||
|
Self::from_parts(t, ::convert_unchecked(mm), ::convert_unchecked(scale))
|
||||||
|
}
|
||||||
|
}
|
512
src/geometry/similarity_ops.rs
Normal file
512
src/geometry/similarity_ops.rs
Normal file
@ -0,0 +1,512 @@
|
|||||||
|
use std::ops::{Mul, MulAssign, Div, DivAssign};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1, U3, U4};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{PointBase, RotationBase, SimilarityBase, TranslationBase, UnitQuaternionBase, IsometryBase};
|
||||||
|
|
||||||
|
// FIXME: there are several cloning of rotations that we could probably get rid of (but we didn't
|
||||||
|
// yet because that would require to add a bound like `where for<'a, 'b> &'a R: Mul<&'b R, Output = R>`
|
||||||
|
// which is quite ugly.
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* In this file, we provide:
|
||||||
|
* =========================
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Operators)
|
||||||
|
*
|
||||||
|
* SimilarityBase × SimilarityBase
|
||||||
|
* SimilarityBase × R
|
||||||
|
* SimilarityBase × IsometryBase
|
||||||
|
*
|
||||||
|
* IsometryBase × SimilarityBase
|
||||||
|
* IsometryBase ÷ SimilarityBase
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* SimilarityBase ÷ SimilarityBase
|
||||||
|
* SimilarityBase ÷ R
|
||||||
|
* SimilarityBase ÷ IsometryBase
|
||||||
|
*
|
||||||
|
* SimilarityBase × PointBase
|
||||||
|
* SimilarityBase × ColumnVector
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* SimilarityBase × TranslationBase
|
||||||
|
* TranslationBase × SimilarityBase
|
||||||
|
*
|
||||||
|
* NOTE: The following are provided explicitly because we can't have R × SimilarityBase.
|
||||||
|
* RotationBase × SimilarityBase<RotationBase>
|
||||||
|
* UnitQuaternion × SimilarityBase<UnitQuaternion>
|
||||||
|
*
|
||||||
|
* RotationBase ÷ SimilarityBase<RotationBase>
|
||||||
|
* UnitQuaternion ÷ SimilarityBase<UnitQuaternion>
|
||||||
|
*
|
||||||
|
* (Assignment Operators)
|
||||||
|
*
|
||||||
|
* SimilarityBase ×= TranslationBase
|
||||||
|
*
|
||||||
|
* SimilarityBase ×= SimilarityBase
|
||||||
|
* SimilarityBase ×= IsometryBase
|
||||||
|
* SimilarityBase ×= R
|
||||||
|
*
|
||||||
|
* SimilarityBase ÷= SimilarityBase
|
||||||
|
* SimilarityBase ÷= IsometryBase
|
||||||
|
* SimilarityBase ÷= R
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
// XXX: code duplication: those macros are the same as for the isometry.
|
||||||
|
macro_rules! similarity_binop_impl(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N, D: DimName, S, R> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Output = $Output;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! similarity_binop_impl_all(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
[val val] => $action_val_val: expr;
|
||||||
|
[ref val] => $action_ref_val: expr;
|
||||||
|
[val ref] => $action_val_ref: expr;
|
||||||
|
[ref ref] => $action_ref_ref: expr;) => {
|
||||||
|
similarity_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_val_val; );
|
||||||
|
|
||||||
|
similarity_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_ref_val; 'a);
|
||||||
|
|
||||||
|
similarity_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_val_ref; 'b);
|
||||||
|
|
||||||
|
similarity_binop_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
$lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_ref_ref; 'a, 'b);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! similarity_binop_assign_impl_all(
|
||||||
|
($OpAssign: ident, $op_assign: ident;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty;
|
||||||
|
[val] => $action_val: expr;
|
||||||
|
[ref] => $action_ref: expr;) => {
|
||||||
|
impl<N, D: DimName, S, R> $OpAssign<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn $op_assign(&mut $lhs, $rhs: $Rhs) {
|
||||||
|
$action_val
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N, D: DimName, S, R> $OpAssign<&'b $Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
R: Rotation<PointBase<N, D, S>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn $op_assign(&mut $lhs, $rhs: &'b $Rhs) {
|
||||||
|
$action_ref
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
// SimilarityBase × SimilarityBase
|
||||||
|
// SimilarityBase ÷ SimilarityBase
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: SimilarityBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => {
|
||||||
|
let mut res = self * &rhs.isometry;
|
||||||
|
res.prepend_scaling_mut(rhs.scaling());
|
||||||
|
res
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: SimilarityBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase ×= TranslationBase
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: TranslationBase<N, D, S>;
|
||||||
|
[val] => *self *= &rhs;
|
||||||
|
[ref] => {
|
||||||
|
let shift = self.isometry.rotation.transform_vector(&rhs.vector) * self.scaling();
|
||||||
|
self.isometry.translation.vector += shift;
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase ×= SimilarityBase
|
||||||
|
// SimilarityBase ÷= SimilarityBase
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: SimilarityBase<N, D, S, R>;
|
||||||
|
[val] => *self *= &rhs;
|
||||||
|
[ref] => {
|
||||||
|
*self *= &rhs.isometry;
|
||||||
|
self.prepend_scaling_mut(rhs.scaling());
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: SimilarityBase<N, D, S, R>;
|
||||||
|
[val] => *self /= &rhs;
|
||||||
|
// FIXME: don't invert explicitly.
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase ×= IsometryBase
|
||||||
|
// SimilarityBase ÷= IsometryBase
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>;
|
||||||
|
[val] => *self *= &rhs;
|
||||||
|
[ref] => {
|
||||||
|
let shift = self.isometry.rotation.transform_vector(&rhs.translation.vector) * self.scaling();
|
||||||
|
self.isometry.translation.vector += shift;
|
||||||
|
self.isometry.rotation *= rhs.rotation.clone();
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>;
|
||||||
|
[val] => *self /= &rhs;
|
||||||
|
// FIXME: don't invert explicitly.
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase ×= R
|
||||||
|
// SimilarityBase ÷= R
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: R;
|
||||||
|
[val] => self.isometry.rotation *= rhs;
|
||||||
|
[ref] => self.isometry.rotation *= rhs.clone();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_assign_impl_all!(
|
||||||
|
DivAssign, div_assign;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: R;
|
||||||
|
// FIXME: don't invert explicitly?
|
||||||
|
[val] => *self *= rhs.inverse();
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase × R
|
||||||
|
// SimilarityBase ÷ R
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: R, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => {
|
||||||
|
let scaling = self.scaling();
|
||||||
|
SimilarityBase::from_isometry(self.isometry * rhs, scaling)
|
||||||
|
};
|
||||||
|
[ref val] => SimilarityBase::from_isometry(&self.isometry * rhs, self.scaling());
|
||||||
|
[val ref] => {
|
||||||
|
let scaling = self.scaling();
|
||||||
|
SimilarityBase::from_isometry(self.isometry * rhs, scaling)
|
||||||
|
};
|
||||||
|
[ref ref] => SimilarityBase::from_isometry(&self.isometry * rhs, self.scaling());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: R, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => {
|
||||||
|
let scaling = self.scaling();
|
||||||
|
SimilarityBase::from_isometry(self.isometry / rhs, scaling)
|
||||||
|
};
|
||||||
|
[ref val] => SimilarityBase::from_isometry(&self.isometry / rhs, self.scaling());
|
||||||
|
[val ref] => {
|
||||||
|
let scaling = self.scaling();
|
||||||
|
SimilarityBase::from_isometry(self.isometry / rhs, scaling)
|
||||||
|
};
|
||||||
|
[ref ref] => SimilarityBase::from_isometry(&self.isometry / rhs, self.scaling());
|
||||||
|
);
|
||||||
|
|
||||||
|
// SimilarityBase × IsometryBase
|
||||||
|
// SimilarityBase ÷ IsometryBase
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => {
|
||||||
|
let shift = self.isometry.rotation.transform_vector(&rhs.translation.vector) * self.scaling();
|
||||||
|
SimilarityBase::from_parts(
|
||||||
|
TranslationBase::from_vector(&self.isometry.translation.vector + shift),
|
||||||
|
self.isometry.rotation.clone() * rhs.rotation.clone(),
|
||||||
|
self.scaling())
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
self: SimilarityBase<N, D, S, R>, rhs: IsometryBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
// IsometryBase × SimilarityBase
|
||||||
|
// IsometryBase ÷ SimilarityBase
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: SimilarityBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => {
|
||||||
|
let scaling = rhs.scaling();
|
||||||
|
SimilarityBase::from_isometry(self * rhs.isometry, scaling)
|
||||||
|
};
|
||||||
|
[ref val] => {
|
||||||
|
let scaling = rhs.scaling();
|
||||||
|
SimilarityBase::from_isometry(self * rhs.isometry, scaling)
|
||||||
|
};
|
||||||
|
[val ref] => {
|
||||||
|
let scaling = rhs.scaling();
|
||||||
|
SimilarityBase::from_isometry(self * &rhs.isometry, scaling)
|
||||||
|
};
|
||||||
|
[ref ref] => {
|
||||||
|
let scaling = rhs.scaling();
|
||||||
|
SimilarityBase::from_isometry(self * &rhs.isometry, scaling)
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
self: IsometryBase<N, D, S, R>, rhs: SimilarityBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase × PointBase
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: SimilarityBase<N, D, S, R>, right: PointBase<N, D, S>, Output = PointBase<N, D, S>;
|
||||||
|
[val val] => {
|
||||||
|
let scaling = self.scaling();
|
||||||
|
self.isometry.translation * (self.isometry.rotation.transform_point(&right) * scaling)
|
||||||
|
};
|
||||||
|
[ref val] => &self.isometry.translation * (self.isometry.rotation.transform_point(&right) * self.scaling());
|
||||||
|
[val ref] => {
|
||||||
|
let scaling = self.scaling();
|
||||||
|
self.isometry.translation * (self.isometry.rotation.transform_point(right) * scaling)
|
||||||
|
};
|
||||||
|
[ref ref] => &self.isometry.translation * (self.isometry.rotation.transform_point(right) * self.scaling());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase × Vector
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: SimilarityBase<N, D, S, R>, right: ColumnVector<N, D, S>, Output = ColumnVector<N, D, S>;
|
||||||
|
[val val] => self.isometry.rotation.transform_vector(&right) * self.scaling();
|
||||||
|
[ref val] => self.isometry.rotation.transform_vector(&right) * self.scaling();
|
||||||
|
[val ref] => self.isometry.rotation.transform_vector(right) * self.scaling();
|
||||||
|
[ref ref] => self.isometry.rotation.transform_vector(right) * self.scaling();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// SimilarityBase × TranslationBase
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: SimilarityBase<N, D, S, R>, right: TranslationBase<N, D, S>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => &self * &right;
|
||||||
|
[ref val] => self * &right;
|
||||||
|
[val ref] => &self * right;
|
||||||
|
[ref ref] => {
|
||||||
|
let shift = self.isometry.rotation.transform_vector(&right.vector) * self.scaling();
|
||||||
|
SimilarityBase::from_parts(
|
||||||
|
TranslationBase::from_vector(&self.isometry.translation.vector + shift),
|
||||||
|
self.isometry.rotation.clone(),
|
||||||
|
self.scaling())
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TranslationBase × SimilarityBase
|
||||||
|
similarity_binop_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
self: TranslationBase<N, D, S>, right: SimilarityBase<N, D, S, R>, Output = SimilarityBase<N, D, S, R>;
|
||||||
|
[val val] => {
|
||||||
|
let scaling = right.scaling();
|
||||||
|
SimilarityBase::from_isometry(self * right.isometry, scaling)
|
||||||
|
};
|
||||||
|
[ref val] => {
|
||||||
|
let scaling = right.scaling();
|
||||||
|
SimilarityBase::from_isometry(self * right.isometry, scaling)
|
||||||
|
};
|
||||||
|
[val ref] => SimilarityBase::from_isometry(self * &right.isometry, right.scaling());
|
||||||
|
[ref ref] => SimilarityBase::from_isometry(self * &right.isometry, right.scaling());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
macro_rules! similarity_from_composition_impl(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N $(, $Dims: $DimsBound)*, SA, SB> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, $R1, $C1>,
|
||||||
|
SB: OwnedStorage<N, $R2, $C2, Alloc = SA::Alloc>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, $R1, $C1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, $R2, $C2, SB> {
|
||||||
|
type Output = $Output;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! similarity_from_composition_impl_all(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($R1: ty, $C1: ty),($R2: ty, $C2: ty) $(for $Dims: ident: $DimsBound: ident),*;
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Output: ty;
|
||||||
|
[val val] => $action_val_val: expr;
|
||||||
|
[ref val] => $action_ref_val: expr;
|
||||||
|
[val ref] => $action_val_ref: expr;
|
||||||
|
[ref ref] => $action_ref_ref: expr;) => {
|
||||||
|
|
||||||
|
similarity_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_val_val; );
|
||||||
|
|
||||||
|
similarity_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: &'a $Lhs, $rhs: $Rhs, Output = $Output;
|
||||||
|
$action_ref_val; 'a);
|
||||||
|
|
||||||
|
similarity_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_val_ref; 'b);
|
||||||
|
|
||||||
|
similarity_from_composition_impl!(
|
||||||
|
$Op, $op;
|
||||||
|
($R1, $C1),($R2, $C2) $(for $Dims: $DimsBound),*;
|
||||||
|
$lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Output;
|
||||||
|
$action_ref_ref; 'a, 'b);
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase × SimilarityBase
|
||||||
|
similarity_from_composition_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(D, D), (D, U1) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: SimilarityBase<N, D, SB, RotationBase<N, D, SA>>,
|
||||||
|
Output = SimilarityBase<N, D, SB, RotationBase<N, D, SA>>;
|
||||||
|
[val val] => &self * &right;
|
||||||
|
[ref val] => self * &right;
|
||||||
|
[val ref] => &self * right;
|
||||||
|
[ref ref] => SimilarityBase::from_isometry(self * &right.isometry, right.scaling());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase ÷ SimilarityBase
|
||||||
|
similarity_from_composition_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(D, D), (D, U1) for D: DimName;
|
||||||
|
self: RotationBase<N, D, SA>, right: SimilarityBase<N, D, SB, RotationBase<N, D, SA>>,
|
||||||
|
Output = SimilarityBase<N, D, SB, RotationBase<N, D, SA>>;
|
||||||
|
// FIXME: don't call iverse explicitly?
|
||||||
|
[val val] => self * right.inverse();
|
||||||
|
[ref val] => self * right.inverse();
|
||||||
|
[val ref] => self * right.inverse();
|
||||||
|
[ref ref] => self * right.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternion × SimilarityBase
|
||||||
|
similarity_from_composition_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, right: SimilarityBase<N, U3, SB, UnitQuaternionBase<N, SA>>,
|
||||||
|
Output = SimilarityBase<N, U3, SB, UnitQuaternionBase<N, SA>>;
|
||||||
|
[val val] => &self * &right;
|
||||||
|
[ref val] => self * &right;
|
||||||
|
[val ref] => &self * right;
|
||||||
|
[ref ref] => SimilarityBase::from_isometry(self * &right.isometry, right.scaling());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternion ÷ SimilarityBase
|
||||||
|
similarity_from_composition_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(U4, U1), (U3, U1);
|
||||||
|
self: UnitQuaternionBase<N, SA>, right: SimilarityBase<N, U3, SB, UnitQuaternionBase<N, SA>>,
|
||||||
|
Output = SimilarityBase<N, U3, SB, UnitQuaternionBase<N, SA>>;
|
||||||
|
// FIXME: don't call inverse explicitly?
|
||||||
|
[val val] => self * right.inverse();
|
||||||
|
[ref val] => self * right.inverse();
|
||||||
|
[val ref] => self * right.inverse();
|
||||||
|
[ref ref] => self * right.inverse();
|
||||||
|
);
|
299
src/geometry/transform.rs
Normal file
299
src/geometry/transform.rs
Normal file
@ -0,0 +1,299 @@
|
|||||||
|
use std::any::Any;
|
||||||
|
use std::fmt::Debug;
|
||||||
|
use std::marker::PhantomData;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::Field;
|
||||||
|
|
||||||
|
use core::{Scalar, SquareMatrix, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1};
|
||||||
|
use core::storage::{Storage, StorageMut};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
|
||||||
|
/// Trait implemented by phantom types identifying the projective transformation type.
|
||||||
|
///
|
||||||
|
/// NOTE: this trait is not intended to be implementable outside of the `nalgebra` crate.
|
||||||
|
pub trait TCategory: Any + Debug + Copy + PartialEq + Send {
|
||||||
|
#[inline]
|
||||||
|
fn has_normalizer() -> bool {
|
||||||
|
true
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Checks that the given matrix is a valid homogeneous representation of an element of the
|
||||||
|
/// category `Self`.
|
||||||
|
fn check_homogeneous_invariants<N, D, S>(mat: &SquareMatrix<N, D, S>) -> bool
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
D: DimName,
|
||||||
|
S: Storage<N, D, D>,
|
||||||
|
N::Epsilon: Copy;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Traits that gives the transformation category that is compatible with the result of the
|
||||||
|
/// multiplication of transformations with categories `Self` and `Other`.
|
||||||
|
pub trait TCategoryMul<Other: TCategory>: TCategory {
|
||||||
|
type Representative: TCategory;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Indicates that `Self` is a more general transformation category than `Other`.
|
||||||
|
pub trait SuperTCategoryOf<Other: TCategory>: TCategory { }
|
||||||
|
|
||||||
|
/// Indicates that `Self` is a more specific transformation category than `Other`.
|
||||||
|
///
|
||||||
|
/// Automatically implemented based on `SuperTCategoryOf`.
|
||||||
|
pub trait SubTCategoryOf<Other: TCategory>: TCategory { }
|
||||||
|
impl<T1, T2> SubTCategoryOf<T2> for T1
|
||||||
|
where T1: TCategory,
|
||||||
|
T2: SuperTCategoryOf<T1> {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Tag representing the most general (not necessarily inversible) transformation type.
|
||||||
|
#[derive(Debug, Copy, Clone, Hash, PartialEq, Eq)]
|
||||||
|
pub struct TGeneral;
|
||||||
|
|
||||||
|
/// Tag representing the most general inversible transformation type.
|
||||||
|
#[derive(Debug, Copy, Clone, Hash, PartialEq, Eq)]
|
||||||
|
pub struct TProjective;
|
||||||
|
|
||||||
|
/// Tag representing an affine transformation. Its bottom-row is equal to `(0, 0 ... 0, 1)`.
|
||||||
|
#[derive(Debug, Copy, Clone, Hash, PartialEq, Eq)]
|
||||||
|
pub struct TAffine;
|
||||||
|
|
||||||
|
impl TCategory for TGeneral {
|
||||||
|
#[inline]
|
||||||
|
fn check_homogeneous_invariants<N, D, S>(_: &SquareMatrix<N, D, S>) -> bool
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
D: DimName,
|
||||||
|
S: Storage<N, D, D>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl TCategory for TProjective {
|
||||||
|
#[inline]
|
||||||
|
fn check_homogeneous_invariants<N, D, S>(mat: &SquareMatrix<N, D, S>) -> bool
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
D: DimName,
|
||||||
|
S: Storage<N, D, D>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
mat.is_invertible()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl TCategory for TAffine {
|
||||||
|
#[inline]
|
||||||
|
fn has_normalizer() -> bool {
|
||||||
|
false
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn check_homogeneous_invariants<N, D, S>(mat: &SquareMatrix<N, D, S>) -> bool
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
D: DimName,
|
||||||
|
S: Storage<N, D, D>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
mat.is_invertible() &&
|
||||||
|
mat[(D::dim(), D::dim())] == N::one() &&
|
||||||
|
(0 .. D::dim()).all(|i| mat[(D::dim(), i)].is_zero())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! category_mul_impl(
|
||||||
|
($($a: ident * $b: ident => $c: ty);* $(;)*) => {$(
|
||||||
|
impl TCategoryMul<$a> for $b {
|
||||||
|
type Representative = $c;
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
// We require stability uppon multiplication.
|
||||||
|
impl<T: TCategory> TCategoryMul<T> for T {
|
||||||
|
type Representative = T;
|
||||||
|
}
|
||||||
|
|
||||||
|
category_mul_impl!(
|
||||||
|
// TGeneral * TGeneral => TGeneral;
|
||||||
|
TGeneral * TProjective => TGeneral;
|
||||||
|
TGeneral * TAffine => TGeneral;
|
||||||
|
|
||||||
|
TProjective * TGeneral => TGeneral;
|
||||||
|
// TProjective * TProjective => TProjective;
|
||||||
|
TProjective * TAffine => TProjective;
|
||||||
|
|
||||||
|
TAffine * TGeneral => TGeneral;
|
||||||
|
TAffine * TProjective => TProjective;
|
||||||
|
// TAffine * TAffine => TAffine;
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! super_tcategory_impl(
|
||||||
|
($($a: ident >= $b: ident);* $(;)*) => {$(
|
||||||
|
impl SuperTCategoryOf<$b> for $a { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl<T: TCategory> SuperTCategoryOf<T> for T { }
|
||||||
|
|
||||||
|
super_tcategory_impl!(
|
||||||
|
TGeneral >= TProjective;
|
||||||
|
TGeneral >= TAffine;
|
||||||
|
TProjective >= TAffine;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
/// A transformation matrix that owns its data.
|
||||||
|
pub type OwnedTransform<N, D, A, C>
|
||||||
|
= TransformBase<N, D, <A as Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>>>::Buffer, C>;
|
||||||
|
|
||||||
|
|
||||||
|
/// A transformation matrix in homogeneous coordinates.
|
||||||
|
///
|
||||||
|
/// It is stored as a matrix with dimensions `(D + 1, D + 1)`, e.g., it stores a 4x4 matrix for a
|
||||||
|
/// 3D transformation.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Debug, Clone, Copy)] // FIXME: Hash
|
||||||
|
pub struct TransformBase<N: Scalar, D: DimNameAdd<U1>, S, C: TCategory> {
|
||||||
|
matrix: SquareMatrix<N, DimNameSum<D, U1>, S>,
|
||||||
|
_phantom: PhantomData<C>
|
||||||
|
}
|
||||||
|
|
||||||
|
// XXX: for some reasons, implementing Clone and Copy manually causes an ICE…
|
||||||
|
|
||||||
|
impl<N, D, S, C: TCategory> Eq for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Eq,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S: Storage<N, DimNameSum<D, U1>, DimNameSum<D, U1>> { }
|
||||||
|
|
||||||
|
impl<N, D, S, C: TCategory> PartialEq for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S: Storage<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &Self) -> bool {
|
||||||
|
self.matrix == right.matrix
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D, S, C: TCategory> TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S: Storage<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
/// Creates a new transformation from the given homogeneous matrix. The transformation category
|
||||||
|
/// of `Self` is not checked to be verified by the given matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_matrix_unchecked(matrix: SquareMatrix<N, DimNameSum<D, U1>, S>) -> Self {
|
||||||
|
TransformBase {
|
||||||
|
matrix: matrix,
|
||||||
|
_phantom: PhantomData
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Moves this transform into one that owns its data.
|
||||||
|
#[inline]
|
||||||
|
pub fn into_owned(self) -> OwnedTransform<N, D, S::Alloc, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.matrix.into_owned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Clones this transform into one that owns its data.
|
||||||
|
#[inline]
|
||||||
|
pub fn clone_owned(&self) -> OwnedTransform<N, D, S::Alloc, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.matrix.clone_owned())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The underlying matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn unwrap(self) -> SquareMatrix<N, DimNameSum<D, U1>, S> {
|
||||||
|
self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A reference to the underlynig matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn matrix(&self) -> &SquareMatrix<N, DimNameSum<D, U1>, S> {
|
||||||
|
&self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// A mutable reference to the underlying matrix.
|
||||||
|
///
|
||||||
|
/// It is `_unchecked` because direct modifications of this matrix may break invariants
|
||||||
|
/// identified by this transformation category.
|
||||||
|
#[inline]
|
||||||
|
pub fn matrix_mut_unchecked(&mut self) -> &mut SquareMatrix<N, DimNameSum<D, U1>, S> {
|
||||||
|
&mut self.matrix
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sets the category of this transform.
|
||||||
|
///
|
||||||
|
/// This can be done only if the new category is more general than the current one, e.g., a
|
||||||
|
/// transform with category `TProjective` cannot be converted to a transform with category
|
||||||
|
/// `TAffine` because not all projective transformations are affine (the other way-round is
|
||||||
|
/// valid though).
|
||||||
|
#[inline]
|
||||||
|
pub fn set_category<CNew: SuperTCategoryOf<C>>(self) -> TransformBase<N, D, S, CNew> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.matrix)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Converts this transform into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc> {
|
||||||
|
self.matrix().clone_owned()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D, S, C> TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
C: TCategory,
|
||||||
|
S: Storage<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
/// Attempts to invert this transformation. You may use `.inverse` instead of this
|
||||||
|
/// transformation has a subcategory of `TProjective`.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_inverse(self) -> Option<OwnedTransform<N, D, S::Alloc, C>> {
|
||||||
|
if let Some(m) = self.matrix.try_inverse() {
|
||||||
|
Some(TransformBase::from_matrix_unchecked(m))
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this transformation. Use `.try_inverse` if this transform has the `TGeneral`
|
||||||
|
/// category (it may not be invertible).
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(self) -> OwnedTransform<N, D, S::Alloc, C>
|
||||||
|
where C: SubTCategoryOf<TProjective> {
|
||||||
|
// FIXME: specialize for TAffine?
|
||||||
|
TransformBase::from_matrix_unchecked(self.matrix.try_inverse().unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D, S, C> TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
C: TCategory,
|
||||||
|
S: StorageMut<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
/// Attempts to invert this transformation in-place. You may use `.inverse_mut` instead of this
|
||||||
|
/// transformation has a subcategory of `TProjective`.
|
||||||
|
#[inline]
|
||||||
|
pub fn try_inverse_mut(&mut self) -> bool {
|
||||||
|
self.matrix.try_inverse_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this transformation in-place. Use `.try_inverse_mut` if this transform has the
|
||||||
|
/// `TGeneral` category (it may not be invertible).
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self)
|
||||||
|
where C: SubTCategoryOf<TProjective> {
|
||||||
|
let _ = self.matrix.try_inverse_mut();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D, S> TransformBase<N, D, S, TGeneral>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S: Storage<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
/// A mutable reference to underlying matrix. Use `.matrix_mut_unchecked` instead if this
|
||||||
|
/// transformation category is not `TGeneral`.
|
||||||
|
#[inline]
|
||||||
|
pub fn matrix_mut(&mut self) -> &mut SquareMatrix<N, DimNameSum<D, U1>, S> {
|
||||||
|
self.matrix_mut_unchecked()
|
||||||
|
}
|
||||||
|
}
|
157
src/geometry/transform_alga.rs
Normal file
157
src/geometry/transform_alga.rs
Normal file
@ -0,0 +1,157 @@
|
|||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractLoop, AbstractMonoid, AbstractQuasigroup,
|
||||||
|
AbstractSemigroup, Field, Real, Inverse, Multiplicative, Identity};
|
||||||
|
use alga::linear::{Transformation, ProjectiveTransformation};
|
||||||
|
|
||||||
|
use core::{Scalar, ColumnVector};
|
||||||
|
use core::dimension::{DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, TransformBase, TCategory, SubTCategoryOf, TProjective};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Algebraic structures.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimNameAdd<U1>, S, C> Identity<Multiplicative> for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: TCategory,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimNameAdd<U1>, S, C> Inverse<Multiplicative> for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: SubTCategoryOf<TProjective>,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.clone().inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.inverse_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimNameAdd<U1>, S, C> AbstractMagma<Multiplicative> for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: TCategory,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_multiplicative_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimNameAdd<U1>, S, C> $marker<$operator> for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: TCategory,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! impl_inversible_multiplicative_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimNameAdd<U1>, S, C> $marker<$operator> for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field + ApproxEq,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: SubTCategoryOf<TProjective>,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_multiplicative_structures!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_inversible_multiplicative_structures!(
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Transformation groups.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimNameAdd<U1>, SA, SB, C> Transformation<PointBase<N, D, SB>> for TransformBase<N, D, SA, C>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
C: TCategory,
|
||||||
|
SA::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, SA> +
|
||||||
|
Allocator<N, D, D> +
|
||||||
|
Allocator<N, D, U1> +
|
||||||
|
Allocator<N, U1, D>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, D, SB>) -> PointBase<N, D, SB> {
|
||||||
|
self * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, D, SB>) -> ColumnVector<N, D, SB> {
|
||||||
|
self * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimNameAdd<U1>, SA, SB, C> ProjectiveTransformation<PointBase<N, D, SB>> for TransformBase<N, D, SA, C>
|
||||||
|
where N: Real,
|
||||||
|
SA: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
C: SubTCategoryOf<TProjective>,
|
||||||
|
SA::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, SA> +
|
||||||
|
Allocator<N, D, D> +
|
||||||
|
Allocator<N, D, U1> +
|
||||||
|
Allocator<N, U1, D>,
|
||||||
|
SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, D, SB>) -> PointBase<N, D, SB> {
|
||||||
|
self.inverse() * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, D, SB>) -> ColumnVector<N, D, SB> {
|
||||||
|
self.inverse() * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// FIXME: we need to implement an SVD for this.
|
||||||
|
//
|
||||||
|
// impl<N, D: DimNameAdd<U1>, SA, SB, C> AffineTransformation<PointBase<N, D, SB>> for TransformBase<N, D, SA, C>
|
||||||
|
// where N: Real,
|
||||||
|
// SA: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
// SB: OwnedStorage<N, D, U1, Alloc = SA::Alloc>,
|
||||||
|
// C: SubTCategoryOf<TAffine>,
|
||||||
|
// SA::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, SA> +
|
||||||
|
// Allocator<N, D, D> +
|
||||||
|
// Allocator<N, D, U1> +
|
||||||
|
// Allocator<N, U1, D>,
|
||||||
|
// SB::Alloc: OwnedAllocator<N, D, U1, SB> {
|
||||||
|
// type PreRotation = OwnedRotation<N, D, SA::Alloc>;
|
||||||
|
// type NonUniformScaling = OwnedColumnVector<N, D, SA::Alloc>;
|
||||||
|
// type PostRotation = OwnedRotation<N, D, SA::Alloc>;
|
||||||
|
// type Translation = OwnedTranslation<N, D, SA::Alloc>;
|
||||||
|
//
|
||||||
|
// #[inline]
|
||||||
|
// fn decompose(&self) -> (Self::Translation, Self::PostRotation, Self::NonUniformScaling, Self::PreRotation) {
|
||||||
|
// unimplemented!()
|
||||||
|
// }
|
||||||
|
// }
|
16
src/geometry/transform_alias.rs
Normal file
16
src/geometry/transform_alias.rs
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U1, U2, U3, DimNameSum};
|
||||||
|
|
||||||
|
use geometry::{TransformBase, TGeneral, TProjective, TAffine};
|
||||||
|
|
||||||
|
pub type Transform<N, D> = TransformBase<N, D, MatrixArray<N, DimNameSum<D, U1>, DimNameSum<D, U1>>, TGeneral>;
|
||||||
|
pub type Projective<N, D> = TransformBase<N, D, MatrixArray<N, DimNameSum<D, U1>, DimNameSum<D, U1>>, TProjective>;
|
||||||
|
pub type Affine<N, D> = TransformBase<N, D, MatrixArray<N, DimNameSum<D, U1>, DimNameSum<D, U1>>, TAffine>;
|
||||||
|
|
||||||
|
pub type Transform2<N> = Transform<N, U2>;
|
||||||
|
pub type Projective2<N> = Projective<N, U2>;
|
||||||
|
pub type Affine2<N> = Affine<N, U2>;
|
||||||
|
|
||||||
|
pub type Transform3<N> = Transform<N, U3>;
|
||||||
|
pub type Projective3<N> = Projective<N, U3>;
|
||||||
|
pub type Affine3<N> = Affine<N, U3>;
|
36
src/geometry/transform_construction.rs
Normal file
36
src/geometry/transform_construction.rs
Normal file
@ -0,0 +1,36 @@
|
|||||||
|
use num::{Zero, One};
|
||||||
|
|
||||||
|
use alga::general::Field;
|
||||||
|
|
||||||
|
use core::{Scalar, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimNameAdd, DimNameSum, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{TransformBase, TCategory};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D, S, C: TCategory> TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Zero + One,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> {
|
||||||
|
/// Creates a new identity transform.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> Self {
|
||||||
|
Self::from_matrix_unchecked(OwnedSquareMatrix::<N, _, S::Alloc>::identity())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D, S, C: TCategory> One for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar + Field,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S: OwnedStorage<N, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
S::Alloc: OwnedAllocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>, S> {
|
||||||
|
/// Creates a new identity transform.
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
67
src/geometry/transform_conversion.rs
Normal file
67
src/geometry/transform_conversion.rs
Normal file
@ -0,0 +1,67 @@
|
|||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{SubsetOf, Field};
|
||||||
|
|
||||||
|
use core::{Scalar, SquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{TransformBase, TCategory, SuperTCategoryOf};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, C1, C2> SubsetOf<TransformBase<N2, D, SB, C2>> for TransformBase<N1, D, SA, C1>
|
||||||
|
where N1: Scalar + Field + ApproxEq + SubsetOf<N2>,
|
||||||
|
N2: Scalar + Field + ApproxEq,
|
||||||
|
C1: TCategory,
|
||||||
|
C2: SuperTCategoryOf<C1>,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA: OwnedStorage<N1, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB>,
|
||||||
|
N1::Epsilon: Copy,
|
||||||
|
N2::Epsilon: Copy {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TransformBase<N2, D, SB, C2> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(t: &TransformBase<N2, D, SB, C2>) -> bool {
|
||||||
|
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(t: &TransformBase<N2, D, SB, C2>) -> Self {
|
||||||
|
Self::from_superset_unchecked(t.matrix())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, C> SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>> for TransformBase<N1, D, SA, C>
|
||||||
|
where N1: Scalar + Field + ApproxEq + SubsetOf<N2>,
|
||||||
|
N2: Scalar + Field + ApproxEq,
|
||||||
|
C: TCategory,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA: OwnedStorage<N1, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB>,
|
||||||
|
N1::Epsilon: Copy,
|
||||||
|
N2::Epsilon: Copy {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SquareMatrix<N2, DimNameSum<D, U1>, SB> {
|
||||||
|
self.matrix().to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> bool {
|
||||||
|
C::check_homogeneous_invariants(m)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> Self {
|
||||||
|
TransformBase::from_matrix_unchecked(::convert_ref_unchecked(m))
|
||||||
|
}
|
||||||
|
}
|
608
src/geometry/transform_ops.rs
Normal file
608
src/geometry/transform_ops.rs
Normal file
@ -0,0 +1,608 @@
|
|||||||
|
use num::{Zero, One};
|
||||||
|
use std::ops::{Index, IndexMut, Mul, MulAssign, Div, DivAssign};
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{Field, Real, ClosedAdd, ClosedMul, ClosedNeg, SubsetOf};
|
||||||
|
|
||||||
|
use core::{Scalar, ColumnVector, OwnedColumnVector, OwnedSquareMatrix};
|
||||||
|
use core::storage::{Storage, StorageMut, OwnedStorage};
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1, U3, U4};
|
||||||
|
|
||||||
|
use geometry::{PointBase, OwnedPoint, TransformBase, OwnedTransform, TCategory, TCategoryMul,
|
||||||
|
SubTCategoryOf, SuperTCategoryOf, TGeneral, TProjective, TAffine, RotationBase,
|
||||||
|
UnitQuaternionBase, IsometryBase, SimilarityBase, TranslationBase};
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Indexing.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D, S, C: TCategory> Index<(usize, usize)> for TransformBase<N, D, S, C>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimName + DimNameAdd<U1>,
|
||||||
|
S: Storage<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
type Output = N;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn index(&self, ij: (usize, usize)) -> &N {
|
||||||
|
self.matrix().index(ij)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Only general transformations are mutably indexable.
|
||||||
|
impl<N, D, S> IndexMut<(usize, usize)> for TransformBase<N, D, S, TGeneral>
|
||||||
|
where N: Scalar,
|
||||||
|
D: DimName + DimNameAdd<U1>,
|
||||||
|
S: StorageMut<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
#[inline]
|
||||||
|
fn index_mut(&mut self, ij: (usize, usize)) -> &mut N {
|
||||||
|
self.matrix_mut().index_mut(ij)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* In the following, we provide:
|
||||||
|
* =========================
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Operators)
|
||||||
|
*
|
||||||
|
* [OK] TransformBase × IsometryBase
|
||||||
|
* [OK] TransformBase × RotationBase
|
||||||
|
* [OK] TransformBase × SimilarityBase
|
||||||
|
* [OK] TransformBase × TransformBase
|
||||||
|
* [OK] TransformBase × UnitQuaternion
|
||||||
|
* [OK] TransformBase × TranslationBase
|
||||||
|
* [OK] TransformBase × ColumnVector
|
||||||
|
* [OK] TransformBase × PointBase
|
||||||
|
*
|
||||||
|
* [OK] IsometryBase × TransformBase
|
||||||
|
* [OK] RotationBase × TransformBase
|
||||||
|
* [OK] SimilarityBase × TransformBase
|
||||||
|
* [OK] TranslationBase × TransformBase
|
||||||
|
* [OK] UnitQuaternionBase × TransformBase
|
||||||
|
*
|
||||||
|
* TransformBase ÷ IsometryBase
|
||||||
|
* [OK] TransformBase ÷ RotationBase
|
||||||
|
* TransformBase ÷ SimilarityBase
|
||||||
|
* [OK] TransformBase ÷ TransformBase
|
||||||
|
* [OK] TransformBase ÷ UnitQuaternion
|
||||||
|
* [OK] TransformBase ÷ TranslationBase
|
||||||
|
*
|
||||||
|
* IsometryBase ÷ TransformBase
|
||||||
|
* [OK] RotationBase ÷ TransformBase
|
||||||
|
* SimilarityBase ÷ TransformBase
|
||||||
|
* [OK] TranslationBase ÷ TransformBase
|
||||||
|
* [OK] UnitQuaternionBase ÷ TransformBase
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Assignment Operators)
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* [OK] TransformBase ×= TransformBase
|
||||||
|
* [OK] TransformBase ×= SimilarityBase
|
||||||
|
* [OK] TransformBase ×= IsometryBase
|
||||||
|
* [OK] TransformBase ×= RotationBase
|
||||||
|
* [OK] TransformBase ×= UnitQuaternionBase
|
||||||
|
* [OK] TransformBase ×= TranslationBase
|
||||||
|
*
|
||||||
|
* [OK] TransformBase ÷= TransformBase
|
||||||
|
* TransformBase ÷= SimilarityBase
|
||||||
|
* TransformBase ÷= IsometryBase
|
||||||
|
* [OK] TransformBase ÷= RotationBase
|
||||||
|
* [OK] TransformBase ÷= UnitQuaternionBase
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × ColumnVector
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Field;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1) for D: DimNameAdd<U1>, C: TCategory
|
||||||
|
where SA::Alloc: Allocator<N, D, D>
|
||||||
|
where SA::Alloc: Allocator<N, D, U1>
|
||||||
|
where SA::Alloc: Allocator<N, U1, D>;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: ColumnVector<N, D, SB>, Output = OwnedColumnVector<N, D, SA::Alloc>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => {
|
||||||
|
let transform = self.matrix().fixed_slice::<D, D>(0, 0);
|
||||||
|
|
||||||
|
if C::has_normalizer() {
|
||||||
|
let normalizer = self.matrix().fixed_slice::<U1, D>(D::dim(), 0);
|
||||||
|
let n = normalizer.tr_dot(&rhs);
|
||||||
|
|
||||||
|
if !n.is_zero() {
|
||||||
|
return transform * (rhs / n);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
transform * rhs
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × PointBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Field;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1) for D: DimNameAdd<U1>, C: TCategory
|
||||||
|
where SA::Alloc: Allocator<N, D, D>
|
||||||
|
where SA::Alloc: Allocator<N, D, U1>
|
||||||
|
where SA::Alloc: Allocator<N, U1, D>;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: PointBase<N, D, SB>, Output = OwnedPoint<N, D, SA::Alloc>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => {
|
||||||
|
let transform = self.matrix().fixed_slice::<D, D>(0, 0);
|
||||||
|
let translation = self.matrix().fixed_slice::<D, U1>(0, D::dim());
|
||||||
|
|
||||||
|
if C::has_normalizer() {
|
||||||
|
let normalizer = self.matrix().fixed_slice::<U1, D>(D::dim(), 0);
|
||||||
|
let n = normalizer.tr_dot(&rhs.coords) + unsafe { *self.matrix().get_unchecked(D::dim(), D::dim()) };
|
||||||
|
|
||||||
|
if !n.is_zero() {
|
||||||
|
return transform * (rhs / n) + translation;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
transform * rhs + translation
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (DimNameSum<D, U1>, DimNameSum<D, U1>) for D: DimNameAdd<U1>, CA: TCategoryMul<CB>, CB: TCategory;
|
||||||
|
self: TransformBase<N, D, SA, CA>, rhs: TransformBase<N, D, SB, CB>, Output = OwnedTransform<N, D, SA::Alloc, CA::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.unwrap());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.unwrap());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.matrix());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × RotationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: One;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, D) for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: RotationBase<N, D, SB>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase × TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: One;
|
||||||
|
(D, D), (DimNameSum<D, U1>, DimNameSum<D, U1>) for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: RotationBase<N, D, SA>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × UnitQuaternionBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(U4, U4), (U4, U1) for C: TCategoryMul<TAffine>
|
||||||
|
where SB::Alloc: Allocator<N, U3, U3>
|
||||||
|
where SB::Alloc: Allocator<N, U4, U4>;
|
||||||
|
self: TransformBase<N, U3, SA, C>, rhs: UnitQuaternionBase<N, SB>, Output = OwnedTransform<N, U3, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternionBase × TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(U4, U1), (U4, U4) for C: TCategoryMul<TAffine>
|
||||||
|
where SA::Alloc: Allocator<N, U3, U3>
|
||||||
|
where SA::Alloc: Allocator<N, U4, U4>;
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: TransformBase<N, U3, SB, C>, Output = OwnedTransform<N, U3, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × IsometryBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: IsometryBase<N, D, SB, R>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
);
|
||||||
|
|
||||||
|
// IsometryBase × TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(D, U1), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SA::Alloc> >
|
||||||
|
where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: IsometryBase<N, D, SA, R>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase × SimilarityBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
where SB::Alloc: Allocator<N, D, D >
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: SimilarityBase<N, D, SB, R>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
);
|
||||||
|
|
||||||
|
// SimilarityBase × TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(D, U1), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SA::Alloc> >
|
||||||
|
where SA::Alloc: Allocator<N, D, D >
|
||||||
|
where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: SimilarityBase<N, D, SA, R>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* FIXME: don't explicitly build the homogeneous translation matrix.
|
||||||
|
* Directly apply the translation, just as in `Matrix::{append,prepend}_translation`. This has not
|
||||||
|
* been done yet because of the `DimNameDiff` requirement (which is not automatically deduced from
|
||||||
|
* `DimNameAdd` requirement).
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
// TransformBase × TranslationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1) for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: TranslationBase<N, D, SB>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
);
|
||||||
|
|
||||||
|
// TranslationBase × TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Mul, mul where N: Real;
|
||||||
|
(D, U1), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TranslationBase<N, D, SA>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
[val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
[ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷ TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: ApproxEq, Field;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (DimNameSum<D, U1>, DimNameSum<D, U1>) for D: DimNameAdd<U1>, CA: TCategoryMul<CB>, CB: SubTCategoryOf<TProjective>;
|
||||||
|
self: TransformBase<N, D, SA, CA>, rhs: TransformBase<N, D, SB, CB>, Output = OwnedTransform<N, D, SA::Alloc, CA::Representative>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.clone_owned().inverse();
|
||||||
|
[ref ref] => self * rhs.clone_owned().inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
// TransformBase ÷ RotationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: One;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, D) for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: RotationBase<N, D, SB>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase ÷ TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: One;
|
||||||
|
(D, D), (DimNameSum<D, U1>, DimNameSum<D, U1>) for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: RotationBase<N, D, SA>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => self.inverse() * rhs;
|
||||||
|
[ref val] => self.inverse() * rhs;
|
||||||
|
[val ref] => self.inverse() * rhs;
|
||||||
|
[ref ref] => self.inverse() * rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷ UnitQuaternionBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: Real;
|
||||||
|
(U4, U4), (U4, U1) for C: TCategoryMul<TAffine>
|
||||||
|
where SB::Alloc: Allocator<N, U3, U3>
|
||||||
|
where SB::Alloc: Allocator<N, U4, U4>;
|
||||||
|
self: TransformBase<N, U3, SA, C>, rhs: UnitQuaternionBase<N, SB>, Output = OwnedTransform<N, U3, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitQuaternionBase ÷ TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: Real;
|
||||||
|
(U4, U1), (U4, U4) for C: TCategoryMul<TAffine>
|
||||||
|
where SA::Alloc: Allocator<N, U3, U3>
|
||||||
|
where SA::Alloc: Allocator<N, U4, U4>;
|
||||||
|
self: UnitQuaternionBase<N, SA>, rhs: TransformBase<N, U3, SB, C>, Output = OwnedTransform<N, U3, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => self.inverse() * rhs;
|
||||||
|
[ref val] => self.inverse() * rhs;
|
||||||
|
[val ref] => self.inverse() * rhs;
|
||||||
|
[ref ref] => self.inverse() * rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// // TransformBase ÷ IsometryBase
|
||||||
|
// md_impl_all!(
|
||||||
|
// Div, div where N: Real;
|
||||||
|
// (DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
// for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
// where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
// self: TransformBase<N, D, SA, C>, rhs: IsometryBase<N, D, SB, R>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
// [val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.inverse().to_homogeneous());
|
||||||
|
// [ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.inverse().to_homogeneous());
|
||||||
|
// [val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.inverse().to_homogeneous());
|
||||||
|
// [ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.inverse().to_homogeneous());
|
||||||
|
// );
|
||||||
|
|
||||||
|
// // IsometryBase ÷ TransformBase
|
||||||
|
// md_impl_all!(
|
||||||
|
// Div, div where N: Real;
|
||||||
|
// (D, U1), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
// for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SA::Alloc> >
|
||||||
|
// where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
// self: IsometryBase<N, D, SA, R>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
// [val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
// [ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
// [val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
// [ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
// );
|
||||||
|
|
||||||
|
|
||||||
|
// // TransformBase ÷ SimilarityBase
|
||||||
|
// md_impl_all!(
|
||||||
|
// Div, div where N: Real;
|
||||||
|
// (DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
// for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
// where SB::Alloc: Allocator<N, D, D >
|
||||||
|
// where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
// self: TransformBase<N, D, SA, C>, rhs: SimilarityBase<N, D, SB, R>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
// [val val] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
// [ref val] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
// [val ref] => Self::Output::from_matrix_unchecked(self.unwrap() * rhs.to_homogeneous());
|
||||||
|
// [ref ref] => Self::Output::from_matrix_unchecked(self.matrix() * rhs.to_homogeneous());
|
||||||
|
// );
|
||||||
|
|
||||||
|
// // SimilarityBase ÷ TransformBase
|
||||||
|
// md_impl_all!(
|
||||||
|
// Div, div where N: Real;
|
||||||
|
// (D, U1), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
// for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SA::Alloc> >
|
||||||
|
// where SA::Alloc: Allocator<N, D, D >
|
||||||
|
// where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
// self: SimilarityBase<N, D, SA, R>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
// [val val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
// [ref val] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.unwrap());
|
||||||
|
// [val ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
// [ref ref] => Self::Output::from_matrix_unchecked(self.to_homogeneous() * rhs.matrix());
|
||||||
|
// );
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷ TranslationBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: Real;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1) for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: TranslationBase<N, D, SB>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => self * rhs.inverse();
|
||||||
|
[ref val] => self * rhs.inverse();
|
||||||
|
[val ref] => self * rhs.inverse();
|
||||||
|
[ref ref] => self * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
// TranslationBase ÷ TransformBase
|
||||||
|
md_impl_all!(
|
||||||
|
Div, div where N: Real;
|
||||||
|
(D, U1), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategoryMul<TAffine>
|
||||||
|
where SA::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TranslationBase<N, D, SA>, rhs: TransformBase<N, D, SB, C>, Output = OwnedTransform<N, D, SA::Alloc, C::Representative>;
|
||||||
|
[val val] => self.inverse() * rhs;
|
||||||
|
[ref val] => self.inverse() * rhs;
|
||||||
|
[val ref] => self.inverse() * rhs;
|
||||||
|
[ref ref] => self.inverse() * rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ×= TransformBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (DimNameSum<D, U1>, DimNameSum<D, U1>) for D: DimNameAdd<U1>, CA: TCategory, CB: SubTCategoryOf<CA>;
|
||||||
|
self: TransformBase<N, D, SA, CA>, rhs: TransformBase<N, D, SB, CB>;
|
||||||
|
[val] => *self.matrix_mut_unchecked() *= rhs.unwrap();
|
||||||
|
[ref] => *self.matrix_mut_unchecked() *= rhs.matrix();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ×= SimilarityBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategory, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >
|
||||||
|
where SB::Alloc: Allocator<N, D, D>;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: SimilarityBase<N, D, SB, R>;
|
||||||
|
[val] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
[ref] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ×= IsometryBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
for D: DimNameAdd<U1>, C: TCategory, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: IsometryBase<N, D, SB, R>;
|
||||||
|
[val] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
[ref] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* FIXME: don't explicitly build the homogeneous translation matrix.
|
||||||
|
* Directly apply the translation, just as in `Matrix::{append,prepend}_translation`. This has not
|
||||||
|
* been done yet because of the `DimNameDiff` requirement (which is not automatically deduced from
|
||||||
|
* `DimNameAdd` requirement).
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
// TransformBase ×= TranslationBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign where N: One;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1) for D: DimNameAdd<U1>, C: TCategory
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: TranslationBase<N, D, SB>;
|
||||||
|
[val] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
[ref] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ×= RotationBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign where N: One;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, D) for D: DimNameAdd<U1>, C: TCategory
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: RotationBase<N, D, SB>;
|
||||||
|
[val] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
[ref] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ×= UnitQuaternionBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
MulAssign, mul_assign where N: Real;
|
||||||
|
(U4, U4), (U4, U1) for C: TCategory
|
||||||
|
where SB::Alloc: Allocator<N, U3, U3>
|
||||||
|
where SB::Alloc: Allocator<N, U4, U4>;
|
||||||
|
self: TransformBase<N, U3, SA, C>, rhs: UnitQuaternionBase<N, SB>;
|
||||||
|
[val] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
[ref] => *self.matrix_mut_unchecked() *= rhs.to_homogeneous();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷= TransformBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
DivAssign, div_assign where N: Field, ApproxEq;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (DimNameSum<D, U1>, DimNameSum<D, U1>)
|
||||||
|
for D: DimNameAdd<U1>, CA: SuperTCategoryOf<CB>, CB: SubTCategoryOf<TProjective>;
|
||||||
|
self: TransformBase<N, D, SA, CA>, rhs: TransformBase<N, D, SB, CB>;
|
||||||
|
[val] => *self *= rhs.clone_owned().inverse();
|
||||||
|
[ref] => *self *= rhs.clone_owned().inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// // TransformBase ÷= SimilarityBase
|
||||||
|
// md_assign_impl_all!(
|
||||||
|
// DivAssign, div_assign;
|
||||||
|
// (DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
// for D: DimNameAdd<U1>, C: TCategory, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
// where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >
|
||||||
|
// where SB::Alloc: Allocator<N, D, D>;
|
||||||
|
// self: TransformBase<N, D, SA, C>, rhs: SimilarityBase<N, D, SB, R>;
|
||||||
|
// [val] => *self *= rhs.inverse();
|
||||||
|
// [ref] => *self *= rhs.inverse();
|
||||||
|
// );
|
||||||
|
//
|
||||||
|
//
|
||||||
|
// // TransformBase ÷= IsometryBase
|
||||||
|
// md_assign_impl_all!(
|
||||||
|
// DivAssign, div_assign;
|
||||||
|
// (DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1)
|
||||||
|
// for D: DimNameAdd<U1>, C: TCategory, R: SubsetOf<OwnedSquareMatrix<N, DimNameSum<D, U1>, SB::Alloc> >
|
||||||
|
// where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
// self: TransformBase<N, D, SA, C>, rhs: IsometryBase<N, D, SB, R>;
|
||||||
|
// [val] => *self *= rhs.inverse();
|
||||||
|
// [ref] => *self *= rhs.inverse();
|
||||||
|
// );
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷= TranslationBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
DivAssign, div_assign where N: One, ClosedNeg;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, U1) for D: DimNameAdd<U1>, C: TCategory
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: TranslationBase<N, D, SB>;
|
||||||
|
[val] => *self *= rhs.inverse();
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷= RotationBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
DivAssign, div_assign where N: One;
|
||||||
|
(DimNameSum<D, U1>, DimNameSum<D, U1>), (D, D) for D: DimNameAdd<U1>, C: TCategory
|
||||||
|
where SB::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1> >;
|
||||||
|
self: TransformBase<N, D, SA, C>, rhs: RotationBase<N, D, SB>;
|
||||||
|
[val] => *self *= rhs.inverse();
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// TransformBase ÷= UnitQuaternionBase
|
||||||
|
md_assign_impl_all!(
|
||||||
|
DivAssign, div_assign where N: Real;
|
||||||
|
(U4, U4), (U4, U1) for C: TCategory
|
||||||
|
where SB::Alloc: Allocator<N, U3, U3>
|
||||||
|
where SB::Alloc: Allocator<N, U4, U4>;
|
||||||
|
self: TransformBase<N, U3, SA, C>, rhs: UnitQuaternionBase<N, SB>;
|
||||||
|
[val] => *self *= rhs.inverse();
|
||||||
|
[ref] => *self *= rhs.inverse();
|
||||||
|
);
|
142
src/geometry/translation.rs
Normal file
142
src/geometry/translation.rs
Normal file
@ -0,0 +1,142 @@
|
|||||||
|
use num::{Zero, One};
|
||||||
|
use std::fmt;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
|
||||||
|
use alga::general::{Real, ClosedNeg};
|
||||||
|
|
||||||
|
use core::{Scalar, ColumnVector, OwnedSquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameSum, DimNameAdd, U1};
|
||||||
|
use core::storage::{Storage, StorageMut, Owned};
|
||||||
|
use core::allocator::Allocator;
|
||||||
|
|
||||||
|
/// A translation with an owned vector storage.
|
||||||
|
pub type OwnedTranslation<N, D, S> = TranslationBase<N, D, Owned<N, D, U1, <S as Storage<N, D, U1>>::Alloc>>;
|
||||||
|
|
||||||
|
/// A translation.
|
||||||
|
#[repr(C)]
|
||||||
|
#[derive(Hash, Debug, Clone, Copy)]
|
||||||
|
pub struct TranslationBase<N: Scalar, D: DimName, S/*: Storage<N, D, U1>*/> {
|
||||||
|
pub vector: ColumnVector<N, D, S>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> TranslationBase<N, D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
/// Creates a new translation from the given vector.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_vector(vector: ColumnVector<N, D, S>) -> TranslationBase<N, D, S> {
|
||||||
|
TranslationBase {
|
||||||
|
vector: vector
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts `self`.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(&self) -> OwnedTranslation<N, D, S>
|
||||||
|
where N: ClosedNeg {
|
||||||
|
TranslationBase::from_vector(-&self.vector)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Converts this translation into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous(&self) -> OwnedSquareMatrix<N, DimNameSum<D, U1>, S::Alloc>
|
||||||
|
where N: Zero + One,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
S::Alloc: Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>> {
|
||||||
|
let mut res = OwnedSquareMatrix::<N, _, S::Alloc>::identity();
|
||||||
|
res.fixed_slice_mut::<D, U1>(0, D::dim()).copy_from(&self.vector);
|
||||||
|
|
||||||
|
res
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + ClosedNeg,
|
||||||
|
S: StorageMut<N, D, U1> {
|
||||||
|
/// Inverts `self` in-place.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self) {
|
||||||
|
self.vector.neg_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Eq for TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + Eq,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> PartialEq for TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + PartialEq,
|
||||||
|
S: Storage<N, D, U1> {
|
||||||
|
#[inline]
|
||||||
|
fn eq(&self, right: &TranslationBase<N, D, S>) -> bool {
|
||||||
|
self.vector == right.vector
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> ApproxEq for TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + ApproxEq,
|
||||||
|
S: Storage<N, D, U1>,
|
||||||
|
N::Epsilon: Copy {
|
||||||
|
type Epsilon = N::Epsilon;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_epsilon() -> Self::Epsilon {
|
||||||
|
N::default_epsilon()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_relative() -> Self::Epsilon {
|
||||||
|
N::default_max_relative()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn default_max_ulps() -> u32 {
|
||||||
|
N::default_max_ulps()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn relative_eq(&self, other: &Self, epsilon: Self::Epsilon, max_relative: Self::Epsilon) -> bool {
|
||||||
|
self.vector.relative_eq(&other.vector, epsilon, max_relative)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
||||||
|
self.vector.ulps_eq(&other.vector, epsilon, max_ulps)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> fmt::Display for TranslationBase<N, D, S>
|
||||||
|
where N: Real + fmt::Display,
|
||||||
|
S: Storage<N, D, U1>,
|
||||||
|
S::Alloc: Allocator<usize, D, U1> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
let precision = f.precision().unwrap_or(3);
|
||||||
|
|
||||||
|
try!(writeln!(f, "TranslationBase {{"));
|
||||||
|
try!(write!(f, "{:.*}", precision, self.vector));
|
||||||
|
writeln!(f, "}}")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// // /*
|
||||||
|
// // *
|
||||||
|
// // * Absolute
|
||||||
|
// // *
|
||||||
|
// // */
|
||||||
|
// // impl<N: Absolute> Absolute for $t<N> {
|
||||||
|
// // type AbsoluteValue = $submatrix<N::AbsoluteValue>;
|
||||||
|
// //
|
||||||
|
// // #[inline]
|
||||||
|
// // fn abs(m: &$t<N>) -> $submatrix<N::AbsoluteValue> {
|
||||||
|
// // Absolute::abs(&m.submatrix)
|
||||||
|
// // }
|
||||||
|
// // }
|
||||||
|
// */
|
209
src/geometry/translation_alga.rs
Normal file
209
src/geometry/translation_alga.rs
Normal file
@ -0,0 +1,209 @@
|
|||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractLoop, AbstractMonoid, AbstractQuasigroup,
|
||||||
|
AbstractSemigroup, Real, Inverse, Multiplicative, Identity, Id};
|
||||||
|
use alga::linear::{Transformation, ProjectiveTransformation, Similarity, AffineTransformation,
|
||||||
|
Isometry, DirectIsometry, Translation};
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::{TranslationBase, PointBase};
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Algebraic structures.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Identity<Multiplicative> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Inverse<Multiplicative> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.inverse_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> AbstractMagma<Multiplicative> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_multiplicative_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N, D: DimName, S> $marker<$operator> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
impl_multiplicative_structures!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Transformation groups.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N, D: DimName, S> Transformation<PointBase<N, D, S>> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
pt + &self.vector
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, D, S>) -> ColumnVector<N, D, S> {
|
||||||
|
v.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> ProjectiveTransformation<PointBase<N, D, S>> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, D, S>) -> PointBase<N, D, S> {
|
||||||
|
pt - &self.vector
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, D, S>) -> ColumnVector<N, D, S> {
|
||||||
|
v.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> AffineTransformation<PointBase<N, D, S>> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Rotation = Id;
|
||||||
|
type NonUniformScaling = Id;
|
||||||
|
type Translation = Self;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn decompose(&self) -> (Self, Id, Id, Id) {
|
||||||
|
(self.clone(), Id::new(), Id::new(), Id::new())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_translation(&self, t: &Self::Translation) -> Self {
|
||||||
|
t * self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_translation(&self, t: &Self::Translation) -> Self {
|
||||||
|
self * t
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation(&self, _: &Self::Rotation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_rotation(&self, _: &Self::Rotation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Similarity<PointBase<N, D, S>> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
type Scaling = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation(&self) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaling(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! marker_impl(
|
||||||
|
($($Trait: ident),*) => {$(
|
||||||
|
impl<N, D: DimName, S> $Trait<PointBase<N, D, S>> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
marker_impl!(Isometry, DirectIsometry);
|
||||||
|
|
||||||
|
|
||||||
|
/// Subgroups of the n-dimensional translation group `T(n)`.
|
||||||
|
impl<N, D: DimName, S> Translation<PointBase<N, D, S>> for TranslationBase<N, D, S>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn to_vector(&self) -> ColumnVector<N, D, S> {
|
||||||
|
self.vector.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn from_vector(v: ColumnVector<N, D, S>) -> Option<Self> {
|
||||||
|
Some(Self::from_vector(v))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn powf(&self, n: N) -> Option<Self> {
|
||||||
|
Some(Self::from_vector(&self.vector * n))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation_between(a: &PointBase<N, D, S>, b: &PointBase<N, D, S>) -> Option<Self> {
|
||||||
|
Some(Self::from_vector(b - a))
|
||||||
|
}
|
||||||
|
}
|
13
src/geometry/translation_alias.rs
Normal file
13
src/geometry/translation_alias.rs
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
use core::MatrixArray;
|
||||||
|
use core::dimension::{U1, U2, U3};
|
||||||
|
|
||||||
|
use geometry::TranslationBase;
|
||||||
|
|
||||||
|
/// A D-dimensional translation.
|
||||||
|
pub type Translation<N, D> = TranslationBase<N, D, MatrixArray<N, D, U1>>;
|
||||||
|
|
||||||
|
/// A 2-dimensional translation.
|
||||||
|
pub type Translation2<N> = Translation<N, U2>;
|
||||||
|
|
||||||
|
/// A 3-dimensional translation.
|
||||||
|
pub type Translation3<N> = Translation<N, U3>;
|
86
src/geometry/translation_construction.rs
Normal file
86
src/geometry/translation_construction.rs
Normal file
@ -0,0 +1,86 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use num::{Zero, One};
|
||||||
|
use rand::{Rng, Rand};
|
||||||
|
|
||||||
|
use alga::general::ClosedAdd;
|
||||||
|
|
||||||
|
use core::{ColumnVector, Scalar};
|
||||||
|
use core::dimension::{DimName, U1, U2, U3, U4, U5, U6};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
|
||||||
|
use geometry::TranslationBase;
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + Zero,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
/// Creates a new square identity rotation of the given `dimension`.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> TranslationBase<N, D, S> {
|
||||||
|
Self::from_vector(ColumnVector::<N, D, S>::from_element(N::zero()))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> One for TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + Zero + ClosedAdd,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, D: DimName, S> Rand for TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + Rand,
|
||||||
|
S: OwnedStorage<N, D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<G: Rng>(rng: &mut G) -> Self {
|
||||||
|
Self::from_vector(rng.gen())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
impl<N, D: DimName, S> Arbitrary for TranslationBase<N, D, S>
|
||||||
|
where N: Scalar + Arbitrary + Send,
|
||||||
|
S: OwnedStorage<N, D, U1> + Send,
|
||||||
|
S::Alloc: OwnedAllocator<N, D, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(rng: &mut G) -> Self {
|
||||||
|
Self::from_vector(Arbitrary::arbitrary(rng))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Small translation construction from components.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
macro_rules! componentwise_constructors_impl(
|
||||||
|
($($D: ty, $($args: ident:$irow: expr),*);* $(;)*) => {$(
|
||||||
|
impl<N, S> TranslationBase<N, $D, S>
|
||||||
|
where N: Scalar,
|
||||||
|
S: OwnedStorage<N, $D, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, $D, U1, S> {
|
||||||
|
/// Initializes this matrix from its components.
|
||||||
|
#[inline]
|
||||||
|
pub fn new($($args: N),*) -> Self {
|
||||||
|
Self::from_vector(ColumnVector::<N, $D, S>::new($($args),*))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
componentwise_constructors_impl!(
|
||||||
|
U1, x:0;
|
||||||
|
U2, x:0, y:1;
|
||||||
|
U3, x:0, y:1, z:2;
|
||||||
|
U4, x:0, y:1, z:2, w:3;
|
||||||
|
U5, x:0, y:1, z:2, w:3, a:4;
|
||||||
|
U6, x:0, y:1, z:2, w:3, a:4, b:5;
|
||||||
|
);
|
159
src/geometry/translation_conversion.rs
Normal file
159
src/geometry/translation_conversion.rs
Normal file
@ -0,0 +1,159 @@
|
|||||||
|
use alga::general::{SubsetOf, SupersetOf, Real};
|
||||||
|
use alga::linear::Rotation;
|
||||||
|
|
||||||
|
use core::{Scalar, ColumnVector, SquareMatrix};
|
||||||
|
use core::dimension::{DimName, DimNameAdd, DimNameSum, U1};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::{Allocator, OwnedAllocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, TranslationBase, IsometryBase, SimilarityBase, TransformBase, SuperTCategoryOf, TAffine};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides the following conversions:
|
||||||
|
* =============================================
|
||||||
|
*
|
||||||
|
* TranslationBase -> TranslationBase
|
||||||
|
* TranslationBase -> IsometryBase
|
||||||
|
* TranslationBase -> SimilarityBase
|
||||||
|
* TranslationBase -> TransformBase
|
||||||
|
* TranslationBase -> Matrix (homogeneous)
|
||||||
|
*/
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB> SubsetOf<TranslationBase<N2, D, SB>> for TranslationBase<N1, D, SA>
|
||||||
|
where N1: Scalar,
|
||||||
|
N2: Scalar + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TranslationBase<N2, D, SB> {
|
||||||
|
TranslationBase::from_vector(self.vector.to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(rot: &TranslationBase<N2, D, SB>) -> bool {
|
||||||
|
::is_convertible::<_, ColumnVector<N1, D, SA>>(&rot.vector)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(rot: &TranslationBase<N2, D, SB>) -> Self {
|
||||||
|
TranslationBase::from_vector(rot.vector.to_subset_unchecked())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R> SubsetOf<IsometryBase<N2, D, SB, R>> for TranslationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
R: Rotation<PointBase<N2, D, SB>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> IsometryBase<N2, D, SB, R> {
|
||||||
|
IsometryBase::from_parts(self.to_superset(), R::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(iso: &IsometryBase<N2, D, SB, R>) -> bool {
|
||||||
|
iso.rotation == R::identity()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(iso: &IsometryBase<N2, D, SB, R>) -> Self {
|
||||||
|
Self::from_superset_unchecked(&iso.translation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D: DimName, SA, SB, R> SubsetOf<SimilarityBase<N2, D, SB, R>> for TranslationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, D, U1>,
|
||||||
|
R: Rotation<PointBase<N2, D, SB>>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, D, U1, SB> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SimilarityBase<N2, D, SB, R> {
|
||||||
|
SimilarityBase::from_parts(self.to_superset(), R::identity(), N2::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(sim: &SimilarityBase<N2, D, SB, R>) -> bool {
|
||||||
|
sim.isometry.rotation == R::identity() &&
|
||||||
|
sim.scaling() == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(sim: &SimilarityBase<N2, D, SB, R>) -> Self {
|
||||||
|
Self::from_superset_unchecked(&sim.isometry.translation)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB, C> SubsetOf<TransformBase<N2, D, SB, C>> for TranslationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
C: SuperTCategoryOf<TAffine>,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, U1> +
|
||||||
|
Allocator<N2, DimNameSum<D, U1>, D> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> TransformBase<N2, D, SB, C> {
|
||||||
|
TransformBase::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(t: &TransformBase<N2, D, SB, C>) -> bool {
|
||||||
|
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(t: &TransformBase<N2, D, SB, C>) -> Self {
|
||||||
|
Self::from_superset_unchecked(t.matrix())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
impl<N1, N2, D, SA, SB> SubsetOf<SquareMatrix<N2, DimNameSum<D, U1>, SB>> for TranslationBase<N1, D, SA>
|
||||||
|
where N1: Real,
|
||||||
|
N2: Real + SupersetOf<N1>,
|
||||||
|
SA: OwnedStorage<N1, D, U1>,
|
||||||
|
SB: OwnedStorage<N2, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
D: DimNameAdd<U1>,
|
||||||
|
SA::Alloc: OwnedAllocator<N1, D, U1, SA> +
|
||||||
|
Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>>,
|
||||||
|
SB::Alloc: OwnedAllocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>, SB> +
|
||||||
|
Allocator<N2, D, U1> +
|
||||||
|
Allocator<N2, DimNameSum<D, U1>, D> {
|
||||||
|
#[inline]
|
||||||
|
fn to_superset(&self) -> SquareMatrix<N2, DimNameSum<D, U1>, SB> {
|
||||||
|
self.to_homogeneous().to_superset()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn is_in_subset(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> bool {
|
||||||
|
let id = m.fixed_slice::<DimNameSum<D, U1>, D>(0, 0);
|
||||||
|
|
||||||
|
// Scalar types agree.
|
||||||
|
m.iter().all(|e| SupersetOf::<N1>::is_in_subset(e)) &&
|
||||||
|
// The block part does nothing.
|
||||||
|
id.is_identity(N2::zero()) &&
|
||||||
|
// The normalization factor is one.
|
||||||
|
m[(D::dim(), D::dim())] == N2::one()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
unsafe fn from_superset_unchecked(m: &SquareMatrix<N2, DimNameSum<D, U1>, SB>) -> Self {
|
||||||
|
let t = m.fixed_slice::<D, U1>(0, D::dim());
|
||||||
|
Self::from_vector(::convert_unchecked(t.into_owned()))
|
||||||
|
}
|
||||||
|
}
|
168
src/geometry/translation_ops.rs
Normal file
168
src/geometry/translation_ops.rs
Normal file
@ -0,0 +1,168 @@
|
|||||||
|
use std::ops::{Mul, MulAssign, Div, DivAssign};
|
||||||
|
|
||||||
|
use alga::general::{ClosedAdd, ClosedSub};
|
||||||
|
|
||||||
|
use core::Scalar;
|
||||||
|
use core::dimension::{DimName, U1};
|
||||||
|
use core::constraint::{ShapeConstraint, SameNumberOfRows, SameNumberOfColumns};
|
||||||
|
use core::storage::{OwnedStorage, Storage};
|
||||||
|
use core::allocator::{OwnedAllocator, SameShapeAllocator};
|
||||||
|
|
||||||
|
use geometry::{PointBase, OwnedPoint, TranslationBase, OwnedTranslation};
|
||||||
|
|
||||||
|
// TranslationBase × TranslationBase
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: &'a TranslationBase<N, D, SA>, right: &'b TranslationBase<N, D, SB>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(&self.vector + &right.vector); 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: &'a TranslationBase<N, D, SB>, right: TranslationBase<N, D, SA>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(&self.vector + right.vector); 'a);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: &'b TranslationBase<N, D, SB>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(self.vector + &right.vector); 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: TranslationBase<N, D, SB>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(self.vector + right.vector); );
|
||||||
|
|
||||||
|
// TranslationBase ÷ TranslationBase
|
||||||
|
// FIXME: instead of calling inverse explicitely, could we just add a `mul_tr` or `mul_inv` method?
|
||||||
|
add_sub_impl!(Div, div, ClosedSub;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: &'a TranslationBase<N, D, SA>, right: &'b TranslationBase<N, D, SB>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(&self.vector - &right.vector); 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Div, div, ClosedSub;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: &'a TranslationBase<N, D, SB>, right: TranslationBase<N, D, SA>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(&self.vector - right.vector); 'a);
|
||||||
|
|
||||||
|
add_sub_impl!(Div, div, ClosedSub;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: &'b TranslationBase<N, D, SB>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(self.vector - &right.vector); 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Div, div, ClosedSub;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: TranslationBase<N, D, SB>, Output = OwnedTranslation<N, D, SA>;
|
||||||
|
TranslationBase::from_vector(self.vector - right.vector); );
|
||||||
|
|
||||||
|
|
||||||
|
// TranslationBase × PointBase
|
||||||
|
// FIXME: we don't handle properly non-zero origins here. Do we want this to be the intended
|
||||||
|
// behavior?
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: &'a TranslationBase<N, D, SB>, right: &'b PointBase<N, D, SA>, Output = OwnedPoint<N, D, SA::Alloc>;
|
||||||
|
right + &self.vector; 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: &'a TranslationBase<N, D, SB>, right: PointBase<N, D, SA>, Output = OwnedPoint<N, D, SA::Alloc>;
|
||||||
|
right + &self.vector; 'a);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: &'b PointBase<N, D, SB>, Output = OwnedPoint<N, D, SB::Alloc>;
|
||||||
|
right + self.vector; 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) -> (D) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SB>, right: PointBase<N, D, SA>, Output = OwnedPoint<N, D, SA::Alloc>;
|
||||||
|
right + self.vector; );
|
||||||
|
|
||||||
|
|
||||||
|
// TranslationBase *= TranslationBase
|
||||||
|
add_sub_assign_impl!(MulAssign, mul_assign, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: &'b TranslationBase<N, D, SB>;
|
||||||
|
self.vector += &right.vector; 'b);
|
||||||
|
|
||||||
|
add_sub_assign_impl!(MulAssign, mul_assign, ClosedAdd;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: TranslationBase<N, D, SB>;
|
||||||
|
self.vector += right.vector; );
|
||||||
|
|
||||||
|
|
||||||
|
add_sub_assign_impl!(DivAssign, div_assign, ClosedSub;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: &'b TranslationBase<N, D, SB>;
|
||||||
|
self.vector -= &right.vector; 'b);
|
||||||
|
|
||||||
|
add_sub_assign_impl!(DivAssign, div_assign, ClosedSub;
|
||||||
|
(D, U1), (D, U1) for D: DimName;
|
||||||
|
self: TranslationBase<N, D, SA>, right: TranslationBase<N, D, SB>;
|
||||||
|
self.vector -= right.vector; );
|
||||||
|
|
||||||
|
/*
|
||||||
|
// TranslationBase × Matrix
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(D1, D1), (R2, C2) for D1, R2, C2;
|
||||||
|
self: &'a TranslationBase<N, D1, SA>, right: &'b Matrix<N, R2, C2, SB>, Output = MatrixMul<N, D1, D1, C2 , SA>;
|
||||||
|
self.vector() * right; 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(D1, D1), (R2, C2) for D1, R2, C2;
|
||||||
|
self: &'a TranslationBase<N, D1, SA>, right: Matrix<N, R2, C2, SB>, Output = MatrixMul<N, D1, D1, C2 , SA>;
|
||||||
|
self.vector() * right; 'a);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(D1, D1), (R2, C2) for D1, R2, C2;
|
||||||
|
self: TranslationBase<N, D1, SA>, right: &'b Matrix<N, R2, C2, SB>, Output = MatrixMul<N, D1, D1, C2 , SA>;
|
||||||
|
self.unwrap() * right; 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(D1, D1), (R2, C2) for D1, R2, C2;
|
||||||
|
self: TranslationBase<N, D1, SA>, right: Matrix<N, R2, C2, SB>, Output = MatrixMul<N, D1, D1, C2 , SA>;
|
||||||
|
self.unwrap() * right; );
|
||||||
|
|
||||||
|
// Matrix × TranslationBase
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(R1, C1), (D2, D2) for R1, C1, D2;
|
||||||
|
self: &'a Matrix<N, R1, C1, SA>, right: &'b TranslationBase<N, D2, SB>, Output = MatrixMul<N, R1, C1, D2, SA>;
|
||||||
|
self * right.vector(); 'a, 'b);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(R1, C1), (D2, D2) for R1, C1, D2;
|
||||||
|
self: &'a Matrix<N, R1, C1, SA>, right: TranslationBase<N, D2, SB>, Output = MatrixMul<N, R1, C1, D2, SA>;
|
||||||
|
self * right.unwrap(); 'a);
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(R1, C1), (D2, D2) for R1, C1, D2;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: &'b TranslationBase<N, D2, SB>, Output = MatrixMul<N, R1, C1, D2, SA>;
|
||||||
|
self * right.vector(); 'b);
|
||||||
|
|
||||||
|
|
||||||
|
add_sub_impl!(Mul, mul;
|
||||||
|
(R1, C1), (D2, D2) for R1, C1, D2;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: TranslationBase<N, D2, SB>, Output = MatrixMul<N, R1, C1, D2, SA>;
|
||||||
|
self * right.unwrap(); );
|
||||||
|
|
||||||
|
// Matrix *= TranslationBase
|
||||||
|
md_assign_impl!(MulAssign, mul_assign;
|
||||||
|
(R1, C1), (C1, C1) for R1, C1;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: &'b TranslationBase<N, C1, SB>;
|
||||||
|
self.mul_assign(right.vector()); 'b);
|
||||||
|
|
||||||
|
md_assign_impl!(MulAssign, mul_assign;
|
||||||
|
(R1, C1), (C1, C1) for R1, C1;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: TranslationBase<N, C1, SB>;
|
||||||
|
self.mul_assign(right.unwrap()); );
|
||||||
|
|
||||||
|
|
||||||
|
md_assign_impl!(DivAssign, div_assign;
|
||||||
|
(R1, C1), (C1, C1) for R1, C1;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: &'b TranslationBase<N, C1, SB>;
|
||||||
|
self.mul_assign(right.inverse().vector()); 'b);
|
||||||
|
|
||||||
|
md_assign_impl!(DivAssign, div_assign;
|
||||||
|
(R1, C1), (C1, C1) for R1, C1;
|
||||||
|
self: Matrix<N, R1, C1, SA>, right: TranslationBase<N, C1, SB>;
|
||||||
|
self.mul_assign(right.inverse().unwrap()); );
|
||||||
|
*/
|
115
src/geometry/unit_complex.rs
Normal file
115
src/geometry/unit_complex.rs
Normal file
@ -0,0 +1,115 @@
|
|||||||
|
use std::fmt;
|
||||||
|
use num_complex::Complex;
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use core::{Unit, SquareMatrix, Vector1};
|
||||||
|
use core::dimension::{U2, U3};
|
||||||
|
use core::allocator::{OwnedAllocator, Allocator};
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use geometry::{RotationBase, OwnedRotation};
|
||||||
|
|
||||||
|
/// A complex number with a norm equal to 1.
|
||||||
|
pub type UnitComplex<N> = Unit<Complex<N>>;
|
||||||
|
|
||||||
|
impl<N: Real> UnitComplex<N> {
|
||||||
|
/// The rotation angle in `]-pi; pi]` of this unit complex number.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle(&self) -> N {
|
||||||
|
self.complex().im.atan2(self.complex().re)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation angle returned as a 1-dimensional vector.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_axis(&self) -> Vector1<N> {
|
||||||
|
Vector1::new(self.angle())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The underlying complex number.
|
||||||
|
///
|
||||||
|
/// Same as `self.as_ref()`.
|
||||||
|
#[inline]
|
||||||
|
pub fn complex(&self) -> &Complex<N> {
|
||||||
|
self.as_ref()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute the conjugate of this unit complex number.
|
||||||
|
#[inline]
|
||||||
|
pub fn conjugate(&self) -> Self {
|
||||||
|
UnitComplex::new_unchecked(self.as_ref().conj())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts this complex number if it is not zero.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse(&self) -> Self {
|
||||||
|
self.conjugate()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The rotation angle needed to make `self` and `other` coincide.
|
||||||
|
#[inline]
|
||||||
|
pub fn angle_to(&self, other: &Self) -> N {
|
||||||
|
let delta = self.rotation_to(other);
|
||||||
|
delta.angle()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit complex number needed to make `self` and `other` coincide.
|
||||||
|
///
|
||||||
|
/// The result is such that: `self.rotation_to(other) * self == other`.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_to(&self, other: &Self) -> Self {
|
||||||
|
other / self
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Compute in-place the conjugate of this unit complex number.
|
||||||
|
#[inline]
|
||||||
|
pub fn conjugate_mut(&mut self) {
|
||||||
|
let me = self.as_mut_unchecked();
|
||||||
|
me.im = -me.im;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverts in-place this unit complex number.
|
||||||
|
#[inline]
|
||||||
|
pub fn inverse_mut(&mut self) {
|
||||||
|
self.conjugate_mut()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Raise this unit complex number to a given floating power.
|
||||||
|
///
|
||||||
|
/// This returns the unit complex number that identifies a rotation angle equal to
|
||||||
|
/// `self.angle() × n`.
|
||||||
|
#[inline]
|
||||||
|
pub fn powf(&self, n: N) -> Self {
|
||||||
|
Self::from_angle(self.angle() * n)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds the rotation matrix corresponding to this unit complex number.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_rotation_matrix<S>(&self) -> RotationBase<N, U2, S>
|
||||||
|
where S: OwnedStorage<N, U2, U2>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U2, S> {
|
||||||
|
let r = self.complex().re;
|
||||||
|
let i = self.complex().im;
|
||||||
|
|
||||||
|
RotationBase::from_matrix_unchecked(
|
||||||
|
SquareMatrix::<_, U2, _>::new(
|
||||||
|
r, -i,
|
||||||
|
i, r
|
||||||
|
)
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Converts this unit complex number into its equivalent homogeneous transformation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn to_homogeneous<S>(&self) -> SquareMatrix<N, U3, S>
|
||||||
|
where S: OwnedStorage<N, U3, U3>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U3, U3, S> +
|
||||||
|
Allocator<N, U2, U2> {
|
||||||
|
let r: OwnedRotation<N, U2, S::Alloc> = self.to_rotation_matrix();
|
||||||
|
r.to_homogeneous()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real + fmt::Display> fmt::Display for UnitComplex<N> {
|
||||||
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
|
write!(f, "UnitComplex angle: {}", self.angle())
|
||||||
|
}
|
||||||
|
}
|
188
src/geometry/unit_complex_alga.rs
Normal file
188
src/geometry/unit_complex_alga.rs
Normal file
@ -0,0 +1,188 @@
|
|||||||
|
use alga::general::{AbstractMagma, AbstractGroup, AbstractLoop, AbstractMonoid, AbstractQuasigroup,
|
||||||
|
AbstractSemigroup, Real, Inverse, Multiplicative, Identity, Id};
|
||||||
|
use alga::linear::{Transformation, AffineTransformation, Similarity, Isometry, DirectIsometry,
|
||||||
|
OrthogonalTransformation, Rotation, ProjectiveTransformation};
|
||||||
|
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::storage::OwnedStorage;
|
||||||
|
use core::allocator::OwnedAllocator;
|
||||||
|
use core::dimension::{U1, U2};
|
||||||
|
use geometry::{PointBase, UnitComplex};
|
||||||
|
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* Implementations for UnitComplex.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
impl<N: Real> Identity<Multiplicative> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn identity() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real> AbstractMagma<Multiplicative> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn operate(&self, rhs: &Self) -> Self {
|
||||||
|
self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real> Inverse<Multiplicative> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse(&self) -> Self {
|
||||||
|
self.inverse()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_mut(&mut self) {
|
||||||
|
self.inverse_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! impl_structures(
|
||||||
|
($($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||||
|
impl<N: Real> $marker<$operator> for UnitComplex<N> {
|
||||||
|
}
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
impl_structures!(
|
||||||
|
AbstractSemigroup<Multiplicative>,
|
||||||
|
AbstractQuasigroup<Multiplicative>,
|
||||||
|
AbstractMonoid<Multiplicative>,
|
||||||
|
AbstractLoop<Multiplicative>,
|
||||||
|
AbstractGroup<Multiplicative>
|
||||||
|
);
|
||||||
|
|
||||||
|
impl<N, S> Transformation<PointBase<N, U2, S>> for UnitComplex<N>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn transform_point(&self, pt: &PointBase<N, U2, S>) -> PointBase<N, U2, S> {
|
||||||
|
self * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn transform_vector(&self, v: &ColumnVector<N, U2, S>) -> ColumnVector<N, U2, S> {
|
||||||
|
self * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> ProjectiveTransformation<PointBase<N, U2, S>> for UnitComplex<N>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_point(&self, pt: &PointBase<N, U2, S>) -> PointBase<N, U2, S> {
|
||||||
|
// FIXME: would it be useful performancewise not to call inverse explicitly (i-e. implement
|
||||||
|
// the inverse transformation explicitly here) ?
|
||||||
|
self.inverse() * pt
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn inverse_transform_vector(&self, v: &ColumnVector<N, U2, S>) -> ColumnVector<N, U2, S> {
|
||||||
|
self.inverse() * v
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> AffineTransformation<PointBase<N, U2, S>> for UnitComplex<N>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> {
|
||||||
|
type Rotation = Self;
|
||||||
|
type NonUniformScaling = Id;
|
||||||
|
type Translation = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn decompose(&self) -> (Id, Self, Id, Self) {
|
||||||
|
(Id::new(), self.clone(), Id::new(), Self::identity())
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_translation(&self, _: &Self::Translation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_translation(&self, _: &Self::Translation) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
r * self
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
||||||
|
self * r
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N, S> Similarity<PointBase<N, U2, S>> for UnitComplex<N>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> {
|
||||||
|
type Scaling = Id;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn translation(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation(&self) -> Self {
|
||||||
|
self.clone()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaling(&self) -> Id {
|
||||||
|
Id::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! marker_impl(
|
||||||
|
($($Trait: ident),*) => {$(
|
||||||
|
impl<N, S> $Trait<PointBase<N, U2, S>> for UnitComplex<N>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> { }
|
||||||
|
)*}
|
||||||
|
);
|
||||||
|
|
||||||
|
marker_impl!(Isometry, DirectIsometry, OrthogonalTransformation);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
impl<N, S> Rotation<PointBase<N, U2, S>> for UnitComplex<N>
|
||||||
|
where N: Real,
|
||||||
|
S: OwnedStorage<N, U2, U1>,
|
||||||
|
S::Alloc: OwnedAllocator<N, U2, U1, S> {
|
||||||
|
#[inline]
|
||||||
|
fn powf(&self, n: N) -> Option<Self> {
|
||||||
|
Some(self.powf(n))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn rotation_between(a: &ColumnVector<N, U2, S>, b: &ColumnVector<N, U2, S>) -> Option<Self> {
|
||||||
|
Some(Self::rotation_between(a, b))
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn scaled_rotation_between(a: &ColumnVector<N, U2, S>, b: &ColumnVector<N, U2, S>, s: N) -> Option<Self> {
|
||||||
|
Some(Self::scaled_rotation_between(a, b, s))
|
||||||
|
}
|
||||||
|
}
|
107
src/geometry/unit_complex_construction.rs
Normal file
107
src/geometry/unit_complex_construction.rs
Normal file
@ -0,0 +1,107 @@
|
|||||||
|
#[cfg(feature = "arbitrary")]
|
||||||
|
use quickcheck::{Arbitrary, Gen};
|
||||||
|
|
||||||
|
use num::One;
|
||||||
|
use num_complex::Complex;
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use core::ColumnVector;
|
||||||
|
use core::dimension::{U1, U2};
|
||||||
|
use core::storage::Storage;
|
||||||
|
use geometry::{UnitComplex, RotationBase};
|
||||||
|
|
||||||
|
|
||||||
|
impl<N: Real> UnitComplex<N> {
|
||||||
|
/// The unit complex number multiplicative identity.
|
||||||
|
#[inline]
|
||||||
|
pub fn identity() -> Self {
|
||||||
|
Self::new_unchecked(Complex::new(N::one(), N::zero()))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds the unit complex number corresponding to the rotation with the angle.
|
||||||
|
#[inline]
|
||||||
|
pub fn new(angle: N) -> Self {
|
||||||
|
let (s, c) = angle.sin_cos();
|
||||||
|
UnitComplex::new_unchecked(Complex::new(c, s))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds the unit complex number corresponding to the rotation with the angle.
|
||||||
|
///
|
||||||
|
/// Same as `Self::new(angle)`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_angle(angle: N) -> Self {
|
||||||
|
Self::new(angle)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds a unit complex rotation from an angle in radian wrapped in a 1-dimensional vector.
|
||||||
|
///
|
||||||
|
/// Equivalent to `Self::new(axisangle[0])`.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_scaled_axis<SB: Storage<N, U1, U1>>(axisangle: ColumnVector<N, U1, SB>) -> Self {
|
||||||
|
Self::from_angle(axisangle[0])
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Creates a new unit complex number from a complex number.
|
||||||
|
///
|
||||||
|
/// The input complex number will be normalized.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_complex(q: Complex<N>) -> Self {
|
||||||
|
Self::new_unchecked(q / (q.im * q.im + q.re * q.re).sqrt())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Builds the unit complex number from the corresponding 2D rotation matrix.
|
||||||
|
#[inline]
|
||||||
|
pub fn from_rotation_matrix<S: Storage<N, U2, U2>>(rotmat: &RotationBase<N, U2, S>) -> Self {
|
||||||
|
Self::new_unchecked(Complex::new(rotmat[(0, 0)], rotmat[(1, 0)]))
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The unit complex needed to make `a` and `b` be collinear and point toward the same
|
||||||
|
/// direction.
|
||||||
|
#[inline]
|
||||||
|
pub fn rotation_between<SB, SC>(a: &ColumnVector<N, U2, SB>, b: &ColumnVector<N, U2, SC>) -> Self
|
||||||
|
where SB: Storage<N, U2, U1>,
|
||||||
|
SC: Storage<N, U2, U1> {
|
||||||
|
Self::scaled_rotation_between(a, b, N::one())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The smallest rotation needed to make `a` and `b` collinear and point toward the same
|
||||||
|
/// direction, raised to the power `s`.
|
||||||
|
#[inline]
|
||||||
|
pub fn scaled_rotation_between<SB, SC>(a: &ColumnVector<N, U2, SB>, b: &ColumnVector<N, U2, SC>, s: N) -> Self
|
||||||
|
where SB: Storage<N, U2, U1>,
|
||||||
|
SC: Storage<N, U2, U1> {
|
||||||
|
if let (Some(na), Some(nb)) = (a.try_normalize(N::zero()), b.try_normalize(N::zero())) {
|
||||||
|
let sang = na.perp(&nb);
|
||||||
|
let cang = na.dot(&nb);
|
||||||
|
|
||||||
|
Self::from_angle(sang.atan2(cang) * s)
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real> One for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn one() -> Self {
|
||||||
|
Self::identity()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<N: Real + Rand> Rand for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<R: Rng>(rng: &mut R) -> Self {
|
||||||
|
UnitComplex::from_angle(N::rand(rng))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature="arbitrary")]
|
||||||
|
impl<N: Real + Arbitrary> Arbitrary for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn arbitrary<G: Gen>(g: &mut G) -> Self {
|
||||||
|
UnitComplex::from_angle(N::arbitrary(g))
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
309
src/geometry/unit_complex_ops.rs
Normal file
309
src/geometry/unit_complex_ops.rs
Normal file
@ -0,0 +1,309 @@
|
|||||||
|
use std::ops::{Mul, MulAssign, Div, DivAssign};
|
||||||
|
|
||||||
|
use alga::general::Real;
|
||||||
|
use core::{Unit, ColumnVector, OwnedColumnVector};
|
||||||
|
use core::dimension::{U1, U2};
|
||||||
|
use core::storage::Storage;
|
||||||
|
use geometry::{UnitComplex, RotationBase, PointBase, OwnedPoint};
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This file provides:
|
||||||
|
* ===================
|
||||||
|
*
|
||||||
|
* UnitComplex × UnitComplex
|
||||||
|
* UnitComplex × RotationBase -> UnitComplex
|
||||||
|
* RotationBase × UnitComplex -> UnitComplex
|
||||||
|
*
|
||||||
|
* UnitComplex ÷ UnitComplex
|
||||||
|
* UnitComplex ÷ RotationBase -> UnitComplex
|
||||||
|
* RotationBase ÷ UnitComplex -> UnitComplex
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* UnitComplex × PointBase
|
||||||
|
* UnitComplex × ColumnVector
|
||||||
|
* UnitComplex × Unit<T>
|
||||||
|
*
|
||||||
|
* NOTE: -UnitComplex is already provided by `Unit<T>`.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* (Assignment Operators)
|
||||||
|
*
|
||||||
|
* UnitComplex ×= UnitComplex
|
||||||
|
* UnitComplex ×= RotationBase
|
||||||
|
*
|
||||||
|
* UnitComplex ÷= UnitComplex
|
||||||
|
* UnitComplex ÷= RotationBase
|
||||||
|
*
|
||||||
|
* FIXME: RotationBase ×= UnitComplex
|
||||||
|
* FIXME: RotationBase ÷= UnitComplex
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
// UnitComplex × UnitComplex
|
||||||
|
impl<N: Real> Mul<UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.unwrap() * rhs.unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N: Real> Mul<UnitComplex<N>> for &'a UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.complex() * rhs.unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N: Real> Mul<&'b UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: &'b UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.unwrap() * rhs.complex())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, 'b, N: Real> Mul<&'b UnitComplex<N>> for &'a UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: &'b UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.complex() * rhs.complex())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// UnitComplex ÷ UnitComplex
|
||||||
|
impl<N: Real> Div<UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn div(self, rhs: UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.unwrap() * rhs.conjugate().unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, N: Real> Div<UnitComplex<N>> for &'a UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn div(self, rhs: UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.complex() * rhs.conjugate().unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N: Real> Div<&'b UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn div(self, rhs: &'b UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.unwrap() * rhs.conjugate().unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, 'b, N: Real> Div<&'b UnitComplex<N>> for &'a UnitComplex<N> {
|
||||||
|
type Output = UnitComplex<N>;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn div(self, rhs: &'b UnitComplex<N>) -> UnitComplex<N> {
|
||||||
|
Unit::new_unchecked(self.complex() * rhs.conjugate().unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! complex_op_impl(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($RDim: ident, $CDim: ident);
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
|
||||||
|
$action: expr; $($lives: tt),*) => {
|
||||||
|
impl<$($lives ,)* N, S> $Op<$Rhs> for $Lhs
|
||||||
|
where N: Real,
|
||||||
|
S: Storage<N, $RDim, $CDim> {
|
||||||
|
type Output = $Result;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
|
||||||
|
$action
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
macro_rules! complex_op_impl_all(
|
||||||
|
($Op: ident, $op: ident;
|
||||||
|
($RDim: ident, $CDim: ident);
|
||||||
|
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty;
|
||||||
|
[val val] => $action_val_val: expr;
|
||||||
|
[ref val] => $action_ref_val: expr;
|
||||||
|
[val ref] => $action_val_ref: expr;
|
||||||
|
[ref ref] => $action_ref_ref: expr;) => {
|
||||||
|
|
||||||
|
complex_op_impl!($Op, $op;
|
||||||
|
($RDim, $CDim);
|
||||||
|
$lhs: $Lhs, $rhs: $Rhs, Output = $Result;
|
||||||
|
$action_val_val; );
|
||||||
|
|
||||||
|
complex_op_impl!($Op, $op;
|
||||||
|
($RDim, $CDim);
|
||||||
|
$lhs: &'a $Lhs, $rhs: $Rhs, Output = $Result;
|
||||||
|
$action_ref_val; 'a);
|
||||||
|
|
||||||
|
complex_op_impl!($Op, $op;
|
||||||
|
($RDim, $CDim);
|
||||||
|
$lhs: $Lhs, $rhs: &'b $Rhs, Output = $Result;
|
||||||
|
$action_val_ref; 'b);
|
||||||
|
|
||||||
|
complex_op_impl!($Op, $op;
|
||||||
|
($RDim, $CDim);
|
||||||
|
$lhs: &'a $Lhs, $rhs: &'b $Rhs, Output = $Result;
|
||||||
|
$action_ref_ref; 'a, 'b);
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// UnitComplex × RotationBase
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U2, U2);
|
||||||
|
self: UnitComplex<N>, rhs: RotationBase<N, U2, S>, Output = UnitComplex<N>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => self * UnitComplex::from_rotation_matrix(rhs);
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitComplex ÷ RotationBase
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(U2, U2);
|
||||||
|
self: UnitComplex<N>, rhs: RotationBase<N, U2, S>, Output = UnitComplex<N>;
|
||||||
|
[val val] => &self / &rhs;
|
||||||
|
[ref val] => self / &rhs;
|
||||||
|
[val ref] => &self / rhs;
|
||||||
|
[ref ref] => self * UnitComplex::from_rotation_matrix(rhs).inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
|
||||||
|
// RotationBase × UnitComplex
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U2, U2);
|
||||||
|
self: RotationBase<N, U2, S>, rhs: UnitComplex<N>, Output = UnitComplex<N>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => UnitComplex::from_rotation_matrix(self) * rhs;
|
||||||
|
);
|
||||||
|
|
||||||
|
// RotationBase ÷ UnitComplex
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Div, div;
|
||||||
|
(U2, U2);
|
||||||
|
self: RotationBase<N, U2, S>, rhs: UnitComplex<N>, Output = UnitComplex<N>;
|
||||||
|
[val val] => &self / &rhs;
|
||||||
|
[ref val] => self / &rhs;
|
||||||
|
[val ref] => &self / rhs;
|
||||||
|
[ref ref] => UnitComplex::from_rotation_matrix(self) * rhs.inverse();
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitComplex × PointBase
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U2, U1);
|
||||||
|
self: UnitComplex<N>, rhs: PointBase<N, U2, S>, Output = OwnedPoint<N, U2, S::Alloc>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => PointBase::from_coordinates(self * &rhs.coords);
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitComplex × ColumnVector
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U2, U1);
|
||||||
|
self: UnitComplex<N>, rhs: ColumnVector<N, U2, S>, Output = OwnedColumnVector<N, U2, S::Alloc>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => {
|
||||||
|
let i = self.as_ref().im;
|
||||||
|
let r = self.as_ref().re;
|
||||||
|
OwnedColumnVector::<_, U2, S::Alloc>::new(r * rhs[0] - i * rhs[0], i * rhs[1] + r * rhs[1])
|
||||||
|
};
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitComplex × Unit<Vector>
|
||||||
|
complex_op_impl_all!(
|
||||||
|
Mul, mul;
|
||||||
|
(U2, U1);
|
||||||
|
self: UnitComplex<N>, rhs: Unit<ColumnVector<N, U2, S>>, Output = Unit<OwnedColumnVector<N, U2, S::Alloc>>;
|
||||||
|
[val val] => &self * &rhs;
|
||||||
|
[ref val] => self * &rhs;
|
||||||
|
[val ref] => &self * rhs;
|
||||||
|
[ref ref] => Unit::new_unchecked(self * rhs.as_ref());
|
||||||
|
);
|
||||||
|
|
||||||
|
// UnitComplex ×= UnitComplex
|
||||||
|
impl<N: Real> MulAssign<UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, rhs: UnitComplex<N>) {
|
||||||
|
*self = &*self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N: Real> MulAssign<&'b UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, rhs: &'b UnitComplex<N>) {
|
||||||
|
*self = &*self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// UnitComplex /= UnitComplex
|
||||||
|
impl<N: Real> DivAssign<UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn div_assign(&mut self, rhs: UnitComplex<N>) {
|
||||||
|
*self = &*self / rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N: Real> DivAssign<&'b UnitComplex<N>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn div_assign(&mut self, rhs: &'b UnitComplex<N>) {
|
||||||
|
*self = &*self / rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// UnitComplex ×= RotationBase
|
||||||
|
impl<N: Real, S: Storage<N, U2, U2>> MulAssign<RotationBase<N, U2, S>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, rhs: RotationBase<N, U2, S>) {
|
||||||
|
*self = &*self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N: Real, S: Storage<N, U2, U2>> MulAssign<&'b RotationBase<N, U2, S>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, rhs: &'b RotationBase<N, U2, S>) {
|
||||||
|
*self = &*self * rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// UnitComplex ÷= RotationBase
|
||||||
|
impl<N: Real, S: Storage<N, U2, U2>> DivAssign<RotationBase<N, U2, S>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn div_assign(&mut self, rhs: RotationBase<N, U2, S>) {
|
||||||
|
*self = &*self / rhs
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'b, N: Real, S: Storage<N, U2, U2>> DivAssign<&'b RotationBase<N, U2, S>> for UnitComplex<N> {
|
||||||
|
#[inline]
|
||||||
|
fn div_assign(&mut self, rhs: &'b RotationBase<N, U2, S>) {
|
||||||
|
*self = &*self / rhs
|
||||||
|
}
|
||||||
|
}
|
1098
src/lib.rs
1098
src/lib.rs
File diff suppressed because it is too large
Load Diff
@ -1,351 +0,0 @@
|
|||||||
use traits::operations::{Transpose, ApproxEq};
|
|
||||||
use traits::structure::{ColumnSlice, Eye, Indexable, SquareMatrix, BaseFloat, Cast};
|
|
||||||
use traits::geometry::Norm;
|
|
||||||
use std::cmp;
|
|
||||||
use std::ops::{Mul, Add, Sub};
|
|
||||||
|
|
||||||
/// Get the householder matrix corresponding to a reflexion to the hyperplane
|
|
||||||
/// defined by `vector`. It can be a reflexion contained in a subspace.
|
|
||||||
///
|
|
||||||
/// # Arguments
|
|
||||||
/// * `dimension` - the dimension of the space the resulting matrix operates in
|
|
||||||
/// * `start` - the starting dimension of the subspace of the reflexion
|
|
||||||
/// * `vector` - the vector defining the reflection.
|
|
||||||
pub fn householder_matrix<N, V, M>(dimension: usize, start: usize, vector: V) -> M
|
|
||||||
where N: BaseFloat,
|
|
||||||
M: Eye + Indexable<(usize, usize), N>,
|
|
||||||
V: Indexable<usize, N> {
|
|
||||||
let mut qk : M = Eye::new_identity(dimension);
|
|
||||||
let subdim = vector.shape();
|
|
||||||
|
|
||||||
let stop = subdim + start;
|
|
||||||
|
|
||||||
assert!(dimension >= stop);
|
|
||||||
|
|
||||||
for j in start .. stop {
|
|
||||||
for i in start .. stop {
|
|
||||||
unsafe {
|
|
||||||
let vv = vector.unsafe_at(i - start) * vector.unsafe_at(j - start);
|
|
||||||
let qkij = qk.unsafe_at((i, j));
|
|
||||||
qk.unsafe_set((i, j), qkij - vv - vv);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
qk
|
|
||||||
}
|
|
||||||
|
|
||||||
/// QR decomposition using Householder reflections.
|
|
||||||
///
|
|
||||||
/// # Arguments
|
|
||||||
/// * `m` - matrix to decompose
|
|
||||||
pub fn qr<N, V, M>(m: &M) -> (M, M)
|
|
||||||
where N: BaseFloat,
|
|
||||||
V: Indexable<usize, N> + Norm<NormType = N>,
|
|
||||||
M: Copy + Eye + ColumnSlice<V> + Transpose + Indexable<(usize, usize), N> +
|
|
||||||
Mul<M, Output = M> {
|
|
||||||
let (rows, cols) = m.shape();
|
|
||||||
assert!(rows >= cols);
|
|
||||||
let mut q : M = Eye::new_identity(rows);
|
|
||||||
let mut r = *m;
|
|
||||||
|
|
||||||
for ite in 0 .. cmp::min(rows - 1, cols) {
|
|
||||||
let mut v = r.column_slice(ite, ite, rows);
|
|
||||||
let alpha =
|
|
||||||
if unsafe { v.unsafe_at(ite) } >= ::zero() {
|
|
||||||
-Norm::norm(&v)
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
Norm::norm(&v)
|
|
||||||
};
|
|
||||||
unsafe {
|
|
||||||
let x = v.unsafe_at(0);
|
|
||||||
v.unsafe_set(0, x - alpha);
|
|
||||||
}
|
|
||||||
if !::is_zero(&v.normalize_mut()) {
|
|
||||||
let qk: M = householder_matrix(rows, ite, v);
|
|
||||||
r = qk * r;
|
|
||||||
q = q * Transpose::transpose(&qk);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
(q, r)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Eigendecomposition of a square symmetric matrix using the qr algorithm
|
|
||||||
pub fn eigen_qr<N, V, VS, M>(m: &M, eps: &N, niter: usize) -> (M, V)
|
|
||||||
where N: BaseFloat,
|
|
||||||
V: Mul<M, Output = V>,
|
|
||||||
VS: Indexable<usize, N> + Norm<NormType = N>,
|
|
||||||
M: Indexable<(usize, usize), N> + SquareMatrix<N, V> + Add<M, Output = M> +
|
|
||||||
Sub<M, Output = M> + ColumnSlice<VS> +
|
|
||||||
ApproxEq<N> + Copy {
|
|
||||||
|
|
||||||
let (mut eigenvectors, mut eigenvalues) = hessenberg(m);
|
|
||||||
|
|
||||||
// Allocate arrays for Givens rotation components
|
|
||||||
let mut c = Vec::<N>::with_capacity(::dimension::<M>() - 1);
|
|
||||||
let mut s = Vec::<N>::with_capacity(::dimension::<M>() - 1);
|
|
||||||
|
|
||||||
if ::dimension::<M>() == 1 {
|
|
||||||
return (eigenvectors, eigenvalues.diagonal());
|
|
||||||
}
|
|
||||||
|
|
||||||
unsafe {
|
|
||||||
c.set_len(::dimension::<M>() - 1);
|
|
||||||
s.set_len(::dimension::<M>() - 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
let mut iter = 0;
|
|
||||||
let mut curdim = ::dimension::<M>() - 1;
|
|
||||||
|
|
||||||
for _ in 0 .. ::dimension::<M>() {
|
|
||||||
|
|
||||||
let mut stop = false;
|
|
||||||
|
|
||||||
while !stop && iter < niter {
|
|
||||||
|
|
||||||
let lambda;
|
|
||||||
|
|
||||||
unsafe {
|
|
||||||
let a = eigenvalues.unsafe_at((curdim - 1, curdim - 1));
|
|
||||||
let b = eigenvalues.unsafe_at((curdim - 1, curdim));
|
|
||||||
let c = eigenvalues.unsafe_at((curdim, curdim - 1));
|
|
||||||
let d = eigenvalues.unsafe_at((curdim, curdim));
|
|
||||||
|
|
||||||
let trace = a + d;
|
|
||||||
let determinant = a * d - b * c;
|
|
||||||
|
|
||||||
let constquarter: N = Cast::from(0.25f64);
|
|
||||||
let consthalf: N = Cast::from(0.5f64);
|
|
||||||
|
|
||||||
let e = (constquarter * trace * trace - determinant).sqrt();
|
|
||||||
|
|
||||||
let lambda1 = consthalf * trace + e;
|
|
||||||
let lambda2 = consthalf * trace - e;
|
|
||||||
|
|
||||||
if (lambda1 - d).abs() < (lambda2 - d).abs() {
|
|
||||||
lambda = lambda1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
lambda = lambda2;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Shift matrix
|
|
||||||
for k in 0 .. curdim + 1 {
|
|
||||||
unsafe {
|
|
||||||
let a = eigenvalues.unsafe_at((k, k));
|
|
||||||
eigenvalues.unsafe_set((k, k), a - lambda);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Givens rotation from left
|
|
||||||
for k in 0 .. curdim {
|
|
||||||
let x_i = unsafe { eigenvalues.unsafe_at((k, k)) };
|
|
||||||
let x_j = unsafe { eigenvalues.unsafe_at((k + 1, k)) };
|
|
||||||
|
|
||||||
let ctmp;
|
|
||||||
let stmp;
|
|
||||||
|
|
||||||
if x_j.abs() < *eps {
|
|
||||||
ctmp = N::one();
|
|
||||||
stmp = N::zero();
|
|
||||||
}
|
|
||||||
else if x_i.abs() < *eps {
|
|
||||||
ctmp = N::zero();
|
|
||||||
stmp = -N::one();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
let r = x_i.hypot(x_j);
|
|
||||||
ctmp = x_i / r;
|
|
||||||
stmp = -x_j / r;
|
|
||||||
}
|
|
||||||
|
|
||||||
c[k] = ctmp;
|
|
||||||
s[k] = stmp;
|
|
||||||
|
|
||||||
for j in k .. (curdim + 1) {
|
|
||||||
|
|
||||||
unsafe {
|
|
||||||
let a = eigenvalues.unsafe_at((k, j));
|
|
||||||
let b = eigenvalues.unsafe_at((k + 1, j));
|
|
||||||
|
|
||||||
eigenvalues.unsafe_set((k, j), ctmp * a - stmp * b);
|
|
||||||
eigenvalues.unsafe_set((k + 1, j), stmp * a + ctmp * b);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Givens rotation from right applied to eigenvalues
|
|
||||||
for k in 0 .. curdim {
|
|
||||||
for i in 0 .. (k + 2) {
|
|
||||||
|
|
||||||
unsafe {
|
|
||||||
let a = eigenvalues.unsafe_at((i, k));
|
|
||||||
let b = eigenvalues.unsafe_at((i, k + 1));
|
|
||||||
|
|
||||||
|
|
||||||
eigenvalues.unsafe_set((i, k), c[k] * a - s[k] * b);
|
|
||||||
eigenvalues.unsafe_set((i, k + 1), s[k] * a + c[k] * b);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Shift back
|
|
||||||
for k in 0 .. curdim + 1 {
|
|
||||||
unsafe {
|
|
||||||
let a = eigenvalues.unsafe_at((k, k));
|
|
||||||
eigenvalues.unsafe_set((k, k), a + lambda);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Givens rotation from right applied to eigenvectors
|
|
||||||
for k in 0 .. curdim {
|
|
||||||
for i in 0 .. ::dimension::<M>() {
|
|
||||||
|
|
||||||
unsafe {
|
|
||||||
let a = eigenvectors.unsafe_at((i, k));
|
|
||||||
let b = eigenvectors.unsafe_at((i, k + 1));
|
|
||||||
|
|
||||||
|
|
||||||
eigenvectors.unsafe_set((i, k), c[k] * a - s[k] * b);
|
|
||||||
eigenvectors.unsafe_set((i, k + 1), s[k] * a + c[k] * b);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
iter = iter + 1;
|
|
||||||
|
|
||||||
stop = true;
|
|
||||||
|
|
||||||
for j in 0 .. curdim {
|
|
||||||
|
|
||||||
// Check last row
|
|
||||||
if unsafe { eigenvalues.unsafe_at((curdim, j)) }.abs() >= *eps {
|
|
||||||
stop = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check last column
|
|
||||||
if unsafe { eigenvalues.unsafe_at((j, curdim)) }.abs() >= *eps {
|
|
||||||
stop = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
if stop {
|
|
||||||
|
|
||||||
if curdim > 1 {
|
|
||||||
curdim = curdim - 1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
(eigenvectors, eigenvalues.diagonal())
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Cholesky decomposition G of a square symmetric positive definite matrix A, such that A = G * G^T
|
|
||||||
///
|
|
||||||
/// # Arguments
|
|
||||||
/// * `m` - square symmetric positive definite matrix to decompose
|
|
||||||
pub fn cholesky<N, V, VS, M>(m: &M) -> Result<M, &'static str>
|
|
||||||
where N: BaseFloat,
|
|
||||||
V: Mul<M, Output = V>,
|
|
||||||
VS: Indexable<usize, N> + Norm<NormType = N>,
|
|
||||||
M: Indexable<(usize, usize), N> + SquareMatrix<N, V> + Add<M, Output = M> +
|
|
||||||
Sub<M, Output = M> + ColumnSlice<VS> +
|
|
||||||
ApproxEq<N> + Copy {
|
|
||||||
|
|
||||||
let mut out = m.transpose();
|
|
||||||
|
|
||||||
if !ApproxEq::approx_eq(&out, &m) {
|
|
||||||
return Err("Cholesky: Input matrix is not symmetric");
|
|
||||||
}
|
|
||||||
|
|
||||||
for i in 0 .. out.nrows() {
|
|
||||||
for j in 0 .. (i + 1) {
|
|
||||||
|
|
||||||
let mut sum: N = out[(i, j)];
|
|
||||||
|
|
||||||
for k in 0 .. j {
|
|
||||||
sum = sum - out[(i, k)] * out[(j, k)];
|
|
||||||
}
|
|
||||||
|
|
||||||
if i > j {
|
|
||||||
out[(i, j)] = sum / out[(j, j)];
|
|
||||||
}
|
|
||||||
else if sum > N::zero() {
|
|
||||||
out[(i, i)] = sum.sqrt();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return Err("Cholesky: Input matrix is not positive definite to machine precision");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
for i in 0 .. out.nrows() {
|
|
||||||
for j in i + 1 .. out.ncols() {
|
|
||||||
out[(i, j)] = N::zero();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Ok(out)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Hessenberg
|
|
||||||
/// Returns the matrix m in Hessenberg form and the corresponding similarity transformation
|
|
||||||
///
|
|
||||||
/// # Arguments
|
|
||||||
/// * `m` - matrix to transform
|
|
||||||
///
|
|
||||||
/// # Returns
|
|
||||||
/// * First return value `q` - Similarity matrix p such that q * h * q^T = m
|
|
||||||
/// * Second return value `h` - Matrix m in Hessenberg form
|
|
||||||
pub fn hessenberg<N, V, M>(m: &M) -> (M, M)
|
|
||||||
where N: BaseFloat,
|
|
||||||
V: Indexable<usize, N> + Norm<NormType = N>,
|
|
||||||
M: Copy + Eye + ColumnSlice<V> + Transpose + Indexable<(usize, usize), N> +
|
|
||||||
Mul<M, Output = M> {
|
|
||||||
|
|
||||||
let mut h = *m;
|
|
||||||
let (rows, cols) = h.shape();
|
|
||||||
|
|
||||||
let mut q : M = Eye::new_identity(cols);
|
|
||||||
|
|
||||||
if cols <= 2 {
|
|
||||||
return (q, h);
|
|
||||||
}
|
|
||||||
|
|
||||||
for ite in 0 .. (cols - 2) {
|
|
||||||
let mut v = h.column_slice(ite, ite + 1, rows);
|
|
||||||
|
|
||||||
let alpha = Norm::norm(&v);
|
|
||||||
|
|
||||||
unsafe {
|
|
||||||
let x = v.unsafe_at(0);
|
|
||||||
v.unsafe_set(0, x - alpha);
|
|
||||||
}
|
|
||||||
|
|
||||||
if !::is_zero(&v.normalize_mut()) {
|
|
||||||
let p: M = householder_matrix(rows, ite + 1, v);
|
|
||||||
|
|
||||||
q = q * p;
|
|
||||||
h = p * h * p;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
(q, h)
|
|
||||||
}
|
|
@ -1,4 +0,0 @@
|
|||||||
|
|
||||||
pub use self::decompositions::{qr, eigen_qr, householder_matrix, cholesky, hessenberg};
|
|
||||||
|
|
||||||
mod decompositions;
|
|
@ -1,29 +0,0 @@
|
|||||||
/// Asserts approximate equality within a given tolerance of two values with the
|
|
||||||
/// `ApproxEq` trait.
|
|
||||||
#[macro_export]
|
|
||||||
macro_rules! assert_approx_eq_eps(
|
|
||||||
($given: expr, $expected: expr, $eps: expr) => ({
|
|
||||||
let eps = &($eps);
|
|
||||||
let (given_val, expected_val) = (&($given), &($expected));
|
|
||||||
if !ApproxEq::approx_eq_eps(given_val, expected_val, eps) {
|
|
||||||
panic!("assertion failed: `left ≈ right` (left: `{:?}`, right: `{:?}`, tolerance: `{:?}`)",
|
|
||||||
*given_val, *expected_val, *eps
|
|
||||||
)
|
|
||||||
}
|
|
||||||
})
|
|
||||||
);
|
|
||||||
|
|
||||||
/// Asserts approximate equality within a given tolerance of two values with the
|
|
||||||
/// `ApproxEq` trait, with tolerance specified in ULPs.
|
|
||||||
#[macro_export]
|
|
||||||
macro_rules! assert_approx_eq_ulps(
|
|
||||||
($given: expr, $expected: expr, $ulps: expr) => ({
|
|
||||||
let ulps = $ulps;
|
|
||||||
let (given_val, expected_val) = (&($given), &($expected));
|
|
||||||
if !ApproxEq::approx_eq_ulps(given_val, expected_val, ulps) {
|
|
||||||
panic!("assertion failed: `left ≈ right` (left: `{:?}`, right: `{:?}`, tolerance: `{:?}`)",
|
|
||||||
*given_val, *expected_val, ulps
|
|
||||||
)
|
|
||||||
}
|
|
||||||
})
|
|
||||||
);
|
|
@ -1 +0,0 @@
|
|||||||
mod assert;
|
|
@ -1,17 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
macro_rules! vector_space_impl(
|
|
||||||
($t: ident, $dimension: expr, $($compN: ident),+) => { }
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! special_orthogonal_group_impl(
|
|
||||||
($t: ident, $point: ident, $vector: ident) => { }
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! euclidean_space_impl(
|
|
||||||
($t: ident, $vector: ident) => { }
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! matrix_group_approx_impl(
|
|
||||||
($t: ident, $($compN: ident),+) => { }
|
|
||||||
);
|
|
@ -1,25 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
macro_rules! use_matrix_group_modules(
|
|
||||||
() => {
|
|
||||||
use algebra::cmp::ApproxEq as AlgebraApproxEq;
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! matrix_group_approx_impl(
|
|
||||||
($t: ident, $($compN: ident),+) => {
|
|
||||||
impl<N: AlgebraApproxEq> AlgebraApproxEq for $t<N> {
|
|
||||||
type Eps = N::Eps;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn default_epsilon() -> N::Eps {
|
|
||||||
N::default_epsilon()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn approx_eq_eps(&self, other: &$t<N>, epsilon: &N::Eps) -> bool {
|
|
||||||
$(AlgebraApproxEq::approx_eq_eps(&self.$compN, &other.$compN, &epsilon))&&+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
@ -1,13 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
#[cfg(not(feature="abstract_algebra"))]
|
|
||||||
mod dummy;
|
|
||||||
|
|
||||||
#[cfg(feature="abstract_algebra")]
|
|
||||||
mod vector;
|
|
||||||
#[cfg(feature="abstract_algebra")]
|
|
||||||
mod rotation;
|
|
||||||
#[cfg(feature="abstract_algebra")]
|
|
||||||
mod point;
|
|
||||||
#[cfg(feature="abstract_algebra")]
|
|
||||||
mod matrix;
|
|
@ -1,34 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
macro_rules! use_euclidean_space_modules(
|
|
||||||
() => {
|
|
||||||
use algebra::structure::{AffineSpaceApprox, EuclideanSpaceApprox,
|
|
||||||
FieldApprox, RealApprox};
|
|
||||||
use algebra::cmp::ApproxEq as AlgebraApproxEq;
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! euclidean_space_impl(
|
|
||||||
($t: ident, $vector: ident) => {
|
|
||||||
impl<N> AffineSpaceApprox<N> for $t<N>
|
|
||||||
where N: Copy + Neg<Output = N> + Add<N, Output = N> +
|
|
||||||
Sub<N, Output = N> + AlgebraApproxEq + FieldApprox {
|
|
||||||
type Translation = $vector<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn translate_by(&self, vector: &Self::Translation) -> Self {
|
|
||||||
*self + *vector
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn subtract(&self, other: &Self) -> Self::Translation {
|
|
||||||
*self - *other
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: RealApprox> EuclideanSpaceApprox<N> for $t<N> {
|
|
||||||
type Vector = $vector<N>;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
@ -1,92 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
macro_rules! use_special_orthogonal_group_modules(
|
|
||||||
() => {
|
|
||||||
use algebra::structure::{EuclideanGroupApprox, SpecialEuclideanGroupApprox,
|
|
||||||
OrthogonalGroupApprox, SpecialOrthogonalGroupApprox,
|
|
||||||
GroupApprox, LoopApprox, MonoidApprox,
|
|
||||||
QuasigroupApprox, SemigroupApprox,
|
|
||||||
RealApprox};
|
|
||||||
use algebra::cmp::ApproxEq as AlgebraApproxEq;
|
|
||||||
use algebra::ident::Identity;
|
|
||||||
use algebra::ops::{Recip, Multiplicative};
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! special_orthogonal_group_impl(
|
|
||||||
($t: ident, $point: ident, $vector: ident) => {
|
|
||||||
/*
|
|
||||||
* Operations.
|
|
||||||
*/
|
|
||||||
impl<N: BaseNum> Identity<Multiplicative> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn id() -> Self {
|
|
||||||
::one()
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: Copy + AlgebraApproxEq<Eps = N>> AlgebraApproxEq for $t<N> {
|
|
||||||
type Eps = N;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn default_epsilon() -> N {
|
|
||||||
<N as AlgebraApproxEq>::default_epsilon()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn approx_eq_eps(&self, other: &$t<N>, epsilon: &N) -> bool {
|
|
||||||
AlgebraApproxEq::approx_eq_eps(&self.submatrix, &other.submatrix, &epsilon)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: Copy> Recip for $t<N> {
|
|
||||||
type Result = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn recip(mut self) -> $t<N> {
|
|
||||||
self.inverse_mut();
|
|
||||||
|
|
||||||
self
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
*
|
|
||||||
* Algebraic structures.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
// FIXME: in the end, we will keep only RealApprox.
|
|
||||||
impl<N: BaseNum + RealApprox> GroupApprox<Multiplicative> for $t<N> { }
|
|
||||||
impl<N: BaseNum + RealApprox> LoopApprox<Multiplicative> for $t<N> { }
|
|
||||||
impl<N: BaseNum + RealApprox> MonoidApprox<Multiplicative> for $t<N> { }
|
|
||||||
impl<N: BaseNum + RealApprox> QuasigroupApprox<Multiplicative> for $t<N> { }
|
|
||||||
impl<N: BaseNum + RealApprox> SemigroupApprox<Multiplicative> for $t<N> { }
|
|
||||||
|
|
||||||
/*
|
|
||||||
*
|
|
||||||
* Matrix groups.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
impl<N: BaseNum + RealApprox> EuclideanGroupApprox<N, $point<N>> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn transform_point(&self, pt: &$point<N>) -> $point<N> {
|
|
||||||
*self * *pt
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn transform_vector(&self, v: &$vector<N>) -> $vector<N> {
|
|
||||||
*self * *v
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: BaseNum + RealApprox> SpecialEuclideanGroupApprox<N, $point<N>> for $t<N> {
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: BaseNum + RealApprox> OrthogonalGroupApprox<N, $point<N>> for $t<N> {
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: BaseNum + RealApprox> SpecialOrthogonalGroupApprox<N, $point<N>> for $t<N> {
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
@ -1,186 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
macro_rules! use_vector_space_modules(
|
|
||||||
() => {
|
|
||||||
use algebra::structure::{FieldApprox, RingCommutativeApprox, GroupAbelianApprox,
|
|
||||||
GroupApprox, LoopApprox, MonoidApprox, QuasigroupApprox,
|
|
||||||
SemigroupApprox, VectorSpaceApprox, ModuleApprox,
|
|
||||||
NormedSpaceApprox, InnerSpaceApprox,
|
|
||||||
FiniteDimVectorSpaceApprox,
|
|
||||||
Field, RingCommutative, GroupAbelian,
|
|
||||||
Group, Loop, Monoid, Quasigroup,
|
|
||||||
Semigroup, VectorSpace, Module, RealApprox};
|
|
||||||
use algebra::cmp::ApproxEq as AlgebraApproxEq;
|
|
||||||
use algebra::ident::Identity;
|
|
||||||
use algebra::ops::Additive;
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! vector_space_impl(
|
|
||||||
($t: ident, $dimension: expr, $($compN: ident),+) => {
|
|
||||||
/*
|
|
||||||
* Identity & ApproxEq
|
|
||||||
*/
|
|
||||||
impl<N: Copy + Identity<Additive>> Identity<Additive> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn id() -> Self {
|
|
||||||
Repeat::repeat(Identity::id())
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: AlgebraApproxEq> AlgebraApproxEq for $t<N> {
|
|
||||||
type Eps = N::Eps;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn default_epsilon() -> N::Eps {
|
|
||||||
N::default_epsilon()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn approx_eq_eps(&self, other: &$t<N>, epsilon: &N::Eps) -> bool {
|
|
||||||
$(AlgebraApproxEq::approx_eq_eps(&self.$compN, &other.$compN, &epsilon))&&+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
*
|
|
||||||
* Approximate algebraic structures.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
product_space_inherit_structure!($t, GroupAbelianApprox<Additive>);
|
|
||||||
product_space_inherit_structure!($t, GroupApprox<Additive>);
|
|
||||||
product_space_inherit_structure!($t, LoopApprox<Additive>);
|
|
||||||
product_space_inherit_structure!($t, MonoidApprox<Additive>);
|
|
||||||
product_space_inherit_structure!($t, QuasigroupApprox<Additive>);
|
|
||||||
product_space_inherit_structure!($t, SemigroupApprox<Additive>);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Module.
|
|
||||||
*/
|
|
||||||
impl<N> ModuleApprox<N> for $t<N> where N: Copy + Neg<Output = N> + Add<N, Output = N> +
|
|
||||||
AlgebraApproxEq + RingCommutativeApprox
|
|
||||||
{ }
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Vector spaces.
|
|
||||||
*/
|
|
||||||
impl<N> VectorSpaceApprox<N> for $t<N>
|
|
||||||
where N: Copy + Neg<Output = N> + Add<N, Output = N> +
|
|
||||||
AlgebraApproxEq + FieldApprox { }
|
|
||||||
|
|
||||||
impl<N> FiniteDimVectorSpaceApprox<N> for $t<N>
|
|
||||||
where N: Copy + Zero + One + Neg<Output = N> + Add<N, Output = N> +
|
|
||||||
AlgebraApproxEq + FieldApprox {
|
|
||||||
#[inline]
|
|
||||||
fn dimension() -> usize {
|
|
||||||
$dimension
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn canonical_basis<F: FnOnce(&[$t<N>])>(f: F) {
|
|
||||||
let basis = [
|
|
||||||
$($t::$compN()),*
|
|
||||||
];
|
|
||||||
|
|
||||||
f(&basis[..])
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn component(&self, i: usize) -> N {
|
|
||||||
self[i]
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
unsafe fn component_unchecked(&self, i: usize) -> N {
|
|
||||||
self.at_fast(i)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: RealApprox> NormedSpaceApprox<N> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn norm_squared(&self) -> N {
|
|
||||||
self.inner_product(self)
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn norm(&self) -> N {
|
|
||||||
self.norm_squared().sqrt()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn normalize(&self) -> Self {
|
|
||||||
*self / self.norm()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn normalize_mut(&mut self) -> N {
|
|
||||||
let n = self.norm();
|
|
||||||
*self /= n;
|
|
||||||
|
|
||||||
n
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn try_normalize(&self, min_norm: &N) -> Option<Self> {
|
|
||||||
let n = self.norm();
|
|
||||||
|
|
||||||
if n <= *min_norm {
|
|
||||||
None
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
Some(*self / n)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn try_normalize_mut(&mut self, min_norm: &N) -> Option<N> {
|
|
||||||
let n = self.norm();
|
|
||||||
|
|
||||||
if n <= *min_norm {
|
|
||||||
None
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
*self /= n;
|
|
||||||
Some(n)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: RealApprox> InnerSpaceApprox<N> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn inner_product(&self, other: &Self) -> N {
|
|
||||||
fold_add!($(self.$compN * other.$compN ),+)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
*
|
|
||||||
* Exact algebraic structures.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
product_space_inherit_structure!($t, GroupAbelian<Additive>);
|
|
||||||
product_space_inherit_structure!($t, Group<Additive>);
|
|
||||||
product_space_inherit_structure!($t, Loop<Additive>);
|
|
||||||
product_space_inherit_structure!($t, Monoid<Additive>);
|
|
||||||
product_space_inherit_structure!($t, Quasigroup<Additive>);
|
|
||||||
product_space_inherit_structure!($t, Semigroup<Additive>);
|
|
||||||
|
|
||||||
impl<N> VectorSpace<N> for $t<N>
|
|
||||||
where N: Copy + Neg<Output = N> + Add<N, Output = N> + AlgebraApproxEq + Field
|
|
||||||
{ }
|
|
||||||
|
|
||||||
impl<N> Module<N> for $t<N>
|
|
||||||
where N: Copy + Neg<Output = N> + Add<N, Output = N> + AlgebraApproxEq + RingCommutative
|
|
||||||
{ }
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! product_space_inherit_structure(
|
|
||||||
($t: ident, $marker: ident<$operator: ident>) => {
|
|
||||||
impl<N> $marker<$operator> for $t<N>
|
|
||||||
where N: Copy + Neg<Output = N> + Add<N, Output = N> + AlgebraApproxEq +
|
|
||||||
$marker<$operator>
|
|
||||||
{ }
|
|
||||||
}
|
|
||||||
);
|
|
@ -1,478 +0,0 @@
|
|||||||
#![macro_use]
|
|
||||||
|
|
||||||
macro_rules! ref_binop(
|
|
||||||
// Symmetric to `std::ops::forward_ref_binop!`
|
|
||||||
(impl $imp:ident, $method:ident for $t:ident; $($compN:ident),+) => {
|
|
||||||
impl<'a, 'b, N> $imp<&'a $t<N>> for &'b $t<N> where &'b N: $imp<&'a N, Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: &'a $t<N>) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(&self.$compN, &right.$compN)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, N> $imp<&'a $t<N>> for $t<N> where N: $imp<&'a N, Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: &'a $t<N>) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(self.$compN, &right.$compN)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, N> $imp<$t<N>> for &'a $t<N> where &'a N: $imp<N, Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: $t<N>) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(&self.$compN, right.$compN)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! pointwise_mul(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Mul<N, Output = N>> Mul<$t<N>> for $t<N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn mul(self, right: $t<N>) -> $t<N> {
|
|
||||||
$t::new($(self.$compN * right.$compN),+)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: MulAssign<N>> MulAssign<$t<N>> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn mul_assign(&mut self, right: $t<N>) {
|
|
||||||
$( self.$compN *= right.$compN; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop!(impl Mul, mul for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! pointwise_div(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Div<N, Output = N>> Div<$t<N>> for $t<N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn div(self, right: $t<N>) -> $t<N> {
|
|
||||||
$t::new($(self.$compN / right.$compN),+)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: DivAssign<N>> DivAssign<$t<N>> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn div_assign(&mut self, right: $t<N>) {
|
|
||||||
$( self.$compN /= right.$compN; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop!(impl Div, div for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! pointwise_add(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Add<N, Output = N>> Add<$t<N>> for $t<N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn add(self, right: $t<N>) -> $t<N> {
|
|
||||||
$t::new($(self.$compN + right.$compN),+)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: AddAssign<N>> AddAssign<$t<N>> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn add_assign(&mut self, right: $t<N>) {
|
|
||||||
$( self.$compN += right.$compN; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop!(impl Add, add for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! pointwise_sub(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Sub<N, Output = N>> Sub<$t<N>> for $t<N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn sub(self, right: $t<N>) -> $t<N> {
|
|
||||||
$t::new($(self.$compN - right.$compN),+)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
impl<N: SubAssign<N>> SubAssign<$t<N>> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn sub_assign(&mut self, right: $t<N>) {
|
|
||||||
$( self.$compN -= right.$compN; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop!(impl Sub, sub for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! ref_binop_scalar_exact(
|
|
||||||
(impl $imp:ident<$lhs:ident>, $method:ident for $t:ident; $($compN:ident),+) => {
|
|
||||||
impl $imp<$t<$lhs>> for $lhs {
|
|
||||||
type Output = $t<$lhs>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: $t<$lhs>) -> $t<$lhs> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(right.$compN, self)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, 'b> $imp<&'a $t<$lhs>> for &'b $lhs {
|
|
||||||
type Output = $t<$lhs>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: &'a $t<$lhs>) -> $t<$lhs> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(right.$compN, self)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a> $imp<$t<$lhs>> for &'a $lhs {
|
|
||||||
type Output = $t<$lhs>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: $t<$lhs>) -> $t<$lhs> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(right.$compN, self)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a> $imp<&'a $t<$lhs>> for $lhs {
|
|
||||||
type Output = $t<$lhs>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: &'a $t<$lhs>) -> $t<$lhs> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(right.$compN, self)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
};
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! ref_binop_scalar(
|
|
||||||
// Symmetric to `std::ops::forward_ref_binop!`
|
|
||||||
(impl $imp:ident, $method:ident for $t:ident; $($compN:ident),+) => {
|
|
||||||
ref_binop_scalar_exact!(impl $imp<f32>, $method for $t; $($compN),+);
|
|
||||||
ref_binop_scalar_exact!(impl $imp<f64>, $method for $t; $($compN),+);
|
|
||||||
|
|
||||||
impl<N: Copy + $imp<N, Output = N>> $imp<N> for $t<N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: N) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(self.$compN, right)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, 'b, N> $imp<&'a N> for &'b $t<N> where &'b N: $imp<&'a N, Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: &'a N) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(&self.$compN, &right)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, N> $imp<N> for &'a $t<N> where for<'b> &'a N: $imp<&'b N, Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: N) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(&self.$compN, &right)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, N> $imp<&'a N> for $t<N> where N: $imp<&'a N, Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self, right: &'a N) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(self.$compN, &right)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! pointwise_scalar_mul(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Copy + MulAssign<N>> MulAssign<N> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn mul_assign(&mut self, right: N) {
|
|
||||||
$( self.$compN *= right; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop_scalar!(impl Mul, mul for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! pointwise_scalar_div(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Copy + DivAssign<N>> DivAssign<N> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn div_assign(&mut self, right: N) {
|
|
||||||
$( self.$compN /= right; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop_scalar!(impl Div, div for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! pointwise_scalar_add(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Copy + AddAssign<N>> AddAssign<N> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn add_assign(&mut self, right: N) {
|
|
||||||
$( self.$compN += right; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop_scalar!(impl Add, add for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! pointwise_scalar_sub(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Copy + SubAssign<N>> SubAssign<N> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn sub_assign(&mut self, right: N) {
|
|
||||||
$( self.$compN -= right; )+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ref_binop_scalar!(impl Sub, sub for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! ref_unop(
|
|
||||||
// Symmetric to `std::ops::forward_ref_unop!`
|
|
||||||
(impl $imp:ident, $method:ident for $t:ident; $($compN:ident),+) => {
|
|
||||||
impl<N: $imp<Output = N> + Copy> $imp for $t<N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(self.$compN)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, N> $imp for &'a $t<N> where &'a N: $imp<Output = N> {
|
|
||||||
type Output = $t<N>;
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn $method(self) -> $t<N> {
|
|
||||||
$t::new(
|
|
||||||
$(
|
|
||||||
$imp::$method(&self.$compN)
|
|
||||||
),+
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! componentwise_neg(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
ref_unop!(impl Neg, neg for $t; $($compN),+);
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! componentwise_repeat(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Copy> Repeat<N> for $t<N> {
|
|
||||||
fn repeat(val: N) -> $t<N> {
|
|
||||||
$t {
|
|
||||||
$($compN: val ),+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! componentwise_absolute(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N: Absolute<N>> Absolute<$t<N>> for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn abs(m: &$t<N>) -> $t<N> {
|
|
||||||
$t::new($(::abs(&m.$compN) ),+)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! componentwise_zero(
|
|
||||||
($t: ident, $($compN: ident),+ ) => (
|
|
||||||
impl<N: Zero> Zero for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn zero() -> $t<N> {
|
|
||||||
$t {
|
|
||||||
$($compN: ::zero() ),+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#[inline]
|
|
||||||
fn is_zero(&self) -> bool {
|
|
||||||
$(::is_zero(&self.$compN) )&&+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! componentwise_one(
|
|
||||||
($t: ident, $($compN: ident),+ ) => (
|
|
||||||
impl<N: BaseNum> One for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn one() -> $t<N> {
|
|
||||||
$t {
|
|
||||||
$($compN: ::one() ),+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// Implements Arbitrary by setting each components to Arbitrary::arbitrary.
|
|
||||||
macro_rules! componentwise_arbitrary(
|
|
||||||
($t: ident, $($compN: ident),+ ) => (
|
|
||||||
#[cfg(feature="arbitrary")]
|
|
||||||
impl<N: Arbitrary> Arbitrary for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn arbitrary<G: Gen>(g: &mut G) -> $t<N> {
|
|
||||||
$t { $($compN: Arbitrary::arbitrary(g),)* }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// Implements Rand by setting each components to Rand::rand.
|
|
||||||
macro_rules! componentwise_rand(
|
|
||||||
($t: ident, $($compN: ident),+ ) => (
|
|
||||||
impl<N: Rand> Rand for $t<N> {
|
|
||||||
#[inline]
|
|
||||||
fn rand<R: Rng>(rng: &mut R) -> $t<N> {
|
|
||||||
$t { $($compN: Rand::rand(rng), )* }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
macro_rules! component_basis_element(
|
|
||||||
($t: ident, $($compN: ident),+ ) => (
|
|
||||||
/*
|
|
||||||
*
|
|
||||||
* Element of the canonical basis.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
impl<N: Zero + One> $t<N> {
|
|
||||||
$(
|
|
||||||
/// Create the element of the canonical basis having this component set to one and
|
|
||||||
/// all the others set to zero.
|
|
||||||
#[inline]
|
|
||||||
pub fn $compN() -> $t<N> {
|
|
||||||
let mut res: $t<N> = ::zero();
|
|
||||||
|
|
||||||
res.$compN = ::one();
|
|
||||||
|
|
||||||
res
|
|
||||||
}
|
|
||||||
)+
|
|
||||||
}
|
|
||||||
)
|
|
||||||
);
|
|
||||||
|
|
||||||
// A function to create a new element from its component values.
|
|
||||||
macro_rules! component_new(
|
|
||||||
($t: ident, $($compN: ident),+) => (
|
|
||||||
impl<N> $t<N> {
|
|
||||||
/// Creation from component values.
|
|
||||||
#[inline]
|
|
||||||
pub fn new($($compN: N ),+) -> $t<N> {
|
|
||||||
$t {
|
|
||||||
$($compN: $compN ),+
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
);
|
|
||||||
);
|
|
||||||
|
|
||||||
|
|
||||||
macro_rules! fold_add(
|
|
||||||
// base case
|
|
||||||
($x:expr) => {
|
|
||||||
$x
|
|
||||||
};
|
|
||||||
// `$x` followed by at least one `$y,`
|
|
||||||
($x:expr, $($y:expr),+) => {
|
|
||||||
// call min! on the tail `$y`
|
|
||||||
Add::add($x, fold_add!($($y),+))
|
|
||||||
}
|
|
||||||
);
|
|
@ -1,226 +0,0 @@
|
|||||||
//! Matrix with dimensions unknown at compile-time.
|
|
||||||
|
|
||||||
use std::cmp;
|
|
||||||
use std::mem;
|
|
||||||
use std::iter::repeat;
|
|
||||||
use std::ops::{Add, Sub, Mul, Div, AddAssign, SubAssign, MulAssign, Index, IndexMut};
|
|
||||||
use std::fmt::{Debug, Formatter, Result};
|
|
||||||
use rand::{self, Rand};
|
|
||||||
use num::{Zero, One};
|
|
||||||
use structs::dvector::{DVector, DVector1, DVector2, DVector3, DVector4, DVector5, DVector6};
|
|
||||||
use traits::operations::{ApproxEq, Inverse, Transpose, Mean, Covariance};
|
|
||||||
use traits::structure::{Cast, Column, ColumnSlice, Row, RowSlice, Diagonal, DiagonalMut, Eye,
|
|
||||||
Indexable, Shape, BaseNum};
|
|
||||||
#[cfg(feature="arbitrary")]
|
|
||||||
use quickcheck::{Arbitrary, Gen};
|
|
||||||
|
|
||||||
|
|
||||||
/// Matrix with dimensions unknown at compile-time.
|
|
||||||
#[derive(Eq, PartialEq, Clone)]
|
|
||||||
pub struct DMatrix<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: Vec<N>
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N> DMatrix<N> {
|
|
||||||
/// Creates an uninitialized matrix.
|
|
||||||
#[inline]
|
|
||||||
pub unsafe fn new_uninitialized(nrows: usize, ncols: usize) -> DMatrix<N> {
|
|
||||||
let mut vector = Vec::with_capacity(nrows * ncols);
|
|
||||||
vector.set_len(nrows * ncols);
|
|
||||||
|
|
||||||
DMatrix {
|
|
||||||
nrows: nrows,
|
|
||||||
ncols: ncols,
|
|
||||||
mij: vector
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N: Clone + Copy> DMatrix<N> {
|
|
||||||
/// Builds a matrix filled with a given constant.
|
|
||||||
#[inline]
|
|
||||||
pub fn from_element(nrows: usize, ncols: usize, val: N) -> DMatrix<N> {
|
|
||||||
DMatrix {
|
|
||||||
nrows: nrows,
|
|
||||||
ncols: ncols,
|
|
||||||
mij: repeat(val).take(nrows * ncols).collect()
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Builds a matrix filled with the components provided by a vector.
|
|
||||||
/// The vector contains the matrix data in row-major order.
|
|
||||||
/// Note that `from_column_vector` is much faster than `from_row_vector` since a `DMatrix` stores its data
|
|
||||||
/// in column-major order.
|
|
||||||
///
|
|
||||||
/// The vector must have exactly `nrows * ncols` elements.
|
|
||||||
#[inline]
|
|
||||||
pub fn from_row_vector(nrows: usize, ncols: usize, vector: &[N]) -> DMatrix<N> {
|
|
||||||
DMatrix::from_row_iter(nrows, ncols, vector.to_vec())
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Builds a matrix filled with the components provided by a vector.
|
|
||||||
/// The vector contains the matrix data in column-major order.
|
|
||||||
/// Note that `from_column_vector` is much faster than `from_row_vector` since a `DMatrix` stores its data
|
|
||||||
/// in column-major order.
|
|
||||||
///
|
|
||||||
/// The vector must have exactly `nrows * ncols` elements.
|
|
||||||
#[inline]
|
|
||||||
pub fn from_column_vector(nrows: usize, ncols: usize, vector: &[N]) -> DMatrix<N> {
|
|
||||||
DMatrix::from_column_iter(nrows, ncols, vector.to_vec())
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Builds a matrix filled with the components provided by a source that may be moved into an iterator.
|
|
||||||
/// The source contains the matrix data in row-major order.
|
|
||||||
/// Note that `from_column_iter` is much faster than `from_row_iter` since a `DMatrix` stores its data
|
|
||||||
/// in column-major order.
|
|
||||||
///
|
|
||||||
/// The source must have exactly `nrows * ncols` elements.
|
|
||||||
#[inline]
|
|
||||||
pub fn from_row_iter<I: IntoIterator<Item = N>>(nrows: usize, ncols: usize, param: I) -> DMatrix<N> {
|
|
||||||
let mut res = DMatrix::from_column_iter(ncols, nrows, param);
|
|
||||||
|
|
||||||
// we transpose because the buffer is row_major
|
|
||||||
res.transpose_mut();
|
|
||||||
|
|
||||||
res
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/// Builds a matrix filled with the components provided by a source that may be moved into an iterator.
|
|
||||||
/// The source contains the matrix data in column-major order.
|
|
||||||
/// Note that `from_column_iter` is much faster than `from_row_iter` since a `DMatrix` stores its data
|
|
||||||
/// in column-major order.
|
|
||||||
///
|
|
||||||
/// The source must have exactly `nrows * ncols` elements.
|
|
||||||
#[inline]
|
|
||||||
pub fn from_column_iter<I: IntoIterator<Item = N>>(nrows: usize, ncols: usize, param: I) -> DMatrix<N> {
|
|
||||||
let mij: Vec<N> = param.into_iter().collect();
|
|
||||||
|
|
||||||
assert!(nrows * ncols == mij.len(), "The ammount of data provided does not matches the matrix size.");
|
|
||||||
|
|
||||||
DMatrix {
|
|
||||||
nrows: nrows,
|
|
||||||
ncols: ncols,
|
|
||||||
mij: mij
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<N> DMatrix<N> {
|
|
||||||
/// Builds a matrix filled with the results of a function applied to each of its component coordinates.
|
|
||||||
#[inline]
|
|
||||||
pub fn from_fn<F: FnMut(usize, usize) -> N>(nrows: usize, ncols: usize, mut f: F) -> DMatrix<N> {
|
|
||||||
DMatrix {
|
|
||||||
nrows: nrows,
|
|
||||||
ncols: ncols,
|
|
||||||
mij: (0 .. nrows * ncols).map(|i| { let m = i / nrows; f(i - m * nrows, m) }).collect()
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Transforms this matrix into an array. This consumes the matrix and is O(1).
|
|
||||||
/// The returned vector contains the matrix data in column-major order.
|
|
||||||
#[inline]
|
|
||||||
pub fn into_vector(self) -> Vec<N> {
|
|
||||||
self.mij
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
dmat_impl!(DMatrix, DVector);
|
|
||||||
|
|
||||||
|
|
||||||
/// A stack-allocated dynamically sized matrix with at most one row and column.
|
|
||||||
pub struct DMatrix1<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: [N; 1],
|
|
||||||
}
|
|
||||||
|
|
||||||
small_dmat_impl!(DMatrix1, DVector1, 1, 0);
|
|
||||||
small_dmat_from_impl!(DMatrix1, 1, ::zero());
|
|
||||||
|
|
||||||
|
|
||||||
/// A stack-allocated dynamically sized square or rectangular matrix with at most 2 rows and columns.
|
|
||||||
pub struct DMatrix2<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: [N; 2 * 2],
|
|
||||||
}
|
|
||||||
|
|
||||||
small_dmat_impl!(DMatrix2, DVector2, 2, 0, 1,
|
|
||||||
2, 3);
|
|
||||||
small_dmat_from_impl!(DMatrix2, 2, ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero());
|
|
||||||
|
|
||||||
|
|
||||||
/// A stack-allocated dynamically sized square or rectangular matrix with at most 3 rows and columns.
|
|
||||||
pub struct DMatrix3<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: [N; 3 * 3],
|
|
||||||
}
|
|
||||||
|
|
||||||
small_dmat_impl!(DMatrix3, DVector3, 3, 0, 1, 2,
|
|
||||||
3, 4, 5,
|
|
||||||
6, 7, 8);
|
|
||||||
small_dmat_from_impl!(DMatrix3, 3, ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero());
|
|
||||||
|
|
||||||
|
|
||||||
/// A stack-allocated dynamically sized square or rectangular matrix with at most 4 rows and columns.
|
|
||||||
pub struct DMatrix4<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: [N; 4 * 4],
|
|
||||||
}
|
|
||||||
|
|
||||||
small_dmat_impl!(DMatrix4, DVector4, 4, 0, 1, 2, 3,
|
|
||||||
4, 5, 6, 7,
|
|
||||||
8, 9, 10, 11,
|
|
||||||
12, 13, 14, 15);
|
|
||||||
small_dmat_from_impl!(DMatrix4, 4, ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero());
|
|
||||||
|
|
||||||
|
|
||||||
/// A stack-allocated dynamically sized square or rectangular matrix with at most 5 rows and columns.
|
|
||||||
pub struct DMatrix5<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: [N; 5 * 5],
|
|
||||||
}
|
|
||||||
|
|
||||||
small_dmat_impl!(DMatrix5, DVector5, 5, 0, 1, 2, 3, 4,
|
|
||||||
5, 6, 7, 8, 9,
|
|
||||||
10, 11, 12, 13, 14,
|
|
||||||
15, 16, 17, 18, 19,
|
|
||||||
20, 21, 22, 23, 24);
|
|
||||||
small_dmat_from_impl!(DMatrix5, 5, ::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero());
|
|
||||||
|
|
||||||
|
|
||||||
/// A stack-allocated dynamically sized square or rectangular matrix with at most 6 rows and columns.
|
|
||||||
pub struct DMatrix6<N> {
|
|
||||||
nrows: usize,
|
|
||||||
ncols: usize,
|
|
||||||
mij: [N; 6 * 6],
|
|
||||||
}
|
|
||||||
|
|
||||||
small_dmat_impl!(DMatrix6, DVector6, 6, 0, 1, 2, 3, 4, 5,
|
|
||||||
6, 7, 8, 9, 10, 11,
|
|
||||||
12, 13, 14, 15, 16, 17,
|
|
||||||
18, 19, 20, 21, 22, 23,
|
|
||||||
24, 25, 26, 27, 28, 29,
|
|
||||||
30, 31, 32, 33, 34, 35);
|
|
||||||
small_dmat_from_impl!(DMatrix6, 6, ::zero(), ::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(), ::zero(),
|
|
||||||
::zero(), ::zero(), ::zero(), ::zero(), ::zero(), ::zero());
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user