SingularityJudger/src/loop_detector.cpp

#include "loop_detector.h"
#include <utility>

const float PI = 3.1415927;

LoopDetector::
LoopDetector(const vector<vector<glm::vec3>> &s_evaluation_, const vector<vector<glm::vec3>> &f_evaluation_,
             const vector<vector<glm::vec3>> &s_tangent_u_, const vector<vector<glm::vec3>> &f_tangent_u_,
             const vector<vector<glm::vec3>> &s_tangent_v_, const vector<vector<glm::vec3>> &f_tangent_v_,
             const vector<vector<glm::vec3>> &s_normal_, const vector<vector<glm::vec3>> &f_normal_,
             const tinynurbs::RationalSurface<float> &s_)
        : s_evaluation(s_evaluation_), f_evaluation(f_evaluation_),
          s_tangent_u(s_tangent_u_), s_tangent_v(s_tangent_v_),
          f_tangent_u(f_tangent_u_), f_tangent_v(f_tangent_v_),
          s_normal(s_normal_), f_normal(f_normal_), s(s_) {}

void LoopDetector::initOrientedDistance() {

    selectedPointsIdx = vector<vector<pair<int, int>>>(s_subPatchEdgeSampleCnt_u,
                                                       vector<pair<int, int>>(s_subPatchEdgeSampleCnt_v));
//    distance = vector<vector<glm::vec3>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec3>(s_subPatchEdgeSampleCnt_v));

    for (int i = 0; i < s_subPatchEdgeSampleCnt_u; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v; j++) {
            float minDis = FLT_MAX;
            int minDisFIdx_u = -1, minDisFIdx_v = -1;
            for (int k = 0; k < f_subPatchEdgeSampleCnt_u; k++) {
                for (int l = 0; l < f_subPatchEdgeSampleCnt_v; l++) {
                    auto dis = glm::distance(
                            s_evaluation[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j],
                            f_evaluation[f_subPatchIdxRange_u.first + k][f_subPatchIdxRange_v.first + l]);
                    // 确定f上的对应点时，依据的最小欧氏距离，而不是有向距离
                    if (dis < minDis) {
                        minDis = dis;
                        minDisFIdx_u = k;
                        minDisFIdx_v = l;
                    }
                }
            }
//            auto N = glm::normalize(s_evaluation[i][j] - f_evaluation[minDisFIdx_u][minDisFIdx_v]);
//            // 直接normalize得到的单位向量总是与SF同向，但实际上N应当总是与f的一侧法向量成锐角
//            auto testAngle = N * glm::cross(f_tangent_u[minDisFIdx_u][minDisFIdx_v], f_tangent_v[minDisFIdx_u][minDisFIdx_v]);
//            N = testAngle.x + testAngle.y + testAngle.z > 0 ? N : -N;
//            distance[i][j] = N * (s_evaluation[i][j] - f_evaluation[minDisFIdx_u][minDisFIdx_v]);
            selectedPointsIdx[i][j] = {minDisFIdx_u, minDisFIdx_v};
        }
    }
}

