Implement intersection algorithm

src/dijkstrasearch.c

@@ -0,0 +1,291 @@
+#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;
+}

src/dijkstrasearch.h

@@ -0,0 +1,48 @@
+#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

src/main.c

@@ -1,7 +1,47 @@
 #include <stdio.h>
+#include "dijkstrasearch.h"
+#include <glib/glist.h>
+
 
 int main()
 {
-	printf("Hello World!\n");
+	GList *nodes = NULL;
+	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;
 }