Skip to content

File analytic_inverse.h

File List > eigen > inverse > analytic_inverse.h

Go to the documentation of this file

#pragma once
#include <muda/muda_def.h>
#include <muda/ext/eigen/eigen_core_cxx20.h>

namespace muda::eigen
{
struct AnalyticalInverse
{
    template <typename T>
    MUDA_INLINE MUDA_GENERIC Eigen::Matrix<T, 2, 2> operator()(const Eigen::Matrix<T, 2, 2>& m)
    {
        Eigen::Matrix<T, 2, 2> result;
        invert2x2(m.data(), result.data());
        return result;
    }

    template <typename T>
    MUDA_INLINE MUDA_GENERIC Eigen::Matrix<T, 3, 3> operator()(const Eigen::Matrix<T, 3, 3>& m)
    {
        Eigen::Matrix<T, 3, 3> result;
        invert3x3(m.data(), result.data());
        return result;
    }

    template <typename T>
    MUDA_INLINE MUDA_GENERIC Eigen::Matrix<T, 4, 4> operator()(const Eigen::Matrix<T, 4, 4>& m)
    {
        Eigen::Matrix<T, 4, 4> result;
        invert4x4(m.data(), result.data());
        return result;
    }

  private:
    template <typename T>
    MUDA_INLINE MUDA_GENERIC void invert2x2(const T* src, T* dst)
    {
        T det;

        /* Compute adjoint: */

        dst[0] = +src[3];
        dst[1] = -src[1];
        dst[2] = -src[2];
        dst[3] = +src[0];

        /* Compute determinant: */

        det = src[0] * dst[0] + src[1] * dst[2];

        /* Multiply adjoint with reciprocal of determinant: */

        det = 1.0f / det;

        dst[0] *= det;
        dst[1] *= det;
        dst[2] *= det;
        dst[3] *= det;
    }

    template <typename T>
    MUDA_INLINE MUDA_GENERIC void invert3x3(const T* src, T* dst)
    {
        T det;

        /* Compute adjoint: */

        dst[0] = +src[4] * src[8] - src[5] * src[7];
        dst[1] = -src[1] * src[8] + src[2] * src[7];
        dst[2] = +src[1] * src[5] - src[2] * src[4];
        dst[3] = -src[3] * src[8] + src[5] * src[6];
        dst[4] = +src[0] * src[8] - src[2] * src[6];
        dst[5] = -src[0] * src[5] + src[2] * src[3];
        dst[6] = +src[3] * src[7] - src[4] * src[6];
        dst[7] = -src[0] * src[7] + src[1] * src[6];
        dst[8] = +src[0] * src[4] - src[1] * src[3];

        /* Compute determinant: */

        det = src[0] * dst[0] + src[1] * dst[3] + src[2] * dst[6];

        /* Multiply adjoint with reciprocal of determinant: */

        det = 1.0f / det;

        dst[0] *= det;
        dst[1] *= det;
        dst[2] *= det;
        dst[3] *= det;
        dst[4] *= det;
        dst[5] *= det;
        dst[6] *= det;
        dst[7] *= det;
        dst[8] *= det;
    }

