Merge pull request #239 from kichikuou/pathfinding

TapirEngine: Implement pathfinding functions

include/reign.h

@@ -221,6 +221,10 @@ 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_optimize_path_line(struct RE_instance *instance);
+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);
 bool RE_motion_set_state(struct motion *motion, int state);

src/3d/3d_internal.h

@@ -131,6 +131,8 @@ struct outline_renderer {
 };
 
 struct RE_renderer {
+	int viewport_width;
+	int viewport_height;
 	GLuint program;
 	GLuint depth_buffer;
 	struct shadow_renderer shadow;
@@ -550,28 +552,24 @@ void opr_load(uint8_t *data, size_t size, struct pol *pol);
 
 // collision.c
 
+struct collider_triangle;
+struct collider_edge;
+
 struct collider {
 	struct collider_triangle *triangles;
 	uint32_t nr_triangles;
 	struct collider_edge *edges;  // boundary edges
 	uint32_t nr_edges;
-};
-
-struct collider_triangle {
-	vec2 vertices[3];  // xz coordinates
-	vec2 aabb[2];
-	vec2 slope;
-	float intercept;
-};
-
-struct collider_edge {
-	vec2 vertices[2];  // xz coordinates
-	vec2 aabb[2];
+	vec3 *path_points;
+	uint32_t nr_path_points;
 };
 
 struct collider *collider_create(struct pol_mesh *mesh);
 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);
+bool collider_optimize_path(struct collider *collider);
 
 #endif /* SYSTEM4_3D_3D_INTERNAL_H */

src/3d/collision.c

@@ -22,75 +22,111 @@
 
 #include "3d_internal.h"
 
-void init_triangles(struct collider *collider, struct pol_mesh *mesh)
+struct collider_triangle {
+	vec3 vertices[3];
+	vec3 center;
+	vec2 aabb[2];  // xz coordinates
+	vec2 slope;
+	float intercept;
+	int neighbors[3];
+};
+
+struct collider_edge {
+	vec2 vertices[2];  // xz coordinates
+	vec2 aabb[2];
+};
+
+static void init_triangles(struct collider *collider, struct pol_mesh *mesh)
 {
 	collider->nr_triangles = mesh->nr_triangles;
 	collider->triangles = xcalloc(mesh->nr_triangles, sizeof(struct collider_triangle));
 	struct collider_triangle *t = collider->triangles;
 	for (int tri_i = 0; tri_i < mesh->nr_triangles; tri_i++, t++) {
-		vec3 vertices[3];
 		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, vertices[i]);
-			t->vertices[i][0] = vertices[i][0];
-			t->vertices[i][1] = vertices[i][2];
-			glm_vec2_minv(t->vertices[i], t->aabb[0], t->aabb[0]);
-			glm_vec2_maxv(t->vertices[i], t->aabb[1], t->aabb[1]);
+			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(vertices[1], vertices[0], v1);
-		glm_vec3_sub(vertices[2], vertices[0], v2);
+		glm_vec3_sub(t->vertices[1], t->vertices[0], v1);
+		glm_vec3_sub(t->vertices[2], t->vertices[0], v2);
 		glm_vec3_cross(v1, v2, normal);
 		t->slope[0] = -normal[0] / normal[1];
 		t->slope[1] = -normal[2] / normal[1];
-		t->intercept = glm_vec3_dot(normal, vertices[0]) / normal[1];
+		t->intercept = glm_vec3_dot(normal, t->vertices[0]) / normal[1];
+
+		for (int i = 0; i < 3; i++) {
+			t->neighbors[i] = -1;
+		}
 	}
 }
 
-void init_edges(struct collider *collider, struct pol_mesh *mesh)
+static void link_neighbors(struct collider *collider, int t1_index, int t2_index)
 {
-	// Find boundary edges, i.e., edges that belong to only one triangle.
+	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)
+{
+	// 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)
@@ -98,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;
 }
 
