Geometry.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <Debug.h>
#include <VisitedMask.h>
#include <array>
#include <cmath>
#include <stack>
 
namespace ttk {
 
  namespace Geometry {
 
    template <typename T>
    T angle(const T *vA0, const T *vA1, const T *vB0, const T *vB1);
 
    template <typename T>
    T angle2D(const T *vA0, const T *vA1, const T *vB0, const T *vB1);
 
    // Computes the BAC angle, in the interval [0, 2pi]
    template <typename T>
    inline double angle2DUndirected(const T *vA, const T *vB, const T *vC) {
      double angle = angle2D<T>(vA, vB, vA, vC);
      if(angle < 0) {
        angle += 2 * M_PI;
      }
      return angle;
    }
 
    template <typename T>
    bool areVectorsColinear(const T *vA0,
                            const T *vA1,
                            const T *vB0,
                            const T *vB1,
                            std::array<T, 3> *coefficients = nullptr,
                            const T *tolerance = NULL);
    template <typename T>
    bool isTriangleColinear2D(const T *pptA,
                              const T *pptB,
                              const T *pptC,
                              const T tolerance);
 
    template <typename T>
    int computeBarycentricCoordinates(const T *p0,
                                      const T *p1,
                                      const T *p,
                                      std::array<T, 2> &baryCentrics,
                                      const int &dimension = 3);
 
    template <typename T>
    int computeBarycentricCoordinates(const T *p0,
                                      const T *p1,
                                      const T *p2,
                                      const T *p,
                                      std::array<T, 3> &baryCentrics);
 
    template <typename T>
    bool computeSegmentIntersection(const T &xA,
                                    const T &yA,
                                    const T &xB,
                                    const T &yB,
                                    const T &xC,
                                    const T &yC,
                                    const T &xD,
                                    const T &yD,
                                    T &x,
                                    T &y);
 
    template <typename T>
    int computeTriangleAngles(const T *p0,
                              const T *p1,
                              const T *p2,
                              std::array<T, 3> &angles);
 
    // Get the angle opposite to edge s2 using cosine law
    template <typename T>
    int computeTriangleAngleFromSides(const T s0,
                                      const T s1,
                                      const T s2,
                                      T &angle);
 
    template <typename T>
    int computeTriangleArea(const T *p0, const T *p1, const T *p2, T &area);
 
    template <typename T>
    int
      computeTriangleAreaFromSides(const T s0, const T s1, const T s2, T &area);
 
    template <typename T>
    int crossProduct(const T *vA0,
                     const T *vA1,
                     const T *vB0,
                     const T *vB1,
                     std::array<T, 3> &crossProduct);
 
    template <typename T>
    int crossProduct(const T *vA, const T *vB, T *crossProduct);
 
    template <typename T>
    T distance(const T *p0, const T *p1, const int &dimension = 3);
 
    template <typename T>
    T distance(const std::vector<T> &p0, const std::vector<T> &p1);
 
    template <typename T>
    inline T distance2D(const T *p0, const T *p1) {
      T distance = 0;
      T di0 = (p0[0] - p1[0]);
      T di1 = (p0[1] - p1[1]);
      if(std::is_same<T, float>::value) { // TODO constexpr when c++17
        distance = fmaf(di0, di0, distance);
        distance = fmaf(di1, di1, distance);
      } else {
        distance = fma(di0, di0, distance);
        distance = fma(di1, di1, distance);
      }
 
      return sqrt(distance);
    }
 
    template <typename T>
    T distanceFlatten(const std::vector<std::vector<T>> &p0,
                      const std::vector<std::vector<T>> &p1);
 
    template <typename T>
    T distanceFlatten(const std::vector<std::vector<T>> &p0,
                      const std::vector<std::vector<T>> &p1);
 
    template <typename T>
    T dotProduct(const T *vA0, const T *vA1, const T *vB0, const T *vB1);
 
    template <typename T>
    T dotProduct(const T *vA, const T *vB, const int &dimension = 3);
 
    template <typename T>
    T dotProduct(const std::vector<T> &vA, const std::vector<T> &vB);
 
    template <typename T>
    T dotProductFlatten(const std::vector<std::vector<T>> &vA,
                        const std::vector<std::vector<T>> &vB);
 
