[Add] A* algorithm to get paths between systems.

src/space.c

@@ -1,6 +1,9 @@
 #include <malloc.h>
 #include <math.h>
+#include <float.h>
 
+#include "xml.h"
+#include "lephisto.h"
 #include "log.h"
 #include "physics.h"
 #include "rng.h"
@@ -65,7 +68,8 @@ static Star* stars = NULL;  // Star array.
 static int nstars = 0;      // Total stars.
 static int mstars = 0;      // Memory stars are taking.
 
-// Intern
+// Intern.
+static StarSystem* system_get(const char* sysname);
 static Planet* planet_get(const char* name);
 static void space_addFleet(Fleet* fleet);
 static StarSystem* system_parse(const xmlNodePtr parent);
@@ -139,6 +143,200 @@ void planets_minimap(const double res,
 }
 #undef PIXEL
 
+// A* Algorithm fo shortest path finding.
+
+// The node struct.
+typedef struct SysNode_ {
+  struct SysNode_* next;
+
+  struct SysNode_* parent;
+  StarSystem* sys;
+  double r; // Ranking.
+  int g;    // Step.
+} SysNode;
+
+static SysNode* gc;
+// Prototypes.
+static SysNode* A_newNode(StarSystem* sys, SysNode* parent);
+static double A_h(StarSystem* n, StarSystem* g);
+static double A_g(SysNode* n);
+static SysNode* A_add(SysNode* first, SysNode* cur);
+static SysNode* A_rm(SysNode* first, StarSystem* cur);
+static int A_in(SysNode* first, StarSystem* cur);
+static SysNode* A_lowest(SysNode* first);
+static void A_freeList(SysNode* first);
+
+// Creates a new node link to star system.
+static SysNode* A_newNode(StarSystem* sys, SysNode* parent) {
+  SysNode* n;
+
+  n = malloc(sizeof(SysNode));
+
+  n->next = NULL;
+  n->parent = parent;
+  n->sys = sys;
+  n->r = DBL_MAX;
+  n->g = 0.;
+
+  A_add(gc, n);
+
+  return n;
+}
+
+static double A_h(StarSystem* n, StarSystem* g) {
+  // Euclidean distance.
+  return sqrt(pow2(n->pos.x - g->pos.x) + pow2(n->pos.y - g->pos.y))/100.;
+}
+
+// Get the g from a node.
+static double A_g(SysNode* n) {
+  return n->g;
+}
+
+// Add a node to the linkes list.
+static SysNode* A_add(SysNode* first, SysNode* cur) {
+  SysNode* n;
+
+  if(first == NULL)
+    return cur;
+
+  n = first;
+  while(n->next != NULL)
+    n = n->next;
+  n->next = cur;
+
+  return first;
+}
+
+// Remove a node from a linked list.
+static SysNode* A_rm(SysNode* first, StarSystem* cur) {
+  SysNode* n, *p;
+
+  if(first->sys == cur) {
+    first->next = NULL;
+    n = first->next;
+    return n;
+  }
+
+  p = first;
+  n = p->next;
+  do {
+    if(n->sys == cur) {
+      n->next = NULL;
+      p->next = n->next;
+      break;
+    }
+  } while((n=n->next) != NULL);
+
+  return first;
+}
+
+// Check if node is in linked list.
+static int A_in(SysNode* first, StarSystem* cur) {
+  SysNode* n;
+  
+  if(first == NULL)
+    return 0;
+
+  n = first;
+  do {
+    if(n->sys == cur)
+      return 1;
+  } while((n=n->next) != NULL);
+  return 0;
+}
+
+// Return the lowest ranking node from a linked list of nodes.
+static SysNode* A_lowest(SysNode* first) {
+  SysNode* lowest, *n;
+
+  if(first == NULL)
+    return NULL;
+
+  n = first;
+  lowest = n;
+  do {
+    if(n->r < lowest->r)
+      lowest = n;
+  } while((n=n->next) != NULL);
+
+  return lowest;
+}
+
+// Free a linked list.
+static void A_freeList(SysNode* first) {
+  SysNode* p, *n;
+
+  if(first == NULL)
+    return;
+
+  p = NULL;
+  n = first;
+  do {
+    if(p != NULL)
+      free(p);
+    p = n;
+  } while((n=n->next) != NULL);
+
+  free(p);
+}
+
+StarSystem** system_getJumpPath(int* njumps, char* sysstart, char* sysend) {
+  int i, cost;
+
+  StarSystem* ssys, *esys, **res;
+
+  SysNode* cur, *neighbour;
+  SysNode* open, *closed;
+
+  gc = NULL;
+
+  // Initial and target systems.
+  ssys = system_get(sysstart);  // Start.
+  ssys = system_get(sysend);    // End.
+
+  // Start the linked lists.
+  open = closed = NULL;
+  cur = A_newNode(ssys, NULL);
+  open = A_add(open, cur); // Initial open node is the start system.
+
+  while((cur = A_lowest(open))->sys != esys) {
+    // Get best from open and toss to closed.
+    open = A_rm(open, cur->sys);
+    closed = A_add(closed, cur);
+
+    for(i = 0; i < cur->sys->njumps; i++) {
+      neighbour = A_newNode(&systems_stack[cur->sys->jumps[i]], cur);
+      cost = A_g(cur) + 1;
+
+      if(A_in(open, neighbour->sys) && (cost < A_g(neighbour)))
+        open = A_rm(open, neighbour->sys); // New path is better.
+
+      if(A_in(closed, neighbour->sys) && (cost < A_g(neighbour)))
+        closed = A_rm(closed, neighbour->sys); // Shouldn't happen.
+
+      if(!A_in(open, neighbour->sys) && !A_in(closed, neighbour->sys)) {
+        neighbour->g = cost;
+        open = A_add(open, neighbour);
+        neighbour->r = (double)A_g(neighbour) + A_h(neighbour->sys, esys);
+      }
+    }
+  }
+  // Build the path backwards.
+  (*njumps) = A_g(cur);
+  res = malloc(sizeof(StarSystem*) * (*njumps));
+  for(i = 0; i < (*njumps); i++) {
+    res[(*njumps)-i-1] = cur->sys;
+    cur = cur->parent;
+  }
+
+  // Free the linked list.
+  //A_freeList(open);
+  //A_freeList(closed);
+  A_freeList(gc);
+  return res;
+}
+
 static PlanetClass planetclass_get(const char a) {
   switch(a) {
   // Planets use letters.
@@ -256,6 +454,18 @@ char* space_getRndPlanet(void) {
   return res;
 }
 
+// Get the system from it's name.
+static StarSystem* system_get(const char* sysname) {
+  int i;
+
+  for(i = 0; i < systems_nstack; i++)
+    if(strcmp(sysname, systems_stack[i].name)==0)
+      return &systems_stack[i];
+
+  DEBUG("System '%s' not found in stack", sysname);
+  return NULL;
+}
+
 // Get the name of a system from a planetname.
 char* planet_getSystem(char* planetname) {
   int i;

src/space.h

@@ -109,6 +109,7 @@ void planets_render(void);
 void space_update(const double dt);
 
 // Misc.
+StarSystem** system_getJumpPath(int* njumps, char* sysstart, char* sysend);
 int space_canHyperspace(Pilot* p);
 int space_hyperspace(Pilot* p);
 char** space_getFactionPlanet(int* nplanets, int* factions, int nfactions);