Rotation.h

Go to the documentation of this file.

/*  YAP - Yet another PWA toolkit
    Copyright 2015, Technische Universitaet Muenchen,
    Authors: Daniel Greenwald, Johannes Rauch

    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/>.
*/


#ifndef yap_Rotation_h
#define yap_Rotation_h

#include "Matrix.h"
#include "ThreeVector.h"

namespace yap {

template <typename T>
const ThreeMatrix<T> rotation(const ThreeVector<T>& V, const T& theta)
{
    T v = abs(V);

    if (v == 0 || theta == 0)
        return unitMatrix<T, 3>();

    // normalize direction vector
    ThreeVector<T> U = V * (T(1) / v);

    T cosTheta = cos(theta);

    return ((T)1 - cosTheta) * outer(U, U) + cosTheta * unitMatrix<T, 3>() + (T)sin(theta) * skewSymmetric(U);
}

template <typename T>
const ThreeMatrix<T> eulerRotationZXZ(const CoordinateSystem<T, 3>& C, const T& alpha, const T& beta, const T& gamma)
{ return rotation(C[2], gamma) * rotation(C[0], beta) * rotation(C[2], alpha); }

template <typename T>
const ThreeMatrix<T> eulerRotationZYZ(const CoordinateSystem<T, 3>& C, const T& alpha, const T& beta, const T& gamma)
{ return rotation(C[2], gamma) * rotation(C[1], beta) * rotation(C[2], alpha); }

}
#endif