From 919b86916ca915e1c2582fc0f5a6c7ecdfcce2ec Mon Sep 17 00:00:00 2001 From: kichikuou Date: Tue, 27 May 2025 08:09:43 +0900 Subject: [PATCH] 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 | 2 + src/3d/3d_internal.h | 3 + src/3d/collision.c | 231 +++++++++++++++++++++++++++++++++++++----- src/3d/reign.c | 18 ++++ src/hll/ReignEngine.c | 44 +++++++- 5 files changed, 269 insertions(+), 29 deletions(-) diff --git a/include/reign.h b/include/reign.h index 345b59d..f080509 100644 --- a/include/reign.h +++ b/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); diff --git a/src/3d/3d_internal.h b/src/3d/3d_internal.h index 1a38ebe..b07f58d 100644 --- a/src/3d/3d_internal.h +++ b/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 */ diff --git a/src/3d/collision.c b/src/3d/collision.c index ba04100..1ba01ee 100644 --- a/src/3d/collision.c +++ b/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; +} diff --git a/src/3d/reign.c b/src/3d/reign.c index e76aec9..5ef1898 100644 --- a/src/3d/reign.c +++ b/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) diff --git a/src/hll/ReignEngine.c b/src/hll/ReignEngine.c index e6123f8..0fc78d0 100644 --- a/src/hll/ReignEngine.c +++ b/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), \