/* Hector -- A Simple Climate Model
   Copyright (C) 2014-2015  Battelle Memorial Institute

   Please see the accompanying file LICENSE.md for additional licensing
   information.
*/
// ocean_csys_class.cpp : Defines the entry point for the console application.
/*  Ocean Carbon Chemistry CODE File:
 *
 *  Created by Corinne Hartin  1/30/13.

 *  This code translated from MATLAB code
 *  Reference: Richard E. Zeebe and Dieter A. Wolf-Gladrow

 *	Alfred Wegener Institute for
 *	Polar and Marine Research
 *	P.O. Box 12 01 61
 *   D-27515 Bremerhaven
 *	Germany
 *	e-mail: rzeebe@awi-bremerhaven.de   wolf@awi-bremerhaven.de
 *
 *   based on the book by Zeebe and Wolf-Gladrow (2001) CO2 in seawater: equilibrium, kintetics, isotopes. 346 p Amsterdam: Elsevier
 *   http://www.soest.hawaii.edu/oceanography/faculty/zeebe_files/CO2_System_in_Seawater/csys.html
 */

#include <math.h>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/tools/roots.hpp>

#include "h_exception.hpp"
#include "ocean_csys.hpp"

namespace Hector {

using namespace std;

//------------------------------------------------------------------------------
/*! \brief new oceanbox logger
 *  oceanbox logger may or may not be defined and therefore we check before logging
 */
#define CS_LOG(log, level)  \
if( log != NULL ) H_LOG( (*log), level )

//------------------------------------------------------------------------------
/*! \brief constructor
 */
oceancsys::oceancsys() : ncoeffs(6), m_a(ncoeffs) {
	logger = NULL;
	S = alk = As = Ks = 0.0;
}

//------------------------------------------------------------------------------
/*! \brief A helper functor class used to evauluate a polynomial and its derivative.
 *  \details This helper class gives an interface that is callable from boost's numerical
 *           solvers which need to evaluate a function and it's derivative.  This class
 *           wrapps a polynomical by taking an array of coefficients in ascending order of
 *           the degree of the term they are associated.
 */
class PolyDerivFunctor {
    public:
        PolyDerivFunctor(const double* coefs, const int degree) {
            using namespace boost::math::tools;
            const int size = degree + 1;
            mPoly = polynomial<double>(coefs, degree);
            double* derivCoef = new double[size-1];
            for(int i = 1; i < size; ++i) {
                derivCoef[i - 1] = coefs[i] * static_cast<double>(i);
            }
            mPolyDeriv = polynomial<double>(derivCoef, degree-1);
            delete[] derivCoef;
        }

        pair<double, double> operator()(const double x) {
            return pair<double, double>(mPoly.evaluate(x), mPolyDeriv.evaluate(x));
        }

    private:
        //! The representation of the polynomial to calculate.
        boost::math::tools::polynomial<double> mPoly;

        //! The representation of the derivative of the polynomial to calculate.
        boost::math::tools::polynomial<double> mPolyDeriv;
};

//------------------------------------------------------------------------------
/*! \brief Find the largest real root, using GSL or appropriate algorithms
 *  \param ncoeff   Number of coefficients
 *  \param *a       Coefficients
 *  \return         Largest real root (H+ ion)
 */
double find_largest_root( const int ncoeffs, double* a ) {

    using namespace boost::math::tools;
    const int degree = ncoeffs-1;
    PolyDerivFunctor polyFunctor(a, degree);
    // Use Fujiwara's method to find an upper bound for the roots of the polynomial
    double max = pow(std::abs(a[0] / ( 2.0 * a[degree])), 1.0 / degree);
    for(int i = 1; i < degree; ++i) {
        max = std::max(max, pow(std::abs(a[i]/a[degree]), 1.0 / static_cast<double>(degree - i)));
    }
    max *= 2.0;
    // Use Newton's method to find the largest real root starting from the Fujiwara upper bound
    // arbitrarily solve unil 60% of the digits are correct.
    const int digits = numeric_limits<double>::digits;
    int get_digits = static_cast<int>(digits * 0.6);
    double h = newton_raphson_iterate(polyFunctor, max-0.001, 0.0, max, get_digits);

	return h;
}

//------------------------------------------------------------------------------
/*! \brief Run Ocean csys
 *
 * DIC and ALK calculate pH, pCO2, omega Ar, omega Ca
 * (from Zeebe and Wolfe-Gladrow 2001)
 * pCO2 is used to calculate ocean-atmosphere fluxes
 * (from Takahashi et al, 2009, eq. 7 & 8)
 */
void oceancsys::ocean_csys_run( unitval tbox, unitval carbon )
{

	double tmp, tmp1, tmp2, tmp3;

    // Convert carbon to dic value and temperature to K
    const double dic = convertToDIC( carbon ).value( U_UMOL_KG )/1e6;   // back to mol/kg
    const double Tc = tbox.value( U_DEGC );
    const double Tk = Tc + 273.15;

	// Check that all is OK with input data
	H_ASSERT( Tk > 265 && Tk < 308, "bad Tk value" ); // Kelvin
    H_ASSERT( dic > 1000e-6 && dic < 3700e-6, "bad dic value" );  // mol/kg

    // alk should be constant once spinup is done, but check anyway
    H_ASSERT( alk >= 2000e-6 && alk <= 2750e-6, "bad alk value" );  // mol/kg

	/*---------------------------------------------------------------
     This section calculates the constants K0, Sc, K1, K2, Ksp, Ksi etc.
     ---------------------------------------------------------------*/

	// --------------------- K0 -----------------------------------
	// solubility of CO2 calculated from Weiss 1974 (mol * L-1 * atm-1)
	// used to calculate CO2 fluxes
	tmp1 = -58.0931 + 90.5069* ( 100/Tk ) + 22.2940 * log( Tk/100 );
	tmp2 = S * ( 0.027766 - 0.025888 * ( Tk/100 ) + 0.0050578 * ( ( Tk/100 ) * ( Tk/100 ) ) );
	const double lnK0 =  tmp1 + tmp2;
	K0.set( exp( lnK0 ), U_MOL_L_ATM );

	//---------------------Sc------------------------------------------
	// Schmidt Number from Wanninkhof 1992
	const double Sc = 2073.1 - ( 125.62 * Tc ) + (3.6276 * Tc * Tc) - ( 0.043219 * Tc * Tc * Tc );

	// --------------------- Kwater -----------------------------------
	// table 1.1 in Part1: Seawater carbonate chemistry Andrew Dickson
	// Millero (1995)(in Dickson and Goyet (1994, Chapter 5, p.18))
	tmp1 = -13847.26/Tk + 148.96502 - 23.6521 * log( Tk );
	tmp2 = + (118.67/Tk - 5.977 + 1.0495*log( Tk ) ) * sqrt( S ) - 0.01615 * S;
	const double lnKw =  tmp1 + tmp2;
	Kw.set( exp(lnKw), U_MOL_KG);

	//---------------------- Kh (K Henry) ----------------------------
	// solubility of CO2 calculated from Weiss 1974 (mol*kg-1*atm-1)
	// Kh and K0 are identical equations with differing constants resulting in different units
	// used to calculate pCO2
	tmp = 9345.17 / Tk - 60.2409 + 23.3585 * log( Tk/100 );
	const double nKhwe74 = tmp + S * ( 0.023517-0.00023656 * Tk + 0.0047036e-4 * Tk * Tk );
	Kh.set( exp( nKhwe74 ), U_MOL_KG_ATM);

	// --------------------- K1 ---------------------------------------
	//   Mehrbach et al (1973) refit by Lueker et al. (2000).
	const double pK1mehr = 3633.86/Tk - 61.2172 + 9.6777*log( Tk ) - 0.011555 * S + 0.0001152 * S * S;
	const unitval K1( pow( 10, -pK1mehr ), U_MOL_KG);

	// --------------------- K2 ----------------------------------------
	//   Mehrbach et al. (1973) refit by Lueker et al. (2000).
	const double pK2mehr = 471.78/Tk + 25.9290 - 3.16967 * log( Tk ) - 0.01781 * S + 0.0001122 * S * S;
	const unitval K2( pow( 10.0, -pK2mehr ), U_MOL_KG);

	// --------------------- Kb  --------------------------------------------
	// boric acid DOE 1994
	tmp1 =  ( -8966.90-2890.53 * sqrt( S ) - 77.942 * S+ 1.728*pow( S,( 3.0/2.0 ) ) - 0.0996 * S * S )/Tk;
	tmp2 =   +148.0248+137.1942 * sqrt( S ) + 1.62142 * S;
	tmp3 = +(-24.4344-25.085 * sqrt( S )-0.2474 * S ) * log( Tk ) + 0.053105 * sqrt( S ) * Tk;
	const double lnKb = tmp1 + tmp2 + tmp3;
	const unitval Kb( exp(lnKb), U_MOL_KG);

	// --------------------- Kspc (calcite) ----------------------------
	// Mucci, Alphonso, Amer. J. of Science 283:781-799, 1983
	tmp1 = -171.9065-0.077993 * Tk + 2839.319/Tk + 71.595 * log10( Tk );
	tmp2 = +( -0.77712+0.0028426 * Tk + 178.34/Tk ) * sqrt( S );
	tmp3 = -0.07711 * S + 0.0041249 * pow( S, 1.5 );
	const double log10Kspc = tmp1 + tmp2 + tmp3;
	const double Kspc = pow( 10.0, log10Kspc ); // mol/kg

	// --------------------- Kspa (aragonite) ----------------------------
	// Mucci, Alphonso, Amer. J. of Science 283:781-799, 1983
	tmp1 = -171.945 - 0.077993 * Tk + 2903.293 / Tk + 71.595 * log10( Tk );
	tmp2 = +( -0.068393+0.0017276 * Tk + 88.135/Tk ) * sqrt( S );
	tmp3 = -0.10018 * S + 0.0059415 * pow( S, 1.5 );
	const double log10Kspa = tmp1 + tmp2 + tmp3;
	const double Kspa = pow( 10.0, log10Kspa ); // mol/kg

	//------------------------- boron --------------------------------------
	// total boron concentration
	// DOE 1994
	const double bor = 1 * ( 416.0 * ( S/35.0 ) ) * 1.e-6;   // (mol/kg), DOE94

	/* ---------------------------------------
     ALK and DIC given solve for pH and pCO2
     ------------------------------------------*/

    const double Kb_val = Kb.value( U_MOL_KG );     // for convenience in eqns below
    const double K1_val = K1.value( U_MOL_KG );
    const double K2_val = K2.value( U_MOL_KG );
    const double Kw_val = Kw.value( U_MOL_KG );

	const double p5 = -1.0;
	const double p4 = -alk - Kb_val - K1_val;
	const double p3 = dic * K1_val - alk * ( Kb_val + K1_val )
        + Kb_val * bor + Kw_val - Kb_val
        * K1_val - K1_val * K2_val;
	tmp = dic * ( Kb_val * K1_val + 2.0 * K1_val * K2_val )
        -alk * (Kb_val * K1_val + K1_val * K2_val)
        + Kb_val * bor * K1_val;
	const double p2 = tmp + ( Kw_val * Kb_val + Kw_val * K1_val - Kb_val * K1_val * K2_val );
	tmp = 2.0 * dic * Kb_val * K1_val * K2_val
        - alk * Kb_val * K1_val * K2_val + Kb_val
        * bor * K1_val * K2_val;
	const double p1 = tmp + ( Kw_val * Kb_val * K1_val + Kw_val * K1_val * K2_val );
	const double p0 = Kw_val * Kb_val * K1_val * K2_val;

	m_a[ 0 ] = p0;
	m_a[ 1 ] = p1;
	m_a[ 2 ] = p2;
	m_a[ 3 ] = p3;
	m_a[ 4 ] = p4;
	m_a[ 5 ] = p5;

	const double h      = find_largest_root( ncoeffs, &m_a[0] );

	const double co2st      = dic/( 1.0 + K1_val / h + K1_val * K2_val / h / h ); // co2st = CO2*
	const double hco3   = dic/( 1.0 + h / K1_val + K2_val / h );
	const double co3    = dic/( 1.0 + h / K2_val + h * h / K1_val / K2_val ); // mol/kg

	const double million = 1e6; // unit conversion

	// Output (all variables beginning with capital letter below)
	TCO2o.set( co2st * million, U_UMOL_KG );
	HCO3.set( hco3 * million, U_UMOL_KG );
	CO3.set( co3 * million, U_UMOL_KG );
	PCO2o.set ( co2st * million/Kh.value( U_MOL_KG_ATM ), U_UATM );
	pH.set (-log10( h ), U_PH);

    // ----------------------------------------------------------------------------
    /*! \brief calculate air-sea flux of carbon
     * based on Takahashi et al, 2009 Deep Sea Research
     * Uses K0 (solubility), Sc (Schmidt number) , U (wind stress), PCO2atm, PCO2o
     */

	Tr.set( ( 0.585 * K0.value( U_MOL_L_ATM )
             * pow( Sc, -0.5 ) * U * U ), U_gC_m2_month_uatm );  // units : gC m-2 month-1 uatm-1.
	// 0.585 is a unit conversion factor. See Takahashi et al, 2009 page 568
	// unit conversion * solubility * Schmidt number * wind speed^2

    //------------------------------------------------------------------------
    /*! \brief calculate Omega of Ca/Ar
     * Uses Ksp of Ca and Ar, CO3, S, and pH
     */

	// this is 0.010285*S/35
	const double calcium = 0.02128/40.087 * ( S/1.80655 ); //mol/kg Riley, and Tongudai, Chemical Geology 2:263-269, 1967
	OmegaCa.set( ( ( co3 * calcium ) / Kspc ), U_UNITLESS );
	OmegaAr.set( ( ( co3 * calcium ) / Kspa ), U_UNITLESS );
}

//-------------------------------------------------------------------------------
/*! \brief Calculate the (monthly) atmosphere-surface box flux
 *  \param Ca           Atmospheric CO2
 *  \param cpoolscale   Scale the box C pool by this amount (1.0=none)
 *  \return             Monthly atmospheric C flux, gC/m2/month
 */
double oceancsys::calc_monthly_surface_flux( const unitval& Ca, const double cpoolscale ) const {
	return ( ( Ca.value( U_PPMV_CO2 ) - PCO2o.value( U_UATM ) * cpoolscale ) * Tr.value( U_gC_m2_month_uatm ) ); // units : gC m-2 month-1
}

//-------------------------------------------------------------------------------
/*! \brief Calculate the (annualized) atmosphere-surface box flux
 *  \param Ca           Atmospheric CO2
 *  \param cpoolscale   Scale the box C pool by this amount (1.0=none)
 *  \return             Annual atmospheric C flux, Pg C/yr
 */
unitval oceancsys::calc_annual_surface_flux( const unitval& Ca, const double cpoolscale ) const {
    return unitval( ( calc_monthly_surface_flux( Ca, cpoolscale ) * As * 12.0 ) / 1e15, U_PGC_YR );
}

//-------------------------------------------------------------------------------
/*! \brief Convert the total carbon pool (PgC) to DIC
 *  \param carbon       Carbon value to convert (Pg C)
 * Uses carbon pool, mass of carbon, density of seawater and volume of the box
 */
unitval oceancsys::convertToDIC( const unitval carbon ) {
	const double dic = ( ( carbon.value( U_PGC ) * 1e15 ) * ( 1.0/12.01 ) * (1.0/1027.0 ) * ( 1.0/volumeofbox ) ); // mol/kg
	return unitval( dic * 1e6, U_UMOL_KG );
}

}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <dpMM/basemeasure.hpp>
#include <dpMM/vmfPriorFull.hpp>

