Permalink
Switch branches/tags
Nothing to show
Find file
Fetching contributors…
Cannot retrieve contributors at this time
373 lines (286 sloc) 8.7 KB
/****
antix.h
Clone this package from git://github.com/rtv/Antix.git
version 2
Richard Vaughan
****/
#include <vector>
#include <set>
#include <list>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#define GRAPHICS 1
#define DEBUGVIS 0
// 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 Antix
{
/** 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 ); }
// bounds type - specifies a range of values
typedef struct
{
double min, max;
} bounds_t;
// bounding box type - specifies a 2d range of values
typedef struct
{
bounds_t x, y;
} bbox_t;
class Home;
class Puck
{
public:
bool held; // true iff carried by a robot
Home* home;
unsigned int index; // the matrix cell that currently contains this puck
uint64_t delivery_time;
double x,y; // location
/** constructor places a puck at specified pose */
Puck( double x, double y );
~Puck();
void Replace();
//Puck()
//: held(false), index(0), lifetime(10), x(0.0), y(0.0)//, count(1)
// { /* do nothing */ }
void Pickup();
void Drop();
};
class Home
{
public:
unsigned int id;
class Color
{
public:
double r, g, b;
Color( double r, double g, double b ) : r(r), g(g), b(b) {}
// get a random color
static Color Random()
{
return Color( drand48(), drand48(), drand48() );
}
} color;
std::list<Puck*> pucks;
unsigned int score;
double x, y, r;
Home( unsigned int id, const Color& color, double x, double y, double r );
void UpdatePucks();
};
class Robot
{
public:
/** initialization: call this before using any other calls. */
static void Init( int argc, char** argv );
static void UpdateGui();
/** 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 );
/** Wrap distances around the torus */
static double WrapDistance( double d );
/** Start running the simulation. Does not return. */
static void Run();
static bool paused; // runs only when this is false
static bool show_data; // controls visualization of pixel data
static double fov; // sensor detects objects within this angular field-of-view about the current heading
static double pickup_range;
static double radius; // radius of all robot's bodies
static double range; // sensor detects objects up tp this maximum distance
static double worldsize; // side length of the toroidal world
static std::vector<Home*> homes;
static std::vector<Robot*> population;
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 home_count; // number of home zones
static unsigned int home_population; // number of robots
static unsigned int puck_count; // number of pucks that exist in the world
static unsigned int sleep_msec; // number of milliseconds to sleep at each update
static unsigned int gui_interval; // number of milliseconds between window redraws
static Robot* first;
class MatrixCell
{
public:
std::vector<Robot*> robots;
std::vector<Puck*> pucks;
};
static std::vector<Robot::MatrixCell> matrix;
static unsigned int matrixwidth;
void TestPucksInCell( const MatrixCell& cell );
void TestRobotsInCell( const MatrixCell& cell );
unsigned int index; // the matrix cell that currently holds this robot
#if GRAPHICS
static int winsize; // initial size of the window in pixels
/** initialization: call this before using any other calls. */
static void InitGraphics( int argc, char* argv[] );
/** render all robots in OpenGL */
static void DrawAll();
// render the robot in OpenGL
void Draw();
#endif
bbox_t sensor_bbox;
void FovBBox( bbox_t& box );
// deliver pucks to this location
Home* home;
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) {}
//Pose( const Pose &p ) : x(p.x), y(p.y), a(p.a) {}
// get a random pose
static Pose Random()
{
return Pose( drand48() * Robot::worldsize,
drand48() * Robot::worldsize,
Robot::AngleNormalize( drand48() * (M_PI*2.0)));
}
} pose; // instance: robot is located at this pose
class Speed
{
public:
double v; // forward speed
double w; // turn speed
// constructor sets speeds to zero
Speed() : v(0.0), w(0.0) {}
} speed; // instance: robot is moving this fast
class SeeRobot
{
public:
const Home* home;
Pose pose;
Speed speed;
double range;
double bearing;
bool haspuck;
SeeRobot( const Home* home, const Pose& p, const Speed& s, const double range, const double bearing, const bool haspuck )
: home(home), pose(p), speed(s), range(range), bearing(bearing), haspuck(haspuck)
{ /* empty */}
};
/** A sense vector containing information about all the robots
detected in my field of view */
std::vector<SeeRobot> see_robots;
static inline unsigned int Cell( double x )
{
const double d = Robot::worldsize / (double)Robot::matrixwidth;
while( x > worldsize ) // wraparound
x -= worldsize;
while( x < 0 ) // wraparound
x += worldsize;
return floor( x / d );
}
static inline int CellNoWrap( double x )
{
const double d = Robot::worldsize / (double)Robot::matrixwidth;
return floor( x / d );
}
static inline unsigned int CellWrap( int x )
{
while( x >= (int)matrixwidth ) // wraparound
x -= matrixwidth;
while( x < 0 ) // wraparound
x += matrixwidth;
return x;
}
static inline unsigned int Cell( double x, double y )
{
return (Cell(x) + (Cell(y) * Robot::matrixwidth) );
}
static std::vector<Puck> pucks;
class SeePuck
{
public:
Puck* puck;
bool held;
double bearing;
double range;
SeePuck( Puck* puck, const double range, const double bearing, const bool held )
: puck(puck), held(held), bearing(bearing), range(range)
{ /* empty */}
};
/** A sense vector containing information about all the pucks
detected in my field of view */
std::vector<SeePuck> see_pucks;
#if DEBUGVIS
std::vector<Robot*> neighbors;
std::vector<Puck*> neighbor_pucks;
std::set<unsigned int> neighbor_cells;
#endif
// constructor
Robot( Home* home, const Pose& pose );
// destructor
virtual ~Robot() {}
/** Attempt to pick up a puck. Returns true if one was picked up,
else false. */
bool Pickup();
/** Attempt to drop a puck. Returns true if one was dropped, else
false. */
bool Drop();
/** Returns true if we are currently holding a puck. */
bool Holding() const;
/** pure virtual - subclasses must implement this method */
virtual void Controller() = 0;
private:
Puck* puck_held;
// move the robot
void UpdatePose();
// update
//void UpdateSensors();
public:
void UpdateRobotSensor();
void UpdatePuckSensor();
};
// fast approximation to atan2
inline double fast_atan2( double y, double x )
{
const double piD2( M_PI/2.0 );
double atan;
double z = y/x;
if ( x == 0.0 ){
if ( y > 0.0 ) return piD2;
if ( y == 0.0 ) return 0.0;
return -piD2;
}
if ( fabs( z ) < 1.0 ){
atan = z/(1.0 + 0.28*z*z);
if ( x < 0.0 ){
if ( y < 0.0 ) return atan - M_PI;
return atan + M_PI;
}
}
else{
atan = piD2 - z/(z*z + 0.28f);
if ( y < 0.0f ) return atan - M_PI;
}
return atan;
}
inline double fast_sin(double x)
{
const double B = 4/M_PI;
const double C = -4/(M_PI*M_PI);
const double P = 0.225;
const double y = B * x + C * x * fabs(x);
return( P * (y * fabs(y) - y) + y );
}
inline double fast_cos(double x)
{
const double B = 4/M_PI;
const double C = -4/(M_PI*M_PI);
const double P = 0.225;
x = x + M_PI/2;
if(x > M_PI){ // Original x > M_PI/2
x -= 2 * M_PI; // Wrap: cos(x) = cos(x - 2 M_PI)
}
double y = B * x + C * x * fabs(x); //fast, inprecise
return( P * (y * fabs(y) - y) + y );
}
}; // namespace Antix