    template <typename T>
    MUDA_INLINE MUDA_GENERIC void invert4x4(const T* src, T* dst)
    {
        T det;

        /* Compute adjoint: */

        dst[0] = +src[5] * src[10] * src[15] - src[5] * src[11] * src[14]
                 - src[9] * src[6] * src[15] + src[9] * src[7] * src[14]
                 + src[13] * src[6] * src[11] - src[13] * src[7] * src[10];

        dst[1] = -src[1] * src[10] * src[15] + src[1] * src[11] * src[14]
                 + src[9] * src[2] * src[15] - src[9] * src[3] * src[14]
                 - src[13] * src[2] * src[11] + src[13] * src[3] * src[10];

        dst[2] = +src[1] * src[6] * src[15] - src[1] * src[7] * src[14]
                 - src[5] * src[2] * src[15] + src[5] * src[3] * src[14]
                 + src[13] * src[2] * src[7] - src[13] * src[3] * src[6];

        dst[3] = -src[1] * src[6] * src[11] + src[1] * src[7] * src[10]
                 + src[5] * src[2] * src[11] - src[5] * src[3] * src[10]
                 - src[9] * src[2] * src[7] + src[9] * src[3] * src[6];

        dst[4] = -src[4] * src[10] * src[15] + src[4] * src[11] * src[14]
                 + src[8] * src[6] * src[15] - src[8] * src[7] * src[14]
                 - src[12] * src[6] * src[11] + src[12] * src[7] * src[10];

        dst[5] = +src[0] * src[10] * src[15] - src[0] * src[11] * src[14]
                 - src[8] * src[2] * src[15] + src[8] * src[3] * src[14]
                 + src[12] * src[2] * src[11] - src[12] * src[3] * src[10];

        dst[6] = -src[0] * src[6] * src[15] + src[0] * src[7] * src[14]
                 + src[4] * src[2] * src[15] - src[4] * src[3] * src[14]
                 - src[12] * src[2] * src[7] + src[12] * src[3] * src[6];

        dst[7] = +src[0] * src[6] * src[11] - src[0] * src[7] * src[10]
                 - src[4] * src[2] * src[11] + src[4] * src[3] * src[10]
                 + src[8] * src[2] * src[7] - src[8] * src[3] * src[6];

        dst[8] = +src[4] * src[9] * src[15] - src[4] * src[11] * src[13]
                 - src[8] * src[5] * src[15] + src[8] * src[7] * src[13]
                 + src[12] * src[5] * src[11] - src[12] * src[7] * src[9];

        dst[9] = -src[0] * src[9] * src[15] + src[0] * src[11] * src[13]
                 + src[8] * src[1] * src[15] - src[8] * src[3] * src[13]
                 - src[12] * src[1] * src[11] + src[12] * src[3] * src[9];

        dst[10] = +src[0] * src[5] * src[15] - src[0] * src[7] * src[13]
                  - src[4] * src[1] * src[15] + src[4] * src[3] * src[13]
                  + src[12] * src[1] * src[7] - src[12] * src[3] * src[5];

        dst[11] = -src[0] * src[5] * src[11] + src[0] * src[7] * src[9]
                  + src[4] * src[1] * src[11] - src[4] * src[3] * src[9]
                  - src[8] * src[1] * src[7] + src[8] * src[3] * src[5];

        dst[12] = -src[4] * src[9] * src[14] + src[4] * src[10] * src[13]
                  + src[8] * src[5] * src[14] - src[8] * src[6] * src[13]
                  - src[12] * src[5] * src[10] + src[12] * src[6] * src[9];

        dst[13] = +src[0] * src[9] * src[14] - src[0] * src[10] * src[13]
                  - src[8] * src[1] * src[14] + src[8] * src[2] * src[13]
                  + src[12] * src[1] * src[10] - src[12] * src[2] * src[9];

        dst[14] = -src[0] * src[5] * src[14] + src[0] * src[6] * src[13]
                  + src[4] * src[1] * src[14] - src[4] * src[2] * src[13]
                  - src[12] * src[1] * src[6] + src[12] * src[2] * src[5];

        dst[15] = +src[0] * src[5] * src[10] - src[0] * src[6] * src[9]
                  - src[4] * src[1] * src[10] + src[4] * src[2] * src[9]
                  + src[8] * src[1] * src[6] - src[8] * src[2] * src[5];

        /* Compute determinant: */

        det = +src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];

        /* Multiply adjoint with reciprocal of determinant: */

        det = 1.0f / det;

        dst[0] *= det;
        dst[1] *= det;
        dst[2] *= det;
        dst[3] *= det;
        dst[4] *= det;
        dst[5] *= det;
        dst[6] *= det;
        dst[7] *= det;
        dst[8] *= det;
        dst[9] *= det;
        dst[10] *= det;
        dst[11] *= det;
        dst[12] *= det;
        dst[13] *= det;
        dst[14] *= det;
        dst[15] *= det;
    }
};
}  // namespace muda::eigen::inverse