lifo-dijkstraserv/src/dijkstragraph.c

587 lines
16 KiB
C
Raw Normal View History

2020-05-02 17:02:26 +02:00
#include "dijkstragraph.h"
2020-05-01 20:48:36 +02:00
#include <stdlib.h>
#include <glib/glist.h>
#include <math.h>
#include "logging.h"
2020-05-01 20:48:36 +02:00
#define FP_THRESHOLD 0.01
2020-05-02 16:51:12 +02:00
2020-05-01 20:48:36 +02:00
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->position.z == node_b->position.z);
2020-05-01 20:48:36 +02:00
return same;
}
2020-05-02 16:51:12 +02:00
/*!
* \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,
2020-05-02 16:59:13 +02:00
float y,
int z,
2020-05-02 16:59:13 +02:00
int uid)
2020-05-02 16:51:12 +02:00
{
struct dijkstra_node *node;
node = malloc(sizeof(*node));
if (node) {
node->position.x = x;
node->position.y = y;
node->position.z = z;
2020-05-02 16:59:13 +02:00
node->uid = uid;
2020-05-02 16:51:12 +02:00
node->paths = NULL;
}
return node;
}
2020-05-01 20:48:36 +02:00
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)
2020-05-01 20:48:36 +02:00
{
struct dijkstra_node *node;
struct dijkstra_node *found;
2020-05-02 16:59:13 +02:00
int uid;
2020-05-01 20:48:36 +02:00
2020-05-02 16:59:13 +02:00
uid = g_list_length(*list);
node = dijkstra_node_new(x, y, z, uid);
2020-05-01 20:48:36 +02:00
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).");
}
2020-05-01 20:48:36 +02:00
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).");
}
2020-05-01 20:48:36 +02:00
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
2020-05-01 20:48:36 +02:00
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;
2020-05-01 20:48:36 +02:00
}
return dup;
}
2020-05-03 19:59:05 +02:00
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,
2021-02-26 19:15:48 +01:00
struct dijkstra_node *node_b,
enum dijkstra_net_type net_type)
{
float dist;
2021-02-26 19:15:48 +01:00
float scale;
switch (net_type) {
case TRAINLINE: // Trainlines and access types use the z-coordinate
case ACCESS: // for cost scaling.
2021-02-26 19:15:48 +01:00
scale = (float)node_a->position.z;
scale += (float)node_b->position.z;
scale /= 200;
break;
default:
scale = 1.0;
break;
}
2020-05-03 19:59:05 +02:00
dist = dijkstra_position_get_distance(&node_a->position, &node_b->position);
2021-02-26 19:15:48 +01:00
dist /= scale;
if (dist < DIJKSTRA_COST_MIN)
dist = DIJKSTRA_COST_MIN;
return (dijkstra_cost) (dist * 100);
}
2020-05-02 16:51:12 +02:00
/*!
* \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,
2021-02-26 19:15:48 +01:00
dijkstra_cost weight,
struct dijkstra_net_meta *net_meta)
2020-05-02 16:51:12 +02:00
{
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,
2021-02-26 19:15:48 +01:00
destination,
net_meta->type);
else
path->weight = weight;
2020-05-02 16:51:12 +02:00
path->source = NULL;
path->destination = NULL;
2021-02-26 19:15:48 +01:00
path->net_meta = net_meta;
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);
}
2020-05-02 16:51:12 +02:00
}
return path;
}
2020-05-01 20:48:36 +02:00
struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path,
struct dijkstra_node *node)
2020-05-01 20:48:36 +02:00
{
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);
2020-05-01 20:48:36 +02:00
if (path->source == path->destination) {
report(LL_CRITICAL, "Zero-length path. Destination node = source node for path %p", path);
}
2020-05-01 20:48:36 +02:00
if (path->source == node) {
path->source = NULL;
}
if (path->destination == node) {
path->destination = NULL;
}
2020-05-01 20:48:36 +02:00
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);
}
}
2020-05-01 20:48:36 +02:00
}
2020-05-02 16:51:12 +02:00
int dijkstra_path_intersect(struct dijkstra_solver *solver,
2020-05-01 20:48:36 +02:00
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;
2021-02-26 19:15:48 +01:00
struct dijkstra_path *access_path = NULL;
struct dijkstra_path *target_path = NULL;
2020-05-01 20:48:36 +02:00
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;
float i_x, i_y;
int i_z;
2020-05-01 20:48:36 +02:00
float s1_x, s1_y, s2_x, s2_y;
float s, t;
if (!path_a || !path_b)
return -1;
2021-02-26 19:15:48 +01:00
if (path_a->net_meta->type == ACCESS) {
access_path = path_a;
target_path = path_b;
} else if (path_b->net_meta->type == ACCESS) {
access_path = path_b;
target_path = path_a;
}
if (access_path) {
if (target_path->net_meta->type != STREET &&
!strstr(access_path->net_meta->access, target_path->name)) {
return 0; // Don't intersect if access != name of target
}
} else {
if (path_a->net_meta->type != path_b->net_meta->type)
return 0; // Never intersect two layers of different types, unless it's through access layers
}
// 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;
2021-02-26 19:15:48 +01:00
if (!access_path && // For access paths, z is invalid, so only check this if we are intersecting streets (trainlines are filtered out above)
!(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
2020-05-01 20:48:36 +02:00
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_z = p0_z; // They must all on the same height when intersecting, so we can pick whichever.
2020-05-01 20:48:36 +02:00
node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y, i_z);
2020-05-01 20:48:36 +02:00
if (node_new != path_a->source && node_new != path_a->destination) {
path_new = dijkstra_path_dup_shallow(path_a);
2020-05-02 16:51:12 +02:00
solver->paths = g_list_append(solver->paths, (gpointer) path_new);
2020-05-01 20:48:36 +02:00
disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
2020-05-01 20:48:36 +02:00
dijkstra_connect_destination_node_to_path(node_new, path_a);
2021-02-26 19:15:48 +01:00
path_a->weight = dijkstra_get_weight_from_distance(
path_a->source,
path_a->destination,
path_new->net_meta->type);
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
2021-02-26 19:15:48 +01:00
path_new->weight = dijkstra_get_weight_from_distance(
path_new->source,
path_new->destination,
path_new->net_meta->type);
// 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.
2020-05-01 20:48:36 +02:00
}
// 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);
2020-05-02 16:51:12 +02:00
solver->paths = g_list_append(solver->paths, (gpointer) path_new);
disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
2020-05-01 20:48:36 +02:00
dijkstra_connect_destination_node_to_path(node_new, path_b);
2021-02-26 19:15:48 +01:00
path_b->weight = dijkstra_get_weight_from_distance(
path_b->source,
path_b->destination,
path_new->net_meta->type);
dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
2021-02-26 19:15:48 +01:00
path_new->weight = dijkstra_get_weight_from_distance(path_new->source,
path_new->destination,
path_new->net_meta->type);
2020-05-01 20:48:36 +02:00
}
// 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.
2020-05-01 20:48:36 +02:00
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;
}
}
2020-05-02 16:51:12 +02:00
struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
2020-05-01 20:48:36 +02:00
float x1,
float y1,
float x2,
float y2,
int z1,
int z2,
2020-05-01 20:48:36 +02:00
char *name,
2021-02-26 19:15:48 +01:00
struct dijkstra_net_meta *net_meta,
2020-05-01 20:48:36 +02:00
dijkstra_cost weight)
{
struct dijkstra_node *n1, *n2;
struct dijkstra_path *path;
GList *l_last;
struct dijkstra_path *path_last = NULL;
2020-05-01 20:48:36 +02:00
n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1);
n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2);
2021-02-26 19:15:48 +01:00
path = dijkstra_path_new(name, n1, n2, weight, net_meta);
2020-05-01 20:48:36 +02:00
2020-05-02 16:51:12 +02:00
l_last = g_list_last(solver->paths);
if (l_last)
path_last = l_last->data;
2020-05-02 16:51:12 +02:00
solver->paths = g_list_append(solver->paths, (gpointer) path);
2020-05-01 20:48:36 +02:00
dijkstra_path_intersect_all(solver, path, path_last);
2020-05-01 20:48:36 +02:00
return path;
}
2020-05-02 16:51:12 +02:00
struct dijkstra_solver *dijkstra_solver_new()
2020-05-01 20:48:36 +02:00
{
2020-05-02 16:51:12 +02:00
struct dijkstra_solver *solver;
2020-05-01 20:48:36 +02:00
2020-05-02 16:51:12 +02:00
solver = malloc(sizeof(*solver));
if (solver) {
solver->nodes = NULL;
solver->paths = NULL;
2021-02-26 19:15:48 +01:00
solver->net_metas = NULL;
2020-05-01 20:48:36 +02:00
}
2020-05-02 16:51:12 +02:00
return solver;
2020-05-01 20:48:36 +02:00
}
2020-05-02 16:51:12 +02:00
void dijkstra_solver_free(struct dijkstra_solver *solver)
2020-05-01 20:48:36 +02:00
{
2020-05-02 16:51:12 +02:00
GList *l;
2020-05-01 20:48:36 +02:00
struct dijkstra_path *path;
2020-05-02 16:51:12 +02:00
struct dijkstra_node *node;
2021-02-26 19:15:48 +01:00
struct dijkstra_net_meta *net_meta;
2020-05-01 20:48:36 +02:00
2020-05-02 16:51:12 +02:00
// 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.
free(path);
2020-05-01 20:48:36 +02:00
}
2020-05-02 16:51:12 +02:00
g_list_free(solver->paths);
2020-05-01 20:48:36 +02:00
2020-05-02 16:51:12 +02:00
for (l = solver->nodes; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data;
free(node);
}
g_list_free(solver->nodes);
2021-02-26 19:15:48 +01:00
for (l = solver->net_metas; l != NULL; l = l->next) {
net_meta = (struct dijkstra_net_meta*) l->data;
free(net_meta->name);
2021-03-04 22:00:44 +01:00
if (net_meta->access)
free(net_meta->access);
2021-02-26 19:15:48 +01:00
free(net_meta);
}
2020-05-01 20:48:36 +02:00
}
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);
}
2020-05-03 19:59:05 +02:00
struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver,
struct position *position) {
GList *l = NULL;
2021-02-26 19:27:26 +01:00
GList *pl = NULL;
2020-05-03 19:59:05 +02:00
struct dijkstra_node *node = NULL;
struct dijkstra_node *closest_node = NULL;
2021-02-26 19:27:26 +01:00
struct dijkstra_path *path = NULL;
2020-05-03 19:59:05 +02:00
float closest_distance;
float distance;
for (l = solver->nodes; l != NULL; l = l->next) {
node = (struct dijkstra_node*) l->data;
2021-02-26 19:27:26 +01:00
for (pl = node->paths; pl != NULL; pl = pl->next) {
path = (struct dijkstra_path*) pl->data;
if (path->net_meta->type == STREET)
break;
}
if (!(path && path->net_meta->type == STREET))
continue; // Only start on nodes that are part of a street
2020-05-03 19:59:05 +02:00
distance = dijkstra_position_get_distance(position, &node->position);
if (!closest_node || distance < closest_distance) {
closest_node = node;
closest_distance = distance;
}
}
return closest_node;
}
2021-02-26 19:15:48 +01:00
struct dijkstra_net_meta *dijkstra_net_meta_new(enum dijkstra_net_type net_type,
char *name,
char *access)
{
struct dijkstra_net_meta *net_meta;
net_meta = malloc(sizeof(*net_meta));
if (net_meta) {
net_meta->type = net_type;
net_meta->name = strdup(name);
2021-03-04 22:00:44 +01:00
if (access)
net_meta->access = strdup(access);
else
net_meta->access = NULL;
2021-02-26 19:15:48 +01:00
}
return net_meta;
}
struct dijkstra_net_meta *dijkstra_net_meta_new_to_list(struct dijkstra_solver *solver,
enum dijkstra_net_type net_type,
char *name,
char *access)
{
struct dijkstra_net_meta *net_meta;
GList *l = NULL;
if (net_type == STREET)
name = "all";
for (l = solver->net_metas; l != NULL; l = l->next) {
net_meta = l->data;
if (net_meta->type == net_type &&
!strcmp(net_meta->name, name) &&
(net_meta->access && (!strcmp(net_meta->access, access)))) {
2021-02-26 19:15:48 +01:00
return net_meta;
}
}
net_meta = dijkstra_net_meta_new(net_type, name, access);
solver->net_metas = g_list_append(solver->net_metas, (void *) net_meta);
return net_meta;
}