示例#1
0
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();
}
示例#3
0
// 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;
}
示例#4
0
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);
}