Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 373 lines (286 sloc) 8.906 kB
ba8130c @rtv starting with rtv/universe code
authored
1 /****
3a7b28e @rtv cleaning up
authored
2 antix.h
3 Clone this package from git://github.com/rtv/Antix.git
ba8130c @rtv starting with rtv/universe code
authored
4 version 2
5 Richard Vaughan
6 ****/
7
8 #include <vector>
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
9 #include <set>
de79bdb @rtv working on sorted axes for speed
authored
10 #include <list>
ba8130c @rtv starting with rtv/universe code
authored
11 #include <math.h>
12 #include <stdio.h>
13 #include <stdlib.h>
b9dd760 @rtv grid based working fast
authored
14 #include <assert.h>
ba8130c @rtv starting with rtv/universe code
authored
15
16 #define GRAPHICS 1
44a5feb @rtv switched to testing squared distances for sensors, to avoid expensive…
authored
17 #define DEBUGVIS 0
ba8130c @rtv starting with rtv/universe code
authored
18
19 // handy STL iterator macro pair. Use FOR_EACH(I,C){ } to get an iterator I to
20 // each item in a collection C.
21 #define VAR(V,init) __typeof(init) V=(init)
22 #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
23
3a7b28e @rtv cleaning up
authored
24 namespace Antix
ba8130c @rtv starting with rtv/universe code
authored
25 {
26 /** Convert radians to degrees. */
27 inline double rtod( double r ){ return( r * 180.0 / M_PI ); }
28 /** Convert degrees to radians */
29 inline double dtor( double d){ return( d * M_PI / 180.0 ); }
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
30
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
31 // bounds type - specifies a range of values
32 typedef struct
33 {
34 double min, max;
35 } bounds_t;
36
37 // bounding box type - specifies a 2d range of values
38 typedef struct
39 {
40 bounds_t x, y;
41 } bbox_t;
42
43 class Home;
44
45 class Puck
46 {
47 public:
48 bool held; // true iff carried by a robot
49 Home* home;
50 unsigned int index; // the matrix cell that currently contains this puck
15f4469 @rtv added puck scoring
authored
51 uint64_t delivery_time;
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
52 double x,y; // location
53
54 /** constructor places a puck at specified pose */
55 Puck( double x, double y );
56 ~Puck();
57
58 void Replace();
59
60 //Puck()
61 //: held(false), index(0), lifetime(10), x(0.0), y(0.0)//, count(1)
62 // { /* do nothing */ }
63
64 void Pickup();
65 void Drop();
66 };
67
68 class Home
69 {
70 public:
15f4469 @rtv added puck scoring
authored
71 unsigned int id;
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
72
73 class Color
74 {
75 public:
76 double r, g, b;
77
78 Color( double r, double g, double b ) : r(r), g(g), b(b) {}
79
80 // get a random color
81 static Color Random()
82 {
83 return Color( drand48(), drand48(), drand48() );
84 }
85
86 } color;
15f4469 @rtv added puck scoring
authored
87
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
88
15f4469 @rtv added puck scoring
authored
89 std::list<Puck*> pucks;
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
90
91 unsigned int score;
92
93 double x, y, r;
94
15f4469 @rtv added puck scoring
authored
95 Home( unsigned int id, const Color& color, double x, double y, double r );
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
96
97 void UpdatePucks();
98 };
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
99
100 class Robot
101 {
102 public:
c11fc4c @rtv splitting up files for clarity
authored
103
ba8130c @rtv starting with rtv/universe code
authored
104 /** initialization: call this before using any other calls. */
105 static void Init( int argc, char** argv );
106
c11fc4c @rtv splitting up files for clarity
authored
107 static void UpdateGui();
108
ba8130c @rtv starting with rtv/universe code
authored
109 /** update all robots */
110 static void UpdateAll();
111
112 /** Normalize a length to within 0 to worldsize. */
113 static double DistanceNormalize( double d );
114
115 /** Normalize an angle to within +/_ M_PI. */
116 static double AngleNormalize( double a );
117
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
118 /** Wrap distances around the torus */
119 static double WrapDistance( double d );
120
ba8130c @rtv starting with rtv/universe code
authored
121 /** Start running the simulation. Does not return. */
122 static void Run();
123
124 static bool paused; // runs only when this is false
125 static bool show_data; // controls visualization of pixel data
c11fc4c @rtv splitting up files for clarity
authored
126 static double fov; // sensor detects objects within this angular field-of-view about the current heading
127 static double pickup_range;
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
128 static double radius; // radius of all robot's bodies
c11fc4c @rtv splitting up files for clarity
authored
129 static double range; // sensor detects objects up tp this maximum distance
130 static double worldsize; // side length of the toroidal world
de79bdb @rtv working on sorted axes for speed
authored
131
132
4208c10 @rtv working. late.
authored
133 static std::vector<Home*> homes;
c11fc4c @rtv splitting up files for clarity
authored
134 static std::vector<Robot*> population;
de79bdb @rtv working on sorted axes for speed
authored
135
c11fc4c @rtv splitting up files for clarity
authored
136 static uint64_t updates; // number of simulation steps so far
137 static uint64_t updates_max; // number of simulation steps to run before quitting (0 means infinity)
138 static unsigned int home_count; // number of home zones
139 static unsigned int home_population; // number of robots
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
140 static unsigned int puck_count; // number of pucks that exist in the world
c11fc4c @rtv splitting up files for clarity
authored
141 static unsigned int sleep_msec; // number of milliseconds to sleep at each update
ba8130c @rtv starting with rtv/universe code
authored
142
b9dd760 @rtv grid based working fast
authored
143 static unsigned int gui_interval; // number of milliseconds between window redraws
f607370 @rtv naive sorted axis version
authored
144 static Robot* first;
b9dd760 @rtv grid based working fast
authored
145
146
147 class MatrixCell
148 {
149 public:
df8be35 @rtv switched from sets to vectors to store contents of matrix cells. Iter…
authored
150 std::vector<Robot*> robots;
151 std::vector<Puck*> pucks;
b9dd760 @rtv grid based working fast
authored
152 };
f607370 @rtv naive sorted axis version
authored
153
b9dd760 @rtv grid based working fast
authored
154 static std::vector<Robot::MatrixCell> matrix;
155 static unsigned int matrixwidth;
156
157 void TestPucksInCell( const MatrixCell& cell );
158 void TestRobotsInCell( const MatrixCell& cell );
7695d17 @rtv working biut slow axis method
authored
159
160 unsigned int index; // the matrix cell that currently holds this robot
161
ba8130c @rtv starting with rtv/universe code
authored
162 #if GRAPHICS
c11fc4c @rtv splitting up files for clarity
authored
163 static int winsize; // initial size of the window in pixels
164
165 /** initialization: call this before using any other calls. */
166 static void InitGraphics( int argc, char* argv[] );
167
ba8130c @rtv starting with rtv/universe code
authored
168 /** render all robots in OpenGL */
169 static void DrawAll();
c11fc4c @rtv splitting up files for clarity
authored
170
171 // render the robot in OpenGL
172 void Draw();
ba8130c @rtv starting with rtv/universe code
authored
173 #endif
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
174
175 bbox_t sensor_bbox;
176 void FovBBox( bbox_t& box );
177
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
178 // deliver pucks to this location
179 Home* home;
ba8130c @rtv starting with rtv/universe code
authored
180
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
181 class Pose
182 {
183 public:
184 double x,y,a; // 2d position and orientation
185
186 Pose( double x, double y, double a ) : x(x), y(y), a(a) {}
187 Pose() : x(0.0), y(0.0), a(0.0) {}
f607370 @rtv naive sorted axis version
authored
188
189 //Pose( const Pose &p ) : x(p.x), y(p.y), a(p.a) {}
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
190
191 // get a random pose
192 static Pose Random()
193 {
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
194 return Pose( drand48() * Robot::worldsize,
195 drand48() * Robot::worldsize,
196 Robot::AngleNormalize( drand48() * (M_PI*2.0)));
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
197 }
198 } pose; // instance: robot is located at this pose
199
200 class Speed
201 {
202 public:
203 double v; // forward speed
204 double w; // turn speed
205
206 // constructor sets speeds to zero
207 Speed() : v(0.0), w(0.0) {}
208 } speed; // instance: robot is moving this fast
4208c10 @rtv working. late.
authored
209
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
210 class SeeRobot
211 {
212 public:
4208c10 @rtv working. late.
authored
213 const Home* home;
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
214 Pose pose;
215 Speed speed;
216 double range;
217 double bearing;
218 bool haspuck;
4208c10 @rtv working. late.
authored
219
220 SeeRobot( const Home* home, const Pose& p, const Speed& s, const double range, const double bearing, const bool haspuck )
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
221 : home(home), pose(p), speed(s), range(range), bearing(bearing), haspuck(haspuck)
4208c10 @rtv working. late.
authored
222 { /* empty */}
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
223 };
b9dd760 @rtv grid based working fast
authored
224
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
225
226 /** A sense vector containing information about all the robots
227 detected in my field of view */
228 std::vector<SeeRobot> see_robots;
b9dd760 @rtv grid based working fast
authored
229
230 static inline unsigned int Cell( double x )
231 {
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
232 const double d = Robot::worldsize / (double)Robot::matrixwidth;
233
234 while( x > worldsize ) // wraparound
235 x -= worldsize;
236
237 while( x < 0 ) // wraparound
238 x += worldsize;
239
240 return floor( x / d );
241 }
242
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
243 static inline int CellNoWrap( double x )
244 {
245 const double d = Robot::worldsize / (double)Robot::matrixwidth;
246 return floor( x / d );
247 }
248
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
249 static inline unsigned int CellWrap( int x )
250 {
251 while( x >= (int)matrixwidth ) // wraparound
252 x -= matrixwidth;
253
254 while( x < 0 ) // wraparound
255 x += matrixwidth;
256
257 return x;
b9dd760 @rtv grid based working fast
authored
258 }
259
260 static inline unsigned int Cell( double x, double y )
261 {
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
262 return (Cell(x) + (Cell(y) * Robot::matrixwidth) );
263 }
dd458d6 @rtv cleanups
authored
264
cad273a @rtv faster opengl using glDrawArrays - bug with pucks at boundaries not b…
authored
265 static std::vector<Puck> pucks;
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
266
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
267 class SeePuck
268 {
269 public:
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
270 Puck* puck;
4208c10 @rtv working. late.
authored
271 bool held;
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
272 double bearing;
273 double range;
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
274
4208c10 @rtv working. late.
authored
275 SeePuck( Puck* puck, const double range, const double bearing, const bool held )
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
276 : puck(puck), held(held), bearing(bearing), range(range)
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
277 { /* empty */}
278 };
279
280 /** A sense vector containing information about all the pucks
281 detected in my field of view */
b9dd760 @rtv grid based working fast
authored
282 std::vector<SeePuck> see_pucks;
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
283 #if DEBUGVIS
7695d17 @rtv working biut slow axis method
authored
284 std::vector<Robot*> neighbors;
b9dd760 @rtv grid based working fast
authored
285 std::vector<Puck*> neighbor_pucks;
286 std::set<unsigned int> neighbor_cells;
861499a @rtv fixed bugs and fixed matrix cell size to be the robot sensor range
authored
287 #endif
f607370 @rtv naive sorted axis version
authored
288
4208c10 @rtv working. late.
authored
289 // constructor
16911af @rtv pucks and homes added, pixels removed and replaced by idealized vecto…
authored
290 Robot( Home* home, const Pose& pose );
ba8130c @rtv starting with rtv/universe code
authored
291
4208c10 @rtv working. late.
authored
292 // destructor
293 virtual ~Robot() {}
294
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
295 /** Attempt to pick up a puck. Returns true if one was picked up,
4208c10 @rtv working. late.
authored
296 else false. */
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
297 bool Pickup();
298
299 /** Attempt to drop a puck. Returns true if one was dropped, else
4208c10 @rtv working. late.
authored
300 false. */
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
301 bool Drop();
4208c10 @rtv working. late.
authored
302
c5c6daa @rtv foraging working - pucks not checked for dual carrying
authored
303 /** Returns true if we are currently holding a puck. */
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
304 bool Holding() const;
c11fc4c @rtv splitting up files for clarity
authored
305
4208c10 @rtv working. late.
authored
306 /** pure virtual - subclasses must implement this method */
ba8130c @rtv starting with rtv/universe code
authored
307 virtual void Controller() = 0;
4208c10 @rtv working. late.
authored
308
309 private:
310 Puck* puck_held;
ba8130c @rtv starting with rtv/universe code
authored
311
312 // move the robot
313 void UpdatePose();
4208c10 @rtv working. late.
authored
314
ba8130c @rtv starting with rtv/universe code
authored
315 // update
f837047 @rtv run puck and robots sensors in separate threads
authored
316 //void UpdateSensors();
317 public:
318 void UpdateRobotSensor();
319 void UpdatePuckSensor();
ba8130c @rtv starting with rtv/universe code
authored
320 };
09b5b13 @rtv more conservative cell inspection - 30% speed improvement when using …
authored
321
322 // fast approximation to atan2
323 inline double fast_atan2( double y, double x )
324 {
325 const double piD2( M_PI/2.0 );
326 double atan;
327 double z = y/x;
328
329 if ( x == 0.0 ){
330 if ( y > 0.0 ) return piD2;
331 if ( y == 0.0 ) return 0.0;
332 return -piD2;
333 }
334
335 if ( fabs( z ) < 1.0 ){
336 atan = z/(1.0 + 0.28*z*z);
337 if ( x < 0.0 ){
338 if ( y < 0.0 ) return atan - M_PI;
339 return atan + M_PI;
340 }
341 }
342 else{
343 atan = piD2 - z/(z*z + 0.28f);
344 if ( y < 0.0f ) return atan - M_PI;
345 }
346 return atan;
347 }
348
349 inline double fast_sin(double x)
350 {
351 const double B = 4/M_PI;
352 const double C = -4/(M_PI*M_PI);
353 const double P = 0.225;
354 const double y = B * x + C * x * fabs(x);
355 return( P * (y * fabs(y) - y) + y );
356 }
357
358 inline double fast_cos(double x)
359 {
360 const double B = 4/M_PI;
361 const double C = -4/(M_PI*M_PI);
362 const double P = 0.225;
363
364 x = x + M_PI/2;
365 if(x > M_PI){ // Original x > M_PI/2
366 x -= 2 * M_PI; // Wrap: cos(x) = cos(x - 2 M_PI)
367 }
368
369 double y = B * x + C * x * fabs(x); //fast, inprecise
370 return( P * (y * fabs(y) - y) + y );
371 }
3a7b28e @rtv cleaning up
authored
372 }; // namespace Antix
Something went wrong with that request. Please try again.