Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 521 lines (435 sloc) 12.158 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.*/
9efe76f fixing some memory leaks
thjc authored
13 Block::Block( Model* mod,
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
14 const std::vector<point_t>& pts,
15 meters_t zmin,
16 meters_t zmax,
17 Color color,
18 bool inherit_color,
19 bool wheel ) :
89dcdaf major changes for 3.1
rtv authored
20 mod( mod ),
17f2908 cleaning up Block class
rtv authored
21 mpts(),
e8d54f8 replacing some C style arrays with vectors and strings, for consistency
Richard Vaughan authored
22 pts(pts),
ff51521 cleaning up and STLization of BlockGroup
rtv authored
23 local_z( zmin, zmax ),
89dcdaf major changes for 3.1
rtv authored
24 color( color ),
25 inherit_color( inherit_color ),
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
26 wheel(wheel),
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
27 rendered_cells(),
ff51521 cleaning up and STLization of BlockGroup
rtv authored
28 gpts()
89dcdaf major changes for 3.1
rtv authored
29 {
30 assert( mod );
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
31 canonicalize_winding(this->pts);
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
32 }
33
89dcdaf major changes for 3.1
rtv authored
34 /** A from-file constructor */
9efe76f fixing some memory leaks
thjc authored
35 Block::Block( Model* mod,
31e71c9 cleaning up
rtv authored
36 Worldfile* wf,
37 int entity)
89dcdaf major changes for 3.1
rtv authored
38 : mod( mod ),
1ce2551 cleaning up
rtv authored
39 mpts(),
17f2908 cleaning up Block class
rtv authored
40 pts(),
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
41 local_z(),
996c281 replaced some c-strings with std::strings
rtv authored
42 color(),
cfa70a3 experimenting with wavefront planner
rtv authored
43 inherit_color(true),
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
44 wheel(),
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
45 rendered_cells(),
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
46 gpts()
89dcdaf major changes for 3.1
rtv authored
47 {
48 assert(mod);
49 assert(wf);
50 assert(entity);
ff51521 cleaning up and STLization of BlockGroup
rtv authored
51
89dcdaf major changes for 3.1
rtv authored
52 Load( wf, entity );
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
53 canonicalize_winding(this->pts);
89dcdaf major changes for 3.1
rtv authored
54 }
55
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
56 Block::~Block()
9efe76f fixing some memory leaks
thjc authored
57 {
ac35755 @rtv double-buffered multithread working
authored
58 if( mapped )
59 {
60 UnMap(0);
61 UnMap(1);
62 }
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
63 }
64
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
65 void Block::Translate( double x, double y )
66 {
aa783ff block refactoring
rtv authored
67 FOR_EACH( it, pts )
cfa70a3 experimenting with wavefront planner
rtv authored
68 {
aa783ff block refactoring
rtv authored
69 it->x += x;
70 it->y += y;
cfa70a3 experimenting with wavefront planner
rtv authored
71 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
72
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
73 mod->blockgroup.BuildDisplayList( mod );
74 }
75
76 double Block::CenterY()
77 {
78 double min = billion;
79 double max = -billion;
ff51521 cleaning up and STLization of BlockGroup
rtv authored
80
aa783ff block refactoring
rtv authored
81 FOR_EACH( it, pts )
cfa70a3 experimenting with wavefront planner
rtv authored
82 {
aa783ff block refactoring
rtv authored
83 if( it->y > max ) max = it->y;
84 if( it->y < min ) min = it->y;
cfa70a3 experimenting with wavefront planner
rtv authored
85 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
86
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
87 // return the value half way between max and min
88 return( min + (max - min)/2.0 );
89 }
90
91 double Block::CenterX()
92 {
93 double min = billion;
94 double max = -billion;
ff51521 cleaning up and STLization of BlockGroup
rtv authored
95
aa783ff block refactoring
rtv authored
96 FOR_EACH( it, pts )
cfa70a3 experimenting with wavefront planner
rtv authored
97 {
aa783ff block refactoring
rtv authored
98 if( it->x > max ) max = it->x;
99 if( it->x < min ) min = it->x;
cfa70a3 experimenting with wavefront planner
rtv authored
100 }
9efe76f fixing some memory leaks
thjc authored
101
41a7d4a grippers partially implemented. they don't lift anything yet
rtv authored
102 // return the value half way between maxx and min
103 return( min + (max - min)/2.0 );
104 }
105
106 void Block::SetCenter( double x, double y )
107 {
108 // move the block by the distance required to bring its center to
109 // the requested position
110 Translate( x-CenterX(), y-CenterY() );
111 }
112
113 void Block::SetCenterY( double y )
114 {
115 // move the block by the distance required to bring its center to
116 // the requested position
117 Translate( 0, y-CenterY() );
118 }
119
120 void Block::SetCenterX( double x )
121 {
122 // move the block by the distance required to bring its center to
123 // the requested position
124 Translate( x-CenterX(), 0 );
125 }
126
127 void Block::SetZ( double min, double max )
128 {
129 local_z.min = min;
130 local_z.max = max;
131
132 // force redraw
133 mod->blockgroup.BuildDisplayList( mod );
134 }
135
c5247e1 fixed alwayson and model property bugs. Fixed stage.pc to include GL …
rtv authored
136 const Color& Block::GetColor()
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
137 {
89dcdaf major changes for 3.1
rtv authored
138 return( inherit_color ? mod->color : color );
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
139 }
140
ff51521 cleaning up and STLization of BlockGroup
rtv authored
141 void Block::AppendTouchingModels( ModelPtrSet& touchers )
ff0dbdd working on charging model and demo
rtv authored
142 {
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
143 unsigned int layer = mod->world->updates % 2;
144
ff0dbdd working on charging model and demo
rtv authored
145 // for every cell we are rendered into
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
146 FOR_EACH( cell_it, rendered_cells[layer] )
147 // for every block rendered into that cell
148 FOR_EACH( block_it, (*cell_it)->GetBlocks(layer) )
31e71c9 cleaning up
rtv authored
149 {
150 if( !mod->IsRelated( (*block_it)->mod ))
151 touchers.insert( (*block_it)->mod );
152 }
ff0dbdd working on charging model and demo
rtv authored
153 }
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
154
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
155 Model* Block::TestCollision()
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
156 {
89dcdaf major changes for 3.1
rtv authored
157 //printf( "model %s block %p test collision...\n", mod->Token(), this );
158
159 // 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
160 //GenerateCandidateCells();
ff51521 cleaning up and STLization of BlockGroup
rtv authored
161
ff0dbdd working on charging model and demo
rtv authored
162 if( mod->vis.obstacle_return )
0e620eb added collision with ground (z=0 plane) using patch #2929553 from RAZOR
rtv authored
163 {
164 if ( global_z.min < 0 )
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
165 return mod->world->GetGround();
0e620eb added collision with ground (z=0 plane) using patch #2929553 from RAZOR
rtv authored
166
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
167 unsigned int layer = mod->world->updates % 2;
168
cfa70a3 experimenting with wavefront planner
rtv authored
169 // for every cell we may be rendered into
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
170 FOR_EACH( cell_it, rendered_cells[layer] )
cfa70a3 experimenting with wavefront planner
rtv authored
171 {
712d746 fixed Z collision detection for blocks
rtv authored
172 // for every block rendered into that cell
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
173 FOR_EACH( block_it, (*cell_it)->GetBlocks(layer) )
712d746 fixed Z collision detection for blocks
rtv authored
174 {
175 Block* testblock = *block_it;
176 Model* testmod = testblock->mod;
177
178 //printf( " testing block %p of model %s\n", testblock, testmod->Token() );
179
180 // if the tested model is an obstacle and it's not attached to this model
181 if( (testmod != this->mod) &&
182 testmod->vis.obstacle_return &&
183 (!mod->IsRelated( testmod )) &&
184 // also must intersect in the Z range
185 testblock->global_z.min <= global_z.max &&
186 testblock->global_z.max >= global_z.min )
187 {
188 //puts( "HIT");
189 return testmod; // bail immediately with the bad news
190 }
191 }
192 }
0e620eb added collision with ground (z=0 plane) using patch #2929553 from RAZOR
rtv authored
193 }
712d746 fixed Z collision detection for blocks
rtv authored
194
89dcdaf major changes for 3.1
rtv authored
195 //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this );
196 return NULL; // no hit
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
197 }
198
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
199 void Block::Map( unsigned int layer )
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
200 {
65dcf8c @rtv working on threaded pose support
authored
201 // calculate the local coords of the block vertices
edbf437 @rtv small optimizations
authored
202 const size_t pt_count(pts.size());
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
203
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
204 if( mpts.size() == 0 )
205 {
206 // no valid cache of model coord points, so generate them
207 mpts.resize( pts.size() );
208
edbf437 @rtv small optimizations
authored
209 for( size_t i=0; i<pt_count; ++i )
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
210 mpts[i] = BlockPointToModelMeters( pts[i] );
211 }
212
213 // now calculate the global pixel coords of the block vertices
214 gpts.clear();
215 mod->LocalToPixels( mpts, gpts );
edbf437 @rtv small optimizations
authored
216
217 // and render this block's polygon into the world
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
218 mod->world->MapPoly( gpts, this, layer );
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
219
220 // update the block's absolute z bounds at this rendering
edbf437 @rtv small optimizations
authored
221 Pose gpose( mod->GetGlobalPose() );
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
222 gpose.z += mod->geom.pose.z;
edbf437 @rtv small optimizations
authored
223 double scalez( mod->geom.size.z / mod->blockgroup.GetSize().z );
55b9c56 @rtv simplified bitmap rendering (back to the old days) for speed and to a…
authored
224 meters_t z = gpose.z - mod->blockgroup.GetOffset().z;
225 global_z.min = (scalez * local_z.min) + z;
226 global_z.max = (scalez * local_z.max) + z;
227
228 mapped = true;
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
229 }
230
0a5ba1d @rtv optmized FOR_EACH macro to assume container is not modified. other sm…
authored
231 #include <algorithm>
232 #include <functional>
233
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
234 void Block::UnMap( unsigned int layer )
8aab383 moved bit to STL
rtv authored
235 {
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
236 // std::for_each( rendered_cells.begin(),
237 // rendered_cells.end(),
238 // std::bind2nd( std::mem_fun(&Cell::RemoveBlock), this));
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
239
240 FOR_EACH( it, rendered_cells[layer] )
047355a @rtv threaded double-buffer with raytracing and moving happening in differ…
authored
241 (*it)->RemoveBlock(this, layer );
6dd2b05 @rtv working on double-buffered bitmap for threaded move + raytrace - part…
authored
242
243 rendered_cells[layer].clear();
65dcf8c @rtv working on threaded pose support
authored
244 mapped = false;
89dcdaf major changes for 3.1
rtv authored
245 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
246
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
247 inline point_t Block::BlockPointToModelMeters( const point_t& bpt )
750e048 fixed raserizer scaling bug
rtv authored
248 {
249 Size bgsize = mod->blockgroup.GetSize();
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
250 point3_t bgoffset = mod->blockgroup.GetOffset();
9efe76f fixing some memory leaks
thjc authored
251
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
252 return point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x),
31e71c9 cleaning up
rtv authored
253 (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y));
750e048 fixed raserizer scaling bug
rtv authored
254 }
255
80cdf19 cleaning up code
rtv authored
256 void Block::InvalidateModelPointCache()
257 {
ff51521 cleaning up and STLization of BlockGroup
rtv authored
258 // this doesn't happen often, so this simple strategy isn't too wasteful
259 mpts.clear();
80cdf19 cleaning up code
rtv authored
260 }
eaba4cb geometry fixes & graphics tweaks
rtv authored
261
750e048 fixed raserizer scaling bug
rtv authored
262 void swap( int& a, int& b )
e96a997 added model rasterization
rtv authored
263 {
750e048 fixed raserizer scaling bug
rtv authored
264 int tmp = a;
265 a = b;
266 b = tmp;
e96a997 added model rasterization
rtv authored
267 }
268
9efe76f fixing some memory leaks
thjc authored
269 void Block::Rasterize( uint8_t* data,
31e71c9 cleaning up
rtv authored
270 unsigned int width,
271 unsigned int height,
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
272 meters_t cellwidth,
273 meters_t cellheight )
750e048 fixed raserizer scaling bug
rtv authored
274 {
275 //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n",
276 // this, width, height, scalex, scaley, offsetx, offsety );
e8d54f8 replacing some C style arrays with vectors and strings, for consistency
Richard Vaughan authored
277
edbf437 @rtv small optimizations
authored
278 const size_t pt_count = pts.size();
279 for( size_t i=0; i<pt_count; ++i )
750e048 fixed raserizer scaling bug
rtv authored
280 {
31e71c9 cleaning up
rtv authored
281 // convert points from local to model coords
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
282 point_t mpt1 = BlockPointToModelMeters( pts[i] );
283 point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
284
31e71c9 cleaning up
rtv authored
285 // record for debug visualization
286 mod->rastervis.AddPoint( mpt1.x, mpt1.y );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
287
31e71c9 cleaning up
rtv authored
288 // shift to the bottom left of the model
289 mpt1.x += mod->geom.size.x/2.0;
290 mpt1.y += mod->geom.size.y/2.0;
291 mpt2.x += mod->geom.size.x/2.0;
292 mpt2.y += mod->geom.size.y/2.0;
ff51521 cleaning up and STLization of BlockGroup
rtv authored
293
31e71c9 cleaning up
rtv authored
294 // convert from meters to cells
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
295 point_int_t a( floor( mpt1.x / cellwidth ),
31e71c9 cleaning up
rtv authored
296 floor( mpt1.y / cellheight ));
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
297 point_int_t b( floor( mpt2.x / cellwidth ),
31e71c9 cleaning up
rtv authored
298 floor( mpt2.y / cellheight ) );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
299
31e71c9 cleaning up
rtv authored
300 bool steep = abs( b.y-a.y ) > abs( b.x-a.x );
301 if( steep )
302 {
303 swap( a.x, a.y );
304 swap( b.x, b.y );
305 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
306
31e71c9 cleaning up
rtv authored
307 if( a.x > b.x )
308 {
309 swap( a.x, b.x );
310 swap( a.y, b.y );
311 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
312
31e71c9 cleaning up
rtv authored
313 double dydx = (double) (b.y - a.y) / (double) (b.x - a.x);
314 double y = a.y;
0a5ba1d @rtv optmized FOR_EACH macro to assume container is not modified. other sm…
authored
315 for(int x=a.x; x<=b.x; ++x)
31e71c9 cleaning up
rtv authored
316 {
317 if( steep )
318 {
319 if( ! (floor(y) >= 0) ) continue;
320 if( ! (floor(y) < (int)width) ) continue;
321 if( ! (x >= 0) ) continue;
322 if( ! (x < (int)height) ) continue;
323 }
324 else
325 {
326 if( ! (x >= 0) ) continue;
327 if( ! (x < (int)width) ) continue;
328 if( ! (floor(y) >= 0) ) continue;
329 if( ! (floor(y) < (int)height) ) continue;
330 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
331
31e71c9 cleaning up
rtv authored
332 if( steep )
333 data[ (int)floor(y) + (x * width)] = 1;
334 else
335 data[ x + ((int)floor(y) * width)] = 1;
336 y += dydx;
337 }
338 }
e96a997 added model rasterization
rtv authored
339 }
340
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
341 void Block::DrawTop()
89dcdaf major changes for 3.1
rtv authored
342 {
343 // draw the top of the block - a polygon at the highest vertical
344 // extent
345 glBegin( GL_POLYGON);
31e71c9 cleaning up
rtv authored
346 FOR_EACH( it, pts )
347 glVertex3f( it->x, it->y, local_z.max );
89dcdaf major changes for 3.1
rtv authored
348 glEnd();
9efe76f fixing some memory leaks
thjc authored
349 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
350
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
351 void Block::DrawSides()
89dcdaf major changes for 3.1
rtv authored
352 {
9efe76f fixing some memory leaks
thjc authored
353 // construct a strip that wraps around the polygon
89dcdaf major changes for 3.1
rtv authored
354 glBegin(GL_QUAD_STRIP);
fec2ad5 cleaning up
rtv authored
355
31e71c9 cleaning up
rtv authored
356 FOR_EACH( it, pts )
357 {
fec2ad5 cleaning up
rtv authored
358 glVertex3f( it->x, it->y, local_z.max );
359 glVertex3f( it->x, it->y, local_z.min );
31e71c9 cleaning up
rtv authored
360 }
89dcdaf major changes for 3.1
rtv authored
361 // close the strip
362 glVertex3f( pts[0].x, pts[0].y, local_z.max );
363 glVertex3f( pts[0].x, pts[0].y, local_z.min );
364 glEnd();
365 }
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
366
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
367 void Block::DrawFootPrint()
89dcdaf major changes for 3.1
rtv authored
368 {
fec2ad5 cleaning up
rtv authored
369 glBegin(GL_POLYGON);
31e71c9 cleaning up
rtv authored
370 FOR_EACH( it, pts )
371 glVertex2f( it->x, it->y );
89dcdaf major changes for 3.1
rtv authored
372 glEnd();
373 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
374
ac35755 @rtv double-buffered multithread working
authored
375 void Block::DrawSolid( bool topview )
89dcdaf major changes for 3.1
rtv authored
376 {
ac35755 @rtv double-buffered multithread working
authored
377 // if( wheel )x
378 // {
6ec6357 merged Apple/Linux conditional build from Rich via Jenny
Richard Vaughan authored
379 // glPushMatrix();
380
381 // glRotatef( 90,0,1,0 );
382 // glRotatef( 90,1,0,0 );
383
384 // glTranslatef( -local_z.max /2.0, 0, 0 );
385
386
387 // GLUquadric* quadric = gluNewQuadric();
388 // gluQuadricDrawStyle( quadric, GLU_FILL );
389 // gluCylinder( quadric, local_z.max, local_z.max, size.x, 16, 16 );
390 // gluDeleteQuadric( quadric );
391
392 // glPopMatrix();
393 // }
394 // else
ac35755 @rtv double-buffered multithread working
authored
395 {
396 if( ! topview )
397 DrawSides();
398
399 DrawTop();
400 }
89dcdaf major changes for 3.1
rtv authored
401 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
402
3680786 API changes! removed the Stg prefix from all class names, and renamed…
rtv authored
403 void Block::Load( Worldfile* wf, int entity )
89dcdaf major changes for 3.1
rtv authored
404 {
edbf437 @rtv small optimizations
authored
405 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
406
9efe76f fixing some memory leaks
thjc authored
407 char key[128];
edbf437 @rtv small optimizations
authored
408 for( size_t p=0; p<pt_count; ++p )
31e71c9 cleaning up
rtv authored
409 {
65dcf8c @rtv working on threaded pose support
authored
410 snprintf(key, sizeof(key), "point[%d]", (int)p );
31e71c9 cleaning up
rtv authored
411
e8d54f8 replacing some C style arrays with vectors and strings, for consistency
Richard Vaughan authored
412 pts.push_back( point_t( wf->ReadTupleLength(entity, key, 0, 0),
413 wf->ReadTupleLength(entity, key, 1, 0) ));
31e71c9 cleaning up
rtv authored
414 }
ff51521 cleaning up and STLization of BlockGroup
rtv authored
415
89dcdaf major changes for 3.1
rtv authored
416 local_z.min = wf->ReadTupleLength( entity, "z", 0, 0.0 );
417 local_z.max = wf->ReadTupleLength( entity, "z", 1, 1.0 );
ff51521 cleaning up and STLization of BlockGroup
rtv authored
418
6ec6357 merged Apple/Linux conditional build from Rich via Jenny
Richard Vaughan authored
419 const std::string& colorstr = wf->ReadString( entity, "color", "" );
420
996c281 replaced some c-strings with std::strings
rtv authored
421 if( colorstr != "" )
cfa70a3 experimenting with wavefront planner
rtv authored
422 {
4ccc5cd replaced stg_color_t with Color class and added static named construc…
rtv authored
423 color = Color( colorstr );
cfa70a3 experimenting with wavefront planner
rtv authored
424 inherit_color = false;
425 }
d60514d geometry & graphics fixes. added normalization of blocks to fit insid…
rtv authored
426 else
08e31b0 added option to prevent stacking of models, based on patch #2919630 f…
rtv authored
427 inherit_color = true;
6ec6357 merged Apple/Linux conditional build from Rich via Jenny
Richard Vaughan authored
428
429 wheel = wf->ReadInt( entity, "wheel", wheel );
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl)…
rtv authored
430 }
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
431
08e31b0 added option to prevent stacking of models, based on patch #2919630 f…
rtv authored
432 /////////////////////////////////////////////////////////////////////////////////////////
433 // utility functions to ensure block winding is consistent and matches OpenGL's default
434
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
435 static
436 /// util; puts angle into [0, 2pi)
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
437 void positivize(radians_t& angle)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
438 {
439 while (angle < 0) angle += 2 * M_PI;
440 }
441
442 static
443 /// util; puts angle into -pi/2, pi/2
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
444 void pi_ize(radians_t& angle)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
445 {
446 while (angle < -M_PI) angle += 2 * M_PI;
447 while (M_PI < angle) angle -= 2 * M_PI;
448 }
449
450 static
451 /// util; How much was v1 rotated to get to v2?
65dcf8c @rtv working on threaded pose support
authored
452 radians_t angle_change(point_t v1, point_t v2)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
453 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
454 radians_t a1 = atan2(v1.y, v1.x);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
455 positivize(a1);
456
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
457 radians_t a2 = atan2(v2.y, v2.x);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
458 positivize(a2);
459
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
460 radians_t angle_change = a2 - a1;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
461 pi_ize(angle_change);
462
463 return angle_change;
464 }
465
466 static
467 /// util; find vectors between adjacent points, pts[next] - pts[cur]
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
468 vector<point_t> find_vectors(vector<point_t> const& pts)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
469 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
470 vector<point_t> vs;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
471 assert(2 <= pts.size());
472 for (unsigned i = 0, n = pts.size(); i < n; ++i)
473 {
474 unsigned j = (i + 1) % n;
65dcf8c @rtv working on threaded pose support
authored
475 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
476 }
477 assert(vs.size() == pts.size());
478 return vs;
479 }
480
481 static
482 /// util; finds sum of angle changes, from each vertex to the
483 /// next one (in current ordering), wrapping around.
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
484 radians_t angles_sum(vector<point_t> const& vs)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
485 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
486 radians_t angle_sum = 0;
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
487 for (unsigned i = 0, n = vs.size(); i < n; ++i)
488 {
489 unsigned j = (i + 1) % n;
490 angle_sum += angle_change(vs[i], vs[j]);
491 }
492 return angle_sum;
493 }
494
495 static
496 /// Util
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
497 bool is_canonical_winding(vector<point_t> const& ps)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
498 {
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
499 // reuse point_t as vector
500 vector<point_t> vs = find_vectors(ps);
501 radians_t sum = angles_sum(vs);
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
502 bool bCanon = 0 < sum;
503
504 return bCanon;
505 }
506
507 static
508 /// util; sums angle changes to see whether it's 2pi or -2pi.
509 /// 2pi is counter-clockwise winding (which OpenGL requires),
510 /// -2pi is clockwise. Reverses <pts> when winding is clockwise.
511 // Note that a simple line that doubles back on itself has an
512 // angle sum of 0, but that's intrinsic to a line - its winding could
513 // be either way.
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
514 void canonicalize_winding(vector<point_t>& ps)
e5a483c removing unused code. applied polygon winding patch from Jeff Donner
rtv authored
515 {
516 if (not is_canonical_winding(ps))
517 {
518 std::reverse(ps.begin(), ps.end());
519 }
520 }
Something went wrong with that request. Please try again.