/*
 * vmf base measure; uses monte carlo integration for p(x|hyperparams)
 * http://eprints.pascal-network.org/archive/00007206/01/iMMM.pdf
 */
template<typename T>
class vMFbase : public BaseMeasure<T>
{
public:
  vMFbase(const vMFpriorFull<T>& vmfPrior);
  vMFbase(const vMFbase<T>& vmf);
  ~vMFbase();

  virtual BaseMeasure<T>* copy();
  virtual vMFbase<T>* copyNative();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;
  T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const VectorXu& z,
    uint32_t k);
  void sample();

  T logPdfUnderPrior() const;
  virtual T logPdfUnderPriorMarginalized() const;

  virtual T logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x);

//  virtual NiwSampled<T>* merge(const NiwSampled<T>& other);
//  void fromMerge(const NiwSampled<T>& niwA, const NiwSampled<T>& niwB);

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(vmf_.D_));};

//  const Matrix<T,Dynamic,Dynamic>& scatter() const {return niw0_.scatter();};
//  const Matrix<T,Dynamic,1>& mean() const {return niw0_.mean();};
//  T count() const {return niw0_.count();};
////  T& count() {return niw0_.count_;};
  const Matrix<T,Dynamic,1>& getMean() const {return vmf_.mu_;};
  const T tau() const {return vmf_.tau();};

  vMFpriorFull<T> vmfPrior_;
  vMF<T> vmf_;
private:

};

// ------------------------- impl -------------------------------------------

template<typename T>
vMFbase<T>::vMFbase(const vMFpriorFull<T>& vmfPrior)
  : vmfPrior_(vmfPrior), vmf_(vmfPrior_.sample())
{};

template<typename T>
vMFbase<T>::vMFbase(const vMFbase<T>& base)
  :  vmfPrior_(base.vmfPrior_), vmf_(base.vmf_)
{};

template<typename T>
vMFbase<T>::~vMFbase()
{};

template<typename T>
BaseMeasure<T>* vMFbase<T>::copy()
{
  return new vMFbase<T>(*this);
};

template<typename T>
vMFbase<T>* vMFbase<T>::copyNative()
{
  return new vMFbase<T>(*this);
};

template<typename T>
T vMFbase<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
//  cout<<vmf_.logPdf(x)<<endl;
  return vmf_.logPdf(x);
};

template<typename T>
void vMFbase<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k)
{
  vmfPrior_.getSufficientStatistics(x,z,k);
  // needs current vmf since it samples tau|mu_old and then mu|tau
  vmf_ = vmfPrior_.sampleFromPosterior(vmf_);
};

template<typename T>
void vMFbase<T>::posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const VectorXu& z,
    uint32_t k)
{
};

template<typename T>
void vMFbase<T>::sample()
{
};

template<typename T>
void vMFbase<T>::print() const
{
  vmf_.print();
};

template<typename T>
T vMFbase<T>::logPdfUnderPrior() const
{
  return 0.;
};

template<typename T>
T vMFbase<T>::logPdfUnderPriorMarginalized() const
{
  return 0.;
};

template<typename T>
T vMFbase<T>::logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x)
{
  // approximate the log pdf under the prior via monte carlo sampling
  T logPdfMarg = 0;
  uint32_t N = 3;
//#pragma omp parallel for reduction(+:logPdfMarg)
  for(uint32_t t=0; t<N; ++t)
  {
    vMF<T> vmf = vmfPrior_.sample();
    logPdfMarg = logPdfMarg + vmf.logPdf(x);
  }
  return logPdfMarg/T(N);
};


/**
 * @file Model.hpp
 * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved.
 */

#ifndef FLEXIVRDK_MODEL_HPP_
#define FLEXIVRDK_MODEL_HPP_

#include <Eigen/Eigen>
#include <memory>
#include <vector>

namespace flexiv {

class ModelHandler;

/**
 * @class Model
 * @brief Integrated dynamics engine with robot model and dynamics.
 */
class Model
{
public:
    Model();
    virtual ~Model();

    /**
     * @brief Update robot model using new joint states data
     * @param[in] positions \f$ \mathbb{R}^{Dof \times 1} \f$ new link positions
     * \f$ q~[rad] \f$
     * @param[in] velocities \f$ \mathbb{R}^{Dof \times 1} \f$ new link
     * velocities \f$ \dot{q}~[rad/s] \f$
     * @return True: success, false: failed
     */
    bool updateModel(const std::vector<double>& positions,
        const std::vector<double>& velocities);

    /**
     * @brief Set tool configuration and add to robot model. The tool is
     * installed on the flange
     * @param[in] mass Total mass of the tool \f$ [kg] \f$
     * @param[in] inertiaAtCom \f$ \mathbb{R}^{3 \times 3} \f$ inertia matrix of
     * the tool at COM \f$ [kg \cdot m^2] \f$
     * @param[in] comInTcp \f$ \mathbb{R}^{3 \times 1} \f$ tool COM position in
     * TCP frame \f$ [m] \f$
     * @param[in] tcpInFlange \f$ \mathbb{R}^{3 \times 1} \f$ TCP position in
     * flange frame \f$ [m] \f$
     * @return True: success, false: failed
     */
    bool setTool(double mass, const Eigen::Matrix3d& inertiaAtCom,
        const Eigen::Vector3d& comInTcp, const Eigen::Vector3d& tcpInFlange);

    /**
     * @brief Compute and get the Jacobian matrix at the frame of the specified
     * link \f$ i \f$, expressed in the base frame.
     * @param[in] linkName Name of the link to get Jacobian for
     * @return \f$ \mathbb{R}^{6 \times Dof} \f$ Jacobian matrix, \f$ ^0 J_i \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     * @note Available links can be found in the provided URDF. They are
     * {"base_link", "link1", "link2", "link3", "link4", "link5", "link6",
     * "link7", "flange"}, plus "tool" after setTool() is called
     */
    const Eigen::MatrixXd getJacobian(const std::string& linkName);

    /**
     * @brief Compute and get the time derivative of Jacobian matrix at the
     * frame of the specified link \f$ i \f$, expressed in the base frame.
     * @param[in] linkName Name of the link to get Jacobian derivative for
     * @return \f$ \mathbb{R}^{6 \times Dof} \f$ Time derivative of Jacobian
     * matrix, \f$ ^0 \dot{J_i} \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     * @note Available links can be found in the provided URDF. They are
     * {"base_link", "link1", "link2", "link3", "link4", "link5", "link6",
     * "link7", "flange"}, plus "tool" after setTool() is called
     */
    const Eigen::MatrixXd getJacobianDot(const std::string& linkName);

    /**
     * @brief Compute and get the mass matrix for the generalized coordinates,
     * i.e. joint space
     * @return \f$ \mathbb{S}^{Dof \times Dof}_{++} \f$ Symmetric positive
     * definite mass matrix \f$ M(q)~[kgm^2] \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     */
    const Eigen::MatrixXd getMassMatrix();

    /**
     * @brief Compute and get the Coriolis/centripetal matrix for the
     * generalized coordinates, i.e. joint space
     * @return \f$ \mathbb{R}^{Dof \times Dof} \f$ Coriolis/centripetal matrix
     * \f$ C(q,\dot{q}) \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     * @par Coriolis matrix factorization
     * The factorization of the Coriolis matrix C is not unique, and this API
     * is using the factorization method found in "A new Coriolis matrix
     * factorization", 2012 by M. Bjerkend and K. Pettersen
     */
    const Eigen::MatrixXd getCoriolisMatrix();

    /**
     * @brief Compute and get the gravity force vector for the generalized
     * coordinates, i.e. joint space
     * @return \f$ \mathbb{R}^{Dof \times 1} \f$ gravity force vector \f$
     * g(q)~[Nm] \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     */
    const Eigen::VectorXd getGravityForce();

    /**
     * @brief Compute and get the Coriolis force vector for the generalized
     * coordinates, i.e. joint space
     * @return \f$ \mathbb{R}^{Dof \times 1} \f$ Coriolis force vector \f$
     * c(q,\dot{q})~[Nm] \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     */
    const Eigen::VectorXd getCoriolisForce();

    friend class RobotClientHandler;

private:
    std::unique_ptr<ModelHandler> m_handler;
};

} /* namespace flexiv */

#endif /* FLEXIVRDK_MODEL_HPP_ */


#ifndef KALMAN_TRACKER_HPP
#define KALMAN_TRACKER_HPP

#include <Eigen/Dense>
#include <boost/any.hpp>

#include <ros/ros.h>

#include <kkl/math/gaussian.hpp>
#include <kkl/alg/kalman_filter.hpp>

namespace hdl_people_tracking {

/**
 * @brief Kalman filter-based tracker with a constant velocity model
 */
class KalmanTracker {
  typedef kkl::alg::KalmanFilter<double, 6, 2, 3> KalmanFilter;
public:
  /**
   * @brief constructor
   * @param id            tracker ID
   * @param time          timestamp
   * @param init_pos      initial position
   * @param associated    associated detection
   */
  KalmanTracker(long id, const ros::Time& time, const Eigen::Vector3d& init_pos, boost::any associated = boost::any())
    : id_(id),
      correction_count(0),
      init_time(time),
      last_prediction_time(time),
      last_correction_time(time),
      last_associated(associated)
  {
    Eigen::Matrix<double, 6, 6> transition = Eigen::Matrix<double, 6, 6>::Identity();
    Eigen::Matrix<double, 6, 2> control = Eigen::Matrix<double, 6, 2>::Zero();
    Eigen::Matrix<double, 3, 6> measurement = Eigen::Matrix<double, 3, 6>::Zero();
    measurement.block<3, 3>(0, 0).setIdentity() * 0.2;

    Eigen::Matrix<double, 6, 6> process_noise = Eigen::Matrix<double, 6, 6>::Zero();
    process_noise.topLeftCorner(3, 3) = Eigen::Matrix3d::Identity() * 0.03;
    process_noise.bottomRightCorner(3, 3) = Eigen::Matrix3d::Identity() * 0.01;
    Eigen::Matrix3d measurement_noise = Eigen::Matrix3d::Identity() * 0.2;

    Eigen::Matrix<double, 6, 1> mean = Eigen::Matrix<double, 6, 1>::Zero();
    mean.head<3>() = init_pos;
    Eigen::Matrix<double, 6, 6> cov = Eigen::Matrix<double, 6, 6>::Identity() * 0.1;

    kalman_filter.reset(new KalmanFilter(transition, control, measurement, process_noise, measurement_noise, mean, cov));
  }
  ~KalmanTracker() {}

