143 lines
3.7 KiB
C
143 lines
3.7 KiB
C
/*
|
|
* A* Search - A search library used in Empire to determine paths between
|
|
* objects.
|
|
* Copyright (C) 1990-1998 Phil Lapsley
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
/*
|
|
* 11/09/98 - Steve McClure
|
|
* Added path list caching structures
|
|
*/
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include "as.h"
|
|
|
|
/*
|
|
* Basic A* search function. "adp" should have been initialized by
|
|
* as_init (any previously allocated data will be freed by as_reset here),
|
|
* and adp->from and adp->to should be set accordingly. On success,
|
|
* returns 0, with adp->path set to a linked list of coordinates to target.
|
|
* If we can't find target, return -1; if malloc fails, return -2.
|
|
*/
|
|
int
|
|
as_search(struct as_data *adp)
|
|
{
|
|
int iter = 0;
|
|
struct as_queue *head;
|
|
struct as_node *np;
|
|
#ifdef DEBUG
|
|
int i;
|
|
struct as_queue *qp;
|
|
struct as_path *pp;
|
|
#endif /* DEBUG */
|
|
|
|
as_reset(adp);
|
|
|
|
/*
|
|
* Jump start the queue by making first element the zero-cost
|
|
* node where we start.
|
|
*/
|
|
AS_NEW_MALLOC(head, struct as_queue, -2);
|
|
adp->head = head;
|
|
head->next = head->prev = NULL;
|
|
AS_NEW(np, struct as_node, -2);
|
|
np->c = adp->from;
|
|
head->np = np;
|
|
as_setcinq(adp, head->np->c, adp->head);
|
|
|
|
for (;;) {
|
|
iter++;
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "Iteration %d, head at %d, %d\n", iter,
|
|
head->np->c.x, head->np->c.y);
|
|
#endif /* DEBUG */
|
|
|
|
/* see if we're done, one way or another */
|
|
if (head == NULL)
|
|
break;
|
|
|
|
/* Add it to the cache */
|
|
as_add_cachepath(adp);
|
|
|
|
if (head->np->c.x == adp->to.x && head->np->c.y == adp->to.y)
|
|
break;
|
|
|
|
/* extend queue by neighbors */
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "\tExtending queue\n");
|
|
#endif /* DEBUG */
|
|
adp->head = head = as_extend(adp);
|
|
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "queue:\n");
|
|
i = 0;
|
|
for (qp = head; qp; qp = qp->next) {
|
|
fprintf(stderr, "\t%d, %d so far %f lb %f sec %f\n",
|
|
qp->np->c.x, qp->np->c.y,
|
|
qp->np->knowncost, qp->np->lbcost, qp->np->seccost);
|
|
i++;
|
|
}
|
|
fprintf(stderr, "\tqueue len %d\n", i);
|
|
#endif /* DEBUG */
|
|
|
|
}
|
|
|
|
if (head == NULL) {
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "Failed\n");
|
|
#endif /* DEBUG */
|
|
return -1;
|
|
}
|
|
|
|
as_makepath(adp);
|
|
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "Succeeded, iter %d, cost %f!\n", iter,
|
|
head->np->knowncost);
|
|
fprintf(stderr, "Path:\n");
|
|
for (pp = adp->path; pp; pp = pp->next) {
|
|
fprintf(stderr, "\t%d, %d\n", pp->c.x, pp->c.y);
|
|
}
|
|
fprintf(stderr, "Tried queue:\n");
|
|
for (qp = adp->tried; qp; qp = qp->next) {
|
|
fprintf(stderr, "\t%d, %d\n", qp->np->c.x, qp->np->c.y);
|
|
}
|
|
fprintf(stderr, "Subsumed queue:\n");
|
|
for (qp = adp->subsumed; qp; qp = qp->next) {
|
|
fprintf(stderr, "\t%d, %d\n", qp->np->c.x, qp->np->c.y);
|
|
}
|
|
#endif /* DEBUG */
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
* Work backwards through the list of nodes (starting at head)
|
|
* to produce a path.
|
|
*/
|
|
void
|
|
as_makepath(struct as_data *adp)
|
|
{
|
|
struct as_path *pp;
|
|
struct as_node *np;
|
|
|
|
for (np = adp->head->np; np; np = np->back) {
|
|
pp = malloc(sizeof(struct as_path));
|
|
pp->c = np->c;
|
|
pp->next = adp->path;
|
|
adp->path = pp;
|
|
}
|
|
}
|