From 6fbd147c54432117a160dd0af8eedb8b77befa06 Mon Sep 17 00:00:00 2001 From: kichikuou Date: Mon, 26 May 2025 10:06:13 +0900 Subject: [PATCH 1/3] Implement TapirEngine.CalcPathFinderIntersectEyeVec It returns the position on the collision mesh corresponding to the mouse coordinates. --- include/reign.h | 1 + src/3d/3d_internal.h | 18 +++++--------- src/3d/collision.c | 55 +++++++++++++++++++++++++++++++++---------- src/3d/reign.c | 28 ++++++++++++++++++++++ src/3d/renderer.c | 2 ++ src/hll/ReignEngine.c | 15 ++++++++++-- 6 files changed, 92 insertions(+), 27 deletions(-) diff --git a/include/reign.h b/include/reign.h index 2717d25..345b59d 100644 --- a/include/reign.h +++ b/include/reign.h @@ -221,6 +221,7 @@ 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_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); diff --git a/src/3d/3d_internal.h b/src/3d/3d_internal.h index 4175210..1a38ebe 100644 --- a/src/3d/3d_internal.h +++ b/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,6 +552,9 @@ 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; @@ -557,21 +562,10 @@ struct collider { 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]; -}; - 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); #endif /* SYSTEM4_3D_3D_INTERNAL_H */ diff --git a/src/3d/collision.c b/src/3d/collision.c index 91ecfd0..ba04100 100644 --- a/src/3d/collision.c +++ b/src/3d/collision.c @@ -22,33 +22,43 @@ #include "3d_internal.h" -void init_triangles(struct collider *collider, struct pol_mesh *mesh) +struct collider_triangle { + vec3 vertices[3]; + vec2 aabb[2]; // xz coordinates + vec2 slope; + float intercept; +}; + +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]); + 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]); } // 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]; } } -void init_edges(struct collider *collider, struct pol_mesh *mesh) +static void init_edges(struct collider *collider, struct pol_mesh *mesh) { // Find boundary edges, i.e., edges that belong to only one triangle. const int nv = mesh->nr_vertices; @@ -111,10 +121,15 @@ void collider_free(struct collider *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; } @@ -213,3 +228,17 @@ 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; +} diff --git a/src/3d/reign.c b/src/3d/reign.c index db1a712..e76aec9 100644 --- a/src/3d/reign.c +++ b/src/3d/reign.c @@ -604,6 +604,34 @@ bool RE_instance_set_debug_draw_shadow_volume(struct RE_instance *inst, bool dra return true; } +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 = { diff --git a/src/3d/renderer.c b/src/3d/renderer.c index 78b053c..7ef36dd 100644 --- a/src/3d/renderer.c +++ b/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); diff --git a/src/hll/ReignEngine.c b/src/hll/ReignEngine.c index b765ff4..e6123f8 100644 --- a/src/hll/ReignEngine.c +++ b/src/hll/ReignEngine.c @@ -1837,7 +1837,18 @@ static bool TapirEngine_CalcInstance2DDetection(int plugin, int instance, float } //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); + +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; +} + //bool TapirEngine_OptimizeInstancePathLine(int PluginNumber, int InstanceNumber); //bool TapirEngine_GetInstancePathLine(int PluginNumber, int InstanceNumber, struct page **pIXArray, struct page **pIYArray, struct page **pIZArray); //bool TapirEngine_CreateInstancePathLineList(int PluginNumber, int InstanceNumber, int PathInstanceNumber); @@ -2249,7 +2260,7 @@ HLL_LIBRARY(ReignEngine, REIGN_EXPORTS, HLL_EXPORT(CalcInstance2DDetectionHeight, TapirEngine_CalcInstance2DDetectionHeight), \ HLL_EXPORT(CalcInstance2DDetection, TapirEngine_CalcInstance2DDetection), \ HLL_TODO_EXPORT(FindInstancePath, TapirEngine_FindInstancePath), \ - HLL_TODO_EXPORT(CalcPathFinderIntersectEyeVec, TapirEngine_CalcPathFinderIntersectEyeVec), \ + HLL_EXPORT(CalcPathFinderIntersectEyeVec, TapirEngine_CalcPathFinderIntersectEyeVec), \ HLL_TODO_EXPORT(OptimizeInstancePathLine, TapirEngine_OptimizeInstancePathLine), \ HLL_TODO_EXPORT(GetInstancePathLine, TapirEngine_GetInstancePathLine), \ HLL_TODO_EXPORT(CreateInstancePathLineList, TapirEngine_CreateInstancePathLineList), \ From 919b86916ca915e1c2582fc0f5a6c7ecdfcce2ec Mon Sep 17 00:00:00 2001 From: kichikuou Date: Tue, 27 May 2025 08:09:43 +0900 Subject: [PATCH 2/3] 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), \ From 378450c625c905a034d73f3b417f93c9c06f630e Mon Sep 17 00:00:00 2001 From: kichikuou Date: Tue, 27 May 2025 12:35:02 +0900 Subject: [PATCH 3/3] Implement TapirEngine.OptimizeInstancePathLine It simplifies the path calculated by TapirEngine.FindInstancePath() by removing unnecessary points. --- include/reign.h | 1 + src/3d/3d_internal.h | 1 + src/3d/collision.c | 106 +++++++++++++++++++++++++++++++++++++----- src/3d/reign.c | 7 +++ src/hll/ReignEngine.c | 7 ++- 5 files changed, 109 insertions(+), 13 deletions(-) diff --git a/include/reign.h b/include/reign.h index f080509..029bdaa 100644 --- a/include/reign.h +++ b/include/reign.h @@ -223,6 +223,7 @@ bool RE_instance_calc_2d_detection(struct RE_instance *instance, float x0, float 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); diff --git a/src/3d/3d_internal.h b/src/3d/3d_internal.h index b07f58d..a5287e7 100644 --- a/src/3d/3d_internal.h +++ b/src/3d/3d_internal.h @@ -570,5 +570,6 @@ 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 */ diff --git a/src/3d/collision.c b/src/3d/collision.c index 1ba01ee..e6ed262 100644 --- a/src/3d/collision.c +++ b/src/3d/collision.c @@ -183,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) @@ -239,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); @@ -423,3 +441,69 @@ bool collider_find_path(struct collider *collider, vec3 start, vec3 goal, mat4 v 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; +} diff --git a/src/3d/reign.c b/src/3d/reign.c index 5ef1898..1c415be 100644 --- a/src/3d/reign.c +++ b/src/3d/reign.c @@ -622,6 +622,13 @@ const vec3 *RE_instance_get_path_line(struct RE_instance *inst, int *nr_path_poi 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) diff --git a/src/hll/ReignEngine.c b/src/hll/ReignEngine.c index 0fc78d0..8ec90e7 100644 --- a/src/hll/ReignEngine.c +++ b/src/hll/ReignEngine.c @@ -1855,7 +1855,10 @@ static bool TapirEngine_CalcPathFinderIntersectEyeVec(int plugin, int instance, return true; } -//bool TapirEngine_OptimizeInstancePathLine(int PluginNumber, int InstanceNumber); +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) { @@ -2297,7 +2300,7 @@ HLL_LIBRARY(ReignEngine, REIGN_EXPORTS, HLL_EXPORT(CalcInstance2DDetection, TapirEngine_CalcInstance2DDetection), \ HLL_EXPORT(FindInstancePath, TapirEngine_FindInstancePath), \ HLL_EXPORT(CalcPathFinderIntersectEyeVec, TapirEngine_CalcPathFinderIntersectEyeVec), \ - HLL_TODO_EXPORT(OptimizeInstancePathLine, TapirEngine_OptimizeInstancePathLine), \ + HLL_EXPORT(OptimizeInstancePathLine, TapirEngine_OptimizeInstancePathLine), \ HLL_EXPORT(GetInstancePathLine, TapirEngine_GetInstancePathLine), \ HLL_TODO_EXPORT(CreateInstancePathLineList, TapirEngine_CreateInstancePathLineList), \ HLL_TODO_EXPORT(SetInstanceMeshShow, TapirEngine_SetInstanceMeshShow), \