    template <typename T, typename Container, size_t dim>
    int getBoundingBox(const Container &points,
                       std::array<std::pair<T, T>, dim> &bBox) {
      if(points.empty()) {
        return -1;
      }
 
      for(size_t i = 0; i < points.size(); i++) {
 
        if(i == 0) {
          for(size_t j = 0; j < dim; j++) {
            bBox[j].first = points[i][j];
            bBox[j].second = points[i][j];
          }
        } else {
          for(size_t j = 0; j < dim; j++) {
            if(points[i][j] < bBox[j].first) {
              bBox[j].first = points[i][j];
            }
            if(points[i][j] > bBox[j].second) {
              bBox[j].second = points[i][j];
            }
          }
        }
      }
 
      return 0;
    }
 
    template <typename T>
    bool isPointInTriangle(const T *p0, const T *p1, const T *p2, const T *p);
 
    template <typename T>
    bool isPointOnSegment(const T &x,
                          const T &y,
                          const T &xA,
                          const T &yA,
                          const T &xB,
                          const T &yB);
 
    template <typename T>
    bool isPointOnSegment(const T *p,
                          const T *pA,
                          const T *pB,
                          const int &dimension = 3);
 
    template <typename T>
    bool isTriangleColinear(const T *p0,
                            const T *p1,
                            const T *p2,
                            const T *tolerance = nullptr);
 
    template <typename T>
    T magnitude(const T *v, const int &dimension = 3);
 
    template <typename T>
    T magnitude(const std::vector<T> &v);
 
    template <typename T>
    T magnitudeFlatten(const std::vector<std::vector<T>> &v);
 
    template <typename T>
    T magnitude(const T *o, const T *d);
 
    template <typename T>
    inline T powInt(const T val, const int n) {
      if(n < 0) {
        return 1.0 / powInt(val, -n);
      } else if(n == 0) {
        return 1;
      } else if(n == 1) {
        return val;
      } else if(n == 2) {
        return val * val;
      } else if(n == 3) {
        return val * val * val;
      } else {
        T ret = val;
        for(int i = 0; i < n - 1; ++i) {
          ret *= val;
        }
        return ret;
      }
    }
 
    template <typename T = double>
    inline T powIntTen(const int n) {
      return powInt(static_cast<T>(10), n);
    }
 
#define TTK_POW_LAMBDA(CALLEXPR, TYPE, EXPN, ...)                      \
  {                                                                    \
    const auto one = [](const TYPE ttkNotUsed(a)) { return TYPE{1}; }; \
    const auto id = [](const TYPE a) { return a; };                    \
    const auto square = [](const TYPE a) { return a * a; };            \
    const auto cube = [](const TYPE a) { return a * a * a; };          \
    const auto powInt                                                  \
      = [EXPN](const TYPE a) { return Geometry::powInt(a, EXPN); };    \
                                                                       \
    if(EXPN == 0) {                                                    \
      CALLEXPR(__VA_ARGS__, one);                                      \
    } else if(EXPN == 1) {                                             \
      CALLEXPR(__VA_ARGS__, id);                                       \
    } else if(EXPN == 2) {                                             \
      CALLEXPR(__VA_ARGS__, square);                                   \
    } else if(EXPN == 3) {                                             \
      CALLEXPR(__VA_ARGS__, cube);                                     \
    } else {                                                           \
      CALLEXPR(__VA_ARGS__, powInt);                                   \
    }                                                                  \
  }
 
    template <typename T1, typename T2>
    inline T1 pow(const T1 val, const T2 n) {
      static_assert(
        std::is_arithmetic<T1>::value && std::is_arithmetic<T2>::value,
        "pow can only be applied on arithmetic values");
 
      if(std::is_integral<T2>::value) {
        return powInt(val, n);
      } else if(std::is_floating_point<T2>::value) {
        return std::pow(val, n);
      }
      // this return statement should be unreachable thanks to the
      // static_assert
      return T1{};
    }
 
    template <typename T>
    void
      projectOnTrianglePlane(const T *p, const T *a, const T *normTri, T *out);
 
    template <typename T>
    void projectOnEdge(const T *p, const T *a, const T *b, T *out);
 