// Yawei Ma的文章中的做法
void LoopDetector::initVectorField0() {
    vectorFields = vector<vector<glm::vec2>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec2>(s_subPatchEdgeSampleCnt_v));
    for (int i = 0; i < s_subPatchEdgeSampleCnt_u; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v; j++) {
            auto fPointIdx = selectedPointsIdx[i][j];
            auto N = glm::normalize(s_evaluation[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j] -
                                    f_evaluation[f_subPatchIdxRange_u.first + fPointIdx.first]
                                    [f_subPatchIdxRange_v.first + fPointIdx.second]);
            if (isnan(N.x) || isnan(N.y) || isnan(N.z)) {
                // 绝了，曲面s和f的交线正好经过采样点，此时就无法定义向量N
                // 这里取两个曲面在交点的法向量的平均做近似
//                auto tmp1 = glm::cross(s_tangent_u[i][j], s_tangent_v[i][j]);
//                auto tmp2 = glm::cross(f_tangent_u[fPointIdx.first][fPointIdx.second],
//                                       f_tangent_v[fPointIdx.first][fPointIdx.second]);
                N = glm::normalize(s_normal[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j] +
                                   f_normal[f_subPatchIdxRange_u.first + fPointIdx.first]
                                   [f_subPatchIdxRange_v.first + fPointIdx.second]);
            }
            // u1、u2两个向量构成和N垂直的曲面
            glm::vec3 u1(1, 1, 1);
            if (N.z != 0)u1.z = (-N.x - N.y) / N.z;
            else if (N.y != 0)u1.y = (-N.x - N.z) / N.y;
            else u1.x = (-N.y - N.z) / N.x;
            u1 = glm::normalize(u1);
            auto u2 = glm::normalize(glm::cross(N, u1));
            // s，f曲面在两个方向上的偏导
            auto ps_pu = s_tangent_u[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j]
            , ps_pv = s_tangent_v[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j];
            auto pf_pp = f_tangent_u[f_subPatchIdxRange_u.first + fPointIdx.first]
            [f_subPatchIdxRange_v.first + fPointIdx.second], pf_pq = f_tangent_v[f_subPatchIdxRange_u.first +
                                                                                 fPointIdx.first]
            [f_subPatchIdxRange_v.first + fPointIdx.second];
            // 构造Aij矩阵，见<Detection of loops and singularities of surface intersections> APPENDIX (A7)
            auto inverseAij = glm::inverse(
                    glm::mat2x2(glm::dot(u1, pf_pp), glm::dot(u1, pf_pq), glm::dot(u2, pf_pp), glm::dot(u2, pf_pq))
            );
            auto mBmn = glm::mat2x2(glm::dot(u1, ps_pu), glm::dot(u1, ps_pv), glm::dot(u2, ps_pu), glm::dot(u2, ps_pv));
            auto tmp = glm::vec2(glm::dot(N, pf_pp), glm::dot(N, pf_pq));
            auto vNfpNfq_inverseAij = glm::vec2(glm::dot(N, pf_pp), glm::dot(N, pf_pq)) * inverseAij;
            auto pd_pu = glm::dot(N, ps_pu) -
                         glm::dot(vNfpNfq_inverseAij, glm::vec2(glm::dot(u1, ps_pu), glm::dot(u2, ps_pu)));
            auto pd_pv = glm::dot(N, ps_pv) -
                         glm::dot(vNfpNfq_inverseAij, glm::vec2(glm::dot(u1, ps_pv), glm::dot(u2, ps_pv)));
            vectorFields[i][j] = glm::vec2(pd_pu, pd_pv);
        }
    }
    int a = 1;
    int b = 2;
}

// G A Kriezis的文章中的做法
void LoopDetector::initVectorField() {
    vectorFields = vector<vector<glm::vec2>>(s_subPatchEdgeSampleCnt_u, vector<glm::vec2>(s_subPatchEdgeSampleCnt_v));
    for (int i = 0; i < s_subPatchEdgeSampleCnt_u; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v; j++) {
            auto fPtRelatedIdx = selectedPointsIdx[i][j];
            auto n2 = f_normal[f_subPatchIdxRange_u.first + fPtRelatedIdx.first][f_subPatchIdxRange_v.first +
                                                                                 fPtRelatedIdx.second];
//            glm::vec3 n2(1.,0.,0.);
            auto ps_pu = s_tangent_u[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j];
            auto ps_pv = s_tangent_v[s_subPatchIdxRange_u.first + i][s_subPatchIdxRange_v.first + j];
            vectorFields[i][j] = glm::vec2(glm::dot(n2, ps_pu), glm::dot(n2, ps_pv));
        }
    }
}

