2013-06-02 02:50:00 +08:00
|
|
|
use std::num::{One, Zero};
|
2013-07-02 18:00:55 +08:00
|
|
|
use std::vec::from_elem;
|
2013-06-02 02:50:00 +08:00
|
|
|
use std::cmp::ApproxEq;
|
2013-05-31 17:28:42 +08:00
|
|
|
use traits::inv::Inv;
|
2013-08-17 23:50:01 +08:00
|
|
|
use traits::ring::DivisionRing;
|
2013-05-31 17:28:42 +08:00
|
|
|
use traits::transpose::Transpose;
|
2013-06-16 04:16:44 +08:00
|
|
|
use traits::rlmul::{RMul, LMul};
|
2013-06-29 08:34:45 +08:00
|
|
|
use dvec::{DVec, zero_vec_with_dim};
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-07-24 22:50:40 +08:00
|
|
|
/// Square matrix with a dimension unknown at compile-time.
|
2013-05-31 17:28:42 +08:00
|
|
|
#[deriving(Eq, ToStr, Clone)]
|
2013-08-05 16:13:44 +08:00
|
|
|
pub struct DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
priv dim: uint, // FIXME: handle more than just square matrices
|
|
|
|
priv mij: ~[N]
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
2013-07-24 22:50:40 +08:00
|
|
|
/// Builds a matrix filled with zeros.
|
|
|
|
///
|
|
|
|
/// # Arguments
|
|
|
|
/// * `dim` - The dimension of the matrix. A `dim`-dimensional matrix contains `dim * dim`
|
|
|
|
/// components.
|
2013-06-28 01:40:37 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
pub fn zero_mat_with_dim<N: Zero + Clone>(dim: uint) -> DMat<N> {
|
|
|
|
DMat { dim: dim, mij: from_elem(dim * dim, Zero::zero()) }
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-07-24 22:50:40 +08:00
|
|
|
/// Tests if all components of the matrix are zeroes.
|
2013-06-28 01:40:37 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
pub fn is_zero_mat<N: Zero>(mat: &DMat<N>) -> bool {
|
|
|
|
mat.mij.iter().all(|e| e.is_zero())
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-07-24 22:50:40 +08:00
|
|
|
/// Builds an identity matrix.
|
|
|
|
///
|
|
|
|
/// # Arguments
|
|
|
|
/// * `dim` - The dimension of the matrix. A `dim`-dimensional matrix contains `dim * dim`
|
|
|
|
/// components.
|
2013-06-28 01:40:37 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
pub fn one_mat_with_dim<N: Clone + One + Zero>(dim: uint) -> DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
let mut res = zero_mat_with_dim(dim);
|
|
|
|
let _1 = One::one::<N>();
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for i in range(0u, dim) {
|
|
|
|
res.set(i, i, &_1);
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
impl<N: Clone> DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn offset(&self, i: uint, j: uint) -> uint {
|
|
|
|
i * self.dim + j
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
|
|
|
|
/// Changes the value of a component of the matrix.
|
|
|
|
///
|
|
|
|
/// # Arguments
|
|
|
|
/// * `i` - 0-based index of the line to be changed
|
|
|
|
/// * `j` - 0-based index of the column to be changed
|
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
pub fn set(&mut self, i: uint, j: uint, t: &N) {
|
2013-08-05 15:44:56 +08:00
|
|
|
assert!(i < self.dim);
|
|
|
|
assert!(j < self.dim);
|
|
|
|
self.mij[self.offset(i, j)] = t.clone()
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Reads the value of a component of the matrix.
|
|
|
|
///
|
|
|
|
/// # Arguments
|
|
|
|
/// * `i` - 0-based index of the line to be read
|
|
|
|
/// * `j` - 0-based index of the column to be read
|
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
pub fn at(&self, i: uint, j: uint) -> N {
|
2013-08-05 15:44:56 +08:00
|
|
|
assert!(i < self.dim);
|
|
|
|
assert!(j < self.dim);
|
|
|
|
self.mij[self.offset(i, j)].clone()
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
impl<N: Clone> Index<(uint, uint), N> for DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn index(&self, &(i, j): &(uint, uint)) -> N {
|
|
|
|
self.at(i, j)
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
Removed occurences of copy/Copy + improved api.
Now, access to vector components are x, y, z, w, a, b, ... instead of at[i].
The method at(i) has the same (read only) effect as the old at[i].
Now, access to matrix components are m11, m12, ... instead of mij[offset(i, j)]...
The method at((i, j)) has the same effect as the old mij[offset(i, j)].
Automatic implementation of all traits the compiler supports has been added on the #[deriving]
clause for both matrices and vectors.
2013-07-20 21:07:49 +08:00
|
|
|
impl<N: Clone + Mul<N, N> + Add<N, N> + Zero>
|
2013-08-05 16:13:44 +08:00
|
|
|
Mul<DMat<N>, DMat<N>> for DMat<N> {
|
|
|
|
fn mul(&self, other: &DMat<N>) -> DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
assert!(self.dim == other.dim);
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
let dim = self.dim;
|
|
|
|
let mut res = zero_mat_with_dim(dim);
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for i in range(0u, dim) {
|
|
|
|
for j in range(0u, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let mut acc = Zero::zero::<N>();
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for k in range(0u, dim) {
|
|
|
|
acc = acc + self.at(i, k) * other.at(k, j);
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res.set(i, j, &acc);
|
|
|
|
}
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
Removed occurences of copy/Copy + improved api.
Now, access to vector components are x, y, z, w, a, b, ... instead of at[i].
The method at(i) has the same (read only) effect as the old at[i].
Now, access to matrix components are m11, m12, ... instead of mij[offset(i, j)]...
The method at((i, j)) has the same effect as the old mij[offset(i, j)].
Automatic implementation of all traits the compiler supports has been added on the #[deriving]
clause for both matrices and vectors.
2013-07-20 21:07:49 +08:00
|
|
|
impl<N: Clone + Add<N, N> + Mul<N, N> + Zero>
|
2013-08-05 16:13:44 +08:00
|
|
|
RMul<DVec<N>> for DMat<N> {
|
|
|
|
fn rmul(&self, other: &DVec<N>) -> DVec<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
assert!(self.dim == other.at.len());
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
let dim = self.dim;
|
|
|
|
let mut res : DVec<N> = zero_vec_with_dim(dim);
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for i in range(0u, dim) {
|
|
|
|
for j in range(0u, dim) {
|
|
|
|
res.at[i] = res.at[i] + other.at[j] * self.at(i, j);
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
Removed occurences of copy/Copy + improved api.
Now, access to vector components are x, y, z, w, a, b, ... instead of at[i].
The method at(i) has the same (read only) effect as the old at[i].
Now, access to matrix components are m11, m12, ... instead of mij[offset(i, j)]...
The method at((i, j)) has the same effect as the old mij[offset(i, j)].
Automatic implementation of all traits the compiler supports has been added on the #[deriving]
clause for both matrices and vectors.
2013-07-20 21:07:49 +08:00
|
|
|
impl<N: Clone + Add<N, N> + Mul<N, N> + Zero>
|
2013-08-05 16:13:44 +08:00
|
|
|
LMul<DVec<N>> for DMat<N> {
|
|
|
|
fn lmul(&self, other: &DVec<N>) -> DVec<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
assert!(self.dim == other.at.len());
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
let dim = self.dim;
|
|
|
|
let mut res : DVec<N> = zero_vec_with_dim(dim);
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for i in range(0u, dim) {
|
|
|
|
for j in range(0u, dim) {
|
|
|
|
res.at[i] = res.at[i] + other.at[j] * self.at(j, i);
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
Removed occurences of copy/Copy + improved api.
Now, access to vector components are x, y, z, w, a, b, ... instead of at[i].
The method at(i) has the same (read only) effect as the old at[i].
Now, access to matrix components are m11, m12, ... instead of mij[offset(i, j)]...
The method at((i, j)) has the same effect as the old mij[offset(i, j)].
Automatic implementation of all traits the compiler supports has been added on the #[deriving]
clause for both matrices and vectors.
2013-07-20 21:07:49 +08:00
|
|
|
impl<N: Clone + Eq + DivisionRing>
|
2013-08-05 16:13:44 +08:00
|
|
|
Inv for DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn inverse(&self) -> Option<DMat<N>> {
|
2013-08-05 15:44:56 +08:00
|
|
|
let mut res : DMat<N> = self.clone();
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
if res.inplace_inverse() {
|
|
|
|
Some(res)
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
None
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
fn inplace_inverse(&mut self) -> bool {
|
2013-08-05 15:44:56 +08:00
|
|
|
let dim = self.dim;
|
|
|
|
let mut res = one_mat_with_dim::<N>(dim);
|
|
|
|
let _0T = Zero::zero::<N>();
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
// inversion using Gauss-Jordan elimination
|
2013-08-05 16:13:44 +08:00
|
|
|
for k in range(0u, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
// 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
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
while (n0 != dim) {
|
|
|
|
if self.at(n0, k) != _0T {
|
|
|
|
break;
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
|
|
|
|
n0 = n0 + 1;
|
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
if n0 == dim {
|
|
|
|
return false
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
|
|
|
|
// swap pivot line
|
2013-08-05 16:13:44 +08:00
|
|
|
if n0 != k {
|
|
|
|
for j in range(0u, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let off_n0_j = self.offset(n0, j);
|
|
|
|
let off_k_j = self.offset(k, j);
|
|
|
|
|
|
|
|
self.mij.swap(off_n0_j, off_k_j);
|
|
|
|
res.mij.swap(off_n0_j, off_k_j);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
let pivot = self.at(k, k);
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for j in range(k, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let selfval = &(self.at(k, j) / pivot);
|
|
|
|
self.set(k, j, selfval);
|
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for j in range(0u, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let resval = &(res.at(k, j) / pivot);
|
|
|
|
res.set(k, j, resval);
|
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for l in range(0u, dim) {
|
|
|
|
if l != k {
|
2013-08-05 15:44:56 +08:00
|
|
|
let normalizer = self.at(l, k);
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for j in range(k, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let selfval = &(self.at(l, j) - self.at(k, j) * normalizer);
|
|
|
|
self.set(l, j, selfval);
|
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for j in range(0u, dim) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let resval = &(res.at(l, j) - res.at(k, j) * normalizer);
|
|
|
|
res.set(l, j, resval);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
*self = res;
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
true
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
impl<N: Clone> Transpose for DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn transposed(&self) -> DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
let mut res = self.clone();
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res.transpose();
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
res
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
fn transpose(&mut self) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let dim = self.dim;
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
for i in range(1u, dim) {
|
|
|
|
for j in range(0u, dim - 1) {
|
2013-08-05 15:44:56 +08:00
|
|
|
let off_i_j = self.offset(i, j);
|
|
|
|
let off_j_i = self.offset(j, i);
|
|
|
|
|
|
|
|
self.mij.swap(off_i_j, off_j_i);
|
|
|
|
}
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
impl<N: ApproxEq<N>> ApproxEq<N> for DMat<N> {
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn approx_epsilon() -> N {
|
|
|
|
ApproxEq::approx_epsilon::<N, N>()
|
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn approx_eq(&self, other: &DMat<N>) -> bool {
|
2013-08-05 15:44:56 +08:00
|
|
|
let mut zip = self.mij.iter().zip(other.mij.iter());
|
2013-06-09 20:09:22 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
do zip.all |(a, b)| {
|
|
|
|
a.approx_eq(b)
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
|
2013-08-05 15:44:56 +08:00
|
|
|
#[inline]
|
2013-08-05 16:13:44 +08:00
|
|
|
fn approx_eq_eps(&self, other: &DMat<N>, epsilon: &N) -> bool {
|
2013-08-05 15:44:56 +08:00
|
|
|
let mut zip = self.mij.iter().zip(other.mij.iter());
|
2013-06-09 20:09:22 +08:00
|
|
|
|
2013-08-05 16:13:44 +08:00
|
|
|
do zip.all |(a, b)| {
|
|
|
|
a.approx_eq_eps(b, epsilon)
|
|
|
|
}
|
2013-08-05 15:44:56 +08:00
|
|
|
}
|
2013-05-31 17:28:42 +08:00
|
|
|
}
|