forked from rtv/universe
-
Notifications
You must be signed in to change notification settings - Fork 1
/
universe.h
140 lines (109 loc) · 3.88 KB
/
universe.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
/****
universe.h
Clone this package from git://github.com/rtv/universe.git
version 2
Richard Vaughan
****/
#include <vector>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <elf.h>
#include <unistd.h>
#define GRAPHICS 1
// handy STL iterator macro pair. Use FOR_EACH(I,C){ } to get an iterator I to
// each item in a collection C.
#define VAR(V,init) __typeof(init) V=(init)
#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
namespace Uni
{
/** Convert radians to degrees. */
inline double rtod( double r ){ return( r * 180.0 / M_PI ); }
/** Convert degrees to radians */
inline double dtor( double d){ return( d * M_PI / 180.0 ); }
class Robot
{
public:
class Pose
{
public:
double x,y,a; // 2d position and orientation
Pose( double x, double y, double a ) : x(x), y(y), a(a) {}
Pose() : x(0.0), y(0.0), a(0.0) {}
// get a random pose
static Pose Random()
{
return Pose( drand48() * Robot::worldsize,
drand48() * Robot::worldsize,
Robot::AngleNormalize( drand48() * (M_PI*2.0)));
} // end Random()
}; // end class Pose
class Speed
{
public:
double v; // forward speed
double w; // turn speed
// constructor sets speeds to zero
Speed() : v(0.0), w(0.0) {}
}; // end class Speed
class Color
{
public:
double r, g, b;
Color( double r, double g, double b ) : r(r), g(g), b(b) {}
}; // end class Color
// each robot has a vector of these to store its observations
class Pixel
{
public:
double range;
Robot* robot;
Pixel() : range(0.0), robot(NULL) {}
}; // end class Pixel
// STATIC DATA AND METHODS ------------------------------------------
/** initialization: call this before using any other calls. */
static void Init( int argc, char** argv );
/** update all robots */
static void UpdateAll();
/** Normalize a length to within 0 to worldsize. */
static double DistanceNormalize( double d );
/** Normalize an angle to within +/_ M_PI. */
static double AngleNormalize( double a );
/** Start running the simulation. Does not return. */
static void Run();
static uint64_t updates; // number of simulation steps so far
static uint64_t updates_max; // number of simulation steps to run before quitting (0 means infinity)
static unsigned int sleep_msec; // number of milliseconds to sleep at each update
static double worldsize; // side length of the toroidal world
static double range; // sensor detects objects up tp this maximum distance
static double fov; // sensor detects objects within this angular field-of-view about the current heading
static unsigned int pixel_count; // number of pixels in sensor
static std::vector<Robot*> population;
static unsigned int population_size; // number of robots
static bool paused; // runs only when this is false
static bool show_data; // controls visualization of pixel data
static int winsize; // initial size of the window in pixels
static int displaylist; // robot body macro
#if GRAPHICS
/** render all robots in OpenGL */
static void DrawAll();
#endif
// NON-STATIC DATA AND METHODS ------------------------------------------
Pose pose; // robot is located at this pose
Speed speed; // robot is moving this fast
Color color; // robot's body has this color
std::vector<Pixel> pixels; // robot's sensor data vector
// create a new robot with these parameters
Robot( const Pose& pose, const Color& color );
virtual ~Robot() {}
// pure virtual - subclasses must implement this method
virtual void Controller() = 0;
// render the robot in OpenGL
void Draw();
// move the robot
void UpdatePose();
// update
void UpdatePixels();
}; // end class Robot
}; // namespace Uni