Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00014 #include "Triangulator.h"
00015
00016 using namespace std;
00017 using namespace hrp;
00018
00019
00020 int Triangulator::apply(const vector<int>& polygon)
00021 {
00022 triangles_.clear();
00023
00024 unsigned int numOrgVertices = polygon.size();
00025
00026 if(numOrgVertices > earMask.size()){
00027 earMask.resize(numOrgVertices);
00028 }
00029
00030 if(numOrgVertices < 3){
00031 return 0;
00032 } else if(numOrgVertices == 3){
00033 triangles_.push_back(0);
00034 triangles_.push_back(1);
00035 triangles_.push_back(2);
00036 return 1;
00037 }
00038
00039 orgPolygon = &polygon;
00040
00041 workPolygon.resize(numOrgVertices);
00042 for(unsigned int i=0; i < numOrgVertices; ++i){
00043 workPolygon[i] = i;
00044 }
00045
00046 ccs << 0.0, 0.0, 0.0;
00047 const Vector3Ref o(vertex(0));
00048 for(unsigned int i=1; i < numOrgVertices - 1; ++i){
00049 ccs += (vertex(i) - o).cross(vertex((i+1) % numOrgVertices) - o);
00050 }
00051
00052 int numTriangles = 0;
00053
00054 while(true) {
00055 int n = workPolygon.size();
00056 if(n < 3){
00057 break;
00058 }
00059 int target = -1;
00060 for(int i=0; i < n; ++i){
00061 Convexity convexity = calcConvexity(i);
00062 if(convexity == FLAT){
00063 target = i;
00064 break;
00065 } else if(convexity == CONVEX){
00066 if(!checkIfEarContainsOtherVertices(i)){
00067 triangles_.push_back(workPolygon[(i + n - 1) % n]);
00068 triangles_.push_back(workPolygon[i]);
00069 triangles_.push_back(workPolygon[(i + 1) % n]);
00070 target = i;
00071 numTriangles++;
00072 break;
00073 }
00074 }
00075 }
00076 if(target < 0){
00077 break;
00078 }
00079 for(int i = target + 1; i < n; ++i){
00080 workPolygon[target++] = workPolygon[i];
00081 }
00082 workPolygon.pop_back();
00083 }
00084
00085 return numTriangles;
00086 }
00087
00088
00089 Triangulator::Convexity Triangulator::calcConvexity(int ear)
00090 {
00091 int n = workPolygon.size();
00092
00093 const Vector3Ref p0(workVertex((ear + n - 1) % n));
00094 Vector3 a(workVertex(ear) - p0);
00095 Vector3 b(workVertex((ear + 1) % n) - p0);
00096 Vector3 ccs(a.cross(b));
00097
00098 Convexity convexity;
00099
00100 if((ccs.norm() / a.norm() + b.norm()) < 1.0e-4){
00101 convexity = FLAT;
00102 } else {
00103 convexity = (this->ccs.dot(ccs) > 0.0) ? CONVEX : CONCAVE;
00104 }
00105
00106 return convexity;
00107 }
00108
00109
00110 bool Triangulator::checkIfEarContainsOtherVertices(int ear)
00111 {
00112 bool contains = false;
00113
00114 const int n = workPolygon.size();
00115
00116 if(n > 3){
00117 const int prev = (ear + n -1) % n;
00118 const int next = (ear+1) % n;
00119 const Vector3Ref a(workVertex(prev));
00120 const Vector3Ref b(workVertex(ear));
00121 const Vector3Ref c(workVertex(next));
00122
00123 earMask[prev] = earMask[ear] = earMask[next] = 1;
00124
00125 for(unsigned int i=0; i < workPolygon.size(); ++i){
00126 if(!earMask[i]){
00127 const Vector3Ref p(workVertex(i));
00128 if((a - p).cross(b - p).dot(ccs) <= 0){
00129 continue;
00130 }
00131 if((b - p).cross(c - p).dot(ccs) <= 0){
00132 continue;
00133 }
00134 if((c - p).cross(a - p).dot(ccs) <= 0){
00135 continue;
00136 }
00137 contains = true;
00138 break;
00139 }
00140 }
00141
00142 earMask[prev] = earMask[ear] = earMask[next] = 0;
00143 }
00144
00145 return contains;
00146 }