Triangulator.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19