muda-doc/edge__edge__mollified_8inl_source.html

namespace muda::distance::details

{

template <class T>

MUDA_GENERIC void g_EECN2(

    T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T g[12])

{

    T t8;

    T t9;

    T t10;

    T t11;

    T t12;

    T t13;

    T t23;

    T t24;

    T t25;

    T t26;

    T t27;

    T t28;

    T t29;

    T t30;

    T t31;

    T t32;

    T t33;


    /* COMPUTEEECROSSSQNORMGRADIENT */

    /*     G = COMPUTEEECROSSSQNORMGRADIENT(V01,V02,V03,V11,V12,V13,V21,V22,V23,V31,V32,V33) */

    /*     This function was generated by the Symbolic Math Toolbox version 8.3. */

    /*     01-Nov-2019 16:54:23 */

    t8    = -v11 + v01;

    t9    = -v12 + v02;

    t10   = -v13 + v03;

    t11   = -v31 + v21;

    t12   = -v32 + v22;

    t13   = -v33 + v23;

    t23   = t8 * t12 + -(t9 * t11);

    t24   = t8 * t13 + -(t10 * t11);

    t25   = t9 * t13 + -(t10 * t12);

    t26   = t8 * t23 * 2.0;

    t27   = t9 * t23 * 2.0;

    t28   = t8 * t24 * 2.0;

    t29   = t10 * t24 * 2.0;

    t30   = t9 * t25 * 2.0;

    t31   = t10 * t25 * 2.0;

    t32   = t11 * t23 * 2.0;

    t33   = t12 * t23 * 2.0;

    t23   = t11 * t24 * 2.0;

    t10   = t13 * t24 * 2.0;

    t9    = t12 * t25 * 2.0;

    t8    = t13 * t25 * 2.0;

    g[0]  = t33 + t10;

    g[1]  = -t32 + t8;

    g[2]  = -t23 - t9;

    g[3]  = -t33 - t10;

    g[4]  = t32 - t8;

    g[5]  = t23 + t9;

    g[6]  = -t27 - t29;

    g[7]  = t26 - t31;

    g[8]  = t28 + t30;

    g[9]  = t27 + t29;

    g[10] = -t26 + t31;

    g[11] = -t28 - t30;

}

template <class T>

MUDA_GENERIC void H_EECN2(

    T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T H[144])

{

    T t8;

    T t9;

    T t10;

    T t11;

    T t12;

    T t13;

    T t32;

    T t33;

    T t34;

    T t35;

    T t48;

    T t36;

    T t49;

    T t37;

    T t38;

    T t39;

    T t40;

    T t41;

    T t42;

    T t43;

    T t44;

    T t45;

    T t46;

    T t47;

    T t50;

    T t51;

    T t52;

    T t20;

    T t23;

    T t24;

    T t25;

    T t86;

    T t87;

    T t88;

    T t74;

    T t75;

    T t76;

    T t77;

    T t78;

    T t79;

    T t89;

    T t90;

    T t91;

    T t92;

    T t93;

    T t94;

    T t95;


    /* COMPUTEEECROSSSQNORMHESSIAN */

    /*     H = COMPUTEEECROSSSQNORMHESSIAN(V01,V02,V03,V11,V12,V13,V21,V22,V23,V31,V32,V33) */

    /*     This function was generated by the Symbolic Math Toolbox version 8.3. */

    /*     01-Nov-2019 16:54:23 */

    t8     = -v11 + v01;

    t9     = -v12 + v02;

    t10    = -v13 + v03;

    t11    = -v31 + v21;

    t12    = -v32 + v22;

    t13    = -v33 + v23;

    t32    = t8 * t9 * 2.0;

    t33    = t8 * t10 * 2.0;

    t34    = t9 * t10 * 2.0;

    t35    = t8 * t11 * 2.0;

    t48    = t8 * t12;

    t36    = t48 * 2.0;

    t49    = t9 * t11;

    t37    = t49 * 2.0;

    t38    = t48 * 4.0;

    t48    = t8 * t13;

    t39    = t48 * 2.0;

    t40    = t49 * 4.0;

    t41    = t9 * t12 * 2.0;

    t49    = t10 * t11;

    t42    = t49 * 2.0;

    t43    = t48 * 4.0;

    t48    = t9 * t13;

    t44    = t48 * 2.0;

    t45    = t49 * 4.0;

    t49    = t10 * t12;

    t46    = t49 * 2.0;

    t47    = t48 * 4.0;

    t48    = t49 * 4.0;

    t49    = t10 * t13 * 2.0;

    t50    = t11 * t12 * 2.0;

    t51    = t11 * t13 * 2.0;

    t52    = t12 * t13 * 2.0;

    t20    = t8 * t8 * 2.0;

    t9     = t9 * t9 * 2.0;

    t8     = t10 * t10 * 2.0;

    t23    = t11 * t11 * 2.0;

    t24    = t12 * t12 * 2.0;

    t25    = t13 * t13 * 2.0;

    t86    = t35 + t41;

    t87    = t35 + t49;

    t88    = t41 + t49;

    t74    = t20 + t9;

    t75    = t20 + t8;

    t76    = t9 + t8;

    t77    = t23 + t24;

    t78    = t23 + t25;

    t79    = t24 + t25;

    t89    = t40 + -t36;

    t90    = t36 + -t40;

    t91    = t37 + -t38;

    t92    = t38 + -t37;

    t93    = t45 + -t39;

    t94    = t39 + -t45;

    t95    = t42 + -t43;

    t37    = t43 + -t42;

    t39    = t48 + -t44;

    t45    = t44 + -t48;

    t38    = t46 + -t47;

    t40    = t47 + -t46;

    t36    = -t35 + -t41;

    t13    = -t35 + -t49;

    t11    = -t41 + -t49;

    t12    = -t20 + -t9;

    t10    = -t20 + -t8;

    t8     = -t9 + -t8;

    t9     = -t23 + -t24;

    t49    = -t23 + -t25;

    t48    = -t24 + -t25;

    H[0]   = t79;

    H[1]   = -t50;

    H[2]   = -t51;

    H[3]   = t48;

    H[4]   = t50;

    H[5]   = t51;

    H[6]   = t11;

    H[7]   = t92;

    H[8]   = t37;

    H[9]   = t88;

    H[10]  = t91;

    H[11]  = t95;

    H[12]  = -t50;

    H[13]  = t78;

    H[14]  = -t52;

    H[15]  = t50;

    H[16]  = t49;

    H[17]  = t52;

    H[18]  = t89;

    H[19]  = t13;

    H[20]  = t40;

    H[21]  = t90;

    H[22]  = t87;

    H[23]  = t38;

    H[24]  = -t51;

    H[25]  = -t52;

    H[26]  = t77;

    H[27]  = t51;

    H[28]  = t52;

    H[29]  = t9;

    H[30]  = t93;

    H[31]  = t39;

    H[32]  = t36;

    H[33]  = t94;

    H[34]  = t45;

    H[35]  = t86;

    H[36]  = t48;

    H[37]  = t50;

    H[38]  = t51;

    H[39]  = t79;

    H[40]  = -t50;

    H[41]  = -t51;

    H[42]  = t88;

    H[43]  = t91;

    H[44]  = t95;

    H[45]  = t11;

    H[46]  = t92;

    H[47]  = t37;

    H[48]  = t50;

    H[49]  = t49;

    H[50]  = t52;

    H[51]  = -t50;

    H[52]  = t78;

    H[53]  = -t52;

    H[54]  = t90;

    H[55]  = t87;

    H[56]  = t38;

    H[57]  = t89;

    H[58]  = t13;

    H[59]  = t40;

    H[60]  = t51;

    H[61]  = t52;

    H[62]  = t9;

    H[63]  = -t51;

    H[64]  = -t52;

    H[65]  = t77;

    H[66]  = t94;

    H[67]  = t45;

    H[68]  = t86;

    H[69]  = t93;

    H[70]  = t39;

    H[71]  = t36;

    H[72]  = t11;

    H[73]  = t89;

    H[74]  = t93;

    H[75]  = t88;

    H[76]  = t90;

    H[77]  = t94;

    H[78]  = t76;

    H[79]  = -t32;

    H[80]  = -t33;

    H[81]  = t8;

    H[82]  = t32;

    H[83]  = t33;

    H[84]  = t92;

    H[85]  = t13;

    H[86]  = t39;

    H[87]  = t91;

    H[88]  = t87;

    H[89]  = t45;

    H[90]  = -t32;

    H[91]  = t75;

    H[92]  = -t34;

    H[93]  = t32;

    H[94]  = t10;

    H[95]  = t34;

    H[96]  = t37;

    H[97]  = t40;

    H[98]  = t36;

    H[99]  = t95;

    H[100] = t38;

    H[101] = t86;

    H[102] = -t33;

    H[103] = -t34;

    H[104] = t74;

    H[105] = t33;

    H[106] = t34;

    H[107] = t12;

    H[108] = t88;

    H[109] = t90;

    H[110] = t94;

    H[111] = t11;

    H[112] = t89;

    H[113] = t93;

    H[114] = t8;

    H[115] = t32;

    H[116] = t33;

    H[117] = t76;

    H[118] = -t32;

    H[119] = -t33;

    H[120] = t91;

    H[121] = t87;

    H[122] = t45;

    H[123] = t92;

    H[124] = t13;

    H[125] = t39;

    H[126] = t32;

    H[127] = t10;

    H[128] = t34;

    H[129] = -t32;

    H[130] = t75;

    H[131] = -t34;

    H[132] = t95;

    H[133] = t38;

    H[134] = t86;

    H[135] = t37;

    H[136] = t40;

    H[137] = t36;

    H[138] = t33;

    H[139] = t34;

    H[140] = t12;

    H[141] = -t33;

    H[142] = -t34;

    H[143] = t74;

}

template <class T>

MUDA_GENERIC void EEM(T input, T eps_x, T& e)

{

    T input_div_eps_x = input / eps_x;

    e                 = (-input_div_eps_x + 2.0) * input_div_eps_x;

}


template <class T>

MUDA_GENERIC void g_EEM(T input, T eps_x, T& g)

{

    T one_div_eps_x = 1.0 / eps_x;

    g               = 2.0 * one_div_eps_x * (-one_div_eps_x * input + 1.0);

}


template <class T>

MUDA_GENERIC void H_EEM(T input, T eps_x, T& H)

{

    H = -2.0 / (eps_x * eps_x);

}

}  // namespace muda::distance::details