@@ -106,15 +141,21 @@ void collider_free(struct collider *collider)
 {
 	free(collider->triangles);
 	free(collider->edges);
+	free(collider->path_points);
 	free(collider);
 }
 
 static bool in_triangle(struct collider_triangle* t, vec2 xz)
 {
+	vec2 v[3];
+	for (int i = 0; i < 3; i++) {
+		v[i][0] = t->vertices[i][0];
+		v[i][1] = t->vertices[i][2];
+	}
 	for (int i = 0; i < 3; i++) {
 		vec2 a, b;
-		glm_vec2_sub(t->vertices[(i + 1) % 3], t->vertices[i], a);
-		glm_vec2_sub(xz, t->vertices[i], b);
+		glm_vec2_sub(v[(i + 1) % 3], v[i], a);
+		glm_vec2_sub(xz, v[i], b);
 		if (glm_vec2_cross(a, b) > 0.f)
 			return false;
 	}
@@ -142,26 +183,44 @@ bool collider_height(struct collider *collider, vec2 xz, float *h_out)
 	return true;
 }
 
-static float distance_point_to_edge(vec2 p, struct collider_edge *e, vec2 closest_point_out)
+// Calculates the squared distance from a point to a line segment in 2D space.
+static float distance2_point_to_segment(vec2 p, vec2 a, vec2 b, vec2 closest_point_out)
 {
 	vec2 v, w;
-	glm_vec2_sub(e->vertices[1], e->vertices[0], v);
-	glm_vec2_sub(p, e->vertices[0], w);
+	glm_vec2_sub(b, a, v);
+	glm_vec2_sub(p, a, w);
 
 	float c1 = glm_vec2_dot(w, v);
 	if (c1 < 0.f) {
-		glm_vec2_copy(e->vertices[0], closest_point_out);
-		return glm_vec2_distance(p, e->vertices[0]);
+		glm_vec2_copy(a, closest_point_out);
+		return glm_vec2_distance2(p, a);
 	}
 	float c2 = glm_vec2_norm2(v);
 	if (c2 <= c1) {
-		glm_vec2_copy(e->vertices[1], closest_point_out);
-		return glm_vec2_distance(p, e->vertices[1]);
+		glm_vec2_copy(b, closest_point_out);
+		return glm_vec2_distance2(p, b);
 	}
 	float t = c1 / c2;
-	glm_vec2_copy(e->vertices[0], closest_point_out);
+	glm_vec2_copy(a, closest_point_out);
 	glm_vec2_muladds(v, t, closest_point_out);
-	return glm_vec2_distance(p, closest_point_out);
+	return glm_vec2_distance2(p, closest_point_out);
+}
+
+static bool segments_intersect(vec2 p0, vec2 p1, vec2 q0, vec2 q1)
+{
+	vec2 r, s;
+	glm_vec2_sub(p1, p0, r);
+	glm_vec2_sub(q1, q0, s);
+	float r_cross_s = glm_vec2_cross(r, s);
+	if (r_cross_s == 0.f) {
+		// The segments are collinear.
+		return false;
+	}
+	vec2 q0_p0;
+	glm_vec2_sub(q0, p0, q0_p0);
+	float t = glm_vec2_cross(q0_p0, s) / r_cross_s;
+	float u = glm_vec2_cross(q0_p0, r) / r_cross_s;
+	return (t >= 0.f && t <= 1.f && u >= 0.f && u <= 1.f);
 }
 
 bool check_collision(struct collider *collider, vec2 p0, vec2 p1, float radius, vec2 out)