    template <typename T>
    void computeTriangleNormal(const T *a, const T *b, const T *c, T *out);
 
    template <typename T, typename triangulationType>
    SimplexId getNearestSurfaceVertex(const T *pa,
                                      std::vector<T> &dists,
                                      const triangulationType &triangulation) {
      for(SimplexId i = 0; i < triangulation.getNumberOfVertices(); ++i) {
        std::array<T, 3> pv{};
        triangulation.getVertexPoint(i, pv[0], pv[1], pv[2]);
        dists[i] = distance(pa, pv.data());
      }
      return std::min_element(dists.begin(), dists.end()) - dists.begin();
    }
 
    template <typename T>
    int
      subtractVectors(const T *a, const T *b, T *out, const int &dimension = 3);
 
    template <typename T>
    int subtractVectors(const std::vector<T> &a,
                        const std::vector<T> &b,
                        std::vector<T> &out);
 
    template <typename T>
    int addVectors(const T *a, const T *b, T *out, const int &dimension = 3);
 
    template <typename T>
    int addVectors(const std::vector<T> &a,
                   const std::vector<T> &b,
                   std::vector<T> &out);
 
    template <typename T>
    int multiAddVectors(const std::vector<std::vector<T>> &a,
                        const std::vector<std::vector<T>> &b,
                        std::vector<std::vector<T>> &out);
 
    template <typename T>
    int
      multiAddVectorsFlatten(const std::vector<std::vector<std::vector<T>>> &a,
                             const std::vector<std::vector<std::vector<T>>> &b,
                             std::vector<std::vector<T>> &out);
 
    template <typename T>
    int
      scaleVector(const T *a, const T factor, T *out, const int &dimension = 3);
 
    template <typename T>
    int
      scaleVector(const std::vector<T> &a, const T factor, std::vector<T> &out);
 
    template <typename T>
    int vectorProjection(const T *a,
                         const T *b,
                         T *out,
                         const int &dimension = 3);
 
    template <typename T>
    int vectorProjection(const std::vector<T> &a,
                         const std::vector<T> &b,
                         std::vector<T> &out);
 
    template <typename T>
    void addVectorsProjection(const std::vector<T> &a,
                              const std::vector<T> &b,
                              std::vector<T> &a_out,
                              std::vector<T> &b_out);
 
    template <typename T>
    void gramSchmidt(const std::vector<std::vector<T>> &a,
                     std::vector<std::vector<T>> &out);
 
    template <typename T>
    bool isVectorUniform(const std::vector<T> &a);
 
    template <typename T>
    bool isVectorNull(const std::vector<T> &a);
 
    template <typename T>
    bool isVectorNullFlatten(const std::vector<std::vector<T>> &a);
 
    template <typename T>
    int flattenMultiDimensionalVector(const std::vector<std::vector<T>> &a,
                                      std::vector<T> &out);
 
    template <typename T>
    int multiFlattenMultiDimensionalVector(
      const std::vector<std::vector<std::vector<T>>> &a,
      std::vector<std::vector<T>> &out);
 
    template <typename T>
    int unflattenMultiDimensionalVector(const std::vector<T> &a,
                                        std::vector<std::vector<T>> &out,
                                        const int &no_columns = 2);
 
    template <typename T>
    void matrixMultiplication(const std::vector<std::vector<T>> &a,
                              const std::vector<std::vector<T>> &b,
                              std::vector<std::vector<T>> &out);
 
    template <typename T>
    void subtractMatrices(const std::vector<std::vector<T>> &a,
                          const std::vector<std::vector<T>> &b,
                          std::vector<std::vector<T>> &out);
 
    template <typename T>
    void addMatrices(const std::vector<std::vector<T>> &a,
                     const std::vector<std::vector<T>> &b,
                     std::vector<std::vector<T>> &out);
 
    template <typename T>
    void scaleMatrix(const std::vector<std::vector<T>> &a,
                     const T factor,
                     std::vector<std::vector<T>> &out);
 
    template <typename T>
    void transposeMatrix(const std::vector<std::vector<T>> &a,
                         std::vector<std::vector<T>> &out);
 
