MatrixBezier.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __geometry_MatrixBezier_h__
#define __geometry_MatrixBezier_h__

namespace fe
{
namespace ext
{

/**************************************************************************//**
    @brief solve B = A^^power, where A is a matrix

    @ingroup geometry

    This version uses a Quadratic Bezier approximation.

    The first column of the matrix is presumed to be the tangent.

    The power should be a real number between 0 and 1.

    For a proper matrix power function, see MatrixPower.

*//***************************************************************************/
template <typename MATRIX>
class MatrixBezier
{
    public:
                MatrixBezier(void)                                          {}

                template <typename T>
        void    solve(MATRIX& B, const MATRIX& A, T a_power) const;
};

template <typename MATRIX>
template <typename T>
inline void MatrixBezier<MATRIX>::solve(MATRIX& B, const MATRIX& A,
        T a_power) const
{
    const T linearity=0.45; //* TODO param

    const Vector<3,T>& rTranslation=A.translation();
    const T distance=magnitude(rTranslation);

    const MATRIX rotation(a_power*SpatialQuaternion(A));

    const T power1=T(1)-a_power;
    const T xStart=distance*a_power;
    const T xFull= -distance*power1;

    //* transform of X-only vector
    const Vector<3,T> locTip=xFull*A.column(0)+rTranslation;

    const Vector<3,T> locBlend(
            locTip[0]*a_power+xStart*power1,
            locTip[1]*a_power,
            locTip[2]*a_power);

    const Vector<3,T> locLinear=rTranslation*a_power;
    const Vector<3,T> locMix=locLinear*linearity+locBlend*(1.0-linearity);

    //* rotation of unit axes
    const Vector<3,T>& xArmBlend=rotation.column(0);
    const Vector<3,T>& yArmBlend=rotation.column(1);

    makeFrameTangentX(B,locMix,xArmBlend,yArmBlend);
}

} /* namespace ext */
} /* namespace fe */

#endif /* __geometry_MatrixBezier_h__ */