From e9f3139f0e0e9e82b4d34b95bd246390163264de Mon Sep 17 00:00:00 2001 From: Ian Date: Sat, 18 Nov 2023 09:22:10 -0500 Subject: [PATCH] NX/VITA: Pathfinding performance improvement --- source/mathlib.c | 9 +-- source/mathlib.h | 1 + source/pr_cmds.c | 152 +++++++++++++++++++++++++++++----------------- source/pr_edict.c | 3 + source/quakedef.h | 4 ++ source/sv_main.c | 128 +++++++++++++++++++------------------- 6 files changed, 172 insertions(+), 125 deletions(-) diff --git a/source/mathlib.c b/source/mathlib.c index 300e526..8f5bfaa 100644 --- a/source/mathlib.c +++ b/source/mathlib.c @@ -352,18 +352,19 @@ vec_t Length(vec3_t v) float VectorNormalize (vec3_t v) { - float ilength; + float length, ilength; - ilength = rsqrt(DotProduct(v,v)); + length = sqrt(DotProduct(v,v)); - if (ilength) + if (length) { + ilength = 1/length; v[0] *= ilength; v[1] *= ilength; v[2] *= ilength; } - return ilength; + return length; } void VectorInverse (vec3_t v) diff --git a/source/mathlib.h b/source/mathlib.h index ba40f05..28018d0 100644 --- a/source/mathlib.h +++ b/source/mathlib.h @@ -68,6 +68,7 @@ static inline int IS_NAN (float x) { #define VectorNegate(a, b) ((b)[0] = -(a)[0], (b)[1] = -(a)[1], (b)[2] = -(a)[2]) #define RAD2DEG( x ) ((float)(x) * (float)(180.f / M_PI)) #define DEG2RAD( a ) ( a * M_PI ) / 180.0F //sB porting seperate viewmodel fov +#define VectorDistanceSquared(a,b)((a[0]-b[0])*(a[0]-b[0])+(a[1]-b[1])*(a[1]-b[1])+(a[2]-b[2])*(a[2]-b[2])) //johnfitz -- courtesy of lordhavoc // QuakeSpasm: To avoid strict aliasing violations, use a float/int union instead of type punning. diff --git a/source/pr_cmds.c b/source/pr_cmds.c index 604dbb1..718f3af 100644 --- a/source/pr_cmds.c +++ b/source/pr_cmds.c @@ -2302,7 +2302,7 @@ int CheckIfWayInList (int listnumber, int waynum) float heuristic_cost_estimate (int start_way, int end_way) { //for now we will just look the distance between. - return VecLength2(waypoints[start_way].origin, waypoints[end_way].origin); + return VectorDistanceSquared(waypoints[start_way].origin, waypoints[end_way].origin); } @@ -2338,6 +2338,7 @@ void reconstruct_path(int start_node, int current_node) //Con_DPrintf("reconstruct_path: waypoints[current].came_from %i is in list!\n", waypoints[current].came_from); for (i = 0;i < 8; i++) { + if (waypoints[waypoints[current].came_from].target_id[i] < 0) break; //Con_DPrintf("reconstruct_path for loop: waypoints[waypoints[current].came_from].target_id[i] = %i, current = %i\n", waypoints[waypoints[current].came_from].target_id[i], current) if (waypoints[waypoints[current].came_from].target_id[i] == current) { @@ -2355,11 +2356,11 @@ void reconstruct_path(int start_node, int current_node) s++; } Con_DPrintf("\nreconstruct_path: dumping the final list\n"); - for (s = MAX_WAYPOINTS - 1; s > -1; s--) + /*for (s = MAX_WAYPOINTS - 1; s > -1; s--) { //if (proces_list[s]) //Con_DPrintf("reconstruct_path final: s = %i, proces_list[s] = %i\n", s, proces_list[s]); - } + }*/ } int Pathfind (int start_way, int end_way)//note thease 2 are ARRAY locations. Not the waypoints names. @@ -2401,8 +2402,8 @@ int Pathfind (int start_way, int end_way)//note thease 2 are ARRAY locations. No for (i = 0;i < 8; i++) { - //Con_DPrintf("Pathfind for start\n"); + if (waypoints[current].target_id[i] < 0) break; if (!waypoints[waypoints[current].target_id[i]].open) { @@ -2454,70 +2455,111 @@ Do_Pathfind float Do_Pathfind (entity zombie, entity target) ================= */ +float max_waypoint_distance = 750; +short closest_waypoints[MAX_EDICTS]; + void Do_Pathfind (void) { - float best_dist; - float dist; - int i, s, best, best_target; + int i, s; trace_t trace; - edict_t *ent; - edict_t *zombie; - int entnum; - entnum = G_EDICTNUM(OFS_PARM0); - - best = 0; Con_DPrintf("Starting Do_Pathfind\n"); //we first need to look for closest point for both zombie and the player - zombie = G_EDICT(OFS_PARM0); - ent = G_EDICT(OFS_PARM1); - best_dist = 1000000000; - dist = 0; + int zombie_entnum = G_EDICTNUM(OFS_PARM0); + int target_entnum = G_EDICTNUM(OFS_PARM1); + edict_t * zombie = G_EDICT(OFS_PARM0); + edict_t * ent = G_EDICT(OFS_PARM1); - for (i = 0; i < MAX_WAYPOINTS; i++) - { - if (waypoints[i].used && waypoints[i].open) - { + float best_dist_z = max_waypoint_distance * max_waypoint_distance; + float dist_z = 0; + int best_z = -1; + float best_dist_e = max_waypoint_distance * max_waypoint_distance; + float dist_e = 0; + int best_e = -1; + + int prevclosest = closest_waypoints[zombie_entnum]; + if (prevclosest >= 0) { + trace = SV_Move (zombie->v.origin, vec3_origin, vec3_origin, waypoints[prevclosest].origin, 1, zombie); + if (trace.fraction >= 1) { + dist_z = VectorDistanceSquared(waypoints[prevclosest].origin, zombie->v.origin); + best_dist_z = dist_z; + best_z = prevclosest; + } else { + for (s = 0; s < 8; s++) { + int neighbor = waypoints[prevclosest].target_id[s]; + if (neighbor < 0) break; + + dist_z = VectorDistanceSquared(waypoints[neighbor].origin, zombie->v.origin); + if (dist_z < best_dist_z) { + trace = SV_Move (zombie->v.origin, vec3_origin, vec3_origin, waypoints[neighbor].origin, 1, zombie); + if (trace.fraction >= 1) { + best_dist_z = dist_z; + best_z = neighbor; + break; + } + } + } + } + } + + // copypasta, forgive me + prevclosest = closest_waypoints[target_entnum]; + if (prevclosest >= 0) { + trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[prevclosest].origin, 1, ent); + if (trace.fraction >= 1) { + dist_e = VectorDistanceSquared(waypoints[prevclosest].origin, ent->v.origin); + best_dist_e = dist_e; + best_e = prevclosest; + } else { + for (s = 0; s < 8; s++) { + int neighbor = waypoints[prevclosest].target_id[s]; + if (neighbor < 0) break; + + dist_e = VectorDistanceSquared(waypoints[neighbor].origin, ent->v.origin); + if (dist_e < best_dist_e) { + trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[neighbor].origin, 1, ent); + if (trace.fraction >= 1) { + best_dist_e = dist_e; + best_e = neighbor; + break; + } + } + } + } + } + + for (i = 0; i < MAX_WAYPOINTS; i++) { + if (!waypoints[i].used || !waypoints[i].open) + continue; + + dist_z = VectorDistanceSquared(waypoints[i].origin, zombie->v.origin); + if (dist_z < best_dist_z) { trace = SV_Move (zombie->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, zombie); - if (trace.fraction >= 1) - { - dist = VecLength2(waypoints[i].origin, zombie->v.origin); - - if(dist < best_dist) - { - best_dist = dist; - best = i; - } + if (trace.fraction >= 1) { + best_dist_z = dist_z; + best_z = i; } } - } - best_dist = 1000000000; - dist = 0; - best_target = 0; - for (i = 0; i < MAX_WAYPOINTS; i++) - { - if (waypoints[i].used && waypoints[i].open) - { + dist_e = VectorDistanceSquared(waypoints[i].origin, ent->v.origin); + if (dist_e < best_dist_e) { trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, ent); - if (trace.fraction >= 1) - { - dist = VecLength2(waypoints[i].origin, ent->v.origin); - - if(dist < best_dist) - { - best_dist = dist; - best_target = i; - } + if (trace.fraction >= 1) { + best_dist_e = dist_e; + best_e = i; } } } - Con_DPrintf("Starting waypoint: %i, Ending waypoint: %i\n", best, best_target); - if (Pathfind(best, best_target)) + + closest_waypoints[zombie_entnum] = best_z; + closest_waypoints[target_entnum] = best_e; + + Con_DPrintf("Starting waypoint: %i, Ending waypoint: %i\n", best_z, best_e); + if (Pathfind(best_z, best_e)) { for (i = 0; i < MaxZombies; i++) { - if (entnum == zombie_list[i].zombienum) + if (zombie_entnum == zombie_list[i].zombienum) { for (s = 0; s < MAX_WAYPOINTS; s++) { @@ -2531,7 +2573,7 @@ void Do_Pathfind (void) { if (!zombie_list[i].zombienum) { - zombie_list[i].zombienum = entnum; + zombie_list[i].zombienum = zombie_entnum; for (s = 0; s < MAX_WAYPOINTS; s++) { zombie_list[i].pathlist[s] = proces_list[s]; @@ -2648,13 +2690,13 @@ void Get_Waypoint_Near (void) { if (waypoints[i].open) { - trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, ent); - dist = VecLength2(waypoints[i].origin, ent->v.origin); + dist = VecLength2(waypoints[i].origin, ent->v.origin); //Con_DPrintf("Waypoint: %i, distance: %f, fraction: %f\n", i, dist, trace.fraction); - if (trace.fraction >= 1) + if(dist < best_dist) { - if(dist < best_dist) + trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, ent); + if (trace.fraction >= 1) { best_dist = dist; best = i; diff --git a/source/pr_edict.c b/source/pr_edict.c index 4ddf883..2576096 100644 --- a/source/pr_edict.c +++ b/source/pr_edict.c @@ -144,6 +144,9 @@ FIXME: walk all entities and NULL out references to this entity */ void ED_Free (edict_t *ed) { + // pathfind optimization: + closest_waypoints[NUM_FOR_EDICT(ed)] = -1; + SV_UnlinkEdict (ed); // unlink from world bsp ed->free = true; diff --git a/source/quakedef.h b/source/quakedef.h index 2055c7d..2f599b9 100644 --- a/source/quakedef.h +++ b/source/quakedef.h @@ -337,6 +337,9 @@ typedef struct #include "cdaudio.h" #include "glquake.h" +#include + +extern short closest_waypoints[MAX_EDICTS]; //============================================================================= @@ -434,6 +437,7 @@ typedef struct } waypoint_ai; extern waypoint_ai waypoints[MAX_WAYPOINTS]; +extern short closest_waypoints[MAX_EDICTS]; #endif /* QUAKEDEFS_H */ diff --git a/source/sv_main.c b/source/sv_main.c index 09f98ae..533e73f 100644 --- a/source/sv_main.c +++ b/source/sv_main.c @@ -1668,12 +1668,22 @@ void Load_Waypoint () Con_DPrintf("No waypoint file (%s/maps/%s.way) found\n", com_gamedir, sv.name); return; } - for (i = 1; i < MAX_WAYPOINTS; i++) + for (i = 0; i < MAX_WAYPOINTS; i++) { waypoints[i].used = 0; + waypoints[i].id = -1; + for (p = 0; p < 8; p++) { + waypoints[i].target[p] = -1; + waypoints[i].target_id[p] = -1; + } } - i = 1; + for (i = 0; i < MAX_EDICTS; i++) + { + closest_waypoints[i] = -1; + } + + i = 0; Con_DPrintf("Loading waypoints\n"); while (1) { @@ -1684,8 +1694,6 @@ void Load_Waypoint () } else { - if (i == MAX_WAYPOINTS) - Sys_Error ("Maximum waypoints loaded {%i)\n", MAX_WAYPOINTS); W_fgets (h); W_stov (W_substring (W_fgets (h), 9, 20), d); @@ -1693,7 +1701,13 @@ void Load_Waypoint () strcpy(temp, W_substring (W_fgets (h), 5, 20)); i = atoi (temp); - waypoints[i].id = atoi (temp); + + if (i >= MAX_WAYPOINTS) + Sys_Error ("Waypoint with id %d past MAX_WAYPOINTS {%i)\n", i, MAX_WAYPOINTS); + + // what's the point of id and index being the same? + waypoints[i].id = i; + VectorCopy (d, waypoints[i].origin); strcpy(waypoints[i].special, W_substring (W_fgets (h), 10, 20)); @@ -1703,35 +1717,18 @@ void Load_Waypoint () else waypoints[i].open = 1; - strcpy(temp, W_substring (W_fgets (h), 9, 20)); - waypoints[i].target[0] = atoi (temp); - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[1] = atoi (temp); - - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[2] = atoi (temp); - - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[3] = atoi (temp); - - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[4] = atoi (temp); - - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[5] = atoi (temp); - - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[6] = atoi (temp); - - - strcpy(temp, W_substring (W_fgets (h), 10, 20)); - waypoints[i].target[7] = atoi (temp); + // Note: this block makes sure that empty/invalid neighbors are always packed to the end + // In other words, when iterating from start, first empty means rest are empty too. + int slot = 0; + for (int t = 0; t < 8; t++) { + int start = t == 0 ? 9 : 10; + strcpy(temp, W_substring (W_fgets (h), start, 20)); + if (isdigit(temp[0])) { + waypoints[i].target[slot] = atoi (temp); + waypoints[i].target_id[slot] = waypoints[i].target[slot]; + slot++; + } + } W_fgets (h); W_fgets (h); @@ -1754,48 +1751,47 @@ void Load_Waypoint () } } Con_DPrintf("Total waypoints: %i\n", i); - for (i = 1;i < MAX_WAYPOINTS; i++) //for sake of saving time later we are now going to save each targets array position and distace to each waypoint + for (i = 0;i < MAX_WAYPOINTS; i++) //for sake of saving time later we are now going to save each targets array position and distace to each waypoint { for (p = 0;waypoints[i].target[p]; p++) { - for (s = 1; s < MAX_WAYPOINTS; s++) + if (waypoints[i].target[p] < 0) break; + + for (s = 0; s < MAX_WAYPOINTS; s++) { - if (s == MAX_WAYPOINTS) - Sys_Error ("Waypoint (%i) without a target!\n", s); - if (waypoints[i].target[p] == waypoints[s].id) + if (waypoints[i].target[p] == s) { waypoints[i].dist[p] = VecLength2(waypoints[s].origin, waypoints[i].origin); - waypoints[i].target_id[p] = s; break; } } } - Con_DPrintf("Waypoint (%i)target: %i (%i, %f), target2: %i (%i, %f), target3: %i (%i, %f), target4: %i (%i, %f), target5: %i (%i, %f), target6: %i (%i, %f), target7: %i (%i, %f), target8: %i (%i, %f)\n", - waypoints[i].id, - waypoints[i].target[0], - waypoints[i].target_id[0], - waypoints[i].dist[0], - waypoints[i].target[1], - waypoints[i].target_id[1], - waypoints[i].dist[1], - waypoints[i].target[2], - waypoints[i].target_id[2], - waypoints[i].dist[2], - waypoints[i].target[3], - waypoints[i].target_id[3], - waypoints[i].dist[3], - waypoints[i].target[4], - waypoints[i].target_id[4], - waypoints[i].dist[4], - waypoints[i].target[5], - waypoints[i].target_id[5], - waypoints[i].dist[5], - waypoints[i].target[6], - waypoints[i].target_id[6], - waypoints[i].dist[6], - waypoints[i].target[7], - waypoints[i].target_id[7], - waypoints[i].dist[7]); + // Con_DPrintf("Waypoint (%i)target: %i (%i, %f), target2: %i (%i, %f), target3: %i (%i, %f), target4: %i (%i, %f), target5: %i (%i, %f), target6: %i (%i, %f), target7: %i (%i, %f), target8: %i (%i, %f)\n", + // waypoints[i].id, + // waypoints[i].target[0], + // waypoints[i].target_id[0], + // waypoints[i].dist[0], + // waypoints[i].target[1], + // waypoints[i].target_id[1], + // waypoints[i].dist[1], + // waypoints[i].target[2], + // waypoints[i].target_id[2], + // waypoints[i].dist[2], + // waypoints[i].target[3], + // waypoints[i].target_id[3], + // waypoints[i].dist[3], + // waypoints[i].target[4], + // waypoints[i].target_id[4], + // waypoints[i].dist[4], + // waypoints[i].target[5], + // waypoints[i].target_id[5], + // waypoints[i].dist[5], + // waypoints[i].target[6], + // waypoints[i].target_id[6], + // waypoints[i].dist[6], + // waypoints[i].target[7], + // waypoints[i].target_id[7], + // waypoints[i].dist[7]); } W_fclose(h); //Z_Free (w_string_temp);