Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

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