void TrailMaxUnit<xyLoc,tDirection,MapEnvironment>::OpenGLDraw( int, MapEnvironment *env, SimulationInfo<xyLoc,tDirection,MapEnvironment>* ) {
GLdouble xx, yy, zz, rad;
Map *m = env->GetMap();
if( current_pos.x >= m->GetMapWidth() || current_pos.y >= m->GetMapHeight() ) {
fprintf( stderr, "Warning: TrailMaxUnit is out of bounds. Could not draw it.\n" );
return;
}
m->GetOpenGLCoord( current_pos.x, current_pos.y, xx, yy, zz, rad );
if( done )
glColor3d( 0., 1., 0. ); // turn green when done
else
glColor3f( r, g, b );
DrawSphere( xx, yy, zz, rad );
// draw the path in front of us
if( pathcache.size() > 0 && !done) {
glBegin(GL_LINE_STRIP);
glVertex3f( xx, yy, zz-rad/2 );
for( unsigned int i = 0; i < pathcache.size(); i++ ) {
m->GetOpenGLCoord( pathcache[i].x, pathcache[i].y, xx, yy, zz, rad );
glVertex3f( xx, yy, zz-rad/2 );
}
glEnd();
}
return;
};
AnyAngleAlgorithm::AnyAngleAlgorithm(const MapEnvironment *env)
{
// Read the height and width of the map.
Map* map = env->GetMap();
width_ = map->GetMapWidth() + 2; // Add +2 for the frame of obstacles.
height_ = map->GetMapHeight() + 2;
unblocked_cells_.clear();
// Generate the cells with the padding.
cells_ = new Cell*[width_];
for (unsigned int x = 0; x < width_; x++) {
cells_[x] = new Cell[height_];
}
// Make unblocked cells traversable.
int num_traversable = 0;
for (unsigned int x = 0; x < width_ - 2; x++) {
for (unsigned int y = 0; y < height_ - 2; y++) {
if (map->GetTerrainType(x, y) == kGround) {
cells_[x+1][y+1].is_traversable = true;
num_traversable++;
unblocked_cells_.push_back(xyLoc(x,y));
}
}
}
#ifdef ANY_ANGLE_VERBOSE
std::cout << "Traversable cells: " << num_traversable << std::endl;
#endif
Initialize();
}
// outputs algorithm/path length/time per step
void generatePaths(char *_map, int mapSizeX, int mapSizeY, int numBuckets, int bucketSize, int pathsPerBucket)
{
// int numBuckets = (int)(mapSize/4.0);
// const int pathsPerBucket = 10;
int pathsLeft = numBuckets*pathsPerBucket;
int iterations = 0;
int maxIterations = 10*numBuckets*pathsPerBucket;
Map *map = new Map(_map);
if ((mapSizeX != -1) && (mapSizeY != -1))
map->Scale(mapSizeX, mapSizeY);
else {
mapSizeX = map->GetMapWidth();
mapSizeY = map->GetMapHeight();
}
MapFlatAbstraction *absMap = new MapFlatAbstraction(map);
Graph *g = absMap->GetAbstractGraph(0);
node *r1, *r2;
// 10 maps/bigMaps/600.map 48 151 55 152 7.4142135624
// bucket, map, sizex, sizey, startx, starty, endx, endy, a* length
printf("# bucket\tmap\tsizex\tsizey\tfromx\tfromy\ttox\ttoy\tA*len\n");
std::vector<int> buckets(numBuckets);
for (int x = 0; x < numBuckets; x++)
buckets[x] = 0;
while ((pathsLeft > 0) && (iterations < maxIterations))
{
iterations++;
// try to find two valid points
do {
r1 = g->GetRandomNode();
r2 = g->GetRandomNode();
} while (!absMap->Pathable(r1, r2) || (r1 == r2) ||
((iterations > maxIterations/2) && (absMap->h(r1, r2) > numBuckets*bucketSize)));
aStar a;
path *p;
p = a.GetPath(absMap, r1, r2);
int cnt = 0;
double length = 0;
for (path *q = p; q; q = q->next)
{
if (q && q->next)
{
double t1, t2;
t1 = q->n->GetLabelL(kFirstData)-q->next->n->GetLabelL(kFirstData);
t2 = q->n->GetLabelL(kFirstData+1)-q->next->n->GetLabelL(kFirstData+1);
length += sqrt(t1*t1+t2*t2);
}
cnt++;
}
if ((length > numBuckets*bucketSize) ||
((iterations > maxIterations/2) && (buckets[(int)length/bucketSize] == pathsPerBucket)))
{
while (p && p->next)
{
double t1, t2;
t1 = p->n->GetLabelL(kFirstData)-p->next->n->GetLabelL(kFirstData);
t2 = p->n->GetLabelL(kFirstData+1)-p->next->n->GetLabelL(kFirstData+1);
length -= sqrt(t1*t1+t2*t2);
path *t = p;
p = p->next;
t->next = 0; delete t;
if (p->next == 0)
{
delete p;
p = 0;
}
if (p == 0)
break;
if (buckets[(int)length/numBuckets] < pathsPerBucket)
break;
}
if (p && p->next)
{
r1 = p->n;
for (path *q = p; q; q = q->next)
{
r2 = q->n;
}
delete p;
p = a.GetPath(absMap, r1, r2);
length = 0; cnt = 0;
for (path *q = p; q; q = q->next)
{
if (q && q->next)
{
double t1, t2;
t1 = q->n->GetLabelL(kFirstData)-q->next->n->GetLabelL(kFirstData);
t2 = q->n->GetLabelL(kFirstData+1)-q->next->n->GetLabelL(kFirstData+1);
length += sqrt(t1*t1+t2*t2);
}
cnt++;
}
}
}
if (p == 0)
continue;
if ((length/bucketSize < numBuckets) && (buckets[(int)length/bucketSize] < pathsPerBucket))
{
buckets[(int)length/bucketSize]++;
pathsLeft--;
}
else {
continue;
}
int x1, x2, y1, y2;
x1 = r1->GetLabelL(kFirstData); y1 = r1->GetLabelL(kFirstData+1);
x2 = r2->GetLabelL(kFirstData); y2 = r2->GetLabelL(kFirstData+1);
printf("%d\t%s\t%d\t%d\t%d\t%d\t%d\t%d\t%1.2f\n",
(int)(length/bucketSize), _map, mapSizeX, mapSizeY, x1, y1, x2, y2, length);
delete p;
}
delete absMap;
}
void MyRandomUnitKeyHandler(unsigned long windowID, tKeyboardModifier mod, char)
{
// if (mod == kShiftDown)
// {
// RunWorkMeasureTest();
// return;
// }
Map *m = unitSims[windowID]->GetEnvironment()->GetMap();
unitSims[windowID]->ClearAllUnits();
m->SetTileSet(kFast);
// recording = true;
// startRecording();
int x1, y1, x2, y2;
if (mod == kShiftDown)
{
x1 = 0; y1 = 0;
x2 = m->GetMapWidth()-1;
// x2 = 0;//m->GetMapWidth()-1;
y2 = m->GetMapHeight()-1;
}
else {
do {
x2 = random()%m->GetMapWidth();
y2 = random()%m->GetMapHeight();
x1 = random()%m->GetMapWidth();
y1 = random()%m->GetMapHeight();
} while ((m->GetTerrainType(x1, y1) != kGround) || (m->GetTerrainType(x2, y2) != kGround));
do {
x2 = random()%m->GetMapWidth();
y2 = random()%m->GetMapHeight();
x1 = random()%m->GetMapWidth();
y1 = random()%m->GetMapHeight();
} while ((m->GetTerrainType(x1, y1) != kGround) || (m->GetTerrainType(x2, y2) != kGround));
}
//x1 = 6; y1 = 3; x2 = 15; y2 = 72;
xySpeedHeading a(x1, y1), b(x2, y2);
//xySpeedHeading a(0, 0), b(mazeSize-1, mazeSize-1);
// xySpeedHeading a(0, 0), b(mazeSize-1, 0);
a1.GetPath(unitSims[windowID]->GetEnvironment(), a, b, path);
std::cout << "Found path -- " << a1.GetNodesExpanded() << " nodes expanded; length: " << unitSims[windowID]->GetEnvironment()->GetPathLength(path) << std::endl;
std::cout << "Solving " << a << " to " << b << std::endl;
stepsPerFrame = 1.0/120.0;
// GLdouble a1, b1, c1, r1;
// m->GetOpenGLCoord((x1+x2)/2, (y1+y2)/2, a1, b1, c1, r1);
// cameraMoveTo(a1, b1, c1-600*r1, 1.0);
// cameraLookAt(a1, b1, c1, 1.0);
// measure.MeasureDifficultly(unitSims[windowID]->GetEnvironment(), a, b);
// measure.ShowHistogram();
// LocalSensing::RIBS<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u1 = new LocalSensing::RIBS<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b);
// u1->SetWeight(1.0);
// u1->SetSpeed(0.02);
// unitSims[windowID]->AddUnit(u1);
// LSSLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u2 = new LSSLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b, new LSSLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(1));
// u2->SetSpeed(0.02);
// unitSims[windowID]->AddUnit(u2);
LSSLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u2 = new LSSLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b, new LSSLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(10));
u2->SetSpeed(0.02);
unitSims[windowID]->AddUnit(u2);
// LSSLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u3 = new LSSLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b, new LSSLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(10));
// u3->SetSpeed(0.02);
// unitSims[windowID]->AddUnit(u3);
// FLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u4 = new FLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b, new FLRTA::FLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(10, 1.5));
// u4->SetSpeed(0.02);
// unitSims[windowID]->AddUnit(u4);
//
// FLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u5 = new FLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b, new FLRTA::FLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(10, 5.5));
// u5->SetSpeed(0.02);
// unitSims[windowID]->AddUnit(u5);
//
FLRTA::FLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *f;
FLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> *u6 = new FLRTAStarUnit<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(a, b, f = new FLRTA::FLRTAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment>(10, 1.5));
f->SetOrderRedundant(false);
f->SetUseLocalGCost(true);
u6->SetSpeed(0.02);
unitSims[windowID]->AddUnit(u6);
unitSims[windowID]->GetStats()->AddFilter("trialDistanceMoved");
unitSims[windowID]->GetStats()->AddFilter("TotalLearning");
unitSims[windowID]->GetStats()->AddFilter("nodesExpanded");
unitSims[windowID]->GetStats()->EnablePrintOutput(true);
unitSims[windowID]->SetTrialLimit(50000);
SetNumPorts(windowID, 1+(unitSims[windowID]->GetNumUnits()-1)%MAXPORTS);
}