namespace muda::distance

{

template <class T>

MUDA_GENERIC void edge_edge_cross_norm2(const Eigen::Vector<T, 3>& ea0,

                                        const Eigen::Vector<T, 3>& ea1,

                                        const Eigen::Vector<T, 3>& eb0,

                                        const Eigen::Vector<T, 3>& eb1,

                                        T&                         result)

{

    result = (ea1 - ea0).cross(eb1 - eb0).squaredNorm();

}


template <class T>

MUDA_GENERIC void edge_edge_cross_norm2_gradient(const Eigen::Vector<T, 3>& ea0,

                                                 const Eigen::Vector<T, 3>& ea1,

                                                 const Eigen::Vector<T, 3>& eb0,

                                                 const Eigen::Vector<T, 3>& eb1,

                                                 Eigen::Vector<T, 12>& grad)

{

    details::g_EECN2(ea0[0],

                     ea0[1],

                     ea0[2],

                     ea1[0],

                     ea1[1],

                     ea1[2],

                     eb0[0],

                     eb0[1],

                     eb0[2],

                     eb1[0],

                     eb1[1],

                     eb1[2],

                     grad.data());

}


template <class T>

MUDA_GENERIC void edge_edge_cross_norm2_hessian(const Eigen::Vector<T, 3>& ea0,

                                                const Eigen::Vector<T, 3>& ea1,

                                                const Eigen::Vector<T, 3>& eb0,

                                                const Eigen::Vector<T, 3>& eb1,

                                                Eigen::Matrix<T, 12, 12>& Hessian)

{

    details::H_EECN2(ea0[0],

                     ea0[1],

                     ea0[2],

                     ea1[0],

                     ea1[1],

                     ea1[2],

                     eb0[0],

                     eb0[1],

                     eb0[2],

                     eb1[0],

                     eb1[1],

                     eb1[2],

                     Hessian.data());

}


template <class T>

MUDA_GENERIC void edge_edge_mollifier(const Eigen::Vector<T, 3>& ea0,

                                      const Eigen::Vector<T, 3>& ea1,

                                      const Eigen::Vector<T, 3>& eb0,

                                      const Eigen::Vector<T, 3>& eb1,

                                      T                          eps_x,

                                      T&                         e)

{

    T EECrossSqNorm;

    edge_edge_cross_norm2(ea0, ea1, eb0, eb1, EECrossSqNorm);

    if(EECrossSqNorm < eps_x)

    {

        details::EEM(EECrossSqNorm, eps_x, e);

    }

    else

    {

        e = 1.0;

    }

}


template <class T>

MUDA_GENERIC void edge_edge_mollifier_gradient(const Eigen::Vector<T, 3>& ea0,

                                               const Eigen::Vector<T, 3>& ea1,

                                               const Eigen::Vector<T, 3>& eb0,

                                               const Eigen::Vector<T, 3>& eb1,

                                               T                          eps_x,

                                               Eigen::Vector<T, 12>&      g)

{

    T EECrossSqNorm;

    edge_edge_cross_norm2(ea0, ea1, eb0, eb1, EECrossSqNorm);

    if(EECrossSqNorm < eps_x)

    {

        T q_g;

        details::g_EEM(EECrossSqNorm, eps_x, q_g);

        edge_edge_cross_norm2_gradient(ea0, ea1, eb0, eb1, g);

        g *= q_g;

    }

    else

    {

        g.setZero();

    }

}


template <class T>

MUDA_GENERIC void edge_edge_mollifier_hessian(const Eigen::Vector<T, 3>& ea0,

                                              const Eigen::Vector<T, 3>& ea1,

                                              const Eigen::Vector<T, 3>& eb0,

                                              const Eigen::Vector<T, 3>& eb1,

                                              T                          eps_x,

                                              Eigen::Matrix<T, 12, 12>&  H)

{

    T EECrossSqNorm;

    edge_edge_cross_norm2(ea0, ea1, eb0, eb1, EECrossSqNorm);

    if(EECrossSqNorm < eps_x)

    {

        T q_g, q_H;

        details::g_EEM(EECrossSqNorm, eps_x, q_g);

        details::H_EEM(EECrossSqNorm, eps_x, q_H);


        Eigen::Matrix<T, 12, 1> g;

        edge_edge_cross_norm2_gradient(ea0, ea1, eb0, eb1, g);


        edge_edge_cross_norm2_hessian(ea0, ea1, eb0, eb1, H);

        H *= q_g;

        H += (q_H * g) * g.transpose();

    }

    else

    {

        H.setZero();

    }

}


template <class T>

MUDA_GENERIC void edge_edge_mollifier_threshold(const Eigen::Vector<T, 3>& ea0_rest,

                                                const Eigen::Vector<T, 3>& ea1_rest,

                                                const Eigen::Vector<T, 3>& eb0_rest,

                                                const Eigen::Vector<T, 3>& eb1_rest,

                                                T& eps_x)

{

    eps_x = 1.0e-3 * (ea0_rest - ea1_rest).squaredNorm()

            * (eb0_rest - eb1_rest).squaredNorm();

}


}  // namespace muda::distance