btConvexHull.h
Go to the documentation of this file.
00001 
00002 /*
00003 Stan Melax Convex Hull Computation
00004 Copyright (c) 2008 Stan Melax http://www.melax.com/
00005 
00006 This software is provided 'as-is', without any express or implied warranty.
00007 In no event will the authors be held liable for any damages arising from the use of this software.
00008 Permission is granted to anyone to use this software for any purpose, 
00009 including commercial applications, and to alter it and redistribute it freely, 
00010 subject to the following restrictions:
00011 
00012 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00013 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00014 3. This notice may not be removed or altered from any source distribution.
00015 */
00016 
00018 
00019 #ifndef BT_CD_HULL_H
00020 #define BT_CD_HULL_H
00021 
00022 #include "btVector3.h"
00023 #include "btAlignedObjectArray.h"
00024 
00025 typedef btAlignedObjectArray<unsigned int> TUIntArray;
00026 
00027 class HullResult
00028 {
00029 public:
00030         HullResult(void)
00031         {
00032                 mPolygons = true;
00033                 mNumOutputVertices = 0;
00034                 mNumFaces = 0;
00035                 mNumIndices = 0;
00036         }
00037         bool                    mPolygons;                  // true if indices represents polygons, false indices are triangles
00038         unsigned int            mNumOutputVertices;         // number of vertices in the output hull
00039         btAlignedObjectArray<btVector3> m_OutputVertices;            // array of vertices
00040         unsigned int            mNumFaces;                  // the number of faces produced
00041         unsigned int            mNumIndices;                // the total number of indices
00042         btAlignedObjectArray<unsigned int>    m_Indices;                   // pointer to indices.
00043 
00044 // If triangles, then indices are array indexes into the vertex list.
00045 // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
00046 };
00047 
00048 enum HullFlag
00049 {
00050         QF_TRIANGLES         = (1<<0),             // report results as triangles, not polygons.
00051         QF_REVERSE_ORDER     = (1<<1),             // reverse order of the triangle indices.
00052         QF_DEFAULT           = QF_TRIANGLES
00053 };
00054 
00055 
00056 class HullDesc
00057 {
00058 public:
00059         HullDesc(void)
00060         {
00061                 mFlags          = QF_DEFAULT;
00062                 mVcount         = 0;
00063                 mVertices       = 0;
00064                 mVertexStride   = sizeof(btVector3);
00065                 mNormalEpsilon  = 0.001f;
00066                 mMaxVertices    = 4096; // maximum number of points to be considered for a convex hull.
00067                 mMaxFaces       = 4096;
00068         };
00069 
00070         HullDesc(HullFlag flag,
00071                  unsigned int vcount,
00072                  const btVector3 *vertices,
00073                  unsigned int stride = sizeof(btVector3))
00074         {
00075                 mFlags          = flag;
00076                 mVcount         = vcount;
00077                 mVertices       = vertices;
00078                 mVertexStride   = stride;
00079                 mNormalEpsilon  = btScalar(0.001);
00080                 mMaxVertices    = 4096;
00081         }
00082 
00083         bool HasHullFlag(HullFlag flag) const
00084         {
00085                 if ( mFlags & flag ) return true;
00086                 return false;
00087         }
00088 
00089         void SetHullFlag(HullFlag flag)
00090         {
00091                 mFlags|=flag;
00092         }
00093 
00094         void ClearHullFlag(HullFlag flag)
00095         {
00096                 mFlags&=~flag;
00097         }
00098 
00099         unsigned int      mFlags;           // flags to use when generating the convex hull.
00100         unsigned int      mVcount;          // number of vertices in the input point cloud
00101         const btVector3  *mVertices;        // the array of vertices.
00102         unsigned int      mVertexStride;    // the stride of each vertex, in bytes.
00103         btScalar             mNormalEpsilon;   // the epsilon for removing duplicates.  This is a normalized value, if normalized bit is on.
00104         unsigned int      mMaxVertices;     // maximum number of vertices to be considered for the hull!
00105         unsigned int      mMaxFaces;
00106 };
00107 
00108 enum HullError
00109 {
00110         QE_OK,            // success!
00111         QE_FAIL           // failed.
00112 };
00113 
00114 class btPlane
00115 {
00116         public:
00117         btVector3       normal;
00118         btScalar        dist;   // distance below origin - the D from plane equasion Ax+By+Cz+D=0
00119                         btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){}
00120                         btPlane():normal(),dist(0){}
00121         
00122 };
00123 
00124 
00125 
00126 class ConvexH 
00127 {
00128   public:
00129         class HalfEdge
00130         {
00131           public:
00132                 short ea;         // the other half of the edge (index into edges list)
00133                 unsigned char v;  // the vertex at the start of this edge (index into vertices list)
00134                 unsigned char p;  // the facet on which this edge lies (index into facets list)
00135                 HalfEdge(){}
00136                 HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
00137         };
00138         ConvexH()
00139         {
00140         }
00141         ~ConvexH()
00142         {
00143         }
00144         btAlignedObjectArray<btVector3> vertices;
00145         btAlignedObjectArray<HalfEdge> edges;
00146         btAlignedObjectArray<btPlane>  facets;
00147         ConvexH(int vertices_size,int edges_size,int facets_size);
00148 };
00149 
00150 
00151 class int4
00152 {
00153 public:
00154         int x,y,z,w;
00155         int4(){};
00156         int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
00157         const int& operator[](int i) const {return (&x)[i];}
00158         int& operator[](int i) {return (&x)[i];}
00159 };
00160 
00161 class PHullResult
00162 {
00163 public:
00164 
00165         PHullResult(void)
00166         {
00167                 mVcount = 0;
00168                 mIndexCount = 0;
00169                 mFaceCount = 0;
00170                 mVertices = 0;
00171         }
00172 
00173         unsigned int mVcount;
00174         unsigned int mIndexCount;
00175         unsigned int mFaceCount;
00176         btVector3*   mVertices;
00177         TUIntArray m_Indices;
00178 };
00179 
00180 
00181 
00184 class HullLibrary
00185 {
00186 
00187         btAlignedObjectArray<class btHullTriangle*> m_tris;
00188 
00189 public:
00190 
00191         btAlignedObjectArray<int> m_vertexIndexMapping;
00192 
00193 
00194         HullError CreateConvexHull(const HullDesc& desc, // describes the input request
00195                                    HullResult&     result);        // contains the resulst
00196         HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
00197 
00198 private:
00199 
00200         bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
00201 
00202         class btHullTriangle*   allocateTriangle(int a,int b,int c);
00203         void    deAllocateTriangle(btHullTriangle*);
00204         void b2bfix(btHullTriangle* s,btHullTriangle*t);
00205 
00206         void removeb2b(btHullTriangle* s,btHullTriangle*t);
00207 
00208         void checkit(btHullTriangle *t);
00209 
00210         btHullTriangle* extrudable(btScalar epsilon);
00211 
00212         int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
00213 
00214         int calchullgen(btVector3 *verts,int verts_count, int vlimit);
00215 
00216         int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow);
00217 
00218         class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
00219 
00220         void extrude(class btHullTriangle* t0,int v);
00221 
00222         ConvexH* test_cube();
00223 
00224         //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. 
00225         //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
00226         //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
00227         //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
00228         void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
00229 
00230         bool CleanupVertices(unsigned int svcount,
00231                              const btVector3* svertices,
00232                              unsigned int stride,
00233                              unsigned int &vcount, // output number of vertices
00234                              btVector3* vertices, // location to store the results.
00235                              btScalar  normalepsilon,
00236                              btVector3& scale);
00237 };
00238 
00239 
00240 #endif //BT_CD_HULL_H
00241 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


bullet
Author(s): Erwin Coumans, ROS package maintained by Tully Foote
autogenerated on Wed Oct 31 2012 07:54:31