lifo-dijkstraserv/src/dijkstragraph.c

601 lines
17 KiB
C

#include "dijkstragraph.h"
#include <stdlib.h>
#include <glib/glist.h>
#include <math.h>
#include "logging.h"
#define FP_THRESHOLD 0.01
int dijkstra_node_same (struct dijkstra_node *node_a,
struct dijkstra_node *node_b)
{
int same = 1;
// Not the proper way to do it, but it works for now.
same &= (fabs(node_a->position.x - node_b->position.x) < FP_THRESHOLD);
same &= (fabs(node_a->position.y - node_b->position.y) < FP_THRESHOLD);
same &= (node_a->layer == node_b->layer);
same &= (node_a->position.z == node_b->position.z);
return same;
}
/*!
* \brief dijkstra_node_new creates a new dijkstra_node
* \param x x position
* \param y y position
* \return a new dijkstra node or NULL. Needs to be freed manually.
*/
struct dijkstra_node *dijkstra_node_new(float x,
float y,
int z,
struct dijkstra_layer *layer,
int uid)
{
struct dijkstra_node *node;
node = malloc(sizeof(*node));
if (node) {
node->position.x = x;
node->position.y = y;
node->position.z = z;
node->layer = layer;
node->uid = uid;
node->paths = NULL;
}
return node;
}
struct dijkstra_node *dijkstra_node_exists(GList *list,
struct dijkstra_node *node)
{
GList *l;
struct dijkstra_node *node_existing;
for (l = list; l != NULL; l = l->next) {
node_existing = (struct dijkstra_node*) l->data;
if (dijkstra_node_same(node, node_existing)) {
return node_existing;
}
}
return NULL;
}
/*!
* \brief dijkstra_node_new_to_list creates a new node entry if it does not already exist in the list
* \param list list of dijkstra_node
* \param x
* \param y
* \param is_new When != NULL: is_new will be set to true if a new node had to be created
* \return a dijkstra_node with the specified position (either newly created or from the list). Or NULL on error.
*/
struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
float x,
float y,
int z,
struct dijkstra_layer *layer)
{
struct dijkstra_node *node;
struct dijkstra_node *found;
int uid;
uid = g_list_length(*list);
node = dijkstra_node_new(x, y, z, layer, uid);
if (!node)
return NULL;
if ((found = dijkstra_node_exists(*list, node))) {
free(node);
node = found;
} else {
*list = g_list_append(*list, node);
}
return node;
}
int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination,
struct dijkstra_path *path)
{
if (!path || !destination)
return -1;
if (path->destination) {
report(LL_WARNING, "Connecting an already connected path (destination).");
}
destination->paths = g_list_append(destination->paths, (gpointer) path);
path->destination = destination;
return 0;
}
int dijkstra_connect_source_node_to_path(struct dijkstra_node *source,
struct dijkstra_path *path)
{
if (!path || !source)
return -1;
if (path->source) {
report(LL_WARNING, "Connecting an already connected path (source).");
}
source->paths = g_list_append(source->paths, (gpointer) path);
path->source = source;
return 0;
}
/*!
* \brief dijkstra_connect_nodes_to_path connect the two nodes using the path
* \param source
* \param destination
* \param path
* \return
*/
int dijkstra_connect_nodes_to_path(struct dijkstra_node *source,
struct dijkstra_node *destination,
struct dijkstra_path *path)
{
int ret = 0;
ret |= dijkstra_connect_source_node_to_path(source, path);
ret |= dijkstra_connect_destination_node_to_path(destination, path);
return ret;
}
// Creates an unlinked (!) copy of the path
struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path)
{
struct dijkstra_path *dup;
if (!path)
return NULL;
dup = malloc(sizeof(*path));
if (dup) {
memcpy(dup, path, sizeof(*path));
dup->source = NULL;
dup->destination = NULL;
}
return dup;
}
float dijkstra_position_get_distance(struct position *position_a,
struct position *position_b) {
return sqrt(pow(position_a->x - position_b->x, 2) +
pow(position_a->y - position_b->y, 2));
}
dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a,
struct dijkstra_node *node_b)
{
float dist;
float factor;
dist = dijkstra_position_get_distance(&node_a->position, &node_b->position);
factor = (node_a->layer->type == TRAINLINE ? ((float) node_a->position.z) / 100.0 : 1.0);
factor += (node_b->layer->type == TRAINLINE ? ((float) node_b->position.z) / 100.0 : 1.0);
factor /= 2;
/*if (node_a->layer->type != STREET || node_b->layer->type != STREET) {
report(LL_INFO, "%d-%d Factor is %f (from %d-%d)",
node_a->layer->type,node_b->layer->type, factor,
node_a->position.z, node_b->position.z);
}*/
dist /= factor;
if (dist < DIJKSTRA_COST_MIN)
dist = DIJKSTRA_COST_MIN;
return (dijkstra_cost) (dist * 100);
}
/*!
* \brief dijkstra_path_new creates a new dijkstra_path between two nodes.
* \param name the name of the path (pointer is not copied and must stay valid!)
* \return a new dijkstra path or NULL. Needs to be freed manually.
*/
struct dijkstra_path *dijkstra_path_new(char *name,
struct dijkstra_node *source,
struct dijkstra_node *destination,
dijkstra_cost weight)
{
struct dijkstra_path *path;
path = malloc(sizeof(*path));
if (path) {
path->name = name;
if (weight == DIJKSTRA_WEIGHT_AUTO)
path->weight = dijkstra_get_weight_from_distance(source,
destination);
else
path->weight = weight;
path->source = NULL;
path->destination = NULL;
if (dijkstra_connect_nodes_to_path(source, destination, path) != 0) {
report(LL_CRITICAL, "Connect nodes failed: %p with %p as %p (%s)",
source, destination, path, path->name);
}
}
return path;
}
struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path,
struct dijkstra_node *node)
{
if (!g_list_find(node->paths, path)) {
report(LL_CRITICAL, "Tried disconnecting node %p from path %p (%s) that wasn't connected in the first place.",
node, path, path->name);
}
node->paths = g_list_remove(node->paths, path);
if (path->source == path->destination) {
report(LL_CRITICAL, "Zero-length path. Destination node = source node for path %p", path);
}
if (path->source == node) {
path->source = NULL;
}
if (path->destination == node) {
path->destination = NULL;
}
return node;
}
void checkycheckcheck(struct dijkstra_solver *solver)
{
GList *l;
struct dijkstra_node *node;
struct dijkstra_path *path;
report(LL_INFO, "");
for (l = solver->nodes; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data;
report(LL_INFO, "Node %5d @%p: %f,\t%f [%d paths]", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths));
for (GList *m = node->paths; m != NULL; m = m->next) {
path = (struct dijkstra_path*) m->data;
struct dijkstra_node *foo = dijkstra_node_get_connection(node, path);
}
}
}
int dijkstra_path_intersect(struct dijkstra_solver *solver,
struct dijkstra_path *path_a,
struct dijkstra_path *path_b)
{
// Yes, I was too lazy to code this myself:
// https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
struct dijkstra_path *path_new;
struct dijkstra_node *node_new;
struct dijkstra_node *disconnected_node;
float p0_x, p0_y, p1_x, p1_y;
float p2_x, p2_y, p3_x, p3_y;
int p0_z, p1_z, p2_z, p3_z;
struct dijkstra_layer *p0_layer, *p1_layer, *p2_layer, *p3_layer;
float i_x, i_y;
int i_z;
struct dijkstra_layer *i_layer;
struct dijkstra_layer *access_layer = NULL;
struct dijkstra_layer *target_layer = NULL;
int target_z = -10000000;
float s1_x, s1_y, s2_x, s2_y;
float s, t;
if (!path_a || !path_b)
return -1;
// Process layer
p0_layer = path_a->source->layer;
p1_layer = path_a->destination->layer;
p2_layer = path_b->source->layer;
p3_layer = path_b->destination->layer;
if (p0_layer->type == ACCESS) {
access_layer = p0_layer;
target_layer = p2_layer;
target_z = path_b->source->position.z;
} else if (p1_layer->type == ACCESS) {
access_layer = p1_layer;
target_layer = p2_layer;
target_z = path_b->source->position.z;
} else if (p2_layer->type == ACCESS) {
access_layer = p2_layer;
target_layer = p0_layer;
target_z = path_a->source->position.z;
} else if (p3_layer->type == ACCESS) {
access_layer = p3_layer;
target_layer = p0_layer;
target_z = path_a->source->position.z;
} else {
target_layer = p0_layer;
target_z = path_a->source->position.z; // For normal intersections, all .z must be the same, so we can pick any.
}
if (access_layer) {
if (target_layer->type == ACCESS)
return 0;
if (target_layer->type == TRAINLINE && strcmp(access_layer->access, target_layer->name))
return 0;
// report(LL_DEBUG, "ACCESS OK");
} else {
if (p0_layer->type == TRAINLINE ||
p1_layer->type == TRAINLINE ||
p2_layer->type == TRAINLINE ||
p3_layer->type == TRAINLINE)
return 0; // Never collide trains (sounds like a good idea, doesn't it!)
}
if (access_layer) {
target_layer = access_layer; // Without this, I will (sometimes??) get a direct link between the two layers
}
/*
if (p0_layer->type == ACCESS || p1_layer->type == ACCESS) {
//if (p2_layer->type == TRAINLINE && strcmp(p0_layer->access, p2_layer->name))
// return 0;
} else if (p2_layer->type == ACCESS || p3_layer->type == ACCESS) {
//if (p0_layer->type == TRAINLINE && strcmp(p2_layer->access, p0_layer->name))
// return 0;
} else if (p0_layer->type == TRAINLINE || p2_layer->type == TRAINLINE) {
report(LL_ERROR, "Prevent %d to %d", p0_layer->type, p2_layer->type);
return 0; // Never collide trains (sounds like a good idea, doesn't it!)
}*/
// Check height
p0_z = path_a->source->position.z;
p1_z = path_a->destination->position.z;
p2_z = path_b->source->position.z;
p3_z = path_b->destination->position.z;
if (p0_layer->type == STREET && p2_layer->type == STREET) {
if (!(p0_z == p1_z && p1_z == p2_z && p2_z == p3_z)) // We don't want to intersect on height, they must match exactly for all four points.
return 0; // No collision
}
p0_x = path_a->source->position.x;
p0_y = path_a->source->position.y;
p1_x = path_a->destination->position.x;
p1_y = path_a->destination->position.y;
p2_x = path_b->source->position.x;
p2_y = path_b->source->position.y;
p3_x = path_b->destination->position.x;
p3_y = path_b->destination->position.y;
s1_x = p1_x - p0_x;
s1_y = p1_y - p0_y;
s2_x = p3_x - p2_x;
s2_y = p3_y - p2_y;
s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y);
t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y);
if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
{
// Collision detected
i_x = p0_x + (t * s1_x);
i_y = p0_y + (t * s1_y);
i_layer = target_layer;
i_z = target_z;
report(LL_DEBUG, "Split layer to type %d", i_layer->type);
node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y, i_z, i_layer);
if (node_new != path_a->source && node_new != path_a->destination) {
path_new = dijkstra_path_dup_shallow(path_a);
solver->paths = g_list_append(solver->paths, (gpointer) path_new);
disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
dijkstra_connect_destination_node_to_path(node_new, path_a);
path_a->weight = dijkstra_get_weight_from_distance(path_a->source, path_a->destination);
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination);
// TODO: I don't have to iterate over the *entire* array here, it should be enough to only iterate over the entries after path_b.
dijkstra_path_intersect_all(solver, path_new, path_new); // Since straight paths can only cross once, we don't need to check any newly created entries.
}
// Now do the same thing for path b
if (node_new != path_b->source && node_new != path_b->destination) {
path_new = dijkstra_path_dup_shallow(path_b);
solver->paths = g_list_append(solver->paths, (gpointer) path_new);
disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
dijkstra_connect_destination_node_to_path(node_new, path_b);
path_b->weight = dijkstra_get_weight_from_distance(path_b->source, path_b->destination);
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination);
}
// In case neither if matches that means we are on the corner where two paths intersect at their ends.
// No need to do anything then, dijkstra_node_new_to_list will have taken care of everything.
return 1;
}
return 0; // No collision
}
void dijkstra_path_intersect_all(struct dijkstra_solver *solver,
struct dijkstra_path *path,
struct dijkstra_path *path_last)
{
GList *l;
struct dijkstra_path *existing_path;
for (l = solver->paths; l != NULL; l = l->next) {
existing_path = (struct dijkstra_path*) l->data;
dijkstra_path_intersect(solver, path, existing_path);
if (l->data == path_last) /* Avoid testing newly added paths */
break;
}
}
struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
float x1,
float y1,
float x2,
float y2,
int z1,
int z2,
struct dijkstra_layer *layer,
char *name,
dijkstra_cost weight)
{
struct dijkstra_node *n1, *n2;
struct dijkstra_path *path;
GList *l_last;
struct dijkstra_path *path_last = NULL;
n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1, layer);
n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2, layer);
path = dijkstra_path_new(name, n1, n2, weight);
l_last = g_list_last(solver->paths);
if (l_last)
path_last = l_last->data;
solver->paths = g_list_append(solver->paths, (gpointer) path);
dijkstra_path_intersect_all(solver, path, path_last);
return path;
}
struct dijkstra_solver *dijkstra_solver_new()
{
struct dijkstra_solver *solver;
solver = malloc(sizeof(*solver));
if (solver) {
solver->nodes = NULL;
solver->paths = NULL;
solver->layers = NULL;
}
return solver;
}
void dijkstra_solver_free(struct dijkstra_solver *solver)
{
GList *l;
struct dijkstra_path *path;
struct dijkstra_node *node;
// Free memory
for (l = solver->paths; l != NULL; l = l->next) {
path = (struct dijkstra_path*) l->data;
// TODO: free(path->name);
// Currently, we reuse the existing memory block when for the
// name when cloning streets. As such, clearing it here will
// cause a double free if the street has been split.
// NOTE: Currently, do NOT free path->access as it is part of
// path->name and will be freed along with it (or not lol).
free(path);
}
g_list_free(solver->paths);
for (l = solver->nodes; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data;
free(node);
}
g_list_free(solver->nodes);
}
inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
struct dijkstra_path *path)
{
if (!node || !path) {
report(LL_CRITICAL, "Called get_connection with NULL node (%p) or path (%p).",
node, path);
}
if (node != path->destination && node != path->source) {
report(LL_WARNING, "get_connection requested for invalid node / path pair: %d with %p (%s).", node->uid, path, path->name);
for (GList *l = node->paths; l != NULL; l = l->next) {
struct dijkstra_path *path = (struct dijkstra_path*) l->data;
report(LL_WARNING, " Available: %p (%s)", path, path->name);
}
getchar();
}
return (path->destination == node ? path->source : path->destination);
}
struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver,
struct position *position) {
GList *l = NULL;
struct dijkstra_node *node = NULL;
struct dijkstra_node *closest_node = NULL;
float closest_distance;
float distance;
for (l = solver->nodes; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data;
if (node->layer->type != STREET)
continue;
distance = dijkstra_position_get_distance(position, &node->position);
if (!closest_node || distance < closest_distance) {
closest_node = node;
closest_distance = distance;
}
}
return closest_node;
}
struct dijkstra_layer *dijkstra_layer_new(enum dijkstra_layer_type type,
char *name,
char *access)
{
struct dijkstra_layer *layer;
layer = malloc(sizeof(*layer));
if (layer) {
layer->name = strdup(name);
layer->type = type;
layer->access = access;
}
return layer;
}
struct dijkstra_layer *dijkstra_layer_new_to_list(struct dijkstra_solver *solver,
enum dijkstra_layer_type type,
char *name,
char *access)
{
struct dijkstra_layer *layer;
GList *l = NULL;
if (type == STREET)
name = "all";
for (l = solver->layers; l != NULL; l = l->next) {
layer = l->data;
if (layer->type == type && !strcmp(layer->name, name)) {
return layer;
}
}
layer = dijkstra_layer_new(type, name, access);
solver->layers = g_list_append(solver->layers, (void *) layer);
return layer;
}