TapirEngine: Implement pathfinding functions

TapirEngine.FindInstancePath() takes a starting point and an end point
and executes the A* algorithm on the collision mesh. The resulting path
can be obtained with TapirEngine.GetInstancePathLine().

include/reign.h

@@ -221,6 +221,8 @@ float RE_instance_calc_height(struct RE_instance *instance, float x, float z);
 float RE_instance_calc_2d_detection_height(struct RE_instance *instance, float x, float z);
 bool RE_instance_calc_2d_detection(struct RE_instance *instance, float x0, float y0, float z0, float x1, float y1, float z1, float *x2, float *y2, float *z2, float radius);
 bool RE_instance_set_debug_draw_shadow_volume(struct RE_instance *instance, bool draw);
+bool RE_instance_find_path(struct RE_instance *instance, vec3 start, vec3 goal);
+const vec3 *RE_instance_get_path_line(struct RE_instance *instance, int *nr_path_points);
 bool RE_instance_calc_path_finder_intersect_eye_vec(struct RE_instance *instance, int mouse_x, int mouse_y, vec3 out);
 
 int RE_motion_get_state(struct motion *motion);

src/3d/3d_internal.h

@@ -560,6 +560,8 @@ struct collider {
 	uint32_t nr_triangles;
 	struct collider_edge *edges;  // boundary edges
 	uint32_t nr_edges;
+	vec3 *path_points;
+	uint32_t nr_path_points;
 };
 
 struct collider *collider_create(struct pol_mesh *mesh);
@@ -567,5 +569,6 @@ void collider_free(struct collider *collider);
 bool collider_height(struct collider *collider, vec2 xz, float *h_out);
 bool check_collision(struct collider *collider, vec2 p0, vec2 p1, float radius, vec2 out);
 bool collider_raycast(struct collider *collider, vec3 origin, vec3 direction, vec3 out);
+bool collider_find_path(struct collider *collider, vec3 start, vec3 goal, mat4 vp_transform);
 
 #endif /* SYSTEM4_3D_3D_INTERNAL_H */

src/3d/collision.c