  using Ptr = std::shared_ptr<KalmanTracker>;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
  /**
   * @brief predict the current state
   * @param time    current time
   */
  void predict(const ros::Time& time) {
    double difftime = (time - last_prediction_time).toSec();
    difftime = std::max(0.001, difftime);

    kalman_filter->transitionMatrix(0, 3) = difftime;
    kalman_filter->transitionMatrix(1, 4) = difftime;
    kalman_filter->transitionMatrix(2, 5) = difftime;

    kalman_filter->predict(Eigen::Matrix<double, 2, 1>::Zero());
    last_prediction_time = time;

    last_associated = boost::any();
  }

  /**
   * @brief correct the state with an observation
   * @param time    current time
   * @param pos     observed position
   * @param associated   associated detection
   */
  void correct(const ros::Time& time, const Eigen::Vector3d& pos, boost::any associated = boost::any()) {
    kalman_filter->correct(pos);

    correction_count++;
    last_correction_time = time;
    last_associated = associated;
  }

public:
  long id() const {
    return id_;
  }

  ros::Duration age(const ros::Time& time) const {
    return (time - init_time);
  }

  const ros::Time& lastCorrectionTime() const {
    return last_correction_time;
  }

  const boost::any& lastAssociated() const {
    return last_associated;
  }

  Eigen::Vector3d position() const {
    return kalman_filter->mean.head<3>();
  }

  Eigen::Vector3d velocity() const {
    return kalman_filter->mean.tail<3>();
  }

  Eigen::Matrix3d positionCov() const {
    return kalman_filter->cov.block<3, 3>(0, 0);
  }

  Eigen::Matrix3d velocityCov() const {
    return kalman_filter->cov.block<3, 3>(3, 3);
  }

  double squaredMahalanobisDistance(const Eigen::Vector3d& p) const {
    return kkl::math::squaredMahalanobisDistance<double, 3>(
          kalman_filter->mean.head<3>(),
          kalman_filter->cov.block<3, 3>(0, 0),
          p);
  }

  int correctionCount() const {
    return correction_count;
  }

private:
  long id_;

  int correction_count;
  ros::Time init_time;              // time when the tracker was initialized
  ros::Time last_prediction_time;   // tiem when prediction was performed
  ros::Time last_correction_time;   // time when correction was performed

  boost::any last_associated;       // associated detection data

  std::unique_ptr<KalmanFilter> kalman_filter;
};

}

#endif // KALMANTRACKER_HPP


/*
Copyright (c) 2017 Ryoichi Ishikawa. All rights reserved.

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

#include "FloorDetector.h"
#include <Eigen\Eigen>
#include <Eigen\Core>
#include <Eigen\Dense>
#include "pch.h"
void FloorDetection(Platform::Array<unsigned char>^ buffer,int rowpitch,int height,double scale,double& HoloHeight, Eigen::Vector3d& floorpt) {
	//Buffer 2 Array

	float * imageData = (float*)buffer->Data;

	//get plane and Height
	//around 1m x 1m
	int dwidth = 1.0 / scale;
	//ransac
	int ransac_max = 100;
	std::vector<Eigen::Vector3d> points;
	std::vector<unsigned int> idces;
	Eigen::Vector3d bestn, bestp;
	bestn << 1, 1, 1;
	unsigned int cnt = 0;
	int maxcnt = -1;
	for (int x = rowpitch / sizeof(float) / 2;x<rowpitch / 2 / sizeof(float) + dwidth;x++) {
		for (int y = height / 2;y<height / 2 + dwidth;y++) {
			Eigen::Vector3d p_temp;
			p_temp << x*scale, y*scale, imageData[x + y*(rowpitch / sizeof(float))] * 3.0;
			points.push_back(p_temp);
			idces.push_back(cnt);
			cnt++;
		}
	}
	std::vector<unsigned int> bestlist;
	for (int ransac_t = 0;ransac_t<ransac_max;ransac_t++) {
		random_shuffle(idces.begin(), idces.end());
		std::vector<unsigned int> candlist;
		Eigen::Vector3d v01, v02, nfloor, cand_p;
		cand_p = points.at(idces.at(0));
		v01 = points.at(idces.at(1)) - cand_p;
		v02 = points.at(idces.at(2)) - cand_p;
		nfloor = v01.cross(v02);
		nfloor = nfloor.normalized();
		int inlcnt = 0;
		for (int idx = 3;idx<idces.size();idx++) {
			Eigen::Vector3d targp = points.at(idces.at(idx)) - cand_p;

			double err = abs(targp.dot(nfloor));
			if (err<0.005) {
				//inlier
				candlist.push_back(idces.at(idx));
				inlcnt++;
			}
		} if (maxcnt<inlcnt) {
			maxcnt = inlcnt;
			bestn = nfloor;
			bestp = cand_p;
			bestlist = std::vector<unsigned int>(candlist);
		}
	}
	//plane fitting
	//solve least square problem
	//ax+by+z+d=0: ax+by+d=-z
	Eigen::MatrixXd A(bestlist.size(), 3);
	Eigen::VectorXd B(bestlist.size());
	for (int idx = 0;idx<bestlist.size();idx++) {
		Eigen::Vector3d targp = points.at(idces.at(idx));
		A(idx, 0) = targp(0);//x
		A(idx, 1) = targp(1);//y
		A(idx, 2) = 1;//1
		B(idx) = -targp(2);//-z
	}
	Eigen::Vector3d ansX = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

	//rendering: 1.0m upper from hololens - 2.0m lower from hololens (3.0m range)
	//hololens point (scale*(dwidth/2),scale*(dwidth/2),1.0)
	//ax+by+z+d=0: n<<a,b,1
	Eigen::Vector3d phl;
	bestn << ansX(0), ansX(1), 1;
	bestp << scale*(rowpitch / sizeof(float) / 2), scale*(rowpitch / sizeof(float) / 2), -(ansX(0) + ansX(1))*scale*(rowpitch / sizeof(float) / 2) - ansX(2);
	bestn = bestn.normalized();
	if (bestn(2)<0)bestn = -bestn;

	//rendering: 1.0m upper from hololens - 2.0m lower from hololens (3.0m range)
	//hololens point (scale*(dwidth/2),scale*(dwidth/2),1.0)
	phl << scale*(rowpitch / sizeof(float) / 2), scale*(rowpitch / sizeof(float) / 2), 1.0;
	HoloHeight = -(phl - bestp).dot(bestn);
	floorpt = -HoloHeight*bestn;//hololens 2 floor

};


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARITHMETIC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-arithmetic Arithmetic functions

    These functions provide scalar and SIMD algorithms for classical arithmetic operators and
    functions of the C and C++ standard library. Other functions are also provided, in particular,
    provision for saturated operations through the use of a @ref group-decorator.

    All these functions can be included individually or all of them just by including
    <boost/simd/arithmetic.hpp>

     - **Possibly saturated operations**

       The functors:

         |                  |                 |                 |           |              |
         |:----------------:|:---------------:|:---------------:|:---------:|:------------:|
         | @ref abs         | @ref dec        | @ref dist       | @ref inc  | @ref minus   |
         | @ref multiplies  | @ref plus       | @ref oneminus   | @ref sqr  | @ref toint   |
         | @ref unary_minus | @ref unary_plus | @ref touint     |           |              |

       can be decorated with `saturated_` (see @ref group-decorator). This decorator
       has no effect on floating calls,  but on integer calls replaces the operation by its
       saturated equivalent.

       Typically: overflows will be replaced by the @ref Valmin/@ref Valmax proper value
       instead of providing undefined behaviour (for signed integral types) or wrapping
       modulo @ref Valmax + 1 (for unsigned ones).

       Peculiarly saturated_(@ref abs) and saturated_(@ref dist) ensure that the result will
       never be stricly negative (which is for instance the case of `abs(Valmin<T>())` for
       @c T being any signed integral type).

       @ref toint is a rather common operation as it converts floating number to signed integers
       of the same bit size, nevertheless it probably is its saturated version you have to use
       because it acts properly on large or not finite values, this is why an alias for
       `saturated_(toint)` is provided as @ref ifix.

       @par Example:

          @snippet saturated_abs.cpp saturated_abs

       @par Possible output:

          @snippet saturated_abs.txt saturated_abs_results

     - **Rounding operations**

         |                 |                 |              |                  |
         |:---------------:|:---------------:|:------------:|:----------------:|
         | @ref ceil       |  @ref fix       | @ref floor   |  @ref iceil      |
         | @ref ifix       |  @ref ifloor    | @ref iround  |  @ref itrunc     |
         | @ref inearbyint |  @ref nearbyint | @ref round   |  @ref trunc      |

          - The operations prefixed by 'i' return a value of the integral type iT
          associated to the entry type. (If T is the entry type iT is
          @c as_integer_t<T>)

          - The other ones return the same type as the entry.

        @par Example:

          @snippet roundings.cpp roundings

        @par Possible output:

          @snippet roundings.txt roundings_results

     - **Division operations**

        @ref divides is the function associated to standard division. There is another one
        which provides more flexibility, namely rounded divisions.

        With two parameters @ref div and @ref divides are equivalent, but @ref div can admit
        a first option parameter that modifies its behaviour.

         | option          |          call           |      result similar to           |
         |-----------------|-------------------------|----------------------------------|
         | @ref ceil       |   div(ceil, a, b)       |      T(ceil(fT(a)/fT(b)))        |
         | @ref floor      |   div(floor, a, b)      |      T(floor(fT(a)/fT(b)))       |
         | @ref fix        |   div(fix, a, b)        |      T(fix(fT(a)/fT(b)))         |
         | @ref round      |   div(round, a, b)      |      T(round(fT(a)/fT(b)))       |
         | @ref nearbyint  |   div(nearbyint, a, b)  |      T(nearbyint(fT(a)/fT(b)))   |
         | @ref iceil      |   div(iceil, a, b)      |      iT(iceil(fT(a)/fT(b)))      |
         | @ref ifloor     |   div(ifloor, a, b)     |      iT(ifloor(fT(a)/fT(b)))     |
         | @ref ifix       |   div(ifix, a, b)       |      iT(ifix(fT(a)/fT(b)))       |
         | @ref iround     |   div(iround, a, b)     |      iT(iround(fT(a)/fT(b)))     |
         | @ref inearbyint |   div(inearbyint, a, b) |      iT(inearbyint(fT(a)/fT(b))) |

           - The option parameter is described in the above table where a and b are of type T,
             fT is a supposed floating type associated to T (`as_floating_t<T>` if it
             exists) and iT is the integer type associated to T (`as_integer_t<T>`).
             (fT and iT are here only to support pseudo code description)

           @par Example:

              @snippet divisions.cpp divisions

           @par Possible output:

              @snippet divisions.txt divisions_results

     - **Remainder operations**

       @ref rem is the remainder functor providing same kind of facilities as @ref div

       With two parameters rem(a, b) is equivalent to  @c rem(fix, a, b), but @c rem can admit
       a first optional parameter that modifies its behaviour and moreover can use the
       pedantic_ decorator to assure some limiting case values (see below).

       The option parameter can be chosen between @ref ceil, @ref floor, @ref fix, @ref round,
       @ref nearbyint and if @c opt is the option, the call:

          @c rem(opt, a, b) is equivalent to  @c a-b*div(opt, a, b)

       For floating entries the underlisted corner cases are handled in the following way:
        - if  @c x is \f$\pm\infty\f$ , @ref Nan is returned
        - if  @c x is \f$\pm0\f$ and  @c y is not  @c 0  @c x is returned if  @c pedantic_
          is used (else  @c 0: the sign bit is not preserved)
        - if  @c y is \f$\pm0\f$, @ref Nan is returned
        - if either argument is a nan,  a nan is returned

       @par Example:

          @snippet remainders.cpp remainders

       @par Possible output:

          @snippet remainders.txt remainders_results

     - **complex operations**

       Boost.SIMD  does not provides complex number operations yet, but it will soon. So
       the following functors that have a meaning as a restriction to real number of complex
       functions, can be seen as a prequel:

        |           |             |             |             |                 |
        |:---------:|:-----------:|:-----------:|:-----------:|:---------------:|
        | @ref arg  | @ref conj   | imag        | real        | @ref sqr_abs    |

        For real entries @ref conj and real are identity, imag always 0,
        @ref sqr_abs coincide with @ref sqr and @ref arg results are always in the
        set \f$\{0, \pi,  Nan\}\f$

     - **Fused multiply-add operations**

        |                 |              |               |                  |
        |:---------------:|:------------:|:-------------:|:----------------:|
        | @ref fma        | @ref fnma    |  @ref two_add | @ref two_split   |
        | @ref fms        | @ref fnms    |  @ref two_prod|                  |

      These operations take three parmeters and compute some \f$\pm a * b \pm c \f$
      kind of expression, "n" standing for negate the result, "a" for add,
      "s" for substract and "m" for multiply.

      Correct fused multiply/add implies

      - only one rounding
      - no "intermediate" overflow

      The functions of this family provide this, BUT ONLY each time it is reasonable
      in terms of performance (mainly if the system has the hard
      wired capability).

      If you need "real" fused multiply-add capabilities in all circumstances in your own
      code you can use @c pedantic_(fma) (although it can be expansive) or
      @c std_(fma) (generally still more expansive) by using the decorators.

      @ref two_add, @ref two_prod and @ref two_split are used internally in @c pedantic_(fma)
      and can be useful in searching extra-accuracy in other circumstances as double-double
      computations.

      @c pedantic_(fma) is never used internally by Boost.SIMD

     - **Standard operations**

       The stdlibc++ provides them but only in scalar mode:

         |               |                 |              |             |
         |:-------------:|:---------------:|:------------:|:-----------:|
         | @ref abs      | @ref ceil       | @ref floor   | @ref fma    |
         | @ref hypot    | @ref max        | @ref maxnum  | @ref min    |
         | @ref minnum   | @ref rem (%)    | @ref remquo  | @ref round  |
         | @ref signbit  | @ref sqrt       |              |             |

       Boost.SIMD provides its own scalar and simd versions, but allows
       the use of the @c std_ @ref group-decorator to call the associated system
       library function if the user needs it.

     - **Other operations**

         |              |                 |               |              |              |
         |:------------:|:---------------:|:-------------:|:------------:|:------------:|
         | @ref average | @ref clamp      | @ref meanof   | @ref minmod  | @ref sqr     |
         | @ref sqrt    | @ref sqrt1pm1   | @ref tenpower | @ref tofloat |              |

       @ref clamp is also provided in stdlibc++ for scalar mode, but only since C++17.
       For now, in Boost.SIMD, the pedantic_  decorated version ensures standard
       conformity for a Nan first parameter.
  **/
} }

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/arg.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/clamp.hpp>
#include <boost/simd/function/conj.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/extract.hpp>
#include <boost/simd/function/fix.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/fnma.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/hypot.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/ifloor.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/meanof.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minmod.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/quadrant.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/rem.hpp>
#include <boost/simd/function/remquo.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/function/signbit.hpp>
#include <boost/simd/function/sqr_abs.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt1pm1.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/function/two_add.hpp>
#include <boost/simd/function/two_prod.hpp>
#include <boost/simd/function/two_split.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/unary_plus.hpp>

