b2referential.H

Go to the documentation of this file.

//------------------------------------------------------------------------
// b2referential.H --
//
//
// written by Mathias Doreille
//
// Copyright (c) 2004-2012,2017
//     SMR Engineering & Development SA
//     2502 Bienne, Switzerland
//
// All Rights Reserved.  Proprietary source code.  The contents of
// this file may not be disclosed to third parties, copied or
// duplicated in any form, in whole or in part, without the prior
// written permission of SMR.
//------------------------------------------------------------------------
 
#ifndef __B2REFERENTIAL_H__
#define __B2REFERENTIAL_H__
 
#include "b2ppconfig.h"
#include "utils/b2object.H"
#include "utils/b2type.H"
 
namespace b2000 {
 
template <typename T>
class Transformation3D {
public:
    Transformation3D() : identity(true), rot_identity(true) {
        mat[0] = inv[0] = T(1);
        mat[1] = inv[1] = T(0);
        mat[2] = inv[2] = T(0);
        mat[3] = inv[3] = T(0);
        mat[4] = inv[4] = T(1);
        mat[5] = inv[5] = T(0);
        mat[6] = inv[6] = T(0);
        mat[7] = inv[7] = T(0);
        mat[8] = inv[8] = T(1);
        mat[9] = inv[9] = T(0);
        mat[10] = inv[10] = T(0);
        mat[11] = inv[11] = T(0);
    }
 
    Transformation3D(const Transformation3D& t)
        : identity(t.identity), rot_identity(t.rot_identity) {
        if (!identity) {
            std::copy(t.mat, t.mat + 12, mat);
            std::copy(t.inv, t.inv + 12, inv);
        }
    }
 
    Transformation3D(
          const T rotation_matrix[9], const T translation_vector[3] = 0,
          const bool invert = false) {
        init(rotation_matrix, translation_vector, invert);
    }
 
    bool operator==(const Transformation3D& trans) const {
        if (&trans == this) { return true; }
        if (identity != trans.identity || rot_identity != trans.rot_identity) { return false; }
        return std::equal(mat, mat + 12, trans.mat);
    }
 
    void init(
          const T rotation_matrix[9], const T translation_vector[3] = 0,
          const bool invert = false) {
        T *m, *i;
        if (invert) {
            m = inv;
            i = mat;
        } else {
            m = mat;
            i = inv;
        }
 
        m[0] = rotation_matrix[0];
        m[1] = rotation_matrix[3];
        m[2] = rotation_matrix[6];
        m[3] = rotation_matrix[1];
        m[4] = rotation_matrix[4];
        m[5] = rotation_matrix[7];
        m[6] = rotation_matrix[2];
        m[7] = rotation_matrix[5];
        m[8] = rotation_matrix[8];
        if (translation_vector) {
            m[9] = translation_vector[0];
            m[10] = translation_vector[1];
            m[11] = translation_vector[2];
        } else {
            m[9] = 0.0;
            m[10] = 0.0;
            m[11] = 0.0;
        }
 
        compute_invert(i, m);
 
        rot_identity = std::equal(mat, mat + 9, identity_transformation.get_rotation_matrix());
        identity = rot_identity || std::equal(mat + 9, mat + 12, inv);
    }
 
    const T* get_rotation_matrix() const { return mat; }
 
    const T* get_inv_rotation_matrix() const { return inv; }
 
    const T* get_translation_vector() const { return mat + 9; }
 
    const T* get_inv_translation_vector() const { return inv + 9; }
 
    bool is_identity() const { return identity; }
 
    bool is_rotation_identity() const { return rot_identity; }
 
    static const Transformation3D& get_identity() { return identity_transformation; }
 
private:
    bool identity;
    bool rot_identity;
    T mat[12];
    T inv[12];
    static const Transformation3D identity_transformation;
 
    static void compute_invert(T i[12], const T m[12]) {
        double inv_det = 1.0
                         / (m[0] * (m[4] * m[8] - m[5] * m[7]) + m[1] * (m[5] * m[6] - m[3] * m[8])
                            + m[2] * (m[3] * m[7] - m[4] * m[6]));
        i[0] = (m[4] * m[8] - m[5] * m[7]) * inv_det;
        i[1] = (m[2] * m[7] - m[1] * m[8]) * inv_det;
        i[2] = (m[1] * m[5] - m[2] * m[4]) * inv_det;
        i[3] = (m[5] * m[6] - m[3] * m[8]) * inv_det;
        i[4] = (m[0] * m[8] - m[2] * m[6]) * inv_det;
        i[5] = (m[2] * m[3] - m[0] * m[5]) * inv_det;
        i[6] = (m[3] * m[7] - m[4] * m[6]) * inv_det;
        i[7] = (m[1] * m[6] - m[0] * m[7]) * inv_det;
        i[8] = (m[0] * m[4] - m[1] * m[3]) * inv_det;
        i[9] = -(i[0] * m[9] + i[1] * m[10] + i[2] * m[11]);
        i[10] = -(i[3] * m[9] + i[4] * m[10] + i[5] * m[11]);
        i[11] = -(i[6] * m[9] + i[7] * m[10] + i[8] * m[11]);
    }
};
 
template <typename T>
const Transformation3D<T> Transformation3D<T>::identity_transformation;
 
}  // namespace b2000
 
#endif /* __B2REFERENTIAL_H__ */