transform.rs 6.97 KB
//
// 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;

#[derive(Debug, Clone, Copy)]
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 unit<T>() -> TMatrix<T>
where T: Add<Output = T> + Sub<Output = T> + Neg<Output = T>
       + Mul<Output = T> + Div<Output = T> + Trig + From<i32> + Copy {
    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())
           , (0.into(), 0.into(), 0.into(), 1.into()) )
}
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 combine<I>(mi :I) -> TMatrix<T>
        where I: IntoIterator<Item = TMatrix<T>> {

        mi.into_iter().fold(unit(), |acc, x| acc * x)
    }

    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 ) )
    }
}