VCG Library
space_index_2d.cpp
1 /****************************************************************************
2 * VCGLib o o *
3 * Visual and Computer Graphics Library o o *
4 * _ O _ *
5 * Copyright(C) 2004-2009 \/)\/ *
6 * Visual Computing Lab /\/| *
7 * ISTI - Italian National Research Council | *
8 * \ *
9 * All rights reserved. *
10 * *
11 * This program is free software; you can redistribute it and/or modify *
12 * it under the terms of the GNU General Public License as published by *
13 * the Free Software Foundation; either version 2 of the License, or *
14 * (at your option) any later version. *
15 * *
16 * This program is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
19 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
20 * for more details. *
21 * *
22 ****************************************************************************/
23 #include <stdio.h>
24 #include <time.h>
25 #include <vcg/space/distance2.h>
26 #include <vcg/space/segment2.h>
27 #include <vcg/space/index/grid_static_ptr2d.h>
28 #include <vcg/space/index/grid_closest2d.h>
29 #include <vcg/space/intersection2.h>
30 
31 typedef double MyScalarType;
32 typedef vcg::Point2<MyScalarType> MyCoordType;
33 typedef vcg::Ray2<MyScalarType> MyRayType;
34 
35 //**BASIC SEGMENT CLASS
36 class MySegmentType:public vcg::Segment2<MyScalarType>
37 {
38 public:
39  int mark;
40  bool IsD(){return false;}
41 
42  MySegmentType(const vcg::Point2<MyScalarType> &_P0,
43  const vcg::Point2<MyScalarType> &_P1)
44  {
45  P0()=_P0;
46  P1()=_P1;
47  mark=0;
48  }
49 
50  int &TMark(){return mark;}
51 
52  MySegmentType(){}
53 
54  MySegmentType(const MySegmentType &s1):vcg::Segment2<MyScalarType>(s1)
55  {
56  P0()=s1.P0();
57  P1()=s1.P1();
58  mark=s1.mark;
59  }
60 };
61 
62 
63 //**ALLOCATED SEGMENTS**//
64 std::vector<MySegmentType> AllocatedSeg;
65 
66 //**GENERATION OF RANDOM SEGMENTS
67 
68 vcg::Point2<MyScalarType> RandomPoint(MyScalarType SpaceSize=100)
69 {
70  int dimension=RAND_MAX;
71  int X=rand();
72  int Y=rand();
73  vcg::Point2<MyScalarType> P0=vcg::Point2<MyScalarType>((MyScalarType)X/dimension,(MyScalarType)Y/dimension);
74  P0*=SpaceSize;
75  return P0;
76 }
77 
78 
79 void RandomSeg(vcg::Point2<MyScalarType> &P0,
80  vcg::Point2<MyScalarType> &P1,
81  MyScalarType SpaceSize=100,
82  MyScalarType maxdim=1)
83 {
84 
85  P0=RandomPoint(SpaceSize);
86  vcg::Point2<MyScalarType> D=RandomPoint(SpaceSize);
87  D.Normalize();
88  D*=maxdim;
89  P1=P0+D;
90 }
91 
92 void InitRandom(int num,
93  MyScalarType SpaceSize=100,
94  MyScalarType maxdim=1)
95 {
96  AllocatedSeg.clear();
97  AllocatedSeg.resize(num);
98  srand(clock());
99  for (int i=0;i<num;i++)
100  {
101  vcg::Point2<MyScalarType> P0,P1;
102  RandomSeg(P0,P1,SpaceSize,maxdim);
103  AllocatedSeg[i]=MySegmentType(P0,P1);
104  //AllocatedSeg[i].deleted=false;
105  }
106 
107 }
108 
109 
110 //**MARKER CLASSES**//
111 class MyMarker
112 {
113 
114 public:
115  int mark;
116 
117  MyMarker(){mark=0;}
118 
119  void UnMarkAll(){mark++;}
120 
121  bool IsMarked(MySegmentType* obj)
122  {
123  int markObj=obj->TMark();
124  return(markObj==mark);
125  }
126 
127  void Mark(MySegmentType* obj)
128  {obj->TMark()=mark;}
129 
130 };
131 
132 //**GRID-RELATED STUFF**//
133 MyMarker mf;
134 vcg::GridStaticPtr2D<MySegmentType,MyScalarType> Grid2D;
135 
136 //**QUERIES
137 MySegmentType * GetClosestSegment(MyCoordType & _p,
138  MyCoordType &_closestPt)
139 {
140  vcg::PointSegment2DEPFunctor<MyScalarType> PDistFunct;
141 
142  MyScalarType _minDist;
143  MyScalarType _maxDist=std::numeric_limits<MyScalarType>::max();
144  return (Grid2D.GetClosest(PDistFunct,mf,_p,_maxDist,_minDist,_closestPt));
145 }
146 
147 void GetInBoxSegments(vcg::Box2<MyScalarType> bbox,std::vector<MySegmentType*> &result)
148 {
149  Grid2D.GetInBox(mf,bbox,result);
150 }
151 
152 MySegmentType * DoRay(MyRayType & _r,
153  MyCoordType &_closestPt)
154 {
155  MyRayType _ray1=_r;
156  _ray1.Normalize();
157  typedef vcg::RaySegmentIntersectionFunctor SintFunct;
158  SintFunct rs;
159  MyScalarType _maxDist=std::numeric_limits<MyScalarType>::max();
160  MyScalarType _t;
161  MySegmentType *seg=Grid2D.DoRay(rs,mf,_ray1,_maxDist,_t);
162 
163  if (seg==NULL)return NULL;
164  _closestPt=_ray1.Origin()+_ray1.Direction()*_t;
165  return seg;
166 }
167 
168 //**BRUTE FORCE QUERIES
169 void GetInBoxSegmentsBruteF( vcg::Box2<MyScalarType> bbox,
170  std::vector<MySegmentType*> &result)
171 {
172  vcg::Box2<MyScalarType> ibbox;
173  for (size_t i=0;i<AllocatedSeg.size();i++)
174  {
175  AllocatedSeg[i].GetBBox(ibbox);
176  if (!ibbox.Collide(bbox)) continue;
177  result.push_back(&AllocatedSeg[i]);
178  }
179 }
180 
181 MySegmentType* GetClosesestSegmentBruteF(MyCoordType & _p,
182  MyCoordType &_closestPt)
183 {
184  MyScalarType _minDist=std::numeric_limits<MyScalarType>::max();
185  MySegmentType *ret=NULL;
186  for (size_t i=0;i<AllocatedSeg.size();i++)
187  {
188  vcg::Point2<MyScalarType> test;
189  test=vcg::ClosestPoint(AllocatedSeg[i],_p);
190  MyScalarType currD=(test-_p).Norm();
191  if (currD<_minDist)
192  {
193  _closestPt=test;
194  _minDist=currD;
195  ret=&AllocatedSeg[i];
196  }
197  }
198  return ret;
199 }
200 
201 MySegmentType * DoRayBruteF(MyRayType & _r,
202  MyCoordType &_closestPt)
203 {
204  MyScalarType _minDist=std::numeric_limits<MyScalarType>::max();
205  MySegmentType *ret=NULL;
206  for (size_t i=0;i<AllocatedSeg.size();i++)
207  {
208  vcg::Point2<MyScalarType> test;
209  bool inters=vcg::RaySegmentIntersection(_r,AllocatedSeg[i],test);
210  if (!inters)continue;
211  MyScalarType currD=(test-_r.Origin()).Norm();
212  if (currD<_minDist)
213  {
214  _closestPt=test;
215  _minDist=currD;
216  ret=&AllocatedSeg[i];
217  }
218  }
219  return ret;
220 }
221 
222 
223 void TestBox(int num_test=100000,
224  MyScalarType SpaceSize=100)
225 {
226  int numWrong=0;
227 
228  for (int i=0;i<num_test;i++)
229  {
230  vcg::Point2<MyScalarType> P0=RandomPoint(SpaceSize);
231  vcg::Point2<MyScalarType> P1=RandomPoint(SpaceSize);
232  vcg::Box2<MyScalarType> bbox;
233  bbox.Add(P0);
234  bbox.Add(P1);
235  std::vector<MySegmentType*> result0;
236  GetInBoxSegments(bbox,result0);
237 
238  std::vector<MySegmentType*> result1;
239  GetInBoxSegmentsBruteF(bbox,result1);
240 
241  std::sort(result0.begin(),result0.end());
242  std::sort(result1.begin(),result1.end());
243 
244  std::vector<MySegmentType*>::iterator new_end=std::unique(result1.begin(),result1.end());
245  int dist=distance(result1.begin(),new_end);
246  result1.resize(dist);
247 
248  if (result0.size()!=result1.size())numWrong++;
249 
250  for (size_t j = 0; j < result0.size(); j++)
251  if (result0[j] != result1[j])
252  {
253  numWrong++;
254  }
255  }
256  printf("WRONG TESTS BBOX %d ON %d \n",numWrong,num_test);
257  fflush(stdout);
258 }
259 
260 void TestClosest(int num_test=100000,
261  MyScalarType SpaceSize=100)
262 {
263 
264  int numWrong=0;
265  for (int i=0;i<num_test;i++)
266  {
267  vcg::Point2<MyScalarType> P0=RandomPoint(SpaceSize);
268 
269  vcg::Point2<MyScalarType> closest0;
270  MySegmentType* result0=GetClosestSegment(P0,closest0);
271 
272  vcg::Point2<MyScalarType> closest1;
273  MySegmentType* result1=GetClosesestSegmentBruteF(P0,closest1);
274 
275 
276  if (result0!=result1)
277  {
278  numWrong++;
279  printf("D0 %5.5f \n",(closest0-P0).Norm());
280  printf("D1 %5.5f \n",(closest1-P0).Norm());
281  fflush(stdout);
282  }
283  }
284  printf("WRONG TESTS CLOSEST %d ON %d \n",numWrong,num_test);
285  fflush(stdout);
286 }
287 
288 void TestRay(int num_test=100000,
289  MyScalarType SpaceSize=100)
290 {
291  int numWrong=0;
292  int NUll0=0;
293  int NUll1=0;
294  for (int i=0;i<num_test;i++)
295  {
296  vcg::Point2<MyScalarType> P0=RandomPoint(SpaceSize);
297  vcg::Point2<MyScalarType> P1=RandomPoint(SpaceSize);
298  vcg::Point2<MyScalarType> Orig=P0;
299  vcg::Point2<MyScalarType> Dir=P1-P0;
300  Dir.Normalize();
301 
302  MyRayType r(Orig,Dir);
303 
304  vcg::Point2<MyScalarType> closest0;
305  MySegmentType* result0=DoRay(r,closest0);
306 
307  vcg::Point2<MyScalarType> closest1;
308  MySegmentType* result1=DoRayBruteF(r,closest1);
309 
310 
311  if (result0!=result1)
312  {
313  numWrong++;
314 // printf("D0 %5.5f \n",(closest0-P0).Norm());
315 // printf("D1 %5.5f \n",(closest1-P0).Norm());
316 // fflush(stdout);
317  }
318  if (result0==NULL) NUll0++;
319  if (result1==NULL) NUll1++;
320  }
321  printf("WRONG TESTS DORAY %d ON %d \n",numWrong,num_test);
322  printf("NULL0 %d \n",NUll0);
323  printf("NULL1 %d \n",NUll1);
324  fflush(stdout);
325 }
326 
327 int main( int argc, char **argv )
328 {
329  (void) argc;
330  (void) argv;
331  int num_sample=20000;
332  int t0=clock();
333 
334  printf("** Random Initialization ** \n");
335  fflush(stdout);
336  InitRandom(num_sample,100,0.3);
337  int t1=clock();
338 
340  printf("** Time elapsed for initialization of %d sample is %d\n \n",num_sample,t1-t0);
341  Grid2D.Set(AllocatedSeg.begin(),AllocatedSeg.end());
342  fflush(stdout);
343 
344  //Box Query correctness
345  TestBox(num_sample);
346  TestClosest(num_sample);
347  TestRay(num_sample);
348 
349  return 0;
350 }