<commit_before>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 <NAME> (<EMAIL>)
 * Copyright (c) 2014 <NAME> (<EMAIL>)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file orientation_state_transition_model.hpp
 * \date July 2015
 * \author <NAME> (<EMAIL>)
 */

#ifndef FL__UTIL__MATH__EULER_VECTOR_HPP
#define FL__UTIL__MATH__EULER_VECTOR_HPP


#include <Eigen/Dense>
#include <fl/util/types.hpp>

namespace fl
{

/// basic functionality for both vectors and blocks ****************************
template <typename Base>
class EulerBase: public Base
{
public:
    // types *******************************************************************
    typedef typename Base::Scalar           Scalar;
    typedef Eigen::Matrix<Scalar, 3, 1>     Axis;
    typedef Eigen::Matrix<Scalar, 3, 1>     Vector;
    typedef Eigen::AngleAxis<Scalar>        AngleAxis;
    typedef Eigen::Quaternion<Scalar>       Quaternion;
    typedef Eigen::Matrix<Scalar, 3, 3>     RotationMatrix;

    // constructor and destructor **********************************************
    EulerBase(const Base& vector): Base(vector) { }
    virtual ~EulerBase() {}

    // operators ***************************************************************
    template <typename T>
    void operator = (const Eigen::MatrixBase<T>& vector)
    {
        *((Base*)(this)) = vector;
    }

    // accessor ****************************************************************
    virtual Scalar angle() const
    {
        return this->norm();
    }
    virtual Axis axis() const
    {
        Axis  ax =  this->normalized();

        if(!std::isfinite(ax.sum()))
        {
            return Axis(1,0,0);
        }
        return ax;
    }
    virtual AngleAxis angle_axis() const
    {
        return AngleAxis(angle(), axis());
    }
    virtual Quaternion quaternion() const
    {
        return Quaternion(angle_axis());
    }
    virtual RotationMatrix rotation_matrix() const
    {
        return RotationMatrix(quaternion());
    }

    // mutators ****************************************************************
    virtual void angle_axis(const Scalar& angle, const Axis& axis)
    {
        *this = angle * axis;

        rescale();
    }
    virtual void angle_axis(const AngleAxis& ang_axis)
    {
        angle_axis(ang_axis.angle(), ang_axis.axis());
    }
    virtual void quaternion(const Quaternion& quat)
    {
        angle_axis(AngleAxis(quat));
    }
    virtual void rotation_matrix(const RotationMatrix& rot_mat)
    {
        angle_axis(AngleAxis(rot_mat));
    }
    virtual void rescale() // make sure the norm is in [0,Pi]
    {
        Scalar alpha = angle();

        if(alpha <= M_PI)
        {
            return;
        }
        if(alpha > 2 * M_PI)
        {
            alpha -= std::floor(alpha / (2 * M_PI)) * 2 * M_PI;
        }
        if(alpha > M_PI)
        {
            alpha -= 2 * M_PI;
        }

        (*this) = axis() * alpha;
    }
};


/// implementation for vectors *************************************************
class EulerVector: public EulerBase<Eigen::Matrix<Real, 3, 1>>
{
public:
    typedef EulerBase<Eigen::Matrix<Real, 3, 1>> Base;

    // constructor and destructor **********************************************
    EulerVector(): Base(Base::Zero()) { }

    template <typename T>
    EulerVector(const Eigen::MatrixBase<T>& vector): Base(vector) { }

    virtual ~EulerVector() {}

    // operators ***************************************************************
    EulerVector operator * (const EulerVector& factor)
    {
        EulerVector product;
        product.quaternion(this->quaternion() * factor.quaternion());
        return product;
    }

    // accessor ****************************************************************
    virtual EulerVector inverse() const
    {
        return - (*this);
    }
};


/// implementation for blocks **************************************************
template <typename Vector>
class EulerBlock: public EulerBase<Eigen::VectorBlock<Vector, 3>>
{
public:
    typedef Eigen::VectorBlock<Vector, 3> Block;
    typedef EulerBase<Block>              Base;

    using Base::operator=;

    // constructor and destructor **********************************************
    EulerBlock(const Block& block): Base(block) { }
    EulerBlock(Vector& vector, int start): Base(Block(vector, start)) { }
    virtual ~EulerBlock() {}
};



}

#endif<commit_msg>moved some functions from eulervector to eulerbase, such that they can also be used by blocks<commit_after>/*
 * This is part of the FL library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2014 <NAME> (<EMAIL>)
 * Copyright (c) 2014 <NAME> (<EMAIL>)
 *
 * Max-Planck Institute for Intelligent Systems, AMD Lab
 * University of Southern California, CLMC Lab
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file orientation_state_transition_model.hpp
 * \date July 2015
 * \author <NAME> (<EMAIL>)
 */