#endif


// Rotations conversion library
// File: rot_conv.cpp
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <rot_conv/rot_conv.h>
#include <Eigen/Eigenvalues>
#include <cmath>

// Defines
#define M_2PI (2.0*M_PI)

// Rotations conversion namespace
namespace rot_conv
{
	// ########################
	// #### Rotation types ####
	// ########################

	// Euler angles constants
	const EulerAngles EulerAngles::Identity(0.0, 0.0, 0.0);

	// Fused angles constants
	const FusedAngles FusedAngles::Identity(0.0, 0.0, 0.0, true);

	// Tilt angles constants
	const TiltAngles TiltAngles::Identity(0.0, 0.0, 0.0);

	// ##########################################
	// #### Rotation checking and validation ####
	// ##########################################

	// Check and validate: Rotation matrix
	bool ValidateRotmat(Rotmat& R, double tol)
	{
		// Make a copy of the input
		Rotmat Rorig = R;

		// Find the closest orthogonal matrix to the input rotation matrix
		Rotmat nonOrth = R.transpose() * R;
		R *= Eigen::SelfAdjointEigenSolver<Rotmat>(nonOrth).operatorInverseSqrt();

		// Filter out invalid left hand coordinate systems
		if(R.determinant() < 0.0)
			R.setIdentity();

		// Return whether the rotation matrix was valid within the given tolerance
		return (R - Rorig).isZero(tol);
	}

	// Check and validate: Quaternion
	bool ValidateQuat(Quat& q, double tol, bool unique)
	{
		// Make a copy of the input
		Quat qorig = q;

		// Renormalise the quaternion
		double normsq = q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z();
		if(normsq <= 0.0)
		{
			q.w() = 1.0;
			q.x() = q.y() = q.z() = 0.0;
		}
		else
		{
			double norm = sqrt(normsq);
			q.w() /= norm;
			q.x() /= norm;
			q.y() /= norm;
			q.z() /= norm;
		}

		// Make the quaternion unique
		if(unique && q.w() < 0.0)
		{
			q.w() = -q.w();
			q.x() = -q.x();
			q.y() = -q.y();
			q.z() = -q.z();
		}

		// Return whether the quaternion was valid within the given tolerance
		return (fabs(q.w() - qorig.w()) <= tol && fabs(q.x() - qorig.x()) <= tol && fabs(q.y() - qorig.y()) <= tol && fabs(q.z() - qorig.z()) <= tol);
	}

	// Check and validate: Euler angles
	bool ValidateEuler(EulerAngles& e, double tol, bool unique)
	{
		// Make a copy of the input
		EulerAngles eorig = e;

		// Wrap the pitch to (-pi,pi] and then collapse it to the [-pi/2,pi/2] interval
		e.pitch += M_2PI*std::floor((M_PI - e.pitch) / M_2PI);
		if(fabs(e.pitch) > M_PI_2)
		{
			e.yaw += M_PI;
			e.pitch = (e.pitch >= 0.0 ? M_PI - e.pitch : -M_PI - e.pitch);
			e.roll += M_PI;
		}

		// Make the positive and negative gimbal lock representations unique
		if(unique)
		{
			double spitch = sin(e.pitch);
			if(fabs(spitch - 1.0) <= tol)
			{
				e.roll -= e.yaw;
				e.yaw = 0.0;
			}
			else if(fabs(spitch + 1.0) <= tol)
			{
				e.roll += e.yaw;
				e.yaw = 0.0;
			}
		}

		// Wrap yaw and roll to (-pi,pi]
		e.yaw += M_2PI*std::floor((M_PI - e.yaw) / M_2PI);
		e.roll += M_2PI*std::floor((M_PI - e.roll) / M_2PI);

		// Return whether the Euler angles were valid within the given tolerance
		return (fabs(e.yaw - eorig.yaw) <= tol && fabs(e.pitch - eorig.pitch) <= tol && fabs(e.roll - eorig.roll) <= tol);
	}

	// Check and validate: Fused angles
	bool ValidateFused(FusedAngles& f, double tol, bool unique)
	{
		// Make a copy of the input
		FusedAngles forig = f;

		// Wrap the angles to (-pi,pi]
		f.fusedYaw += M_2PI*std::floor((M_PI - f.fusedYaw) / M_2PI);
		f.fusedPitch += M_2PI*std::floor((M_PI - f.fusedPitch) / M_2PI);
		f.fusedRoll += M_2PI*std::floor((M_PI - f.fusedRoll) / M_2PI);

		// Coerce the L1 norm
		double L1Norm = fabs(f.fusedPitch) + fabs(f.fusedRoll);
		if(L1Norm > M_PI_2)
		{
			double scale = M_PI_2 / L1Norm;
			f.fusedPitch *= scale;
			f.fusedRoll *= scale;
		}

		// Make the representation unique if required
		if(unique)
		{
			double spitch = sin(f.fusedPitch);
			double sroll = sin(f.fusedRoll);
			double sineSum = spitch*spitch + sroll*sroll;
			if(sineSum >= 1.0 - tol)
				f.hemi = true;
			L1Norm = fabs(f.fusedPitch) + fabs(f.fusedRoll);
			if(L1Norm <= tol && !f.hemi)
				f.fusedYaw = 0.0;
		}

		// Return whether the fused angles were valid within the given tolerance
		return (fabs(f.fusedYaw - forig.fusedYaw) <= tol && fabs(f.fusedPitch - forig.fusedPitch) <= tol && fabs(f.fusedRoll - forig.fusedRoll) <= tol && f.hemi == forig.hemi);
	}

	// Check and validate: Tilt angles
	bool ValidateTilt(TiltAngles& t, double tol, bool unique)
	{
		// Make a copy of the input
		TiltAngles torig = t;

		// Wrap the angles to (-pi,pi]
		t.fusedYaw += M_2PI*std::floor((M_PI - t.fusedYaw) / M_2PI);
		t.tiltAxisAngle += M_2PI*std::floor((M_PI - t.tiltAxisAngle) / M_2PI);
		t.tiltAngle += M_2PI*std::floor((M_PI - t.tiltAngle) / M_2PI);

		// Handle the case of a negative tilt angle
		if(t.tiltAngle < 0.0)
		{
			t.tiltAxisAngle = (t.tiltAxisAngle > 0.0 ? -M_PI + t.tiltAxisAngle : M_PI + t.tiltAxisAngle);
			t.tiltAngle = -t.tiltAngle;
		}

		// Make the representation unique if required
		if(unique)
		{
			double ctilt = cos(t.tiltAngle);
			bool near0 = (fabs(ctilt - 1.0) <= tol);
			bool near180 = (fabs(ctilt + 1.0) <= tol);
			if(near0 || near180)
				t.tiltAxisAngle = 0.0;
			if(near180)
				t.fusedYaw = 0.0;
		}

		// Return whether the tilt angles were valid within the given tolerance
		return (fabs(t.fusedYaw - torig.fusedYaw) <= tol && fabs(t.tiltAxisAngle - torig.tiltAxisAngle) <= tol && fabs(t.tiltAngle - torig.tiltAngle) <= tol);
	}

	// ###########################
	// #### Rotation equality ####
	// ###########################

	// Check equality: Rotation matrix
	bool RotmatEqual(const Rotmat& Ra, const Rotmat& Rb, double tol)
	{
		// Return whether none of the elements of the rotation matrices differ by more than the tolerance
		return (Ra - Rb).isZero(tol);
	}

	// Check equality: Quaternion
	bool QuatEqual(const Quat& qa, const Quat& qb, double tol)
	{
		// Return whether to the specified tolerance the quaternions are the same
		bool isSame = (fabs(qa.w() - qb.w()) <= tol && fabs(qa.x() - qb.x()) <= tol && fabs(qa.y() - qb.y()) <= tol && fabs(qa.z() - qb.z()) <= tol);
		bool isOpp  = (fabs(qa.w() + qb.w()) <= tol && fabs(qa.x() + qb.x()) <= tol && fabs(qa.y() + qb.y()) <= tol && fabs(qa.z() + qb.z()) <= tol);
		return (isSame || isOpp);
	}

	// Check equality: Euler angles
	bool EulerEqual(const EulerAngles& ea, const EulerAngles& eb, double tol)
	{
		// Convert both Euler angles to their unique representations
		EulerAngles eau = ea, ebu = eb;
		ValidateEuler(eau, tol, true);
		ValidateEuler(ebu, tol, true);

		// Handle angle wrapping issues
		if(fabs(eau.yaw - ebu.yaw) > M_PI)
		{
			if(eau.yaw > ebu.yaw)
				ebu.yaw += M_2PI;
			else
				eau.yaw += M_2PI;
		}
		if(fabs(eau.roll - ebu.roll) > M_PI)
		{
			if(eau.roll > ebu.roll)
				ebu.roll += M_2PI;
			else
				eau.roll += M_2PI;
		}

		// Return whether to the specified tolerance the Euler angles are the same
		return (fabs(eau.yaw - ebu.yaw) <= tol && fabs(sin(eau.pitch) - sin(ebu.pitch)) <= tol && fabs(eau.roll - ebu.roll) <= tol); // The pitch suffers from the numerical insensitivity of asin, so the sine thereof is checked
	}

	// Check equality: Fused angles
	bool FusedEqual(const FusedAngles& fa, const FusedAngles& fb, double tol)
	{
		// Convert both fused angles to their unique representations
		FusedAngles fau = fa, fbu = fb;
		ValidateFused(fau, tol, true);
		ValidateFused(fbu, tol, true);

		// Handle angle wrapping issues
		if(fabs(fau.fusedYaw - fbu.fusedYaw) > M_PI)
		{
			if(fau.fusedYaw > fbu.fusedYaw)
				fbu.fusedYaw += M_2PI;
			else
				fau.fusedYaw += M_2PI;
		}

		// Return whether to the specified tolerance the fused angles are the same
		return (fabs(fau.fusedYaw - fbu.fusedYaw) <= tol && fabs(sin(fau.fusedPitch) - sin(fbu.fusedPitch)) <= tol && fabs(sin(fau.fusedRoll) - sin(fbu.fusedRoll)) <= tol && fau.hemi == fbu.hemi); // The fused pitch and roll suffer from the numerical insensitivity of asin, so the sine's thereof are checked
	}

