Triangulator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
14 #include "Triangulator.h"
15 
16 using namespace std;
17 using namespace hrp;
18 
19 
20 int Triangulator::apply(const vector<int>& polygon)
21 {
22  triangles_.clear();
23 
24  unsigned int numOrgVertices = polygon.size();
25 
26  if(numOrgVertices > earMask.size()){
27  earMask.resize(numOrgVertices);
28  }
29 
30  if(numOrgVertices < 3){
31  return 0;
32  } else if(numOrgVertices == 3){
33  triangles_.push_back(0);
34  triangles_.push_back(1);
35  triangles_.push_back(2);
36  return 1;
37  }
38 
39  orgPolygon = &polygon;
40 
41  workPolygon.resize(numOrgVertices);
42  for(unsigned int i=0; i < numOrgVertices; ++i){
43  workPolygon[i] = i;
44  }
45 
46  ccs << 0.0, 0.0, 0.0;
47  const Vector3Ref o(vertex(0));
48  for(unsigned int i=1; i < numOrgVertices - 1; ++i){
49  ccs += (vertex(i) - o).cross(vertex((i+1) % numOrgVertices) - o);
50  }
51 
52  int numTriangles = 0;
53 
54  while(true) {
55  int n = workPolygon.size();
56  if(n < 3){
57  break;
58  }
59  int target = -1;
60  for(int i=0; i < n; ++i){
61  Convexity convexity = calcConvexity(i);
62  if(convexity == FLAT){
63  target = i;
64  break;
65  } else if(convexity == CONVEX){
66  if(!checkIfEarContainsOtherVertices(i)){
67  triangles_.push_back(workPolygon[(i + n - 1) % n]);
68  triangles_.push_back(workPolygon[i]);
69  triangles_.push_back(workPolygon[(i + 1) % n]);
70  target = i;
71  numTriangles++;
72  break;
73  }
74  }
75  }
76  if(target < 0){
77  break;
78  }
79  for(int i = target + 1; i < n; ++i){
80  workPolygon[target++] = workPolygon[i];
81  }
82  workPolygon.pop_back();
83  }
84 
85  return numTriangles;
86 }
87 
88 
89 Triangulator::Convexity Triangulator::calcConvexity(int ear)
90 {
91  int n = workPolygon.size();
92 
93  const Vector3Ref p0(workVertex((ear + n - 1) % n));
94  Vector3 a(workVertex(ear) - p0);
95  Vector3 b(workVertex((ear + 1) % n) - p0);
96  Vector3 ccs(a.cross(b));
97 
98  Convexity convexity;
99 
100  if((ccs.norm() / a.norm() + b.norm()) < 1.0e-4){
101  convexity = FLAT;
102  } else {
103  convexity = (this->ccs.dot(ccs) > 0.0) ? CONVEX : CONCAVE;
104  }
105 
106  return convexity;
107 }
108 
109 
110 bool Triangulator::checkIfEarContainsOtherVertices(int ear)
111 {
112  bool contains = false;
113 
114  const int n = workPolygon.size();
115 
116  if(n > 3){
117  const int prev = (ear + n -1) % n;
118  const int next = (ear+1) % n;
119  const Vector3Ref a(workVertex(prev));
120  const Vector3Ref b(workVertex(ear));
121  const Vector3Ref c(workVertex(next));
122 
123  earMask[prev] = earMask[ear] = earMask[next] = 1;
124 
125  for(unsigned int i=0; i < workPolygon.size(); ++i){
126  if(!earMask[i]){
127  const Vector3Ref p(workVertex(i));
128  if((a - p).cross(b - p).dot(ccs) <= 0){
129  continue;
130  }
131  if((b - p).cross(c - p).dot(ccs) <= 0){
132  continue;
133  }
134  if((c - p).cross(a - p).dot(ccs) <= 0){
135  continue;
136  }
137  contains = true;
138  break;
139  }
140  }
141 
142  earMask[prev] = earMask[ear] = earMask[next] = 0;
143  }
144 
145  return contains;
146 }
int c
Definition: autoplay.py:16
png_uint_32 i
Definition: png.h:2735
long b
Definition: jpegint.h:371
Eigen::Vector3d Vector3Ref
Definition: Eigen3d.h:19
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
string a
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:15
double dot(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:19


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05