static void
ps_begin_page(point page, double scale, int rot, point offset)
{
point sz;
Cur_page++;
sz = sub_points(PB.UR,PB.LL);
fprintf(Outfile,"%%%%Page: %d %d\n",Cur_page,Cur_page);
fprintf(Outfile,"%%%%PageBoundingBox: %d %d %d %d\n",
PB.LL.x,PB.LL.y,PB.UR.x+1,PB.UR.y+1);
fprintf(Outfile,"%%%%PageOrientation: %s\n",(rot?"Landscape":"Portrait"));
fprintf(Outfile,"gsave\n%d %d %d %d boxprim clip newpath\n",
PB.LL.x-1, PB.LL.y-1, sz.x + 2, sz.y + 2);
fprintf(Outfile,"%d %d translate\n",PB.LL.x,PB.LL.y);
if (rot) fprintf(Outfile,"gsave %d %d translate %d rotate\n",
PB.UR.x-PB.LL.x,0,rot);
fprintf(Outfile,"%d %d %d beginpage\n",page.x,page.y,N_pages);
if (rot) fprintf(Outfile,"grestore\n");
if (scale != 1.0) fprintf(Outfile,"%.4f set_scale\n",scale);
fprintf(Outfile,"%d %d translate %d rotate\n",offset.x,offset.y,rot);
assert(SP == 0);
S[SP].font = S[SP].color = ""; S[SP].size = 0.0;
/* Define the size of the PS canvas */
if (PB.UR.x >= PDFMAX || PB.UR.y >= PDFMAX)
fprintf(stderr,
"dot: warning, canvas size (%d,%d) exceeds PDF limit (%d)\n"
"\t(suggest setting a bounding box size, see dot(1))\n",
PB.UR.x, PB.UR.y, PDFMAX);
fprintf(Outfile,"[ /CropBox [%d %d %d %d] /PAGES pdfmark\n",
PB.LL.x, PB.LL.y, PB.UR.x+1, PB.UR.y+1);
}
/*cluster data*/
void cluster_init(std::vector<std::vector<double> > &points, int dimension, double lower_bound, double upper_bound, double sigma)
{
int center_num = 10;
int each_num = points.size() / center_num;
int current_index = 0;
for(int i = 0; i < center_num; i++){
double mu = get_rand(0.2,0.8);
std::vector<std::vector<double> > sub_points(each_num);
normal_init(sub_points,dimension,mu,sigma,lower_bound,upper_bound);
for(int j = 0; j < sub_points.size(); j++){
points[current_index] = sub_points[j];
current_index++;
}
}
}
void osize_label(textlabel_t * label, int *b, int *t, int *l, int *r)
{
point pt, sz2;
pointf dimen;
dimen = label->dimen;
PAD(dimen);
sz2 = cvt2pt(label->dimen);
sz2.x /= 2;
sz2.y /= 2;
pt = add_points(label->p, sz2);
if (*r < pt.x)
*r = pt.x;
if (*t < pt.y)
*t = pt.y;
pt = sub_points(label->p, sz2);
if (*l > pt.x)
*l = pt.x;
if (*b > pt.y)
*b = pt.y;
}
/* compute_bb:
* Compute bounding box of g using nodes, splines, and clusters.
* Assumes bb of clusters already computed.
* store in GD_bb.
*/
void compute_bb(graph_t * g)
{
node_t *n;
edge_t *e;
box b, bb;
point pt, s2;
int i, j;
bb.LL = pointof(MAXINT, MAXINT);
bb.UR = pointof(-MAXINT, -MAXINT);
for (n = agfstnode(g); n; n = agnxtnode(g, n)) {
pt = coord(n);
s2.x = ND_xsize(n) / 2 + 1;
s2.y = ND_ysize(n) / 2 + 1;
b.LL = sub_points(pt, s2);
b.UR = add_points(pt, s2);
EXPANDBB(bb,b);
for (e = agfstout(g, n); e; e = agnxtout(g, e)) {
if (ED_spl(e) == 0)
continue;
for (i = 0; i < ED_spl(e)->size; i++) {
for (j = 0; j < ED_spl(e)->list[i].size; j++) {
pt = ED_spl(e)->list[i].list[j];
EXPANDBP(bb,pt);
}
}
if (ED_label(e) && ED_label(e)->set)
bb = addLabelBB(bb, ED_label(e), GD_flip(g));
}
}
for (i = 1; i <= GD_n_cluster(g); i++) {
EXPANDBB(bb,GD_clust(g)[i]->u.bb);
}
GD_bb(g) = bb;
}
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;
}
}