	// Check equality: Tilt angles
	bool TiltEqual(const TiltAngles& ta, const TiltAngles& tb, double tol)
	{
		// Convert both tilt angles to their unique representations
		TiltAngles tau = ta, tbu = tb;
		ValidateTilt(tau, tol, true);
		ValidateTilt(tbu, tol, true);

		// Handle angle wrapping issues
		if(fabs(tau.fusedYaw - tbu.fusedYaw) > M_PI)
		{
			if(tau.fusedYaw > tbu.fusedYaw)
				tbu.fusedYaw += M_2PI;
			else
				tau.fusedYaw += M_2PI;
		}

		// Return whether to the specified tolerance the fused angles are the same
		double stilta = sin(tau.tiltAngle);
		double stiltb = sin(tbu.tiltAngle);
		double stiltasq = stilta*stilta;
		double stiltbsq = stiltb*stiltb;
		return (fabs(tau.fusedYaw - tbu.fusedYaw) <= tol && fabs(stiltasq*cos(tau.tiltAxisAngle) - stiltbsq*cos(tbu.tiltAxisAngle)) <= tol && fabs(stiltasq*sin(tau.tiltAxisAngle) - stiltbsq*sin(tbu.tiltAxisAngle)) <= tol && fabs(cos(tau.tiltAngle) - cos(tbu.tiltAngle)) <= tol); // The tilt angle suffers from the numerical insensitivity of acos, so the cosine thereof is checked / The tilt axis angle has a singularity when the tilt angle is zero, so two geometrically relevant terms are checked instead of the tilt axis angle directly
	}

	// #########################
	// #### Yaw of rotation ####
	// #########################

	// Euler yaw of: Rotation matrix
	double EYawOfRotmat(const Rotmat& R)
	{
		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(R.coeff(1,0), R.coeff(0,0));
	}

	// Fused yaw of: Rotation matrix
	double FYawOfRotmat(const Rotmat& R)
	{
		// Calculate, wrap and return the fused yaw
		double fusedYaw, trace = R.coeff(0,0) + R.coeff(1,1) + R.coeff(2,2);
		if(trace >= 0.0)
			fusedYaw = 2.0*atan2(R.coeff(1,0) - R.coeff(0,1), 1.0 + trace);
		else if(R.coeff(2,2) >= R.coeff(1,1) && R.coeff(2,2) >= R.coeff(0,0))
			fusedYaw = 2.0*atan2(1.0 - R.coeff(0,0) - R.coeff(1,1) + R.coeff(2,2), R.coeff(1,0) - R.coeff(0,1));
		else if(R.coeff(1,1) >= R.coeff(0,0))
			fusedYaw = 2.0*atan2(R.coeff(2,1) + R.coeff(1,2), R.coeff(0,2) - R.coeff(2,0));
		else
			fusedYaw = 2.0*atan2(R.coeff(0,2) + R.coeff(2,0), R.coeff(2,1) - R.coeff(1,2));
		if(fusedYaw > M_PI) fusedYaw -= M_2PI;   // fusedYaw is now in [-2*pi,pi]
		if(fusedYaw <= -M_PI) fusedYaw += M_2PI; // fusedYaw is now in (-pi,pi]
		return fusedYaw;
	}

	// Euler yaw of: Quaternion
	double EYawOfQuat(const Quat& q)
	{
		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(q.w()*q.z() + q.x()*q.y(), 0.5 - (q.y()*q.y() + q.z()*q.z()));
	}

	// Fused yaw of: Quaternion
	double FYawOfQuat(const Quat& q)
	{
		// Calculate, wrap and return the fused yaw
		double fusedYaw = 2.0*atan2(q.z(), q.w()); // Output of atan2 is [-pi,pi], so this expression is in [-2*pi,2*pi]
		if(fusedYaw > M_PI) fusedYaw -= M_2PI;     // fusedYaw is now in [-2*pi,pi]
		if(fusedYaw <= -M_PI) fusedYaw += M_2PI;   // fusedYaw is now in (-pi,pi]
		return fusedYaw;
	}

	// Fused yaw of: Euler angles
	double FYawOfEuler(const EulerAngles& e)
	{
		// Calculate and return the fused yaw of the rotation
		return FYawOfRotmat(RotmatFromEuler(e));
	}

	// Euler yaw of: Fused angles
	double EYawOfFused(const FusedAngles& f)
	{
		// Precalculate the sin values
		double sth  = sin(f.fusedPitch);
		double sphi = sin(f.fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the cosine of the tilt angle alpha
		double calpha;
		if(crit >= 1.0)
			calpha = 0.0;
		else
			calpha = (f.hemi ? sqrt(1.0-crit) : -sqrt(1.0-crit));

		// Calculate the tilt axis gamma
		double gamma = atan2(sth,sphi);

		// Precalculate trigonometric terms
		double psigam = f.fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double A = sgam*calpha;

		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam);
	}