    template <typename triangulationType>
    std::array<float, 3>
      computeSurfaceNormalAtPoint(const SimplexId a,
                                  const triangulationType &triangulation) {
 
      std::array<float, 3> res{};
 
      // store for each triangle in a's star a's two direct neighbors
      std::vector<std::array<SimplexId, 2>> edges{};
 
      const auto nStar{triangulation.getVertexStarNumber(a)};
 
      if(triangulation.getCellVertexNumber(0) == 3) {
        // triangle mesh
        for(SimplexId i = 0; i < nStar; ++i) {
          SimplexId c{};
          triangulation.getVertexStar(a, i, c);
          SimplexId v0{}, v1{};
          triangulation.getCellVertex(c, 0, v0);
          if(v0 == a) {
            triangulation.getCellVertex(c, 1, v0);
          } else {
            triangulation.getCellVertex(c, 1, v1);
            if(v1 == a) {
              triangulation.getCellVertex(c, 2, v1);
            }
          }
          edges.emplace_back(std::array<SimplexId, 2>{v0, v1});
        }
      } else if(triangulation.getCellVertexNumber(0) == 4) {
        // quad mesh
        for(SimplexId i = 0; i < nStar; ++i) {
          SimplexId c{};
          triangulation.getVertexStar(a, i, c);
          std::array<SimplexId, 4> q{};
          triangulation.getCellVertex(c, 0, q[0]);
          triangulation.getCellVertex(c, 1, q[1]);
          triangulation.getCellVertex(c, 2, q[2]);
          triangulation.getCellVertex(c, 3, q[3]);
          if(q[0] == a) {
            edges.emplace_back(std::array<SimplexId, 2>{
              static_cast<SimplexId>(q[3]),
              static_cast<SimplexId>(q[1]),
            });
          } else if(q[1] == a) {
            edges.emplace_back(std::array<SimplexId, 2>{
              static_cast<SimplexId>(q[0]),
              static_cast<SimplexId>(q[2]),
            });
          } else if(q[2] == a) {
            edges.emplace_back(std::array<SimplexId, 2>{
              static_cast<SimplexId>(q[1]),
              static_cast<SimplexId>(q[3]),
            });
          } else if(q[3] == a) {
            edges.emplace_back(std::array<SimplexId, 2>{
              static_cast<SimplexId>(q[2]),
              static_cast<SimplexId>(q[0]),
            });
          }
        }
      }
 
      // current vertex 3d coordinates
      std::array<float, 3> pa{};
      triangulation.getVertexPoint(a, pa[0], pa[1], pa[2]);
 
      // triangle normals around current surface vertex
      std::vector<std::array<float, 3>> normals{};
      normals.reserve(nStar);
 
      for(auto &e : edges) {
        std::array<float, 3> pb{}, pc{};
        triangulation.getVertexPoint(e[0], pb[0], pb[1], pb[2]);
        triangulation.getVertexPoint(e[1], pc[0], pc[1], pc[2]);
 
        std::array<float, 3> normal{};
        computeTriangleNormal(pa.data(), pb.data(), pc.data(), normal.data());
        // ensure normal not null
        if(normal[0] != -1.0F && normal[1] != -1.0F && normal[2] != -1.0F) {
          // unitary normal vector
          normals.emplace_back(normal);
        }
      }
 
      if(!normals.empty()) {
 
        // ensure normals have same direction
        for(size_t i = 1; i < normals.size(); ++i) {
          const auto dotProd = dotProduct(normals[0].data(), normals[i].data());
          if(dotProd < 0.0F) {
            scaleVector(normals[i].data(), -1.0F, normals[i].data());
          }
        }
 
        // compute mean of normals
        for(unsigned int i = 0; i < normals.size(); ++i)
          addVectors(res.data(), normals[i].data(), res.data());
        scaleVector(res.data(), static_cast<float>(normals.size()), res.data());
 
      } else {
 
        // set error value directly in output variable...
        res[0] = NAN;
      }
 
      return res;
    }
 
    struct ProjectionResult {
      std::array<float, 3> pt;
      SimplexId nearestVertex;
      size_t trianglesChecked;
      bool projSuccess;
      bool reverseProjection;
    };
 
    struct ProjectionInput {
      size_t id;
      std::array<float, 3> &pt;
      SimplexId nearestVertex;
    };
 