void LoopDetector::getRotationNumber() {
    // vectorFields的参数域是与s（第一个曲面）一致的
    auto edgeSampleCnt = s_evaluation.size();
    auto uParamCellSize = (*(s.knots_u.end() - 1) - *s.knots_u.begin()) / (float) edgeSampleCnt;
    auto vParamCellSize = (*(s.knots_v.end() - 1) - *s.knots_v.begin()) / (float) edgeSampleCnt;
    vector<vector<float>> pu_pt = {{0.,  1.},
                                   {-1., 0.}};
    vector<vector<float>> pv_pt = {{1., 0.},
                                   {0., -1.}};
    rotationNumbers = vector<vector<int>>(s_subPatchEdgeSampleCnt_u,
                                                     vector<int>(s_subPatchEdgeSampleCnt_v));

    // 以小格子为单位遍历
    for (int i = 0; i < s_subPatchEdgeSampleCnt_u - 1; i++) {
        for (int j = 0; j < s_subPatchEdgeSampleCnt_v - 1; j++) {
            vector<vector<float>> F(2, vector<float>(2));
            vector<vector<float>> G(2, vector<float>(2));
            for (int biasI = 0; biasI < 2; biasI++) {
                for (int biasJ = 0; biasJ < 2; biasJ++) {
                    auto idxI = i + biasI;
                    auto idxJ = j + biasJ;
                    auto v = vectorFields[idxI][idxJ];
                    auto vSquareSum = v.x * v.x + v.y * v.y;
                    auto pTheta_pChi = -v.y / vSquareSum;
                    auto pTheta_pPhi = v.x / vSquareSum;
//                    auto pChi_pu =
//                            (vectorFields[idxI + 1][idxJ].x - vectorFields[idxI - 1][idxJ].x) / (2 * uParamCellSize);
//                    auto pChi_pv =
//                            (vectorFields[idxI][idxJ + 1].x - vectorFields[idxI][idxJ - 1].x) / (2 * vParamCellSize);
                    glm::vec2 pV_pu;
                    glm::vec2 pV_pv;
                    if (idxI == 0)
                        pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI][idxJ]) / uParamCellSize;
                    else if (idxI == s_subPatchEdgeSampleCnt_u - 1)
                        pV_pu = (vectorFields[idxI][idxJ] - vectorFields[idxI - 1][idxJ]) / uParamCellSize;
                    else
                        pV_pu = (vectorFields[idxI + 1][idxJ] - vectorFields[idxI - 1][idxJ]) / (2 * uParamCellSize);
                    if (idxJ == 0)
                        pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ]) / vParamCellSize;
                    else if (idxJ == s_subPatchEdgeSampleCnt_v - 1)
                        pV_pv = (vectorFields[idxI][idxJ] - vectorFields[idxI][idxJ - 1]) / vParamCellSize;
                    else
                        pV_pv = (vectorFields[idxI][idxJ + 1] - vectorFields[idxI][idxJ - 1]) / (2 * vParamCellSize);
                    F[biasI][biasJ] = pTheta_pChi * pV_pu.x + pTheta_pPhi * pV_pu.y;
                    G[biasI][biasJ] = pTheta_pChi * pV_pv.x + pTheta_pPhi * pV_pv.y;
                }
            }
            rotationNumbers[i][j] = int(
                    round(((F[0][1] + F[1][1] - F[0][0] - F[1][0]) * uParamCellSize +
                           (G[0][0] + G[0][1] - G[1][0] - G[1][1]) * vParamCellSize) / 2.));
        }
    }
    int a = 3;
    int b = 1;
}

vector<pair<int, int>> LoopDetector::detect(pair<int, int> _s_subPatchIdxRange_u, pair<int, int> _s_subPatchIdxRange_v,
                                            pair<int, int> _f_subPatchIdxRange_u,
                                            pair<int, int> _f_subPatchIdxRange_v) {
    s_subPatchIdxRange_u = _s_subPatchIdxRange_u;
    s_subPatchIdxRange_v = _s_subPatchIdxRange_v;
    f_subPatchIdxRange_u = _f_subPatchIdxRange_u;
    f_subPatchIdxRange_v = _f_subPatchIdxRange_v;

    // subPatch每条边上的采样点个数。边上的格子个数=range.second-range.first+1，采样点个数=格子个数+1
    s_subPatchEdgeSampleCnt_u = s_subPatchIdxRange_u.second - s_subPatchIdxRange_u.first + 2;
    s_subPatchEdgeSampleCnt_v = s_subPatchIdxRange_v.second - s_subPatchIdxRange_v.first + 2;
    f_subPatchEdgeSampleCnt_u = f_subPatchIdxRange_u.second - f_subPatchIdxRange_u.first + 2;
    f_subPatchEdgeSampleCnt_v = f_subPatchIdxRange_v.second - f_subPatchIdxRange_v.first + 2;

    initOrientedDistance();
    initVectorField();
    getRotationNumber();
    return {};
}