bool RobotAgent::isObjectCollision() {
for (int i = 0; i < gAgentWidth; i++) {
for (int j = 0; j < gAgentHeight; j++) {
Uint32 pixel = getPixel32(gEnvironmentImage, _x+i, _y+j);
if (pixel != G_COLOR_WHITE) {
Uint8 r, g, b;
SDL_GetRGB(pixel, gEnvironmentImage->format, &r, &g, &b);
if (r == 0xFF) {
int id = gPuckMap[_x+i][_y+j];
gPucks[id].replace(true);
SimpleShellsAgentWorldModel* wm = static_cast<SimpleShellsAgentWorldModel*>(_wm);
if (wm) {
wm->collectPuck(g);
}
// TODO: this might bethe right way to do it, but better tweak the distribution, so it won't fall twice at same place.
return isObjectCollision();
} else {
return true;
}
}
}
}
return false;
}
SDL_Palette genScanSurface(SDL_Surface * surf, unsigned char colors)
{
int max_attempts = surf->w * SCAN_ATTEMPTS_MULTIPLIER;
SDL_Palette pal;
int i = 1;
int z = 0;
pal.colors = calloc(sizeof(SDL_Color), (unsigned char)-1);
pal.ncolors = 1;
// black color by default
pal.colors[0].r = 0;
pal.colors[0].g = 0;
pal.colors[0].b = 0;
while(i++ < max_attempts && pal.ncolors <= colors)
{
// Getting a random pixel from input surface
SDL_Color col;
unsigned int c = getPixel32(surf, (int)rand()%surf->w, (int)rand()%surf->h);
RGBA_TO_SDL_COLOR(col,c);
for (z = 0; z < pal.ncolors; z++) {
if (col.r == pal.colors[z].r && col.g == pal.colors[z].g && col.b == pal.colors[z].b) break;
}
if (z < pal.ncolors) continue; //pixel exist!
pal.colors[pal.ncolors++] = col;
}
return pal;
}
void RobotAgent::updateSensors() {
// update sensors
double orientationAngle = _wm->_agentAbsoluteOrientation * M_PI / 180;
gUseDitchSensors = false;
for (int i = 0; i < _wm->_sensorCount; i++) {
// Warning: the following is repeated in the show method because coordinates are not stored, but are needed to display the sensor rays.
_wm->_rangeSensors[i]->update(_wm->_xReal, _wm->_yReal, orientationAngle);
_wm->_rangeSensors[i]->cast(gEnvironmentImage);
_wm->_sensors[i][5] = _wm->_rangeSensors[i]->_obstacleRange;
_wm->_sensors[i][6] = _wm->_rangeSensors[i]->_obstacleId;
// Cast another sensor on the zone map to detect ditches. TODO: Can see ditches behind obstacles?
if (gUseDitchSensors) {
_wm->_rangeSensors[i]->cast(gZoneImage);
_wm->_ditchSensor[i] = _wm->_rangeSensors[i]->_obstacleRange;
}
}
/*
updateSensorArray(_wm->_rangeSensors, _wm->_sensorCount, _wm->_xReal, _wm->_yReal, orientationAngle);
for (int i = 0; i < _wm->_sensorCount; i++) {
_wm->_sensors[i][5] = _wm->_rangeSensors[i]->_obstacleRange;
_wm->_sensors[i][6] = _wm->_rangeSensors[i]->_obstacleId;
}
*/
Uint32 pixel;
Uint8 r, g, b;
pixel = getPixel32(gZoneImage, _wm->_xReal, _wm->_yReal);
SDL_GetRGB(pixel, gZoneImage->format, &r, &g, &b);
_wm->_floorSensor = r;
}
void DefaultSensors::update(Point2d position, double orientation){
// update sensors
for (int i = 0; i < _sensorCount; i++) {
// Warning: the following is repeated in the show method because coordinates are not stored, but are needed to display the sensor rays.
double x1 = (position.x + _sensors[i][1] * cos(_sensors[i][2] + orientation * M_PI / 180));
double y1 = (position.y + _sensors[i][1] * sin(_sensors[i][2] + orientation * M_PI / 180));
double x2 = (position.x + _sensors[i][3] * cos(_sensors[i][4] + orientation * M_PI / 180));
double y2 = (position.y + _sensors[i][3] * sin(_sensors[i][4] + orientation * M_PI / 180));
// cast sensor ray.
_sensors[i][5] = castSensorRay(gEnvironmentImage, x1, y1, &x2, &y2, getSensorMaximumDistanceValue(i)); // x2 and y2 are overriden with collision coordinate if ray hits object. -- not used here.
Uint8 r, g, b;
Uint32 pixel = getPixel32(gEnvironmentImage, x2, y2);
SDL_GetRGB(pixel, gEnvironmentImage->format, &r, &g, &b);
_sensors[i][6] = (r << 16)+(g << 8) + b; // R=objects, B=agents, can only be one agent OR one object in each location.
if(gEnergyMode) {
// Cast another sensor on the energy map to detect energy points
ResourceFactory<EnergyPoint>::ResourceFactoryPtr factory = ResourceFactory<EnergyPoint>::getInstance();
double xx1 = (position.x + _sensors[i][1] * cos(_sensors[i][2] + orientation * M_PI / 180));
double yy1 = (position.y + _sensors[i][1] * sin(_sensors[i][2] + orientation * M_PI / 180));
double xx2 = (position.x + _sensors[i][3] * cos(_sensors[i][4] + orientation * M_PI / 180));
double yy2 = (position.y + _sensors[i][3] * sin(_sensors[i][4] + orientation * M_PI / 180));
_energySensor[i][0] = factory->castRay(xx1, yy1, &xx2, &yy2, getSensorMaximumDistanceValue(i));
Uint8 r, g, b;
factory->getRGB(xx2, yy2, &r, &g, &b);
if(r != 0xff){
_energySensor[i][1] = r; // R=level of energy
}else{
_energySensor[i][1] = 0; // white = no energy
}
}
} //(old: (r<<16)+(g<<8)+b;)
Uint8 r, g, b;
Uint32 pixel = getPixel32(gZoneImage, position.x, position.y);
SDL_GetRGB(pixel, gZoneImage->format, &r, &g, &b);
_floorSensor = r;
}
bool Puck::testEnvironment() {
Sint16 xc = _xCenterPixel, yc = _yCenterPixel, r = _radius;
for (Sint16 xi = xc - r; xi < xc + r; xi++) {
for (Sint16 yi = yc - r; yi < yc + r; yi++) {
if (getPixel32(gEnvironmentImage, xi, yi) != COLOR_WHITE) {
return false;
}
}
}
return true;
}
void composite(SDL_Surface* src, SDL_Surface *dest, int offsetX, int offsetY)
{
if(SDL_MUSTLOCK(dest)) SDL_LockSurface(dest);
if(SDL_MUSTLOCK(src)) SDL_LockSurface(src);
for(int x=0; x < src->w; x++)
for(int y=0; y<src->h; y++)
{
Uint32 srcpixel = getPixel32(src, x, y);
if(srcpixel & 0xff000000)
{
putPixel32(dest, x+offsetX, y+offsetY, srcpixel);
continue;
}
Uint32 destpixel = getPixel32(dest, x+offsetX, y+offsetY);
putPixel32(dest, x+offsetX, y+offsetY, destpixel);
}
if(SDL_MUSTLOCK(dest)) SDL_UnlockSurface(dest);
if(SDL_MUSTLOCK(src)) SDL_UnlockSurface(src);
}
/**
* Display agent on screen. Add information caption if needed.
* (render mode only)
*/
void Agent::show() // display on screen
{
//Show the dot
if (gUseOrganisms && _connectToOthers == POSITIVE) {
apply_surface(_x - gCamera.x, _y - gCamera.y, gAgentPositiveMaskImage, gScreen);
} else if (gUseOrganisms && _connectToOthers == NEGATIVE) {
apply_surface(_x - gCamera.x, _y - gCamera.y, gAgentNegativeMaskImage, gScreen);
} else {
apply_surface(_x - gCamera.x, _y - gCamera.y, gAgentMaskImage, gScreen);
}
Uint32 pixel = getPixel32(gZoneImage, _x, _y);
if (pixel != SDL_MapRGB(gZoneImage->format, 0xFF, 0xFF, 0xFF)) // check if there's a "story" to display
{
// extract story index (if any)
Uint8 r, g, b;
SDL_GetRGB(pixel, gZoneImage->format, &r, &g, &b);
int storyId = b; // assume the blue component holds the story index.
if (storyId >= 0 && storyId < 256 && gZoneCaptionImage[storyId] != NULL && gZoneStatus[storyId]) // security check: story exists?
{
// display story caption
//set caption position
int xCaption = 0, yCaption = 0;
if (_x < gAreaWidth / 2)
xCaption = _x - gCamera.x + 40;
else
xCaption = _x - gCamera.x - gZoneCaptionImage[storyId]->w - 40;
if (_y < gAreaHeight / 2)
yCaption = _y - gCamera.y + 40;
else
yCaption = _y - gCamera.y - gZoneCaptionImage[storyId]->h - 40;
//display caption
apply_surface(xCaption, yCaption, gZoneCaptionImage[storyId], gScreen);
// update story flags (if needed)
if (storyId >= 100 && storyId < 200 && storyId % 10 == 0) // key story, btw 100 and 199: activate story flag of the 9 next sub-stories
{
for (int i = 1; i != 10; i++)
gZoneStatus[storyId + i] = true;
} else
if (storyId >= 200 && storyId < 256 && storyId % 10 != 9) // sub-story, btw 200 and 299: activate the next story flag (if not last)
{
gZoneStatus[storyId + 1] = true;
}
}
}
}
SDL_Surface* flipSurface(SDL_Surface* surf, int flags)
{
SDL_Surface* flipped = NULL;
flipped = SDL_CreateRGBSurface( SDL_SWSURFACE, surf->w, surf->h, surf->format->BitsPerPixel, surf->format->Rmask,
surf->format->Gmask, surf->format->Bmask, surf->format->Amask );
if(SDL_MUSTLOCK(surf)) SDL_LockSurface(surf);
for(int x=0, rx=flipped->w - 1; x < flipped->w; x++, rx--)
for(int y=0, ry=flipped->h-1; y<flipped->h; y++, ry--)
{
Uint32 pixel = getPixel32(surf, x, y);
if((flags & FLIP_HORIZONTAL) && (flags & FLIP_VERTICAL)) putPixel32(flipped, rx, ry, pixel);
else if(flags & FLIP_VERTICAL) putPixel32(flipped, x, ry, pixel);
else putPixel32(flipped, rx, y, pixel);
}
if(SDL_MUSTLOCK(surf)) SDL_UnlockSurface(surf);
return flipped;
}
void Sprite::flipSurface( SDL_Surface* src, SDL_Surface* desk, Vec2i begin, Vec2i end ) {
if (SDL_MUSTLOCK( src )) {
SDL_LockSurface( src );
}
for ( int y = begin.y ; y < end.y ; y++ ) {
//Go through columns
for ( int x = begin.x ; x < end.x ; x++ ) {
putPixel32( desk, (x - begin.x), (y - begin.y), getPixel32( src, x, y ));
}
}
if ( SDL_MUSTLOCK( src ) ) {
SDL_UnlockSurface( src );
}
//Copy color key
if ( src->flags & SDL_SRCCOLORKEY ) {
SDL_SetColorKey( desk, SDL_RLEACCEL | SDL_SRCCOLORKEY,
src->format->colorkey );
}
}
bool ParcoursWorldInterface::checkDitch(double x, double y){
Uint8 r, g, b;
Uint32 pixel = getPixel32(gZoneImage, x, y);
SDL_GetRGB(pixel, gZoneImage->format, &r, &g, &b);
return (r == ParcoursSharedData::ZONE_VALUE);
}
/**
cast ray from (x1,y1) to (x2,y2). Stops whenever ray encounters something. (x2,y2) are update with point of contact
__maxValue is the maximum distance possible -- ie. if no collision during ray casting (makes it possible to return an exact value without the cost of distance (with sqrt) computation)
*/
int castSensorRay(SDL_Surface * image, double x1, double y1, double *x2pt, double *y2pt, int __maxValue )
{
double x2 = *x2pt;
double y2 = *y2pt;
bool isCollision = false; // check collision btw sensor ray and object.
if ( abs(x1-x2) > abs (y1-y2) )
{
int it;
double dy = (y1-y2) / (x1-x2);
if ( (x1-x2) < 0 )
{
it=1;
}
else
{
it=-1;
dy=-dy;
}
double yReal = y1;
for ( int x = (int)(x1+0.5) ; x != (int)(x2+0.5) ; x = x + it )
{
if ( getPixel32 ( image, x, (int)(yReal+0.5) ) != SDL_MapRGB( image->format, 0xFF, 0xFF, 0xFF ) )
{
*x2pt = (double)x;
*y2pt = yReal;
isCollision = true;
break;
}
yReal += dy;
}
}
else
{
int it;
double dx = (x1-x2) / (y1-y2);
if ( (y1-y2) < 0 )
{
it=1;
}
else
{
it=-1;
dx=-dx;
}
double xReal = x1;
for ( int y = (int)(y1+0.5) ; y != (int)(y2+0.5) ; y = y + it )
{
if ( getPixel32 ( image, (int)(xReal+0.5), y ) != SDL_MapRGB( image->format, 0xFF, 0xFF, 0xFF ) )
{
*x2pt = xReal;
*y2pt = (double)y;
isCollision = true;
break;
}
xReal += dx;
}
}
if ( isCollision == false && __maxValue != -1 )
return __maxValue;
else
return sqrt ( ( x1 - *x2pt ) * ( x1 - *x2pt ) + ( y1 - *y2pt ) * ( y1 - *y2pt ) );
// should be equal to gSensorRange; // no hit
}
MONEEControlArchitecture::MONEEControlArchitecture( RobotAgentWorldModel *__wm ) : BehaviorControlArchitecture ( __wm ) {
_wm = (MONEEAgentWorldModel*)__wm;
/**
* Adds the extra puckSensors to the active core sensor list, such that it
* can be used
*/
//register sensors
_rangeSensors.resize(_wm->getDefaultSensors()->getSensorCount());
for (int x = 0; x != gAgentWidth; x++) {
for (int y = 0; y != gAgentHeight; y++) {
Uint32 pixel = getPixel32(gAgentSpecsImage, x, y);
if (pixel != SDL_MapRGB(gAgentSpecsImage->format, 0xFF, 0xFF, 0xFF)) {
// sensor found, register sensor.
Uint8 r, g, b;
SDL_GetRGB(pixel, gAgentSpecsImage->format, &r, &g, &b);
// if (_wm->getDefaultSensors()->getSensors()[r][0] != -1) {
// std::cout << "[ERROR] robot sensor id already in use -- check agent specification image." << std::endl;
// exit(-1);
// }
//
if (r >= _wm->getDefaultSensors()->getSensorCount()) {
std::cout << "[ERROR] robot sensor id is not permitted (must be defined btw 0 and " << (_wm->getDefaultSensors()->getSensorCount() - 1) << ", got: " << r << ") -- check agent specification image." << std::endl;
exit(-1);
}
int sensorId = r;
double dxOrigin = x - gAgentWidth / 2;
double dyOrigin = y - gAgentHeight / 2;
double originDistance = sqrt(dxOrigin * dxOrigin + dyOrigin * dyOrigin);
double cosOrigin = dxOrigin / originDistance;
double sinOrigin = dyOrigin / originDistance;
// atan2 ?
double originAngle;
if (sinOrigin >= 0)
originAngle = acos(cosOrigin) + M_PI * 0.5;
else
originAngle = -acos(cosOrigin) + M_PI * 2.5;
// sensor target point location wrt. agent center -- sensor target angle is (green+blue) component values
// note: '-90deg' is due to image definition convention (in image, 0° means front of agent, which is upward -- while 0° in simulation means facing right)
double targetAngle = (g + b - 90) * M_PI / 180;
double sinTarget, cosTarget;
sinOrigin = std::sin(targetAngle);
cosOrigin = std::cos(targetAngle);
sinTarget = std::sin(targetAngle);
cosTarget = std::cos(targetAngle);
double dxTarget = dxOrigin + cosTarget * gSensorRange;
double dyTarget = dyOrigin + sinTarget * gSensorRange;
double targetDistance = sqrt(dxTarget * dxTarget + dyTarget * dyTarget); // length (**from agent center**)
// Now relatively to the center of agent, not the origin point
cosTarget = dxTarget / targetDistance;
sinTarget = dyTarget / targetDistance;
if (sinTarget >= 0)
targetAngle = acos(cosTarget) + M_PI * 0.5;
else
targetAngle = -acos(cosTarget) + M_PI * 2.5;
_rangeSensors[sensorId] = new PuckSensors(sensorId, originDistance, originAngle, targetDistance, targetAngle, gSensorRange);
// THIS IS UGLY GETTING ADDRESS FROM REFERENCE, DO NOT COPY!
_rangeSensors[sensorId]->getOldSensorData(&(_wm->getDefaultSensors()->getSensors()[sensorId].front()));
r++;
}
}
}
for(unsigned int i = 0; i < _rangeSensors.size(); i++){
_wm->addSensors(_rangeSensors[i]);
}
int tmpInt = 0;
gProperties.checkAndGetPropertyValue("gMaxGenePool", &tmpInt, true);
gMaxGenePool = tmpInt;
gProperties.checkAndGetPropertyValue("gMaxLifetimeGathering", &tmpInt, true);
_maxLifetime[PHASE_GATHERING] = tmpInt;
gProperties.checkAndGetPropertyValue("gMaxLifetimeMating", &tmpInt, true);
_maxLifetime[PHASE_MATING] = tmpInt;
gProperties.checkAndGetPropertyValue("gHiddenNeuronCount", &tmpInt, true);
_hiddenNeuronCount = tmpInt;
_randomSelection = false;
gProperties.checkAndGetPropertyValue("gRandomSelection", &_randomSelection, false);
_useMarket = true;
gProperties.checkAndGetPropertyValue("gUseMarket", &_useMarket, true);
_useSpecBonus = false;
gProperties.checkAndGetPropertyValue("gUseSpecBonus", &_useSpecBonus, false);
_task1Premium = 1.0;
gProperties.checkAndGetPropertyValue("gTask1Premium", &_task1Premium, 1.0);
_selectionPressure = 1.5;
gProperties.checkAndGetPropertyValue("gSelectionPressure", &_selectionPressure, 1.5);
if (_hiddenNeuronCount > 0) {
_parameterCount = (_wm->getDefaultSensors()->getSensorCount() * (gPuckColors + 1) + 1 + 2) * _hiddenNeuronCount + (_hiddenNeuronCount + 1) * 2;
_response.assign(_hiddenNeuronCount, .0);
} else {
_parameterCount = (_wm->getDefaultSensors()->getSensorCount() * (gPuckColors + 1) + 1 + 2) * 2;
}
_wm->_genePool.reserve(gMaxGenePool);
_nearbyGenomes.reserve(gNbOfAgents); // Agent counter is unpredictable at this time
_activeGenome.parameters.assign(_parameterCount, .0);
mutate(_activeGenome.parameters, 1.0);
_wm->_puckCounters = &(_activeGenome.pucks);
}
int TilesLoad(const char *fname)
{
nil_tile = IMG_Load(NILT_FN);
if(nil_tile == NULL)
{
printf("File`s offline: %s\n", NILT_FN);
return -1;
}
SDL_Surface *t = SDL_DisplayFormatAlpha(nil_tile);
SDL_FreeSurface(nil_tile);
SDL_Surface *s = zoomSurface(t, (double)tile_w_/t->w, (double)tile_h_/t->h, SMOOTHING_OFF);
SDL_FreeSurface(t);
nil_tile = s;
FILE *f;
if((f = fopen(fname, "r")) == NULL)
{
printf("Can`t reach the file: %s\n", fname);
return -1;
}
/* Skipping to second line */
//fscanf(f, "\n");
int recieved = 0;
int current_tile = 0;
while(!feof(f))
{
#define RESERVED 0x400
if(tiles == NULL) tiles = malloc(sizeof(tilerecord_t));
else tiles = realloc(tiles, sizeof(tilerecord_t)*(current_tile+1));
tiles[current_tile].alright = 1; /* will be set to 0, once any fail happens */
/* Reserving memory */
tiles[current_tile].filename = malloc(RESERVED);
memset(tiles[current_tile].filename, '\0', RESERVED);
tiles[current_tile].dummy = malloc(RESERVED);
if(fscanf(f, "%u:%u:%s\n", &tiles[current_tile].id, &tiles[current_tile].type,
tiles[current_tile].filename) != 3)
{
printf("TilesLoad(): WARNING: It seems like the input \
file have something wrong aboard (comments, etc.). Proceeding may result in fault!\n");
tiles[current_tile].alright = 0;
}
tiles[current_tile].filename = realloc(tiles[current_tile].filename, strlen(tiles[current_tile].filename));
free(tiles[current_tile].dummy);
char *fn = (char*)malloc(strlen(tiles[current_tile].filename)+strlen(TILEDIR EXTEN)+1);
memset(fn, '\0', strlen(tiles[current_tile].filename)+strlen(TILEDIR EXTEN)+1);
strcat(fn, TILEDIR);
strcat(fn, tiles[current_tile].filename);
strcat(fn, EXTEN);
SDL_Surface *tmpsurf = SDL_LoadBMP(fn);
if(tmpsurf == NULL)
{
printf("TilesLoad(): Image wasn`t loaded successfully: %s\n", fn);
tiles[current_tile].alright = 0;
current_tile++;
free(fn);
continue;
}
free(fn);
int x,y;
tiles[current_tile].tile = SDL_DisplayFormatAlpha(tmpsurf);
SDL_FreeSurface(tmpsurf);
for(x = 0; x < tiles[current_tile].tile->w; x++)
{
for(y = 0; y < tiles[current_tile].tile->h; y++)
{
if(!(getPixel32(tiles[current_tile].tile, x, y)&0xffffff))
putPixel32(tiles[current_tile].tile, x, y, 0);
}
}
tiles[current_tile].tile_noscale = tiles[current_tile].tile;
// if(tiles[current_tile].tile_noscale->w != tile_w_ || tiles[current_tile].tile_noscale->h != tile_h_)
// {
tiles[current_tile].tile = zoomSurface(tiles[current_tile].tile_noscale,
(double)tile_w_/tiles[current_tile].tile_noscale->w,
(double)tile_h_/tiles[current_tile].tile_noscale->h, SMOOTHING_OFF);
// }
current_tile++;
recieved++;
}
void Collect2RobotSensorsWorldObserver::updateMonitoring()
{
// * Log at end of each generation
//std::cout << gWorld->getIterations() << std::endl;
if( _lifeIterationCount >= Collect2RobotSensorsSharedData::gEvaluationTime ) // end of generation.
{
if ( gVerbose )
{
std::cout << "[gen:" << (gWorld->getIterations()/Collect2RobotSensorsSharedData::gEvaluationTime)
<< "]\n";
}
// Logging here
double sumFitness = 0.0;
double sumAvgLocalPopFitness = 0.0;
int gatheredGenomes = 0;
for ( int i = 0 ; i != gNumberOfRobots ; i++ )
{
sumFitness += (dynamic_cast<Collect2RobotSensorsController*>(gWorld->getRobot(i)->getController()))
-> getFitness();
sumAvgLocalPopFitness += (dynamic_cast<Collect2RobotSensorsController*>
(gWorld->getRobot(i)->getController())) -> getAvgPopFitness();
gatheredGenomes += (dynamic_cast<Collect2RobotSensorsController*>
(gWorld->getRobot(i)->getController())) ->_genomesList.size();
}
//std::cout << gWorld->getIterations() << " ";
//<< (sumFitness / gNumberOfRobots) / Collect2SharedData::gEvaluationTime
// divided by two because each item gives 1 fitness point to both agents
std::cout << sumFitness / 2 << std::endl;
}
if (gWorld->getIterations() == (gMaxIt - 1))
{
double sumFitness = 0.0;
for ( int i = 0 ; i != gNumberOfRobots ; i++ )
{
sumFitness += (dynamic_cast<Collect2RobotSensorsController*>(gWorld->getRobot(i)
->getController()))-> getFitness();
}
std::cout << "End fitness: " << sumFitness / gNumberOfRobots
<< " at it: " << gWorld->getIterations() << std::endl;
if(Collect2RobotSensorsSharedData::gSaveGenome)
{
for (int i = 0 ; i != gNumberOfRobots ; i++ )
{
(dynamic_cast<Collect2RobotSensorsController*>(gWorld->getRobot(i)->getController()))
-> logGenome(Collect2RobotSensorsSharedData::gOutGenomeFile + std::to_string(i) + ".log");
}
}
}
switch (Collect2RobotSensorsSharedData::gFitness)
{
case 2:
for(int i = 0; i < gNbOfPhysicalObjects;i++)
{
if(listCollected[i].size() >= 2)
{
gPhysicalObjects[i]->isWalked(0); //Default agent for callback (unused callback)
for(auto it = listCollected[i].begin(); it != listCollected[i].end();it++)
{
dynamic_cast<Collect2RobotSensorsController*>(gWorld->getRobot((*it))
->getController())->updateFitness(1.0);
}
}
listCollected[i].clear();
}
//updateDisplay();
for(int i = 0; i < gNumberOfRobots; i++)
{
Uint8 r, g, b;
RobotWorldModel* wm = gWorld->getRobot(i)->getWorldModel();
Uint32 pixel = getPixel32( gGroundSensorImage, wm->_xReal+0.5, wm->_yReal+0.5);
SDL_GetRGB(pixel,gGroundSensorImage->format,&r,&g,&b);
wm->_groundSensorValue[0] = r;
wm->_groundSensorValue[1] = g;
wm->_groundSensorValue[2] = b;
}
break;
default:
break;
}
if(gWorld->getIterations() ==
Collect2RobotSensorsSharedData::gTimeSeq[Collect2RobotSensorsSharedData::gTaskIdx])
{
//std::cout << "Task changed!" << std::endl;
Collect2RobotSensorsSharedData::gFitness =
Collect2RobotSensorsSharedData::gTaskSeq[Collect2RobotSensorsSharedData::gTaskIdx];
Collect2RobotSensorsSharedData::gTaskIdx++;
}
}
void World::initWorld()
{
// * load environment and agents files
if( loadFiles() == false )
{
std::cout << "[CRITICAL] cannot load image files." << std::endl;
exit(-1);
}
// * initialize landmarks
for ( int i = 0 ; i != gNbOfLandmarks ; i++)
{
LandmarkObject landmarkObject;
gLandmarks.push_back(landmarkObject);
}
// * initialize physical objects
for ( int i = 0 ; i != gNbOfPhysicalObjects ; i++)
{
PhysicalObjectFactory::makeObject();
}
// * Analyse agent mask and make it into a list of coordinates
int nbPointsInMask = 0;
// count number of significant pixels in mask.
for ( int i = 0 ; i != gRobotWidth ; i++ )
for ( int j = 0 ; j != gRobotHeight ; j++ )
if ( getPixel32( gRobotMaskImage , i , j) != SDL_MapRGBA( gForegroundImage->format, 0xFF, 0xFF, 0xFF, 0 ) )
{
nbPointsInMask++;
}
gRobotMaskData.resize(nbPointsInMask);
for ( int i = 0 ; i != nbPointsInMask ; i++)
(gRobotMaskData.at(i)).reserve(2);
// count number of significant pixels in mask.
int currentIndex = 0;
for ( int i = 0 ; i != gRobotWidth ; i++ )
for ( int j = 0 ; j != gRobotHeight ; j++ )
if ( getPixel32( gRobotMaskImage , i , j) != SDL_MapRGBA( gForegroundImage->format, 0xFF, 0xFF, 0xFF, 0 ) )
{
gRobotMaskData[currentIndex][0]=i;
gRobotMaskData[currentIndex][1]=j;
currentIndex++;
}
// * initialize agents
for ( int i = 0 ; i != gInitialNumberOfRobots ; i++ )
{
Robot *robot = new Robot(this);
this->addRobot(robot);
}
_worldObserver->reset();
for ( int i = 0 ; i != gNumberOfRobots ; i++ )
robotRegistry[i]=false;
}
void OriginalWorldObserver::updateMonitoring()
{
// * Log at end of each generation
//std::cout << gWorld->getIterations() << std::endl;
if( _lifeIterationCount >= OriginalSharedData::gEvaluationTime ) // end of generation.
{
if ( gVerbose )
{
std::cout << "[gen:" << (gWorld->getIterations()/OriginalSharedData::gEvaluationTime)
<< "]\n";
}
// Logging here
double sumFitness = 0.0;
int gatheredGenomes = 0;
double forgetMeasure = 0.0;
for ( int i = 0 ; i != gNumberOfRobots ; i++ )
{
sumFitness += (dynamic_cast<OriginalController*>(gWorld->getRobot(i)->getController()))
-> getFitness();
OriginalController* ctrl = (dynamic_cast<OriginalController*>
(gWorld->getRobot(i)->getController()));
gatheredGenomes += (ctrl->_doEvoTopo? ctrl ->_genomesList.size():ctrl ->_genomesFList.size());
if(ctrl -> _storedF.empty())
{
forgetMeasure += -1.0;
}
else
{
forgetMeasure += ctrl -> forget();
}
//Forget measure for each robot ? what to do
}
//std::cout << gWorld->getIterations() << " ";
//<< (sumFitness / gNumberOfRobots) / Collect2SharedData::gEvaluationTime
//average forget measure. TODO other estimator/or all the data?
std::cout << sumFitness
//<< " " << (forgetMeasure/ gNumberOfRobots)
<< std::endl;
}
if (gWorld->getIterations() == (gMaxIt - 1))
{
double sumFitness = 0.0;
for ( int i = 0 ; i != gNumberOfRobots ; i++ )
{
sumFitness += (dynamic_cast<OriginalController*>(gWorld->getRobot(i)
->getController()))-> getFitness();
}
std::cout << "End fitness: " << sumFitness / gNumberOfRobots
<< " at it: " << gWorld->getIterations() << std::endl;
if(OriginalSharedData::gSaveGenome)
{
for (int i = 0 ; i != gNumberOfRobots ; i++ )
{
OriginalController* ctrl = (dynamic_cast<OriginalController*>(gWorld->getRobot(i)->getController()));
if(!ctrl->_doEvoTopo)
ctrl -> logGenomeF(OriginalSharedData::gOutGenomeFile + std::to_string(i) + ".log");
else
{//TODO
}
}
}
}
switch (OriginalSharedData::gFitness)
{
case 2:
for(int i = 0; i < gNbOfPhysicalObjects;i++)
{
double color = -2.0;
bool isSynchColor = false;
if(listCollected[i].size() >= 2)
{
//test if all agents (maybe more than 2) same color
for(auto it = listCollected[i].begin(); it != listCollected[i].end();it++)
{
if(color == -2.0)
{
color = it->second;
isSynchColor = true;
}
else
{
if(color != it->second)
{
isSynchColor = false;
break;
}
}
}
if(isSynchColor)
{
gPhysicalObjects[i]->isWalked(0); //Default agent for callback (unused callback)
for(auto it = listCollected[i].begin(); it != listCollected[i].end();it++)
{
//Share reward of one item between the agents involved
dynamic_cast<OriginalController*>(gWorld->getRobot(it->first)
->getController())->updateFitness(1.0 / (double)listCollected[i].size());
}
}
}
listCollected[i].clear();
}
for(int i = 0; i < gNumberOfRobots; i++)
{
Uint8 r, g, b;
RobotWorldModel* wm = gWorld->getRobot(i)->getWorldModel();
Uint32 pixel = getPixel32( gGroundSensorImage, wm->_xReal+0.5, wm->_yReal+0.5);
SDL_GetRGB(pixel,gGroundSensorImage->format,&r,&g,&b);
wm->_groundSensorValue[0] = r;
wm->_groundSensorValue[1] = g;
wm->_groundSensorValue[2] = b;
}
break;
default:
break;
}
//TOERASE test seamless node mutation
/*if(gWorld->getIterations() == 10000)
{
Helper::mutateLinkWeightsProb = 0.0;
Helper::mutateAddNodeProb = 1.0;
std::cout << "Now only node mutation" << std::endl;
}*/
if(gWorld->getIterations() ==
OriginalSharedData::gTimeSeq[OriginalSharedData::gTaskIdx])
{
//std::cout << "----------------------------" << std::endl;
//std::cout << "Task changed!" << std::endl;
OriginalSharedData::gFitness =
OriginalSharedData::gTaskSeq[OriginalSharedData::gTaskIdx];
OriginalSharedData::gTaskIdx++;
//Store representative for forget measure? For each robot? (previous task)
for (int i = 0 ; i != gNumberOfRobots ; i++)
{
(dynamic_cast<OriginalController*>(gWorld->getRobot(i)->getController()))->storeRepresentative();
}
}
}
bool World::loadFiles()
{
bool returnValue = true;
// Load the dot image
gRobotMaskImage = load_image( gRobotMaskImageFilename );
gRobotDisplayImage = load_image( gRobotDisplayImageFilename );
// Load the agent specifications image
gRobotSpecsImage = load_image( gRobotSpecsImageFilename ); // no jpg (color loss)
// Load the foreground image (active borders)
gForegroundImage = load_image( gForegroundImageFilename ); // RECOMMENDED: png rather than jpeg (pb with transparency otw)
if ( gForegroundImageFilename.compare(gForegroundImageFilename.length()-3, 3, "jpg", 0, 3) == 0 )
{
std::cerr << "foreground: PNG format is *mandatory* (JPG may feature transparency problems due to compression with loss)\n";
returnValue = false;
}
gEnvironmentImage = load_image( gEnvironmentImageFilename );
if ( gEnvironmentImageFilename.compare(gEnvironmentImageFilename.length()-3, 3, "jpg", 0, 3) == 0 )
{
std::cerr << "environment: PNG format is *mandatory* (JPG may feature transparency problems due to compression with loss)\n";
returnValue = false;
}
//gTrajectoryMonitorImage = load_image( gEnvironmentImageFilename ); // prepare for logging trajectories (useful if requested in the config file) ---- // Created in roborobo::initTrajectoriesMonitor
// load background image
gBackgroundImage = load_image( gBackgroundImageFilename );
// Load the ground type image
gGroundSensorImage = load_image( gGroundSensorImageFilename );
// Managing problems with loading files (agent mask and specs)
if( gRobotMaskImage == NULL )
{
std::cerr << "Could not load agent mask image\n";
returnValue = false;
}
if ( gRobotDisplayImage == NULL )
{
std::cerr << "Could not load agent display image\n";
returnValue = false;
}
if( gRobotSpecsImage == NULL )
{
std::cerr << "Could not load agent specification image\n";
returnValue = false;
}
//If there was a problem in loading the foreground image
if( gForegroundImage == NULL )
{
std::cerr << "Could not load foreground image\n";
returnValue = false;
}
if ( gEnvironmentImage == NULL )
{
std::cerr << "Could not load environment image\n";
returnValue = false;
}
//no background image (not a critical error)
if ( gBackgroundImage == NULL )
{
std::cout << "warning: could not load background image (will proceed anyway)\n";
}
// mandatory: image dimensions must be more than 1024x768 (otw: screen underfitting)
if ( gForegroundImage->w < gScreenWidth || gForegroundImage->h < gScreenHeight )
{
std::cerr << "foreground image dimensions must be " << gScreenWidth << "x" << gScreenHeight << " or higher (given: " << gForegroundImage->w << "x" << gForegroundImage->h << ") \n";
returnValue = false;
}
//If there was a problem in loading the ground type image
if( gGroundSensorImage == NULL )
{
std::cerr << "Could not load ground image\n";
returnValue = false;
}
else
{
if( ( gGroundSensorImage->w != gForegroundImage->w ) || ( gGroundSensorImage->h != gForegroundImage->h ) )
{
std::cerr << "Ground image dimensions do not match that of the foreground image\n";
returnValue = false;
}
}
// set reference dimensions
gRobotWidth = gRobotMaskImage->w ;
gRobotHeight = gRobotMaskImage->h ;
if ( gMaxTranslationalSpeed > gRobotWidth || gMaxTranslationalSpeed > gRobotHeight )
{
std::cerr << "[ERROR] gMaxTranslationalSpeed value *should not* be superior to agent dimensions (image width and/or height) -- may impact collision accuracy (e.g. teleporting through walls)\n";
returnValue = false;
}
gAreaWidth = gForegroundImage->w;
gAreaHeight = gForegroundImage->h;
// set transparency color
SDL_SetColorKey( gRobotMaskImage, SDL_SRCCOLORKEY, SDL_MapRGBA( gRobotMaskImage->format, 0xFF, 0xFF, 0xFF,0 ) );
SDL_SetColorKey( gRobotDisplayImage, SDL_SRCCOLORKEY, SDL_MapRGBA( gRobotMaskImage->format, 0xFF, 0xFF, 0xFF,0 ) );
SDL_SetColorKey( gForegroundImage, SDL_SRCCOLORKEY, SDL_MapRGBA( gForegroundImage->format, 0xFF, 0xFF, 0xFF,0 ) );
SDL_SetColorKey( gEnvironmentImage, SDL_SRCCOLORKEY, SDL_MapRGBA( gEnvironmentImage->format, 0xFF, 0xFF, 0xFF,0 ) );
// preparing Environment Image (ie. only the BLUE component is used)
for ( int x = 0 ; x != gEnvironmentImage->w ; x++ )
for ( int y = 0 ; y != gEnvironmentImage->h ; y++ )
{
Uint32 pixel = getPixel32(gEnvironmentImage,x,y);
if ( pixel != SDL_MapRGBA( gEnvironmentImage->format, 0xFF, 0xFF, 0xFF, 0 ) )
putPixel32( gEnvironmentImage, x, y, SDL_MapRGBA( gEnvironmentImage->format, 0, 0, pixel&0x0000FF,0 ) );
}
//If everything loaded fine
if ( returnValue == false )
return false;
else
return true;
}
void DefaultSensors::init(Point2d position, double orientation){
_sensorCount = 0;
//count sensors
for (int x = 0; x != gAgentWidth; x++){ // image is analysed a first time to count the number of sensors (faster than dynamically re-allocating array size for every new sensor)
for (int y = 0; y != gAgentHeight; y++) {
Uint32 pixel = getPixel32(gAgentSpecsImage, x, y);
if (pixel != SDL_MapRGB(gAgentSpecsImage->format, 0xFF, 0xFF, 0xFF)) {
_sensorCount++;
}
}
}
// _sensors = new double[_sensorCount][7]; // see header for details.
_ditchSensor = new double[_sensorCount];
_energySensor = new double[_sensorCount][2];
for (int i = 0; i < _sensorCount; i++) {
std::vector<double> _sensor(7);
_sensor[0] = -1;
// 1 - 4 set below
_sensor[5] = -1;
_sensor[6] = -1;
_sensors.push_back(_sensor);
_ditchSensor[i] = -1;
_energySensor[i][0] = -1;
_energySensor[i][1] = -1;
}
//int sensorIt = 0;
//register sensors
for (int x = 0; x != gAgentWidth; x++){
for (int y = 0; y != gAgentHeight; y++) {
Uint32 pixel = getPixel32(gAgentSpecsImage, x, y);
if (pixel != SDL_MapRGB(gAgentSpecsImage->format, 0xFF, 0xFF, 0xFF)) {
// sensor found, register sensor.
Uint8 r, g, b;
SDL_GetRGB(pixel, gAgentSpecsImage->format, &r, &g, &b);
if (_sensors[r][0] != -1) {
std::cout << "[ERROR] robot sensor id already in use -- check agent specification image." << std::endl;
exit(-1);
}
if (r >= _sensorCount) {
std::cout << "[ERROR] robot sensor id is not permitted (must be defined btw 0 and " << (_sensorCount - 1) << ", got: " << r << ") -- check agent specification image." << std::endl;
exit(-1);
}
_sensors[r][0] = r; // no. sensor
// sensor origini point location wrt. agent center
_sensors[r][1] = sqrt((x - gAgentWidth / 2) * (x - gAgentWidth / 2) + (y - gAgentHeight / 2) * (y - gAgentHeight / 2)); // length
double angleCosinus = ((x - (gAgentWidth / 2)) / _sensors[r][1]);
double angleSinus = ((y - (gAgentHeight / 2)) / _sensors[r][1]);
if (angleSinus >= 0)
_sensors[r][2] = acos(angleCosinus) + M_PI / 2; // angle (in radian)
else
_sensors[r][2] = -acos(angleCosinus) + M_PI / 2 + M_PI * 2; // angle (in radian)
// sensor target point location wrt. agent center -- sensor target angle is (green+blue) component values
double angle = g + b - 90; // note: '-90deg' is due to image definition convention (in image, 0° means front of agent, which is upward -- while 0° in simulation means facing right)
double xTarget = (x - gAgentWidth / 2) + cos(angle * M_PI / 180) * gSensorRange;
double yTarget = (y - gAgentHeight / 2) + sin(angle * M_PI / 180) * gSensorRange;
_sensors[r][3] = sqrt(xTarget * xTarget + yTarget * yTarget); // length (**from agent center**)
angleCosinus = xTarget / _sensors[r][3];
angleSinus = yTarget / _sensors[r][3];
if (angleSinus >= 0)
_sensors[r][4] = acos(angleCosinus) + M_PI / 2; // angle (in radian) wrt. agent center
else
_sensors[r][4] = -acos(angleCosinus) + M_PI / 2 + M_PI * 2;
r++;
}
}
}
}
RobotAgent::RobotAgent(World *__world) {
COLOR_WHITE = SDL_MapRGB(gEnvironmentImage->format, 0xFF, 0xFF, 0xFF);
_wm = gConfigurationLoader->make_RobotAgentWorldModel(); // TODO: externalize object referenced to create the new instance
_registered = false;
_wm->_world = __world;
_wm->_agentId = gAgentCounter;
gAgentCounter++;
_organism = NULL;
connected = new std::vector<RobotAgentPtr>();
//Process agent specification (ie. IR/US/laser sensors)
// create dynamic array
// parse image and add (sensor.x/y, orientation) or (sensorStart.x/y, sensorEnd.x/y)
_wm->_sensorCount = 0;
//count sensors
for (int x = 0; x != gAgentWidth; x++) // image is analysed a first time to count the number of sensors (faster than dynamically re-allocating array size for every new sensor)
for (int y = 0; y != gAgentHeight; y++) {
Uint32 pixel = getPixel32(gAgentSpecsImage, x, y);
if (pixel != SDL_MapRGB(gAgentSpecsImage->format, 0xFF, 0xFF, 0xFF))
_wm->_sensorCount++;
}
_wm->_sensors = new double[_wm->_sensorCount][7]; // see header for details.
_wm->_ditchSensor = new double[_wm->_sensorCount];
_wm->_rangeSensors.resize(_wm->_sensorCount, 0); // Because we can't use push_back -- we might go through the sensors in any random order.
for (int i = 0; i < _wm->_sensorCount; i++) {
_wm->_sensors[i][0] = -1;
// 1 - 4 set below
_wm->_sensors[i][5] = -1;
_wm->_sensors[i][6] = -1;
_wm->_ditchSensor[i] = -1;
}
//int sensorIt = 0;
//register sensors
for (int x = 0; x != gAgentWidth; x++) {
for (int y = 0; y != gAgentHeight; y++) {
Uint32 pixel = getPixel32(gAgentSpecsImage, x, y);
if (pixel != SDL_MapRGB(gAgentSpecsImage->format, 0xFF, 0xFF, 0xFF)) {
// sensor found, register sensor.
Uint8 r, g, b;
SDL_GetRGB(pixel, gAgentSpecsImage->format, &r, &g, &b);
if (_wm->_sensors[r][0] != -1) {
std::cout << "[ERROR] robot sensor id already in use -- check agent specification image." << std::endl;
exit(-1);
}
if (r >= _wm->_sensorCount) {
std::cout << "[ERROR] robot sensor id is not permitted (must be defined btw 0 and " << (_wm->_sensorCount - 1) << ", got: " << r << ") -- check agent specification image." << std::endl;
exit(-1);
}
int sensorId = r;
double dxOrigin = x - gAgentWidth / 2;
double dyOrigin = y - gAgentHeight / 2;
double originDistance = sqrt(dxOrigin * dxOrigin + dyOrigin * dyOrigin);
double cosOrigin = dxOrigin / originDistance;
double sinOrigin = dyOrigin / originDistance;
// atan2 ?
double originAngle;
if (sinOrigin >= 0)
originAngle = acos(cosOrigin) + M_PI * 0.5;
else
originAngle = -acos(cosOrigin) + M_PI * 2.5;
// sensor target point location wrt. agent center -- sensor target angle is (green+blue) component values
// note: '-90deg' is due to image definition convention (in image, 0° means front of agent, which is upward -- while 0° in simulation means facing right)
double targetAngle = (g + b - 90) * M_PI / 180;
double sinTarget, cosTarget;
sincos(targetAngle, &sinTarget, &cosTarget);
double dxTarget = dxOrigin + cosTarget * gSensorRange;
double dyTarget = dyOrigin + sinTarget * gSensorRange;
double targetDistance = sqrt(dxTarget * dxTarget + dyTarget * dyTarget); // length (**from agent center**)
// Now relatively to the center of agent, not the origin point
cosTarget = dxTarget / targetDistance;
sinTarget = dyTarget / targetDistance;
if (sinTarget >= 0)
targetAngle = acos(cosTarget) + M_PI * 0.5;
else
targetAngle = -acos(cosTarget) + M_PI * 2.5;
_wm->_rangeSensors[sensorId] = new RangeSensor(sensorId, originDistance, originAngle, targetDistance, targetAngle, gSensorRange);
_wm->_rangeSensors[sensorId]->getOldSensorData(_wm->_sensors[sensorId]);
r++;
}
}
}
_agentObserver = gConfigurationLoader->make_AgentObserver(_wm); // TODO: externalize
_behavior = gConfigurationLoader->make_BehaviorControlArchitecture(_wm); // TODO: externalize
reset();
}
/**
* Display agent on screen. Add information caption if needed.
*
* (render mode only)
*/
void RobotAgent::show() // display on screen
{
//Show the dot
if (gNiceRendering) {
if (gUseOrganisms && _connectToOthers == POSITIVE) {
apply_surface(_x - gCamera.x, _y - gCamera.y, gAgentPositiveMaskImage, gScreen);
} else if (gUseOrganisms && _connectToOthers == NEGATIVE) {
apply_surface(_x - gCamera.x, _y - gCamera.y, gAgentNegativeMaskImage, gScreen);
} else {
apply_surface(_x - gCamera.x, _y - gCamera.y, gAgentMaskImage, gScreen); // OPTIONAL (agent is already visible/registered through the environment image -- but: may be useful for image capture
}
}
if(gRenderRobotId){
std::string str = "";
str += boost::lexical_cast<std::string > (this->_wm->_agentId);
RenderTextToSurface(str, _x + (gAgentWidth / 4) - gCamera.x, _y - gCamera.y, gScreen);
}
if (getWorldModel()->getRobotLED_status() == true) {
int dx = 1;
int dy = 1;
int xcenter = (int)(_wm->_xReal + 0.5);
int ycenter = (int)(_wm->_yReal + 0.5);
int r = getWorldModel()->getRobotLED_redValue();
int g = getWorldModel()->getRobotLED_greenValue();
int b = getWorldModel()->getRobotLED_blueValue();
for (int xTmp = xcenter - dx; xTmp != xcenter + dx + 1; xTmp++)
for (int yTmp = ycenter - dy - 1; yTmp != ycenter + dy; yTmp++) {
putPixel32secure(gScreen, xTmp - gCamera.x, yTmp + dy - gCamera.y, SDL_MapRGB(gScreen->format, r, g, b));
}
}
if (_wm->_agentId == gAgentIndexFocus && gUserCommandMode) // && _iterations%10 < 5)
{
int dx = 10;
int dy = 10;
int xcenter = (int) _wm->_xReal + 0.5;
int ycenter = (int) _wm->_yReal + 0.5;
int r = 255.0 * ranf();
int g = 255.0 * ranf();
int b = 255.0 * ranf();
for (int xTmp = xcenter - dx; xTmp != xcenter + dx + 1; xTmp++) {
putPixel32secure(gScreen, xTmp - gCamera.x, ycenter - dy - gCamera.y, SDL_MapRGB(gScreen->format, r, g, b));
putPixel32secure(gScreen, xTmp - gCamera.x, ycenter + dy - gCamera.y, SDL_MapRGB(gScreen->format, r, g, b));
}
for (int yTmp = ycenter - dy; yTmp != ycenter + dy + 1; yTmp++) {
putPixel32secure(gScreen, xcenter - dx - gCamera.x, yTmp - gCamera.y, SDL_MapRGB(gScreen->format, r, g, b));
putPixel32secure(gScreen, xcenter + dx - gCamera.x, yTmp - gCamera.y, SDL_MapRGB(gScreen->format, r, g, b));
}
// for ( int xTmp = (int)_wm->_xReal - dx ; xTmp != (int)_wm->_xReal + dx + 1 ; xTmp++ )
// for ( int yTmp = (int)_wm->_yReal - dy ; yTmp != (int)_wm->_yReal + dy + 1 ; yTmp++ )
// putPixel32secure( gScreen, xTmp - gCamera.x, yTmp - gCamera.y , SDL_MapRGB( gScreen->format, 0xFF, 0x00, ranf() ) );
}
if (gDisplaySensors == true) {
// * show orientation
int xOrientationMarker = (int) (_wm->_xReal + 0.5) + gAgentWidth / 2 * cos(_wm->_agentAbsoluteOrientation * M_PI / 180);
int yOrientationMarker = (int) (_wm->_yReal + 0.5) + gAgentWidth / 2 * sin(_wm->_agentAbsoluteOrientation * M_PI / 180);
if (_wm->_agentId == gAgentIndexFocus && gUserCommandMode) {
int g, b;
g = b = (32 * _iterations % 256) > 128 ? 0 : 255;
for (int xTmp = -2; xTmp != 3; xTmp++)
for (int yTmp = -2; yTmp != 3; yTmp++)
putPixel32secure(gScreen, xOrientationMarker - gCamera.x + xTmp, yOrientationMarker - gCamera.y + yTmp, SDL_MapRGB(gScreen->format, 0x00, b, g));
} else {
for (int xTmp = -2; xTmp != 3; xTmp++)
for (int yTmp = -2; yTmp != 3; yTmp++)
putPixel32secure(gScreen, xOrientationMarker - gCamera.x + xTmp, yOrientationMarker - gCamera.y + yTmp, SDL_MapRGB(gScreen->format, 0xFF, 0x00, 0x00));
}
// * show sensors
for (int i = 0; i < _wm->_sensorCount; i++) {
// Warning: the following is a repetition of code already in the move method (sensor ray casting) in order to display it (coordinates are not stored)
double x1 = (_wm->_xReal + _wm->_sensors[i][1] * cos(_wm->_sensors[i][2] + _wm->_agentAbsoluteOrientation * M_PI / 180));
double y1 = (_wm->_yReal + _wm->_sensors[i][1] * sin(_wm->_sensors[i][2] + _wm->_agentAbsoluteOrientation * M_PI / 180));
double x2 = (_wm->_xReal + _wm->_sensors[i][3] * cos(_wm->_sensors[i][4] + _wm->_agentAbsoluteOrientation * M_PI / 180));
double y2 = (_wm->_yReal + _wm->_sensors[i][3] * sin(_wm->_sensors[i][4] + _wm->_agentAbsoluteOrientation * M_PI / 180));
// sensor ray casting is also performed in the move method -- this time we dont store data (already done). -- this one is only used to *display* the ray.
/*_sensors[i][5] = */castSensorRay(gEnvironmentImage, x1, y1, &x2, &y2, _wm->getSensorMaximumDistanceValue(i)); // x2 and y2 are overriden with collision coordinate if ray hits object.
// display on screen
if (_wm->_sensors[i][5] < _wm->getSensorMaximumDistanceValue(i) - 1) //gSensorRange-1 )
traceRayRGBA(gScreen, int(x1 + 0.5) - gCamera.x, int(y1 + 0.5) - gCamera.y, int(x2 + 0.5) - gCamera.x, int(y2 + 0.5) - gCamera.y, 255, 0, 0, 255);
else
traceRayRGBA(gScreen, int(x1 + 0.5) - gCamera.x, int(y1 + 0.5) - gCamera.y, int(x2 + 0.5) - gCamera.x, int(y2 + 0.5) - gCamera.y, 0, 0, 255, 255);
}
}
// caption for storyzones
if (gDisplayZoneCaption) {
Uint32 pixel = getPixel32(gZoneImage, _x + gAgentWidth / 2, _y + gAgentHeight / 2);
if (pixel != SDL_MapRGB(gZoneImage->format, 0x00, 0x00, 0x00)) // check if there's a "story" to display
{
// extract story index (if any)
Uint8 r, g, b;
SDL_GetRGB(pixel, gZoneImage->format, &r, &g, &b);
int storyId = r; // assume the red component holds the story index.
if (storyId >= 0 && storyId < 256 && gZoneCaptionImage[storyId] != NULL && gZoneStatus[storyId]) // security check: story exists?
{
// display story caption
//set caption position
int xCaption = 0, yCaption = 0;
if (_x < gAreaWidth / 2)
xCaption = _x - gCamera.x + 40;
else
xCaption = _x - gCamera.x - gZoneCaptionImage[storyId]->w - 40;
if (_y < gAreaHeight / 2)
yCaption = _y - gCamera.y + 40;
else
yCaption = _y - gCamera.y - gZoneCaptionImage[storyId]->h - 40;
//display caption
apply_surface(xCaption, yCaption, gZoneCaptionImage[storyId], gScreen);
// update story flags (if needed)
if (storyId >= 100 && storyId < 200 && storyId % 10 == 0) // key story, btw 100 and 199: activate story flag of the 9 next sub-stories
{
for (int i = 1; i != 10; i++)
gZoneStatus[storyId + i] = true;
} else
if (storyId >= 200 && storyId < 256 && storyId % 10 != 9) // sub-story, btw 200 and 299: activate the next story flag (if not last)
{
gZoneStatus[storyId + 1] = true;
}
}
}
}
}
/**
* update the agent position in the environment. Apply simple physics (ie. obstacle collision detection and consequences).
*/
void Agent::move() // the interface btw agent and world -- in more complex envt, this should be handled by the "world".
{
// max speed limit (opt: compress with above)
/**/
if (_xDelta > gMaxSpeedOnXaxis)
_xDelta = gMaxSpeedOnXaxis;
else
if (_xDelta < -gMaxSpeedOnXaxis)
_xDelta = -gMaxSpeedOnXaxis;
if (_yDelta > gMaxSpeedOnYaxis)
_yDelta = gMaxSpeedOnYaxis;
else
if (_yDelta < -gMaxSpeedOnYaxis)
_yDelta = -gMaxSpeedOnYaxis;
/**/
// update position
int x_old = _x; // backup old position in case of collision
int y_old = _y;
_x += _xDelta; //Move the dot left or right
_y += _yDelta; //Move the dot up or down
// * collision with (image) border of the environment - position at border, then bounce
//Move the collision boxes
//shift_boxes(); // NOT IMPLEMENTED
//If the dot went too far to the left or right
if ((_x < 0) || (_x + gAgentWidth > gAreaWidth)) {
_xDelta = -_xDelta / 2; // bounce
_x -= _xDelta; // move back
_x = _x < 0 ? 0 : gAreaWidth - gAgentWidth;
}
//Move the collision boxes
//shift_boxes(); // NOT IMPLEMENTED
//If the dot went too far up or down
if ((_y < 0) || (_y + gAgentHeight > gAreaHeight)) {
_yDelta = -_yDelta / 2; // bounce
_y -= _yDelta; // move back
_y = _y < 0 ? 0 : gAreaHeight - gAgentHeight;
}
// * Object/environment collision check
bool collision = false;
//int collision_counter = 0;
for (int i = 0; i != gAgentWidth; i++)
for (int j = 0; j != gAgentHeight; j++) {
if (getPixel32(gAgentMaskImage, i, j) != SDL_MapRGB(gForegroundImage->format, 0xFF, 0xFF, 0xFF)) // opt: bounding box instead of pixel-to-pixel test.
{
Uint32 pixel = getPixel32(gForegroundImage, _x + i, _y + j);
if (pixel != SDL_MapRGB(gForegroundImage->format, 0xFF, 0xFF, 0xFF)) {
collision = true;
break;
}
}
}
if (collision) // bounce (TODO: position at collision point)
{
_x = x_old;
_y = y_old;
_xDelta = -_xDelta / 2;
_yDelta = -_yDelta / 2;
}
}