    template <typename triangulationType0, typename triangulationType1>
    ProjectionResult
      findProjection(const ProjectionInput &pi,
                     VisitedMask &trianglesTested,
                     std::vector<float> &dists,
                     std::stack<SimplexId> &trianglesToTest,
                     const bool reverseProjection,
                     const triangulationType0 &triangulationToSmooth,
                     const triangulationType1 &triangulation) {
 
      ProjectionResult res{pi.pt, pi.nearestVertex, 0, false, false};
 
      std::array<float, 3> surfaceNormal{};
      if(reverseProjection) {
        surfaceNormal
          = computeSurfaceNormalAtPoint(pi.id, triangulationToSmooth);
        res.reverseProjection = !std::isnan(surfaceNormal[0]);
      }
 
      // clean trianglesToTest
      while(!trianglesToTest.empty()) {
        trianglesToTest.pop();
      }
 
      // init pipeline by checking in the first triangle around selected vertex
      if(triangulation.getVertexTriangleNumber(res.nearestVertex) > 0) {
        SimplexId next{};
        triangulation.getVertexTriangle(res.nearestVertex, 0, next);
        trianglesToTest.push(next);
      }
 
      while(!trianglesToTest.empty()) {
 
        const auto curr = trianglesToTest.top();
        trianglesToTest.pop();
 
        // skip if already tested
        if(trianglesTested.isVisited_[curr]) {
          continue;
        }
 
        // get triangle vertices
        std::array<SimplexId, 3> tverts{};
        triangulation.getTriangleVertex(curr, 0, tverts[0]);
        triangulation.getTriangleVertex(curr, 1, tverts[1]);
        triangulation.getTriangleVertex(curr, 2, tverts[2]);
 
        // get coordinates of triangle vertices (lets name is MNO)
        std::array<std::array<float, 3>, 3>
          mno{}; // [0] is M, [1] is N and [2] is O
        triangulation.getVertexPoint(
          tverts[0], mno[0][0], mno[0][1], mno[0][2]);
        triangulation.getVertexPoint(
          tverts[1], mno[1][0], mno[1][1], mno[1][2]);
        triangulation.getVertexPoint(
          tverts[2], mno[2][0], mno[2][1], mno[2][2]);
 
        std::array<float, 3> normTri{};
        computeTriangleNormal(
          mno[0].data(), mno[1].data(), mno[2].data(), normTri.data());
 
        static const float PREC_FLT{powf(10, -FLT_DIG)};
 
        if(res.reverseProjection) {
 
          const auto denom = dotProduct(surfaceNormal.data(), normTri.data());
 
          // check if triangle plane is parallel to surface to project normal
          if(std::abs(denom) < PREC_FLT) {
            // fill pipeline with neighboring triangles
            for(auto &vert : tverts) {
              auto ntr = triangulation.getVertexTriangleNumber(vert);
              for(SimplexId j = 0; j < ntr; ++j) {
                SimplexId tid;
                triangulation.getVertexTriangle(vert, j, tid);
                if(tid != curr) {
                  trianglesToTest.push(tid);
                }
              }
            }
            continue;
          }
 
          // compute intersection between _surface to project normal at
          // current vertex line_ and _reference surface triangle plane_
          std::array<float, 3> tmp{};
          subtractVectors(pi.pt.data(), mno[0].data(), tmp.data());
          auto alpha = dotProduct(tmp.data(), normTri.data()) / denom;
          std::array<float, 3> surfaceNormalScaled{};
          scaleVector(surfaceNormal.data(), alpha, surfaceNormalScaled.data());
          addVectors(pi.pt.data(), surfaceNormalScaled.data(), res.pt.data());
 
        } else {
          projectOnTrianglePlane(
            pi.pt.data(), mno[0].data(), normTri.data(), res.pt.data());
        }
 
        // compute barycentric coords of projection
        std::array<float, 3> baryCoords{};
        computeBarycentricCoordinates(mno[0].data(), mno[1].data(),
                                      mno[2].data(), res.pt.data(), baryCoords);
 
        // check if projection in triangle
        bool inTriangle = true;
 
        for(auto &coord : baryCoords) {
          if(coord < -PREC_FLT) {
            inTriangle = false;
          }
          if(coord > 1 + PREC_FLT) {
            inTriangle = false;
          }
        }
 
        // mark triangle as tested
        trianglesTested.insert(curr);
        res.trianglesChecked++;
 
        if(inTriangle) {
          res.projSuccess = true;
          // should we check if we have the nearest triangle?
          break;
        }
 
        // extrema values in baryCoords
        const auto extrema
          = std::minmax_element(baryCoords.begin(), baryCoords.end());
 
        // find the nearest triangle vertices local ids (with the
        // highest/positive values in baryCoords) from proj
        std::array<SimplexId, 2> vid{
          static_cast<SimplexId>(extrema.second - baryCoords.begin()), 0};
        for(size_t j = 0; j < baryCoords.size(); j++) {
          if(j != static_cast<size_t>(extrema.first - baryCoords.begin())
             && j != static_cast<size_t>(extrema.second - baryCoords.begin())) {
            vid[1] = j;
            break;
          }
        }
 
        // store vertex with highest barycentric coordinate
        res.nearestVertex = tverts[vid[0]];
        const auto secondNearVert{tverts[vid[1]]};
 
        // get the triangle edge with the two vertices
        SimplexId edge{};
        const auto nEdges{triangulation.getVertexEdgeNumber(res.nearestVertex)};
        for(SimplexId i = 0; i < nEdges; ++i) {
          triangulation.getVertexEdge(res.nearestVertex, i, edge);
          SimplexId v{};
          triangulation.getEdgeVertex(edge, 0, v);
          if(v == res.nearestVertex) {
            triangulation.getEdgeVertex(edge, 1, v);
          }
          if(v == secondNearVert) {
            break;
          }
        }
 
        // next triangle to visit
        SimplexId next{};
        const auto nTri{triangulation.getEdgeTriangleNumber(edge)};
        for(SimplexId i = 0; i < nTri; ++i) {
          triangulation.getEdgeTriangle(edge, i, next);
          if(next != curr) {
            break;
          }
        }
 
        const auto nVisited{trianglesTested.visitedIds_.size()};
        if(nVisited > 1 && next == trianglesTested.visitedIds_[nVisited - 2]) {
          // the relaxed point is probably over the edge separating the
          // current and the last visited triangles
          projectOnEdge(pi.pt.data(), mno[vid[0]].data(), mno[vid[1]].data(),
                        res.pt.data());
          // check that projected point is indeed on segment
          res.projSuccess = isPointOnSegment(
            res.pt.data(), mno[vid[0]].data(), mno[vid[1]].data());
          if(!res.projSuccess) {
            // if not, replace with nearest triangle vertex
            triangulation.getVertexPoint(
              tverts[vid[0]], res.pt[0], res.pt[1], res.pt[2]);
            res.projSuccess = true;
          }
          break;
        }
 
        if(!trianglesTested.isVisited_[next]) {
          trianglesToTest.push(next);
        }
      }
 
      const size_t maxTrChecked = 100;
 
      if(res.projSuccess && res.trianglesChecked > maxTrChecked) {
        res.projSuccess = false;
      }
 
      std::array<float, 3> nearestCoords{};
      triangulation.getVertexPoint(
        pi.nearestVertex, nearestCoords[0], nearestCoords[1], nearestCoords[2]);
      if(Geometry::distance(res.pt.data(), pi.pt.data())
         > 5.0 * Geometry::distance(res.pt.data(), nearestCoords.data())) {
        res.projSuccess = false;
        if(triangulationToSmooth.getDimensionality() == 2
           && !reverseProjection) {
          // clean trianglesTested
          for(const auto t : trianglesTested.visitedIds_) {
            trianglesTested.isVisited_[t] = false;
          }
          trianglesTested.visitedIds_.clear();
          return findProjection(pi, trianglesTested, dists, trianglesToTest,
                                true, triangulationToSmooth, triangulation);
        }
      }
 
      if(!res.projSuccess) {
        // replace proj by the nearest vertex?
        res.nearestVertex = Geometry::getNearestSurfaceVertex(
          pi.pt.data(), dists, triangulation);
        triangulation.getVertexPoint(
          res.nearestVertex, res.pt[0], res.pt[1], res.pt[2]);
      }
 
      return res;
    }
 
  } // namespace Geometry
} // namespace ttk