29 #include <pcl/console/print.h>
83 int i,j,*remapTable,*pointCount,idx[3];
86 double Ratio=12.0/sqrt(3.0);
88 remapTable=
new int[positions.size()];
89 pointCount=
new int[positions.size()];
90 for(i=0;i<int(positions.size());i++){
94 for(i=
int(triangles.size()-1);i>=0;i--){
96 idx[j]=triangles[i].idx[j];
97 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
99 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
100 triangles[i]=triangles[triangles.size()-1];
101 triangles.pop_back();
105 p[j].
coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]];
106 p[j].
coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]];
107 p[j].
coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]];
117 if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
124 if(idx[j]<idx[(j+1)%3]){
132 positions[idx1].coords[0]+=positions[idx2].coords[0];
133 positions[idx1].coords[1]+=positions[idx2].coords[1];
134 positions[idx1].coords[2]+=positions[idx2].coords[2];
136 (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0];
137 (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1];
138 (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2];
140 pointCount[idx1]+=pointCount[idx2];
141 remapTable[idx2]=idx1;
142 triangles[i]=triangles[triangles.size()-1];
143 triangles.pop_back();
147 for(i=0;i<int(positions.size());i++){
148 for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
151 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
153 if(remapTable[i]==i){
154 positions[pCount]=positions[i];
155 if(normals){(*normals)[pCount]=(*normals)[i];}
156 pointCount[i]=pCount;
160 positions.resize(pCount);
161 for(i=
int(triangles.size()-1);i>=0;i--){
163 idx[j]=triangles[i].idx[j];
164 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
165 triangles[i].idx[j]=pointCount[idx[j]];
167 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
168 triangles[i]=triangles[triangles.size()-1];
169 triangles.pop_back();
178 int i,j,*remapTable,*pointCount,idx[3];
181 double Ratio=12.0/sqrt(3.0);
183 remapTable=
new int[positions.size()];
184 pointCount=
new int[positions.size()];
185 for(i=0;i<int(positions.size());i++){
189 for(i=
int(triangles.size()-1);i>=0;i--){
191 idx[j]=triangles[i].idx[j];
192 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
194 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
195 triangles[i]=triangles[triangles.size()-1];
196 triangles.pop_back();
200 p[j].
coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]];
201 p[j].
coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]];
202 p[j].
coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]];
212 if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
243 positions[idx1].coords[0]+=positions[idx2].coords[0]+positions[idx3].coords[0];
244 positions[idx1].coords[1]+=positions[idx2].coords[1]+positions[idx3].coords[1];
245 positions[idx1].coords[2]+=positions[idx2].coords[2]+positions[idx3].coords[2];
247 (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0]+(*normals)[idx3].coords[0];
248 (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1]+(*normals)[idx3].coords[1];
249 (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2]+(*normals)[idx3].coords[2];
251 pointCount[idx1]+=pointCount[idx2]+pointCount[idx3];
252 remapTable[idx2]=idx1;
253 remapTable[idx3]=idx1;
254 triangles[i]=triangles[triangles.size()-1];
255 triangles.pop_back();
259 for(i=0;i<int(positions.size());i++){
260 for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
263 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
265 if(remapTable[i]==i){
266 positions[pCount]=positions[i];
267 if(normals){(*normals)[pCount]=(*normals)[i];}
268 pointCount[i]=pCount;
272 positions.resize(pCount);
273 for(i=
int(triangles.size()-1);i>=0;i--){
275 idx[j]=triangles[i].idx[j];
276 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
277 triangles[i].idx[j]=pointCount[idx[j]];
279 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
280 triangles[i]=triangles[triangles.size()-1];
281 triangles.pop_back();
294 if(p1>p2) {
return ((
long long)(p1)<<32) | ((
long long)(p2));}
295 else {
return ((
long long)(p2)<<32) | ((
long long)(p1));}
300 if(triangles[tIndex].eIndex[0]<0 || triangles[tIndex].eIndex[1]<0 || triangles[tIndex].eIndex[2]<0){
return 0;}
301 if(edges[triangles[tIndex].eIndex[0]].tIndex[0]==tIndex){p1=edges[triangles[tIndex].eIndex[0]].pIndex[0];}
302 else {p1=edges[triangles[tIndex].eIndex[0]].pIndex[1];}
303 if(edges[triangles[tIndex].eIndex[1]].tIndex[0]==tIndex){p2=edges[triangles[tIndex].eIndex[1]].pIndex[0];}
304 else {p2=edges[triangles[tIndex].eIndex[1]].pIndex[1];}
305 if(edges[triangles[tIndex].eIndex[2]].tIndex[0]==tIndex){p3=edges[triangles[tIndex].eIndex[2]].pIndex[0];}
306 else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];}
312 for(
int i=0;i<3;i++){
313 q1.
coords[i]=points[p2].coords[i]-points[p1].coords[i];
314 q2.
coords[i]=points[p3].coords[i]-points[p1].coords[i];
322 factor(tIndex,p1,p2,p3);
323 return area(p1,p2,p3);
328 for(
int i=0;i<int(triangles.size());i++){a+=area(i);}
333 hash_map<long long,int>::iterator iter;
339 tIdx=int(triangles.size())-1;
343 long long e =
EdgeIndex(p[i],p[(i+1)%3]);
344 iter=edgeMap.find(e);
345 if(iter==edgeMap.end())
349 edge.
pIndex[1]=p[(i+1)%3];
350 edges.push_back(edge);
351 eIdx=int(edges.size())-1;
353 edges[eIdx].tIndex[0]=tIdx;
357 if(edges[eIdx].pIndex[0]==p[i]){
358 if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;}
359 else{PCL_DEBUG(
"Edge Triangle in use 1\n");
return 0;}
362 if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;}
363 else{PCL_DEBUG(
"Edge Triangle in use 2\n");
return 0;}
367 triangles[tIdx].eIndex[i]=eIdx;
373 double oldArea,newArea;
374 int oldP[3],oldQ[3],newP[3],newQ[3];
377 if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){
return 0;}
379 if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){
return 0;}
380 if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){
return 0;}
382 oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]);
384 for(idxP=0;idxP<3;idxP++){
386 for(i=0;i<3;i++){
if(oldP[idxP]==oldQ[i]){
break;}}
389 for(idxQ=0;idxQ<3;idxQ++){
391 for(i=0;i<3;i++){
if(oldP[i]==oldQ[idxQ]){
break;}}
394 if(idxP==3 || idxQ==3){
return 0;}
396 newP[1]=oldP[(idxP+1)%3];
399 newQ[1]=oldP[(idxP+2)%3];
402 newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]);
403 if(oldArea<=newArea){
return 0;}
406 edgeMap.erase(
EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
408 edges[eIndex].pIndex[0]=newP[0];
409 edges[eIndex].pIndex[1]=newQ[0];
411 edgeMap[
EdgeIndex(newP[0],newQ[0])]=eIndex;
413 for(
int i=0;i<3;i++){
415 idx=edgeMap[
EdgeIndex(newQ[i],newQ[(i+1)%3])];
416 triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx;
418 if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];}
419 if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];}
422 idx=edgeMap[
EdgeIndex(newP[i],newP[(i+1)%3])];
423 triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx;
425 if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];}
426 if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];}
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
int addTriangle(int p1, int p2, int p3)
Point3D< Real > RandomSpherePoint(void)
double Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
static long long EdgeIndex(int p1, int p2)
double Length(const Point3D< Real > &p)
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
void TriangleCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Point3D< Real > RandomBallPoint(void)
int factor(int tIndex, int &p1, int &p2, int &p3)
int flipMinimize(int eIndex)
void EdgeCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
double SquareLength(const Point3D< Real > &p)