btGrahamScan2dConvexHull.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2011 Advanced Micro Devices, Inc.  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H
00018 #define GRAHAM_SCAN_2D_CONVEX_HULL_H
00019 
00020 
00021 #include "btVector3.h"
00022 #include "btAlignedObjectArray.h"
00023 
00024 struct GrahamVector3 : public btVector3
00025 {
00026         GrahamVector3(const btVector3& org, int orgIndex)
00027                 :btVector3(org),
00028                         m_orgIndex(orgIndex)
00029         {
00030         }
00031         btScalar        m_angle;
00032         int m_orgIndex;
00033 };
00034 
00035 
00036 struct btAngleCompareFunc {
00037         btVector3 m_anchor;
00038         btAngleCompareFunc(const btVector3& anchor)
00039         : m_anchor(anchor) 
00040         {
00041         }
00042         bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
00043                 if (a.m_angle != b.m_angle)
00044                         return a.m_angle < b.m_angle;
00045                 else
00046                 {
00047                         btScalar al = (a-m_anchor).length2();
00048                         btScalar bl = (b-m_anchor).length2();
00049                         if (al != bl)
00050                                 return  al < bl;
00051                         else
00052                         {
00053                                 return a.m_orgIndex < b.m_orgIndex;
00054                         }
00055                 }
00056         }
00057 };
00058 
00059 inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
00060 {
00061         btVector3 axis0,axis1;
00062         btPlaneSpace1(normalAxis,axis0,axis1);
00063         
00064 
00065         if (originalPoints.size()<=1)
00066         {
00067                 for (int i=0;i<originalPoints.size();i++)
00068                         hull.push_back(originalPoints[0]);
00069                 return;
00070         }
00071         //step1 : find anchor point with smallest projection on axis0 and move it to first location
00072         for (int i=0;i<originalPoints.size();i++)
00073         {
00074 //              const btVector3& left = originalPoints[i];
00075 //              const btVector3& right = originalPoints[0];
00076                 btScalar projL = originalPoints[i].dot(axis0);
00077                 btScalar projR = originalPoints[0].dot(axis0);
00078                 if (projL < projR)
00079                 {
00080                         originalPoints.swap(0,i);
00081                 }
00082         }
00083 
00084         //also precompute angles
00085         originalPoints[0].m_angle = -1e30f;
00086         for (int i=1;i<originalPoints.size();i++)
00087         {
00088                 btVector3 xvec = axis0;
00089                 btVector3 ar = originalPoints[i]-originalPoints[0];
00090                 originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
00091         }
00092 
00093         //step 2: sort all points, based on 'angle' with this anchor
00094         btAngleCompareFunc comp(originalPoints[0]);
00095         originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
00096 
00097         int i;
00098         for (i = 0; i<2; i++) 
00099                 hull.push_back(originalPoints[i]);
00100 
00101         //step 3: keep all 'convex' points and discard concave points (using back tracking)
00102         for (; i != originalPoints.size(); i++) 
00103         {
00104                 bool isConvex = false;
00105                 while (!isConvex&& hull.size()>1) {
00106                         btVector3& a = hull[hull.size()-2];
00107                         btVector3& b = hull[hull.size()-1];
00108                         isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
00109                         if (!isConvex)
00110                                 hull.pop_back();
00111                         else 
00112                                 hull.push_back(originalPoints[i]);
00113                 }
00114         }
00115 }
00116 
00117 #endif //GRAHAM_SCAN_2D_CONVEX_HULL_H