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