You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
167 lines
6.4 KiB
167 lines
6.4 KiB
//
|
|
// Transformation of vectors in a given coordinate system...
|
|
//
|
|
// Georg Hopp <georg@steffers.org>
|
|
//
|
|
// Copyright © 2019 Georg Hopp
|
|
//
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
//
|
|
use std::ops::{Add, Sub, Neg, Mul, Div};
|
|
use crate::Vector;
|
|
use crate::trigonometry::Trig;
|
|
|
|
pub struct TMatrix<T>( (T, T, T, T)
|
|
, (T, T, T, T)
|
|
, (T, T, T, T)
|
|
, (T, T, T, T) )
|
|
where T: Add + Sub + Neg + Mul + Div + Trig + From<i32> + Copy;
|
|
|
|
pub fn translate<T>(v :Vector<T>) -> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
let Vector(x, y, z) = v;
|
|
|
|
TMatrix( (1.into(), 0.into(), 0.into(), 0.into())
|
|
, (0.into(), 1.into(), 0.into(), 0.into())
|
|
, (0.into(), 0.into(), 1.into(), 0.into())
|
|
, ( x, y, z, 1.into()) )
|
|
}
|
|
|
|
pub fn rotate_x<T>(a :i32) -> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
let sin :T = Trig::sin(a);
|
|
let cos :T = Trig::cos(a);
|
|
|
|
TMatrix( (1.into(), 0.into(), 0.into(), 0.into())
|
|
, (0.into(), cos , -sin , 0.into())
|
|
, (0.into(), sin , cos , 0.into())
|
|
, (0.into(), 0.into(), 0.into(), 1.into()) )
|
|
}
|
|
|
|
pub fn rotate_y<T>(a :i32) -> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
let sin :T = Trig::sin(a);
|
|
let cos :T = Trig::cos(a);
|
|
|
|
TMatrix( (cos , 0.into(), sin , 0.into())
|
|
, (0.into(), 1.into(), 0.into(), 0.into())
|
|
, (-sin , 0.into(), cos , 0.into())
|
|
, (0.into(), 0.into(), 0.into(), 1.into()) )
|
|
}
|
|
|
|
pub fn rotate_z<T>(a :i32) -> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
let sin :T = Trig::sin(a);
|
|
let cos :T = Trig::cos(a);
|
|
|
|
TMatrix( (cos , -sin , 0.into(), 0.into())
|
|
, (sin , cos , 0.into(), 0.into())
|
|
, (0.into(), 0.into(), 1.into(), 0.into())
|
|
, (0.into(), 0.into(), 0.into(), 1.into()) )
|
|
}
|
|
|
|
pub fn rotate_v<T>(v :&Vector<T>, a :i32) -> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
let Vector(x, y, z) = *v;
|
|
|
|
let sin :T = Trig::sin(a);
|
|
let cos :T = Trig::cos(a);
|
|
|
|
let zero :T = 0.into();
|
|
let one :T = 1.into();
|
|
|
|
TMatrix( ( (one - cos) * x * x + cos
|
|
, (one - cos) * x * y - sin * z
|
|
, (one - cos) * x * z + sin * y
|
|
, zero )
|
|
, ( (one - cos) * x * y + sin * z
|
|
, (one - cos) * y * y + cos
|
|
, (one - cos) * y * z - sin * x
|
|
, zero )
|
|
, ( (one - cos) * x * z - sin * y
|
|
, (one - cos) * y * z + sin * x
|
|
, (one - cos) * z * z + cos
|
|
, zero )
|
|
, (0.into(), 0.into(), 0.into(), 1.into()) )
|
|
}
|
|
|
|
pub fn scale<T>(v :Vector<T>) -> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
let Vector(x, y, z) = v;
|
|
|
|
TMatrix( ( x, 0.into(), 0.into(), 0.into())
|
|
, (0.into(), y, 0.into(), 0.into())
|
|
, (0.into(), 0.into(), z, 0.into())
|
|
, (0.into(), 0.into(), 0.into(), 1.into()) )
|
|
}
|
|
|
|
impl<T> TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
pub fn apply(&self, v :&Vector<T>) -> Vector<T> {
|
|
let TMatrix( (a11, a12, a13, a14)
|
|
, (a21, a22, a23, a24)
|
|
, (a31, a32, a33, a34)
|
|
, (a41, a42, a43, a44) ) = *self;
|
|
let Vector(x, y, z) = *v;
|
|
|
|
let v = Vector( a11 * x + a21 * y + a31 * z + a41
|
|
, a12 * x + a22 * y + a32 * z + a42
|
|
, a13 * x + a23 * y + a33 * z + a43 );
|
|
let w = a14 * x + a24 * y + a34 * z + a44;
|
|
|
|
v.mul(&w.recip())
|
|
}
|
|
}
|
|
|
|
impl<T> Mul for TMatrix<T>
|
|
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
|
|
+ Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
|
|
type Output = Self;
|
|
|
|
// ATTENTION: This is not commutative, nor assoziative.
|
|
fn mul(self, other :Self) -> Self {
|
|
let TMatrix( (a11, a12, a13, a14)
|
|
, (a21, a22, a23, a24)
|
|
, (a31, a32, a33, a34)
|
|
, (a41, a42, a43, a44) ) = self;
|
|
let TMatrix( (b11, b12, b13, b14)
|
|
, (b21, b22, b23, b24)
|
|
, (b31, b32, b33, b34)
|
|
, (b41, b42, b43, b44) ) = other;
|
|
|
|
TMatrix( ( a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41
|
|
, a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42
|
|
, a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43
|
|
, a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44 )
|
|
, ( a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41
|
|
, a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42
|
|
, a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43
|
|
, a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44 )
|
|
, ( a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41
|
|
, a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42
|
|
, a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43
|
|
, a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44 )
|
|
, ( a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41
|
|
, a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42
|
|
, a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43
|
|
, a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44 ) )
|
|
}
|
|
}
|