Permalink
Switch branches/tags
Nothing to show
Find file
Fetching contributors…
Cannot retrieve contributors at this time
690 lines (556 sloc) 17 KB
/****
antix.cc
version 1
Richard Vaughan
Clone this package from git://github.com/rtv/Antix.git
****/
#include <assert.h>
#include <unistd.h>
#include <algorithm>
#include <sys/time.h> // for gettimeofday(3)
#include "antix.h"
using namespace Antix;
static uint64_t score_time( 200 );
static double start_seconds(0);
static pthread_mutex_t sync_mutex;
static pthread_cond_t cond_start;
static pthread_cond_t cond_done;
static unsigned int worker_count;
// initialize static members
bool Robot::paused( false );
bool Robot::show_data( false );
double Robot::fov( dtor(90.0) );
double Robot::pickup_range( Robot::range/5.0 );
double Robot::radius(0.01);
double Robot::range( 0.1 );
double Robot::worldsize(1.0);
std::vector<Home*> Robot::homes;
std::vector<Robot*> Robot::population;
uint64_t Robot::updates(0);
uint64_t Robot::updates_max( 0.0 );
unsigned int Robot::home_count(1);
unsigned int Robot::home_population( 20 );
unsigned int Robot::puck_count(100);
unsigned int Robot::sleep_msec( 10 );
std::vector<Robot::MatrixCell> Robot::matrix;
unsigned int Robot::gui_interval(100);
Robot* Robot::first(NULL);
unsigned int Robot::matrixwidth( Robot::worldsize / (Robot::range) );
template <class T, class C>
void EraseAll( T thing, C& cont )
{ cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
const char usage[] = "Antix understands these command line arguments:\n"
" -? : Prints this helpful message.\n"
" -a <int> : sets the number of pucks in the world.\n"
" -c <int> : sets the number of pixels in the robots' sensor.\n"
" -d Enables drawing the sensor field of view. Speeds things up a bit.\n"
" -f <float> : sets the sensor field of view angle in degrees.\n"
" -g <int> : sets the interval between GUI redraws in milliseconds.\n"
" -p <int> : set the size of the robot population.\n"
" -r <float> : sets the sensor field of view range.\n"
" -s <float> : sets the side length of the (square) world.\n"
" -u <int> : sets the number of updates to run before quitting.\n"
" -w <int> : sets the initial size of the window, in pixels.\n"
" -z <int> : sets the number of milliseconds to sleep between updates.\n";
Home::Home( unsigned int id, const Color& color, double x, double y, double r )
: id(id), color(color), pucks(), score(0), x(x), y(y), r(r)
{
Robot::homes.push_back(this);
}
Robot::Robot( Home* home,
const Pose& pose )
: index(0),
home(home),
pose(pose),
speed(),
see_robots(),
see_pucks(),
puck_held(NULL)
{
// add myself to the static vector of all robots
population.push_back( this );
if( ! first )
first = this;
}
// wrapper requred to work around C++'s inability to use methods as callbacks
static void PuckWorkerFunc( Robot* r )
{
r->UpdatePuckSensor();
}
static void RobotWorkerFunc( Robot* r )
{
r->UpdateRobotSensor();
}
void* WorkerThreadEntry( void (*func)(Robot*) )
{
pthread_mutex_lock(&sync_mutex);
// wait for signal
while(true)
{
// wait for the main thread to wake us up
//printf( "worker %p sleeping\n", func );
pthread_cond_wait( &cond_start, &sync_mutex );
//printf( "worker %p waking\n", func );
pthread_mutex_unlock( &sync_mutex );
// call func for every robot
FOR_EACH( it, Robot::population )
(*func)(*it);
// signal done
pthread_mutex_lock( &sync_mutex );
// decrement the thread count. if we're the last thread done, signal the main thread
if( --(worker_count) == 0 )
{
//printf( "last worker (%p) signalling main thread\n", func );
pthread_cond_signal( &cond_done );
}
// keep lock going round the loop
}
return NULL; // compiler satisfaction
}
void Robot::Init( int argc, char** argv )
{
// seed the random number generator with the current time
//srand48(time(NULL));
srand48(0); // for debugging - start the same every time
// parse arguments to configure Robot static members
int c;
while( ( c = getopt( argc, argv, "?dh:a:p:s:f:g:r:c:u:z:w:")) != -1 )
switch( c )
{
case 'h':
home_count = atoi( optarg );
printf( "[Antix] home count: %d\n", home_count );
break;
case 'a':
puck_count = atoi( optarg );
printf( "[Antix] puck count: %d\n", puck_count );
break;
case 'p':
home_population = atoi( optarg );
printf( "[Antix] home_population: %d\n", home_population );
break;
case 's':
worldsize = atof( optarg );
printf( "[Antix] worldsize: %.2f\n", worldsize );
break;
case 'f':
fov = dtor(atof( optarg )); // degrees to radians
printf( "[Antix] fov: %.2f\n", fov );
break;
case 'g':
gui_interval = atol( optarg );
printf( "[Antix] gui_interval: %lu\n", (long unsigned)gui_interval );
break;
case 'r':
range = atof( optarg );
printf( "[Antix] range: %.2f\n", range );
break;
case 'u':
updates_max = atol( optarg );
printf( "[Antix] updates_max: %lu\n", (long unsigned)updates_max );
break;
case 'z':
sleep_msec = atoi( optarg );
printf( "[Antix] sleep_msec: %d\n", sleep_msec );
break;
#if GRAPHICS
case 'w': winsize = atoi( optarg );
printf( "[Antix] winsize: %d\n", winsize );
break;
case 'd': show_data=true;
puts( "[Antix] show data" );
break;
#endif
case '?':
puts( usage );
exit(0); // ok
break;
default:
fprintf( stderr, "[Antix] Option parse error.\n" );
puts( usage );
exit(-1); // error
}
Robot::matrixwidth = floor( Robot::worldsize / Robot::range );
Robot::matrix.resize( Robot::matrixwidth * Robot::matrixwidth );
#if GRAPHICS
InitGraphics( argc, argv );
#endif // GRAPHICS
pthread_mutex_init( &sync_mutex, NULL );
pthread_cond_init( &cond_start, NULL );
pthread_cond_init( &cond_done, NULL );
// enter worker threads - they do nothing until signalled in UpdateAll()
pthread_t pt;
pthread_create( &pt, NULL, (void*(*)(void*))WorkerThreadEntry, (void*)RobotWorkerFunc );
pthread_create( &pt, NULL, (void*(*)(void*))WorkerThreadEntry, (void*)PuckWorkerFunc );
// record the starting time to measure how long we have run for
struct timeval tv;
gettimeofday( &tv, NULL );
start_seconds = tv.tv_sec + tv.tv_usec/1e6;
}
void Robot::TestRobotsInCell( const MatrixCell& cell )
{
// test squared ranges to avoid expensive sqrt()
double rngsqrd( range * range );
FOR_EACH( it, cell.robots )
{
Robot* other( *it );
// discard if it's the same robot
if( other == this )
continue;
#if DEBUGVIS
neighbors.push_back( other );
#endif
// discard if it's out of range. We put off computing the
// hypotenuse as long as we can, as it's relatively expensive.
const double dx( WrapDistance( other->pose.x - pose.x ) );
if( fabs(dx) > Robot::range )
continue; // out of range
const double dy( WrapDistance( other->pose.y - pose.y ) );
if( fabs(dy) > Robot::range )
continue; // out of range
// test distance squared
const double dsq( dx*dx + dy*dy );
if( dsq > rngsqrd )
continue;
// discard if it's out of field of view
const double absolute_heading( fast_atan2( dy, dx ) );
const double relative_heading( AngleNormalize((absolute_heading - pose.a) ));
if( fabs(relative_heading) > fov/2.0 )
continue;
see_robots.push_back( SeeRobot( other->home,
other->pose,
other->speed,
sqrt( dsq ),
relative_heading,
other->Holding() ) );
}
}
void Robot::TestPucksInCell( const MatrixCell& cell )
{
// test squared ranges to avoid expensive sqrt()
double rngsqrd( range * range );
FOR_EACH( it, cell.pucks )
{
Puck* puck( *it );
#if DEBUGVIS
neighbor_pucks.push_back( puck );
#endif
// discard if it's out of range. We put off computing the
// hypotenuse as long as we can, as it's relatively expensive.
const double dx( WrapDistance( puck->x - pose.x ) );
if( fabs(dx) > Robot::range )
continue; // out of range
const double dy( WrapDistance( puck->y - pose.y ) );
if( fabs(dy) > Robot::range )
continue; // out of range
const double dsq( dx*dx + dy*dy );
if( dsq > rngsqrd )
continue;
// discard if it's out of field of view
const double absolute_heading( fast_atan2( dy, dx ) );
const double relative_heading( AngleNormalize((absolute_heading - pose.a)));
if( fabs(relative_heading) > fov/2.0 )
continue;
// passes all the tests, so we record a puck detection in the
// vector
see_pucks.push_back( SeePuck( puck, sqrt(dsq),
relative_heading,
puck->held));
}
}
// void SensePuckThreadEntry( std::vector<Robot*> &robots )
// {
// // wait for signal
// FOR_EACH( it, robots )
// (*it)->UpdateRobotSensor();
// // signal done
// }
void Robot::UpdateRobotSensor()
{
see_robots.clear();
const int lastx( CellNoWrap(sensor_bbox.x.max) );
const int lasty( CellNoWrap(sensor_bbox.y.max) );
for( int x(CellNoWrap(sensor_bbox.x.min)); x<=lastx; x++ )
for( int y(CellNoWrap(sensor_bbox.y.min)); y<=lasty; y++ )
TestRobotsInCell( matrix[ CellWrap(x) + ( CellWrap(y) * matrixwidth )] );
}
void Robot::UpdatePuckSensor()
{
see_pucks.clear();
// note: the following two large sensing operations could safely be
// done in parallel since they do not modify any common data
const int lastx( CellNoWrap(sensor_bbox.x.max) );
const int lasty( CellNoWrap(sensor_bbox.y.max) );
for( int x(CellNoWrap(sensor_bbox.x.min)); x<=lastx; x++ )
for( int y(CellNoWrap(sensor_bbox.y.min)); y<=lasty; y++ )
TestPucksInCell( matrix[ CellWrap(x) + ( CellWrap(y) * matrixwidth ) ] );
}
/*
void Robot::UpdateSensors()
{
see_robots.clear();
see_pucks.clear();
// note: the following two large sensing operations could safely be
// done in parallel since they do not modify any common data
#if DEBUGVIS
// debug visualization
neighbors.clear();
neighbor_pucks.clear();
neighbor_cells.clear();
#endif
const int lastx( CellNoWrap(sensor_bbox.x.max) );
const int lasty( CellNoWrap(sensor_bbox.y.max) );
for( int x(CellNoWrap(sensor_bbox.x.min)); x<=lastx; x++ )
for( int y(CellNoWrap(sensor_bbox.y.min)); y<=lasty; y++ )
{
unsigned int index( CellWrap(x) + ( CellWrap(y) * matrixwidth ));
TestRobotsInCell( matrix[index] );
TestPucksInCell( matrix[index] );
#if DEBUGVIS
neighbor_cells.insert( index );
#endif
}
}
*/
bool Robot::Pickup()
{
if( ! puck_held )
FOR_EACH( it, see_pucks )
{
// is the puck close enough and is it not held already?
if( (it->range < pickup_range) && !it->puck->held)
{
// pick it up
puck_held = it->puck;
puck_held->Pickup();
return true;
}
}
// already holding or nothing close enough
return false;
}
bool Robot::Holding() const
{
return (bool)puck_held;
}
bool Robot::Drop()
{
if( puck_held )
{
puck_held->Drop();
puck_held = NULL;
return true; // dropped successfully
}
return false; // nothing to drop
}
void Robot::UpdatePose()
{
// move according to the current speed
const double dx( speed.v * fast_cos(pose.a) );
const double dy( speed.v * fast_sin(pose.a) );
const double da( speed.w );
pose.x = DistanceNormalize( pose.x + dx );
pose.y = DistanceNormalize( pose.y + dy );
pose.a = AngleNormalize( pose.a + da );
const unsigned int newindex( Cell( pose.x, pose.y ) );
// if we're carrying a puck, update it's position
if( puck_held )
{
puck_held->x = pose.x;
puck_held->y = pose.y;
}
if( newindex != index )
{
EraseAll( this, matrix[index].robots );
matrix[newindex].robots.push_back( this );
if( puck_held )
{
EraseAll( puck_held, matrix[index].pucks );
matrix[newindex].pucks.push_back( puck_held );
}
index = newindex;
}
// compute the new bounding box of the fov
FovBBox( sensor_bbox );
}
static inline void grow_bounds( bounds_t& b, double val )
{
if( val < b.min ) b.min = val;
if( val > b.max ) b.max = val;
}
// find the axis-aligned bounding box of our field of view
void Robot::FovBBox( bbox_t& box )
{
box.x.min = pose.x;
box.x.max = pose.x;
box.y.min = pose.y;
box.y.max = pose.y;
const double halffov = fov/2.0;
const double lefta( pose.a + halffov );
const double righta( pose.a - halffov );
// extreme left of FOV
grow_bounds( box.x, pose.x + range * fast_cos( lefta ) );
grow_bounds( box.y, pose.y + range * fast_sin( lefta ) );
// extreme right of FOV
grow_bounds( box.x, pose.x + range * fast_cos( righta ) );
grow_bounds( box.y, pose.y + range * fast_sin( righta ) );
// points where the fov crosses an axis
if( lefta > 0 && righta < 0 )
grow_bounds( box.x, pose.x + range );
if( lefta > M_PI/2.0 && righta < M_PI/2.0 )
grow_bounds( box.y, pose.y + range );
if( lefta > M_PI && righta < M_PI )
grow_bounds( box.x, pose.x - range );
if( lefta > -M_PI && righta < -M_PI )
grow_bounds( box.x, pose.x - range );
if( lefta > -M_PI/2.0 && righta < -M_PI/2.0 )
grow_bounds( box.y, pose.y - range );
}
void Home::UpdatePucks()
{
// we score 1 point for each puck that timed out here
while( pucks.size() && (Robot::updates - (*pucks.begin())->delivery_time > score_time) )
{
(*pucks.begin())->Replace();
score++;
// printf( "%llu home: %d score: %d\n", Robot::updates, id, score );
}
}
void Robot::UpdateAll()
{
// if we've done enough updates, exit the program
if( updates_max > 0 && updates > updates_max )
exit(1);
if( ! Robot::paused )
{
// not safe to do in parallel
FOR_EACH( r, homes )
(*r)->UpdatePucks();
// not safe to do in parallel
FOR_EACH( r, population )
(*r)->UpdatePose();
// unblock the workers - they are waiting on this condition var
pthread_mutex_lock( &sync_mutex );
worker_count = 2;
//puts( "main thread signalling workers" );
pthread_cond_broadcast( &cond_start );
pthread_mutex_unlock( &sync_mutex );
// wait for all the last update job to complete - it will
// signal the worker_threads_done condition var
pthread_mutex_lock( &sync_mutex );
while( worker_count )
{
//puts( "main thread waiting for workers to finish" );
pthread_cond_wait( &cond_done, &sync_mutex );
}
pthread_mutex_unlock( &sync_mutex );
//puts( "main thread awakes" );
// wait for worker threads to complete
// not necessarily safe to do in parallel
FOR_EACH( r, population )
(*r)->Controller();
++updates;
static double lastseconds=0;
if( updates % 10 == 0 ) // every hundred updates
{
static struct timeval tv;
gettimeofday( &tv, NULL );
double seconds = tv.tv_sec + tv.tv_usec/1e6;
double interval = seconds - lastseconds;
printf( "[%llu] %.2f (%.2f)\n", updates, 10.0/interval, updates/(seconds-start_seconds) );
lastseconds = seconds;
}
}
// possibly snooze to save CPU and slow things down
if( paused || sleep_msec > 0 )
usleep( sleep_msec * 1e3 );
}
void Robot::Run()
{
#if GRAPHICS
UpdateGui();
#else
while( 1 )
UpdateAll();
#endif
}
// wrap around torus
double Robot::WrapDistance( double d )
{
const double halfworld( worldsize * 0.5 );
if( d > halfworld )
d -= worldsize;
else if( d < -halfworld )
d += worldsize;
return d;
}
/** Normalize a length to within 0 to worldsize. */
double Robot::DistanceNormalize( double d )
{
while( d < 0 ) d += worldsize;
while( d > worldsize ) d -= worldsize;
return d;
}
/** Normalize an angle to within +/_ M_PI. */
double Robot::AngleNormalize( double a )
{
while( a < -M_PI ) a += 2.0*M_PI;
while( a > M_PI ) a -= 2.0*M_PI;
return a;
}
Puck::Puck( double x, double y )
: held(true), home(NULL), index(0), delivery_time(0), x(x), y(y)
{
Robot::matrix[Robot::Cell(x,y)].pucks.push_back(this);
Drop();
}
Puck::~Puck()
{
EraseAll( this, Robot::matrix[Robot::Cell(x,y)].pucks );
}
void Puck::Replace()
{
EraseAll( this, Robot::matrix[Robot::Cell(x,y)].pucks );
x = drand48() * Robot::worldsize;
y = drand48() * Robot::worldsize;
Robot::matrix[Robot::Cell(x,y)].pucks.push_back(this);
if( home )
{
EraseAll( this, home->pucks );
home = NULL;
}
Drop();
}
void Puck::Pickup()
{
//printf( "puck %p picked up with home %p\n", this, home );
assert( held == false );
held = true;
if( home )
{
EraseAll( this, home->pucks );
home = NULL;
}
}
void Puck::Drop()
{
assert( home == NULL );
held = false;
double closest_range( 1e12 ); // huge
FOR_EACH( h, Robot::homes )
{
double range = hypot( (*h)->x-x, (*h)->y-y );
if( range < closest_range && range < (*h)->r )
{
home = *h;
closest_range = range;
}
}
if( home )
{
// record the time of delivery
delivery_time = Robot::updates;
home->pucks.push_back( this );
}
//printf( "puck %p dropped at home %p\n", this, home );
}