Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 437 lines (365 sloc) 10.475 kB
ec56d04 header clean up
rtv authored
1 #include "region.hh"
2 #include "worldfile.hh"
ec21888 removed world size limitation
rtv authored
3
ec56d04 header clean up
rtv authored
4 using namespace Stg;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
5 using std::vector;
6
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
7 static void canonicalize_winding(vector<point_t>& pts);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
8
ec21888 removed world size limitation
rtv authored
9
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
10 /** Create a new block. A model's body is a list of these
cfa70a3 experimenting with wavefront planner
rtv authored
11 blocks. The point data is copied, so pts can safely be freed
12 after calling this.*/
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
13 Block::Block( BlockGroup* group,
14 const std::vector<point_t>& pts,
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
15 const Bounds& zrange ) :
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
16 group(group),
e8d54f8 replacing some C style arrays with vectors and strings, for consistency
Richard Vaughan authored
17 pts(pts),
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
18 local_z( zrange ),
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
19 global_z(),
c57874b @rtv cleaning up
authored
20 rendered_cells()
89dcdaf major changes for 3.1
rtv authored
21 {
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
22 assert( group );
1e01c9d @rtv cleaning up API
authored
23 //canonicalize_winding(this->pts);
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
24 }
25
89dcdaf major changes for 3.1
rtv authored
26 /** A from-file constructor */
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
27 Block::Block( BlockGroup* group,
28 Worldfile* wf,
29 int entity)
30 : group(group),
17f2908 cleaning up Block class
rtv authored
31 pts(),
6b5f2c7 @rtv minor clean ups and speed ups
authored
32 local_z(),
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
33 global_z(),
c57874b @rtv cleaning up
authored
34 rendered_cells()
89dcdaf major changes for 3.1
rtv authored
35 {
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
36 assert(group);
89dcdaf major changes for 3.1
rtv authored
37 assert(wf);
38 assert(entity);
ff51521 cleaning up and STLization of BlockGroup
rtv authored
39
89dcdaf major changes for 3.1
rtv authored
40 Load( wf, entity );
41 }
42
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
43 Block::~Block()
9efe76f fixing some memory leaks
thjc authored
44 {
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
45 UnMap(0);
46 UnMap(1);
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
47 }
48
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
49
50
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
51 void Block::Translate( double x, double y )
52 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
53 FOR_EACH( it, pts )
cfa70a3 experimenting with wavefront planner
rtv authored
54 {
aa783ff block refactoring
rtv authored
55 it->x += x;
56 it->y += y;
cfa70a3 experimenting with wavefront planner
rtv authored
57 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
58
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
59 group->BuildDisplayList();
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
60 }
61
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
62 /** Return the value half way between the min and max Y position of
63 the polygon points
64 */
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
65 double Block::CenterY()
66 {
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
67 // assume the polygon fits in a square a billion m a side
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
68 double min = billion;
69 double max = -billion;
ff51521 cleaning up and STLization of BlockGroup
rtv authored
70
6b5f2c7 @rtv minor clean ups and speed ups
authored
71 FOR_EACH( it, pts )
cfa70a3 experimenting with wavefront planner
rtv authored
72 {
aa783ff block refactoring
rtv authored
73 if( it->y > max ) max = it->y;
74 if( it->y < min ) min = it->y;
cfa70a3 experimenting with wavefront planner
rtv authored
75 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
76
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
77 // return the value half way between max and min
78 return( min + (max - min)/2.0 );
79 }
80
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
81 /** Return the value half way between the min and max X position of
82 the polygon points
83 */
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
84 double Block::CenterX()
85 {
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
86 // assume the polygon fits in a square a billion m a side
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
87 double min = billion;
88 double max = -billion;
ff51521 cleaning up and STLization of BlockGroup
rtv authored
89
6b5f2c7 @rtv minor clean ups and speed ups
authored
90 FOR_EACH( it, pts )
cfa70a3 experimenting with wavefront planner
rtv authored
91 {
aa783ff block refactoring
rtv authored
92 if( it->x > max ) max = it->x;
93 if( it->x < min ) min = it->x;
cfa70a3 experimenting with wavefront planner
rtv authored
94 }
9efe76f fixing some memory leaks
thjc authored
95
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
96 // return the value half way between maxx and min
97 return( min + (max - min)/2.0 );
98 }
99
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
100 /** move the block by the distance required to bring its center to
101 the requested position */
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
102 void Block::SetCenter( double x, double y )
103 {
104 Translate( x-CenterX(), y-CenterY() );
105 }
106
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
107 /** move the block by the distance required to bring its center to
108 the requested position */
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
109 void Block::SetCenterY( double y )
110 {
111 Translate( 0, y-CenterY() );
112 }
113
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
114 /** move the block by the distance required to bring its center to
115 the requested position */
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
116 void Block::SetCenterX( double x )
117 {
118 Translate( x-CenterX(), 0 );
119 }
120
121 void Block::SetZ( double min, double max )
122 {
123 local_z.min = min;
124 local_z.max = max;
125
126 // force redraw
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
127 group->BuildDisplayList();
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
128 }
129
5408297 @rtv removed some poorly chosen container typedefs
authored
130 void Block::AppendTouchingModels( std::set<Model*>& touchers )
ff0dbdd working on charging model and demo
rtv authored
131 {
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
132 unsigned int layer = group->mod.world->updates % 2;
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
133
ff0dbdd working on charging model and demo
rtv authored
134 // for every cell we are rendered into
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
135 FOR_EACH( cell_it, rendered_cells[layer] )
6b5f2c7 @rtv minor clean ups and speed ups
authored
136 // for every block rendered into that cell
137 FOR_EACH( block_it, (*cell_it)->GetBlocks(layer) )
138 {
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
139 if( !group->mod.IsRelated( &(*block_it)->group->mod ))
140 touchers.insert( &(*block_it)->group->mod );
6b5f2c7 @rtv minor clean ups and speed ups
authored
141 }
ff0dbdd working on charging model and demo
rtv authored
142 }
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
143
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
144 Model* Block::TestCollision()
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
145 {
89dcdaf major changes for 3.1
rtv authored
146 //printf( "model %s block %p test collision...\n", mod->Token(), this );
147
148 // find the set of cells we would render into given the current global pose
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
149 //GenerateCandidateCells();
ff51521 cleaning up and STLization of BlockGroup
rtv authored
150
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
151 if( group->mod.vis.obstacle_return )
6b5f2c7 @rtv minor clean ups and speed ups
authored
152 {
153 if ( global_z.min < 0 )
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
154 return group->mod.world->GetGround();
0e620eb added collision with ground (z=0 plane) using patch #2929553 from RAZOR
rtv authored
155
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
156 unsigned int layer = group->mod.world->updates % 2;
6b5f2c7 @rtv minor clean ups and speed ups
authored
157
158 // for every cell we may be rendered into
159 FOR_EACH( cell_it, rendered_cells[layer] )
160 {
161 // for every block rendered into that cell
162 FOR_EACH( block_it, (*cell_it)->GetBlocks(layer) )
163 {
164 Block* testblock = *block_it;
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
165 Model* testmod = &testblock->group->mod;
712d746 fixed Z collision detection for blocks
rtv authored
166
6b5f2c7 @rtv minor clean ups and speed ups
authored
167 //printf( " testing block %p of model %s\n", testblock, testmod->Token() );
712d746 fixed Z collision detection for blocks
rtv authored
168
6b5f2c7 @rtv minor clean ups and speed ups
authored
169 // if the tested model is an obstacle and it's not attached to this model
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
170 if( (testmod != &group->mod) &&
6b5f2c7 @rtv minor clean ups and speed ups
authored
171 testmod->vis.obstacle_return &&
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
172 (!group->mod.IsRelated( testmod )) &&
6b5f2c7 @rtv minor clean ups and speed ups
authored
173 // also must intersect in the Z range
174 testblock->global_z.min <= global_z.max &&
175 testblock->global_z.max >= global_z.min )
176 {
177 //puts( "HIT");
178 return testmod; // bail immediately with the bad news
712d746 fixed Z collision detection for blocks
rtv authored
179 }
6b5f2c7 @rtv minor clean ups and speed ups
authored
180 }
181 }
182 }
712d746 fixed Z collision detection for blocks
rtv authored
183
89dcdaf major changes for 3.1
rtv authored
184 //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this );
185 return NULL; // no hit
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
186 }
187
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
188 void Block::Map( unsigned int layer )
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
189 {
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
190 // calculate the global pixel coords of the block vertices
6b5f2c7 @rtv minor clean ups and speed ups
authored
191 // and render this block's polygon into the world
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
192 group->mod.world->MapPoly( group->mod.LocalToPixels( pts ), this, layer );
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
193
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
194 // update the block's absolute z bounds at this rendering
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
195 Pose gpose( group->mod.GetGlobalPose() );
196 gpose.z += group->mod.geom.pose.z;
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
197 global_z.min = local_z.min + gpose.z;
198 global_z.max = local_z.max + gpose.z;
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
199 }
200
0a5ba1d @rtv optmized FOR_EACH macro to assume container is not modified. other sm…
authored
201
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
202 void Block::UnMap( unsigned int layer )
8aab383 moved bit to STL
rtv authored
203 {
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
204 FOR_EACH( it, rendered_cells[layer] )
6b5f2c7 @rtv minor clean ups and speed ups
authored
205 (*it)->RemoveBlock(this, layer );
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
206
207 rendered_cells[layer].clear();
89dcdaf major changes for 3.1
rtv authored
208 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
209
750e048 fixed raserizer scaling bug
rtv authored
210 void swap( int& a, int& b )
e96a997 added model rasterization
rtv authored
211 {
750e048 fixed raserizer scaling bug
rtv authored
212 int tmp = a;
213 a = b;
214 b = tmp;
e96a997 added model rasterization
rtv authored
215 }
216
9efe76f fixing some memory leaks
thjc authored
217 void Block::Rasterize( uint8_t* data,
6b5f2c7 @rtv minor clean ups and speed ups
authored
218 unsigned int width,
219 unsigned int height,
220 meters_t cellwidth,
221 meters_t cellheight )
750e048 fixed raserizer scaling bug
rtv authored
222 {
223 //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n",
224 // this, width, height, scalex, scaley, offsetx, offsety );
e8d54f8 replacing some C style arrays with vectors and strings, for consistency
Richard Vaughan authored
225
6b5f2c7 @rtv minor clean ups and speed ups
authored
226 const size_t pt_count = pts.size();
edbf437 @rtv small optimizations
authored
227 for( size_t i=0; i<pt_count; ++i )
750e048 fixed raserizer scaling bug
rtv authored
228 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
229 // convert points from local to model coords
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
230 point_t mpt1 = pts[i]; //BlockPointToModelMeters( pts[i] );
231 point_t mpt2 = pts[(i+1)%pt_count]; // BlockPointToModelMeters( pts[(i+1)%pt_count] );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
232
6b5f2c7 @rtv minor clean ups and speed ups
authored
233 // record for debug visualization
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
234 group->mod.rastervis.AddPoint( mpt1.x, mpt1.y );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
235
6b5f2c7 @rtv minor clean ups and speed ups
authored
236 // shift to the bottom left of the model
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
237 mpt1.x += group->mod.geom.size.x/2.0;
238 mpt1.y += group->mod.geom.size.y/2.0;
239 mpt2.x += group->mod.geom.size.x/2.0;
240 mpt2.y += group->mod.geom.size.y/2.0;
ff51521 cleaning up and STLization of BlockGroup
rtv authored
241
6b5f2c7 @rtv minor clean ups and speed ups
authored
242 // convert from meters to cells
243 point_int_t a( floor( mpt1.x / cellwidth ),
244 floor( mpt1.y / cellheight ));
245 point_int_t b( floor( mpt2.x / cellwidth ),
246 floor( mpt2.y / cellheight ) );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
247
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
248 // render a line in the output bitmap for this edge, from mpt1
249 // to mpt2
6b5f2c7 @rtv minor clean ups and speed ups
authored
250 bool steep = abs( b.y-a.y ) > abs( b.x-a.x );
251 if( steep )
252 {
253 swap( a.x, a.y );
254 swap( b.x, b.y );
255 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
256
6b5f2c7 @rtv minor clean ups and speed ups
authored
257 if( a.x > b.x )
258 {
259 swap( a.x, b.x );
260 swap( a.y, b.y );
261 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
262
6b5f2c7 @rtv minor clean ups and speed ups
authored
263 double dydx = (double) (b.y - a.y) / (double) (b.x - a.x);
264 double y = a.y;
265 for(int x=a.x; x<=b.x; ++x)
266 {
267 if( steep )
268 {
269 if( ! (floor(y) >= 0) ) continue;
270 if( ! (floor(y) < (int)width) ) continue;
271 if( ! (x >= 0) ) continue;
272 if( ! (x < (int)height) ) continue;
273 }
274 else
275 {
276 if( ! (x >= 0) ) continue;
277 if( ! (x < (int)width) ) continue;
278 if( ! (floor(y) >= 0) ) continue;
279 if( ! (floor(y) < (int)height) ) continue;
280 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
281
6b5f2c7 @rtv minor clean ups and speed ups
authored
282 if( steep )
283 data[ (int)floor(y) + (x * width)] = 1;
284 else
285 data[ x + ((int)floor(y) * width)] = 1;
286 y += dydx;
287 }
288 }
e96a997 added model rasterization
rtv authored
289 }
290
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
291
292
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
293 void Block::DrawTop()
89dcdaf major changes for 3.1
rtv authored
294 {
295 // draw the top of the block - a polygon at the highest vertical
296 // extent
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
297
89dcdaf major changes for 3.1
rtv authored
298 glBegin( GL_POLYGON);
31e71c9 cleaning up
rtv authored
299 FOR_EACH( it, pts )
6b5f2c7 @rtv minor clean ups and speed ups
authored
300 glVertex3f( it->x, it->y, local_z.max );
89dcdaf major changes for 3.1
rtv authored
301 glEnd();
9efe76f fixing some memory leaks
thjc authored
302 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
303
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
304 void Block::DrawSides()
89dcdaf major changes for 3.1
rtv authored
305 {
9efe76f fixing some memory leaks
thjc authored
306 // construct a strip that wraps around the polygon
89dcdaf major changes for 3.1
rtv authored
307 glBegin(GL_QUAD_STRIP);
fec2ad5 cleaning up
rtv authored
308
31e71c9 cleaning up
rtv authored
309 FOR_EACH( it, pts )
6b5f2c7 @rtv minor clean ups and speed ups
authored
310 {
fec2ad5 cleaning up
rtv authored
311 glVertex3f( it->x, it->y, local_z.max );
312 glVertex3f( it->x, it->y, local_z.min );
6b5f2c7 @rtv minor clean ups and speed ups
authored
313 }
89dcdaf major changes for 3.1
rtv authored
314 // close the strip
315 glVertex3f( pts[0].x, pts[0].y, local_z.max );
316 glVertex3f( pts[0].x, pts[0].y, local_z.min );
317 glEnd();
318 }
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
319
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
320 void Block::DrawFootPrint()
89dcdaf major changes for 3.1
rtv authored
321 {
fec2ad5 cleaning up
rtv authored
322 glBegin(GL_POLYGON);
31e71c9 cleaning up
rtv authored
323 FOR_EACH( it, pts )
6b5f2c7 @rtv minor clean ups and speed ups
authored
324 glVertex2f( it->x, it->y );
89dcdaf major changes for 3.1
rtv authored
325 glEnd();
326 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
327
ac35755 @rtv double-buffered multithread working
authored
328 void Block::DrawSolid( bool topview )
89dcdaf major changes for 3.1
rtv authored
329 {
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
330 DrawSides();
e008aea @rtv cleaned up block and blockgroup implementation. Blocks are now scaled…
authored
331 DrawTop();
89dcdaf major changes for 3.1
rtv authored
332 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
333
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
334 void Block::Load( Worldfile* wf, int entity )
89dcdaf major changes for 3.1
rtv authored
335 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
336 const size_t pt_count = wf->ReadInt( entity, "points", 0);
e8d54f8 replacing some C style arrays with vectors and strings, for consistency
Richard Vaughan authored
337
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
338 char key[256];
edbf437 @rtv small optimizations
authored
339 for( size_t p=0; p<pt_count; ++p )
6b5f2c7 @rtv minor clean ups and speed ups
authored
340 {
341 snprintf(key, sizeof(key), "point[%d]", (int)p );
4ba9941 @rtv improved worldfile interface
authored
342
343 point_t pt( 0, 0 );
344 wf->ReadTuple( entity, key, 0, 2, "ll", &pt.x, &pt.y );
345 pts.push_back( pt );
6b5f2c7 @rtv minor clean ups and speed ups
authored
346 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
347
9e1e848 @rtv refactored blocks and blockgroups to simplify code and API
authored
348 canonicalize_winding(pts);
4ba9941 @rtv improved worldfile interface
authored
349
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
350 wf->ReadTuple( entity, "z", 0, 2, "ll", &local_z.min, &local_z.max );
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
351 }
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
352
08e31b0 added option to prevent stacking of models, based on patch #2919630 f…
rtv authored
353 /////////////////////////////////////////////////////////////////////////////////////////
354 // utility functions to ensure block winding is consistent and matches OpenGL's default
355
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
356 static
357 /// util; puts angle into [0, 2pi)
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
358 void positivize(radians_t& angle)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
359 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
360 while (angle < 0) angle += 2 * M_PI;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
361 }
362
363 static
364 /// util; puts angle into -pi/2, pi/2
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
365 void pi_ize(radians_t& angle)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
366 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
367 while (angle < -M_PI) angle += 2 * M_PI;
368 while (M_PI < angle) angle -= 2 * M_PI;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
369 }
370
371 static
372 /// util; How much was v1 rotated to get to v2?
65dcf8c @rtv working on threaded pose support
authored
373 radians_t angle_change(point_t v1, point_t v2)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
374 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
375 radians_t a1 = atan2(v1.y, v1.x);
376 positivize(a1);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
377
6b5f2c7 @rtv minor clean ups and speed ups
authored
378 radians_t a2 = atan2(v2.y, v2.x);
379 positivize(a2);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
380
6b5f2c7 @rtv minor clean ups and speed ups
authored
381 radians_t angle_change = a2 - a1;
382 pi_ize(angle_change);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
383
6b5f2c7 @rtv minor clean ups and speed ups
authored
384 return angle_change;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
385 }
386
387 static
388 /// util; find vectors between adjacent points, pts[next] - pts[cur]
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
389 vector<point_t> find_vectors(vector<point_t> const& pts)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
390 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
391 vector<point_t> vs;
392 assert(2 <= pts.size());
393 for (unsigned i = 0, n = pts.size(); i < n; ++i)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
394 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
395 unsigned j = (i + 1) % n;
396 vs.push_back(point_t(pts[j].x - pts[i].x, pts[j].y - pts[i].y));
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
397 }
6b5f2c7 @rtv minor clean ups and speed ups
authored
398 assert(vs.size() == pts.size());
399 return vs;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
400 }
401
402 static
403 /// util; finds sum of angle changes, from each vertex to the
404 /// next one (in current ordering), wrapping around.
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
405 radians_t angles_sum(vector<point_t> const& vs)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
406 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
407 radians_t angle_sum = 0;
408 for (unsigned i = 0, n = vs.size(); i < n; ++i)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
409 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
410 unsigned j = (i + 1) % n;
411 angle_sum += angle_change(vs[i], vs[j]);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
412 }
6b5f2c7 @rtv minor clean ups and speed ups
authored
413 return angle_sum;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
414 }
415
416 static
417 /// Util
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
418 bool is_canonical_winding(vector<point_t> const& ps)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
419 {
f0e1c49 @rtv blocks are now contours of complex polygons, and are no longer limite…
authored
420 return( 0 < angles_sum( find_vectors(ps) ) );
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
421 }
422
423 static
424 /// util; sums angle changes to see whether it's 2pi or -2pi.
425 /// 2pi is counter-clockwise winding (which OpenGL requires),
426 /// -2pi is clockwise. Reverses <pts> when winding is clockwise.
427 // Note that a simple line that doubles back on itself has an
428 // angle sum of 0, but that's intrinsic to a line - its winding could
429 // be either way.
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
430 void canonicalize_winding(vector<point_t>& ps)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
431 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
432 if (not is_canonical_winding(ps))
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
433 {
6b5f2c7 @rtv minor clean ups and speed ups
authored
434 std::reverse(ps.begin(), ps.end());
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
435 }
436 }
Something went wrong with that request. Please try again.