@@ -24,9 +24,11 @@
 
 struct collider_triangle {
 	vec3 vertices[3];
+	vec3 center;
 	vec2 aabb[2];  // xz coordinates
 	vec2 slope;
 	float intercept;
+	int neighbors[3];
 };
 
 struct collider_edge {
@@ -43,10 +45,13 @@ static void init_triangles(struct collider *collider, struct pol_mesh *mesh)
 		glm_aabb2d_invalidate(t->aabb);
 		for (int i = 0; i < 3; i++) {
 			glm_vec3_copy(mesh->vertices[mesh->triangles[tri_i].vert_index[i]].pos, t->vertices[i]);
+			glm_vec3_add(t->center, t->vertices[i], t->center);
 			vec2 xz = { t->vertices[i][0], t->vertices[i][2] };
 			glm_vec2_minv(xz, t->aabb[0], t->aabb[0]);
 			glm_vec2_maxv(xz, t->aabb[1], t->aabb[1]);
 		}
+		glm_vec3_divs(t->center, 3.f, t->center);
+
 		// Do some precomputation so that we can calculate height(x, z) quickly.
 		vec3 v1, v2, normal;
 		glm_vec3_sub(t->vertices[1], t->vertices[0], v1);
@@ -55,52 +60,73 @@ static void init_triangles(struct collider *collider, struct pol_mesh *mesh)
 		t->slope[0] = -normal[0] / normal[1];
 		t->slope[1] = -normal[2] / normal[1];
 		t->intercept = glm_vec3_dot(normal, t->vertices[0]) / normal[1];
+
+		for (int i = 0; i < 3; i++) {
+			t->neighbors[i] = -1;
+		}
+	}
+}
+
+static void link_neighbors(struct collider *collider, int t1_index, int t2_index)
+{
+	struct collider_triangle *t1 = &collider->triangles[t1_index];
+	struct collider_triangle *t2 = &collider->triangles[t2_index];
+	for (int i = 0; i < 3; i++) {
+		if (t1->neighbors[i] == -1) {
+			t1->neighbors[i] = t2_index;
+			break;
+		}
+	}
+	for (int i = 0; i < 3; i++) {
+		if (t2->neighbors[i] == -1) {
+			t2->neighbors[i] = t1_index;
+			break;
+		}
 	}
 }
 
 static void init_edges(struct collider *collider, struct pol_mesh *mesh)
 {
-	// Find boundary edges, i.e., edges that belong to only one triangle.
+	// Link neighboring triangles.
+	if (mesh->nr_triangles >= 65535)
+		ERROR("Too many triangles in collision mesh: %d", mesh->nr_triangles);
 	const int nv = mesh->nr_vertices;
-	const int bv_size = (nv * nv + 31) / 32;
-	uint32_t *bv = xcalloc(bv_size, 4);
+	const int table_size = nv * nv;
+	uint16_t *table = xcalloc(table_size, sizeof(uint16_t));
 	int nr_edges = 0;
 	for (int i = 0; i < mesh->nr_triangles; i++) {
 		for (int j = 0; j < 3; j++) {
 			int v1 = mesh->triangles[i].vert_index[j];
 			int v2 = mesh->triangles[i].vert_index[(j + 1) % 3];
-			int b = v2 * nv + v1;
-			if (bv[b >> 5] & 1U << (b & 31)) {
-				bv[b >> 5] &= ~(1U << (b & 31));
+			int k = v2 * nv + v1;
+			if (table[k]) {
+				link_neighbors(collider, table[k] - 1, i);
+				table[k] = 0;
 				nr_edges--;
 			} else {
-				b = v1 * nv + v2;
-				bv[b >> 5] |= 1U << (b & 31);
+				table[v1 * nv + v2] = i + 1;
 				nr_edges++;
 			}
 		}
 	}
+	// Collect boundary edges, i.e., edges that belong to only one triangle.
 	collider->nr_edges = nr_edges;
 	collider->edges = xcalloc(nr_edges, sizeof(struct collider_edge));
 	struct collider_edge* edge = collider->edges;
-	for (int i = 0; i < bv_size; i++) {
-		if (!bv[i]) continue;
-		for (int j = 0; j < 32; j++) {
-			if (!(bv[i] & 1U << j)) continue;
-			int b = i << 5 | j;
-			int v1 = b / nv;
-			int v2 = b % nv;
-			edge->vertices[0][0] = mesh->vertices[v1].pos[0];
-			edge->vertices[0][1] = mesh->vertices[v1].pos[2];
-			edge->vertices[1][0] = mesh->vertices[v2].pos[0];
-			edge->vertices[1][1] = mesh->vertices[v2].pos[2];
-			glm_vec2_minv(edge->vertices[0], edge->vertices[1], edge->aabb[0]);
-			glm_vec2_maxv(edge->vertices[0], edge->vertices[1], edge->aabb[1]);
-			edge++;
-		}
+	for (int i = 0; i < table_size; i++) {
+		if (!table[i]) continue;
+		int v1 = i / nv;
+		int v2 = i % nv;
+		edge->vertices[0][0] = mesh->vertices[v1].pos[0];
+		edge->vertices[0][1] = mesh->vertices[v1].pos[2];
+		edge->vertices[1][0] = mesh->vertices[v2].pos[0];
+		edge->vertices[1][1] = mesh->vertices[v2].pos[2];
+		glm_vec2_minv(edge->vertices[0], edge->vertices[1], edge->aabb[0]);
+		glm_vec2_maxv(edge->vertices[0], edge->vertices[1], edge->aabb[1]);
+		edge++;
 	}
 	assert(edge == collider->edges + nr_edges);
-	free(bv);
+	free(table);
 }
 
 struct collider *collider_create(struct pol_mesh *mesh)
@@ -108,7 +134,6 @@ struct collider *collider_create(struct pol_mesh *mesh)
 	struct collider *collider = xcalloc(1, sizeof(struct collider));
 	init_triangles(collider, mesh);
 	init_edges(collider, mesh);
-	NOTICE("collider: %d triangles %d edges", collider->nr_triangles, collider->nr_edges);
 	return collider;
 }
 
@@ -116,6 +141,7 @@ void collider_free(struct collider *collider)
 {
 	free(collider->triangles);
 	free(collider->edges);
+	free(collider->path_points);
 	free(collider);
 }
 
@@ -242,3 +268,158 @@ bool collider_raycast(struct collider *collider, vec3 origin, vec3 direction, ve
 	}
 	return false;
 }
