00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 #include <cstdio>
00043
00044
00045 namespace pcl {
00046 namespace poisson {
00047
00049 template<class Real>
00050 Real Random ()
00051 {
00052 return Real(rand())/RAND_MAX;
00053 }
00054
00055
00057 template<class Real>
00058 Point3D<Real> RandomBallPoint ()
00059 {
00060 Point3D<Real> p;
00061 while(1)
00062 {
00063 p.coords[0] = Real(1.0-2.0*Random<Real> ());
00064 p.coords[1] = Real(1.0-2.0*Random<Real> ());
00065 p.coords[2] = Real(1.0-2.0*Random<Real> ());
00066 double l = SquareLength (p);
00067 if (l <= 1)
00068 return p;
00069 }
00070 }
00071
00072
00074 template<class Real>
00075 Point3D<Real> RandomSpherePoint ()
00076 {
00077 Point3D<Real> p = RandomBallPoint<Real> ();
00078 Real l = Real (Length(p));
00079 p.coords[0] /= l;
00080 p.coords[1] /= l;
00081 p.coords[2] /= l;
00082 return p;
00083 }
00084
00085
00087 template<class Real>
00088 double SquareLength (const Point3D<Real>& p)
00089 {
00090 return p.coords[0]*p.coords[0]+p.coords[1]*p.coords[1]+p.coords[2]*p.coords[2];
00091 }
00092
00093
00095 template<class Real>
00096 double Length (const Point3D<Real>& p)
00097 {
00098 return sqrt(SquareLength(p));
00099 }
00100
00101
00103 template<class Real>
00104 double SquareDistance (const Point3D<Real>& p1, const Point3D<Real>& p2)
00105 {
00106 return (p1.coords[0]-p2.coords[0])*(p1.coords[0]-p2.coords[0]) +
00107 (p1.coords[1]-p2.coords[1])*(p1.coords[1]-p2.coords[1]) +
00108 (p1.coords[2]-p2.coords[2])*(p1.coords[2]-p2.coords[2]);
00109 }
00110
00111
00113 template<class Real>
00114 double Distance (const Point3D<Real>& p1, const Point3D<Real>& p2)
00115 {
00116 return sqrt (SquareDistance (p1,p2));
00117 }
00118
00119
00121 template <class Real>
00122 void CrossProduct (const Point3D<Real>& p1, const Point3D<Real>& p2, Point3D<Real>& p)
00123 {
00124 p.coords[0] = p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1];
00125 p.coords[1] = -p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0];
00126 p.coords[2] = p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0];
00127 }
00128
00129
00131 template<class Real>
00132 void EdgeCollapse (const Real& edgeRatio,
00133 std::vector<TriangleIndex>& triangles,
00134 std::vector< Point3D<Real> >& positions,
00135 std::vector< Point3D<Real> >* normals)
00136 {
00137 int i, j, *remapTable, *pointCount, idx[3];
00138 Point3D<Real> p[3],q[2],c;
00139 double d[3],a;
00140 double Ratio = 12.0/sqrt(3.0);
00141
00142 remapTable = new int[positions.size ()];
00143 pointCount = new int[positions.size ()];
00144
00145 for (i = 0; i < int (positions.size ()); i++)
00146 {
00147 remapTable[i] = i;
00148 pointCount[i] = 1;
00149 }
00150
00151 for (i = int (triangles.size ()-1); i >= 0; i--)
00152 {
00153 for (j = 0; j < 3; j++)
00154 {
00155 idx[j] = triangles[i].idx[j];
00156 while (remapTable[idx[j]] < idx[j])
00157 idx[j] = remapTable[idx[j]];
00158 }
00159
00160 if (idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2])
00161 {
00162 triangles[i] = triangles[triangles.size()-1];
00163 triangles.pop_back();
00164 continue;
00165 }
00166
00167 for (j = 0; j < 3; j++)
00168 {
00169 p[j].coords[0] = positions[idx[j]].coords[0]/pointCount[idx[j]];
00170 p[j].coords[1] = positions[idx[j]].coords[1]/pointCount[idx[j]];
00171 p[j].coords[2] = positions[idx[j]].coords[2]/pointCount[idx[j]];
00172 }
00173 for(j = 0; j < 3; j++)
00174 {
00175 q[0].coords[j] = p[1].coords[j]-p[0].coords[j];
00176 q[1].coords[j] = p[2].coords[j]-p[0].coords[j];
00177 d[j] = SquareDistance(p[j],p[(j+1)%3]);
00178 }
00179 CrossProduct(q[0],q[1],c);
00180 a = Length(c)/2;
00181
00182 if ((d[0]+d[1]+d[2])*edgeRatio > a*Ratio)
00183 {
00184
00185 j = 0;
00186 if (d[1]<d[j])
00187 j = 1;
00188 if (d[2]<d[j])
00189 j = 2;
00190
00191 int idx1,idx2;
00192 if (idx[j] < idx[(j+1)%3])
00193 {
00194 idx1 = idx[j];
00195 idx2 = idx[(j+1)%3];
00196 }
00197 else
00198 {
00199 idx2 = idx[j];
00200 idx1 = idx[(j+1)%3];
00201 }
00202 positions[idx1].coords[0] += positions[idx2].coords[0];
00203 positions[idx1].coords[1] += positions[idx2].coords[1];
00204 positions[idx1].coords[2] += positions[idx2].coords[2];
00205 if (normals)
00206 {
00207 (*normals)[idx1].coords[0] += (*normals)[idx2].coords[0];
00208 (*normals)[idx1].coords[1] += (*normals)[idx2].coords[1];
00209 (*normals)[idx1].coords[2] += (*normals)[idx2].coords[2];
00210 }
00211 pointCount[idx1] += pointCount[idx2];
00212 remapTable[idx2] = idx1;
00213 triangles[i] = triangles[triangles.size()-1];
00214 triangles.pop_back();
00215 }
00216 }
00217 int pCount = 0;
00218
00219 for (i = 0; i < int (positions.size()); i++)
00220 {
00221 for (j = 0; j < 3; j++)
00222 positions[i].coords[j] /= pointCount[i];
00223 if (normals)
00224 {
00225 Real l = Real (Length ((*normals)[i]));
00226 for (j = 0; j < 3; j++)
00227 (*normals)[i].coords[j] /= l;
00228 }
00229 if (remapTable[i] == i)
00230 {
00231 positions[pCount] = positions[i];
00232 if(normals){(*normals)[pCount] = (*normals)[i];}
00233 pointCount[i] = pCount;
00234 pCount++;
00235 }
00236 }
00237 positions.resize(pCount);
00238 for(i = int (triangles.size ()-1); i >= 0; i--)
00239 {
00240 for(j = 0; j < 3; j++)
00241 {
00242 idx[j] = triangles[i].idx[j];
00243 while (remapTable[idx[j]]<idx[j])
00244 idx[j] = remapTable[idx[j]];
00245 triangles[i].idx[j] = pointCount[idx[j]];
00246 }
00247
00248 if(idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2])
00249 {
00250 triangles[i] = triangles[triangles.size()-1];
00251 triangles.pop_back();
00252 }
00253 }
00254
00255 delete[] pointCount;
00256 delete[] remapTable;
00257 }
00258
00260 template<class Real>
00261 void TriangleCollapse (const Real& edgeRatio,
00262 std::vector<TriangleIndex>& triangles,
00263 std::vector< Point3D<Real> >& positions,
00264 std::vector< Point3D<Real> >* normals)
00265 {
00266 int i,j,*remapTable,*pointCount,idx[3];
00267 Point3D<Real> p[3],q[2],c;
00268 double d[3],a;
00269 double Ratio = 12.0/sqrt(3.0);
00270
00271 remapTable = new int[positions.size()];
00272 pointCount = new int[positions.size()];
00273 for (i = 0; i < int (positions.size ()); i++)
00274 {
00275 remapTable[i] = i;
00276 pointCount[i] = 1;
00277 }
00278
00279 for (i = int (triangles.size ()-1); i >= 0; i--)
00280 {
00281 for (j = 0; j < 3; j++)
00282 {
00283 idx[j] = triangles[i].idx[j];
00284 while(remapTable[idx[j]]<idx[j]){idx[j] = remapTable[idx[j]];}
00285 }
00286 if(idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2])
00287 {
00288 triangles[i] = triangles[triangles.size()-1];
00289 triangles.pop_back();
00290 continue;
00291 }
00292
00293 for (j = 0; j < 3; j++)
00294 {
00295 p[j].coords[0] = positions[idx[j]].coords[0]/pointCount[idx[j]];
00296 p[j].coords[1] = positions[idx[j]].coords[1]/pointCount[idx[j]];
00297 p[j].coords[2] = positions[idx[j]].coords[2]/pointCount[idx[j]];
00298 }
00299 for (j = 0; j < 3; j++)
00300 {
00301 q[0].coords[j] = p[1].coords[j]-p[0].coords[j];
00302 q[1].coords[j] = p[2].coords[j]-p[0].coords[j];
00303 d[j] = SquareDistance(p[j],p[(j+1)%3]);
00304 }
00305 CrossProduct(q[0],q[1],c);
00306 a = Length(c)/2;
00307
00308 if ((d[0]+d[1]+d[2])*edgeRatio > a*Ratio)
00309 {
00310
00311 j = 0;
00312 if (d[1]<d[j])
00313 j = 1;
00314 if (d[2]<d[j])
00315 j = 2;
00316
00317 int idx1,idx2,idx3;
00318 if (idx[0]<idx[1])
00319 {
00320 if (idx[0]<idx[2])
00321 {
00322 idx1 = idx[0];
00323 idx2 = idx[2];
00324 idx3 = idx[1];
00325 }
00326 else
00327 {
00328 idx1 = idx[2];
00329 idx2 = idx[0];
00330 idx3 = idx[1];
00331 }
00332 }
00333 else
00334 {
00335 if (idx[1]<idx[2])
00336 {
00337 idx1 = idx[1];
00338 idx2 = idx[2];
00339 idx3 = idx[0];
00340 }
00341 else
00342 {
00343 idx1 = idx[2];
00344 idx2 = idx[1];
00345 idx3 = idx[0];
00346 }
00347 }
00348 positions[idx1].coords[0] += positions[idx2].coords[0]+positions[idx3].coords[0];
00349 positions[idx1].coords[1] += positions[idx2].coords[1]+positions[idx3].coords[1];
00350 positions[idx1].coords[2] += positions[idx2].coords[2]+positions[idx3].coords[2];
00351 if(normals){
00352 (*normals)[idx1].coords[0] += (*normals)[idx2].coords[0]+(*normals)[idx3].coords[0];
00353 (*normals)[idx1].coords[1] += (*normals)[idx2].coords[1]+(*normals)[idx3].coords[1];
00354 (*normals)[idx1].coords[2] += (*normals)[idx2].coords[2]+(*normals)[idx3].coords[2];
00355 }
00356 pointCount[idx1] += pointCount[idx2]+pointCount[idx3];
00357 remapTable[idx2] = idx1;
00358 remapTable[idx3] = idx1;
00359 triangles[i] = triangles[triangles.size()-1];
00360 triangles.pop_back();
00361 }
00362 }
00363
00364 int pCount = 0;
00365 for (i = 0; i < int (positions.size ()); i++)
00366 {
00367 for (j = 0; j < 3; j++)
00368 positions[i].coords[j] /= pointCount[i];
00369 if (normals)
00370 {
00371 Real l = Real (Length ((*normals)[i]));
00372 for (j = 0; j < 3; j++)
00373 (*normals)[i].coords[j] /= l;
00374 }
00375 if (remapTable[i] == i)
00376 {
00377 positions[pCount] = positions[i];
00378 if(normals){(*normals)[pCount] = (*normals)[i];}
00379 pointCount[i] = pCount;
00380 pCount++;
00381 }
00382 }
00383 positions.resize (pCount);
00384 for (i = int (triangles.size ()-1); i >= 0; i--)
00385 {
00386 for (j = 0; j < 3; j++)
00387 {
00388 idx[j] = triangles[i].idx[j];
00389 while(remapTable[idx[j]]<idx[j]){idx[j] = remapTable[idx[j]];}
00390 triangles[i].idx[j] = pointCount[idx[j]];
00391 }
00392 if(idx[0] == idx[1] || idx[0] == idx[2] || idx[1] == idx[2])
00393 {
00394 triangles[i] = triangles[triangles.size()-1];
00395 triangles.pop_back();
00396 }
00397 }
00398 delete[] pointCount;
00399 delete[] remapTable;
00400 }
00401
00402
00404 template<class Real>
00405 long long Triangulation<Real>::EdgeIndex(const int& p1,const int& p2)
00406 {
00407 if (p1>p2)
00408 return (static_cast <long long> (p1)<<32) | (static_cast<long long> (p2));
00409 else
00410 return (static_cast<long long> (p2)<<32) | (static_cast<long long> (p1));
00411 }
00412
00413
00415 template<class Real> int
00416 Triangulation<Real>::factor (const int& tIndex, int& p1, int& p2, int& p3)
00417 {
00418 if (triangles[tIndex].eIndex[0]<0 || triangles[tIndex].eIndex[1]<0 || triangles[tIndex].eIndex[2]<0)
00419 return (0);
00420
00421 if (edges[triangles[tIndex].eIndex[0]].tIndex[0] == tIndex)
00422 p1 = edges[triangles[tIndex].eIndex[0]].pIndex[0];
00423 else
00424 p1 = edges[triangles[tIndex].eIndex[0]].pIndex[1];
00425 if (edges[triangles[tIndex].eIndex[1]].tIndex[0] == tIndex)
00426 p2 = edges[triangles[tIndex].eIndex[1]].pIndex[0];
00427 else
00428 p2 = edges[triangles[tIndex].eIndex[1]].pIndex[1];
00429
00430 if (edges[triangles[tIndex].eIndex[2]].tIndex[0] == tIndex)
00431 p3 = edges[triangles[tIndex].eIndex[2]].pIndex[0];
00432 else
00433 p3 = edges[triangles[tIndex].eIndex[2]].pIndex[1];
00434 return (1);
00435 }
00436
00437
00439 template<class Real>
00440 double Triangulation<Real>::area(const int& p1,const int& p2,const int& p3)
00441 {
00442 Point3D<Real> q1,q2,q;
00443 for (int i = 0; i < 3; i++)
00444 {
00445 q1.coords[i] = points[p2].coords[i]-points[p1].coords[i];
00446 q2.coords[i] = points[p3].coords[i]-points[p1].coords[i];
00447 }
00448 CrossProduct (q1,q2,q);
00449 return Length (q);
00450 }
00451
00452
00454 template<class Real>
00455 double Triangulation<Real>::area(const int& tIndex)
00456 {
00457 int p1,p2,p3;
00458 factor (tIndex, p1, p2, p3);
00459 return area(p1, p2, p3);
00460 }
00461
00462
00464 template<class Real>
00465 double Triangulation<Real>::area()
00466 {
00467 double a = 0;
00468 for (int i = 0; i < int (triangles.size ()); i++)
00469 a += area(i);
00470 return a;
00471 }
00472
00473
00475 template<class Real>
00476 int Triangulation<Real>::addTriangle(const int& p1,const int& p2,const int& p3)
00477 {
00478 hash_map<long long,int>::iterator iter;
00479 int tIdx,eIdx,p[3];
00480 p[0] = p1;
00481 p[1] = p2;
00482 p[2] = p3;
00483 triangles.push_back(TriangulationTriangle());
00484 tIdx = int(triangles.size())-1;
00485
00486 for(int i = 0; i < 3; i++)
00487 {
00488 long long e = EdgeIndex(p[i],p[(i+1)%3]);
00489 iter = edgeMap.find(e);
00490 if (iter == edgeMap.end())
00491 {
00492 TriangulationEdge edge;
00493 edge.pIndex[0] = p[i];
00494 edge.pIndex[1] = p[(i+1)%3];
00495 edges.push_back(edge);
00496 eIdx = int(edges.size())-1;
00497 edgeMap[e] = eIdx;
00498 edges[eIdx].tIndex[0] = tIdx;
00499 }
00500 else
00501 {
00502 eIdx = edgeMap[e];
00503 if (edges[eIdx].pIndex[0] == p[i])
00504 {
00505 if(edges[eIdx].tIndex[0]<0)
00506 edges[eIdx].tIndex[0] = tIdx;
00507 else
00508 {
00509 printf("Edge Triangle in use 1\n");
00510 return 0;
00511 }
00512 }
00513 else
00514 {
00515 if(edges[eIdx].tIndex[1]<0)
00516 edges[eIdx].tIndex[1] = tIdx;
00517 else
00518 {
00519 printf("Edge Triangle in use 2\n");
00520 return 0;
00521 }
00522 }
00523
00524 }
00525 triangles[tIdx].eIndex[i] = eIdx;
00526 }
00527 return tIdx;
00528 }
00529
00530
00532 template<class Real>
00533 int Triangulation<Real>::flipMinimize(const int& eIndex)
00534 {
00535 double oldArea,newArea;
00536 int oldP[3],oldQ[3],newP[3],newQ[3];
00537 TriangulationEdge newEdge;
00538
00539 if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0)
00540 return 0;
00541
00542 if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2]))
00543 return 0;
00544 if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2]))
00545 return 0;
00546
00547 oldArea = area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]);
00548 int idxP,idxQ;
00549 for (idxP = 0; idxP < 3; idxP++)
00550 {
00551 int i;
00552 for(i = 0;i<3;i++)
00553 if(oldP[idxP] == oldQ[i])
00554 break;
00555 if(i == 3)
00556 break;
00557 }
00558 for (idxQ = 0; idxQ < 3; idxQ++)
00559 {
00560 int i;
00561 for (i = 0; i < 3; i++)
00562 if(oldP[i] == oldQ[idxQ])
00563 break;
00564 if (i == 3)
00565 break;
00566 }
00567
00568 if(idxP == 3 || idxQ == 3)
00569 return 0;
00570 newP[0] = oldP[idxP];
00571 newP[1] = oldP[(idxP+1)%3];
00572 newP[2] = oldQ[idxQ];
00573 newQ[0] = oldQ[idxQ];
00574 newQ[1] = oldP[(idxP+2)%3];
00575 newQ[2] = oldP[idxP];
00576
00577 newArea = area(newP[0],newP[1],newP[2]) + area(newQ[0],newQ[1],newQ[2]);
00578 if(oldArea <= newArea)
00579 return 0;
00580
00581
00582 edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
00583
00584 edges[eIndex].pIndex[0] = newP[0];
00585 edges[eIndex].pIndex[1] = newQ[0];
00586
00587 edgeMap[EdgeIndex(newP[0],newQ[0])] = eIndex;
00588
00589 for (int i = 0; i < 3; i++)
00590 {
00591 int idx;
00592 idx = edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])];
00593 triangles[edges[eIndex].tIndex[0]].eIndex[i] = idx;
00594 if (idx != eIndex)
00595 {
00596 if (edges[idx].tIndex[0] == edges[eIndex].tIndex[1])
00597 edges[idx].tIndex[0] = edges[eIndex].tIndex[0];
00598 if (edges[idx].tIndex[1] == edges[eIndex].tIndex[1])
00599 edges[idx].tIndex[1] = edges[eIndex].tIndex[0];
00600 }
00601
00602 idx = edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])];
00603 triangles[edges[eIndex].tIndex[1]].eIndex[i] = idx;
00604 if (idx != eIndex)
00605 {
00606 if (edges[idx].tIndex[0] == edges[eIndex].tIndex[0])
00607 edges[idx].tIndex[0] = edges[eIndex].tIndex[1];
00608 if (edges[idx].tIndex[1] == edges[eIndex].tIndex[0])
00609 edges[idx].tIndex[1] = edges[eIndex].tIndex[1];
00610 }
00611 }
00612 return 1;
00613 }
00614
00615
00616 }
00617 }