void sort_v(t_numbr **a, t_numbr **b)
{
display(a, b);
while ((list_check_a(a)) != 0)
{
if ((*a)->data >= (last_node(a))->data)
{
cmd_ra(a);
display(a, b);
}
if ((*a)->data > (*a)->next->data)
{
cmd_sa(a);
display(a, b);
}
if ((*a)->data <= (last_node(a))->data && ((*a)->data <= (*a)->next->data))
{
cmd_pb(a, b);
display(a, b);
sort_b(a, b);
}
}
while ((*b) != NULL)
{
cmd_pa(a, b);
display(a, b);
}
}
//!Allocates a singly linked list of n nodes ending in null pointer.
multiallocation_chain allocate_nodes(const size_type n)
{
//Preallocate all needed blocks to fulfill the request
size_type cur_nodes = m_freelist.size();
if(cur_nodes < n){
priv_alloc_block(((n - cur_nodes) - 1)/m_nodes_per_block + 1);
}
//We just iterate the needed nodes to get the last we'll erase
typedef typename free_nodes_t::iterator free_iterator;
free_iterator before_last_new_it = m_freelist.before_begin();
for(size_type j = 0; j != n; ++j){
++before_last_new_it;
}
//Cache the first node of the allocated range before erasing
free_iterator first_node(m_freelist.begin());
free_iterator last_node (before_last_new_it);
//Erase the range. Since we already have the distance, this is O(1)
m_freelist.erase_after( m_freelist.before_begin()
, ++free_iterator(before_last_new_it)
, n);
//Now take the last erased node and just splice it in the end
//of the intrusive list that will be traversed by the multialloc iterator.
multiallocation_chain chain;
chain.incorporate_after(chain.before_begin(), &*first_node, &*last_node, n);
m_allocated += n;
return boost::move(chain);
}
/* Remove node *node from queue */
void remove_node(command_queue_t *q, command_queue_node_t *node)
{
if (empty_queue(q))
{
return;
}
command_queue_node_t *current = q->head;
if (node == first_node(q))
{
q->head = node->next;
current = q->head;
}
else
{
while (current->next != node)
{
if (current->next == NULL)
{
/* Node not in queue */
return;
}
current = current->next;
}
current->next = node->next;
}
if (node == last_node(q))
{
q->last = current;
}
}
void input_byte(command_queue_t *q, uint8_t data)
{
int status = 0;
if (empty_queue(q))
{
put_queue(q, new_node());
status = set_node_command(last_node(q), data);
}
else if (!command_recieved(node_data(last_node(q))))
{
status = set_node_command(last_node(q), data);
}
else
{
put_queue(q, new_node());
status = set_node_command(last_node(q), data);
}
}
void sort_v_b(t_numbr **a, t_numbr **b)
{
display(a, b);
while ((list_check_b(b)) != 0)
{
if ((*b)->data <= (last_node(b))->data)
{
cmd_rb(b);
display(a, b);
}
if ((*b)->data < (*b)->next->data)
{
cmd_sb(b);
display(a, b);
}
if ((*b)->data >= (last_node(b))->data && ((*b)->data >= (*b)->next->data))
{
cmd_pa(a, b);
display(a, b);
}
display(a, b);
}
}
EXPORT void set_node_doubly_linked_list(
INTERFACE *intfc)
{
NODE **n;
n = intfc->nodes;
first_node(intfc) = *n;
prev_node(*n) = NULL;
for (; *(n+1); ++n)
{
next_node(*n) = *(n+1);
prev_node(*(n+1)) = (*n);
}
last_node(intfc) = *n;
next_node(*n) = NULL;
} /*end set_node_doubly_linked_list*/
linked_list_node *prime_numbers_less_than(int n) {
int cur = 3;
linked_list_node * head = create_linked_list_node(2);
while ((last_node(head))->data < n) {
if (cur >= n) {
break;
} else if (!list_can_divide_num(head, cur)) {
printf("%d\n", cur);
append_to_list(head, cur);
}
cur ++;
}
return head;
}
void pvs_construct_hint(int selection, int forward, Char *hint)
{
PVSProof *p;
FILE *f;
Char *hintstr;
ProofStep **cstep;
KeywordItem *kwi;
p = pvs_temporary_file(lasthintfile,&f);
copy_selection(&p->selection, get_selection(selection));
{
char lemmabuf[500];
sprintf(lemmabuf, "%s%i", UstrtoLocale(pvs_lemma_name), lastlemmanr);
lastlemmanr++;
p->lemma = strdup(lemmabuf);
fprintf(f,"%s: THEOREM\n\t\t", lemmabuf);
}
cstep = &p->step;
/* add an empty proof step to store the starting expression */
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=0;
(*cstep)->pvsinput=0;
(*cstep)->comment=translate("starting formula");
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->steptype=SkipStep;
(*cstep)->use_result=0;
cstep = &((*cstep)->next);
/* add the initial proof step to start the hints */
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=get_pvskeyword(translate("INITSTEP"));
(*cstep)->pvsinput=0;
(*cstep)->comment=translate("initialisation steps");
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->use_result=0;
(*cstep)->steptype=InitStep;
cstep = &((*cstep)->next);
kwi = get_pvskeyword(hint);
if (kwi) {
if (kwi->induct) {
void *node;
char *str=0;
char *h;
void *upsel;
upsel=0;
copy_selection(&upsel, p->selection);
up_selection(upsel);
tex_set_string(&str);
tex_placeholders(0);
latex_all_parens(MP_True);
tex_mode(ASCII);
node = first_node(upsel);
latex_node(node);
out_latex_char('=');
node = last_node(upsel);
latex_node(node);
tex_unset();
latex_all_parens(MP_False);
destruct_selection(upsel);
/*
** Filter out '+(1)' and '+1'.
** This should be adjusted for allow induction on other types
*/
h=str;
while (h && ((h=strstr(h,"+(1)")))) {
int i;
for (i=0; i<4;i++) h[i]=' ';
h=h+4;
}
h=str;
while (h && ((h=strstr(h,"+1")))) {
int i;
for (i=0; i<2;i++) h[i]=' ';
h=h+2;
}
fprintf(f, "(%s) IMPLIES\n",str);
free(str);
}
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=kwi;
(*cstep)->pvsinput=0;
(*cstep)->comment=0;
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->steptype=KeywordStep;
(*cstep)->use_result=0;
cstep = &((*cstep)->next);
}
{
char *str=0;
tex_set_string(&str);
tex_placeholders(0);
latex_all_parens(MP_True);
tex_mode(ASCII);
latex_selection(selection);
tex_unset();
latex_all_parens(MP_False);
if (using_booleans) {
fprintf(f,"%s\n\n", str);
} else {
fprintf(f, "(%s) = Tresult\n\n", str);
}
}
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=get_pvskeyword(translate("RESULTSTEP"));
(*cstep)->pvsinput=0;
(*cstep)->comment=0;
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->steptype=FinishStep;
(*cstep)->use_result=(forward? 2:1);
cstep = &((*cstep)->next);
fprintf(f, "\nEND %s\n", p->theory);
fclose(f);
lasthintfile++;
pvs_start_proof(p);
}
void pvs_check_hint(int selection)
{
int nr,vnr;
unsigned long uvnr=0;
Char *hintstr;
PVSProof *pvsproof;
ProofStep **cstep;
char *head;
int assumptions=0;
int np=0;
int curlinenr=0;
FILE *f;
char buffer[1024];
/* check if hint is selected */
nr = selected_notation(selection, &vnr);
if (nr>=0) uvnr = which_notation(nnr_vnr2innr(nr,0))->vers[vnr].vnr;
if (nr<0 || uvnr<firsthintnr || uvnr> lasthintnr) {
message(MP_ERROR, translate("No valid hint selected"));
return;
}
pvsproof=pvs_temporary_file(lasthintfile, &f);
{
int pos[2];
copy_selection(&pvsproof->selection, get_selection(selection));
pos[0]=1;pos[1]=0;
change_selection(pvsproof->selection, pos,2);
}
{
char lemmabuf[500];
sprintf(lemmabuf, "%s%i", UstrtoLocale(pvs_lemma_name), lastlemmanr);
lastlemmanr++;
pvsproof->lemma = strdup(lemmabuf);
fprintf(f,"%s: THEOREM\n\t\t", lemmabuf);
curlinenr++;
}
assumptions=0;
cstep = &pvsproof->step;
/* add an empty proof step to store the starting expression */
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=0;
(*cstep)->pvsinput=0;
(*cstep)->comment=translate("starting formula");
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->steptype=SkipStep;
(*cstep)->use_result=0;
cstep = &((*cstep)->next);
/* add the initial proof step to start the hints */
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=get_pvskeyword(translate("INITSTEP"));
(*cstep)->pvsinput=0;
(*cstep)->comment=translate("initialisation steps");
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->use_result=0;
(*cstep)->steptype=InitStep;
cstep = &((*cstep)->next);
/* parse text in hint:
** - expression -> add to assumptions
** - "induction" -> add 'Ef = El' to assumptions
** (Ef: first expr., El: last expr, =: weakest operator)
** add (inst?)(ground)(try-triv-step (replace*)) to proof
** - "name" -> add (modulo-assoc (bidi-rewrite "name")) to proof
** - "definition" -> add (expand-simp* "def1" ... "defn") to proof
** (def1 ... defn extracted from expressions)
** - hidden PVS -> add content to proof
*/
{
int pos[2];
pos[0]=1;
pos[1]=0;
hintstr = get_subnode_string(selection, pos,2);
}
if (!hintstr || !hintstr[0]) {
/* empty hints usually indicate trivial steps */
hintstr = translate("trivial");
}
while (*hintstr) {
if (IsPh(hintstr[0])) {
/* An expression, identifier, operator of text */
void *node;
int pos[3];
char *str=0;
pos[0]=1;
pos[1]=0;
pos[2]=np++;
node = get_subnode(selection, pos, 3);
nr = node_notation(node, &vnr);
if (nr!= -1) uvnr = which_notation(nnr_vnr2innr(nr,0))->vers[0].vnr;
tex_set_string(&str);
tex_placeholders(0);
latex_all_parens(MP_True);
tex_mode(ASCII);
latex_node(node);
tex_unset();
latex_all_parens(MP_False);
if (str && str[0]) {
/* the expression produces output */
if (uvnr==pvshiddenstep) {
/* a special PVS related template to add steps to the proof list */
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=0;
(*cstep)->pvsinput=str;
(*cstep)->comment=translate("hidden proof steps");
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->use_result=0;
(*cstep)->steptype=HiddenStep;
cstep = &((*cstep)->next);
} else {
/* A normal expression. For now, an assumption, but it could be
** an identifier (as in "definition of $fold$") or
** a single operator (as in "$\times$ distributes over $\plus$")
*/
if (!assumptions) {
fprintf(f,"(\t");
} else {
fprintf(f,"\t\t AND ");
}
fprintf(f, "%s\n",str);
curlinenr++;
{
char *h;
for (h=str; *h; h++) { if (*h=='\n') curlinenr++; }
}
free(str);
assumptions++;
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=get_pvskeyword(translate("EXPRESSIONSTEP"));
(*cstep)->pvsinput=0;
(*cstep)->comment=translate("assumption step");
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->use_result=0;
(*cstep)->steptype=ExpressionStep;
cstep = &((*cstep)->next);
}
}
hintstr++;
} else {
KeywordItem *kwi= kwlist;
kwi = get_pvskeyword(hintstr);
if (kwi) {
if (kwi->induct) {
void *node;
char *str=0;
char *h;
tex_set_string(&str);
tex_placeholders(0);
latex_all_parens(MP_True);
tex_mode(ASCII);
node = first_node(get_selection(selection));
latex_node(node);
out_latex_char('=');
node = last_node(get_selection(selection));
latex_node(node);
tex_unset();
latex_all_parens(MP_False);
/*
** Filter out '+(1)' and '+1'.
** This should be adjusted for allow induction on other types
*/
h=str;
while (h && ((h=strstr(h,"+(1)")))) {
int i;
for (i=0; i<4;i++) h[i]=' ';
h=h+4;
}
h=str;
while (h && ((h=strstr(h,"+1")))) {
int i;
for (i=0; i<2;i++) h[i]=' ';
h=h+2;
}
if (!assumptions) {
fprintf(f,"(\t");
} else {
fprintf(f,"\t\t AND ");
}
fprintf(f, "%s\n",str);
curlinenr++;
{
char *nlc;
for (nlc=str; *nlc; nlc++) { if (*nlc=='\n') curlinenr++; }
}
free(str);
assumptions++;
}
if (kwi->step && kwi->step[0]) {
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=kwi;
(*cstep)->pvsinput=0;
(*cstep)->comment=0;
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->use_result=0;
(*cstep)->steptype=KeywordStep;
cstep = &((*cstep)->next);
}
hintstr=hintstr+kwi->len;
}
/* might want to skip to the next place holder/word boundary */
hintstr++;
}
}
if (assumptions) {
fprintf(f,"\t\t) IMPLIES\n\t\t\t");
curlinenr++;
}
/* generate lemma */
{
char *str=0;
tex_set_string(&str);
tex_placeholders(0);
latex_all_parens(MP_True);
tex_mode(ASCII);
latex_selection(selection);
tex_unset();
latex_all_parens(MP_False);
{
char *nlc;
for (nlc=str; *nlc; nlc++) if (*nlc=='\n') curlinenr++;
}
fprintf(f, "%s\n\n", str);
curlinenr += 2;
}
*cstep = malloc(sizeof(ProofStep));
(*cstep)->next=0;
(*cstep)->keyword=get_pvskeyword(translate("FINISHSTEP"));
(*cstep)->pvsinput=0;
(*cstep)->comment=0;
(*cstep)->result=0;
(*cstep)->failed=0;
(*cstep)->use_result=0;
(*cstep)->steptype=FinishStep;
cstep = &((*cstep)->next);
fprintf(f, "\nEND %s\n", pvsproof->theory);
fclose(f);
lasthintfile++;
/* start PVS proof of temporary lemma */
pvs_start_proof(pvsproof);
/* pvs_start_proof starts the proof of the selected hint(s)
** and checks if each step is correct, by sending the step,
** parse the output, check for errors, send next step, etc.
** At the end, Q.E.D. should appear and the
** delivery.
*/
if (pvsproof) {
ProofStep *ps;
for (ps=pvsproof->step; ps; ps=ps->next) {
if (ps->pvsinput) {
string_to_window(translate("PVS Generated Proof"),
LocaletoUstr((unsigned char*)ps->pvsinput));
} else if (ps->keyword && ps->keyword->step) {
string_to_window(translate("PVS Generated Proof"),
LocaletoUstr((unsigned char*)ps->keyword->step));
}
}
}
/* set PVS parse function correct */
}
void OSMDocument::SplitWays()
{
while (!m_Ways.empty()) {
Way* currentWay = m_Ways.back();
std::vector<Node*>::const_iterator it_node( currentWay->m_NodeRefs.begin());
std::vector<Node*>::const_iterator last_node( currentWay->m_NodeRefs.end());
Node* backNode = currentWay->m_NodeRefs.back();
while(it_node!=last_node)
{
Node* node = *it_node++;
Node* secondNode=0;
Node* lastNode=0;
Way* splitted_way = new Way(node->id, ++wayid);
splitted_way->m_attributes = currentWay->m_attributes;
splitted_way->AddNodeRef(node);
bool found=false;
if(it_node!=last_node)
{
while(it_node!=last_node && !found)
{
splitted_way->AddNodeRef(*it_node);
if((*it_node)->numsOfUse>1)
{
found=true;
secondNode = *it_node;
splitted_way->AddNodeRef(secondNode);
}
else if(backNode==(*it_node))
{
lastNode=*it_node++;
splitted_way->AddNodeRef(lastNode);
}
else
{
*it_node++;
}
}
}
if(splitted_way->m_NodeRefs.front()!=splitted_way->m_NodeRefs.back())
m_SplittedWays.push_back(splitted_way);
else
{
delete splitted_way;
splitted_way=0;
}
}
m_Ways.pop_back();
delete currentWay;
}
} // end SplitWays
void OSMDocument::SplitWays()
{
std::vector<Way*>::const_iterator it(m_Ways.begin());
std::vector<Way*>::const_iterator last(m_Ways.end());
//splitted ways get a new ID
long long id=0;
while(it!=last)
{
Way* currentWay = *it++;
std::vector<Node*>::const_iterator it_node( currentWay->m_NodeRefs.begin());
std::vector<Node*>::const_iterator last_node( currentWay->m_NodeRefs.end());
Node* backNode = currentWay->m_NodeRefs.back();
while(it_node!=last_node)
{
Node* node = *it_node++;
Node* secondNode=0;
Node* lastNode=0;
Way* splitted_way = new Way( ++id, currentWay->osmId, currentWay->visible );
splitted_way->name=currentWay->name;
splitted_way->categories=currentWay->categories;
splitted_way->oneway=currentWay->oneway;
splitted_way->geom="LINESTRING("+ boost::lexical_cast<std::string>(node->lon) + " " + boost::lexical_cast<std::string>(node->lat) +",";
splitted_way->AddNodeRef(node);
bool found=false;
if(it_node!=last_node)
{
while(it_node!=last_node && !found)
{
splitted_way->AddNodeRef(*it_node);
if((*it_node)->numsOfUse>1)
{
found=true;
secondNode = *it_node;
splitted_way->AddNodeRef(secondNode);
double length = getLength(node,secondNode);
if(length<0)
length*=-1;
splitted_way->length+=length;
splitted_way->geom+= boost::lexical_cast<std::string>(secondNode->lon) + " " + boost::lexical_cast<std::string>(secondNode->lat) + ")";
}
else if(backNode==(*it_node))
{
lastNode=*it_node++;
splitted_way->AddNodeRef(lastNode);
double length = getLength(node,lastNode);
if(length<0)
length*=-1;
splitted_way->length+=length;
splitted_way->geom+= boost::lexical_cast<std::string>(lastNode->lon) + " " + boost::lexical_cast<std::string>(lastNode->lat) + ")";
}
else
{
splitted_way->geom+= boost::lexical_cast<std::string>((*it_node)->lon) + " " + boost::lexical_cast<std::string>((*it_node)->lat) + ",";
*it_node++;
}
}
}
if(splitted_way->m_NodeRefs.front()!=splitted_way->m_NodeRefs.back())
m_SplittedWays.push_back(splitted_way);
else
{
delete splitted_way;
splitted_way=0;
}
}
}
} // end SplitWays
void OSMDocument::SplitWays()
{
std::vector<Way*>::const_iterator it(m_Ways.begin());
std::vector<Way*>::const_iterator last(m_Ways.end());
//splitted ways get a new ID
long long id=0;
while(it!=last)
{
Way* currentWay = *it++;
// ITERATE THROUGH THE NODES
std::vector<Node*>::const_iterator it_node( currentWay->m_NodeRefs.begin());
std::vector<Node*>::const_iterator last_node( currentWay->m_NodeRefs.end());
Node* backNode = currentWay->m_NodeRefs.back();
while(it_node!=last_node)
{
Node* node = *it_node++;
Node* secondNode=0;
Node* lastNode=0;
Way* splitted_way = new Way( ++id, currentWay->visible, currentWay->osm_id );
splitted_way->name=currentWay->name;
splitted_way->type=currentWay->type;
splitted_way->clss=currentWay->clss;
splitted_way->oneway=currentWay->oneway;
std::vector<Tag*>::iterator it_tag( currentWay->m_Tags.begin() );
std::vector<Tag*>::iterator last_tag( currentWay->m_Tags.end() );
// std::cout << "Number of tags: " << currentWay->m_Tags.size() << std::endl;
// std::cout << "First tag: " << currentWay->m_Tags.front()->key << ":" << currentWay->m_Tags.front()->value << std::endl;
// ITERATE THROUGH THE TAGS
while(it_tag!=last_tag)
{
Tag* tag = *it_tag++;
splitted_way->AddTag(tag);
}
//GeometryFromText('LINESTRING('||x1||' '||y1||','||x2||' '||y2||')',4326);
splitted_way->geom="LINESTRING("+ boost::lexical_cast<std::string>(node->lon) + " " + boost::lexical_cast<std::string>(node->lat) +",";
splitted_way->AddNodeRef(node);
bool found=false;
if(it_node!=last_node)
{
while(it_node!=last_node && !found)
{
splitted_way->AddNodeRef(*it_node);
if((*it_node)->numsOfUse>1)
{
found=true;
secondNode = *it_node;
splitted_way->AddNodeRef(secondNode);
double length = getLength(node,secondNode);
if(length<0)
length*=-1;
splitted_way->length+=length;
splitted_way->geom+= boost::lexical_cast<std::string>(secondNode->lon) + " " + boost::lexical_cast<std::string>(secondNode->lat) + ")";
}
else if(backNode==(*it_node))
{
lastNode=*it_node++;
splitted_way->AddNodeRef(lastNode);
double length = getLength(node,lastNode);
if(length<0)
length*=-1;
splitted_way->length+=length;
splitted_way->geom+= boost::lexical_cast<std::string>(lastNode->lon) + " " + boost::lexical_cast<std::string>(lastNode->lat) + ")";
}
else
{
splitted_way->geom+= boost::lexical_cast<std::string>((*it_node)->lon) + " " + boost::lexical_cast<std::string>((*it_node)->lat) + ",";
*it_node++;
}
}
}
if(splitted_way->m_NodeRefs.front()!=splitted_way->m_NodeRefs.back())
m_SplittedWays.push_back(splitted_way);
else
{
delete splitted_way;
splitted_way=0;
}
}
}
} // end SplitWays
EXPORT NODE *reorder_node_loop(
NODE *oldn,
NODE *newn)
{
INTERFACE *intfc;
NODE *next, *prev;
NODE *next_old_node;
if (oldn != NULL)
{
intfc = oldn->interface;
next = next_node(oldn);
if (next == NULL)
{
screen("ERROR in reorder_node_loop(), "
"next_node(oldn) == NULL\n");
print_node(oldn);
(void) printf("Old interface information\n");
print_linked_node_list(intfc);
print_interface(intfc);
if (newn != NULL)
{
intfc = newn->interface;
(void) printf("New interface information\n");
print_linked_node_list(intfc);
print_interface(intfc);
}
clean_up(ERROR);
}
prev = prev_node(oldn);
prev_node(next) = prev;
if (prev != NULL)
next_node(prev) = next;
else
first_node(intfc) = next;
next_node(last_node(intfc)) = oldn;
next_node(oldn) = NULL;
prev_node(oldn) = last_node(intfc);
last_node(intfc) = oldn;
next_old_node = next;
}
else
next_old_node = NULL;
if (newn != NULL)
{
intfc = newn->interface;
next = next_node(newn);
if (next == NULL)
{
screen("ERROR in reorder_node_loop(), "
"next_node(newn) == NULL\n");
print_node(newn);
if (oldn != NULL)
{
(void) printf("Old interface information\n");
print_linked_node_list(intfc);
print_interface(intfc);
}
intfc = newn->interface;
(void) printf("New interface information\n");
print_linked_node_list(intfc);
print_interface(intfc);
clean_up(ERROR);
}
prev = prev_node(newn);
prev_node(next) = prev;
if (prev != NULL)
next_node(prev) = next;
else
first_node(intfc) = next;
next_node(last_node(intfc)) = newn;
next_node(newn) = NULL;
prev_node(newn) = last_node(intfc);
last_node(intfc) = newn;
}
return next_old_node;
} /*end reorder_node_loop*/
// Find the path between (sx,sy) and (dx,dy). Returns PATH_FAILED or PATH_OK.
int PATHFINDER::find_path(int sx_, int sy_, int dx_, int dy_) {
// Check that the target tile is reachable
if(map_solid(dx_, dy_))
return PATH_FAILED;
// Helper array which tells us if a tile is on the open nodes list, or
// on the closed nodes list.
// 0 == It isn't on the lists
// 1 == It is on the closed list
// 2 == It is on the open list
int map_nodes[MAP_W][MAP_H];
// Initialize the pathfinder
dx = dx_;
dy = dy_;
sx = sx_;
sy = sy_;
nodes_open.clear();
nodes_closed.clear();
path.clear();
for(int fyy = 0; fyy < MAP_H; fyy++)
for(int fxx = 0; fxx < MAP_W; fxx++)
map_nodes[fxx][fyy] = 0;
// Add the starting point to the open nodes
NODE st;
st.clear();
st.x = sx;
st.y = sy;
nodes_open.push_back(st);
map_nodes[sx][sy] = 2;
// Current node
NODE current;
current.clear();
// State of the search
int search_state = -1;
// Search the path
while(search_state == -1) {
// Sort the open list by F value
nodes_open.sort();
// Current tile shall be the one with the lowest F (the first after sorting)
current = *(nodes_open.begin());
// Move it to the closed list
nodes_open.pop_front();
nodes_closed.push_back(current);
map_nodes[current.x][current.y] = 1;
// Look for adjacent reachable tiles
for(int f=0; f < 4; f++) {
int mx,my;
switch(f) {
default:
case 0: mx = current.x; my = current.y - 1; break;
case 1: mx = current.x + 1; my = current.y; break;
case 2: mx = current.x; my = current.y + 1; break;
case 3: mx = current.x - 1; my = current.y; break;
}
// Check the tile
if(map_solid(mx, my))
continue; // Solid tile, skip it
// Check if the tile is on the closed nodes list
if(map_nodes[mx][my] == 1)
continue;
/* bool is_closed = false;
list<NODE>::iterator i;
for(i = nodes_open.begin(); i != nodes_open.end(); ++i) {
if((*i).x == mx && (*i).y == my) {
is_closed = true;
break;
}
}
if(is_closed)
continue; // It was on the closed nodes list, skip it
*/
// Check if the tile isn't on the open nodes list
if(map_nodes[mx][my] == 0) {
// Add a new node and make the current tile it's parent
NODE n;
n.clear();
n.x = mx;
n.y = my;
n.g = current.g + 10; // No diagonal movement
n.h = tile_dist(mx, my, dx, dy); // Manhattan distance
n.f = n.g + n.h; // F = G + H
n.parent = last_node(nodes_closed);
nodes_open.push_back(n);
map_nodes[mx][my] = 2;
// If we just added our target tile (dx, dy), we've found the path.
// Bail out.
if(mx == dx && my == dy) {
search_state = PATH_OK;
break;
}
}
}
// If the list of open nodes is empty, we can't find the path
if(nodes_open.empty()) {
search_state = PATH_FAILED;
break;
}
}
// At this point, the path is either found or failed
if(search_state == PATH_OK) {
// Path was found
// The destination node is the last node added to the open list
NODE *dest = last_node(nodes_open);
// Make sure it is so
if(dest->x != dx || dest->y != dy)
printf("find_path() error:\nThe path destination is (%d,%d) while it should be (%d,%d)!\n", dest->x, dest->y, dx, dy);
// Add the destination to the path
PATHPOINT point;
point.x = dx;
point.y = dy;
path.insert(path.begin(), point);
// Traverse from the destination along the path, and save the path points
NODE *p = dest->parent;
while(p) {
PATHPOINT point;
point.x = p->x;
point.y = p->y;
path.insert(path.begin(), point);
p = p->parent;
}
// Remove the start point from the path as it isn't needed.
path.erase(path.begin());
// Clear the nodes list as we don't need them any more
nodes_open.clear();
nodes_closed.clear();
return PATH_OK;
}
else {
// Path was not found
return PATH_FAILED;
}
}
