int main (int argc, char const *argv[]){
matrix2d m = FileManip::readDouble("C:/Users/Carrie/Documents/Visual Studio 2012/Projects/projects/IROS2015/IROS2015/agent_map/obstacle_map.csv");
matrix2d mems = FileManip::readDouble("C:/Users/Carrie/Documents/Visual Studio 2012/Projects/projects/IROS2015/IROS2015/agent_map/membership_map.csv");
matrix2d conns = FileManip::readDouble("C:/Users/Carrie/Documents/Visual Studio 2012/Projects/projects/IROS2015/IROS2015/agent_map/connections.csv");
matrix2d agentxy = FileManip::readDouble("C:/Users/Carrie/Documents/Visual Studio 2012/Projects/projects/IROS2015/IROS2015/agent_map/agent_map.csv");
Matrix<bool,2> * obstacles = new Matrix<bool,2>(m.size(),m[0].size());
Matrix<int, 2> * members = new Matrix<int,2>(mems.size(),mems[0].size());
Matrix<bool,2> * connections = new Matrix<bool,2>(conns.size(), conns[0].size());
for (int i=0; i<conns.size(); i++){
for (int j=0; j<conns[0].size(); j++){
(*connections)(i,j) = conns[i][j];
}
}
for (int i=0; i<m.size(); i++){
for (int j=0; j<m[i].size(); j++){
obstacles->row(i)[j] = m[i][j]>0.0;
}
}
for (int i=0; i<mems.size(); i++){
for(int j=0; j<mems[i].size(); j++){
members->row(i)[j] = mems[i][j];
}
}
grid_lookup masks = getMasks(obstacles,connections,members);
system("pause");
AStar_grid a(obstacles);
/*XY source(140,35);
XY goal(180,56);
int msource = (*members)(source.x,source.y);
int mgoal = (*members)(goal.x,goal.y);
std::vector<XY> fullpath = masks[msource][mgoal]->m.get_solution_path(source,goal);
//std::vector<XY> fullpath = a.m.get_solution_path(source, goal);
//int dist = a.m.solve(source.x,source.y,goal.x,goal.y);
printf("Dist=%i",fullpath.size());
system("pause");
/*
a.m.solve(0,29,agentxy[0][0],agentxy[0][1]);
std::cout << "Soln length" << a.m.m_solution_length << std::endl;
std::cout << a.m << std::endl;
system("pause");
exit(1);
/**/
matrix2d voronoi_mems(obstacles->dim1());
for (int i=0; i<voronoi_mems.size(); i++){
voronoi_mems[i] = matrix1d(obstacles->dim2());
}
for (int i=0; i<agentxy.size(); i++){
if ((*obstacles)(agentxy[i][0],agentxy[i][1])){
printf("found one! (obstacles)");
}
mt::vertex_descriptor u = {{agentxy[i][0],agentxy[i][1]}};
if (a.m.has_barrier(u)){
printf("found one! (barrier grid)");
}
}
std::system("pause");
for (int y=0; y<obstacles->dim2(); y++){
for (int x=0; x<obstacles->dim1(); x++){
if(!((*obstacles)(x,y))){
printf("(%i,%i),",x,y);
std::vector<double> dist(agentxy.size(),DBL_MAX);
for (int k=0; k<agentxy.size(); k++){
int dx = agentxy[k][0]-x;
int dy = agentxy[k][1]-y;
double dmin = *min_element(dist.begin(),dist.end()); // Get current shortest path to xy
if (dmin<sqrt(dx*dx+dy*dy)) continue; // Compare to shortest possible from current agent
a.m.m_solution.clear();
dist[k] = a.m.solve(x,y,agentxy[k][0],agentxy[k][1]);
// cout << a.m << endl;
}
// find the shortest distance
std::vector<double>::iterator shortest_dist = min_element(dist.begin(),dist.end());
if (*shortest_dist == DBL_MAX){
voronoi_mems[x][y] = -2; // open space, but it can't get to any other sector
} else {
voronoi_mems[x][y] = std::distance(dist.begin(), shortest_dist);
}
printf("%f,%f\n",*shortest_dist,voronoi_mems[x][y]);
}
}
}
PrintOut::toFile2D(voronoi_mems,"voronoi_mems.csv");
//AStar_grid a(obstacles,members,0,1);
// make an A* grid: base
// filter based on given set
/*
std::map<int,std::map<int,AStar_grid> > m2astar;
for (int i=0; i<conns.size(); i++){
for (int j=i+1; j<conns[i].size(); j++){
if (conns[i][j]>0){
m2astar[i].insert(std::make_pair(j,AStar_grid(obstacles,members,i,j)));
}
}
std::cout << i <<std::endl;
}
*/
//printf("%i Astars created!!",m2astar.size());
/*
std::vector<AStar_grid> astars;
for (int i=0; i<1000; i++){
astars.push_back(AStar_grid(obstacles));
std::cout << i << std::endl;
}
*/
// a.m.solve(0,0,1,1);
// The default maze size is 20x10. A different size may be specified on
// the command line.
/*
if (a.m.solve(0,0,1,1))
std::cout << "Solved the maze." << std::endl;
else
std::cout << "The maze is not solvable." << std::endl;
std::cout << a.m << std::endl;*/
system("pause");
return 0;
}
void easymath::zero(matrix2d * m) {
if (m->empty())
return;
else
*m = matrix2d(m->size(), matrix1d(m[0].size(), 0.0));
}
void easymath::zero(matrix1d * m) {
*m = matrix1d(m->size(), 0.0);
}
matrix1d easymath::zeros(size_t dim1) {
return matrix1d(dim1, 0.0);
}
matrix1d zeros(int dim) {
return matrix1d(dim,0.0);
}