29#include <pcl/console/print.h>
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]);
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];
93 for(i=
int(triangles.size()-1);i>=0;i--){
95 idx[j]=triangles[i].idx[j];
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();
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];
123 if(idx[j]<idx[(j+1)%3]){
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];
141 triangles[i]=triangles[triangles.size()-1];
142 triangles.pop_back();
150 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
154 if(normals){(*normals)[
pCount]=(*normals)[i];}
160 for(i=
int(triangles.size()-1);i>=0;i--){
162 idx[j]=triangles[i].idx[j];
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();
189 for(i=
int(triangles.size()-1);i>=0;i--){
191 idx[j]=triangles[i].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();
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];
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];
254 triangles[i]=triangles[triangles.size()-1];
255 triangles.pop_back();
263 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
267 if(normals){(*normals)[
pCount]=(*normals)[i];}
273 for(i=
int(triangles.size()-1);i>=0;i--){
275 idx[j]=triangles[i].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];}
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];
324 factor(tIndex,p1,p2,p3);
325 return area(p1,p2,p3);
331 for(
int i=0;i<
int(triangles.size());i++){a+=area(i);}
347 if(edgeMap.count(
e) == 0)
351 edge.pIndex[1]=p[(i+1)%3];
352 edges.push_back(
edge);
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;}
364 if(edges[
eIdx].tIndex[1]<0){edges[
eIdx].tIndex[1]=
tIdx;}
365 else{PCL_DEBUG(
"Edge Triangle in use 2\n");
return 0;}
380 if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){
return 0;}
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;}
409 edgeMap.erase(
EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
411 edges[eIndex].pIndex[0]=
newP[0];
412 edges[eIndex].pIndex[1]=
newQ[0];
416 for(
int i=0;i<3;i++){
419 triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx;
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];}
426 triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx;
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];}
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
int flipMinimize(int eIndex)
static long long EdgeIndex(int p1, int p2)
int factor(int tIndex, int &p1, int &p2, int &p3)
int addTriangle(int p1, int p2, int p3)
double Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Point3D< Real > RandomSpherePoint(void)
double SquareLength(const Point3D< Real > &p)
void TriangleCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
void EdgeCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Point3D< Real > RandomBallPoint(void)
double Length(const Point3D< Real > &p)