Point Cloud Library (PCL) 1.12.0
geometry.hpp
1/*
2Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without modification,
6are permitted provided that the following conditions are met:
7
8Redistributions of source code must retain the above copyright notice, this list of
9conditions and the following disclaimer. Redistributions in binary form must reproduce
10the above copyright notice, this list of conditions and the following disclaimer
11in the documentation and/or other materials provided with the distribution.
12
13Neither the name of the Johns Hopkins University nor the names of its contributors
14may be used to endorse or promote products derived from this software without specific
15prior written permission.
16
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26DAMAGE.
27*/
28
29#include <pcl/console/print.h>
30
31namespace pcl
32{
33 namespace poisson
34 {
35 template<class Real>
36 Real Random(void){return Real(rand())/RAND_MAX;}
37
38 template<class Real>
41 while(1){
42 p.coords[0]=Real(1.0-2.0*Random<Real>());
43 p.coords[1]=Real(1.0-2.0*Random<Real>());
44 p.coords[2]=Real(1.0-2.0*Random<Real>());
45 double l=SquareLength(p);
46 if(l<=1){return p;}
47 }
48 }
49 template<class Real>
51 Point3D<Real> p=RandomBallPoint<Real>();
52 Real l=Real(Length(p));
53 p.coords[0]/=l;
54 p.coords[1]/=l;
55 p.coords[2]/=l;
56 return p;
57 }
58
59 template<class Real>
60 double SquareLength(const Point3D<Real>& p){return p.coords[0]*p.coords[0]+p.coords[1]*p.coords[1]+p.coords[2]*p.coords[2];}
61
62 template<class Real>
63 double Length(const Point3D<Real>& p){return sqrt(SquareLength(p));}
64
65 template<class Real>
66 double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2){
67 return (p1.coords[0]-p2.coords[0])*(p1.coords[0]-p2.coords[0])+(p1.coords[1]-p2.coords[1])*(p1.coords[1]-p2.coords[1])+(p1.coords[2]-p2.coords[2])*(p1.coords[2]-p2.coords[2]);
68 }
69
70 template<class Real>
71 double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2){return sqrt(SquareDistance(p1,p2));}
72
73 template <class Real>
75 p.coords[0]= p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1];
76 p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0];
77 p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0];
78 }
79
80 template<class Real>
81 void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
82 int i,j,*remapTable,*pointCount,idx[3];
83 Point3D<Real> p[3],q[2],c;
84 double d[3],a;
85 double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle
86
87 remapTable=new int[positions.size()];
88 pointCount=new int[positions.size()];
89 for(i=0;i<int(positions.size());i++){
90 remapTable[i]=i;
91 pointCount[i]=1;
92 }
93 for(i=int(triangles.size()-1);i>=0;i--){
94 for(j=0;j<3;j++){
95 idx[j]=triangles[i].idx[j];
96 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
97 }
98 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
99 triangles[i]=triangles[triangles.size()-1];
100 triangles.pop_back();
101 continue;
102 }
103 for(j=0;j<3;j++){
104 p[j].coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]];
105 p[j].coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]];
106 p[j].coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]];
107 }
108 for(j=0;j<3;j++){
109 q[0].coords[j]=p[1].coords[j]-p[0].coords[j];
110 q[1].coords[j]=p[2].coords[j]-p[0].coords[j];
111 d[j]=SquareDistance(p[j],p[(j+1)%3]);
112 }
113 CrossProduct(q[0],q[1],c);
114 a=Length(c)/2;
115
116 if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
117 // Find the smallest edge
118 j=0;
119 if(d[1]<d[j]){j=1;}
120 if(d[2]<d[j]){j=2;}
121
122 int idx1,idx2;
123 if(idx[j]<idx[(j+1)%3]){
124 idx1=idx[j];
125 idx2=idx[(j+1)%3];
126 }
127 else{
128 idx2=idx[j];
129 idx1=idx[(j+1)%3];
130 }
131 positions[idx1].coords[0]+=positions[idx2].coords[0];
132 positions[idx1].coords[1]+=positions[idx2].coords[1];
133 positions[idx1].coords[2]+=positions[idx2].coords[2];
134 if(normals){
135 (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0];
136 (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1];
137 (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2];
138 }
139 pointCount[idx1]+=pointCount[idx2];
140 remapTable[idx2]=idx1;
141 triangles[i]=triangles[triangles.size()-1];
142 triangles.pop_back();
143 }
144 }
145 int pCount=0;
146 for(i=0;i<int(positions.size());i++){
147 for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
148 if(normals){
149 Real l=Real(Length((*normals)[i]));
150 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
151 }
152 if(remapTable[i]==i){ // If vertex i is being used
153 positions[pCount]=positions[i];
154 if(normals){(*normals)[pCount]=(*normals)[i];}
155 pointCount[i]=pCount;
156 pCount++;
157 }
158 }
159 positions.resize(pCount);
160 for(i=int(triangles.size()-1);i>=0;i--){
161 for(j=0;j<3;j++){
162 idx[j]=triangles[i].idx[j];
163 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
164 triangles[i].idx[j]=pointCount[idx[j]];
165 }
166 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
167 triangles[i]=triangles[triangles.size()-1];
168 triangles.pop_back();
169 }
170 }
171
172 delete[] pointCount;
173 delete[] remapTable;
174 }
175
176 template<class Real>
177 void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
178 int i,j,*remapTable,*pointCount,idx[3];
179 Point3D<Real> p[3],q[2],c;
180 double d[3],a;
181 double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle
182
183 remapTable=new int[positions.size()];
184 pointCount=new int[positions.size()];
185 for(i=0;i<int(positions.size());i++){
186 remapTable[i]=i;
187 pointCount[i]=1;
188 }
189 for(i=int(triangles.size()-1);i>=0;i--){
190 for(j=0;j<3;j++){
191 idx[j]=triangles[i].idx[j];
192 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
193 }
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();
197 continue;
198 }
199 for(j=0;j<3;j++){
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]];
203 }
204 for(j=0;j<3;j++){
205 q[0].coords[j]=p[1].coords[j]-p[0].coords[j];
206 q[1].coords[j]=p[2].coords[j]-p[0].coords[j];
207 d[j]=SquareDistance(p[j],p[(j+1)%3]);
208 }
209 CrossProduct(q[0],q[1],c);
210 a=Length(c)/2;
211
212 if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
213 // Find the smallest edge
214 j=0;
215 if(d[1]<d[j]){j=1;}
216 if(d[2]<d[j]){j=2;}
217
218 int idx1,idx2,idx3;
219 if(idx[0]<idx[1]){
220 if(idx[0]<idx[2]){
221 idx1=idx[0];
222 idx2=idx[2];
223 idx3=idx[1];
224 }
225 else{
226 idx1=idx[2];
227 idx2=idx[0];
228 idx3=idx[1];
229 }
230 }
231 else{
232 if(idx[1]<idx[2]){
233 idx1=idx[1];
234 idx2=idx[2];
235 idx3=idx[0];
236 }
237 else{
238 idx1=idx[2];
239 idx2=idx[1];
240 idx3=idx[0];
241 }
242 }
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];
246 if(normals){
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];
250 }
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();
256 }
257 }
258 int pCount=0;
259 for(i=0;i<int(positions.size());i++){
260 for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
261 if(normals){
262 Real l=Real(Length((*normals)[i]));
263 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
264 }
265 if(remapTable[i]==i){ // If vertex i is being used
266 positions[pCount]=positions[i];
267 if(normals){(*normals)[pCount]=(*normals)[i];}
268 pointCount[i]=pCount;
269 pCount++;
270 }
271 }
272 positions.resize(pCount);
273 for(i=int(triangles.size()-1);i>=0;i--){
274 for(j=0;j<3;j++){
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]];
278 }
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();
282 }
283 }
284 delete[] pointCount;
285 delete[] remapTable;
286 }
287
288 ///////////////////
289 // Triangulation //
290 ///////////////////
291 template<class Real>
292 long long Triangulation<Real>::EdgeIndex( int p1 , int p2 )
293 {
294 if(p1>p2) {return ((long long)(p1)<<32) | ((long long)(p2));}
295 else {return ((long long)(p2)<<32) | ((long long)(p1));}
296 }
297
298 template<class Real>
299 int Triangulation<Real>::factor(int tIndex,int& p1,int& p2,int & p3){
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];}
307 return 1;
308 }
309
310 template<class Real>
311 double Triangulation<Real>::area(int p1,int p2,int p3){
312 Point3D<Real> q1,q2,q;
313 for(int i=0;i<3;i++){
314 q1.coords[i]=points[p2].coords[i]-points[p1].coords[i];
315 q2.coords[i]=points[p3].coords[i]-points[p1].coords[i];
316 }
317 CrossProduct(q1,q2,q);
318 return Length(q);
319 }
320
321 template<class Real>
322 double Triangulation<Real>::area(int tIndex){
323 int p1,p2,p3;
324 factor(tIndex,p1,p2,p3);
325 return area(p1,p2,p3);
326 }
327
328 template<class Real>
330 double a=0;
331 for(int i=0;i<int(triangles.size());i++){a+=area(i);}
332 return a;
333 }
334
335 template<class Real>
336 int Triangulation<Real>::addTriangle(int p1,int p2,int p3){
337 int tIdx,eIdx,p[3];
338 p[0]=p1;
339 p[1]=p2;
340 p[2]=p3;
341 triangles.push_back(TriangulationTriangle());
342 tIdx=int(triangles.size())-1;
343
344 for(int i=0;i<3;i++)
345 {
346 long long e = EdgeIndex(p[i],p[(i+1)%3]);
347 if(edgeMap.count(e) == 0)
348 {
350 edge.pIndex[0]=p[i];
351 edge.pIndex[1]=p[(i+1)%3];
352 edges.push_back(edge);
353 eIdx=int(edges.size())-1;
354 edgeMap[e]=eIdx;
355 edges[eIdx].tIndex[0]=tIdx;
356 }
357 else{
358 eIdx=edgeMap[e];
359 if(edges[eIdx].pIndex[0]==p[i]){
360 if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;}
361 else{PCL_DEBUG("Edge Triangle in use 1\n");return 0;}
362 }
363 else{
364 if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;}
365 else{PCL_DEBUG("Edge Triangle in use 2\n");return 0;}
366 }
367
368 }
369 triangles[tIdx].eIndex[i]=eIdx;
370 }
371 return tIdx;
372 }
373
374 template<class Real>
376 double oldArea,newArea;
377 int oldP[3],oldQ[3],newP[3],newQ[3];
378 TriangulationEdge newEdge;
379
380 if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;}
381
382 if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;}
383 if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;}
384
385 oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]);
386 int idxP,idxQ;
387 for(idxP=0;idxP<3;idxP++){
388 int i;
389 for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}}
390 if(i==3){break;}
391 }
392 for(idxQ=0;idxQ<3;idxQ++){
393 int i;
394 for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}}
395 if(i==3){break;}
396 }
397 if(idxP==3 || idxQ==3){return 0;}
398 newP[0]=oldP[idxP];
399 newP[1]=oldP[(idxP+1)%3];
400 newP[2]=oldQ[idxQ];
401 newQ[0]=oldQ[idxQ];
402 newQ[1]=oldP[(idxP+2)%3];
403 newQ[2]=oldP[idxP];
404
405 newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]);
406 if(oldArea<=newArea){return 0;}
407
408 // Remove the entry in the hash_table for the old edge
409 edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
410 // Set the new edge so that the zero-side is newQ
411 edges[eIndex].pIndex[0]=newP[0];
412 edges[eIndex].pIndex[1]=newQ[0];
413 // Insert the entry into the hash_table for the new edge
414 edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex;
415 // Update the triangle information
416 for(int i=0;i<3;i++){
417 int idx;
418 idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])];
419 triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx;
420 if(idx!=eIndex){
421 if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];}
422 if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];}
423 }
424
425 idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])];
426 triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx;
427 if(idx!=eIndex){
428 if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];}
429 if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];}
430 }
431 }
432 return 1;
433 }
434 }
435}
int flipMinimize(int eIndex)
Definition: geometry.hpp:375
static long long EdgeIndex(int p1, int p2)
Definition: geometry.hpp:292
int factor(int tIndex, int &p1, int &p2, int &p3)
Definition: geometry.hpp:299
int addTriangle(int p1, int p2, int p3)
Definition: geometry.hpp:336
pcl::detail::MeshIndex< struct EdgeIndexTag > EdgeIndex
Index used to access elements in the half-edge mesh.
Definition: mesh_indices.h:204
double Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:71
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:66
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:74
Real Random(void)
Definition: geometry.hpp:36
Point3D< Real > RandomSpherePoint(void)
Definition: geometry.hpp:50
double SquareLength(const Point3D< Real > &p)
Definition: geometry.hpp:60
void TriangleCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Definition: geometry.hpp:177
void EdgeCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Definition: geometry.hpp:81
Point3D< Real > RandomBallPoint(void)
Definition: geometry.hpp:39
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:63