00001 #include <iostream>
00002 #include <math.h>
00003
00004 #include "vuConvexHull.h"
00005
00006 void vuConvexHull::setPoints(int npoints, float* plist)
00007 {
00008 for(int i=0;i<npoints;i++)
00009 {
00010 CHPoint p(i,*(plist++),*(plist++));
00011 m_PList.add(p);
00012 }
00013 m_CHCalculated = false;
00014 }
00015
00016 void vuConvexHull::setPoints(const vuDVector<CHPoint>& plist)
00017 {
00018 int npoints = (int)plist.getLength();
00019 for(int i=0;i<npoints;i++)
00020 {
00021 m_PList.add(plist[i]);
00022 }
00023 m_CHCalculated = false;
00024 }
00025
00026 void vuConvexHull::addPoint(const CHPoint& p)
00027 {
00028 bool isset = false;
00029 int index = p.index;
00030 for(int i=0;i<(int)m_PList.getLength();i++)
00031 if(m_PList[i].index==index)
00032 {
00033 m_PList[i] = p;
00034 isset = true;
00035 break;
00036 }
00037 if(!isset) m_PList.add(p);
00038 m_CHCalculated = false;
00039 }
00040
00041 void vuConvexHull::addPoint(float x, float y)
00042 {
00043 m_PList.add(CHPoint(getNPoints(), x, y));
00044 m_CHCalculated = false;
00045 }
00046
00047 void vuConvexHull::addPoint(int index, float x, float y)
00048 {
00049 bool isset = false;
00050 for(int i=0;i<(int)m_PList.getLength();i++)
00051 if(m_PList[i].index==index)
00052 {
00053 m_PList[i] = CHPoint(index, x, y);
00054 isset = true;
00055 break;
00056 }
00057 if(!isset) m_PList.add(CHPoint(index, x, y));
00058 m_CHCalculated = false;
00059 }
00060
00061 void vuConvexHull::clearPoints()
00062 {
00063 m_PList.removeRange(0,m_PList.getLength());
00064 }
00065
00066 bool vuConvexHull::getCHull(vuDVector<int>& indices)
00067 {
00068 if(!m_CHCalculated) calcConvexHull();
00069
00070 int lenHull = (int)m_CHull.getLength();
00071 indices.removeAll();
00072
00073 for(int i=0;i<lenHull;i++)
00074 indices.add(m_CHull[i].index);
00075
00076 return true;
00077 }
00078
00079 bool vuConvexHull::getCHull(int & lenHull, int *indices)
00080 {
00081 if((int)m_CHull.getLength() > lenHull) return false;
00082
00083 if(!m_CHCalculated) calcConvexHull();
00084
00085 lenHull = (int)m_CHull.getLength();
00086
00087 for(int i=0;i<lenHull;i++)
00088 *(indices++) = m_CHull[i].index;
00089
00090 return true;
00091 }
00092
00093 void vuConvexHull::sortPList()
00094 {
00095 bool sorted = false;
00096 for(int n=getNPoints()-1; n>0 && !sorted; n--)
00097 {
00098 sorted = true;
00099 for(int i=0;i<n;i++)
00100 {
00101 if(m_PList[i].x > m_PList[i+1].x)
00102 {
00103 CHPoint t = m_PList[i];
00104 m_PList[i] = m_PList[i+1];
00105 m_PList[i+1] = t;
00106 sorted = false;
00107 }
00108 }
00109 }
00110 }
00111
00112 void vuConvexHull::sweepLine()
00113 {
00114 m_UHull.removeAll();
00115 m_LHull.removeAll();
00116 m_UHull.add(m_PList[0]);
00117 m_LHull.add(m_PList[0]);
00118 for(int i=1;i<getNPoints();i++)
00119 {
00120 while(m_UHull.getLength()>1 &&
00121 knickTest(m_UHull[m_UHull.getLength()-2],
00122 m_UHull[m_UHull.getLength()-1],m_PList[i]) > 0.0)
00123 m_UHull.remove(m_UHull.getLength()-1);
00124
00125 while(m_LHull.getLength()>1 &&
00126 knickTest(m_LHull[m_LHull.getLength()-2],
00127 m_LHull[m_LHull.getLength()-1],m_PList[i]) < 0.0)
00128 m_LHull.remove(m_LHull.getLength()-1);
00129 m_UHull.add(m_PList[i]);
00130 m_LHull.add(m_PList[i]);
00131 }
00132 }
00133
00134 float vuConvexHull::knickTest(const CHPoint & b, const CHPoint & q, const CHPoint & r)
00135 {
00136 return (q.x-b.x)*(r.y-b.y) - (q.y-b.y)*(r.x-b.x);
00137 }
00138
00139 void vuConvexHull::mergeULHulls()
00140 {
00141 m_CHull.removeAll();
00142 int i;
00143 for(i=0;i<(int)m_LHull.getLength();i++)
00144 m_CHull.add(m_LHull[i]);
00145 for(i=m_UHull.getLength()-2;i>0;i--)
00146 m_CHull.add(m_UHull[i]);
00147 }
00148
00149 void vuConvexHull::calcConvexHull()
00150 {
00151 sortPList();
00152 sweepLine();
00153 mergeULHulls();
00154 m_CHCalculated = true;
00155 }
00156
00157 int vuConvexHull::angleThreshold(float angle_th)
00158 {
00159 int nrem=1;
00160 if(!m_CHCalculated) calcConvexHull();
00161 if(m_CHull.getLength()<4) return 0;
00162
00163 float cosa = cos(angle_th*M_PI/180);
00164 vuVector last;
00165 m_CHull[m_CHull.getLength()-1].toVector(last);
00166 for(int i=0;i<(int)m_CHull.getLength();i++)
00167 {
00168 vuVector next;
00169 if (i==(int)m_CHull.getLength()-1) m_CHull[0].toVector(next);
00170 else m_CHull[i+1].toVector(next);
00171 vuVector c;
00172 m_CHull[i].toVector(c);
00173 vuVector a = c;
00174 a-=last;
00175 vuVector b = next;
00176 b-= c;
00177 if (a.normalize() == 0.0 ||
00178 b.normalize() == 0.0)
00179 {
00180 last = c;
00181 } else {
00182
00183 if (a.dot(b)>cosa) {
00184 m_CHull.remove(i--);
00185 nrem++;
00186 } else last = c;
00187 }
00188 }
00189 return nrem;
00190 }
00191