int main(int argc, char *argv[])
{
struct Map_info in, out, vis;
struct GModule *module; /* GRASS module for parsing arguments */
struct Option *input, *output; /* The input map */
struct Option *coor, *ovis;
char *mapset;
struct Point *points;
struct Line *lines;
int num_points, num_lines;
int n = 0;
/* initialize GIS environment */
G_gisinit(argv[0]); /* reads grass env, stores program name to G_program_name() */
/* initialize module */
module = G_define_module();
module->keywords = _("vector, path, visibility");
module->description = _("Visibility graph construction.");
/* define the arguments needed */
input = G_define_standard_option(G_OPT_V_INPUT);
output = G_define_standard_option(G_OPT_V_OUTPUT);
coor = G_define_option();
coor->key = "coordinate";
coor->key_desc = "x,y";
coor->type = TYPE_STRING;
coor->required = NO;
coor->multiple = YES;
coor->description = _("One or more coordinates");
ovis = G_define_option();
ovis->key = "vis";
ovis->type = TYPE_STRING;
ovis->required = NO;
ovis->description = _("Add points after computing the vis graph");
/* options and flags parser */
if (G_parser(argc, argv))
exit(EXIT_FAILURE);
Vect_check_input_output_name(input->answer, output->answer,
GV_FATAL_EXIT);
Vect_set_open_level(2);
mapset = G_find_vector2(input->answer, NULL); /* finds the map */
if (mapset == NULL)
G_fatal_error("Vector map <%s> not found", input->answer);
if (Vect_open_old(&in, input->answer, mapset) < 1) /* opens the map */
G_fatal_error(_("Unable to open vector map <%s>"),
G_fully_qualified_name(input->answer, mapset));
if (Vect_open_new(&out, output->answer, WITHOUT_Z) < 0) {
Vect_close(&in);
G_fatal_error(_("Unable to create vector map <%s>"), output->answer);
}
if (ovis->answer != NULL) {
mapset = G_find_vector2(ovis->answer, NULL);
if (Vect_open_old(&vis, ovis->answer, mapset) < 1)
G_fatal_error(_("Unable to open vector map <%s>"),
G_fully_qualified_name(ovis->answer, mapset));
if (Vect_copy_map_lines(&vis, &out) > 0)
G_fatal_error(_("Unable to copy elements from vector map <%s>"),
G_fully_qualified_name(ovis->answer, mapset));
}
if (G_projection() == PROJECTION_LL)
G_warning(_("Lat-long projection"));
/* counting how many points and lines we have to allocate */
count(&in, &num_points, &num_lines);
/* modify the number if we have new points to add */
if (coor->answers != NULL)
num_points += count_new(coor->answers);
/* and allocate */
points = G_malloc(num_points * sizeof(struct Point));
lines = G_malloc(num_lines * sizeof(struct Line));
/* and finally set the lines */
load_lines(&in, &points, &num_points, &lines, &num_lines);
if (coor->answers != NULL)
add_points(coor->answers, &points, &num_points);
if (ovis->answer == NULL)
construct_visibility(points, num_points, lines, num_lines, &out);
else
visibility_points(points, num_points, lines, num_lines, &out, n);
G_free(points);
G_free(lines);
Vect_build(&out);
Vect_close(&out);
Vect_close(&in);
exit(EXIT_SUCCESS);
}
int main(int argc, char *argv[])
{
struct Map_info in, out, vis;
struct GModule *module; /* GRASS module for parsing arguments */
struct Option *input, *output; /* The input map */
struct Option *coor, *ovis;
struct Point *points;
struct Line *lines;
int num_points, num_lines;
int n = 0;
/* initialize GIS environment */
G_gisinit(argv[0]); /* reads grass env, stores program name to G_program_name() */
/* initialize module */
module = G_define_module();
G_add_keyword(_("vector"));
G_add_keyword(_("network"));
G_add_keyword(_("shortest path"));
G_add_keyword(_("visibility"));
module->description = _("Performs visibility graph construction.");
/* define the arguments needed */
input = G_define_standard_option(G_OPT_V_INPUT);
output = G_define_standard_option(G_OPT_V_OUTPUT);
coor = G_define_standard_option(G_OPT_M_COORDS);
ovis = G_define_standard_option(G_OPT_V_MAP);
ovis->key = "visibility";
ovis->required = NO;
ovis->label = _("Name of input vector map containing visible points");
ovis->description = _("Add points after computing the visibility graph");
/* options and flags parser */
if (G_parser(argc, argv))
exit(EXIT_FAILURE);
Vect_check_input_output_name(input->answer, output->answer,
G_FATAL_EXIT);
Vect_set_open_level(2);
if (Vect_open_old(&in, input->answer, "") < 1) /* opens the map */
G_fatal_error(_("Unable to open vector map <%s>"), input->answer);
if (Vect_open_new(&out, output->answer, WITHOUT_Z) < 0) {
Vect_close(&in);
G_fatal_error(_("Unable to create vector map <%s>"), output->answer);
}
if (ovis->answer != NULL) {
if (Vect_open_old(&vis, ovis->answer, "") < 1)
G_fatal_error(_("Unable to open vector map <%s>"), ovis->answer);
if (Vect_copy_map_lines(&vis, &out) > 0)
G_fatal_error(_("Unable to copy elements from vector map <%s>"),
ovis->answer);
}
if (G_projection() == PROJECTION_LL)
G_warning(_("Lat-long projection"));
/* counting how many points and lines we have to allocate */
count(&in, &num_points, &num_lines);
/* modify the number if we have new points to add */
if (coor->answers != NULL)
num_points += count_new(coor->answers);
/* and allocate */
points = G_malloc(num_points * sizeof(struct Point));
lines = G_malloc(num_lines * sizeof(struct Line));
/* and finally set the lines */
load_lines(&in, &points, &num_points, &lines, &num_lines);
if (coor->answers != NULL)
add_points(coor->answers, &points, &num_points);
if (ovis->answer == NULL)
construct_visibility(points, num_points, lines, num_lines, &out);
else
visibility_points(points, num_points, lines, num_lines, &out, n);
G_free(points);
G_free(lines);
Vect_copy_head_data(&in, &out);
Vect_hist_copy(&in, &out);
Vect_hist_command(&out);
Vect_build(&out);
Vect_close(&out);
Vect_close(&in);
exit(EXIT_SUCCESS);
}
int main( int argc, char **argv) {
int i, len, sitem, mitem90, mitem99, mitemcpl, gitem, cplus;
if (argc < 2 || argc > 4)
usage();
len = strlen( argv[ 1]);
for (i = 0; i < len; i++) {
if (! isdigit( argv[ 1][ i]))
usage();
}
if ((cases = atoi( argv[ 1])) > MAX_CASES)
usage();
if (argc > 2)
if (freopen( argv[ 2], "r", stdin) == NULL)
usage();
if (argc == 4)
if (freopen( argv[ 3], "w", stdout) == NULL)
usage();
sitem = mitem90 = mitem99 = mitemcpl = gitem = cplus = 0;
while (fgets( buf, MAX_LLEN, stdin) != NULL) {
if (isalpha( buf[ 0]) && buf[ 1] == '.' && isdigit( buf[ 2])) {
add_points( C90);
sitem++;
} else if (isalpha( buf[ 0]) && buf[ 1] == '.' && isalpha( buf[ 2])) {
if (cplus) {
add_points( CPL);
mitemcpl++;
} else {
add_points( C99);
mitem99++;
}
} else if (memcmp( buf, "stotal", 6) == 0) {
put_subtotal();
fprintf( stderr, "sitem:%d\n", sitem);
mitem90 += sitem;
sitem = 0;
} else if (memcmp( buf, "mttl90", 6) == 0) {
put_mttl( C90);
fprintf( stderr, " mitem90:%d\n", mitem90);
gitem += mitem90;
mitem90 = 0;
} else if (memcmp( buf, "mttl99", 6) == 0) {
put_mttl( C99);
fprintf( stderr, " mitem99:%d\n", mitem99);
gitem += mitem99;
mitem99 = 0;
} else if (memcmp( buf, "mttl++", 6) == 0) {
put_mttl( CPL);
fprintf( stderr, " mitem++:%d\n", mitemcpl);
gitem += mitemcpl;
mitemcpl = cplus = 0;
} else if (memcmp( buf, "gtotal", 6) == 0) {
put_grandtotal();
fprintf( stderr, " gitem:%d\n", gitem);
} else if (memcmp( buf, "[C++:", 5) == 0) {
cplus = 1;
}
/* Else as it is. */
fputs( buf, stdout);
}
return 0;
}
int AStar::generate_path(Node start, Node goal)
{
start_ = start;
goal_ = goal;
// Ensure that start node is in map
if (!map_->inMap(start_.point())) {
return -3;
}
// Ensure that goal node is in map
if (!map_->inMap(goal_.point())) {
return -4;
}
//// Get the pointer to the starting node
Node *start_ptr;
start_ptr = node_map_[start_.point().x][start_.point().y];
// Add starting node to the open list
start_ptr->set_list(Node::Open);
open_.push_back(start_ptr);
bool goal_reached = false;
do {
// If the open list is empty, we didn't find the goal node
if (open_.empty()) {
goal_reached = false;
break;
}
// Grab the first item off the list (list is sorted by F cost)
Node *cur_node = open_.front();
open_.pop_front();
// Switch the lowest cost F node from the open list
// to the closed list;
cur_node->set_list(Node::Closed);
closed_.push_back(cur_node);
// Was the last node added to the closed list the goal node?
if (goal_.point() == cur_node->point()) {
goal_reached = true;
break;
}
// Loop through all directions
std::vector<Direction>::iterator dir_it;
for (dir_it = directions_.begin(); dir_it != directions_.end(); dir_it++) {
Point<int> point;
Node *adj_node;
point = cur_node->point() + dir_it->point();
// Check to see if point is within the map boundary
if (!map_->inMap(point)) {
continue;
}
adj_node = node_map_[point.x][point.y];
if (map_->at(point) > 50 || adj_node->list() == Node::Closed) {
// ignore the node (not walkable, in the closed list)
continue;
}
if (adj_node->list() != Node::Open) {
adj_node->set_list(Node::Open);
adj_node->set_parent(cur_node, dir_it->dir(), dir_it->cost());
adj_node->compute_costs(cur_node->point());
// Add the node into the sorted open list
bool node_inserted = false;
std::list<Node*>::iterator it;
for (it = open_.begin(); it != open_.end(); it++) {
if (*adj_node < **it) {
open_.insert(it, adj_node);
node_inserted = true;
break;
}
}
// If the node wasn't inserted in the middle of the
// list, add it to the back
if (!node_inserted) {
open_.push_back(adj_node);
}
} else {
// Node is already on open list, check to see if
// this path is lower cost, resort
if ((cur_node->g() + dir_it->cost()) < adj_node->g()) {
adj_node->set_parent(cur_node, dir_it->dir(), dir_it->cost());
adj_node->compute_costs(cur_node->point());
open_.sort();
}
}
}
} while(true);
if (goal_reached) {
// Generate the path by walking backwards from the goal node
// to the start node
Node * node = node_map_[goal_.point().x][goal_.point().y];
do {
path_.push_front(node);
node = node->parent();
} while(node->point() != start_.point());
// Compute decomposed waypoints
// Add the starting position
waypts_.push_back(path_.front());
Point<double> mu;
Point<double> var(0,0);
double alpha = 0.5;
bool first = true;
Point<int> prev;
std::list<syllo::Node*>::iterator it;
// Get the first vector (requires first two points
it = path_.begin();
Point<double> prev_double((*it)->point().x, (*it)->point().y);
it++;
Point<double> cur_double((*it)->point().x, (*it)->point().y);
mu = cur_double - prev_double;
for (; it != path_.end(); it++) {
Point<int> vel = (*it)->point() - prev;
mu = add_points(mu*(alpha), vel*(1-alpha));
Point<double> diff = sub_points(vel, mu);
diff = diff.absolute();
var = var*(alpha) + diff*diff*(1-alpha);
bool change = false;
double k = 0.2;
cout << "---------------" << endl;
cout << "Diff: " << abs(vel.x - mu.x) << endl;
cout << "Sqrt: " << sqrt(var.x) << endl;
if ( (abs(vel.x - mu.x) > k+sqrt(var.x)) || (abs(vel.y - mu.y) > k+sqrt(var.y))) {
waypts_.push_back(*it);
change = true;
var = Point<double>(0,0);
Point<double> prev_double((*it)->point().x, (*it)->point().y);
it++;
if (it != path_.end()) {
Point<double> cur_double((*it)->point().x, (*it)->point().y);
mu = cur_double - prev_double;
}
}
prev = (*it)->point();
}
// Add the last waypoint:
waypts_.push_back(path_.back());
return 0;
} else {
return -1;
}
}
