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 #ifndef GEOMETRY_INCLUDED
00030 #define GEOMETRY_INCLUDED
00031
00032 #if defined __GNUC__
00033 # pragma GCC system_header
00034 #endif
00035
00036 #include <pcl/pcl_macros.h>
00037 #include <math.h>
00038 #include <vector>
00039 #include "hash.h"
00040
00041 namespace pcl
00042 {
00043 namespace poisson
00044 {
00045
00046 template<class Real>
00047 Real Random(void);
00048
00049 template< class Real >
00050 struct Point3D
00051 {
00052 Real coords[3];
00053 Point3D( void ) { coords[0] = coords[1] = coords[2] = Real(0); }
00054 inline Real& operator[] ( int i ) { return coords[i]; }
00055 inline const Real& operator[] ( int i ) const { return coords[i]; }
00056 inline Point3D& operator += ( Point3D p ){ coords[0] += p.coords[0] , coords[1] += p.coords[1] , coords[2] += p.coords[2] ; return *this; }
00057 inline Point3D& operator -= ( Point3D p ){ coords[0] -= p.coords[0] , coords[1] -= p.coords[1] , coords[2] -= p.coords[2] ; return *this; }
00058 inline Point3D& operator *= ( Real r ){ coords[0] *= r , coords[1] *= r , coords[2] *= r ; return *this; }
00059 inline Point3D& operator /= ( Real r ){ coords[0] /= r , coords[1] /= r , coords[2] /= r ; return *this; }
00060 inline Point3D operator + ( Point3D p ) const { Point3D q ; q.coords[0] = coords[0] + p.coords[0] , q.coords[1] = coords[1] + p.coords[1] , q.coords[2] = coords[2] + p.coords[2] ; return q; }
00061 inline Point3D operator - ( Point3D p ) const { Point3D q ; q.coords[0] = coords[0] - p.coords[0] , q.coords[1] = coords[1] - p.coords[1] , q.coords[2] = coords[2] - p.coords[2] ; return q; }
00062 inline Point3D operator * ( Real r ) const { Point3D q ; q.coords[0] = coords[0] * r , q.coords[1] = coords[1] * r , q.coords[2] = coords[2] * r ; return q; }
00063 inline Point3D operator / ( Real r ) const { return (*this) * ( Real(1.)/r ); }
00064 };
00065
00066 template<class Real>
00067 Point3D<Real> RandomBallPoint(void);
00068
00069 template<class Real>
00070 Point3D<Real> RandomSpherePoint(void);
00071
00072 template<class Real>
00073 double Length(const Point3D<Real>& p);
00074
00075 template<class Real>
00076 double SquareLength(const Point3D<Real>& p);
00077
00078 template<class Real>
00079 double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2);
00080
00081 template<class Real>
00082 double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2);
00083
00084 template <class Real>
00085 void CrossProduct(const Point3D<Real>& p1,const Point3D<Real>& p2,Point3D<Real>& p);
00086
00087 class Edge
00088 {
00089 public:
00090 double p[2][2];
00091 double Length(void) const
00092 {
00093 double d[2];
00094 d[0]=p[0][0]-p[1][0];
00095 d[1]=p[0][1]-p[1][1];
00096
00097 return sqrt(d[0]*d[0]+d[1]*d[1]);
00098 }
00099 };
00100 class Triangle
00101 {
00102 public:
00103 double p[3][3];
00104 double Area(void) const
00105 {
00106 double v1[3] , v2[3] , v[3];
00107 for( int d=0 ; d<3 ; d++ )
00108 {
00109 v1[d] = p[1][d] - p[0][d];
00110 v2[d] = p[2][d] - p[0][d];
00111 }
00112 v[0] = v1[1]*v2[2] - v1[2]*v2[1];
00113 v[1] = -v1[0]*v2[2] + v1[2]*v2[0];
00114 v[2] = v1[0]*v2[1] - v1[1]*v2[0];
00115 return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] ) / 2;
00116 }
00117 double AspectRatio(void) const
00118 {
00119 double d=0;
00120 int i,j;
00121 for(i=0;i<3;i++){
00122 for(i=0;i<3;i++)
00123 for(j=0;j<3;j++){d+=(p[(i+1)%3][j]-p[i][j])*(p[(i+1)%3][j]-p[i][j]);}
00124 }
00125 return Area()/d;
00126 }
00127
00128 };
00129 class PCL_EXPORTS CoredPointIndex
00130 {
00131 public:
00132 int index;
00133 char inCore;
00134
00135 int operator == (const CoredPointIndex& cpi) const {return (index==cpi.index) && (inCore==cpi.inCore);};
00136 int operator != (const CoredPointIndex& cpi) const {return (index!=cpi.index) || (inCore!=cpi.inCore);};
00137 };
00138 class EdgeIndex{
00139 public:
00140 int idx[2];
00141 };
00142 class CoredEdgeIndex{
00143 public:
00144 CoredPointIndex idx[2];
00145 };
00146 class TriangleIndex{
00147 public:
00148 int idx[3];
00149 };
00150
00151 class TriangulationEdge
00152 {
00153 public:
00154 TriangulationEdge(void);
00155 int pIndex[2];
00156 int tIndex[2];
00157 };
00158
00159 class TriangulationTriangle
00160 {
00161 public:
00162 TriangulationTriangle(void);
00163 int eIndex[3];
00164 };
00165
00166 template<class Real>
00167 class Triangulation
00168 {
00169 public:
00170
00171 std::vector<Point3D<Real> > points;
00172 std::vector<TriangulationEdge> edges;
00173 std::vector<TriangulationTriangle> triangles;
00174
00175 int factor( int tIndex,int& p1,int& p2,int& p3);
00176 double area(void);
00177 double area( int tIndex );
00178 double area( int p1 , int p2 , int p3 );
00179 int flipMinimize( int eIndex);
00180 int addTriangle( int p1 , int p2 , int p3 );
00181
00182 protected:
00183 hash_map<long long,int> edgeMap;
00184 static long long EdgeIndex( int p1 , int p2 );
00185 double area(const Triangle& t);
00186 };
00187
00188
00189 template<class Real>
00190 void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector<Point3D<Real> >* normals);
00191 template<class Real>
00192 void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector<Point3D<Real> >& positions,std::vector<Point3D<Real> >* normals);
00193
00194 struct CoredVertexIndex
00195 {
00196 int idx;
00197 bool inCore;
00198 };
00199 class PCL_EXPORTS CoredMeshData
00200 {
00201 public:
00202 std::vector<Point3D<float> > inCorePoints;
00203 virtual void resetIterator( void ) = 0;
00204
00205 virtual int addOutOfCorePoint( const Point3D<float>& p ) = 0;
00206 virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0;
00207
00208 virtual int nextOutOfCorePoint( Point3D<float>& p )=0;
00209 virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0;
00210
00211 virtual int outOfCorePointCount(void)=0;
00212 virtual int polygonCount( void ) = 0;
00213 };
00214
00215 class PCL_EXPORTS CoredMeshData2
00216 {
00217 public:
00218 struct Vertex
00219 {
00220 Point3D< float > start , end;
00221 float value;
00222 Vertex( void ) { ; }
00223 Vertex( Point3D< float > s , Point3D< float > e , float v ) { start = s , end = e , value = v; }
00224 Vertex( Point3D< float > s , Point3D< float > e , Point3D< float > p )
00225 {
00226 start = s , end = e;
00227
00228
00229
00230 Point3D< float > p1 = p-s , p2 = e-s;
00231 value = ( p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2] ) / ( p2[0] * p2[0] + p2[1] * p2[1] + p2[2] * p2[2] );
00232 }
00233 };
00234 std::vector< Vertex > inCorePoints;
00235 virtual void resetIterator( void ) = 0;
00236
00237 virtual int addOutOfCorePoint( const Vertex& v ) = 0;
00238 virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0;
00239
00240 virtual int nextOutOfCorePoint( Vertex& v ) = 0;
00241 virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0;
00242
00243 virtual int outOfCorePointCount( void )=0;
00244 virtual int polygonCount( void ) = 0;
00245 };
00246
00247 class PCL_EXPORTS CoredVectorMeshData : public CoredMeshData
00248 {
00249 std::vector<Point3D<float> > oocPoints;
00250 std::vector< std::vector< int > > polygons;
00251 int polygonIndex;
00252 int oocPointIndex;
00253 public:
00254 CoredVectorMeshData(void);
00255
00256 void resetIterator(void);
00257
00258 int addOutOfCorePoint( const Point3D<float>& p );
00259 int addPolygon( const std::vector< CoredVertexIndex >& vertices );
00260
00261 int nextOutOfCorePoint( Point3D<float>& p );
00262 int nextPolygon( std::vector< CoredVertexIndex >& vertices );
00263
00264 int outOfCorePointCount(void);
00265 int polygonCount( void );
00266 };
00267 class PCL_EXPORTS CoredVectorMeshData2 : public CoredMeshData2
00268 {
00269 std::vector< CoredMeshData2::Vertex > oocPoints;
00270 std::vector< std::vector< int > > polygons;
00271 int polygonIndex;
00272 int oocPointIndex;
00273 public:
00274 CoredVectorMeshData2( void );
00275
00276 void resetIterator(void);
00277
00278 int addOutOfCorePoint( const CoredMeshData2::Vertex& v );
00279 int addPolygon( const std::vector< CoredVertexIndex >& vertices );
00280
00281 int nextOutOfCorePoint( CoredMeshData2::Vertex& v );
00282 int nextPolygon( std::vector< CoredVertexIndex >& vertices );
00283
00284 int outOfCorePointCount( void );
00285 int polygonCount( void );
00286 };
00287 class CoredFileMeshData : public CoredMeshData
00288 {
00289 FILE *oocPointFile , *polygonFile;
00290 int oocPoints , polygons;
00291 public:
00292 CoredFileMeshData(void);
00293 ~CoredFileMeshData(void);
00294
00295 void resetIterator(void);
00296
00297 int addOutOfCorePoint(const Point3D<float>& p);
00298 int addPolygon( const std::vector< CoredVertexIndex >& vertices );
00299
00300 int nextOutOfCorePoint(Point3D<float>& p);
00301 int nextPolygon( std::vector< CoredVertexIndex >& vertices );
00302
00303 int outOfCorePointCount(void);
00304 int polygonCount( void );
00305 };
00306 class CoredFileMeshData2 : public CoredMeshData2
00307 {
00308 FILE *oocPointFile , *polygonFile;
00309 int oocPoints , polygons;
00310 public:
00311 CoredFileMeshData2( void );
00312 ~CoredFileMeshData2( void );
00313
00314 void resetIterator( void );
00315
00316 int addOutOfCorePoint( const CoredMeshData2::Vertex& v );
00317 int addPolygon( const std::vector< CoredVertexIndex >& vertices );
00318
00319 int nextOutOfCorePoint( CoredMeshData2::Vertex& v );
00320 int nextPolygon( std::vector< CoredVertexIndex >& vertices );
00321
00322 int outOfCorePointCount( void );
00323 int polygonCount( void );
00324 };
00325 }
00326 }
00327
00328 #include "geometry.hpp"
00329
00330
00331
00332
00333 #endif // GEOMETRY_INCLUDED