void MapFlatAbstraction::buildConnectivityGroups()
{
int nextNum = 0;
Graph *g = abstractions[0];
groups.resize(g->GetNumNodes());
for (unsigned int x = 0; x < groups.size(); x++)
groups[x] = -1;
node_iterator ni = g->getNodeIter();
for (node *iter = g->nodeIterNext(ni); iter; iter = g->nodeIterNext(ni))
{
std::vector<unsigned int> stack;
if (groups[iter->GetNum()] == -1)
{
stack.push_back(iter->GetNum());
while (stack.size() > 0)
{
unsigned int next = stack.back();
stack.pop_back();
if (groups[next] == -1)
{
groups[next] = nextNum;
}
neighbor_iterator n = g->GetNode(next)->getNeighborIter();
for (int val = g->GetNode(next)->nodeNeighborNext(n); val != -1; val = g->GetNode(next)->nodeNeighborNext(n))
if (groups[val] == -1)
stack.push_back(val);
}
nextNum++;
}
}
groupsValid = true;
}
void MapLineAbstraction::addEdges(Graph *aGraph)
{
Graph *g = abstractions.back();
edge_iterator ei = g->getEdgeIter();
for (edge *e = g->edgeIterNext(ei); e; e = g->edgeIterNext(ei))
{
int from = g->GetNode(e->getFrom())->GetLabelL(kParent);
int to = g->GetNode(e->getTo())->GetLabelL(kParent);
edge *f=0;
if ((from == -1) || (to == -1))
{
printf("Error, (%d parent %d) or (%d parent %d) didn't get abstracted!\n",
e->getFrom(), from, e->getTo(), to);
assert(false);
}
if ((from != to) && (!(f = aGraph->FindEdge(to, from))))
{
double weight = h(aGraph->GetNode(from), aGraph->GetNode(to));
f = new edge(from, to, weight);
f->SetLabelL(kEdgeCapacity, 1);
aGraph->AddEdge(f);
}
else if (f) f->SetLabelL(kEdgeCapacity, f->GetLabelL(kEdgeCapacity)+1);
}
}
void runProblemSet2(char *problems, int multiplier)
{
Map *map = new Map(gDefaultMap);
map->Scale(512, 512);
msa = new MapSectorAbstraction(map, 8, multiplier);
Graph *g = msa->GetAbstractGraph(1);
GraphAbstractionHeuristic gah1(msa, 1);
GraphDistanceHeuristic localGDH(g);
localGDH.SetPlacement(kAvoidPlacement);
for (unsigned int x = 0; x < 10; x++)
localGDH.AddHeuristic();
GraphHeuristicContainer ghc(g);
GraphRefinementEnvironment env1(msa, 1, &localGDH);
GraphRefinementEnvironment env2(msa, 1, 0);
// ghc.AddHeuristic(&localGDH);
// ghc.AddHeuristic(&gah1);
env1.SetDirected(false);
FILE *f = fopen(problems, "r");
if (f == 0)
{
printf("Cannot open file: '%s'\n", problems);
exit(0);
}
Timer t;
printf("len\tnodes\ttoucht\tlen\ttime\tdiff_n\tdiff_t\tdiff_l\ttime\n");
while (!feof(f))
{
int from, to, cost;
if (fscanf(f, "%d\t%d\t%d\n", &from, &to, &cost) != 3)
break;
node *s1 = g->GetNode(from);
node *g1 = g->GetNode(to);
graphState gs, gg;
gs = s1->GetNum();
gg = g1->GetNum();
std::vector<graphState> thePath;
t.StartTimer();
astar.GetPath(&env2, gs, gg, thePath);
t.EndTimer();
printf("%d\t", cost);
printf("%llu\t%llu\t%1.2f\t%e\t",
astar.GetNodesExpanded(), astar.GetNodesTouched(),
env1.GetPathLength(thePath), t.GetElapsedTime());
t.StartTimer();
astar.GetPath(&env1, gs, gg, thePath);
t.EndTimer();
printf("%llu\t%llu\t%1.2f\t%e",
astar.GetNodesExpanded(), astar.GetNodesTouched(),
env1.GetPathLength(thePath), t.GetElapsedTime());
printf("\n");
}
fclose(f);
exit(0);
}
TEST(GraphTests, GetNode_ReturnCorrect)
{
Graph graph;
auto p_node = graph.AddNode(NodePtr(new GraphNode));
auto p_node_1 = graph.AddNode(NodePtr(new GraphNode));
ASSERT_EQ(p_node, graph.GetNode(0));
ASSERT_EQ(p_node_1, graph.GetNode(1));
}
Map *ReduceMap(Map *inputMap)
{
Graph *g = GraphSearchConstants::GetGraph(inputMap);
int biggest = LabelConnectedComponents(g);
Map *m = new Map(inputMap->GetMapWidth(), inputMap->GetMapHeight());
for (int x = 0; x < inputMap->GetMapWidth(); x++)
{
for (int y = 0; y < inputMap->GetMapHeight(); y++)
{
if (inputMap->GetTerrainType(x, y) == kTrees)
m->SetTerrainType(x, y, kTrees);
else if (inputMap->GetTerrainType(x, y) == kWater)
m->SetTerrainType(x, y, kWater);
else m->SetTerrainType(x, y, kOutOfBounds);
}
}
for (int x = 0; x < g->GetNumNodes(); x++)
{
if (g->GetNode(x)->GetLabelL(GraphSearchConstants::kTemporaryLabel) == biggest)
{
int theX, theY;
theX = g->GetNode(x)->GetLabelL(GraphSearchConstants::kMapX);
theY = g->GetNode(x)->GetLabelL(GraphSearchConstants::kMapY);
if (g->GetNode(inputMap->GetNodeNum(theX+1, theY)) &&
(g->GetNode(inputMap->GetNodeNum(theX+1, theY))->GetLabelL(GraphSearchConstants::kTemporaryLabel) == biggest) &&
(!g->FindEdge(x, inputMap->GetNodeNum(theX+1, theY))))
{
m->SetTerrainType(theX, theY, kOutOfBounds);
}
else if (g->GetNode(inputMap->GetNodeNum(theX, theY+1)) &&
(g->GetNode(inputMap->GetNodeNum(theX, theY+1))->GetLabelL(GraphSearchConstants::kTemporaryLabel) == biggest) &&
(!g->FindEdge(x, inputMap->GetNodeNum(theX, theY+1))))
{
m->SetTerrainType(theX, theY, kOutOfBounds);
}
// else if (g->GetNode(inputMap->GetNodeNum(theX+1, theY+1)) &&
// (g->GetNode(inputMap->GetNodeNum(theX+1, theY+1))->GetLabelL(GraphSearchConstants::kTemporaryLabel) == biggest) &&
// (!g->FindEdge(x, inputMap->GetNodeNum(theX+1, theY+1))))
// {
// m->SetTerrainType(theX, theY, kOutOfBounds);
// }
else {
if (inputMap->GetTerrainType(theX, theY) == kSwamp)
m->SetTerrainType(theX, theY, kSwamp);
else
m->SetTerrainType(theX, theY, kGround);
}
}
else if (inputMap->GetTerrainType(g->GetNode(x)->GetLabelL(GraphSearchConstants::kMapX),
g->GetNode(x)->GetLabelL(GraphSearchConstants::kMapY)) == kGround)
m->SetTerrainType(g->GetNode(x)->GetLabelL(GraphSearchConstants::kMapX),
g->GetNode(x)->GetLabelL(GraphSearchConstants::kMapY), kTrees);
}
return m;
}
void DrawEdge(const GraphEdge& edge, const Graph& g)
{
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
const GraphNode& from = g.GetNode(edge.GetFrom());
const GraphNode& to = g.GetNode(edge.GetTo());
const Vec2f& u = from.GetPos();
const Vec2f& v = to.GetPos();
glVertex3f(u.x, 0, u.y);
glVertex3f(v.x, 0, v.y);
glEnd();
glEnable(GL_LIGHTING);
}
void SGE_Initialize()
{
cursor.Load("cursor.png");
tiles[0].Load("block.png");
tiles[1].Load("grass.png");
tiles[2].Load("brick.png");
tiles[3].Load("sand.png");
tiles[4].Load("water.png");
startSprite.Load("start.png");
endSprite.Load("end.png");
graph.Create(kWidth, kHeight);
const SVector2 offset(kTileSize * 0.5f, kTileSize * 0.5f);
for (int y = 0; y < kHeight; ++y)
{
for (int x = 0; x < kWidth; ++x)
{
Node* node = graph.GetNode(x,y);
SVector2 position((float)x * kTileSize, (float)y * kTileSize);
node->position = position + offset;
if (x != 0 && x != kWidth - 1 && y != 0 && y != kHeight - 1)
{
node->walkable = true;
}
else
{
node->walkable = false;
}
}
}
}
void MapSectorAbstraction::addEdges(Graph *aGraph)
{
Graph *g = abstractions.back();
edge_iterator ei = g->getEdgeIter();
for (edge *e = g->edgeIterNext(ei); e; e = g->edgeIterNext(ei))
{
int from = g->GetNode(e->getFrom())->GetLabelL(kParent);
int to = g->GetNode(e->getTo())->GetLabelL(kParent);
edge *f=0;
if ((from != to) && (!(f = aGraph->FindEdge(to, from))))
{
double weight = h(aGraph->GetNode(from), aGraph->GetNode(to));
f = new edge(from, to, weight);
f->SetLabelL(kEdgeCapacity, 1);
aGraph->AddEdge(f);
}
else if (f) f->SetLabelL(kEdgeCapacity, f->GetLabelL(kEdgeCapacity)+1);
}
}
TEST(PreorderIterator, ComplexGraph) {
Graph graph;
for (size_t i = 0; i < 10; ++i)
graph.CreateNode();
graph.GetNode(0).AddChild(1);
graph.GetNode(0).AddChild(6);
graph.GetNode(0).AddChild(7);
graph.GetNode(1).AddChild(2);
graph.GetNode(1).AddChild(3);
graph.GetNode(1).AddChild(4);
graph.GetNode(1).AddChild(5);
graph.GetNode(7).AddChild(8);
graph.GetNode(8).AddChild(9);
std::vector<NodeId> expected_nodes = {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
std::vector<size_t> expected_depths = {
0, 1, 2, 2, 2, 2, 1, 1, 2, 3};
std::vector<size_t> expected_child_index = {
0, 0, 0, 1, 2, 3, 1, 2, 0, 0};
ExpectTraversal(expected_nodes, expected_depths, expected_child_index, graph);
}
void DrawGraphNodes(const Graph& g)
{
int num = g.GetNumNodes();
for (int i = 0; i < num; i++)
{
const GraphNode& node = g.GetNode(i);
const Vec2f& v = node.GetPos();
glPushMatrix();
glTranslatef(v.x, 0, v.y);
//glutSolidCube(NODE_SIZE);
glutSolidSphere(NODE_SIZE, 8, 8);
glPopMatrix();
}
}
void doExport()
{
Map *map = new Map(gDefaultMap);
map->Scale(512, 512);
msa = new MapSectorAbstraction(map, 8);
msa->ToggleDrawAbstraction(1);
Graph *g = msa->GetAbstractGraph(1);
printf("g\n%d %d\n", g->GetNumNodes(), g->GetNumEdges());
for (int x = 0; x < g->GetNumNodes(); x++)
{
node *n = g->GetNode(x);
int x1, y1;
msa->GetTileFromNode(n, x1, y1);
printf("%d %d %d\n", x, x1, y1);
}
for (int x = 0; x < g->GetNumEdges(); x++)
{
edge *e = g->GetEdge(x);
printf("%d %d\n", e->getFrom(), e->getTo());//, (int)(100.0*e->GetWeight())); // %d 0
}
exit(0);
}
bool SGE_Update(float deltaTime)
{
cursor.Update(deltaTime);
rgb+=10;
rgb %= 0x00ffff;
if (Input_IsMousePressed(Mouse::MBUTTON) || Input_IsKeyPressed(Keys::SPACE))
{
if (!started)
{
start.x = Input_GetMouseScreenX()/kTileSize;
start.y = Input_GetMouseScreenY()/kTileSize;
startSprite.SetPosition(start*kTileSize);
started = true;
}
else if (!ended)
{
end.x = Input_GetMouseScreenX()/kTileSize;
end.y = Input_GetMouseScreenY()/kTileSize;
endSprite.SetPosition(end*kTileSize);
ended = true;
}
else if (started && ended)
{
started = false;
ended = false;
bfs = false;
dfs = false;
ds = false;
as = false;
}
}
if (Input_IsKeyPressed(Keys::F1) && started && ended)
{
if (!bfs)
{
BreadthFirstSearch search(graph);
search.Run((int)start.x, (int)start.y, (int)end.x, (int)end.y);
if (search.IsFound())
{
last = search.GetPath();
bfs = true;
}
ClosedList = search.GetClosedList();
}
else
{
bfs = false;
}
}
if (Input_IsKeyPressed(Keys::F2) && started && ended)
{
if (!dfs)
{
DepthFirstSearch search(graph);
search.Run((int)start.x, (int)start.y, (int)end.x, (int)end.y);
if (search.IsFound())
{
last = search.GetPath();
dfs = true;
ClosedList = search.GetClosedList();
}
}
else
{
dfs = false;
}
}
if (Input_IsKeyPressed(Keys::F3) && started && ended)
{
if (!ds)
{
DijkstraSearch search(graph, GetG());
search.Run((int)start.x, (int)start.y, (int)end.x, (int)end.y);
if (search.IsFound())
{
last = search.GetPath();
ds = true;
ClosedList = search.GetClosedList();
}
}
else
{
ds = false;
}
}
if (Input_IsKeyPressed(Keys::F4) && started && ended)
{
if (!as)
{
AStarSearch search(graph, GetG(), GetH());
search.Run((int)start.x, (int)start.y, (int)end.x, (int)end.y);
if (search.IsFound())
{
last = search.GetPath();
as = true;
ClosedList = search.GetClosedList();
}
}
else
{
as = false;
}
}
// Change Tile
if (Input_IsMousePressed(Mouse::LBUTTON))
{
int x = Input_GetMouseScreenX()/kTileSize;
int y = Input_GetMouseScreenY()/kTileSize;
map.IncrementTile(x, y);
map.ModTile(x,y,kTerrainTiles);
unsigned int tileSprite = map.GetTile(x,y);
if(tileSprite != 0 && tileSprite != 2)
{
graph.GetNode(x, y)->walkable = true;
}
else
{
graph.GetNode(x, y)->walkable = false;
}
}
// Set tile to brick
if (Input_IsKeyDown(Keys::LCONTROL) && Input_IsMouseDown(Mouse::LBUTTON))
{
int x = Input_GetMouseScreenX()/kTileSize;
int y = Input_GetMouseScreenY()/kTileSize;
map.SetTile(x, y, 2);
graph.GetNode(x, y)->walkable = false;
}
// Set tile to grass
if (Input_IsKeyDown(Keys::LCONTROL) && Input_IsMouseDown(Mouse::RBUTTON))
{
int x = Input_GetMouseScreenX()/kTileSize;
int y = Input_GetMouseScreenY()/kTileSize;
map.SetTile(x, y, 1);
graph.GetNode(x, y)->walkable = true;
}
// Reset map
if (Input_IsKeyDown(Keys::LCONTROL) && Input_IsKeyDown(Keys::NUMPAD0))
{
map.ResetMap();
}
// Reset map with no border
if (Input_IsKeyDown(Keys::LCONTROL) && Input_IsKeyDown(Keys::NUMPAD1))
{
map.NoBorder();
}
// Reset map with random tiles
if (Input_IsKeyDown(Keys::LCONTROL) && Input_IsKeyDown(Keys::NUMPAD2) && !started)
{
map.RandomMap(kTerrainTiles);
for (unsigned int x = 0; x < kWidth; ++x)
{
for (unsigned int y = 0; y < kHeight; ++y)
{
if(map.GetTile(x,y) != 0 && map.GetTile(x,y) != 2)
{
graph.GetNode(x, y)->walkable = true;
}
else
{
graph.GetNode(x, y)->walkable = false;
}
}
}
}
// Reset map with random tiles (smart)
if (Input_IsKeyDown(Keys::LCONTROL) && Input_IsKeyPressed(Keys::NUMPAD3) && !started)
{
map.RandomMapSmart();
for (unsigned int x = 0; x < kWidth; ++x)
{
for (unsigned int y = 0; y < kHeight; ++y)
{
if(map.GetTile(x,y) != 0 && map.GetTile(x,y) != 2)
{
graph.GetNode(x, y)->walkable = true;
}
else
{
graph.GetNode(x, y)->walkable = false;
}
}
}
}
return Input_IsKeyPressed(Keys::ESCAPE);
}
void runProblemSet(char *problems, int multiplier)
{
Map *map = new Map(gDefaultMap);
map->Scale(512, 512);
msa = new MapSectorAbstraction(map, 8, multiplier);
Graph *g = msa->GetAbstractGraph(1);
GraphAbstractionHeuristic gah2(msa, 2);
GraphAbstractionHeuristic gah1(msa, 1);
GraphRefinementEnvironment env2(msa, 2, &gah2);
GraphRefinementEnvironment env1(msa, 1, &gah1);
env1.SetDirected(false);
env2.SetDirected(false);
FILE *f = fopen(problems, "r");
if (f == 0)
{
printf("Cannot open file: '%s'\n", problems);
exit(0);
}
printf("len\tlvl2n\tlvl2nt\tlvl2len\tlvl2tim\tlvl1nf\tlvl1ntf\tlvl1tn\tlvl1tt\tlvl1len_f\ttot\ttott\ttot_len\n");
Timer t;
while (!feof(f))
{
int from, to, cost;
if (fscanf(f, "%d\t%d\t%d\n", &from, &to, &cost) != 3)
break;
node *s1 = g->GetNode(from);
node *g1 = g->GetNode(to);
node *s2 = msa->GetParent(s1);
node *g2 = msa->GetParent(g1);
uint64_t nodesExpanded = 0;
uint64_t nodesTouched = 0;
double totalTime = 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;
t.StartTimer();
astar.GetPath(&env2, gs1, gs2, abstractPath);
totalTime = t.EndTimer();
printf("%d\t", cost);
printf("%llu\t%llu\t%1.2f\t%f\t", astar.GetNodesExpanded(), astar.GetNodesTouched(), env2.GetPathLength(abstractPath), totalTime);
if (abstractPath.size() == 0)
{
printf("%llu\t%llu\t%llu\t%llu\t%1.2f\t%f\t", (uint64_t)0, (uint64_t)0, astar.GetNodesExpanded(), astar.GetNodesTouched(), 0.0, 0.0);
printf("%llu\t%llu\t%1.2f\t%f\t%d\t%d\n", astar.GetNodesExpanded(), astar.GetNodesTouched(), 0.0, 0.0, 0, 0);
// printf("\n");
continue;
}
nodesExpanded += astar.GetNodesExpanded();
nodesTouched += astar.GetNodesTouched();
env1.SetPlanningCorridor(abstractPath, 2);
gs1 = s1->GetNum();
gs2 = g1->GetNum();
t.StartTimer();
astar.GetPath(&env1, gs1, gs2, thePath);
t.EndTimer();
printf("%llu\t%llu\t%llu\t%llu\t%1.2f\t%f\t",
astar.GetNodesExpanded(), astar.GetNodesTouched(),
astar.GetNodesExpanded()+nodesExpanded, astar.GetNodesTouched()+nodesTouched,
env1.GetPathLength(thePath), totalTime+t.GetElapsedTime());
int abstractStart = 0;
gs1 = s1->GetNum();
double totalLength = 0;
int refineAmt = 2;
int refinedPathNodes = 0;
do { // not working yet -- fully check!
env1.SetPlanningCorridor(abstractPath, 2, abstractStart);
gs2 = g1->GetNum();
if (abstractPath.size()-abstractStart > refineAmt)
{
env1.SetUseAbstractGoal(true, 2);
gs2 = abstractPath[abstractStart+refineAmt];
}
else {
env1.SetUseAbstractGoal(false, 0);
}
t.StartTimer();
astar.GetPath(&env1, gs1, gs2, thePath);
t.EndTimer();
refinedPathNodes += thePath.size();
totalTime+=t.GetElapsedTime();
abstractStart += refineAmt;
gs1 = thePath.back();
nodesExpanded += astar.GetNodesExpanded();
nodesTouched += astar.GetNodesTouched();
totalLength += env1.GetPathLength(thePath);
if (thePath.back() == gs2)
break;
} while (thePath.back() != g1->GetNum());
// printf("%llu\t%llu\t%1.2f\t", astar.GetNodesExpanded(), astar.GetNodesTouched(), env1.GetPathLength(thePath));
thePath.resize(0);
printf("%llu\t%llu\t%1.2f\t%f\t%d\t%d\n", nodesExpanded, nodesTouched, totalLength, totalTime, abstractPath.size(), refinedPathNodes);
// gs1 = s1->GetNum();
// gs2 = g1->GetNum();
// env1.SetPlanningCorridor(abstractPath, 2);
// astar.GetPath(&env1, gs1, gs2, thePath);
// printf("%llu\t%1.2f\n", astar.GetNodesExpanded(), env1.GetPathLength(thePath));
}
fclose(f);
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 MapLineAbstraction::addNodes(Graph *g)
{
int count = abstractions.back()->GetNumNodes();
//printf("Initial count: %d\n", count);
int xstep = pow(lineDistance,((abstractions.size()+1)/2));
int ystep = pow(lineDistance,((abstractions.size())/2));
int zstep = pow(lineDistance,((abstractions.size()-1)/2));
Graph *toAbstract = abstractions.back();
//printf("Size: %d.[%d] XStep: %d, YStep: %d, ZStep %d\n", abstractions.size(), lineDistance, xstep, ystep, zstep);
if (abstractUniformly)
{
for (int x = 0; x < GetMap()->GetMapWidth(); x += xstep)
{
for (int y = 0; y < GetMap()->GetMapHeight(); y+= ystep)
{
//printf("Next check starts from (%d, %d)\n", x, y);
std::vector<node *> nodes;
std::vector<node *> parents;
for (int z = 0; z < lineDistance; z++)
{
if ((abstractions.size()%2) != 0) // vertical abstraction
{
//printf("->(%d, %d)\n", x+z*zstep, y);
nodes.push_back(GetNodeFromMap(x+z*zstep, y));
}
else {
//printf("->(%d, %d)\n", x, y+z*zstep);
nodes.push_back(GetNodeFromMap(x, y+z*zstep));
}
if (nodes.back() == 0)
{
//printf("(null)\n");
nodes.resize(0);
break;
}
parents.push_back(GetNthParent(nodes.back(), abstractions.size()-1));
if ((parents.back() == 0) ||
(parents.back()->GetLabelL(kParent) != -1) ||
((z > 0) && (!toAbstract->FindEdge(parents[z]->GetNum(), parents[z-1]->GetNum()))))
{
//if (parents.back() == 0) printf("(null)\n");
//else if (parents.back()->GetLabelL(kParent) != -1) printf("(parent)\n");
//else printf("(disconnected)\n");
parents.resize(0);
break;
}
}
if ((nodes.size() == 0) || (parents.size() == 0))
continue;
//printf("Match!\n");
node *parent = createParent(g, parents[0]);
//printf("-->Creating parent (%d)\n", parent->GetNum());
for (unsigned int z = 0; z < parents.size(); z++)
{
if (parents[z]->GetLabelL(kParent) != -1)
break;
buildNodeIntoParent(parents[z], parent);
count--;
//printf("Abstracting %d, count now %d\n", parents[z]->GetNum(), count);
}
}
}
}
// node_iterator ni = toAbstract->getNodeIter();
// for (node *n = toAbstract->nodeIterNext(ni); n; n = toAbstract->nodeIterNext(ni))
std::vector<node *> stopList; // nodes with no parents aren't abstracted
while (count > 0)
{
// select a random node
node *n = abstractions.back()->GetRandomNode();
assert(n!=NULL);
if ((n->GetLabelL(kParent) == -1) && (n->GetNumEdges() != 0))
{
node *parent = createParent(g, n);
//printf("==>Creating parent (%d)\n", parent->GetNum());
buildNodeIntoParent(n, parent);
count -= 1;
//printf("Abstracting %d, count now %d\n", n->GetNum(), count);
for (int x = 0; x < lineDistance - 1; x++)
{
node *choice = 0;
double prob = 0;
neighbor_iterator nbi = n->getNeighborIter();
for (long tmp = n->nodeNeighborNext(nbi); tmp != -1; tmp = n->nodeNeighborNext(nbi))
{
node *neighbor = toAbstract->GetNode(tmp);
if (neighbor->GetLabelL(kParent) == -1)
{
prob++;
if ((random()%100) < 100.0/prob)
{
choice = neighbor;
}
}
}
if (choice)
{
buildNodeIntoParent(choice, parent);
count -= 1;
n = choice;
//printf("Abstracting %d, count now %d\n", n->GetNum(), count);
}
else {
break;
}
}
}
else if (n->GetNumEdges() == 0)
{
if ((n->key < stopList.size()) && (stopList[n->key] == n))
{
//printf("Already Removed %d from consideration\n", n->GetNum());
}
else {
//printf("Removing %d from consideration\n", n->GetNum());
count--;
n->key = stopList.size();
stopList.push_back(n);
//printf("Abstracting [%d], count now %d\n", n->GetNum(), count);
}
}
}
}
Graph *BuildInconsistentGraph(int d, int w, int h, int c1, int c2)
{
//int d = 2;
gFrom = d*w*h-1;
gTo = 0;
Graph *g = new Graph();
int cost[d][w][h];
int index[d][w][h];
for (int z = 0; z < d; z++)
{
for (int x = 0; x < w; x++)
{
for (int y = 0; y < h; y++)
{
node *n;
cost[z][x][y] = MAXINT;
index[z][x][y] = g->AddNode(n = new node(""));
n->SetLabelF(GraphSearchConstants::kXCoordinate, -1.0+2.0*(double)x/(double)(w-1.0));
n->SetLabelF(GraphSearchConstants::kYCoordinate, -1.0+2.0*(double)y/(double)(h-1.0));
if (d > 1)
n->SetLabelF(GraphSearchConstants::kZCoordinate, -1.0+2.0*(double)z/(double)(d-1.0));
else
n->SetLabelF(GraphSearchConstants::kZCoordinate, 0);
}
}
}
for (int z = 0; z < d; z++)
{
for (int x = 0; x < w; x++)
{
for (int y = 0; y < h; y++)
{
if ((x == 0) && (y == 0) && (z == 0))
{
if (d > 1)
g->AddEdge(new edge(index[z+1][x][y], index[z][x][y], c2));
if (w > 1)
g->AddEdge(new edge(index[z][x+1][y], index[z][x][y], c2));
g->AddEdge(new edge(index[z][x][y+1], index[z][x][y], c2));
continue;
}
if (z+1 <d)
g->AddEdge(new edge(index[z+1][x][y], index[z][x][y], 1+random()%c1));
if (x+1 <w)
g->AddEdge(new edge(index[z][x+1][y], index[z][x][y], 1+random()%c1));
if (y+1 < h)
g->AddEdge(new edge(index[z][x][y+1], index[z][x][y], 1+random()%c1));
}
}
}
cost[0][0][0] = 0;
while (1)
{
int changes = 0;
for (int z = 0; z < d; z++)
{
for (int x = 0; x < w; x++)
{
for (int y = 0; y < h; y++)
{
if (z+1 <d)
{
int from1 = index[z][x][y];
int to1 = index[z+1][x][y];
edge *e = g->FindEdge(from1, to1);
if (cost[z+1][x][y] > cost[z][x][y] + e->GetWeight())
{
cost[z+1][x][y] = cost[z][x][y] + g->FindEdge(index[z][x][y], index[z+1][x][y])->GetWeight();
changes++;
}
}
if (x+1 <w)
{
if (cost[z][x+1][y] > cost[z][x][y] + g->FindEdge(index[z][x][y], index[z][x+1][y])->GetWeight())
{
cost[z][x+1][y] = cost[z][x][y] + g->FindEdge(index[z][x][y], index[z][x+1][y])->GetWeight();
changes++;
}
}
if (y+1 < h)
{
if (cost[z][x][y+1] > cost[z][x][y] + g->FindEdge(index[z][x][y], index[z][x][y+1])->GetWeight())
{
cost[z][x][y+1] = cost[z][x][y] + g->FindEdge(index[z][x][y], index[z][x][y+1])->GetWeight();
changes++;
}
}
}
}
}
if (changes == 0)
break;
}
for (int z = 0; z < d; z++)
{
for (int x = 0; x < w; x++)
{
for (int y = 0; y < h; y++)
{
//printf("cost of node %d (%d, %d) is %d\n", x, y, index[x][y], cost[x][y]);
node *n = g->GetNode(index[z][x][y]);
if (cost[z][x][y] == 0)
n->SetLabelF(GraphSearchConstants::kHCost, 0);
else {
//int val = ((random()%4) == 0)?(random()%(cost[z][x][y]+1)):0;
int val = random()%(cost[z][x][y]+1);
//printf("Assigning h = %d\n", val);
n->SetLabelF(GraphSearchConstants::kHCost, (double)val);
}
}
}
}
return g;
}