@@ -198,8 +257,8 @@ bool check_collision(struct collider *collider, vec2 p0, vec2 p1, float radius,
 		if (!glm_aabb2d_aabb(e->aabb, aabb))
 			continue;
 		vec2 q;
-		float distance = distance_point_to_edge(out, e, q);
-		if (distance < radius) {
+		float sq_distance = distance2_point_to_segment(out, e->vertices[0], e->vertices[1], q);
+		if (sq_distance < radius * radius) {
 			// out = q + normalize(out - q) * radius
 			vec2 v;
 			glm_vec2_sub(out, q, v);
@@ -213,3 +272,238 @@ bool check_collision(struct collider *collider, vec2 p0, vec2 p1, float radius,
 	}
 	return true;
 }
+
+bool collider_raycast(struct collider *collider, vec3 origin, vec3 direction, vec3 out)
+{
+	struct collider_triangle* t = collider->triangles;
+	for (; t < collider->triangles + collider->nr_triangles; t++) {
+		float d;
+		if (glm_ray_triangle(origin, direction, t->vertices[0], t->vertices[1], t->vertices[2], &d)) {
+			glm_vec3_scale(direction, d, out);
+			glm_vec3_add(origin, out, out);
+			return true;
+		}
+	}
+	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;
+}
+
+// determines if a segment intersects with any collider edges, or is close enough to them.
+static bool test_segment(struct collider *collider, vec2 p0, vec2 p1, float threshold)
+{
+	vec2 aabb[2];
+	glm_vec2_minv(p0, p1, aabb[0]);
+	glm_vec2_maxv(p0, p1, aabb[1]);
+	aabb[0][0] -= threshold;
+	aabb[0][1] -= threshold;
+	aabb[1][0] += threshold;
+	aabb[1][1] += threshold;
+	float threshold2 = threshold * threshold;
+	for (int i = 0; i < collider->nr_edges; i++) {
+		struct collider_edge *e = &collider->edges[i];
+		if (!glm_aabb2d_aabb(e->aabb, aabb))
+			continue;
+		if (segments_intersect(p0, p1, e->vertices[0], e->vertices[1]))
+			return true;
+		vec2 closest_point;
+		float distance2 = distance2_point_to_segment(p0, e->vertices[0], e->vertices[1], closest_point);
+		if (distance2 < threshold2)
+			return true;
+		distance2 = distance2_point_to_segment(p1, e->vertices[0], e->vertices[1], closest_point);
+		if (distance2 < threshold2)
+			return true;
+		distance2 = distance2_point_to_segment(p0, p1, e->vertices[0], closest_point);
+		if (distance2 < threshold2)
+			return true;
+		distance2 = distance2_point_to_segment(p0, p1, e->vertices[1], closest_point);
+		if (distance2 < threshold2)
+			return true;
+	}
+	return false;
+}
+
+static void optimize_path_rec(struct collider *collider, vec3 *points, int start, int end)
+{
+	if (end - start >= 2) {
+		// If the segment (points[start], points[end]) does not intersect any
+		// collider edges, we can skip all points in between.
+		vec2 p0 = { points[start][0], points[start][2] };
+		vec2 p1 = { points[end][0], points[end][2] };
+		if (test_segment(collider, p0, p1, 0.5f)) {
+			int mid = (start + end) / 2;
+			optimize_path_rec(collider, points, start, mid);
+			optimize_path_rec(collider, points, mid, end);
+			return;
+		}
+	}
+	glm_vec3_copy(points[start], collider->path_points[collider->nr_path_points++]);
+}
+
+bool collider_optimize_path(struct collider *collider)
+{
+	if (collider->nr_path_points < 3)
+		return collider->nr_path_points > 0;  // nothing to optimize
+
+	int nr_points = collider->nr_path_points;
+	vec3 *points = xmalloc(nr_points * sizeof(vec3));
+	memcpy(points, collider->path_points, nr_points * sizeof(vec3));
+	collider->nr_path_points = 0;
+	optimize_path_rec(collider, points, 0, nr_points - 1);
+	glm_vec3_copy(points[nr_points - 1], collider->path_points[collider->nr_path_points++]);
+	free(points);
+	return true;
+}

src/3d/reign.c

@@ -604,6 +604,59 @@ 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_optimize_path_line(struct RE_instance *inst)
+{
+	if (!inst || !inst->model->collider)
+		return false;
+	return collider_optimize_path(inst->model->collider);
+}
+
+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)
+		return false;
+
+	float ndc_x = 2.f * mouse_x / inst->plugin->renderer->viewport_width - 1.f;
+	float ndc_y = 1.f - 2.f * mouse_y / inst->plugin->renderer->viewport_height;
+	vec4 near = { ndc_x, ndc_y, -1.f, 1.f };
+	vec4 far  = { ndc_x, ndc_y,  1.f, 1.f };
+
+	mat4 inv_vp;
+	RE_calc_view_matrix(&inst->plugin->camera, GLM_YUP, inv_vp);
+	glm_mat4_mul(inst->plugin->proj_transform, inv_vp, inv_vp);
+	glm_mat4_inv(inv_vp, inv_vp);
+
+	glm_mat4_mulv(inv_vp, near, near);
+	glm_mat4_mulv(inv_vp, far, far);
+	glm_vec3_divs(near, near[3], near);
+	glm_vec3_divs(far, far[3], far);
+
+	vec3 origin, direction;
+	glm_vec3_copy(near, origin);
+	glm_vec3_sub(far, near, direction);
+	glm_vec3_normalize(direction);
+
+	return collider_raycast(inst->model->collider, origin, direction, out);
+}
+
 void RE_instance_update_local_transform(struct RE_instance *inst)
 {
 	vec3 euler = {

src/3d/renderer.c

@@ -268,6 +268,8 @@ struct RE_renderer *RE_renderer_new(void)
 
 void RE_renderer_set_viewport_size(struct RE_renderer *r, int width, int height)
 {
+	r->viewport_width = width;
+	r->viewport_height = height;
 	glBindRenderbuffer(GL_RENDERBUFFER, r->depth_buffer);
 	glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, width, height);
 	glBindRenderbuffer(GL_RENDERBUFFER, 0);

src/hll/ReignEngine.c

@@ -23,6 +23,7 @@
 
 #include "hll.h"
 #include "reign.h"
+#include "vm/page.h"
 
 #define RE_MAX_PLUGINS 2
 
@@ -1836,10 +1837,59 @@ 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);
-//bool TapirEngine_CalcPathFinderIntersectEyeVec(int nPlugin, int nInstance, int nMouseX, int nMouseY, float *pfX, float *pfY, float *pfZ);
-//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_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)
+{
+	vec3 result;
+	if (!RE_instance_calc_path_finder_intersect_eye_vec(get_instance(plugin, instance), mouse_x, mouse_y, result))
+		return false;
+	*x_out = result[0];
+	*y_out = result[1];
+	*z_out = -result[2];
+	return true;
+}
+
+static bool TapirEngine_OptimizeInstancePathLine(int plugin, int instance)
+{
+	return RE_instance_optimize_path_line(get_instance(plugin, instance));
+}
+
+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);
 
@@ -2248,10 +2298,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_TODO_EXPORT(CalcPathFinderIntersectEyeVec, TapirEngine_CalcPathFinderIntersectEyeVec), \
-	    HLL_TODO_EXPORT(OptimizeInstancePathLine, TapirEngine_OptimizeInstancePathLine), \
-	    HLL_TODO_EXPORT(GetInstancePathLine, TapirEngine_GetInstancePathLine), \
+	    HLL_EXPORT(FindInstancePath, TapirEngine_FindInstancePath), \
+	    HLL_EXPORT(CalcPathFinderIntersectEyeVec, TapirEngine_CalcPathFinderIntersectEyeVec), \
+	    HLL_EXPORT(OptimizeInstancePathLine, TapirEngine_OptimizeInstancePathLine), \
+	    HLL_EXPORT(GetInstancePathLine, TapirEngine_GetInstancePathLine), \
 	    HLL_TODO_EXPORT(CreateInstancePathLineList, TapirEngine_CreateInstancePathLineList), \
 	    HLL_TODO_EXPORT(SetInstanceMeshShow, TapirEngine_SetInstanceMeshShow), \
 	    HLL_EXPORT(SetDrawOption, TapirEngine_SetDrawOption), \