Skip to content
This repository
Newer
Older
100644 259 lines (204 sloc) 6.468 kb
2de05ab1 »
2009-05-18 very much faster raytracing, with one tiny and infrequent bug that ne…
1 // Author: Richard Vaughan
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
2
36bf60ee »
2008-04-14 removed gdk-pixbuf dependency (and thus GTK is gone)
3 #include <FL/Fl_Shared_Image.H>
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
4
ec56d041 »
2009-01-22 header clean up
5 #include "stage.hh"
08f0500c »
2008-07-07 added hooks to build config.h and find rgb.txt
6 #include "config.h" // results of cmake's system configuration tests
139ea99f »
2008-08-01 stage: major FileManager updates, rgb.txt is now searched for in STAG…
7 #include "file_manager.hh"
ec56d041 »
2009-01-22 header clean up
8 using namespace Stg;
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
9
10 static bool init_called = false;
11
98e61f19 »
2009-06-25 fixed std::remove bug in Linux
12 const char* Stg::Version()
13 {
14 return VERSION;
15 }
16
639ffa93 »
2010-01-19 added revision keyword props to all source files
17
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
18 void Stg::Init( int* argc, char** argv[] )
19 {
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
20 PRINT_DEBUG( "Stg::Init()" );
a0200b4e »
2009-07-15 added simple cmdline arg database. need to clean up startup to cope
21
22 // copy the command line args for controllers to inspect
23 World::args.clear();
24 for( int i=0; i<*argc; i++ )
c57874bd »
2011-10-02 cleaning up
25 World::args.push_back( (*argv)[i] );
22a1279c »
2008-06-10 fixed indentation issues (converted to tabs)
26
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
27 // seed the RNG
28 srand48( time(NULL) );
22a1279c »
2008-06-10 fixed indentation issues (converted to tabs)
29
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
30 if(!setlocale(LC_ALL,"POSIX"))
c57874bd »
2011-10-02 cleaning up
31 PRINT_WARN("Failed to setlocale(); config file may not be parse correctly\n" );
89dcdaf1 »
2008-11-15 major changes for 3.1
32
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
33 RegisterModels();
34
c57874bd »
2011-10-02 cleaning up
35 // ask FLTK to load support for various image formats
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
36 fl_register_images();
e8d54f87 »
2010-08-27 replacing some C style arrays with vectors and strings, for consistency
37
38 init_called = true;
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
39 }
40
41 bool Stg::InitDone()
42 {
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
43 return init_called;
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
44 }
45
649ba768 »
2010-06-15 O(n log n) fiducial finder
46 const Color Color::blue( 0,0,1 );
47 const Color Color::red( 1,0,0 );
48 const Color Color::green( 0,1,0 );
49 const Color Color::yellow( 1,1,0 );
50 const Color Color::magenta( 1,0,1 );
51 const Color Color::cyan( 0,1,1 );
52
53
36bf60ee »
2008-04-14 removed gdk-pixbuf dependency (and thus GTK is gone)
54 // set all the pixels in a rectangle
e75c767d »
2010-07-02 before major naming change
55 static inline void pb_set_rect( Fl_Shared_Image* pb,
c57874bd »
2011-10-02 cleaning up
56 const unsigned int x, const unsigned int y,
7fc43a8d »
2011-10-02 very marginal speed improvements in scanning images for rectangles on…
57 const unsigned int rwidth, const unsigned int rheight,
c57874bd »
2011-10-02 cleaning up
58 const uint8_t val )
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
59 {
e8d54f87 »
2010-08-27 replacing some C style arrays with vectors and strings, for consistency
60 const unsigned int bytes_per_sample = 1;
7fc43a8d »
2011-10-02 very marginal speed improvements in scanning images for rectangles on…
61 const unsigned int depth = pb->d();
62 const unsigned int width = pb->w();
63
64 for( unsigned int a = y; a < y+rheight; a++ )
65 {
66 // zeroing
67 //uint8_t* pix = pb_get_pixel( pb, x, a );
78812641 »
2011-11-04 fixed bug in bitmap scanning that was creating too many rectangles
68 uint8_t* pix = (uint8_t*)(pb->data()[0] + (a*width*depth) + x*depth);
7fc43a8d »
2011-10-02 very marginal speed improvements in scanning images for rectangles on…
69 memset( pix, val, rwidth * depth * bytes_per_sample );
70 }
22a1279c »
2008-06-10 fixed indentation issues (converted to tabs)
71 }
36bf60ee »
2008-04-14 removed gdk-pixbuf dependency (and thus GTK is gone)
72
7fc43a8d »
2011-10-02 very marginal speed improvements in scanning images for rectangles on…
73
74 static inline bool pixel_is_set( uint8_t* pixels,
75 const unsigned int width,
76 const unsigned int depth,
77 const unsigned int x,
78 const unsigned int y,
79 uint8_t threshold )
36bf60ee »
2008-04-14 removed gdk-pixbuf dependency (and thus GTK is gone)
80 {
7fc43a8d »
2011-10-02 very marginal speed improvements in scanning images for rectangles on…
81 return( (pixels + (y*width*depth) + x*depth)[0] > threshold );
36bf60ee »
2008-04-14 removed gdk-pixbuf dependency (and thus GTK is gone)
82 }
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
83
84
f0e1c49b »
2012-01-16 blocks are now contours of complex polygons, and are no longer limite…
85 double direction( double a )
c57874bd »
2011-10-02 cleaning up
86 {
f0e1c49b »
2012-01-16 blocks are now contours of complex polygons, and are no longer limite…
87 if( a == 0.0 )
88 return 0;
89 else
90 return sgn(a);
91 }
c57874bd »
2011-10-02 cleaning up
92
f0e1c49b »
2012-01-16 blocks are now contours of complex polygons, and are no longer limite…
93 int Stg::polys_from_image_file( const std::string& filename,
94 std::vector<std::vector<point_t> >& polys )
95 {
96 // TODO: make this a parameter
97 const int threshold = 127;
98
99 Fl_Shared_Image *img = Fl_Shared_Image::get(filename.c_str());
100 if( img == NULL )
101 {
102 std::cerr << "failed to open file: " << filename << std::endl;
c57874bd »
2011-10-02 cleaning up
103
f0e1c49b »
2012-01-16 blocks are now contours of complex polygons, and are no longer limite…
104 assert( img ); // easy access to this point in debugger
105 exit(-1);
106 }
107
108 //printf( "loaded image %s w %d h %d d %d count %d ld %d\n",
109 // filename, img->w(), img->h(), img->d(), img->count(), img->ld() );
110
111 const unsigned int width = img->w();
112 const unsigned height = img->h();
113 const unsigned int depth = img->d();
114 uint8_t* pixels = (uint8_t*)img->data()[0];
115
116 // a set of previously seen directed edges, The key is a 4-element vector
117 // [x1,y1,x2,y2].
118 std::set<std::vector<uint32_t> > edges;
119
120 for(unsigned int y = 0; y < height; y++)
121 {
122 for(unsigned int x = 0; x < width; x++)
123 {
124 // skip blank (white) pixels
125 if( pixel_is_set( pixels, width, depth, x, y, threshold) )
126 continue;
127
128 // generate the four directed edges for this pixel
129 std::vector<uint32_t> edge[4];
130
131 for( int i=0; i<4; i++ )
132 edge[i].resize(4);
133
134 edge[0][0] = x+0;
135 edge[0][1] = y+0;
136 edge[0][2] = x+1;
137 edge[0][3] = y+0;
138
139 edge[1][0] = x+1;
140 edge[1][1] = y+0;
141 edge[1][2] = x+1;
142 edge[1][3] = y+1;
143
144 edge[2][0] = x+1;
145 edge[2][1] = y+1;
146 edge[2][2] = x+0;
147 edge[2][3] = y+1;
148
149 edge[3][0] = x+0;
150 edge[3][1] = y+1;
151 edge[3][2] = x+0;
152 edge[3][3] = y+0;
153
154 // put them in the set of the inverse edge does not exist
155 for( int i=0; i<4; i++ )
156 {
157 // the same edge with the opposite direction
158 std::vector<uint32_t> inv(4);
159 inv[0] = edge[i][2];
160 inv[1] = edge[i][3];
161 inv[2] = edge[i][0];
162 inv[3] = edge[i][1];
163
164 std::set<std::vector<uint32_t> >::iterator it =
165 edges.find( inv );
166
167 if( it == edges.end() ) // inverse not found
168 edges.insert( edge[i] ); // add the new edge
169 else // inverse found! delete it
170 edges.erase( it );
171 }
172 }
173 }
174
175 std::multimap<point_t,point_t> mmap;
176
177 FOR_EACH( it, edges )
178 {
179 // fill a multimap with start-point / end-point pairs.
180 std::pair<point_t,point_t> p( point_t( (*it)[0], (*it)[1] ),
181 point_t( (*it)[2], (*it)[3] ));
182 mmap.insert( p );
183 }
184
185 for( std::multimap<point_t,point_t>::iterator seedit = mmap.begin();
186 seedit != mmap.end();
187 seedit = mmap.begin() )
c57874bd »
2011-10-02 cleaning up
188 {
f0e1c49b »
2012-01-16 blocks are now contours of complex polygons, and are no longer limite…
189 std::vector<point_t> poly;
190
191 while( seedit != mmap.end() )
192 {
193 // invert y axis and add the new point to the poly
194 point_t pt = seedit->first;
195 pt.y = -pt.y;
196
197 // can this vector simply extend the previous one?
198 size_t psize = poly.size();
199 if( psize > 2 ) // need at least two points already
200 {
201 // find the direction of the vector descrived by the two previous points
202 double ldx = direction( poly[psize-1].x - poly[psize-2].x );
203 double ldy = direction( poly[psize-1].y - poly[psize-2].y );
204
205 // find the direction of the vector described by the new point and the previous point
206 double ndx = direction( pt.x - poly[psize-1].x );
207 double ndy = direction( pt.y - poly[psize-1].y );
208
209 // if the direction is the same, we can replace the
210 // previous point with this one, rather than adding a
211 // new point
212 if( ldx == ndx && ldy == ndy )
213 poly[psize-1] = pt;
214 else
215 poly.push_back( pt );
216 }
217 else
218 poly.push_back( pt );
219
220 point_t next = seedit->second;
221 mmap.erase( seedit );
222
223 seedit = mmap.find( next );
224 }
225
226 polys.push_back( poly );
c57874bd »
2011-10-02 cleaning up
227 }
f0e1c49b »
2012-01-16 blocks are now contours of complex polygons, and are no longer limite…
228
229 if( img ) img->release(); // frees all resources for this image
230 return 0; // ok
c57874bd »
2011-10-02 cleaning up
231 }
232
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
233 // POINTS -----------------------------------------------------------
234
e45b70c4 »
2010-07-20 combined laser and ranger functionality into ranger - still buggy
235 point_t* Stg::unit_square_points_create( void )
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
236 {
e45b70c4 »
2010-07-20 combined laser and ranger functionality into ranger - still buggy
237 point_t * pts = new point_t[4];
22a1279c »
2008-06-10 fixed indentation issues (converted to tabs)
238
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
239 pts[0].x = 0;
240 pts[0].y = 0;
241 pts[1].x = 1;
242 pts[1].y = 0;
243 pts[2].x = 1;
244 pts[2].y = 1;
245 pts[3].x = 0;
246 pts[3].y = 1;
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
247
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
248 return pts;
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
249 }
250
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
251 // return a value based on val, but limited minval <= val >= maxval
e75c767d »
2010-07-02 before major naming change
252 double Stg::constrain( double val, const double minval, const double maxval )
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
253 {
76d30e62 »
2011-11-03 cleaning up
254 if( val < minval ) return minval;
255 if( val > maxval ) return maxval;
4ccc5cd7 »
2009-07-03 replaced stg_color_t with Color class and added static named construc…
256 return val;
36bfbdb6 »
2008-01-15 first commit since shifting the Stage-3.0 development branch (opengl)…
257 }
258
Something went wrong with that request. Please try again.