Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

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) to...
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 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
13 Block::Block( BlockGroup* group,
14 const std::vector<point_t>& pts,
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
15 const Bounds& zrange ) :
9e1e848 Richard Vaughan 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 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
18 local_z( zrange ),
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
19 global_z(),
c57874b Richard Vaughan cleaning up
authored
20 rendered_cells()
89dcdaf major changes for 3.1
rtv authored
21 {
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
22 assert( group );
1e01c9d Richard Vaughan cleaning up API
authored
23 //canonicalize_winding(this->pts);
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
24 }
25
89dcdaf major changes for 3.1
rtv authored
26 /** A from-file constructor */
9e1e848 Richard Vaughan 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 Richard Vaughan minor clean ups and speed ups
authored
32 local_z(),
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
33 global_z(),
c57874b Richard Vaughan cleaning up
authored
34 rendered_cells()
89dcdaf major changes for 3.1
rtv authored
35 {
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
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 so...
rtv authored
43 Block::~Block()
9efe76f fixing some memory leaks
thjc authored
44 {
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
45 UnMap(0);
46 UnMap(1);
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
47 }
48
9e1e848 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan 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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan 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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
132 unsigned int layer = group->mod.world->updates % 2;
6dd2b05 Richard Vaughan working on double-buffered bitmap for threaded move + raytrace - partial...
authored
133
ff0dbdd working on charging model and demo
rtv authored
134 // for every cell we are rendered into
6dd2b05 Richard Vaughan working on double-buffered bitmap for threaded move + raytrace - partial...
authored
135 FOR_EACH( cell_it, rendered_cells[layer] )
6b5f2c7 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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) to...
rtv authored
143
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
144 Model* Block::TestCollision()
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
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 Richard Vaughan simplified bitmap rendering (back to the old days) for speed and to allo...
authored
149 //GenerateCandidateCells();
ff51521 cleaning up and STLization of BlockGroup
rtv authored
150
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
151 if( group->mod.vis.obstacle_return )
6b5f2c7 Richard Vaughan minor clean ups and speed ups
authored
152 {
153 if ( global_z.min < 0 )
9e1e848 Richard Vaughan 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 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
156 unsigned int layer = group->mod.world->updates % 2;
6b5f2c7 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan minor clean ups and speed ups
authored
169 // if the tested model is an obstacle and it's not attached to this model
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
170 if( (testmod != &group->mod) &&
6b5f2c7 Richard Vaughan minor clean ups and speed ups
authored
171 testmod->vis.obstacle_return &&
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
172 (!group->mod.IsRelated( testmod )) &&
6b5f2c7 Richard Vaughan 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 Richard Vaughan 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) to...
rtv authored
186 }
187
047355a Richard Vaughan threaded double-buffer with raytracing and moving happening in different...
authored
188 void Block::Map( unsigned int layer )
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
189 {
e008aea Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
authored
190 // calculate the global pixel coords of the block vertices
6b5f2c7 Richard Vaughan minor clean ups and speed ups
authored
191 // and render this block's polygon into the world
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
192 group->mod.world->MapPoly( group->mod.LocalToPixels( pts ), this, layer );
e008aea Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
authored
193
55b9c56 Richard Vaughan simplified bitmap rendering (back to the old days) for speed and to allo...
authored
194 // update the block's absolute z bounds at this rendering
9e1e848 Richard Vaughan 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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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) to...
rtv authored
199 }
200
0a5ba1d Richard Vaughan optmized FOR_EACH macro to assume container is not modified. other small...
authored
201
047355a Richard Vaughan threaded double-buffer with raytracing and moving happening in different...
authored
202 void Block::UnMap( unsigned int layer )
8aab383 moved bit to STL
rtv authored
203 {
6dd2b05 Richard Vaughan working on double-buffered bitmap for threaded move + raytrace - partial...
authored
204 FOR_EACH( it, rendered_cells[layer] )
6b5f2c7 Richard Vaughan minor clean ups and speed ups
authored
205 (*it)->RemoveBlock(this, layer );
6dd2b05 Richard Vaughan working on double-buffered bitmap for threaded move + raytrace - partial...
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 Richard Vaughan 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 Richard Vaughan minor clean ups and speed ups
authored
226 const size_t pt_count = pts.size();
edbf437 Richard Vaughan small optimizations
authored
227 for( size_t i=0; i<pt_count; ++i )
750e048 fixed raserizer scaling bug
rtv authored
228 {
6b5f2c7 Richard Vaughan minor clean ups and speed ups
authored
229 // convert points from local to model coords
e008aea Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 Richard Vaughan minor clean ups and speed ups
authored
233 // record for debug visualization
9e1e848 Richard Vaughan 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 Richard Vaughan minor clean ups and speed ups
authored
236 // shift to the bottom left of the model
9e1e848 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
authored
248 // render a line in the output bitmap for this edge, from mpt1
249 // to mpt2
6b5f2c7 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
291
292
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
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 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
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 Richard Vaughan 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 so...
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 Richard Vaughan 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 Richard Vaughan 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) to...
rtv authored
319
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
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 Richard Vaughan 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 Richard Vaughan double-buffered multithread working
authored
328 void Block::DrawSolid( bool topview )
89dcdaf major changes for 3.1
rtv authored
329 {
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
330 DrawSides();
e008aea Richard Vaughan cleaned up block and blockgroup implementation. Blocks are now scaled to...
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 so...
rtv authored
334 void Block::Load( Worldfile* wf, int entity )
89dcdaf major changes for 3.1
rtv authored
335 {
6b5f2c7 Richard Vaughan 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 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
authored
338 char key[256];
edbf437 Richard Vaughan small optimizations
authored
339 for( size_t p=0; p<pt_count; ++p )
6b5f2c7 Richard Vaughan minor clean ups and speed ups
authored
340 {
341 snprintf(key, sizeof(key), "point[%d]", (int)p );
4ba9941 Richard Vaughan 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 Richard Vaughan minor clean ups and speed ups
authored
346 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
347
9e1e848 Richard Vaughan refactored blocks and blockgroups to simplify code and API
authored
348 canonicalize_winding(pts);
4ba9941 Richard Vaughan improved worldfile interface
authored
349
f0e1c49 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
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) to...
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 from...
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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan 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 Richard Vaughan blocks are now contours of complex polygons, and are no longer limited t...
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 Richard Vaughan 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 Richard Vaughan 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.