geometry.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012,  Willow Garage,  Inc.
00006  *  Copyright (c) 2006,  Michael Kazhdan and Matthew Bolitho,
00007  *                      Johns Hopkins University
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms,  with or without
00012  *  modification,  are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice,  this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice,  this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of Willow Garage,  Inc. nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,  INCLUDING,  BUT NOT
00027  *  LIMITED TO,  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,  INDIRECT,
00030  *  INCIDENTAL,  SPECIAL,  EXEMPLARY,  OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO,  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE,  DATA,  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY,  WHETHER IN CONTRACT,  STRICT
00034  *  LIABILITY,  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE,  EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  * $Id: geometry.h 5531 2012-04-08 09:14:33Z aichim $
00039  *
00040  */
00041 
00042 #ifndef PCL_POISSON_GEOMETRY_H_
00043 #define PCL_POISSON_GEOMETRY_H_
00044 
00045 #include <math.h>
00046 #include <vector>
00047 #include "hash.h"
00048 
00049 namespace pcl 
00050 {
00051   namespace poisson 
00052   {
00053     template<class Real>
00054     Real Random (void);
00055 
00056     template<class Real>
00057     struct Point3D{Real coords[3];};
00058 
00059     template<class Real>
00060     Point3D<Real> RandomBallPoint (void);
00061 
00062     template<class Real>
00063     Point3D<Real> RandomSpherePoint (void);
00064 
00065     template<class Real>
00066     double Length (const Point3D<Real>& p);
00067 
00068     template<class Real>
00069     double SquareLength (const Point3D<Real>& p);
00070 
00071     template<class Real>
00072     double Distance (const Point3D<Real>& p1, const Point3D<Real>& p2);
00073 
00074     template<class Real>
00075     double SquareDistance (const Point3D<Real>& p1, const Point3D<Real>& p2);
00076 
00077     template <class Real>
00078     void CrossProduct (const Point3D<Real>& p1, const Point3D<Real>& p2, Point3D<Real>& p);
00079 
00080     class Edge
00081     {
00082       public:
00083         double p[2][2];
00084         double Length (void) const
00085         {
00086           double d[2];
00087           d[0]=p[0][0]-p[1][0];
00088           d[1]=p[0][1]-p[1][1];
00089 
00090           return sqrt (d[0]*d[0]+d[1]*d[1]);
00091         }
00092     };
00093     
00094     class Triangle
00095     {
00096       public:
00097         double p[3][3];
00098         
00099         double 
00100         Area (void) const
00101         {
00102           double v1[3], v2[3], v[3];
00103           for (int d=0;d<3;d++)
00104           {
00105             v1[d] = p[1][d]-p[0][d];
00106             v2[d] = p[2][d]-p[0][d];
00107           }
00108           v[0] =  v1[1]*v2[2]-v1[2]*v2[1];
00109           v[1] = -v1[0]*v2[2]+v1[2]*v2[0];
00110           v[2] =  v1[0]*v2[1]-v1[1]*v2[0];
00111 
00112           return (sqrt (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]) / 2);
00113         }
00114 
00115         double 
00116         AspectRatio (void) const
00117         {
00118           double d=0;
00119           int i, j;
00120           for (i = 0; i < 3; i++)
00121           {
00122             for (i = 0; i < 3; i++)
00123               for (j = 0; j < 3; j++)
00124               {
00125                 d += (p[(i+1)%3][j]-p[i][j])* (p[(i+1)%3][j]-p[i][j]);
00126               }
00127           }
00128           return (Area () / d);
00129         }
00130     };
00131 
00132     class CoredPointIndex
00133     {
00134       public:
00135         int index;
00136         char inCore;
00137 
00138         int operator == (const CoredPointIndex& cpi) const {return (index==cpi.index) && (inCore==cpi.inCore);};
00139         int operator != (const CoredPointIndex& cpi) const {return (index!=cpi.index) || (inCore!=cpi.inCore);};
00140     };
00141 
00142     class EdgeIndex
00143     {
00144       public:
00145         int idx[2];
00146     };
00147 
00148     class CoredEdgeIndex
00149     {
00150       public:
00151         CoredPointIndex idx[2];
00152     };
00153 
00154     class TriangleIndex
00155     {
00156       public:
00157         int idx[3];
00158     };
00159 
00160     class TriangulationEdge
00161     {
00162       public:
00163         TriangulationEdge (void);
00164         int pIndex[2];
00165         int tIndex[2];
00166     };
00167 
00168     class TriangulationTriangle
00169     {
00170       public:
00171         TriangulationTriangle (void);
00172         int eIndex[3];
00173     };
00174 
00175     template<class Real>
00176     class Triangulation
00177     {
00178       public:
00179         Triangulation () : points (),  edges (),  triangles (),  edgeMap () {}
00180 
00181         std::vector<Point3D<Real> >        points;
00182         std::vector<TriangulationEdge>     edges;
00183         std::vector<TriangulationTriangle> triangles;
00184 
00185         int 
00186         factor (const int& tIndex, int& p1, int& p2, int& p3);
00187         
00188         double 
00189         area (void);
00190 
00191         double 
00192         area (const int& tIndex);
00193 
00194         double 
00195         area (const int& p1, const int& p2, const int& p3);
00196 
00197         int 
00198         flipMinimize (const int& eIndex);
00199 
00200         int 
00201         addTriangle (const int& p1, const int& p2, const int& p3);
00202 
00203       protected:
00204         hash_map<long long, int> edgeMap;
00205         static long long EdgeIndex (const int& p1, const int& p2);
00206         double area (const Triangle& t);
00207     };
00208 
00209 
00210     template<class Real> void 
00211     EdgeCollapse (const Real& edgeRatio,
00212                   std::vector<TriangleIndex>& triangles,
00213                   std::vector< Point3D<Real> >& positions,
00214                   std::vector<Point3D<Real> >* normals);
00215     
00216     template<class Real> void 
00217     TriangleCollapse (const Real& edgeRatio,
00218                       std::vector<TriangleIndex>& triangles,
00219                       std::vector<Point3D<Real> >& positions,
00220                       std::vector<Point3D<Real> >* normals);
00221 
00222     struct CoredVertexIndex
00223     {
00224       int idx;
00225       bool inCore;
00226     };
00227 
00228     class CoredMeshData
00229     {
00230       public:
00231         CoredMeshData () : inCorePoints () {}
00232 
00233         virtual ~CoredMeshData () {}
00234 
00235         std::vector<Point3D<float> > inCorePoints;
00236         
00237         virtual void 
00238         resetIterator () = 0;
00239 
00240         virtual int 
00241         addOutOfCorePoint (const Point3D<float>& p) = 0;
00242 
00243         virtual int 
00244         addPolygon (const std::vector< CoredVertexIndex >& vertices) = 0;
00245 
00246         virtual int 
00247         nextOutOfCorePoint (Point3D<float>& p) = 0;
00248         
00249         virtual int 
00250         nextPolygon (std::vector<CoredVertexIndex >& vertices) = 0;
00251 
00252         virtual int 
00253         outOfCorePointCount () = 0;
00254         
00255         virtual int 
00256         polygonCount () = 0;
00257     };
00258 
00259     class CoredVectorMeshData : public CoredMeshData
00260     {
00261       std::vector<Point3D<float> > oocPoints;
00262       std::vector< std::vector< int > > polygons;
00263       int polygonIndex;
00264       int oocPointIndex;
00265 
00266       public:
00267         CoredVectorMeshData ();
00268         
00269         virtual ~CoredVectorMeshData () {}
00270 
00271         void resetIterator (void);
00272 
00273         int addOutOfCorePoint (const Point3D<float>& p);
00274         int addPolygon (const std::vector< CoredVertexIndex >& vertices);
00275 
00276         int nextOutOfCorePoint (Point3D<float>& p);
00277         int nextPolygon (std::vector< CoredVertexIndex >& vertices);
00278 
00279         int outOfCorePointCount (void);
00280         int polygonCount (void);
00281     };
00282 
00283     class CoredFileMeshData : public CoredMeshData
00284     {
00285       FILE *oocPointFile ,  *polygonFile;
00286       int oocPoints ,  polygons;
00287       public:
00288         CoredFileMeshData ();
00289         virtual ~CoredFileMeshData ();
00290 
00291         void resetIterator (void);
00292 
00293         int addOutOfCorePoint (const Point3D<float>& p);
00294         int addPolygon (const std::vector< CoredVertexIndex >& vertices);
00295 
00296         int nextOutOfCorePoint (Point3D<float>& p);
00297         int nextPolygon (std::vector< CoredVertexIndex >& vertices);
00298 
00299         int outOfCorePointCount (void);
00300         int polygonCount (void);
00301     };
00302   }
00303 }
00304 
00305 #include <pcl/surface/impl/poisson/geometry.hpp>
00306 
00307 #endif /* PCL_POISSON_GEOMETRY_H_ */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:13