Angles3d.h

Go to the documentation of this file.

// Angles3d.h:
// -----------
//

#ifndef __IPSDKMATH_ANGLES3D_H__
#define __IPSDKMATH_ANGLES3D_H__

#include <IPSDKMath/IPSDKMathExports.h>
#include <IPSDKMath/Geometry/Angles.h>
#include <IPSDKMath/Geometry/3d/Point3d.h>
#include <IPSDKMath/Tools/MathUtils.h>
#include <IPSDKMath/LinearAlgebra/LinearAlgebraTypes.h>

namespace ipsdk {
namespace math {


template <typename TOut, typename TIn>
IPSDK_FORCEINLINE TOut
sphericalToXCartesian(const TIn rho,
                      const TIn cosTheta, const TIn sinTheta,
                      const TIn cosPhi, const TIn sinPhi)
{
    return math::round<TOut>(rho * sinTheta * cosPhi);
}
template <typename TOut, typename TIn>
IPSDK_FORCEINLINE TOut
sphericalToYCartesian(const TIn rho,
                      const TIn cosTheta, const TIn sinTheta,
                      const TIn cosPhi, const TIn sinPhi)
{
    return math::round<TOut>(rho * sinTheta * sinPhi);
}
template <typename TOut, typename TIn>
IPSDK_FORCEINLINE TOut
sphericalToZCartesian(const TIn rho,
                      const TIn cosTheta, const TIn sinTheta,
                      const TIn cosPhi, const TIn sinPhi)
{
    return math::round<TOut>(rho * cosTheta);
}
template <typename TOut, typename TIn>
IPSDK_FORCEINLINE void
sphericalToCartesian(const TIn rho, const TIn cosTheta, const TIn sinTheta,
                     const TIn cosPhi, const TIn sinPhi, TOut& x, TOut& y, TOut& z)
{
    x = sphericalToXCartesian<TOut>(rho, cosTheta, sinTheta, cosPhi, sinPhi);
    y = sphericalToYCartesian<TOut>(rho, cosTheta, sinTheta, cosPhi, sinPhi);
    z = sphericalToZCartesian<TOut>(rho, cosTheta, sinTheta, cosPhi, sinPhi);
}
template <typename TOut, typename TIn>
IPSDK_FORCEINLINE void
sphericalToCartesian(const TIn rho, const TIn theta, const TIn phi,
                     TOut& x, TOut& y, TOut& z)
{
    sphericalToCartesian(rho, std::cos(theta), std::sin(theta),
                         std::cos(phi), std::sin(phi), x, y, z);
}

template <typename TOut, typename TIn>
IPSDK_FORCEINLINE void
cartesianToSpherical(const TIn x, const TIn y, const TIn z,
                     TOut& rho, TOut& theta, TOut& phi)
{
    rho = math::round<TOut>(normL2(x, y, z));
    if (rho != 0)
        theta = std::acos(z / rho);
    else
        theta = 0;
    phi = std::atan2(y, x);
}

template <typename T>
IPSDK_FORCEINLINE void
eulerToMatrix(const T cosChi, const T sinChi,
              const T cosBeta, const T sinBeta,
              const T cosAlpha, const T sinAlpha,
              T& rxx, T& rxy, T& rxz,
              T& ryx, T& ryy, T& ryz,
              T& rzx, T& rzy, T& rzz)
{
    rxx = cosChi*cosBeta;
    rxy = cosChi*sinBeta*sinAlpha - sinChi*cosAlpha;
    rxz = cosChi*sinBeta*cosAlpha + sinChi*sinAlpha;
    ryx = sinChi*cosBeta;
    ryy = sinChi*sinBeta*sinAlpha + cosChi*cosAlpha;
    ryz = sinChi*sinBeta*cosAlpha - cosChi*sinAlpha;
    rzx = -sinBeta;
    rzy = cosBeta*sinAlpha;
    rzz = cosBeta*cosAlpha;
}
template <typename T>
IPSDK_FORCEINLINE void
eulerToMatrix(const T chi, const T beta, const T alpha,
              T& rxx, T& rxy, T& rxz,
              T& ryx, T& ryy, T& ryz,
              T& rzx, T& rzy, T& rzz)
{
    eulerToMatrix(std::cos(chi), std::sin(chi),
                  std::cos(beta), std::sin(beta),
                  std::cos(alpha), std::sin(alpha),
                  rxx, rxy, rxz,
                  ryx, ryy, ryz,
                  rzx, rzy, rzz);
}
IPSDK_FORCEINLINE Matrix3d
eulerToMatrix(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha)
{
    Matrix3d matRot;
    eulerToMatrix(chi, beta, alpha,
                  matRot(0, 0), matRot(0, 1), matRot(0, 2),
                  matRot(1, 0), matRot(1, 1), matRot(1, 2),
                  matRot(2, 0), matRot(2, 1), matRot(2, 2));

    return matRot;
}

template <typename T>
IPSDK_FORCEINLINE void
matrixToEuler(const T rxx, const T rxy, const T rxz,
              const T ryx, const T ryy, const T ryz,
              const T rzx, const T rzy, const T rzz,
              T& chi, T& beta, T& alpha)
{
    // retrieve epsilon value
    const T epsilonValue = NumericLimits<T>::sqrt_epsilon();

    // handle degenerate case
    if ((1-std::abs(rzx)) > epsilonValue) {

        beta = -std::asin(rzx);
        const T cosBeta = cos(beta);
        chi = std::atan2(ryx/cosBeta, rxx/cosBeta);
        
        alpha = std::atan2(rzy/cosBeta, rzz/cosBeta);
    }
    else { // degenerate case

        chi = 0;
        if (rzx < 0) {
            beta = static_cast<T>(M_PI / 2);
            alpha = std::atan2(rxy, rxz);
        }
        else {
            beta = static_cast<T>(-M_PI / 2);
            alpha = std::atan2(-rxy, -rxz);
        }
    }
}
IPSDK_FORCEINLINE void
matrixToEuler(const Matrix3d& matRot,
              ipReal64& chi, ipReal64& beta, ipReal64& alpha)
{
    matrixToEuler(matRot(0, 0), matRot(0, 1), matRot(0, 2),
                  matRot(1, 0), matRot(1, 1), matRot(1, 2),
                  matRot(2, 0), matRot(2, 1), matRot(2, 2),
                  chi, beta, alpha);
}

IPSDKMATH_API void
eulerToQuaternion(const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
                  ipReal64& q0, ipReal64& q1, ipReal64& q2, ipReal64& q3);

IPSDKMATH_API void
quaternionToEuler(const ipReal64 q0, const ipReal64 q1, const ipReal64 q2, const ipReal64 q3,
                  ipReal64& chi, ipReal64& beta, ipReal64& alpha);

template <typename T>
IPSDK_FORCEINLINE void
rotate3d(const T xIn, const T yIn, const T zIn,
         const ipReal64 rxx, const ipReal64 rxy, const ipReal64 rxz,
         const ipReal64 ryx, const ipReal64 ryy, const ipReal64 ryz,
         const ipReal64 rzx, const ipReal64 rzy, const ipReal64 rzz,
         T& xOut, T& yOut, T& zOut)
{
    xOut = math::round<T>(rxx * xIn + rxy * yIn + rxz * zIn);
    yOut = math::round<T>(ryx * xIn + ryy * yIn + ryz * zIn);
    zOut = math::round<T>(rzx * xIn + rzy * yIn + rzz * zIn);
}
template <typename T>
IPSDK_FORCEINLINE void
rotate3dInSitu(const ipReal64 rxx, const ipReal64 rxy, const ipReal64 rxz,
               const ipReal64 ryx, const ipReal64 ryy, const ipReal64 ryz,
               const ipReal64 rzx, const ipReal64 rzy, const ipReal64 rzz,
               T& x, T& y, T& z)
{
    T xOut, yOut, zOut;
    rotate3d(x, y, z,
             rxx, rxy, rxz,
             ryx, ryy, ryz,
             rzx, rzy, rzz,
             xOut, yOut, zOut);
    x = xOut;
    y = yOut;
    z = zOut;
}
template <typename T>
IPSDK_FORCEINLINE void
rotate3d(const T xIn, const T yIn, const T zIn,
         const ipReal64 cosChi, const ipReal64 sinChi,
         const ipReal64 cosBeta, const ipReal64 sinBeta,
         const ipReal64 cosAlpha, const ipReal64 sinAlpha,
         T& xOut, T& yOut, T& zOut)
{
    ipReal64 rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz;
    eulerToMatrix(cosChi, sinChi, cosBeta, sinBeta, cosAlpha, sinAlpha,
                  rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz);
    rotate3d(xIn, yIn, zIn,
             rxx, rxy, rxz,
             ryx, ryy, ryz,
             rzx, rzy, rzz,
             xOut, yOut, zOut);
}
template <typename T>
IPSDK_FORCEINLINE void
rotate3dInSitu(const ipReal64 cosChi, const ipReal64 sinChi,
               const ipReal64 cosBeta, const ipReal64 sinBeta,
               const ipReal64 cosAlpha, const ipReal64 sinAlpha,
               T& x, T& y, T& z)
{
    T xOut, yOut, zOut;
    rotate3d(x, y, z,
             cosChi, sinChi,
             cosBeta, sinBeta,
             cosAlpha, sinAlpha,
             xOut, yOut, zOut);
    x = xOut;
    y = yOut;
    z = zOut;
}
template <typename T>
IPSDK_FORCEINLINE void
rotate3d(const T xIn, const T yIn, const T zIn,
         const ipReal64 chi, const ipReal64 beta, const ipReal64 alpha,
         T& xOut, T& yOut, T& zOut)
{
    rotate3d(xIn, yIn, zIn,
             std::cos(chi), std::sin(chi),
             std::cos(beta), std::sin(beta),
             std::cos(alpha), std::sin(alpha),
             xOut, yOut, zOut);
}


} // end of namespace math
} // end of namespace ipsdk

#endif // __IPSDKMATH_ANGLES3D_H__