+
+struct pathfinder_node {
+	int pred;
+	float g_score;
+	enum { UNDISCOVERED, IN_FRONTIER, VISITED, NOT_VISIBLE } state;
+};
+
+struct frontier {
+	int triangle_index;
+	float f_score;
+};
+
+struct pathfinder {
+	struct pathfinder_node *nodes;  // indexed by triangle index
+	struct frontier *frontiers;  // min-heap
+	int nr_frontiers;
+};
+
+static int frontier_pop(struct pathfinder *pf)
+{
+	if (pf->nr_frontiers == 0) {
+		return -1;
+	}
+	int index = pf->frontiers[0].triangle_index;
+	if (--pf->nr_frontiers > 0) {
+		pf->frontiers[0] = pf->frontiers[pf->nr_frontiers];
+		int i = 0;
+		while (i < pf->nr_frontiers) {
+			int left = 2 * i + 1;
+			int right = 2 * i + 2;
+			int smallest = i;
+			if (left < pf->nr_frontiers && pf->frontiers[left].f_score < pf->frontiers[smallest].f_score) {
+				smallest = left;
+			}
+			if (right < pf->nr_frontiers && pf->frontiers[right].f_score < pf->frontiers[smallest].f_score) {
+				smallest = right;
+			}
+			if (smallest == i) {
+				break;
+			}
+			struct frontier tmp = pf->frontiers[i];
+			pf->frontiers[i] = pf->frontiers[smallest];
+			pf->frontiers[smallest] = tmp;
+			i = smallest;
+		}
+	}
+	return index;
+}
+
+static void frontier_push(struct pathfinder *pf, int index, float f_score)
+{
+	pf->frontiers[pf->nr_frontiers++] = (struct frontier){ index, f_score };
+	int i = pf->nr_frontiers - 1;
+	while (i > 0) {
+		int parent = (i - 1) / 2;
+		if (pf->frontiers[i].f_score >= pf->frontiers[parent].f_score) {
+			break;
+		}
+		struct frontier tmp = pf->frontiers[i];
+		pf->frontiers[i] = pf->frontiers[parent];
+		pf->frontiers[parent] = tmp;
+		i = parent;
+	}
+}
+
+static bool is_point_visible(vec3 point, mat4 vp_transform)
+{
+	vec4 p = { point[0], point[1], point[2], 1.f };
+	glm_mat4_mulv(vp_transform, p, p);
+	// Check if the point is in the view frustum.
+	return p[0] >= -p[3] && p[0] <= p[3] &&
+	       p[1] >= -p[3] && p[1] <= p[3] &&
+	       p[2] >= -p[3] && p[2] <= p[3];
+}
+
+bool collider_find_path(struct collider *collider, vec3 start, vec3 goal, mat4 vp_transform)
+{
+	if (collider->path_points) {
+		collider->nr_path_points = 0;
+		free(collider->path_points);
+		collider->path_points = NULL;
+	}
+
+	// A* search for a graph with triangle centers as nodes and neighbors as edges.
+	struct collider_triangle* start_t = find_triangle(collider, (vec2){ start[0], start[2] });
+	if (!start_t || !is_point_visible(start_t->center, vp_transform))
+		return false;
+	struct collider_triangle* goal_t = find_triangle(collider, (vec2){ goal[0], goal[2] });
+	if (!goal_t || !is_point_visible(goal_t->center, vp_transform))
+		return false;
+
+	int start_i = start_t - collider->triangles;
+	int goal_i = goal_t - collider->triangles;
+	struct pathfinder pf;
+	pf.nodes = xcalloc(collider->nr_triangles, sizeof(struct pathfinder_node));
+	pf.nodes[goal_i].pred = -2;
+	pf.nodes[start_i].pred = -1;
+	pf.nodes[start_i].g_score = 0.f;
+	pf.nodes[start_i].state = IN_FRONTIER;
+	pf.frontiers = xcalloc(collider->nr_triangles, sizeof(struct frontier));
+	pf.nr_frontiers = 0;
+	frontier_push(&pf, start_i, glm_vec3_distance(start_t->center, goal_t->center));
+
+	while (pf.nr_frontiers > 0) {
+		int current_i = frontier_pop(&pf);
+		if (pf.nodes[current_i].state == VISITED)
+			continue;
+		struct collider_triangle *current_t = &collider->triangles[current_i];
+		if (current_i == goal_i)
+			break;
+		pf.nodes[current_i].state = VISITED;
+		for (int j = 0; j < 3; j++) {
+			int neighbor_i = current_t->neighbors[j];
+			if (neighbor_i == -1)
+				continue;
+			struct collider_triangle *neighbor_t = &collider->triangles[neighbor_i];
+			struct pathfinder_node *neighbor_n = &pf.nodes[neighbor_i];
+			if (neighbor_n->state == NOT_VISIBLE)
+				continue;
+			if (neighbor_n->state == UNDISCOVERED && !is_point_visible(neighbor_t->center, vp_transform)) {
+				neighbor_n->state = NOT_VISIBLE;
+				continue;
+			}
+			float g = pf.nodes[current_i].g_score + glm_vec3_distance(current_t->center, neighbor_t->center);
+			if (neighbor_n->state == UNDISCOVERED || g < neighbor_n->g_score) {
+				neighbor_n->pred = current_i;
+				neighbor_n->g_score = g;
+				neighbor_n->state = IN_FRONTIER;
+				float f_score = g + glm_vec3_distance(neighbor_t->center, goal_t->center);
+				frontier_push(&pf, neighbor_i, f_score);
+			}
+		}
+	}
+	if (pf.nodes[goal_i].pred != -2) {
+		// Reconstruct the path.
+		int path_length = 0;
+		int current_i = goal_i;
+		while (current_i != -1) {
+			path_length++;
+			current_i = pf.nodes[current_i].pred;
+		}
+		collider->nr_path_points = path_length + 2;
+		collider->path_points = xcalloc(path_length + 2, sizeof(vec3));
+		glm_vec3_copy(start, collider->path_points[0]);
+		glm_vec3_copy(goal, collider->path_points[path_length + 1]);
+		current_i = goal_i;
+		for (int i = path_length - 1; i >= 0; i--) {
+			glm_vec3_copy(collider->triangles[current_i].center, collider->path_points[i + 1]);
+			current_i = pf.nodes[current_i].pred;
+		}
+	}
+	free(pf.nodes);
+	free(pf.frontiers);
+	return !!collider->path_points;
+}