	// Euler yaw of: Tilt angles
	double EYawOfTilt(const TiltAngles& t)
	{
		// Precalculate trigonometric terms
		double psigam = t.fusedYaw + t.tiltAxisAngle;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double cgam = cos(t.tiltAxisAngle);
		double sgam = sin(t.tiltAxisAngle);
		double calpha = cos(t.tiltAngle);
		double A = sgam*calpha;

		// Calculate and return the Euler ZYX yaw of the rotation
		return atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam);
	}

	// ##################################
	// #### Remove yaw from rotation ####
	// ##################################

	// Remove Euler yaw from: Rotation matrix
	void RotmatNoEYaw(const Rotmat& R, Rotmat& Rout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfRotmat(R);

		// Precalculate trigonometric terms
		double cEYaw = cos(EYaw);
		double sEYaw = sin(EYaw);

		// Construct the Euler ZYX yaw component of the rotation
		Rotmat REYawTrans;
		REYawTrans << cEYaw, sEYaw, 0.0, -sEYaw, cEYaw, 0.0, 0.0, 0.0, 1.0;

		// Remove the Euler ZYX yaw component of the rotation
		Rout = REYawTrans * R;
	}

	// Remove fused yaw from: Rotation matrix
	void RotmatNoFYaw(const Rotmat& R, Rotmat& Rout)
	{
		// Calculate the fused yaw of the input
		double FYaw = FYawOfRotmat(R);

		// Precalculate trigonometric terms
		double cFYaw = cos(FYaw);
		double sFYaw = sin(FYaw);

		// Construct the fused yaw component of the rotation
		Rotmat RFYawTrans;
		RFYawTrans << cFYaw, sFYaw, 0.0, -sFYaw, cFYaw, 0.0, 0.0, 0.0, 1.0;

		// Remove the fused yaw component of the rotation
		Rout = RFYawTrans * R;
	}

	// Remove Euler yaw from: Quaternion
	void QuatNoEYaw(const Quat& q, Quat& qout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfQuat(q);

		// Construct the Euler ZYX yaw component of the rotation
		double hcEYaw = cos(0.5*EYaw);
		double hsEYaw = sin(0.5*EYaw);

		// Remove the Euler ZYX yaw component of the rotation
		qout.w() = hcEYaw*q.w() + hsEYaw*q.z();
		qout.x() = hcEYaw*q.x() + hsEYaw*q.y();
		qout.y() = hcEYaw*q.y() - hsEYaw*q.x();
		qout.z() = hcEYaw*q.z() - hsEYaw*q.w();
	}

	// Remove fused yaw from: Quaternion
	void QuatNoFYaw(const Quat& q, Quat& qout)
	{
		// Calculate the fused yaw of the input
		double FYaw = FYawOfQuat(q);

		// Construct the fused yaw component of the rotation
		double hcFYaw = cos(0.5*FYaw);
		double hsFYaw = sin(0.5*FYaw);

		// Remove the fused yaw component of the rotation
		qout.w() = hcFYaw*q.w() + hsFYaw*q.z();
		qout.x() = hcFYaw*q.x() + hsFYaw*q.y();
		qout.y() = hcFYaw*q.y() - hsFYaw*q.x();
		qout.z() = hcFYaw*q.z() - hsFYaw*q.w();
	}

	// Remove yaw from: Euler angles
	void EulerNoFYaw(const EulerAngles& e, EulerAngles& eout)
	{
		// Calculate the fused yaw of the input
		double FYaw = FYawOfEuler(e);

		// Remove the fused yaw component of the rotation
		eout.yaw = e.yaw - FYaw;
		eout.pitch = e.pitch;
		eout.roll = e.roll;
	}

	// Remove yaw from: Fused angles
	void FusedNoEYaw(const FusedAngles& f, FusedAngles& fout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfFused(f);

		// Remove the Euler ZYX yaw component of the rotation
		fout.fusedYaw = f.fusedYaw - EYaw;
		fout.fusedPitch = f.fusedPitch;
		fout.fusedRoll = f.fusedRoll;
		fout.hemi = f.hemi;
	}

	// Remove yaw from: Tilt angles
	void TiltNoEYaw(const TiltAngles& t, TiltAngles& tout)
	{
		// Calculate the Euler ZYX yaw of the input
		double EYaw = EYawOfTilt(t);

		// Remove the Euler ZYX yaw component of the rotation
		tout.fusedYaw = t.fusedYaw - EYaw;
		tout.tiltAxisAngle = t.tiltAxisAngle;
		tout.tiltAngle = t.tiltAngle;
	}

	// ###########################
	// #### Rotation inverses ####
	// ###########################

	// Inverse: Rotation matrix
	void RotmatInv(const Rotmat& R, Rotmat& Rinv)
	{
		// Calculate the inverse of the rotation
		Rinv = R.transpose();
	}

	// Inverse: Quaternion
	void QuatInv(const Quat& q, Quat& qinv)
	{
		// Calculate the inverse of the rotation
		qinv.w() = q.w();
		qinv.x() = -q.x();
		qinv.y() = -q.y();
		qinv.z() = -q.z();
	}

	// Inverse: Euler angles
	void EulerInv(const EulerAngles& e, EulerAngles& einv)
	{
		// Precalculate the required sin and cos values
		double cpsi = cos(e.yaw);
		double spsi = sin(e.yaw);
		double cth = cos(e.pitch);
		double sth = sin(e.pitch);
		double cphi = cos(e.roll);
		double sphi = sin(e.roll);

		// Calculate the sine of the inverse pitch angle
		double sthinv = -(cpsi*sth*cphi + spsi*sphi);
		sthinv = (sthinv >= 1.0 ? 1.0 : (sthinv <= -1.0 ? -1.0 : sthinv)); // Coerce sthinv to [-1,1]

		// Calculate the required inverse Euler angles representation
		einv.yaw = atan2(cpsi*sth*sphi - spsi*cphi, cpsi*cth);
		einv.pitch = asin(sthinv);
		einv.roll = atan2(spsi*sth*cphi - cpsi*sphi, cth*cphi);
	}

	// Inverse: Fused angles
	void FusedInv(const FusedAngles& f, FusedAngles& finv)
	{
		// Precalculate the sin values
		double sth  = sin(f.fusedPitch);
		double sphi = sin(f.fusedRoll);

		// Calculate the sine of the tilt angle alpha
		double crit = sth*sth + sphi*sphi;
		double salpha = (crit >= 1.0 ? 1.0 : sqrt(crit));

		// Calculate the tilt axis gamma
		double gamma = atan2(sth,sphi);

		// Precalculate trigonometric values
		double psigam = f.fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);

		// Calculate the inverse fused pitch and roll
		double thinv = asin(-salpha*spsigam);
		double phinv = asin(-salpha*cpsigam);

		// Construct the inverse fused angles rotation
		finv.fusedYaw = -f.fusedYaw;
		finv.fusedPitch = thinv;
		finv.fusedRoll = phinv;
		finv.hemi = f.hemi;
	}

	// Inverse: Tilt angles
	void TiltInv(const TiltAngles& t, TiltAngles& tinv)
	{
		// Calculate the inverse tilt axis angle
		double gammainv = t.fusedYaw + t.tiltAxisAngle - M_PI;
		gammainv += M_2PI*std::floor((M_PI - gammainv) / M_2PI);

		// Construct the inverse tilt angles rotation
		tinv.fusedYaw = -t.fusedYaw;
		tinv.tiltAxisAngle = gammainv;
		tinv.tiltAngle = t.tiltAngle;
	}

	// ##########################
	// #### Vector rotations ####
	// ##########################

	// Rotate vector by: Rotation matrix
	Vec3 RotmatRotVec(const Rotmat& R, const Vec3& v)
	{
		// Return the required rotated vector
		return R*v;
	}

	// Rotate vector by: Quaternion
	Vec3 QuatRotVec(const Quat& q, const Vec3& v)
	{
		// Precalculate an intermediate vector term
		double tx = 2.0*(q.y()*v.z() - v.y()*q.z());
		double ty = 2.0*(q.z()*v.x() - v.z()*q.x());
		double tz = 2.0*(q.x()*v.y() - v.x()*q.y());

		// Calculate and return the required vector
		Vec3 vout = v;
		vout.x() += q.w()*tx + q.y()*tz - ty*q.z();
		vout.y() += q.w()*ty + q.z()*tx - tz*q.x();
		vout.z() += q.w()*tz + q.x()*ty - tx*q.y();
		return vout;
	}

	// Rotate vector by: Euler angles
	Vec3 EulerRotVec(const EulerAngles& e, const Vec3& v)
	{
		// Return the required rotated vector
		return RotmatFromEuler(e)*v;
	}

	// Rotate vector by: Fused angles
	Vec3 FusedRotVec(const FusedAngles& f, const Vec3& v)
	{
		// Return the required rotated vector
		return RotmatFromFused(f)*v;
	}

	// Rotate vector by: Tilt angles
	Vec3 TiltRotVec(const TiltAngles& t, const Vec3& v)
	{
		// Return the required rotated vector
		return RotmatFromTilt(t)*v;
	}

	// ##############################
	// #### Pure yaw conversions ####
	// ##############################

	// Conversion: Pure yaw --> Rotation matrix
	void RotmatFromYaw(double yaw, Rotmat& R)
	{
		// Precalculate trigonometric values
		double cyaw = cos(yaw);
		double syaw = sin(yaw);

		// Set the required rotation matrix
		R << cyaw, -syaw, 0.0, syaw, cyaw, 0.0, 0.0, 0.0, 1.0;
	}

	// Conversion: Pure yaw --> Quaternion
	void QuatFromYaw(double yaw, Quat& q)
	{
		// Precalculate the sin values
		double hpsi = 0.5*yaw;
		double chpsi = cos(hpsi);
		double shpsi = sin(hpsi);

		// Set the required quaternion orientation
		q.w() = chpsi;
		q.x() = 0.0;
		q.y() = 0.0;
		q.z() = shpsi;
	}

	// ############################################
	// #### Conversions from rotation matrices ####
	// ############################################

	//
	// Conversion: Rotation matrix --> Quaternion
	//

	// Conversion: Rotation matrix --> Quaternion
	void QuatFromRotmat(const Rotmat& R, Quat& q)
	{
		// Perform the required conversion in a numerically stable manner
		double r, s, t = R.coeff(0,0) + R.coeff(1,1) + R.coeff(2,2);
		if(t >= 0.0)
		{
			r = sqrt(1.0 + t);
			s = 0.5/r;
			q.w() = 0.5*r;
			q.x() = s*(R.coeff(2,1) - R.coeff(1,2));
			q.y() = s*(R.coeff(0,2) - R.coeff(2,0));
			q.z() = s*(R.coeff(1,0) - R.coeff(0,1));
		}
		else if(R.coeff(2,2) >= R.coeff(1,1) && R.coeff(2,2) >= R.coeff(0,0))
		{
			r = sqrt(1.0 - (R.coeff(0,0) + R.coeff(1,1) - R.coeff(2,2)));
			s = 0.5/r;
			q.w() = s*(R.coeff(1,0) - R.coeff(0,1));
			q.x() = s*(R.coeff(0,2) + R.coeff(2,0));
			q.y() = s*(R.coeff(2,1) + R.coeff(1,2));
			q.z() = 0.5*r;
		}
		else if(R.coeff(1,1) >= R.coeff(0,0))
		{
			r = sqrt(1.0 - (R.coeff(0,0) - R.coeff(1,1) + R.coeff(2,2)));
			s = 0.5/r;
			q.w() = s*(R.coeff(0,2) - R.coeff(2,0));
			q.x() = s*(R.coeff(1,0) + R.coeff(0,1));
			q.y() = 0.5*r;
			q.z() = s*(R.coeff(2,1) + R.coeff(1,2));
		}
		else
		{
			r = sqrt(1.0 + (R.coeff(0,0) - R.coeff(1,1) - R.coeff(2,2)));
			s = 0.5/r;
			q.w() = s*(R.coeff(2,1) - R.coeff(1,2));
			q.x() = 0.5*r;
			q.y() = s*(R.coeff(1,0) + R.coeff(0,1));
			q.z() = s*(R.coeff(0,2) + R.coeff(2,0));
		}
	}

	//
	// Conversion: Rotation matrix --> Euler angles
	//

	// Conversion: Rotation matrix --> Euler angles
	void EulerFromRotmat(const Rotmat& R, double& yaw, double& pitch, double& roll)
	{
		// Calculate the sine of the pitch angle
		double sth = -R.coeff(2,0);
		sth = (sth >= 1.0 ? 1.0 : (sth <= -1.0 ? -1.0 : sth)); // Coerce sth to [-1,1]

		// Calculate the required Euler angles
		yaw = atan2(R.coeff(1,0), R.coeff(0,0));
		pitch = asin(sth);
		roll = atan2(R.coeff(2,1), R.coeff(2,2));
	}

	//
	// Conversion: Rotation matrix --> Fused angles
	//

	// Conversion: Rotation matrix --> Fused angles (2D)
	void FusedFromRotmat(const Rotmat& R, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused pitch and roll
		double stheta = -R.coeff(2,0);
		double sphi   = R.coeff(2,1);
		stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1]
		sphi   = (sphi   >= 1.0 ? 1.0 : (sphi   <= -1.0 ? -1.0 : sphi  )); // Coerce sphi   to [-1,1]
		fusedPitch = asin(stheta);
		fusedRoll  = asin(sphi);
	}

	// Conversion: Rotation matrix --> Fused angles (3D)
	void FusedFromRotmat(const Rotmat& R, double& fusedYaw, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused yaw, pitch and roll
		fusedYaw = FYawOfRotmat(R);
		FusedFromRotmat(R, fusedPitch, fusedRoll);
	}

	// Conversion: Rotation matrix --> Fused angles (4D)
	void FusedFromRotmat(const Rotmat& R, double& fusedYaw, double& fusedPitch, double& fusedRoll, bool& hemi)
	{
		// Calculate the fused yaw, pitch and roll
		fusedYaw = FYawOfRotmat(R);
		FusedFromRotmat(R, fusedPitch, fusedRoll);

		// Calculate the hemisphere of the rotation
		hemi = (R.coeff(2,2) >= 0.0);
	}

	//
	// Conversion: Rotation matrix --> Tilt angles
	//

	// Conversion: Rotation matrix --> Tilt angles (2D)
	void TiltFromRotmat(const Rotmat& R, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the tilt axis angle
		tiltAxisAngle = atan2(-R.coeff(2,0), R.coeff(2,1));

		// Calculate the tilt angle
		double calpha = R.coeff(2,2);
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		tiltAngle = acos(calpha);
	}

	// Conversion: Rotation matrix --> Tilt angles (3D)
	void TiltFromRotmat(const Rotmat& R, double& fusedYaw, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the fused yaw, tilt axis angle and tilt angle
		fusedYaw = FYawOfRotmat(R);
		TiltFromRotmat(R, tiltAxisAngle, tiltAngle);
	}

	// ######################################
	// #### Conversions from quaternions ####
	// ######################################

	//
	// Conversion: Quaternion --> Rotation matrix
	//

	// Conversion: Quaternion --> Rotation matrix
	void RotmatFromQuat(const Quat& q, Rotmat& R)
	{
		// Construct the required rotation matrix
		R << 1.0 - 2.0*(q.y()*q.y() + q.z()*q.z()),       2.0*(q.x()*q.y() - q.z()*q.w()),       2.0*(q.x()*q.z() + q.y()*q.w()),
		           2.0*(q.x()*q.y() + q.z()*q.w()), 1.0 - 2.0*(q.x()*q.x() + q.z()*q.z()),       2.0*(q.y()*q.z() - q.x()*q.w()),
		           2.0*(q.x()*q.z() - q.y()*q.w()),       2.0*(q.y()*q.z() + q.x()*q.w()), 1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
	}

	//
	// Conversion: Quaternion --> Euler angles
	//

	// Conversion: Quaternion --> Euler angles
	void EulerFromQuat(const Quat& q, double& yaw, double& pitch, double& roll)
	{
		// Calculate the sine of the pitch angle
		double sth = 2.0*(q.w()*q.y() - q.x()*q.z());
		sth = (sth >= 1.0 ? 1.0 : (sth <= -1.0 ? -1.0 : sth)); // Coerce sth to [-1,1]

		// Calculate the required Euler angles
		double qysq = q.y()*q.y();
		yaw = atan2(q.x()*q.y() + q.z()*q.w(), 0.5 - (qysq + q.z()*q.z()));
		pitch = asin(sth);
		roll = atan2(q.y()*q.z() + q.x()*q.w(), 0.5 - (q.x()*q.x() + qysq));
	}

	//
	// Conversion: Quaternion --> Fused angles
	//

	// Conversion: Quaternion --> Fused angles (2D)
	void FusedFromQuat(const Quat& q, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused pitch and roll
		double stheta = 2.0*(q.y()*q.w() - q.x()*q.z());
		double sphi   = 2.0*(q.y()*q.z() + q.x()*q.w());
		stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1]
		sphi   = (sphi   >= 1.0 ? 1.0 : (sphi   <= -1.0 ? -1.0 : sphi  )); // Coerce sphi   to [-1,1]
		fusedPitch = asin(stheta);
		fusedRoll  = asin(sphi);
	}

	// Conversion: Quaternion --> Fused angles (3D)
	void FusedFromQuat(const Quat& q, double& fusedYaw, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused yaw, pitch and roll
		FusedFromQuat(q, fusedYaw);
		FusedFromQuat(q, fusedPitch, fusedRoll);
	}

	// Conversion: Quaternion --> Fused angles (4D)
	void FusedFromQuat(const Quat& q, double& fusedYaw, double& fusedPitch, double& fusedRoll, bool& hemi)
	{
		// Calculate the fused yaw, pitch and roll
		FusedFromQuat(q, fusedYaw);
		FusedFromQuat(q, fusedPitch, fusedRoll);

		// Calculate the hemisphere of the rotation
		hemi = (0.5 - (q.x()*q.x() + q.y()*q.y()) >= 0.0);
	}

	//
	// Conversion: Quaternion --> Tilt angles
	//

	// Conversion: Quaternion --> Tilt angles (2D)
	void TiltFromQuat(const Quat& q, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the tilt axis angle
		tiltAxisAngle = atan2(q.w()*q.y() - q.x()*q.z(), q.w()*q.x() + q.y()*q.z());

		// Calculate the tilt angle
		double calpha = 1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		tiltAngle = acos(calpha);
	}

	// Conversion: Quaternion --> Tilt angles (3D)
	void TiltFromQuat(const Quat& q, double& fusedYaw, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the fused yaw, tilt axis angle and tilt angle
		FusedFromQuat(q, fusedYaw);
		TiltFromQuat(q, tiltAxisAngle, tiltAngle);
	}

	//
	// Conversion: Quaternion --> Z vector
	//

	// Conversion: Quaternion --> Z vector
	void ZVecFromQuat(const Quat& q, ZVec& z)
	{
		// Calculate the required Z vector
		z.x() = 2.0*(q.x()*q.z() - q.y()*q.w());
		z.y() = 2.0*(q.y()*q.z() + q.x()*q.w());
		z.z() = 1.0 - 2.0*(q.x()*q.x() + q.y()*q.y());
	}

	// #######################################
	// #### Conversions from Euler angles ####
	// #######################################

	// Conversion: Euler angles --> Rotation matrix
	Rotmat RotmatFromEuler(double yaw, double pitch, double roll)
	{
		// Precalculate the trigonometric values
		double cpsi = cos(yaw);
		double spsi = sin(yaw);
		double cth  = cos(pitch);
		double sth  = sin(pitch);
		double cphi = cos(roll);
		double sphi = sin(roll);

		// Calculate and return the required rotation matrix
		Rotmat R;
		R << cpsi*cth, cpsi*sth*sphi - spsi*cphi, cpsi*sth*cphi + spsi*sphi,
		     spsi*cth, spsi*sth*sphi + cpsi*cphi, spsi*sth*cphi - cpsi*sphi,
		         -sth,                  cth*sphi,                  cth*cphi;
		return R;
	}

	// Conversion: Euler angles --> Quaternion
	Quat QuatFromEuler(double yaw, double pitch, double roll)
	{
		// Halve the Euler angles
		double hpsi = 0.5*yaw;
		double hth = 0.5*pitch;
		double hphi = 0.5*roll;

		// Precalculate the trigonometric values
		double hcpsi = cos(hpsi);
		double hspsi = sin(hpsi);
		double hcth  = cos(hth);
		double hsth  = sin(hth);
		double hcphi = cos(hphi);
		double hsphi = sin(hphi);

		// Calculate and return the required quaternion
		return Quat(hcphi*hcth*hcpsi + hsphi*hsth*hspsi, hsphi*hcth*hcpsi - hcphi*hsth*hspsi, hcphi*hsth*hcpsi + hsphi*hcth*hspsi, hcphi*hcth*hspsi - hsphi*hsth*hcpsi);
	}

	// Conversion: Euler angles --> Fused angles
	FusedAngles FusedFromEuler(double yaw, double pitch, double roll)
	{
		// Construct a fused angles object
		FusedAngles f;

		// Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation
		Rotmat R = RotmatFromEuler(yaw, pitch, roll);

		// Calculate the fused yaw
		f.fusedYaw = FYawOfRotmat(R);

		// Calculate the fused pitch
		f.fusedPitch = pitch; // ZYX Euler pitch is equivalent to fused pitch!

		// Calculate the fused roll
		double sphi = R.coeff(2,1);
		sphi = (sphi >= 1.0 ? 1.0 : (sphi <= -1.0 ? -1.0 : sphi)); // Coerce sphi to [-1,1]
		f.fusedRoll  = asin(sphi);

		// See which hemisphere we're in
		f.hemi = (R.coeff(2,2) >= 0.0);

		// Return the calculated fused angles
		return f;
	}

	// Conversion: Euler angles --> Tilt angles
	TiltAngles TiltFromEuler(double yaw, double pitch, double roll)
	{
		// Construct a tilt angles object
		TiltAngles t;

		// Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation
		Rotmat R = RotmatFromEuler(yaw, pitch, roll);

		// Calculate the fused yaw
		t.fusedYaw = FYawOfRotmat(R);

		// Calculate the tilt axis angle
		t.tiltAxisAngle = atan2(-R.coeff(2,0), R.coeff(2,1));

		// Calculate the tilt angle
		double calpha = R.coeff(2,2);
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		t.tiltAngle = acos(calpha);

		// Return the calculated tilt angles
		return t;
	}

	// Conversion: Euler angles --> Z vector
	ZVec ZVecFromEuler(double pitch, double roll)
	{
		// Precalculate the trigonometric values
		double cth  = cos(pitch);
		double sth  = sin(pitch);
		double cphi = cos(roll);
		double sphi = sin(roll);

		// Calculate and return the required Z vector
		return ZVec(-sth, cth*sphi, cth*cphi);
	}

	// #######################################
	// #### Conversions from fused angles ####
	// #######################################

	//
	// Conversion: Fused angles --> Rotation matrix
	//

	// Conversion: Fused angles (2D) --> Rotation matrix
	Rotmat RotmatFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;
		double calpha = (crit >= 1.0 ? 0.0 : sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms involved in the rotation matrix expression
		double calphabar = 1.0 - calpha;
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double A = calpha + calphabar*cgam*cgam;
		double B = calpha + calphabar*sgam*sgam;
		double C = calphabar*cgam*sgam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R <<    A,    C,    sth,
		        C,    B,  -sphi,
		     -sth, sphi, calpha;
		return R;
	}

	// Conversion: Fused angles (3D/4D) --> Rotation matrix
	Rotmat RotmatFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the tilt angle alpha
		double calpha, salpha;
		if(crit >= 1.0)
		{
			calpha = 0.0;
			salpha = 1.0;
		}
		else
		{
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));
			salpha = sqrt(crit);
		}

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms involved in the rotation matrix expression
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double psigam = fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = cgam*cpsigam;
		double B = sgam*cpsigam;
		double C = cgam*spsigam;
		double D = sgam*spsigam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R << A + D*calpha, B - C*calpha,  salpha*spsigam,
		     C - B*calpha, D + A*calpha, -salpha*cpsigam,
		          -sth,          sphi,    calpha;
		return R;
	}

	//
	// Conversion: Fused angles --> Quaternion
	//

	// Conversion: Fused angles (2D) --> Quaternion
	Quat QuatFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the tilt angle alpha
		double alpha = (crit >= 1.0 ? M_PI_2 : acos(sqrt(1.0 - crit)));
		double halpha = 0.5*alpha;
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);
		double cgamma = cos(gamma);
		double sgamma = sin(gamma);

		// Return the required quaternion orientation (a rotation about (cgamma, sgamma, 0) by angle alpha)
		return Quat(chalpha, cgamma*shalpha, sgamma*shalpha, 0.0); // Order: (w,x,y,z)
	}

	// Conversion: Fused angles (3D/4D) --> Quaternion
	Quat QuatFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the sine sum criterion
		double crit = sth*sth + sphi*sphi;

		// Calculate the tilt angle alpha
		double alpha;
		if(crit >= 1.0)
			alpha = M_PI_2;
		else
			alpha = acos(hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Evaluate the required intermediate angles
		double halpha = 0.5*alpha;
		double hpsi = 0.5*fusedYaw;
		double hgampsi = gamma + hpsi;

		// Precalculate trigonometric terms involved in the quaternion expression
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);
		double chpsi = cos(hpsi);
		double shpsi = sin(hpsi);
		double chgampsi = cos(hgampsi);
		double shgampsi = sin(hgampsi);

		// Calculate and return the required quaternion
		return Quat(chalpha*chpsi, shalpha*chgampsi, shalpha*shgampsi, chalpha*shpsi); // Order: (w,x,y,z)
	}

	//
	// Conversion: Fused angles --> Euler angles
	//

	// Conversion: Fused angles (2D) --> Euler angles
	EulerAngles EulerFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = (crit >= 1.0 ? 0.0 : sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double A = cgam*(1.0 - calpha);

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(A*sgam, calpha + A*cgam), fusedPitch, atan2(sphi, calpha));
	}

	// Conversion: Fused angles (3D/4D) --> Euler angles
	EulerAngles EulerFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = 0.0;
		if(crit < 1.0)
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Calculate the tilt axis angle gamma
		double gamma = atan2(sth,sphi);

		// Precalculate terms
		double cgam = cos(gamma);
		double sgam = sin(gamma);
		double psigam = fusedYaw + gamma;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = sgam*calpha;

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam), fusedPitch, atan2(sphi, calpha));
	}

	//
	// Conversion: Fused angles --> Tilt angles
	//

	// Conversion: Fused angles (2D) --> Tilt angles
	TiltAngles TiltFromFused(double fusedPitch, double fusedRoll) // Assume: fusedYaw = 0, hemi = true
	{
		// Construct a tilt angles object
		TiltAngles t;

		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = (crit >= 1.0 ? 0.0 : sqrt(1.0 - crit));

		// Calculate and return the tilt angles representation
		t.fusedYaw = 0.0;
		t.tiltAxisAngle = atan2(sth,sphi);
		t.tiltAngle = acos(calpha);
		return t;
	}

	// Conversion: Fused angles (3D/4D) --> Tilt angles
	TiltAngles TiltFromFused(double fusedYaw, double fusedPitch, double fusedRoll, bool hemi)
	{
		// Construct a tilt angles object
		TiltAngles t;

		// Precalculate the sine values
		double sth = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = 0.0;
		if(crit < 1.0)
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Calculate and return the tilt angles representation
		t.fusedYaw = fusedYaw;
		t.tiltAxisAngle = atan2(sth,sphi);
		t.tiltAngle = acos(calpha);
		return t;
	}

	//
	// Conversion: Fused angles --> Z vector
	//

	// Conversion: Fused angles --> Z vector
	ZVec ZVecFromFused(double fusedPitch, double fusedRoll, bool hemi)
	{
		// Precalculate the sin values
		double sth  = sin(fusedPitch);
		double sphi = sin(fusedRoll);

		// Calculate the cosine of the tilt angle
		double crit = sth*sth + sphi*sphi;
		double calpha = 0.0;
		if(crit < 1.0)
			calpha = (hemi ? sqrt(1.0 - crit) : -sqrt(1.0 - crit));

		// Return the required Z vector
		return ZVec(-sth, sphi, calpha);
	}

	// ######################################
	// #### Conversions from tilt angles ####
	// ######################################

	//
	// Conversion: Tilt angles --> Rotation matrix
	//

	// Conversion: Tilt angles (2D) --> Rotation matrix
	Rotmat RotmatFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Precalculate terms involved in the rotation matrix expression
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double calphabar = 1.0 - calpha;
		double sth = salpha*sgam;
		double sphi = salpha*cgam;
		double A = calpha + calphabar*cgam*cgam;
		double B = calpha + calphabar*sgam*sgam;
		double C = calphabar*cgam*sgam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R <<    A,    C,    sth,
		        C,    B,  -sphi,
		     -sth, sphi, calpha;
		return R;
	}

	// Conversion: Tilt angles (3D) --> Rotation matrix
	Rotmat RotmatFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate terms involved in the rotation matrix expression
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double psigam = fusedYaw + tiltAxisAngle;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = cgam*cpsigam;
		double B = sgam*cpsigam;
		double C = cgam*spsigam;
		double D = sgam*spsigam;

		// Calculate and return the required rotation matrix
		Rotmat R;
		R << A + D*calpha, B - C*calpha,  salpha*spsigam,
		     C - B*calpha, D + A*calpha, -salpha*cpsigam,
		     -sgam*salpha,  cgam*salpha,  calpha;
		return R;
	}

	//
	// Conversion: Tilt angles --> Quaternion
	//

	// Conversion: Tilt angles (2D) --> Quaternion
	Quat QuatFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Precalculate the required angles
		double halpha = 0.5*tiltAngle;

		// Precalculate the required trigonometric values
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);
		double cgamma = cos(tiltAxisAngle);
		double sgamma = sin(tiltAxisAngle);

		// Return the required quaternion orientation
		return Quat(chalpha, shalpha*cgamma, shalpha*sgamma, 0.0); // Order: (w,x,y,z)
	}

	// Conversion: Tilt angles (3D) --> Quaternion
	Quat QuatFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate the required angles
		double hpsi = 0.5*fusedYaw;
		double halpha = 0.5*tiltAngle;
		double hgampsi = tiltAxisAngle + hpsi;

		// Precalculate the required trigonometric values
		double chpsi = cos(hpsi);
		double shpsi = sin(hpsi);
		double chalpha = cos(halpha);
		double shalpha = sin(halpha);
		double chgampsi = cos(hgampsi);
		double shgampsi = sin(hgampsi);

		// Return the required quaternion orientation
		return Quat(chalpha*chpsi, shalpha*chgampsi, shalpha*shgampsi, chalpha*shpsi); // Order: (w,x,y,z)
	}

	//
	// Conversion: Tilt angles --> Euler angles
	//

	// Conversion: Tilt angles (2D) --> Euler angles
	EulerAngles EulerFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Precalculate terms
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double sth = sgam*salpha;
		double sphi = cgam*salpha;
		double A = cgam*(1.0 - calpha);

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(A*sgam, calpha + A*cgam), asin(sth), atan2(sphi, calpha));
	}

	// Conversion: Tilt angles (3D) --> Euler angles
	EulerAngles EulerFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate terms
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);
		double sth = sgam*salpha;
		double sphi = cgam*salpha;
		double psigam = fusedYaw + tiltAxisAngle;
		double cpsigam = cos(psigam);
		double spsigam = sin(psigam);
		double A = sgam*calpha;

		// Calculate and return the required Euler angles
		return EulerAngles(atan2(cgam*spsigam - A*cpsigam, cgam*cpsigam + A*spsigam), asin(sth), atan2(sphi, calpha));
	}

	//
	// Conversion: Tilt angles --> Fused angles
	//

	// Conversion: Tilt angles (2D) --> Fused angles
	FusedAngles FusedFromTilt(double tiltAxisAngle, double tiltAngle) // Assume: fusedYaw = 0
	{
		// Calculate and return the fused angles representation
		return FusedFromTilt(0.0, tiltAxisAngle, tiltAngle);
	}

	// Conversion: Tilt angles (3D) --> Fused angles
	FusedAngles FusedFromTilt(double fusedYaw, double tiltAxisAngle, double tiltAngle)
	{
		// Construct a fused angles object
		FusedAngles f;

		// Precalculate terms
		double cgam = cos(tiltAxisAngle);
		double sgam = sin(tiltAxisAngle);
		double salpha = sin(tiltAngle);

		// Calculate and return the fused angles representation
		f.fusedYaw = fusedYaw;
		f.fusedPitch = asin(salpha*sgam);
		f.fusedRoll = asin(salpha*cgam);
		f.hemi = (tiltAngle <= M_PI_2);
		return f;
	}

	//
	// Conversion: Tilt angles --> Z vector
	//

	// Conversion: Tilt angles --> Z vector
	ZVec ZVecFromTilt(double tiltAxisAngle, double tiltAngle)
	{
		// Precalculate the required trigonometric terms
		double cgamma = cos(tiltAxisAngle);
		double sgamma = sin(tiltAxisAngle);
		double calpha = cos(tiltAngle);
		double salpha = sin(tiltAngle);

		// Return the required Z vector
		return ZVec(-salpha*sgamma, salpha*cgamma, calpha);
	}

	// ####################################
	// #### Conversions from Z vectors ####
	// ####################################

	//
	// Conversion: Z vector --> Fused angles
	//

	// Conversion: Z vector --> Fused angles (2D)
	void FusedFromZVec(const ZVec& z, double& fusedPitch, double& fusedRoll)
	{
		// Calculate the fused pitch and roll
		double stheta = -z.x();
		double sphi   = z.y();
		stheta = (stheta >= 1.0 ? 1.0 : (stheta <= -1.0 ? -1.0 : stheta)); // Coerce stheta to [-1,1]
		sphi   = (sphi   >= 1.0 ? 1.0 : (sphi   <= -1.0 ? -1.0 : sphi  )); // Coerce sphi   to [-1,1]
		fusedPitch = asin(stheta);
		fusedRoll  = asin(sphi);
	}

	// Conversion: Z vector --> Fused angles (3D)
	void FusedFromZVec(const ZVec& z, double& fusedPitch, double& fusedRoll, bool& hemi)
	{
		// Calculate the fused pitch and roll
		FusedFromZVec(z, fusedPitch, fusedRoll);

		// Calculate the hemisphere
		hemi = (z.z() >= 0.0);
	}

	//
	// Conversion: Z vector --> Tilt angles
	//

	// Conversion: Z vector --> Tilt angles (2D)
	void TiltFromZVec(const ZVec& z, double& tiltAxisAngle, double& tiltAngle)
	{
		// Calculate the tilt axis angle
		tiltAxisAngle = atan2(-z.x(), z.y());

		// Calculate the tilt angle
		double calpha = z.z();
		calpha = (calpha >= 1.0 ? 1.0 : (calpha <= -1.0 ? -1.0 : calpha)); // Coerce calpha to [-1,1]
		tiltAngle = acos(calpha);
	}
}
// EOF


