void EstimateLongPath(Map *m)
{
Graph *g = GraphSearchConstants::GetGraph(m);
GraphMapHeuristic gh(m, g);
double heur = 0;
double dist = 0;
graphState from, to;
for (int x = 0; x < 20; x++)
{
node *n = g->GetRandomNode();
double newDist = FindFarDist(g, n, from, to);
if (newDist > dist)
{
dist = newDist;
heur = gh.HCost(from, to);
}
printf("%f\t%f\t%f\t%f\n", dist, dist/g->GetNumNodes(), heur, dist/heur);
}
}
void EstimateDimension(Map *m)
{
// Graph *g = GraphSearchConstants::GetGraph(m);
// GraphEnvironment ge(g);
// std::vector<graphState> endPath;
// node *n = g->GetRandomNode();
Graph *g = GraphSearchConstants::GetGraph(m);
GraphEnvironment ge(g);
std::vector<graphState> endPath;
// 1. choose a random point
node *n = g->GetRandomNode();
// 2. search to depth d (all g-costs >= d)
double limit = 0;
TemplateAStar<graphState, graphMove, GraphEnvironment> theSearch;
theSearch.SetStopAfterGoal(false);
theSearch.InitializeSearch(&ge, n->GetNum(), n->GetNum(), endPath);
while (1)
{
double gCost;
graphState s = theSearch.CheckNextNode();
theSearch.GetClosedListGCost(s, gCost);
//printf("Expanding g-cost %f next\n", gCost);
if (gCost >= limit)
{
printf("%d\t%d\n", (int)limit, theSearch.GetNodesExpanded());
limit++;
}
if (theSearch.DoSingleSearchStep(endPath))
break;
}
graphState start = n->GetNum();
BFS<graphState, graphMove> b;
b.GetPath(&ge, start, start, endPath);
}
// 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 w, tKeyboardModifier , char)
{
astars.resize(0);
static uint64_t average1=0, average2 = 0;
static int count = 0;
Graph *g = msa->GetAbstractGraph(1);
GraphAbstractionHeuristic *gah2 = new GraphAbstractionHeuristic(msa, 2);
GraphAbstractionHeuristic *gah1 = new GraphAbstractionHeuristic(msa, 1);
GraphRefinementEnvironment *env2 = new GraphRefinementEnvironment(msa, 2, gah2, unitSims[w]->GetEnvironment()->GetMap());
GraphRefinementEnvironment *env1 = new GraphRefinementEnvironment(msa, 1, gah1, unitSims[w]->GetEnvironment()->GetMap());
env1->SetDirected(false);
env2->SetDirected(false);
for (unsigned int x = 0; x < 1; x++)
{
// node *s1 = g->GetRandomNode();
// node *g1 = g->GetRandomNode();
// node *s2 = msa->GetParent(s1);
// node *g2 = msa->GetParent(g1);
// int from, to, cost;
// if (fscanf(f, "%d\t%d\t%d\n", &from, &to, &cost) != 3)
// break;
node *s1 = g->GetRandomNode();//g->GetNode(from);
node *g1 = g->GetRandomNode();//g->GetNode(to);
node *s2 = msa->GetParent(s1);
node *g2 = msa->GetParent(g1);
uint64_t nodesExpanded = 0;
uint64_t nodesTouched = 0;
printf("Searching from %d to %d in level 1; %d to %d in level 2\n",
s1->GetNum(), g1->GetNum(), s2->GetNum(), g2->GetNum());
graphState gs1, gs2;
gs1 = s2->GetNum();
gs2 = g2->GetNum();
std::vector<graphState> thePath;
std::vector<graphState> abstractPath;
astar.GetPath(env2, gs1, gs2, abstractPath);
//printf("Abstract length %d\n", abstractPath.size());
// printf("%d\t", cost);
printf("%llu\t%llu\t%1.2f\t", astar.GetNodesExpanded(), astar.GetNodesTouched(), env2->GetPathLength(abstractPath));
if (abstractPath.size() == 0)
break;
nodesExpanded += astar.GetNodesExpanded();
nodesTouched += astar.GetNodesTouched();
env1->SetPlanningCorridor(abstractPath, 2);
gs1 = s1->GetNum();
gs2 = g1->GetNum();
astar.GetPath(env1, gs1, gs2, thePath);
printf("Full\t%llu\t%1.2f\t", astar.GetNodesExpanded(), env1->GetPathLength(thePath));
// for (unsigned int x = 0; x < thePath.size(); x++)
// printf("{%d}", thePath[x]);
int abstractStart = 0;
gs1 = s1->GetNum();
double totalLength = 0;
int refineAmt = 5;
do { // not working yet -- fully check!
astars.resize(astars.size()+1);
env1->SetPlanningCorridor(abstractPath, 2, abstractStart);
gs2 = g1->GetNum();
if (abstractPath.size()-abstractStart > refineAmt)
{
env1->SetUseAbstractGoal(true, 2);
gs2 = abstractPath[abstractStart+refineAmt];
//printf("Using abstract goal of %d\n", abstractStart+refineAmt);
}
else {
env1->SetUseAbstractGoal(false, 0);
}
astars.back().GetPath(env1, gs1, gs2, thePath);
abstractStart += refineAmt;
gs1 = thePath.back();
// for (unsigned int x = 0; x < thePath.size(); x++)
// printf("(%d)", thePath[x]);
nodesExpanded += astar.GetNodesExpanded();
nodesTouched += astar.GetNodesTouched();
totalLength += env1->GetPathLength(thePath);
if (thePath.back() == gs2)
break;
} while (thePath.back() != g1->GetNum());
printf("Part\t%llu\t%llu\t%1.2f\t", astar.GetNodesExpanded(), astar.GetNodesTouched(), totalLength);
thePath.resize(0);
printf("Tot\t%llu\t%llu\n", nodesExpanded, nodesTouched);
}
}
void buildProblemSet()
{
ScenarioLoader s;
printf(gDefaultMap); printf("\n");
Map map(gDefaultMap);
Graph *g = GraphSearchConstants::GetGraph(&map);
GraphDistanceHeuristic gdh(g);
gdh.SetPlacement(kFarPlacement);
// make things go fast; we're doing tons of searches, so use a good heuristic
for (unsigned int x = 0; x < 30; x++)
gdh.AddHeuristic();
GraphEnvironment *ge = new GraphEnvironment(&map, g, &gdh);
ge->SetDirected(true);
std::vector<std::vector<Experiment> > experiments;
for (unsigned int x = 0; x < 100000; x++)
{
if (0==x%100)
{ printf("\r%d", x); fflush(stdout); }
node *s1 = g->GetRandomNode();
node *g1 = g->GetRandomNode();
// uint64_t nodesExpanded = 0;
// printf("%d\t%d\t", s1->GetNum(), g1->GetNum());
graphState gs1, gs2;
gs1 = s1->GetNum();
gs2 = g1->GetNum();
std::vector<graphState> thePath;
astar.GetPath(ge, gs1, gs2, thePath);
// printf("%d\n", (int)ge->GetPathLength(thePath));
if (thePath.size() == 0)
continue;
if (experiments.size() <= ge->GetPathLength(thePath)/4)
experiments.resize(ge->GetPathLength(thePath)/4+1);
if (experiments[ge->GetPathLength(thePath)/4].size() < 10)
{
Experiment e(s1->GetLabelL(GraphSearchConstants::kMapX), s1->GetLabelL(GraphSearchConstants::kMapY),
g1->GetLabelL(GraphSearchConstants::kMapX), g1->GetLabelL(GraphSearchConstants::kMapY),
map.GetMapWidth(), map.GetMapHeight(), ge->GetPathLength(thePath)/4, ge->GetPathLength(thePath), gDefaultMap);
experiments[ge->GetPathLength(thePath)/4].push_back(e);
}
bool done = true;
for (unsigned int y = 0; y < experiments.size(); y++)
{
if (experiments[y].size() != 10)
{ done = false; break; }
}
if (done) break;
}
for (unsigned int x = 1; x < experiments.size(); x++)
{
if (experiments[x].size() != 10)
break;
for (unsigned int y = 0; y < experiments[x].size(); y++)
s.AddExperiment(experiments[x][y]);
}
printf("\n");
char name[255];
sprintf(name, "%s.scen", gDefaultMap);
s.Save(name);
exit(0);
}
void MeasureHighwayDimension(Map *m, int depth)
{
srandom(10);
Graph *g = GraphSearchConstants::GetGraph(m);
GraphEnvironment ge(g);
ge.SetDirected(true);
std::vector<graphState> endPath;
for (int x = 0; x < g->GetNumNodes(); x++)
g->GetNode(x)->SetLabelL(GraphSearchConstants::kTemporaryLabel, 0);
// 1. choose a random point
node *n = g->GetRandomNode();
// 2. search to depth d (all g-costs >= d)
TemplateAStar<graphState, graphMove, GraphEnvironment> theSearch;
theSearch.SetStopAfterGoal(false);
theSearch.InitializeSearch(&ge, n->GetNum(), n->GetNum(), endPath);
while (1)
{
double gCost;
graphState s = theSearch.CheckNextNode();
if (theSearch.DoSingleSearchStep(endPath))
break;
assert(theSearch.GetClosedListGCost(s, gCost));
if (gCost > depth)
break;
}
// 3. mark all nodes on OPEN
unsigned int radiusCount = theSearch.GetNumOpenItems();
for (unsigned int x = 0; x < radiusCount; x++)
{
g->GetNode(theSearch.GetOpenItem(x).data)->SetLabelL(GraphSearchConstants::kTemporaryLabel, 1);
}
// 4. continue search to depth 4d (all g-costs >= 4d)
while (1)
{
double gCost;
graphState s = theSearch.CheckNextNode();
if (theSearch.DoSingleSearchStep(endPath))
break;
assert(theSearch.GetClosedListGCost(s, gCost));
if (gCost > 4*depth)
break;
}
// 5. for every state on open, trace back to marked node
// (marking?)
radiusCount = theSearch.GetNumOpenItems();
for (unsigned int x = 0; x < radiusCount; x++)
{
graphState theNode = theSearch.GetOpenItem(x).data;
theSearch.ExtractPathToStart(theNode, endPath);
for (unsigned int y = 0; y < endPath.size(); y++)
{
if (g->GetNode(endPath[y])->GetLabelL(GraphSearchConstants::kTemporaryLabel) == 1)
g->GetNode(endPath[y])->SetLabelL(GraphSearchConstants::kTemporaryLabel, 2);
}
}
int dimension = 0;
// 6. count marked nodes to see how many were found
for (int x = 0; x < g->GetNumNodes(); x++)
if (g->GetNode(x)->GetLabelL(GraphSearchConstants::kTemporaryLabel) == 2)
dimension++;
printf("%d states at radius %d; %d states [highway dimension] at radius %d\n", radiusCount, depth, dimension, 4*depth);
}
void MyPathfindingKeyHandler(unsigned long windowID, tKeyboardModifier , char)
{
std::vector<graphState> thePath;
Directional2DEnvironment *env = unitSims[windowID]->GetEnvironment();
Map *m = env->GetMap();
Graph *g = GraphSearchConstants::GetGraph(m);
// GraphDistanceHeuristic diffHeuristic(g);
// diffHeuristic.SetPlacement(kAvoidPlacement);
// for (int x = 0; x < 20; x++)
// diffHeuristic.AddHeuristic();
GraphMapInconsistentHeuristic diffHeuristic(m, g);
diffHeuristic.SetPlacement(kAvoidPlacement);
for (int x = 0; x < 20; x++)
diffHeuristic.AddHeuristic();
GraphEnvironment gEnv(g, &diffHeuristic);
gEnv.SetDirected(true);
diffHeuristic.SetMode(kRandom);
diffHeuristic.SetNumUsedHeuristics(2);
TemplateAStar<graphState, graphMove, GraphEnvironment> taNew;
TemplateAStar<graphState, graphMove, GraphEnvironment> taOld;
Timer t;
for (int x = 0; x < 500; x++)
{
graphState s1, g1;
do {
s1 = g->GetRandomNode()->GetNum();
g1 = g->GetRandomNode()->GetNum();
} while (gEnv.HCost(s1, g1) < 100);
double firstLength;
taNew.SetUseBPMX(false);
t.StartTimer();
taNew.GetPath(&gEnv, s1, g1, thePath);
t.EndTimer();
printf("old: %lld nodes expanded. Path length %d / %f. Time: %f\nrate0: %lld %f\n",
taNew.GetNodesExpanded(), (int)thePath.size(), gEnv.GetPathLength(thePath), t.GetElapsedTime(),
taNew.GetNodesExpanded(), taNew.GetNodesExpanded()/t.GetElapsedTime());
firstLength = gEnv.GetPathLength(thePath);
for (int y = 1; y < 5; y++)
{
taNew.SetUseBPMX(y);
t.StartTimer();
taNew.GetPath(&gEnv, s1, g1, thePath);
t.EndTimer();
printf("new: %lld nodes expanded. Path length %d / %f. Time: %f\nrate%d: %lld %f\n",
taNew.GetNodesExpanded(), (int)thePath.size(), gEnv.GetPathLength(thePath), t.GetElapsedTime(),
y, taNew.GetNodesExpanded(), taNew.GetNodesExpanded()/t.GetElapsedTime());
if (gEnv.GetPathLength(thePath) != firstLength)
printf("\n\n\n!!!!!!!!!!!!!!!!!! IT FAILED!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n\n");
}
printf("--\n");
}
delete g;
}