src/3d/reign.c

@@ -604,6 +604,24 @@ bool RE_instance_set_debug_draw_shadow_volume(struct RE_instance *inst, bool dra
 	return true;
 }
 
+bool RE_instance_find_path(struct RE_instance *inst, vec3 start, vec3 goal)
+{
+	if (!inst || !inst->model->collider)
+		return false;
+	mat4 vp_transform;
+	RE_calc_view_matrix(&inst->plugin->camera, GLM_YUP, vp_transform);
+	glm_mat4_mul(inst->plugin->proj_transform, vp_transform, vp_transform);
+	return collider_find_path(inst->model->collider, start, goal, vp_transform);
+}
+
+const vec3 *RE_instance_get_path_line(struct RE_instance *inst, int *nr_path_points)
+{
+	if (!inst || !inst->model->collider)
+		return NULL;
+	*nr_path_points = inst->model->collider->nr_path_points;
+	return inst->model->collider->path_points;
+}
+
 bool RE_instance_calc_path_finder_intersect_eye_vec(struct RE_instance *inst, int mouse_x, int mouse_y, vec3 out)
 {
 	if (!inst || !inst->model->collider)

src/hll/ReignEngine.c

@@ -23,6 +23,7 @@
 
 #include "hll.h"
 #include "reign.h"
+#include "vm/page.h"
 
 #define RE_MAX_PLUGINS 2
 
@@ -1836,7 +1837,12 @@ static bool TapirEngine_CalcInstance2DDetection(int plugin, int instance, float
 	return RE_instance_calc_2d_detection(get_instance(plugin, instance), x0, y0, z0, x1, y1, z1, x2, y2, z2, radius);
 }
 
-//bool TapirEngine_FindInstancePath(int PluginNumber, int InstanceNumber, float StartX, float StartY, float StartZ, float GoalX, float GoalY, float GoalZ);
+static bool TapirEngine_FindInstancePath(int plugin, int instance, float start_x, float start_y, float start_z, float goal_x, float goal_y, float goal_z)
+{
+	vec3 start = { start_x, start_y, -start_z };
+	vec3 goal = { goal_x, goal_y, -goal_z };
+	return RE_instance_find_path(get_instance(plugin, instance), start, goal);
+}
 
 static bool TapirEngine_CalcPathFinderIntersectEyeVec(int plugin, int instance, int mouse_x, int mouse_y, float *x_out, float *y_out, float *z_out)
 {
@@ -1850,7 +1856,37 @@ static bool TapirEngine_CalcPathFinderIntersectEyeVec(int plugin, int instance,
 }
 
 //bool TapirEngine_OptimizeInstancePathLine(int PluginNumber, int InstanceNumber);
-//bool TapirEngine_GetInstancePathLine(int PluginNumber, int InstanceNumber, struct page **pIXArray, struct page **pIYArray, struct page **pIZArray);
+
+static bool TapirEngine_GetInstancePathLine(int plugin, int instance, struct page **x_array, struct page **y_array, struct page **z_array)
+{
+	int nr_path_points;
+	const vec3 *path_points = RE_instance_get_path_line(get_instance(plugin, instance), &nr_path_points);
+	if (!path_points)
+		return false;
+	if (*x_array) {
+		delete_page_vars(*x_array);
+		free_page(*x_array);
+	}
+	if (*y_array) {
+		delete_page_vars(*y_array);
+		free_page(*y_array);
+	}
+	if (*z_array) {
+		delete_page_vars(*z_array);
+		free_page(*z_array);
+	}
+	union vm_value dim = { .i = nr_path_points };
+	*x_array = alloc_array(1, &dim, AIN_ARRAY_FLOAT, 0, false);
+	*y_array = alloc_array(1, &dim, AIN_ARRAY_FLOAT, 0, false);
+	*z_array = alloc_array(1, &dim, AIN_ARRAY_FLOAT, 0, false);
+	for (int i = 0; i < nr_path_points; i++) {
+		(*x_array)->values[i].f = path_points[i][0];
+		(*y_array)->values[i].f = path_points[i][1];
+		(*z_array)->values[i].f = -path_points[i][2];
+	}
+	return true;
+}
+
 //bool TapirEngine_CreateInstancePathLineList(int PluginNumber, int InstanceNumber, int PathInstanceNumber);
 //bool TapirEngine_SetInstanceMeshShow(int PluginNumber, int InstanceNumber, struct string *pIMeshName, bool Show);
 
@@ -2259,10 +2295,10 @@ HLL_LIBRARY(ReignEngine, REIGN_EXPORTS,
 	    HLL_TODO_EXPORT(GetInstanceDrawParam, TapirEngine_GetInstanceDrawParam), \
 	    HLL_EXPORT(CalcInstance2DDetectionHeight, TapirEngine_CalcInstance2DDetectionHeight), \
 	    HLL_EXPORT(CalcInstance2DDetection, TapirEngine_CalcInstance2DDetection), \
-	    HLL_TODO_EXPORT(FindInstancePath, TapirEngine_FindInstancePath), \
+	    HLL_EXPORT(FindInstancePath, TapirEngine_FindInstancePath), \
 	    HLL_EXPORT(CalcPathFinderIntersectEyeVec, TapirEngine_CalcPathFinderIntersectEyeVec), \
 	    HLL_TODO_EXPORT(OptimizeInstancePathLine, TapirEngine_OptimizeInstancePathLine), \
-	    HLL_TODO_EXPORT(GetInstancePathLine, TapirEngine_GetInstancePathLine), \
+	    HLL_EXPORT(GetInstancePathLine, TapirEngine_GetInstancePathLine), \
 	    HLL_TODO_EXPORT(CreateInstancePathLineList, TapirEngine_CreateInstancePathLineList), \
 	    HLL_TODO_EXPORT(SetInstanceMeshShow, TapirEngine_SetInstanceMeshShow), \
 	    HLL_EXPORT(SetDrawOption, TapirEngine_SetDrawOption), \