int main(int argc, char* argv[])
{
int nang, N;
float dx, dz;
int nx, nz;
float ox, oz;
int gnx, gnz;
float gdx, gdz, gox, goz;
struct point length;
int inter;
float TETAMAX, alpha2;
FILE *outfile;
int ii;
int rays, wfront, gap;
int lomx, first;
int nr, nrmax, nt;
int prcube, pr, ste;
int ns, nou;
float DSmax, dt, T, freq;
float *vel;
float ds, os, goox;
float xmin, xmax, zmin, zmax;
float depth;
struct point *pos;
struct heptagon *cube;
struct grid *out;
sf_file inp, ampl, time;
sf_init(argc,argv);
inp = sf_input("in");
/* GET MODEL PARAMETERS */
if (!sf_histint(inp,"n1",&nz)) sf_error("No n1= in input");
if (!sf_histint(inp,"n2",&nx)) sf_error("No n2= in input");
if (!sf_histfloat(inp,"d1",&dz)) sf_error("No d1= in input");
if (!sf_histfloat(inp,"d2",&dx)) sf_error("No d2= in input");
if (!sf_histfloat(inp,"o1",&oz)) sf_error("No o1= in input");
if (!sf_histfloat(inp,"o2",&ox)) sf_error("No o2= in input");
/* GET TRACING PARAMETERS */
if(!sf_getint("nang",&nang)) nang = 10; /* Number of take-off angles */
if(!sf_getint("rays",&rays)) rays = 0; /* If draw rays */
if(!sf_getint("wfront",&wfront)) wfront = 0; /* If draw wavefronts */
if(!sf_getint("gap",&gap)) gap = 1; /* Draw wavefronts every gap intervals */
if(!sf_getint("inter",&inter)) inter = 1; /* If use linear interpolation */
if(!sf_getfloat("DSmax",&DSmax)) DSmax = 5; /* Maximum distance between contiguos points of a wavefront */
if(!sf_getfloat("dt",&dt)) dt = 0.0005; /* time step */
if(!sf_getint("nt",&nt)) nt = 5; /* Number of time steps between wavefronts */
if(!sf_getint("nrmax",&nrmax)) nrmax = 2000; /* Maximum number of points that define a wavefront */
if(!sf_getint("lomx",&lomx)) lomx = 1; /* Use Lomax's waveray method */
if(!sf_getint("first",&first)) first = 1; /* Obtain first arrivals only */
if(!sf_getint("nou",&nou)) nou = 6;
/* GET GRIDDING PARAMETERS */
if(!sf_getint("gnx",&gnx)) gnx = nx; /* Coordinates of output grid */
if(!sf_getint("gnz",&gnz)) gnz = nz;
if(!sf_getfloat("gdx",&gdx)) gdx = dx;
if(!sf_getfloat("gdz",&gdz)) gdz = dz;
if(!sf_getfloat("gox",&goox)) goox = ox;
if(!sf_getfloat("goz",&goz)) goz = oz;
/* GET LOMAX SPECIFIC PARAMETERS */
if(!sf_getint("N",&N)) N = 3; /* Number of control points */
if(!sf_getfloat("TETAMAX",&TETAMAX)) TETAMAX = 1.5; /* Truncation parameter */
if(!sf_getfloat("alpha2",&alpha2)) alpha2 = 4.0; /* Width of gaussian weighting function */
if(!sf_getfloat("freq",&freq)) freq = 100.; /* Pseudo-frequency of waverays */
/* GET DEBUGGING INFO */
if(!sf_getint("prcube",&prcube)) prcube=0; /* For debugging porpouses */
if(!sf_getint("pr",&pr)) pr=0; /* For debugging porpouses */
/* GET SOURCE LOCATIONS */
if(!sf_getint("ns",&ns) || ns==0) ns=1; /* Number of source locations */
if(!sf_getfloat("ds",&ds)) ds=1.; /* interval between sources */
if(!sf_getfloat("os",&os)) os=0.; /* first source location */
if(!sf_getfloat("depth",&depth)) depth=dz; /* Depth location of sources */
pos = (struct point *) sf_alloc (ns,sizeof(struct point));
for(ii=0;ii<ns;ii++) {
pos[ii] = makepoint(ii*ds + os, depth);
}
/* PREPARE OUTPUT */
ampl = sf_output("ampl");
sf_putint(ampl,"n1",gnz);
sf_putint(ampl,"n2",gnx);
sf_putint(ampl,"n3",ns);
sf_putfloat(ampl,"d1",gdz);
sf_putfloat(ampl,"d2",gdx);
sf_putfloat(ampl,"d3",ds);
sf_putfloat(ampl,"o1",goz);
sf_putfloat(ampl,"o2",goox);
sf_putfloat(ampl,"o3",os);
time = sf_output("time");
sf_putint(time,"n1",gnz);
sf_putint(time,"n2",gnx);
sf_putint(time,"n3",ns);
sf_putfloat(time,"d1",gdz);
sf_putfloat(time,"d2",gdx);
sf_putfloat(time,"d3",ds);
sf_putfloat(time,"o1",goz);
sf_putfloat(time,"o2",goox);
sf_putfloat(time,"o3",os);
/* READ VELOCITY MODEL */
vel = sf_floatalloc(nx*nz+2);
sf_floatread(vel,nx*nz,inp);
/* ALLOCATE MEMORY FOR OUTPUT */
out = (struct grid *) sf_alloc (1,sizeof(struct grid));
out->time = sf_floatalloc (gnx*gnz);
out->ampl = sf_floatalloc (gnx*gnz);
out->flag = sf_intalloc (gnx*gnz);
T = 1. / freq;
length = makepoint((nx-1)*dx,(nz-1)*dz);
cube = (struct heptagon *) sf_alloc (nrmax,sizeof(struct heptagon));
/* FOR DEBUGGING PORPOUSES, PRINT TO FILE */
if(pr||prcube) {
outfile = fopen("junk","w");
} else {
outfile = NULL;
}
/* SET DISPLAY IN ORDER TO SHOW RAYS ON SCREEN */
/* NOTE: THIS PROGRAM USES DIRECT CALLS TO LIB_VPLOT
* TO DRAW THE RAYS AND THE WAVEFRONTS */
if(rays || wfront) {
setgraphics(ox, oz, length.x, length.z);
/*
vp_color(BLUE);
for(ii=0;ii<gnx;ii++) {
vp_umove(ii*gdx+gox, goz); vp_udraw(ii*gdx+gox, (gnz-1)*gdz+goz); }
for(ii=0;ii<gnz;ii++) {
vp_umove(gox, ii*gdz+goz); vp_udraw((gnx-1)*gdx+gox, ii*gdz+goz); }
*/
}
norsar_init(gnx,gnz,
TETAMAX,N,alpha2,inter,
nx,nz,ox,oz,dx,dz,length);
/* ALGORITHM:
* For every source: */
for(ii=0;ii<ns;ii++) {
ste = 0;
gox = goox + pos[ii].x;
sf_warning("\nSource #%d\n", ii);
/* 1.- Construct the inital wavefront */
nr = nang;
initial (pos[ii], cube, vel, dt, nt, T, lomx, nr, out);
gridding_init(gnx,gnz,
gdx,gdz,gox,goz,
outfile);
/* run while the wavefront is not too small */
while (nr > 4) {
ste++;
/* 2.- Propagate wavefront */
wavefront (cube, nr, vel, dt, nt, T, lomx);
if(prcube || pr) {
fprintf(outfile,"\n\nwavefront");
printcube(cube, nr, outfile);
}
/* 3.- Get rid of caustics */
if(first) {
if(ste%2==1) {
caustics_up (cube, 0, nr);
} else {
caustics_down (cube, nr-1, nr);
}
if(prcube || pr) {
fprintf(outfile,"\n\ncaustics");
printcube(cube, nr, outfile);
}
}
/* 4.- Eliminate rays that cross boundaries, defined
by xmin, xmax, zmin, zmax.
Note that the computational grid is a little bigger
than the ouput grid. */
xmin = gox-nou*gdx; xmax = 2*pos[ii].x-gox+nou*gdx;
zmin = oz-nou*gdz; zmax = length.z+oz+nou*gdz;
mark_pts_outofbounds (cube, nr, xmin, xmax, zmin, zmax);
if(prcube) {
fprintf(outfile, "\n\nboundaries");
printcube(cube, nr, outfile);
}
/* 5.- Rearrange cube */
makeup(cube, &nr);
if(nr<4) break;
if(prcube || pr) {
fprintf(outfile, "\n\nmakeup");
printcube(cube, nr, outfile);
}
/* 6.- Calculate amplitudes for new wavefront */
amplitudes (cube, nr);
if(prcube) {
fprintf(outfile, "\n\namplitudes");
printcube(cube, nr, outfile);
}
/* 7.- Draw rays */
if(rays)
draw_rays (cube, nr);
/* 8.- Draw wavefront */
if(wfront && (ste%gap==0))
draw_wavefronts (cube, nr, DSmax);
/* 9.- Parameter estimation at receivers */
gridding (cube, nr, out, DSmax, (ste-1)*nt*dt, vel, first); /* pos[ii]); */
/* 10.- Interpolate new points of wavefront */
interpolation (cube, &nr, nrmax, DSmax); /* 0); */
if(prcube) {
fprintf(outfile,"\n\ninterpolation");
printcube(cube, nr, outfile);
}
/* 11.- Prepare to trace new wavefront */
movwavf (cube, nr);
if(prcube) {
fprintf(outfile,"\n\nmovwavf");
printcube(cube, nr, outfile);
}
if((wfront || rays) && (ste%gap==0)) vp_erase();
}
/* Finally interpolate amplitude and traveltime values to
receivers that has not being covered. */
TwoD_interp (out, gnx, gnz);
sf_floatwrite(out->time, gnx * gnz, time);
sf_floatwrite(out->ampl, gnx * gnz, ampl);
}
if(pr||prcube)
fclose(outfile);
exit(0);
}
// graph where s_i = {pos_D, pos_R, cost}, pointer to parents.
// possible entries (diamond pushes) are added with wavefront
// the shortest path till now is explored further (heap)
// hash table to check if node exists
bool Sokoban::findPath(){
printMap(map_);
std::cout << "Init. finding paths..." << std::endl;
// graph to store the tree of possibilities
graph paths;
// has a solution been found
bool solution_found = false;
// - make needed maps
// heuristic map
std::vector< std::vector<char> > heuristic_map = map_;
wavefront(heuristic_map, goals_);
// printMap(heuristic_map);
// std::cout << std::endl;
// deadlock map
std::vector< std::vector<char> > deadlock_map = map_;
deadlocks(deadlock_map);
// printMap(deadlock_map);
// the next child to consider (lowest cost)
node * cheapest_leaf = nullptr;
// node to add
node nextnode(0, cheapest_leaf, heuristic_map);
nextnode.setRobot(pos_t(robot_.x_, robot_.y_));
nextnode.setDiamonds(diamonds_);
std::cout << "# of Diamonds: " << diamonds_.size() << std::endl;
// add the initial position
paths.createChild(nextnode);
int lastCost = 0;
std::cout << "Exploring paths..." << std::endl;
// run this until the path is found or no paths can be taken
int iterations = 0; // debug thing
while(!solution_found && paths.getNextChild(cheapest_leaf)){
iterations++;
// if(lastCost < cheapest_leaf->getCost() + cheapest_leaf->getHeuristic()){
// lastCost = cheapest_leaf->getCost() + cheapest_leaf->getHeuristic();
// std::cout << "current cost: " << lastCost << ", open-/closed-list size: " << paths.getOpenListSize() << " / " << paths.getClosedListSize() << std::endl;
// }
// check cheapest leaf if it is the solution
solution_found = cheapest_leaf->compSolution(map_);
if(!solution_found){
// generate the key for the element
std::string key = cheapest_leaf->getKey(map_size_.x_);
// check that this node has not already been found, if so, remove it
bool unique_node = paths.nodeUnique(key);
bool valid_node = false;
std::vector< pos_t > * diamonds;
if(unique_node){
paths.addChild(cheapest_leaf, key);
// get diamonds pointer
diamonds = cheapest_leaf->getDiamonds();
// check that the node is valid (all diamonds are in valid positions)
valid_node = validNode(diamonds, deadlock_map);
}
else{
paths.deleteChild(cheapest_leaf);
}
std::vector< std::vector<char> > wf_map;
if(unique_node && valid_node){
wf_map = map_;
// make wf map
for(int d = 0; d < diamonds->size(); d++){
int x = (*diamonds)[d].x_;
int y = (*diamonds)[d].y_;
wf_map[y][x] = MAP_DIAMOND;
}
// make wavefront from pos to see the distance to the diamonds
std::vector< pos_t > robot_position = {*(cheapest_leaf->getRobotPos())};
wavefront(wf_map, robot_position);
// find the diamonds that can be moved
std::vector< node > moves;
possibleMoves(wf_map, diamonds, moves, cheapest_leaf);
// add the moves to the heap (does not take care of states being the same)
for(int newChild = 0; newChild < moves.size(); newChild++){
// update heuristics first
moves[newChild].updateHeuristic(heuristic_map);
// add the child
// paths.createChild(moves[newChild]);
}
paths.createChild(moves);
}
}
else{
// return shortest path
node * parent = cheapest_leaf->getParent();
node * child = cheapest_leaf;
path_ = "";
while(parent != nullptr){
std::vector< pos_t > p_diamonds;
std::vector< pos_t > c_diamonds;
std::vector< std::vector<char> > wf_map;
p_diamonds = *(parent->getDiamonds());
c_diamonds = *(child->getDiamonds());
// set diamonds to walls to generate wfamp to move with
wf_map = map_;
for(int d = 0; d < p_diamonds.size(); d++){
int x = (p_diamonds)[d].x_;
int y = (p_diamonds)[d].y_;
wf_map[y][x] = MAP_DIAMOND;
}
// robot pos
pos_t p_position = *(parent->getRobotPos());
pos_t c_position = *(child->getRobotPos());
std::vector< pos_t > wf_start = {p_position};
wavefront(wf_map, wf_start);
// find the diamond that was moved
int p_d = 0;
int c_d = 0;
while(p_diamonds.size() != 1){
bool somethingremoved = false;
c_d = 0;
while(c_d < c_diamonds.size() && !somethingremoved){
if((p_diamonds)[p_d].y_ == (c_diamonds)[c_d].y_ &&(p_diamonds)[p_d].x_ == (c_diamonds)[c_d].x_){
// remove the diamond
p_diamonds.erase(p_diamonds.begin() + p_d);
c_diamonds.erase(c_diamonds.begin() + c_d);
somethingremoved = true;
}
c_d++;
}
p_d++;
if(p_d >= p_diamonds.size()){
p_d = 0;
}
}
// adjust the start pos to be t push spot
int x = (p_diamonds)[0].x_ - (c_diamonds)[0].x_;
int y = (p_diamonds)[0].y_ - (c_diamonds)[0].y_;
c_position.x_ += x;
c_position.y_ += y;
// find sub path
std::string subpath = "";
findSubPath(subpath,wf_map, p_position, c_position);
// add the final push
if(x == 1 && y == 0){
subpath += "W";
} else if(x == -1 && y == 0){
subpath += "E";
} else if(x == 0 && y == 1){
subpath += "N";
} else if(x == 0 && y == -1){
subpath += "S";
}
// add the path
path_ = subpath + path_;
// find new nodes
child = parent;
parent = child->getParent();
}
std::cout << "Cheapest path length: " << cheapest_leaf->getCost() << std::endl;
}
}
if(!paths.getNextChild(cheapest_leaf)){
std::cout << "No more possible paths left to explore.\n";
}
std::cout << "# of iterations gone through: " << iterations << "\n";
std::cout << "# of elements in closed list: " << paths.getClosedListSize() << "\n";
std::cout << "# of elements in open list: " << paths.getOpenListSize() << "\n";
return solution_found;
}