#include <Rcpp.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "bckPotentialsAsymp.hpp"
#include "interp_1d.hpp"

using namespace Rcpp;
using namespace std;
using namespace boost::numeric::odeint ;

// The stiff algorith only accepts these data types
typedef boost::numeric::ublas::vector< long double > state_type;
typedef boost::numeric::ublas::matrix< long double > matrix_type;

struct VQCD
{
    long double x;
    long double tau0;
    long double W0;
    long double V0;
    long double lambda0;
    VQCD():x(1.0), tau0(1.0), W0(12.0/11), V0(12.0), lambda0(8.0 * M_PI * M_PI){}
    VQCD(long double xi, long double ti, long double w0, long double v0, long double l0):x(xi),tau0(ti), W0(w0), V0(v0), lambda0(l0){}
    void operator()(const state_type &X , state_type &dXdt , long double A)
    {
        long double e2A = exp(2.0 * A);
        long double kNr = k(X[1], x) ;
        long double dkNr = dk(X[1],x);
        long double vf = Vf(X[1], X[2], x, W0) ;
        long double dvfdl = dVfdl(X[1], X[2], x, W0) ;
        long double dlogvfdt = dLogVfdt(X[2], x, W0) ;
        long double dlogvfdl = dLogVfdl(X[1], x, W0) ;
        long double G = sqrt(1.0 + kNr * pow(X[4], 2.0) / ( e2A * pow(X[0], 2.0 ) ) ) ;
        // 1st Aeom in Mathematica Notebook
        dXdt[0] = - X[0] + (4.0 / 9.0) * X[0] * pow(X[3], 2.0) / pow(X[1], 2.0) + x * e2A * pow(X[0], 3.0) * kNr * vf * pow(X[4], 2.0) * G / ( 6.0 * ( e2A * pow( X[0], 2.0 ) + kNr * pow(X[4], 2.0) ) ) ;
        dXdt[1] = X[3] ;
        dXdt[2] = X[4] ;
        // 2nd Aeom in Mathematica Notebook
        dXdt[3] = - (3.0 / 8) * e2A * pow( X[0] * X[1], 2.0 ) * dVg(X[1], V0, lambda0) + 9.0 * pow(X[1], 2.0) / X[3] \
        - 3 * e2A * pow(X[0] * X[1], 2.0) * Vg(X[1], V0, lambda0) / ( 4.0 * X[3] ) - 5.0 * X[3] + pow( X[3], 2.0) / X[1] \
        + ( 4.0 / 9) * pow(X[3], 3.0) / pow(X[1], 2.0) + 3 * e2A * x * pow( X[0] * X[1], 2.0) * vf / (4.0 * X[3] * G) \
        + x * kNr * vf * X[3] * pow( X[4], 2.0) / ( 6.0 * G) + 3.0 * x * vf * pow(X[1] * X[4], 2.0) * dkNr / ( 16.0 * G) \
        + 3 * e2A * x * pow(X[0] * X[1], 2.0) * dvfdl / ( 8.0 * G) + 3.0 * x * kNr * pow(X[1] * X[4], 2.0) * dvfdl / ( 8.0 * G) ;
        // 3rd Aeom in Mathematica Notebook
        dXdt[4] = - 4.0 * X[4] + ( 4.0 / 9 ) * pow( X[3] / X[1] , 2.0) * X[4] - 4.0 * kNr * pow(X[4], 3.0) / ( e2A * X[0] * X[0]) \
        + e2A * x * pow( X[0], 2.0 ) * kNr * vf * pow( X[4], 3.0) * G / ( 6 * ( e2A * pow(X[0],2.0) + kNr * pow(X[4], 2.0) ) )\
        - X[3] * X[4] * dkNr / kNr - X[3] * pow(X[4], 3.0) * dkNr / (2.0 * e2A * pow( X[0], 2.0) ) \
        + e2A * pow(X[0], 2.0) * dlogvfdt / kNr + pow(X[4], 2.0) * dlogvfdt - X[3] * X[4] * dlogvfdl \
        - kNr * X[3] * pow(X[4], 3.0) * dlogvfdl / ( e2A * pow(X[0], 2.0) ) ;
    }
};

