Compare commits
No commits in common. "f9e9726974e7a3119e2c401908bc31b3c6de1bfd" and "fa36deba031b6061432ff1d386ad6bbb0ade13ee" have entirely different histories.
f9e9726974
...
fa36deba03
@ -18,7 +18,3 @@ FILE(GLOB_RECURSE HEADERS "src/*.h")
|
|||||||
aux_source_directory(./src SRC_LIST)
|
aux_source_directory(./src SRC_LIST)
|
||||||
|
|
||||||
add_executable(${PROJECT_NAME} ${SRC_LIST} "${HEADERS}")
|
add_executable(${PROJECT_NAME} ${SRC_LIST} "${HEADERS}")
|
||||||
|
|
||||||
target_link_libraries(${PROJECT_NAME} ${SOUP_LIBRARIES} ${JSON_LIBRARIES})
|
|
||||||
|
|
||||||
install (TARGETS ${PROJECT_NAME} DESTINATION bin)
|
|
||||||
|
|||||||
@ -1,291 +0,0 @@
|
|||||||
#include "dijkstrasearch.h"
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <glib/glist.h>
|
|
||||||
#include <math.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);
|
|
||||||
|
|
||||||
return same;
|
|
||||||
}
|
|
||||||
|
|
||||||
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 *is_new)
|
|
||||||
{
|
|
||||||
struct dijkstra_node *node;
|
|
||||||
struct dijkstra_node *found;
|
|
||||||
|
|
||||||
node = dijkstra_node_new(x, y);
|
|
||||||
if (!node)
|
|
||||||
return NULL;
|
|
||||||
|
|
||||||
if ((found = dijkstra_node_exists(*list, node))) {
|
|
||||||
free(node);
|
|
||||||
node = found;
|
|
||||||
if (is_new)
|
|
||||||
*is_new = 0;
|
|
||||||
} else {
|
|
||||||
*list = g_list_append(*list, node);
|
|
||||||
if (is_new)
|
|
||||||
*is_new = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
return node;
|
|
||||||
}
|
|
||||||
|
|
||||||
int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination,
|
|
||||||
struct dijkstra_path *path)
|
|
||||||
{
|
|
||||||
if (!path || !destination)
|
|
||||||
return -1;
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// WARNING: This does not update the references in the nodes!!!
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
|
|
||||||
return dup;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path,
|
|
||||||
struct dijkstra_node *node,
|
|
||||||
int ref_only)
|
|
||||||
{
|
|
||||||
node->paths = g_list_remove(node->paths, (gpointer) node);
|
|
||||||
|
|
||||||
if (ref_only)
|
|
||||||
return node;
|
|
||||||
|
|
||||||
if (path->source == node)
|
|
||||||
path->source = NULL;
|
|
||||||
if (path->destination == node)
|
|
||||||
path->destination = NULL;
|
|
||||||
|
|
||||||
return node;
|
|
||||||
}
|
|
||||||
|
|
||||||
int dijkstra_path_intersect(GList **nodes,
|
|
||||||
GList **paths,
|
|
||||||
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;
|
|
||||||
|
|
||||||
float p0_x, p0_y, p1_x, p1_y;
|
|
||||||
float p2_x, p2_y, p3_x, p3_y;
|
|
||||||
float i_x, i_y;
|
|
||||||
|
|
||||||
float s1_x, s1_y, s2_x, s2_y;
|
|
||||||
float s, t;
|
|
||||||
|
|
||||||
int is_new;
|
|
||||||
|
|
||||||
if (!path_a || !path_b)
|
|
||||||
return -1;
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
node_new = dijkstra_node_new_to_list(nodes, i_x, i_y, &is_new);
|
|
||||||
|
|
||||||
if (node_new != path_a->source && node_new != path_a->destination) {
|
|
||||||
path_new = dijkstra_path_dup_shallow(path_a);
|
|
||||||
*paths = g_list_append(*paths, (gpointer) path_new);
|
|
||||||
|
|
||||||
dijkstra_disconnect_node_from_path(path_a, path_a->destination, 1);
|
|
||||||
dijkstra_connect_destination_node_to_path(node_new, path_a);
|
|
||||||
dijkstra_connect_source_node_to_path(node_new, path_new);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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);
|
|
||||||
*paths = g_list_append(*paths, (gpointer) path_new);
|
|
||||||
dijkstra_disconnect_node_from_path(path_b, path_b->destination, 1);
|
|
||||||
dijkstra_connect_destination_node_to_path(node_new, path_b);
|
|
||||||
dijkstra_connect_source_node_to_path(node_new, path_new);
|
|
||||||
}
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0; // No collision
|
|
||||||
}
|
|
||||||
|
|
||||||
struct dijkstra_path *dijkstra_path_new_to_list(GList **nodes,
|
|
||||||
GList **paths,
|
|
||||||
float x1,
|
|
||||||
float y1,
|
|
||||||
float x2,
|
|
||||||
float y2,
|
|
||||||
char *name,
|
|
||||||
dijkstra_cost weight)
|
|
||||||
{
|
|
||||||
struct dijkstra_node *n1, *n2;
|
|
||||||
struct dijkstra_path *path;
|
|
||||||
struct dijkstra_path *existing_path;
|
|
||||||
GList *l, *l_last;
|
|
||||||
|
|
||||||
n1 = dijkstra_node_new_to_list(nodes, x1, y1, NULL);
|
|
||||||
n2 = dijkstra_node_new_to_list(nodes, x2, y2, NULL);
|
|
||||||
path = dijkstra_path_new(name, n1, n2, weight);
|
|
||||||
|
|
||||||
l_last = g_list_last(*paths);
|
|
||||||
*paths = g_list_append(*paths, (gpointer) path);
|
|
||||||
|
|
||||||
for (l = *paths; l != NULL; l = l->next) {
|
|
||||||
existing_path = (struct dijkstra_path*) l->data;
|
|
||||||
dijkstra_path_intersect(nodes, paths, path, existing_path);
|
|
||||||
if (l == l_last) /* Avoid testing newly added paths */
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
return path;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*!
|
|
||||||
* \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)
|
|
||||||
{
|
|
||||||
struct dijkstra_node *node;
|
|
||||||
|
|
||||||
node = malloc(sizeof(*node));
|
|
||||||
if (node) {
|
|
||||||
node->position.x = x;
|
|
||||||
node->position.y = y;
|
|
||||||
node->paths = NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
return node;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*!
|
|
||||||
* \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;
|
|
||||||
path->weight = weight;
|
|
||||||
|
|
||||||
dijkstra_connect_nodes_to_path(source, destination, path);
|
|
||||||
}
|
|
||||||
|
|
||||||
return path;
|
|
||||||
}
|
|
||||||
@ -1,48 +0,0 @@
|
|||||||
#ifndef DIJKSTRASEARCH_H
|
|
||||||
#define DIJKSTRASEARCH_H
|
|
||||||
|
|
||||||
#include <glib.h>
|
|
||||||
|
|
||||||
typedef int dijkstra_cost;
|
|
||||||
|
|
||||||
struct position {
|
|
||||||
float x;
|
|
||||||
float y;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct dijkstra_path {
|
|
||||||
struct dijkstra_node *source;
|
|
||||||
struct dijkstra_node *destination;
|
|
||||||
char *name;
|
|
||||||
dijkstra_cost weight;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct dijkstra_node {
|
|
||||||
struct position position;
|
|
||||||
|
|
||||||
GList *paths;
|
|
||||||
|
|
||||||
dijkstra_cost cost;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct dijkstra_node *dijkstra_node_new(float x,
|
|
||||||
float y);
|
|
||||||
struct dijkstra_path *dijkstra_path_new(char *name,
|
|
||||||
struct dijkstra_node *source,
|
|
||||||
struct dijkstra_node *destination,
|
|
||||||
dijkstra_cost weight);
|
|
||||||
|
|
||||||
int dijkstra_path_intersect(GList **nodes,
|
|
||||||
GList **paths,
|
|
||||||
struct dijkstra_path *path_a,
|
|
||||||
struct dijkstra_path *path_b);
|
|
||||||
|
|
||||||
struct dijkstra_path *dijkstra_path_new_to_list(GList **nodes,
|
|
||||||
GList **paths,
|
|
||||||
float x1,
|
|
||||||
float y1,
|
|
||||||
float x2,
|
|
||||||
float y2,
|
|
||||||
char *name,
|
|
||||||
dijkstra_cost weight);
|
|
||||||
#endif // DIJKSTRASEARCH_H
|
|
||||||
42
src/main.c
42
src/main.c
@ -1,47 +1,7 @@
|
|||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include "dijkstrasearch.h"
|
|
||||||
#include <glib/glist.h>
|
|
||||||
|
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
GList *nodes = NULL;
|
printf("Hello World!\n");
|
||||||
GList *paths = NULL;
|
|
||||||
|
|
||||||
GList *l = NULL;
|
|
||||||
struct dijkstra_node *node;
|
|
||||||
struct dijkstra_path *path;
|
|
||||||
|
|
||||||
dijkstra_path_new_to_list(&nodes,
|
|
||||||
&paths,
|
|
||||||
0,0,
|
|
||||||
0,10,
|
|
||||||
"Vertical",
|
|
||||||
1);
|
|
||||||
|
|
||||||
dijkstra_path_new_to_list(&nodes,
|
|
||||||
&paths,
|
|
||||||
0,5,
|
|
||||||
10,5,
|
|
||||||
"Horizontal",
|
|
||||||
1);
|
|
||||||
|
|
||||||
for (l = nodes; l != NULL; l = l->next) {
|
|
||||||
node = (struct dijkstra_node*) l->data;
|
|
||||||
printf("Node %p: %f,\t%f [%d paths]\n", node, node->position.x, node->position.y, g_list_length(node->paths));
|
|
||||||
}
|
|
||||||
printf("\n");
|
|
||||||
|
|
||||||
for (l = paths; l != NULL; l = l->next) {
|
|
||||||
path = (struct dijkstra_path*) l->data;
|
|
||||||
printf("Path %p: %p (%f,%f) -> %p (%f,%f) \t\"%s\"\n", path,
|
|
||||||
path->source, path->source->position.x, path->source->position.y,
|
|
||||||
path->destination, path->destination->position.x, path->destination->position.y,
|
|
||||||
path->name);
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("\nDone.\n");
|
|
||||||
|
|
||||||
// TODO: Free memory
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user