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>
|
2020-05-03 00:00:41 +02:00
|
|
|
#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);
|
2021-02-04 20:43:32 +01:00
|
|
|
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,
|
2021-02-04 20:43:32 +01:00
|
|
|
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;
|
2021-02-04 20:43:32 +01:00
|
|
|
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,
|
2021-02-04 20:43:32 +01:00
|
|
|
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);
|
|
|
|
|
2021-02-04 20:43:32 +01:00
|
|
|
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;
|
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
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;
|
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
// 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));
|
2020-05-03 00:00:41 +02:00
|
|
|
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));
|
|
|
|
}
|
|
|
|
|
2020-05-02 17:13:27 +02:00
|
|
|
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)
|
2020-05-02 17:13:27 +02:00
|
|
|
{
|
|
|
|
float dist;
|
2021-02-26 19:15:48 +01:00
|
|
|
float scale;
|
|
|
|
|
|
|
|
switch (net_type) {
|
2021-03-13 13:02:19 +01:00
|
|
|
case TRAINLINE:
|
|
|
|
scale = 5.0;
|
|
|
|
break;
|
|
|
|
case ACCESS:
|
|
|
|
scale = 0.5;
|
2021-02-26 19:15:48 +01:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
scale = 1.0;
|
|
|
|
break;
|
|
|
|
}
|
2020-05-02 17:13:27 +02:00
|
|
|
|
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;
|
2020-05-02 17:13:27 +02:00
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
if (dist < DIJKSTRA_COST_MIN)
|
|
|
|
dist = DIJKSTRA_COST_MIN;
|
|
|
|
|
|
|
|
return (dijkstra_cost) (dist * 100);
|
2020-05-02 17:13:27 +02:00
|
|
|
}
|
|
|
|
|
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;
|
2020-05-03 00:00:41 +02:00
|
|
|
if (weight == DIJKSTRA_WEIGHT_AUTO)
|
2020-05-02 17:13:27 +02:00
|
|
|
path->weight = dijkstra_get_weight_from_distance(source,
|
2021-02-26 19:15:48 +01:00
|
|
|
destination,
|
|
|
|
net_meta->type);
|
2020-05-02 17:13:27 +02:00
|
|
|
else
|
|
|
|
path->weight = weight;
|
2020-05-02 16:51:12 +02:00
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
path->source = NULL;
|
|
|
|
path->destination = NULL;
|
|
|
|
|
2021-02-26 19:15:48 +01:00
|
|
|
path->net_meta = net_meta;
|
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
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,
|
2020-05-03 00:00:41 +02:00
|
|
|
struct dijkstra_node *node)
|
2020-05-01 20:48:36 +02:00
|
|
|
{
|
2020-05-03 00:00:41 +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
|
|
|
|
2020-05-03 00:00:41 +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
|
|
|
|
2020-05-03 00:00:41 +02:00
|
|
|
if (path->source == node) {
|
2020-05-03 12:09:29 +02:00
|
|
|
path->source = NULL;
|
2020-05-03 00:00:41 +02:00
|
|
|
}
|
|
|
|
if (path->destination == node) {
|
2020-05-03 12:09:29 +02:00
|
|
|
path->destination = NULL;
|
2020-05-03 00:00:41 +02:00
|
|
|
}
|
2020-05-01 20:48:36 +02:00
|
|
|
|
|
|
|
return node;
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
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;
|
2020-05-03 00:00:41 +02:00
|
|
|
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;
|
2021-02-04 20:43:32 +01:00
|
|
|
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
|
2021-03-13 11:27:50 +01:00
|
|
|
// path_a and path_b types are now always guaranteed to be the same
|
|
|
|
if (path_a->net_meta->type == TRAINLINE)
|
|
|
|
return 0; // Never intersect trainlines (apart from with access types)
|
2021-02-26 19:15:48 +01:00
|
|
|
}
|
|
|
|
|
2021-02-04 20:43:32 +01:00
|
|
|
// 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.
|
2021-02-04 20:43:32 +01:00
|
|
|
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);
|
2021-02-04 20:43:32 +01:00
|
|
|
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
|
|
|
|
2021-02-04 20:43:32 +01: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
|
|
|
|
2020-05-03 00:00:41 +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);
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
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);
|
2021-02-04 23:36:37 +01:00
|
|
|
|
|
|
|
// 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);
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
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);
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
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
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
|
2021-02-04 23:36:37 +01: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
|
|
|
|
}
|
|
|
|
|
2021-02-04 23:36:37 +01:00
|
|
|
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,
|
2021-02-04 20:43:32 +01:00
|
|
|
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;
|
2021-02-04 23:36:37 +01:00
|
|
|
GList *l_last;
|
|
|
|
struct dijkstra_path *path_last = NULL;
|
2020-05-01 20:48:36 +02:00
|
|
|
|
2021-02-04 20:43:32 +01: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);
|
2021-02-04 23:36:37 +01:00
|
|
|
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
|
|
|
|
2021-02-04 23:36:37 +01: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
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
|
|
|
|
inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
|
2020-05-03 12:09:29 +02:00
|
|
|
struct dijkstra_path *path)
|
2020-05-03 00:00:41 +02:00
|
|
|
{
|
2020-05-03 12:09:29 +02:00
|
|
|
if (!node || !path) {
|
|
|
|
report(LL_CRITICAL, "Called get_connection with NULL node (%p) or path (%p).",
|
|
|
|
node, path);
|
|
|
|
}
|
2020-05-03 00:00:41 +02:00
|
|
|
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;
|
2021-03-04 22:01:13 +01:00
|
|
|
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;
|
|
|
|
}
|