// [[Rcpp::export]]
List solveHVQCD(long double xi = 1, long double ti = 1, long double W0 = 12/11.0, long double V0 = 12, long double lambda0 = 8 * M_PI * M_PI)
{
    // Computes dr/dA, tau, lambda, dtau/dA and dlambda/dA given x and tau0
    // x - long double. Physically it means x = N_f / N_c when N_f, N_c -> inf but with fixed coefficient
    // tau0 - > parameter related with the exponential behaviour of the tachyon in the IR
    // Returns a list with quantitites that depend on A, mq and zIR

    // Create a vectors containing the values of the fields
    vector< long double > Z, AA, dZ, L, T;
    // Boundary conditions in the IR
    long double zIR = log(70.0 / ti) / CI(xi, W0, V0, lambda0) ;
    long double aIR = AIR(zIR, V0, lambda0) ;
    AA.push_back(aIR) ;
    long double lambdair = lambdaIR(zIR, lambda0) ;
    L.push_back(lambdair);
    long double tauir = 70.0 ;
    T.push_back(tauir) ;
    // dA/dz at zIR
    long double daIR =  173.0 / ( 1728.0 * pow(zIR, 3.0)) + 0.5 /zIR - 2.0 * zIR  ;
    // dtau/dz at zIR
    long double dtauir = CI(xi, W0, V0, lambda0) * tauir ;
    // dlambda/dz at zIR. Expression got from eom2 in Mathematica notebook.
    long double dlambdair = sqrt(1.5) * lambdair * sqrt( 6 * pow(daIR, 2.0) + exp( 2.0 * aIR) * xi * Vf(lambdair,tauir, xi, W0) \
         / (2.0 * sqrt(1+ dtauir * dtauir * k(lambdair, xi) / exp(2 * aIR ) ) ) - 0.5 * exp( 2.0 * aIR) * Vg(lambdair, V0, lambda0) );
    long double dzIR = 1.0 / daIR ;
    dZ.push_back(dzIR) ;
    dlambdair = dlambdair / daIR;
    dtauir = dtauir / daIR;
    // Define the type of the state. We have X = {dz, lambda, tau, dlambda, dtau}
    state_type X (5);
    X <<= dzIR, lambdair, tauir, dlambdair, dtauir;
    // Now compute the starting value of dX
    cout << "Solving HVQCD for x = " << xi << ", tau0 = " << ti << ", W0 = " << W0 << ", V0 = " << V0 << ", lambda0 = " << lambda0 << endl;
    long double Amax = 100.0;
    long double h = 0.1 ;
    dense_output_runge_kutta< controlled_runge_kutta< runge_kutta_dopri5< state_type > > > stepper;
    stepper.initialize( X , aIR , h );
    long double A = aIR;
    while ( A < Amax )
    {
        stepper.do_step( VQCD(xi, ti, W0, V0, lambda0) ) ;
        X = stepper.current_state();
        A = stepper.current_time();
        //cout << A << '\t' <<  X(0) << '\t' <<  X(1) << '\t' <<  X(2) << endl;
        AA.push_back(A) ;
        dZ.push_back(X(0)) ;
        L.push_back(X(1)) ;
        T.push_back(X(2)) ;
    }
    double mq = X(4) / X(0) ;
    Spline_Interp<long double> dzfun = Spline_Interp<long double>(AA, dZ);
    int n = AA.size() ;
    long double zmin = zIR + dzfun.integrate(AA[ n - 1]) ;
    for(int i = 0; i < n; i++)
    {
        Z.push_back( zIR + dzfun.integrate(AA[i]) - zmin ) ;
    }
    // We need to reverse the lists because later
    // we will use them to compute the spectra of vector mesons.
    reverse(AA.begin(),AA.end()) ;
    reverse(Z.begin(),Z.end()) ;
    reverse(L.begin(),L.end()) ;
    reverse(T.begin(),T.end()) ;
    // Return A, Z, L(A), T(A), zIR and mq
    return List::create(Named("z") = Z, Named("A") = AA, Named("lambda") = L, Named("tau") = T, Named("zIR") = zIR - zmin, Named("mq") = mq );
} ;


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_GAMMALN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_GAMMALN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-euler
    This function object computes the natural logarithm of the absolute
    value of the Gamma function:
     \f$\displaystyle \log |\Gamma(x)|\f$

    @par Header <boost/simd/function/gammaln.hpp>

    @par Notes

    - The accuracy of the function is not uniformly good for negative entries
      The algorithm used is currently an adapted vesion of the cephes one.
      For better accuracy in the negative entry case, one can use the extern
      boost_math gammaln function but at a loss of speed.

      However, as stated in boost math:

      "While the relative errors near the positive roots of lgamma are very low,
       the  function has an infinite number of irrational roots for negative arguments:
       very close to these negative roots only a low absolute error can be guaranteed."

    - The call `gammaln(x, sgn)` also returns the sign of gamma in the output parameter @c sgn.

       Be aware that POSIX version of @c lgamma is not thread-safe: each execution of the function
       stores the sign of the gamma function of @c x in the static external variable signgam.

       boost.simd also provides @ref signgam which independantly computes the sign.

    @par Decorators

      - std_ fcalls @c std::lgamma

    @see gamma, signgam

    @par Example:

      @snippet gammaln.cpp gammaln

    @par Possible output:

      @snippet gammaln.txt gammaln
  **/
  IEEEValue gammaln(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/gammaln.hpp>
#include <boost/simd/function/simd/gammaln.hpp>

#endif


#include <cstdlib>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/triangular.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef ublas::vector<double> vector;
  typedef ublas::matrix<double, ublas::column_major> matrix;
  typedef ublas::triangular_matrix<double, ublas::upper, ublas::column_major> triangular_matrix;
  typedef typename vector::size_type size_type;

  rand_normal<double>::reset();
  size_type n=128;
  triangular_matrix A(n, n);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=0; i<=j; ++i)
      A(i, j)=rand_normal<double>::get();
  }
  {
    vector lambda(n);
    matrix vr(n, n);
    triangular_matrix A_bak(A);
    int info=lapack::spev('V', A, lambda, vr);
    if (info==0) {
      for (int i=0; i<n; ++i) {
  	// res <- A*vr(i) - lambda(i)*vr(i)
  	ublas::matrix_column<matrix> v(vr, i);
  	vector res(v);
   	blas::spmv(1., A_bak, v, -lambda(i), res);
  	std::cout << "norm of residual (right eigen vector " << i
  		  << " ): " << blas::nrm2(res) << '\n';
      }
    } else
      if (info>0)
  	std::cout << "unable to compute all eigen values\n";
      else
  	std::cout << "illegal arguments\n";
  }
  return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_ERFC_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_ERFC_HPP_INCLUDED

#include <nt2/euler/functions/erfc.hpp>
#include <nt2/euler/functions/details/erf_kernel.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/twothird.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/rec.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( erfc_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(nt2::is_nan(a0)) return a0;
      #endif
      A0 x =  nt2::abs(a0);
      A0 xx =  nt2::sqr(x);
      A0 z =  nt2::Zero<A0>();
      if(x <= A0(0.0000000001))
      {
        z = nt2::oneminus(x*nt2::Two<A0>()/nt2::sqrt(nt2::Pi<A0>()));
      }
      else if (x< A0(0.65))
      {
        z = nt2::oneminus(x*details::erf_kernel<A0>::erf1(xx));
      }
      else if(x< A0(2.2))
      {
        z = nt2::exp(-xx)*details::erf_kernel<A0>::erfc2(x);
      }
      else if(x< A0(6))
      {
        z = nt2::exp(-xx)*details::erf_kernel<A0>::erfc3(x);
      }
      else
      {
        z = nt2::exp(-xx)*details::erf_kernel<A0>::erfc4(rec(x));
      }
      return (a0 < 0.0) ? 2.0-z : z;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( erfc_, tag::cpu_
                              , (A0)
                              , ((scalar_<single_<A0> >))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      A0 x =  nt2::abs(a0);
      A0 r1 = nt2::Zero<A0>();
      A0 z =  x/oneplus(x);
      if (x < Twothird<A0>())
      {
        r1 = details::erf_kernel<A0>::erfc3(z);
      }
      #ifndef BOOST_SIMD_NO_INFINITIES
      else if (BOOST_UNLIKELY(x == Inf<A0>()))
      {
        r1 = Zero<A0>();
      }
      #endif
      else
      {
       z-= 0.4f;
        r1 = exp(-sqr(x))*details::erf_kernel<A0>::erfc2(z);
      }
      return (a0 < 0.0f) ? 2.0f-r1 : r1;
    }
  };

} }

#endif