#ifndef FL__UTIL__MATH__EULER_VECTOR_HPP
#define FL__UTIL__MATH__EULER_VECTOR_HPP


#include <Eigen/Dense>
#include <fl/util/types.hpp>

namespace fl
{

/// basic functionality for both vectors and blocks ****************************
template <typename Base>
class EulerBase: public Base
{
public:
    // types *******************************************************************
    typedef typename Base::Scalar           Scalar;
    typedef Eigen::Matrix<Scalar, 3, 1>     Axis;
    typedef Eigen::Matrix<Scalar, 3, 1>     Vector;
    typedef Eigen::AngleAxis<Scalar>        AngleAxis;
    typedef Eigen::Quaternion<Scalar>       Quaternion;
    typedef Eigen::Matrix<Scalar, 3, 3>     RotationMatrix;

    typedef EulerBase<Eigen::Matrix<Real, 3, 1>> EulerVector;

    // constructor and destructor **********************************************
    EulerBase(const Base& vector): Base(vector) { }
    virtual ~EulerBase() {}

    // operators ***************************************************************
    template <typename T>
    void operator = (const Eigen::MatrixBase<T>& vector)
    {
        *((Base*)(this)) = vector;
    }

    // accessor ****************************************************************
    virtual Scalar angle() const
    {
        return this->norm();
    }
    virtual Axis axis() const
    {
        Axis  ax =  this->normalized();

        if(!std::isfinite(ax.sum()))
        {
            return Axis(1,0,0);
        }
        return ax;
    }
    virtual AngleAxis angle_axis() const
    {
        return AngleAxis(angle(), axis());
    }
    virtual Quaternion quaternion() const
    {
        return Quaternion(angle_axis());
    }
    virtual RotationMatrix rotation_matrix() const
    {
        return RotationMatrix(quaternion());
    }
    virtual EulerVector inverse() const
    {
        return EulerVector(-*this);
    }

    // mutators ****************************************************************
    virtual void angle_axis(const Scalar& angle, const Axis& axis)
    {
        *this = angle * axis;

        rescale();
    }
    virtual void angle_axis(const AngleAxis& ang_axis)
    {
        angle_axis(ang_axis.angle(), ang_axis.axis());
    }
    virtual void quaternion(const Quaternion& quat)
    {
        angle_axis(AngleAxis(quat));
    }
    virtual void rotation_matrix(const RotationMatrix& rot_mat)
    {
        angle_axis(AngleAxis(rot_mat));
    }
    virtual void rescale() // make sure the norm is in [0,Pi]
    {
        Scalar alpha = angle();

        if(alpha <= M_PI)
        {
            return;
        }
        if(alpha > 2 * M_PI)
        {
            alpha -= std::floor(alpha / (2 * M_PI)) * 2 * M_PI;
        }
        if(alpha > M_PI)
        {
            alpha -= 2 * M_PI;
        }

        (*this) = axis() * alpha;
    }

    // operators ***************************************************************
    template <typename T>
    EulerVector operator * (const EulerBase<T>& factor)
    {
        EulerVector product(EulerVector::Zero());
        product.quaternion(this->quaternion() * factor.quaternion());
        return product;
    }
};


/// implementation for vectors *************************************************
class EulerVector: public EulerBase<Eigen::Matrix<Real, 3, 1>>
{
public:
    typedef EulerBase<Eigen::Matrix<Real, 3, 1>> Base;

    // constructor and destructor **********************************************
    EulerVector(): Base(Base::Zero()) { }

    template <typename T>
    EulerVector(const Eigen::MatrixBase<T>& vector): Base(vector) { }

    virtual ~EulerVector() {}
};


/// implementation for blocks **************************************************
template <typename Vector>
class EulerBlock: public EulerBase<Eigen::VectorBlock<Vector, 3>>
{
public:
    typedef Eigen::VectorBlock<Vector, 3> Block;
    typedef EulerBase<Block>              Base;

    using Base::operator=;

    // constructor and destructor **********************************************
    EulerBlock(const Block& block): Base(block) { }
    EulerBlock(Vector& vector, int start): Base(Block(vector, start)) { }
    virtual ~EulerBlock() {}
};



}

#endif
{'fraction_non_alphanumeric': 0.23327437311414004, 'fraction_numerical': 0.006867131411923837, 'mean_word_length': 3.065989847715736, 'pattern_counts': {'":': 0, '<': 63, '<?xml version=': 0, '>': 71, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1993886', 'n_tokens_mistral': 2789, 'n_tokens_neox': 2594, 'n_words': 691}