miscutils/mesh3d.cpp at main · dvoromar/miscutils · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
// Copyright (c) 2018 Marek Dvoroznak
// Licensed under the MIT License.

#include "mesh3d.h"
#include <miscutils/macros.h>
#include <igl/readOBJ.h>

using namespace std;
using namespace Eigen;

Mesh3D::Mesh3D()
{
}

Mesh3D::~Mesh3D()
{
}

void Mesh3D::createFromMesh(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F)
{
  VRest = VCurr = V;
  this->F = F;
}

void Mesh3D::loadMesh(const std::string &fn)
{
  using namespace igl;
  using namespace Eigen;
  MatrixXd V;
  MatrixXi F;
  readOBJ(fn, V, F);
  createFromMesh(V, F);
}

// Works for triangular faces only.
int Mesh3D::getMeshFace(const MatrixXd &V, const MatrixXi &F, double x, double y, bool highestZ, bool reverseDirection, double &w1, double &w2, double &w3)
{
  auto edgeFunction = [](const Vector3d &a, const Vector3d &b, const Vector3d &c)->double
  { return (c(0) - a(0)) * (b(1) - a(1)) - (c(1) - a(1)) * (b(0) - a(0)); };

  Vector3d pos(x,y,0);
  double extZ = highestZ ? -numeric_limits<double>::infinity() : numeric_limits<double>::infinity();
  int ind = -1;
  vector<double*> ws{&w1, &w2, &w3};
  fora(i, 0, F.rows()) {
    const VectorXi &f = F.row(i);
    bool inside = true;
    double area = 0;
    double z = 0;
    fora(j, 0, f.size()) {
      int ptIdFrom = f(j);
      int ptIdTo = f((j+1)%f.size());
      int ptIdOpposite = f((j+2)%f.size()); // only for triangular faces!
      const Vector3d &pFrom = V.row(ptIdFrom);
      const Vector3d &pTo = V.row(ptIdTo);
      const Vector3d &pOpposite = V.row(ptIdOpposite);
      double &w = *ws[j];
      w = edgeFunction(pFrom, pTo, pos);
      if (reverseDirection) w *= -1;
      if (w < 0) {
        inside = false;
        break;
      }
      z += w * pOpposite(2);
      area += w;
    }

    if (inside) {
      z /= area;
      if (highestZ ? z>extZ : z<extZ) {
        extZ = z;
        ind = i;
      }
    }
  }
  return ind;
}

// beware: probably works for triangular faces only
int Mesh3D::getMeshFace(const MatrixXd &V, const MatrixXi &F, double x, double y, bool highestZ, bool reverseDirection)
{
  double w1, w2, w3;
  return getMeshFace(V, F, x, y, highestZ, reverseDirection, w1, w2, w3);
}

// Find a point on the mesh of which planar projection is nearest to the specified point [x,y] while also being
// in the specified radius and with highest or lowest z-coordinate (i.e. nearest/farthest from the plane)
int Mesh3D::getMeshPoint(const MatrixXd &V, double x, double y, double planarRadius, bool highest)
{
  using namespace std;
  using namespace Eigen;

  int ind = -1;
  double min = numeric_limits<double>::infinity();
  double extZ = highest ? -numeric_limits<double>::infinity() : numeric_limits<double>::infinity();
  const Vector2d pos(x,y);
  fora(i, 0, V.rows()) {
    const Vector3d &pt = V.row(i);
    const Vector2d p(pt(0),pt(1));
    double d = (pos-p).norm();
    double z = pt(2);
    if (d <= planarRadius && d < min && (highest ? z>extZ : z<extZ)) {
      min = d;
      extZ = z;
      ind = i;
    }
  }
  return ind;
}

int Mesh3D::getMeshPoint(const MatrixXd &V, double x, double y, double radius, double depth, bool considerDepth)
{
  using namespace std;
  using namespace Eigen;

  int ind = -1;
  double min = numeric_limits<double>::infinity();
  Vector3d pos(x,y,depth);
  fora(i, 0, V.rows()) {
    const Vector3d &pt = V.row(i);
    Vector3d p(pt(0),pt(1),pt(2));
    double d;
    if (considerDepth) d = (pos-p).norm();
    else d = (pos.head(2)-p.head(2)).norm();
    if (d <= radius && d < min) {
      min = d;
      ind = i;
    }
  }
  return ind;
}

int Mesh3D::getNumPoints() const
{
  return VCurr.rows();
}

int Mesh3D::getNumFaces() const
{
  return F.rows();
}

void Mesh3D::reset(const Eigen::MatrixXd &V)
{
  VRest = VCurr = V;
}