From 91d516960dc84f8c1bbe8ed834b4b404ab9ac8bc Mon Sep 17 00:00:00 2001 From: Tyler Young Date: Sun, 4 Aug 2024 20:13:17 -0400 Subject: [PATCH] merge blubs micro ai revamp tested in a round 20 game in warehouse2. --- source/mathlib.h | 3 + source/pr_cmds.c | 1424 ++++++++++++++++++++++++--------------------- source/quakedef.h | 30 +- source/sv_main.c | 315 ++++++---- 4 files changed, 986 insertions(+), 786 deletions(-) diff --git a/source/mathlib.h b/source/mathlib.h index 28018d0..6abdb8b 100644 --- a/source/mathlib.h +++ b/source/mathlib.h @@ -61,6 +61,7 @@ static inline int IS_NAN (float x) { #define DotProduct(x,y) (x[0]*y[0]+x[1]*y[1]+x[2]*y[2]) #define DoublePrecisionDotProduct(x,y) ((double)x[0]*y[0]+(double)x[1]*y[1]+(double)x[2]*y[2]) +#define VectorLerp( v1, lerp, v2, c ) ((c)[0] = (v1)[0] + (lerp) * ((v2)[0] - (v1)[0]), (c)[1] = (v1)[1] + (lerp) * ((v2)[1] - (v1)[1]), (c)[2] = (v1)[2] + (lerp) * ((v2)[2] - (v1)[2])) #define VectorSubtract(a,b,c) {c[0]=a[0]-b[0];c[1]=a[1]-b[1];c[2]=a[2]-b[2];} #define VectorAdd(a,b,c) {c[0]=a[0]+b[0];c[1]=a[1]+b[1];c[2]=a[2]+b[2];} #define VectorCopy(a,b) {b[0]=a[0];b[1]=a[1];b[2]=a[2];} @@ -69,6 +70,8 @@ static inline int IS_NAN (float x) { #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])) +#define VectorMax(a,b,out) {out[0]=a[0]>b[0]?a[0]:b[0]; out[1]=a[1]>b[1]?a[1]:b[1]; out[2]=a[2]>b[2]?a[2]:b[2];} +#define VectorMin(a,b,out) {out[0]=a[0] min) - { - if(max - min == 1) - { - int i; - for(i = opensetLength; i > max ; i--) - { - openset[i] = openset[i-1]; + // Special logic for waypoint open-set + if(set == WAYPOINT_SET_OPEN) { + int min = -1; + int max = openset_length; + int test; + float way_f_score = waypoints[waypoint_idx].f_score; + float test_f_score; + + // Binary insert into the open set + while(max > min) { + if(max - min == 1) { + // Shift elements up in the sorted openset_waypoints list + for(int i = openset_length; i > max ; i--) { + openset_waypoints[i] = openset_waypoints[i-1]; } - openset[max] = waynum; - opensetLength += 1; - opensetRef[waynum] = 1; - //printSortedOpenSet(); for debug only - return max; + openset_waypoints[max] = waypoint_idx; + openset_length += 1; + // sv_way_print_sorted_open_set(); // For debug only + break; } test = (int)((min + max)/2); - if(wayVal > waypoints[openset[test]].f_score) - { + test_f_score = waypoints[openset_waypoints[test]].f_score; + if(way_f_score > test_f_score) { min = test; } - else if(wayVal < waypoints[openset[test]].f_score) - { + else if(way_f_score < test_f_score) { max = test; } - if(wayVal == waypoints[openset[test]].f_score) - { + else if(way_f_score == test_f_score) { max = test; min = test - 1; } } } + + // Assign the waypoint to the set + waypoint_set[waypoint_idx] = set; +} + +// +// Returns the waypoint with the lowest F-score from the open-set, or -1 if the open-set is empty. +// +int sv_way_get_lowest_f_score_openset_waypoint() { + if(openset_length > 0) { + return openset_waypoints[0]; + } return -1; } -int GetLowestFromOpenSet() -{ - return openset[0]; -} +// +// Return `true` if a set contains 0 waypoints, `false` otherwise +// +qboolean sv_way_is_set_empty(char set) { + // Special case for openset + if(set == WAYPOINT_SET_OPEN) { + return (openset_length == 0); + } -int CheckIfEmptyList (int listnumber) -{ - int i; - - for (i = 0; i < MAX_WAYPOINTS; i++) - { - if (listnumber == 1) - { - if (closedset[i]) - { - //Con_DPrintf ("CheckIfEmptyList: closedset[%i]\n", i); - return 0; - } - } - else if (listnumber == 2) - { - if (openset[i]) - { - //Con_DPrintf ("CheckIfEmptyList: openset[%i]\n", i); - return 0; - } + // Check if any waypoints belong to this set + for (int i = 0; i < n_waypoints; i++) { + if(waypoint_set[i] == set) { + return false; } } - return 1; + return true; } -int CheckIfWayInList (int listnumber, int waynum) -{ - if(listnumber == 1) - { - if(closedset[waynum]) - { - //Con_DPrintf ("CheckIfWayInList: closedset[%i] = %i\n", waynum, 1); - return 1; - } - } - if(listnumber == 2) - { - if(opensetRef[waynum]) - { - //Con_DPrintf ("CheckIfWayInList: openset[%i] = %i\n", waynum, 1); - return 1; - } - } - return 0; +// +// Return `true` if waypoint `waypoint_idx` belongs to set `set` +// +qboolean sv_way_in_set(char set, int waypoint_idx) { + return (waypoint_set[waypoint_idx] == set); } -float heuristic_cost_estimate (int start_way, int end_way) -{ - //for now we will just look the distance between. - return VectorDistanceSquared(waypoints[start_way].origin, waypoints[end_way].origin); +// +// Compute A* heuristic between two waypoints +// +float sv_way_heuristic_cost_estimate(int waypoint_idx_a, int waypoint_idx_b) { + // Compute distance squared between: + return VectorDistanceSquared(waypoints[waypoint_idx_a].origin, waypoints[waypoint_idx_b].origin); } +// Global array in which to store pathfinding results +int process_list[MAX_WAYPOINTS]; +int process_list_length; +// +// Follows the path found by `Pathfind()` invocation, storing result path i global `process_list` +// +void sv_way_reconstruct_path(int start_node, int current_node) { + process_list_length = 0; -int proces_list[MAX_WAYPOINTS]; - -void reconstruct_path(int start_node, int current_node) -{ - int i, s, current; - current = current_node; - s = 0; - - Con_DPrintf ("\n"); - Con_DPrintf ("reconstruct_path: start_node = %i, current_node = %i\n\n", start_node, current_node); - for (i = 0;i < MAX_WAYPOINTS; i++) - { - //if (closedset[i]) - // Con_DPrintf ("reconstruct_path: closedset[%i] = %i\n", i, closedset[i]); - proces_list[i] = 0; - } - proces_list[s] = -1;//-1 means the enemy is the last waypoint - s = 1; - while (1) - { + // loop through the waypoints on the path + while (current_node >= 0) { //Con_DPrintf("\nreconstruct_path: current = %i, waypoints[current].came_from = %i\n", current, waypoints[current].came_from); - proces_list[s] = current;//blubs, we now add the first waypoint to the path list - if (current == start_node) - { - Con_DPrintf("reconstruct_path: path done!\n"); + // Add the current waypoint to the path list + process_list[process_list_length] = current_node; + process_list_length++; + + if (current_node == start_node) { break; } - if (CheckIfWayInList (1, waypoints[current].came_from)) - { - //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) - { - //Con_DPrintf("reconstruct_path: current %i is viable target!\n", current); - current = waypoints[current].came_from;//woohoo, this waypoint is viable. So set it now as the current one - break; - } - } - } - else - { - //Con_DPrintf("reconstruct_path: skipped waypoint %i\n", waypoints[current].came_from); - break; - } - s++; + current_node = waypoints[current_node].came_from; } - Con_DPrintf("\nreconstruct_path: dumping the final list\n"); - /*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. -{ - int current, last_way;//current is for the waypoint array, last_way is a way that was used last +// +// start_way -- Start waypoint index in global waypoints array +// end_way -- End waypoint index in global waypoints array +// +int sv_way_pathfind(int start_way, int end_way) { + int current; float tentative_g_score, tentative_f_score; int i; - last_way = 0; - for (i = 0; i < MAX_WAYPOINTS;i++)// clear all the waypoints - { - openset[i] = 0; - opensetRef[i] = 0; - closedset[i] = 0; + // -------------–-------------–-------------–-------------– + // Clear the path data for all waypoints + // -------------–-------------–-------------–-------------– + for (i = 0; i < n_waypoints; i++) { + waypoint_set[i] = WAYPOINT_SET_NONE; waypoints[i].f_score = 0; waypoints[i].g_score = 0; - waypoints[i].came_from = 0; + waypoints[i].came_from = -1; } - opensetLength = 0; + openset_length = 0; + // -------------–-------------–-------------–-------------– - waypoints[start_way].g_score = 0; // Cost from start along best known path. - // Estimated total cost from start to goal through y. - waypoints[start_way].f_score = waypoints[start_way].g_score + heuristic_cost_estimate(start_way, end_way); + // Cost from start along best known path. + waypoints[start_way].g_score = 0; + // Estimated total cost from start to goal through y + waypoints[start_way].f_score = waypoints[start_way].g_score + sv_way_heuristic_cost_estimate(start_way, end_way); - AddWayToList (2, start_way);// The set of tentative nodes to be evaluated, initially containing the start node + // The set of tentative nodes to be evaluated, initially containing the start node + sv_way_add_way_to_set(WAYPOINT_SET_OPEN, start_way); + + while (!sv_way_is_set_empty(WAYPOINT_SET_OPEN)) { + current = sv_way_get_lowest_f_score_openset_waypoint(); - while (!CheckIfEmptyList (2)) - { - //Con_DPrintf("\n"); - current = GetLowestFromOpenSet(); //Con_DPrintf("Pathfind current: %i, f_score: %f, g_score: %f\n", current, waypoints[current].f_score, waypoints[current].g_score); - if (current == end_way) - { - Con_DPrintf("Pathfind goal reached\n"); - reconstruct_path(start_way, end_way); + if (current == end_way) { + sv_way_reconstruct_path(start_way, end_way); return 1; } - AddWayToList (1, current); - RemoveWayFromList (2, current); + sv_way_remove_way_from_set(WAYPOINT_SET_OPEN, current); + sv_way_add_way_to_set(WAYPOINT_SET_CLOSED, current); - for (i = 0;i < 8; i++) - { - //Con_DPrintf("Pathfind for start\n"); - if (waypoints[current].target_id[i] < 0) break; + // Add each neighbor to the open set + for (i = 0;i < 8; i++) { + int neighbor_waypoint_idx = waypoints[current].target[i]; - if (!waypoints[waypoints[current].target_id[i]].open) - { + // Skip unused neighbor slots + if (neighbor_waypoint_idx < 0) { + break; + } + + // Check if waypoint is enabled (e.g. door waypoints) + if (!waypoints[neighbor_waypoint_idx].open) { //if (waypoints[current].target_id[i]) //Con_DPrintf("Pathfind for: %i, waypoints[waypoints[current].target_id[i]].open = %i, current = %i\n", waypoints[current].target_id[i], waypoints[waypoints[current].target_id[i]].open, current); continue; } - tentative_g_score = waypoints[current].g_score + waypoints[current].dist[i]; - tentative_f_score = tentative_g_score + heuristic_cost_estimate(waypoints[current].target_id[i], end_way); - //Con_DPrintf("Pathfind for: %i, t_f_score: %f, t_g_score: %f\n", waypoints[current].target_id[i], tentative_f_score, tentative_g_score); - - //if (CheckIfWayInList (1, waypoints[current].target_id[i]) && tentative_f_score >= waypoints[waypoints[current].target_id[i]].f_score) - if (CheckIfWayInList (1, waypoints[current].target_id[i]))//it was the above, but why do we care about this waypoint if it's already in the closed list? we never check 2 waypoints twice m8, the first iteration that we reach this waypoint is also the fastest way, so lets not EVER check it again. - { - //if (CheckIfWayInList (1, waypoints[current].target_id[i])) - //Con_DPrintf("Pathfind: waypoint %i in closed list\n", waypoints[current].target_id[i]); + // If this waypoint is already in the closed set, skip it + if (sv_way_in_set(WAYPOINT_SET_CLOSED, neighbor_waypoint_idx)) { continue; } + tentative_g_score = waypoints[current].g_score + waypoints[current].dist[i]; + tentative_f_score = tentative_g_score + sv_way_heuristic_cost_estimate(neighbor_waypoint_idx, end_way); - if(tentative_f_score < waypoints[waypoints[current].target_id[i]].f_score) - { - //Con_DPrintf("Pathfind waypoint is better\n"); - waypoints[waypoints[current].target_id[i]].g_score = tentative_g_score; - waypoints[waypoints[current].target_id[i]].f_score = tentative_f_score; + if (sv_way_in_set(WAYPOINT_SET_OPEN, neighbor_waypoint_idx)) { + if(tentative_f_score < waypoints[neighbor_waypoint_idx].f_score) { + waypoints[neighbor_waypoint_idx].g_score = tentative_g_score; + waypoints[neighbor_waypoint_idx].f_score = tentative_f_score; + waypoints[neighbor_waypoint_idx].came_from = current; + // The score has been updated, remove and re-insert into its new location in the sorted open-set + sv_way_remove_way_from_set(WAYPOINT_SET_OPEN, neighbor_waypoint_idx); + sv_way_add_way_to_set(WAYPOINT_SET_OPEN, neighbor_waypoint_idx); + } } - - if (!CheckIfWayInList (2, waypoints[current].target_id[i])) - { - //Con_DPrintf("Pathfind waypoint not in list\n"); - waypoints[waypoints[current].target_id[i]].g_score = tentative_g_score; - waypoints[waypoints[current].target_id[i]].f_score = tentative_f_score; - - waypoints[waypoints[current].target_id[i]].came_from = current; - AddWayToList (2, waypoints[current].target_id[i]); - //Con_DPrintf("Pathfind: %i added to the openset with waypoints[current].came_from = %i, current = %i\n", waypoints[current].target_id[i], waypoints[current].came_from, current); + else { + waypoints[neighbor_waypoint_idx].g_score = tentative_g_score; + waypoints[neighbor_waypoint_idx].f_score = tentative_f_score; + waypoints[neighbor_waypoint_idx].came_from = current; + sv_way_add_way_to_set(WAYPOINT_SET_OPEN, neighbor_waypoint_idx); } } - last_way = current; } return 0; } - /* ================= -Do_Pathfind +Get_Waypoint_Near -float Do_Pathfind (entity zombie, entity target) +vector Get_Waypoint_Near (entity) ================= */ -float max_waypoint_distance = 750; -short closest_waypoints[MAX_EDICTS]; -void Do_Pathfind (void) -{ - int i, s; +void Get_Waypoint_Near (void) { + float best_dist; + float dist; + int i, best; trace_t trace; + edict_t *ent; - Con_DPrintf("Starting Do_Pathfind\n"); //we first need to look for closest point for both zombie and the player - - 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); - - 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; - } - } - } - } - } + best = 0; + Con_DPrintf("Starting Get_Waypoint_Near\n"); + ent = G_EDICT(OFS_PARM0); + best_dist = 1000000000; + dist = 0; 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) { - best_dist_z = dist_z; - best_z = i; - } - } + if (waypoints[i].open) { + dist = VecLength2(waypoints[i].origin, ent->v.origin); + if(dist < best_dist) { + trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, ent); - 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) { - best_dist_e = dist_e; - best_e = i; - } - } - } - - 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 (zombie_entnum == zombie_list[i].zombienum) - { - for (s = 0; s < MAX_WAYPOINTS; s++) - { - zombie_list[i].pathlist[s] = proces_list[s]; + //Con_DPrintf("Waypoint: %i, distance: %f, fraction: %f\n", i, dist, trace.fraction); + if (trace.fraction >= 1) { + best_dist = dist; + best = i; } - break; - } - if (i == MaxZombies - 1)//zombie was not in list - { - for (i = 0; i < MaxZombies; i++) - { - if (!zombie_list[i].zombienum) - { - zombie_list[i].zombienum = zombie_entnum; - for (s = 0; s < MAX_WAYPOINTS; s++) - { - zombie_list[i].pathlist[s] = proces_list[s]; - } - break; - } - } - break; } } - - if(zombie_list[i].pathlist[2] == 0 && zombie_list[i].pathlist[1] != 0)//then we are at player's waypoint! - { - Con_DPrintf("We are at player's waypoint already!\n"); - G_FLOAT(OFS_RETURN) = -1; - return; - } - - Con_DPrintf("Path found!\n"); - G_FLOAT(OFS_RETURN) = 1; - } - else - { - Con_DPrintf("Path not found!\n"); - G_FLOAT(OFS_RETURN) = 0; } + Con_DPrintf("'%5.1f %5.1f %5.1f', %f is %f, (%i, %i)\n", waypoints[best].origin[0],waypoints[best].origin[1], waypoints[best].origin[2], best_dist, dist, i, best); + VectorCopy (waypoints[best].origin, G_VECTOR(OFS_RETURN)); } /* @@ -2765,24 +2597,21 @@ Open_Waypoint void Open_Waypoint (string, string, string, string, string, string, string, string) ================= */ -void Open_Waypoint (void) -{ - int i, t; +void Open_Waypoint (void) { + int i; char *p = G_STRING(OFS_PARM0); //Con_DPrintf("Open_Waypoint\n"); - for (i = 1; i < MAX_WAYPOINTS; i++) - { - if (waypoints[i].special[0])//no need to open without tag - { - if (!strcmp(p, waypoints[i].special)) - { + for (i = 0; i < MAX_WAYPOINTS; i++) { + //no need to open without tag + if (waypoints[i].special[0]) { + if (!strcmp(p, waypoints[i].special)) { waypoints[i].open = 1; //Con_DPrintf("Open_Waypoint: %i, opened\n", i); - t = 1; } - else + else { continue; + } } } //if (t == 0) @@ -2800,71 +2629,254 @@ void Close_Waypoint (string, string, string, string, string, string, string, str cypress - basically a carbon copy of open_waypoint lol ================= */ -void Close_Waypoint (void) -{ - int i, t; +void Close_Waypoint (void) { + int i; char *p = G_STRING(OFS_PARM0); - for (i = 1; i < MAX_WAYPOINTS; i++) - { - if (waypoints[i].special[0])//no need to open without tag - { - if (!strcmp(p, waypoints[i].special)) - { + for (i = 0; i < MAX_WAYPOINTS; i++) { + //no need to open without tag + if (waypoints[i].special[0]) { + if (!strcmp(p, waypoints[i].special)) { waypoints[i].open = 0; - t = 1; } - else + else { continue; + } } } } - /* ================= -Get_Waypoint_Near +Do_Pathfind -vector Get_Waypoint_Near (entity) +float Do_Pathfind (entity zombie, entity target) ================= */ -void Get_Waypoint_Near (void) -{ - float best_dist; - float dist; - int i, best; - trace_t trace; - edict_t *ent; +// #define MEASURE_PF_PERF +float max_waypoint_distance = 750; +short closest_waypoints[MAX_EDICTS]; - best = 0; - Con_DPrintf("Starting Get_Waypoint_Near\n"); - ent = G_EDICT(OFS_PARM0); - best_dist = 1000000000; - dist = 0; - for (i = 0; i < MAX_WAYPOINTS; i++) - { - if (waypoints[i].open) - { - dist = VecLength2(waypoints[i].origin, ent->v.origin); - //Con_DPrintf("Waypoint: %i, distance: %f, fraction: %f\n", i, dist, trace.fraction); - 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; - } - } +// +// Returns true iff we can tracebox from (start + [0,0,ofs]) to (end + [0,0,ofs]) +// + +// Dynamic hull sizes for hit detection cause chaos on movement code. Treat all AI ents as same size as player hull for movement +vec3_t ai_hull_mins = {-16, -16, -36}; +vec3_t ai_hull_maxs = { 16, 16, 40}; + +qboolean ofs_tracebox(vec3_t start, vec3_t mins, vec3_t maxs, vec3_t end, int type, edict_t *ignore_ent) { + trace_t trace; + vec3_t start_ofs; + vec3_t end_ofs; + VectorCopy(start, start_ofs); + VectorCopy(end, end_ofs); + start_ofs[2] += 8; // Move 8qu up to work better on uneven terrain + end_ofs[2] += 8; + trace = SV_Move(start_ofs, mins, maxs, end_ofs, type, ignore_ent); + return (trace.fraction >= 1); +} + + + + + +// +// Returns the clsoest waypoint to an entity that the entity can walk to +// Sorts all waypoints by distance, returns first waypoint we can tracebox to +// +int get_closest_waypoint(int entnum) { + edict_t *ent = EDICT_NUM(entnum); + + vec3_t ent_mins; + vec3_t ent_maxs; + // VectorMin(ent->v.mins, ai_hull_mins, ent_mins); + // VectorMax(ent->v.maxs, ai_hull_maxs, ent_maxs); + VectorCopy(ai_hull_mins, ent_mins); + VectorCopy(ai_hull_maxs, ent_maxs); + + // Get all waypoint indices sorted by distance to ent + argsort_entry_t waypoint_sort_values[MAX_WAYPOINTS]; + for(int i = 0; i < n_waypoints; i++) { + waypoint_sort_values[i].index = i; + waypoint_sort_values[i].value = VectorDistanceSquared(waypoints[i].origin, ent->v.origin); + } + qsort(waypoint_sort_values, n_waypoints, sizeof(argsort_entry_t), argsort_comparator); + + + + int best_waypoint_idx = -1; + // Sweep through waypoints from closest to farthest, stop when we can tracebox to one + for(int i = 0; i < n_waypoints; i++) { + int waypoint_idx = waypoint_sort_values[i].index; + + if(ofs_tracebox(ent->v.origin, ent_mins, ent_maxs, waypoints[waypoint_idx].origin, MOVE_NOMONSTERS, ent)) { + best_waypoint_idx = waypoint_idx; + break; } } - Con_DPrintf("'%5.1f %5.1f %5.1f', %f is %f, (%i, %i)\n", waypoints[best].origin[0],waypoints[best].origin[1], waypoints[best].origin[2], best_dist, dist, i, best); - VectorCopy (waypoints[best].origin, G_VECTOR(OFS_RETURN)); + + return best_waypoint_idx; } + +void Do_Pathfind (void) { + #ifdef MEASURE_PF_PERF + u64 t1, t2; + sceRtcGetCurrentTick(&t1); + #endif + + int i, s; + trace_t trace; + + Con_DPrintf("====================\n"); + Con_DPrintf("Starting Do_Pathfind\n"); + Con_DPrintf("====================\n"); + + 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); + + if(developer.value == 3) { + Con_Printf("Finding start waypoint\n"); + } + int start_waypoint = get_closest_waypoint(zombie_entnum); + if(developer.value == 3) { + Con_Printf("Finding goal waypoint\n"); + } + int goal_waypoint = get_closest_waypoint(target_entnum); + + if(start_waypoint == -1 || goal_waypoint == -1) { + Con_DPrintf("Pathfind failure. Invalid start or goal waypoint. (Start: %d, Goal: %d)\n", start_waypoint, goal_waypoint); + G_FLOAT(OFS_RETURN) = 0; + return; + } + + Con_DPrintf("\tStarting waypoint: %i, Ending waypoint: %i\n", start_waypoint, goal_waypoint); + if (sv_way_pathfind(start_waypoint, goal_waypoint)) { + + // -------------------------------------------------------------------- + // Debug print zombie path + // -------------------------------------------------------------------- + if(developer.value == 3) { + Con_Printf("\tPrinting zombie (%d) (%d --> %d) path: [", zombie_entnum, start_waypoint, goal_waypoint); + for(i = process_list_length - 1; i >= 0; i--) { + Con_Printf("%d, ", process_list[i]); + } + Con_Printf("]\n"); + + Con_Printf("\tWaypoint path distances: ["); + for(i = process_list_length - 1; i >= 0; i--) { + float waypoint_dist = VectorDistanceSquared(zombie->v.origin, waypoints[process_list[i]].origin); + Con_Printf("%.2f, ", waypoint_dist); + } + Con_Printf("]\n"); + + Con_Printf("\tWaypoint path traceboxes: ["); + for(i = process_list_length - 1; i >= 0; i--) { + int waypoint_tracebox_result = ofs_tracebox(zombie->v.origin, ai_hull_mins, ai_hull_maxs, waypoints[process_list[i]].origin, MOVE_NOMONSTERS, ent); + Con_Printf("%d, ", waypoint_tracebox_result); + } + Con_Printf("]\n"); + } + + // -------------------------------------------------------------------- + + int zombie_slot = -1; + int free_slot = -1; + + for(i = 0; i < MaxZombies; i++) { + // If we see any free slots, keep track of it, we might need it + if(free_slot == -1 && !zombie_list[i].zombienum) { + free_slot = i; + } + else if(zombie_entnum == zombie_list[i].zombienum) { + zombie_slot = i; + break; + } + } + + // If this zombie ent doesn't have a slot, take the free slot we saw + if(zombie_slot == -1 && free_slot != -1) { + zombie_slot = free_slot; + } + if(zombie_slot != -1) { + // Claim the slot + zombie_list[zombie_slot].zombienum = zombie_entnum; + for (s = 0; s < process_list_length; s++) { + zombie_list[zombie_slot].pathlist[s] = process_list[s]; + } + zombie_list[zombie_slot].pathlist_length = process_list_length; + +#ifdef MEASURE_PF_PERF + sceRtcGetCurrentTick(&t2); + double elapsed = (t2 - t1) * 0.000001; + Con_Printf("PF time: %f\n", elapsed); +#endif + + // If there is only one waypoint on the path, we are already at the player's waypoint + if(zombie_list[zombie_slot].pathlist_length == 1) { + Con_DPrintf("\tWe are at player's waypoint already!\n"); + G_FLOAT(OFS_RETURN) = -1; + } + else { + Con_DPrintf("\tPath found!\n"); + G_FLOAT(OFS_RETURN) = 1; + } + return; + } + } + +#ifdef MEASURE_PF_PERF + sceRtcGetCurrentTick(&t2); + double elapsed = (t2 - t1) * 0.000001; + Con_Printf("PF time: %f\n", elapsed); +#endif + + Con_DPrintf("Pathfind failure. Goal waypoint not reachable.\n"); + G_FLOAT(OFS_RETURN) = 0; +} + +// +// Returns distance (squared) between point q and the line segment (a,b) +// +// https://www.desmos.com/calculator/pwabcrtil0 +// +float dist_to_line_segment(vec3_t a, vec3_t b, vec3_t q) { + + vec3_t ab; + VectorSubtract(b,a,ab); // ab = b - a + vec3_t aq; + VectorSubtract(q,a,aq); // aq = q - a + + float aq_dot_ab = DotProduct(aq,ab); + float ab_dot_ab = DotProduct(ab,ab); + + // Compute fraction along line segment (a,b) closest to point q + float t = aq_dot_ab / ab_dot_ab; + + // If t < 0, return distance to point a + if(t < 0) { + return VectorDistanceSquared(q,a); + } + // If t > 1, return distance to point b + if(t > 1) { + return VectorDistanceSquared(q,b); + } + + // Otherwise, return distance to point on a,b at fraction t + vec3_t point_on_ab; + VectorLerp(a, t, b, point_on_ab); + return VectorDistanceSquared(q, point_on_ab); +} + + + + /* ================= Get_Next_Waypoint This function will return the next waypoint in zombies path and then remove it from the list @@ -2872,136 +2884,345 @@ Get_Next_Waypoint This function will return the next waypoint in zombies path an vector Get_Next_Waypoint (entity) ================= */ -void Get_Next_Waypoint (void) -{ - int i, s; - s = 0;//useless initialize, because compiler likes to yell at me - int entnum; - edict_t *ent;//blubs added - vec3_t move; - float *start,*mins, *maxs; - int currentWay = 0; - //int zomb = 0; - int skippedWays = 0; +void Get_Next_Waypoint (void) { + int entnum; + edict_t *ent; + // vec3_t move; + vec3_t start; + // vec3_t mins; + // vec3_t maxs; - move [0] = 0; - move [1] = 0; - move [2] = 0; + // Initialize to world origin + // VectorCopy(vec3_origin, move); entnum = G_EDICTNUM(OFS_PARM0); - ent = G_EDICT(OFS_PARM0);//blubsadded - start = G_VECTOR(OFS_PARM1); - mins = G_VECTOR(OFS_PARM2); - maxs = G_VECTOR(OFS_PARM3); - - mins[0] -= 2; - mins[1] -= 2; - - maxs[0] += 2; - maxs[1] += 2; + ent = G_EDICT(OFS_PARM0); + VectorCopy(G_VECTOR(OFS_PARM1), start); + // VectorCopy(G_VECTOR(OFS_PARM2), mins); + // VectorCopy(G_VECTOR(OFS_PARM3), maxs); - for (i = 0; i < MaxZombies; i++) - { - if (entnum == zombie_list[i].zombienum) - { - for (s = MAX_WAYPOINTS - 1; s > -1; s--) - { - if (zombie_list[i].pathlist[s]) - { - zombie_list[i].pathlist[s] = 0;//This is get_next, so remove our current waypoint from the list. + edict_t *goal_ent = PROG_TO_EDICT(ent->v.enemy); + vec3_t goal; + VectorCopy(goal_ent->v.origin, goal); - if(s == 1) - { - VectorCopy (move, G_VECTOR(OFS_RETURN));//we are at our last waypoint, so just return 0,0,0, this should never happen anyways, because we'll make pathfind return something else - Con_Printf("Warning, only one waypoint in path!\n"); - return; - } - s-= 1; - currentWay = s;//We want the next waypoint - break; - } - } + if(developer.value == 3){ + Con_Printf("Get_Next_Waypoint for ent %d\n", entnum); + Con_Printf("\tEnt origin: (%f, %f, %f)\n", ent->v.origin[0], ent->v.origin[1], ent->v.origin[2]); + Con_Printf("\tSearch start origin: (%f, %f, %f)\n", start[0], start[1], start[2]); + } + + int zombie_idx = -1; + for (int i = 0; i < MaxZombies; i++) { + if(entnum == zombie_list[i].zombienum) { + zombie_idx = i; break; } } - //s is the index in our path, so if s == 1 - if(s == -1 || s == 0) - { - //-1? - //then that means only player was in path, this is just in case... - //we are at our last waypoint, so just return 0,0,0, this should never happen anyways, because we'll make pathfind return something else - //0? - //only 1 waypoint left in path, we can't possibly smooth the path in this scenario. - //next waypoint in any case is going to be player, so.... - VectorCopy (move, G_VECTOR(OFS_RETURN)); + // If we didn't find the ent in our list of data, stop. Return the enemy ent's origin + if(zombie_idx == -1) { + if(developer.value == 3){ + Con_Printf("Warning: no pathing data found for ent %d.\n", entnum); + } + VectorCopy(goal, G_VECTOR(OFS_RETURN)); return; } - int iterations = 5;//that's how many segments per waypoint, pretty important number - float Scale = 0.5; - float curScale = 1; - float Scalar = Scale; - float TraceResult; - vec3_t toAdd; - vec3_t curStart; - vec3_t temp; - int q; - VectorCopy(waypoints[zombie_list[i].pathlist[currentWay]].origin,temp); - VectorCopy(temp,move); - while(1) - { - //Con_Printf("Main Vector Start: %f, %f, %f Vector End: %f, %f, %f\n",start[0],start[1],start[2],waypoints[zombie_list[i].pathlist[currentWay]].origin[0],waypoints[zombie_list[i].pathlist[currentWay]].origin[1],waypoints[zombie_list[i].pathlist[currentWay]].origin[2]); - TraceResult = TraceMove(start,mins,maxs,waypoints[zombie_list[i].pathlist[currentWay]].origin,MOVE_NOMONSTERS,ent); - if(TraceResult == 1) - { - VectorCopy(waypoints[zombie_list[i].pathlist[currentWay]].origin,move); - if(currentWay == 1)//we're at the end of the list, we better not go out of bounds, was 0, now 1, since 0 is for player index - { - break; - } - currentWay -= 1; - skippedWays += 1; + if(developer.value == 3){ + // Print path (stored in reverse order from zombie to target ent) + Con_Printf("\tpath before: ["); + for(int i = zombie_list[zombie_idx].pathlist_length - 1; i >= 0; i--) { + Con_Printf(" %d,", zombie_list[zombie_idx].pathlist[i]); } - else - { - if(skippedWays > 0) - { - VectorCopy(waypoints[zombie_list[i].pathlist[currentWay + 1]].origin,temp); - VectorCopy(temp,curStart); - VectorSubtract(waypoints[zombie_list[i].pathlist[currentWay]].origin,curStart,toAdd); - for(q = 0;q < iterations; q++) - { - curScale *= Scalar; - VectorScale(toAdd,curScale,temp); - VectorAdd(temp,curStart,temp); - TraceResult = TraceMove(start,mins,maxs,temp,MOVE_NOMONSTERS,ent); - if(TraceResult ==1) - { - Scalar = Scale + 1; - VectorCopy(temp,move); - } - else - { - Scalar = Scale; - } - } - } - break; + Con_Printf("]\n"); + } + + + // if(developer.value == 3){ + // float dist; + // if(zombie_list[zombie_idx].pathlist_length > 0) { + // int first_waypoint_idx = zombie_list[zombie_idx].pathlist[zombie_list[zombie_idx].pathlist_length - 1]; + // dist = VectorDistanceSquared(ent->v.origin, waypoints[first_waypoint_idx].origin); + // Con_Printf("\tDist squared to first waypoint (%d): %.2f\n", first_waypoint_idx, dist); + // Con_Printf("\t\tEnt pos: (%.2f, %.2f, %.2f)\n", ent->v.origin[0], ent->v.origin[1], ent->v.origin[2]); + // Con_Printf("\t\tFirst waypoint pos: (%.2f, %.2f, %.2f)\n", waypoints[first_waypoint_idx].origin[0], waypoints[first_waypoint_idx].origin[1], waypoints[first_waypoint_idx].origin[2]); + // } + // dist = VectorDistanceSquared(ent->v.origin, goal_ent->v.origin); + // Con_Printf("\tDist squared to goal ent: %.2f\n", dist); + // } + + + // Check if our path is now empty. + // If it's empty, we have no more waypoints to chase, follow the enemy entity. + if(zombie_list[zombie_idx].pathlist_length < 1) { + if(developer.value == 3){ + Con_Printf("\tZombie path length: %d, returning enemy origin.\n", zombie_list[zombie_idx].pathlist_length); + } + // The zombie's path is empty, return the enemy origin + VectorCopy(goal, G_VECTOR(OFS_RETURN)); + return; + } + + + // ---------------–---------------–---------------–---------------– + // + // There is an unfortunate edge case in the following situation: + // + // On uneven terrain, tracebox may fail for the true closest waypoint, + // yielding a nonoptimal path we instead go for a waypoint farther than + // the one we should've gone for. + // + // In some instances, this causes us to run away from the optimal path + // to some start waypoint, only to run through back through the point + // we were originally standing on. + // + // To attempt to catch this edge case, check the distance from where we are + // standing to the closest point on each edge along the waypoint path, + // to see if we are already somewhere along the path. + // if so, skip waypoints up to the point we are standing. + // + // ---------------–---------------–---------------–---------------– + float dist_threshold = 400; // Max distance squared to path + // -- + float best_edge_idx = -2; // -2 = None, -1 = Closest to edge connecting final waypoint and goal + float best_edge_dist = INFINITY; + + + for(int i = zombie_list[zombie_idx].pathlist_length - 1; i >= 0; i--) { + float dist; + if(i > 0) { + dist = dist_to_line_segment(waypoints[zombie_list[zombie_idx].pathlist[i]].origin, waypoints[zombie_list[zombie_idx].pathlist[i-1]].origin, start); + } + // If on i == 0, endpoint of edge is the goal position + else { + dist = dist_to_line_segment(waypoints[zombie_list[zombie_idx].pathlist[i]].origin, goal, start); + } + if(dist < best_edge_dist) { + best_edge_idx = i; + best_edge_dist = dist; } } - Con_DPrintf("Get Next Way returns: list[%i], waypoint:%i\n",s,waypoints[zombie_list[i].pathlist[s]]); + // If we are within the threshold of a waypoint edge, drop all waypoints up to and including the start waypoint for that edge + if(best_edge_dist <= dist_threshold) { + zombie_list[zombie_idx].pathlist_length = best_edge_idx; + } - //VectorCopy(waypoints[zombie_list[i].pathlist[s]].origin,move); //for old get_next_way - zombie_list[i].pathlist[s] = 0; + if(developer.value == 3){ + // Print path (stored in reverse order from zombie to target ent) + Con_Printf("\tpath after pruning: ["); + for(int i = zombie_list[zombie_idx].pathlist_length - 1; i >= 0; i--) { + Con_Printf(" %d,", zombie_list[zombie_idx].pathlist[i]); + } + Con_Printf("]\n"); + } - //Con_Printf("Skipped %i waypoints, we're moving to the %f percentage in between 2 waypoints\n",skippedWays,curScale); - //Con_DPrintf("'%5.1f %5.1f %5.1f'\n", move[0], move[1], move[2]); - VectorCopy (move, G_VECTOR(OFS_RETURN)); + // ---------------–---------------–---------------–---------------– + + + // ---------------–---------------–---------------–---------------– + // FIXME - Check if we are already somewhere along the path + // Check distance to each line segment + // If distance < 40qu, we're going to consider ourselves already on that edge, and skip the initial waypoints + + + + // ---------------–---------------–---------------–---------------– + // Check to see if we can walk directly to any waypoints farther + // along the path. + // ---------------–---------------–---------------–---------------– + vec3_t ent_mins; + vec3_t ent_maxs; + VectorCopy(ai_hull_mins, ent_mins); + VectorCopy(ai_hull_maxs, ent_maxs); + + + // Get the index of the farthest waypoint we can walk to in the path: + int farthest_walkable_path_node_idx = -2; // -2 means no waypoints were walkable, -1 means we can walk to goal ent position + for(int i = zombie_list[zombie_idx].pathlist_length - 1; i >= 0; i--) { + if(ofs_tracebox(start, ent_mins, ent_maxs, waypoints[zombie_list[zombie_idx].pathlist[i]].origin, MOVE_NOMONSTERS, ent)) { + farthest_walkable_path_node_idx = i; + continue; + } + break; + } + + // If we were able to walk all the way to the final waypoint, check if we can walk to the goal entity position + if(farthest_walkable_path_node_idx == 0) { + if(ofs_tracebox(start, ent_mins, ent_maxs, goal, MOVE_NOMONSTERS, ent)) { + farthest_walkable_path_node_idx = -1; + } + } + + + // If weren't able to walk to any waypoints, return first waypoint in path + if(farthest_walkable_path_node_idx == -2) { + int waypoint_idx = zombie_list[zombie_idx].pathlist[zombie_list[zombie_idx].pathlist_length - 1]; + + // Remove first waypoint from path + zombie_list[zombie_idx].pathlist_length -= 1; + + + if(developer.value == 3){ + Con_Printf("\tReturning walk to first path node. (path node: %d, waypoint: %d)\n", (zombie_list[zombie_idx].pathlist_length - 1) + 1, waypoint_idx); + Con_Printf("\tpath after: ["); + for(int i = zombie_list[zombie_idx].pathlist_length - 1; i >= 0; i--) { + Con_Printf(" %d,", zombie_list[zombie_idx].pathlist[i]); + } + Con_Printf("]\n"); + } + + + VectorCopy(waypoints[waypoint_idx].origin, G_VECTOR(OFS_RETURN)); + return; + } + + // If we were able to walk all the way to goal entity, return that point, clear the path + if(farthest_walkable_path_node_idx == -1) { + if(developer.value == 3){ + Con_Printf("\tReturning can walk to goal. (path node: %d)\n", farthest_walkable_path_node_idx); + } + VectorCopy(goal, G_VECTOR(OFS_RETURN)); + // Remove all nodes from the path + zombie_list[zombie_idx].pathlist_length = 0; + return; + } + + + if(developer.value == 3){ + Con_Printf("Farthest walkable path node: %d (waypoint: %d)\n", + (zombie_list[zombie_idx].pathlist_length - 1) - farthest_walkable_path_node_idx, + zombie_list[zombie_idx].pathlist[farthest_walkable_path_node_idx] + ); + } + + // Otherwise, we were able to walk to at least one node. + // Binary search + + // Perform a binary search along the edge from cur to next + int edge_start_waypoint_idx; + int edge_end_waypoint_idx; + vec3_t edge_start; + vec3_t edge_end; + + if(farthest_walkable_path_node_idx > 0) { + edge_start_waypoint_idx = zombie_list[zombie_idx].pathlist[farthest_walkable_path_node_idx]; + edge_end_waypoint_idx = zombie_list[zombie_idx].pathlist[farthest_walkable_path_node_idx - 1]; + if(developer.value == 3){ + Con_Printf("\tPerforming binary search between waypoint %d (%d in path, can walk: 1) and %d (%d in path, can walk: 0)\n", + edge_start_waypoint_idx, (zombie_list[zombie_idx].pathlist_length - 1) - farthest_walkable_path_node_idx, + edge_end_waypoint_idx, ((zombie_list[zombie_idx].pathlist_length - 1) - farthest_walkable_path_node_idx) + 1 + ); + } + VectorCopy(waypoints[edge_start_waypoint_idx].origin, edge_start); + VectorCopy(waypoints[edge_end_waypoint_idx].origin, edge_end); + } + else { + edge_start_waypoint_idx = zombie_list[zombie_idx].pathlist[farthest_walkable_path_node_idx]; + edge_end_waypoint_idx = -1; + + if(developer.value == 3){ + Con_Printf("\tPerforming binary search between waypoint %d (%d in path, can walk: 1) and goal ent pos\n", + edge_start_waypoint_idx, (zombie_list[zombie_idx].pathlist_length - 1) - farthest_walkable_path_node_idx + ); + } + VectorCopy(waypoints[edge_start_waypoint_idx].origin, edge_start); + VectorCopy(goal, edge_end); + } + + + int n_iters = 3; + int cur_frac_numerator = 1; + float cur_frac; + + vec3_t cur_point; + vec3_t best_point; + VectorCopy(edge_start, best_point); + float best_point_frac = 0; + + for(int i = 0; i < n_iters; i++) { + // Calculate the number in [0,1] corresponding to how far along the edge we are checking + cur_frac = ((float) cur_frac_numerator) / (2 << i); + if(developer.value == 3){ + Con_Printf("\tBinary search iter: %d/%d, frac: %f\n", i, n_iters, cur_frac); + } + VectorLerp(edge_start, cur_frac, edge_end, cur_point); + + // Check if we can walk from the ent's current location directly to `cur_point` + if(ofs_tracebox(start, ent_mins, ent_maxs, cur_point, MOVE_NOMONSTERS, ent)) { + cur_frac_numerator = (cur_frac_numerator * 2) + 1; + best_point_frac = cur_frac; + VectorCopy(cur_point, best_point); + } + else { + cur_frac_numerator = (cur_frac_numerator * 2) - 1; + } + } + + if(developer.value == 3){ + Con_Printf("\tpath after binary search: (%f x between waypoints (%d,%d), then [", + best_point_frac, + edge_start_waypoint_idx, + edge_end_waypoint_idx + ); + for(int i = farthest_walkable_path_node_idx - 1; i >= 0; i--) { + Con_Printf(" %d,", zombie_list[zombie_idx].pathlist[i]); + } + Con_Printf("]\n"); + } + // Remove all points up to and including `farthest_walkable_path_node_idx` from the path + zombie_list[zombie_idx].pathlist_length = farthest_walkable_path_node_idx; + + + + // ------------------------------------------------------------------------ + // If we're already incredibly close to the goal point along the path + // + // Get_Next_Waypoint should've returned somewhere farther along the path, + // but is running into tricky edge cases regarding tracebox. + // For this case, force-advance to the next waypoint / goal along the path + // ------------------------------------------------------------------------ + if(VectorDistanceSquared(start,best_point) < 64) { + // If trying to walk to the next waypoint already, skip a waypoint on the path + if(best_point_frac >= 1.0) { + zombie_list[zombie_idx].pathlist_length -= 1; + } + + // If we have at least one waypoint, walk directly to it, pop from path + if(zombie_list[zombie_idx].pathlist_length > 0) { + int waypoint_idx = zombie_list[zombie_idx].pathlist[zombie_list[zombie_idx].pathlist_length - 1]; + VectorCopy(waypoints[waypoint_idx].origin, best_point); + zombie_list[zombie_idx].pathlist_length -= 1; + } + // If we have no waypoints on the path, walk to goal, clear the path + else { + zombie_list[zombie_idx].pathlist_length = 0; + VectorCopy(goal, best_point); + } + + if(developer.value == 3) { + Con_Printf("\tForce-truncated path to %d waypoints.\n", zombie_list[zombie_idx].pathlist_length); + } + } + // ------------------------------------------------------------------------ + + + if(developer.value == 3){ + Con_Printf("\tfinal path ["); + for(int i = zombie_list[zombie_idx].pathlist_length - 1; i >= 0; i--) { + Con_Printf(" %d,", zombie_list[zombie_idx].pathlist[i]); + } + Con_Printf("]\n"); + + Con_Printf("\tFinal best point: (%f, %f, %f)\n", best_point[0], best_point[1], best_point[2]); + } + + VectorCopy(best_point, G_VECTOR(OFS_RETURN)); + return; } + /* ================= Get_First_Waypoint This function will return the waypoint waypoint in zombies path and then remove it from the list @@ -3009,124 +3230,11 @@ Get_First_Waypoint This function will return the waypoint waypoint in zombies pa vector Get_First_Waypoint (entity) ================= */ -void Get_First_Waypoint (void) -{ - int i, s; - s = 0;//useless initialize, because compiler likes to yell at me - int entnum; - edict_t *ent;//blubs added - vec3_t move; - float *start,*mins, *maxs; - int currentWay = 0; - //int zomb = 0; - int skippedWays = 0; - - move [0] = 0; - move [1] = 0; - move [2] = 0; - - entnum = G_EDICTNUM(OFS_PARM0); - ent = G_EDICT(OFS_PARM0);//blubsadded - start = G_VECTOR(OFS_PARM1); - mins = G_VECTOR(OFS_PARM2); - maxs = G_VECTOR(OFS_PARM3); - - mins[0] -= 2; - mins[1] -= 2; - - maxs[0] += 2; - maxs[1] += 2; - - - for (i = 0; i < MaxZombies; i++) - { - if (entnum == zombie_list[i].zombienum) - { - for (s = MAX_WAYPOINTS - 1; s > -1; s--) - { - if (zombie_list[i].pathlist[s]) - { - currentWay = s; - break; - } - } - break; - } - } - - if(s == 0) - { - //0? - //currentway is player, just return world - VectorCopy (move, G_VECTOR(OFS_RETURN)); - return; - } - //1? only one way in list, we can't possibly smooth list when we only have one... - - int iterations = 5;//that's how many segments per waypoint, pretty important number - float Scale = 0.5; - float curScale = 1; - float Scalar = Scale; - float TraceResult; - vec3_t toAdd; - vec3_t curStart; - vec3_t temp; - int q; - VectorCopy(waypoints[zombie_list[i].pathlist[currentWay]].origin,temp); - VectorCopy(temp,move); - - while(1) - { - //Con_Printf("Main Vector Start: %f, %f, %f Vector End: %f, %f, %f\n",start[0],start[1],start[2],waypoints[zombie_list[i].pathlist[currentWay]].origin[0],waypoints[zombie_list[i].pathlist[currentWay]].origin[1],waypoints[zombie_list[i].pathlist[currentWay]].origin[2]); - TraceResult = TraceMove(start,mins,maxs,waypoints[zombie_list[i].pathlist[currentWay]].origin,MOVE_NOMONSTERS,ent); - if(TraceResult == 1) - { - VectorCopy(waypoints[zombie_list[i].pathlist[currentWay]].origin,move); - if(currentWay == 1)//we're at the end of the list, we better not go out of bounds//was 0, now 1 since 0 is for enemy - { - break; - } - currentWay -= 1; - skippedWays += 1; - } - else - { - if(skippedWays > 0) - { - VectorCopy(waypoints[zombie_list[i].pathlist[currentWay + 1]].origin,temp); - VectorCopy(temp,curStart); - VectorSubtract(waypoints[zombie_list[i].pathlist[currentWay]].origin,curStart,toAdd); - for(q = 0;q < iterations; q++) - { - curScale *= Scalar; - VectorScale(toAdd,curScale,temp); - VectorAdd(temp,curStart,temp); - //Con_Printf("subVector Start: %f, %f, %f Vector End: %f, %f, %f\n",start[0],start[1],start[2],temp[0],temp[1],temp[2]); - TraceResult = TraceMove(start,mins,maxs,temp,MOVE_NOMONSTERS,ent); - if(TraceResult ==1) - { - Scalar = Scale + 1; - VectorCopy(temp,move); - } - else - { - Scalar = Scale; - }//we need a way to go back to the other value if it doesn't work!, so lets work with temp, but RETURN a different value other than temp! - } - } - break; - } - } - - Con_DPrintf("Get First Way returns: %i\n",s); - //VectorCopy(waypoints[zombie_list[i].pathlist[s]].origin,move);//for old get_first_way - zombie_list[i].pathlist[s] = 0; - //Con_Printf("Skipped %i waypoints, we're moving to the %f percentage in between 2 waypoints\n",skippedWays,curScale); - //Con_DPrintf("'%5.1f %5.1f %5.1f'\n", move[0], move[1], move[2]); - VectorCopy (move, G_VECTOR(OFS_RETURN)); +void Get_First_Waypoint (void) { + // TODO - Remove `Get_First_Waypoint`, replace references with `Get_Next_Waypoint` + Get_Next_Waypoint(); } - // 2001-09-20 QuakeC file access by FrikaC/Maddes start /* ================= diff --git a/source/quakedef.h b/source/quakedef.h index 2f599b9..2c8153d 100644 --- a/source/quakedef.h +++ b/source/quakedef.h @@ -419,25 +419,47 @@ extern int minimum_memory; typedef struct { int pathlist [MAX_WAYPOINTS]; + int pathlist_length; int zombienum; } zombie_ai; typedef struct { vec3_t origin; - int id; float g_score, f_score; - int open; // Determine if the waypoint is "open" a.k.a avaible - int target_id [8]; // Targets array number + int open; // Determine if the waypoint is "open" a.k.a active char special[64]; //special tag is required for the closed waypoints int target [8]; //Each waypoint can have up to 8 targets float dist [8]; // Distance to the next waypoints int came_from; // Used for pathfinding store where we got here to this - qboolean used; //if the waypoint is in use + qboolean used; // Set to `qtrue` if this waypoint contains valid data (not an empty slot in a list) } waypoint_ai; extern waypoint_ai waypoints[MAX_WAYPOINTS]; +extern int n_waypoints; extern short closest_waypoints[MAX_EDICTS]; +// ---------------------------------------------------------------------------- +// Utils for using cstdlib qsort (Quick sort) +// +// Usage: +// argsort_entry_t sort_values[10]; +// +// for(int i = 0; i < 10; i++) { +// sort_values[i].index = i; +// sort_values[i].value = something; +// } +// +// qsort(sort_values, 10, sizeof(argsort_entry_t), argsort_comparator); +// +// ---------------------------------------------------------------------------- +// Struct used for sorting a list of indices by some value +typedef struct argsort_entry_s { + int index; + float value; +} argsort_entry_t; +extern int argsort_comparator(const void *lhs, const void *rhs); +// ---------------------------------------------------------------------------- + #endif /* QUAKEDEFS_H */ diff --git a/source/sv_main.c b/source/sv_main.c index ce889bb..a00dea2 100644 --- a/source/sv_main.c +++ b/source/sv_main.c @@ -1682,17 +1682,21 @@ void W_stov (char *v, vec3_t out) } waypoint_ai waypoints[MAX_WAYPOINTS]; +int n_waypoints; // // Load_Waypoint_NZPBETA // Attempts to load an NZ:P Beta formatted // Waypoint file. // -void Load_Waypoint_NZPBETA() -{ +void Load_Waypoint_NZPBETA() { char temp[64]; int i, p, s; int h = 0; + + // Keep track of the waypoint with the highest index we've loaded + int max_waypoint_idx = -1; + int n_waypoints_parsed = 0; h = W_fopenbeta(); @@ -1700,18 +1704,16 @@ void Load_Waypoint_NZPBETA() return; // don't bother notifying.. } - for (i = 0; i < MAX_WAYPOINTS; i++) - { + for (i = 0; i < MAX_WAYPOINTS; i++) { waypoints[i].used = 0; - waypoints[i].id = -1; + // waypoints[i].id = -1; for (p = 0; p < 8; p++) { waypoints[i].target[p] = -1; - waypoints[i].target_id[p] = -1; + // waypoints[i].target_id[p] = -1; } } - for (i = 0; i < MAX_EDICTS; i++) - { + for (i = 0; i < MAX_EDICTS; i++) { closest_waypoints[i] = -1; } @@ -1721,99 +1723,174 @@ void Load_Waypoint_NZPBETA() vec3_t way_origin; int way_id; - while (1) - { + while (1) { // End of file. - if (!strcmp(W_fgets(h), "")) + if (!strcmp(W_fgets(h), "")) { break; + } W_stov(w_string_temp, way_origin); // - way_id = atoi(W_fgets(h)); // + int waypoint_idx = atoi(W_fgets(h)) - 1; // (1-based index, swap to 0-based) - if (way_id >= MAX_WAYPOINTS) - Sys_Error ("Waypoint with id %d past MAX_WAYPOINTS {%i)\n", way_id, MAX_WAYPOINTS); + n_waypoints_parsed += 1; + if(i > max_waypoint_idx) { + max_waypoint_idx = i; + } - waypoints[way_id].id = way_id; - VectorCopy(way_origin, waypoints[way_id].origin); + if(waypoint_idx >= MAX_WAYPOINTS) { + Sys_Error ("Waypoint with idx %d past MAX_WAYPOINTS {%i)\n", waypoint_idx, MAX_WAYPOINTS); + } - // - , - + // waypoints[waypoint_idx].id = way_id; + VectorCopy(way_origin, waypoints[waypoint_idx].origin); + + // [link1, link2, link3, link4, owner1, owner2, owner3, owner4] for(i = 0; i < 8; i++) { W_fgets(h); - - if (i < 4) { - int id = atoi(w_string_temp); - if (id > 0) { - waypoints[way_id].target[i] = id; - waypoints[way_id].target_id[i] = waypoints[way_id].target[i]; + // Skip "link1..link4" + // Parse "owner1..owner4" + if (i >= 4) { + int src_waypoint_idx = atoi(w_string_temp) - 1; // Fix 0-based index + if (src_waypoint_idx >= 0) { + // Search for an empty slot in waypoint `src_waypoint_idx` + for(int j = 0; j < 8; j++) { + if(waypoints[src_waypoint_idx].target[j] < 0) { + waypoints[src_waypoint_idx].target[j] = waypoint_idx; + } + } } } } - waypoints[way_id].used = 1; - waypoints[way_id].open = 1; + waypoints[waypoint_idx].used = 1; + waypoints[waypoint_idx].open = 1; } - Con_DPrintf("Total waypoints: %i\n", way_id); - 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++) - { - if (waypoints[i].target[p] < 0) break; - - for (s = 0; s < MAX_WAYPOINTS; s++) - { - if (waypoints[i].target[p] == s) - { - waypoints[i].dist[p] = VecLength2(waypoints[s].origin, waypoints[i].origin); - break; - } + Con_DPrintf("Total waypoints: %i, num parsed: %i\n", max_waypoint_idx, n_waypoints_parsed); + // Store in global `n_waypoints` + n_waypoints = max_waypoint_idx; + + // Cache distance between waypoints + for(i = 0; i < MAX_WAYPOINTS; i++) { + for(p = 0; p < 8; p++) { + if(waypoints[i].target[p] < 0) { + continue; } + float dist = VecLength2(waypoints[s].origin, waypoints[i].origin); + waypoints[i].dist[p] = dist; } + Con_DPrintf("Waypoint (%i)\n target1: (%i, %f),\n target2: (%i, %f),\n target3: (%i, %f),\n target4: (%i, %f),\n target5: (%i, %f),\n target6: (%i, %f),\n target7: (%i, %f),\n target8: (%i, %f)\n", + i, + waypoints[i].target[0], waypoints[i].dist[0], + waypoints[i].target[1], waypoints[i].dist[1], + waypoints[i].target[2], waypoints[i].dist[2], + waypoints[i].target[3], waypoints[i].dist[3], + waypoints[i].target[4], waypoints[i].dist[4], + waypoints[i].target[5], waypoints[i].dist[5], + waypoints[i].target[6], waypoints[i].dist[6], + waypoints[i].target[7], waypoints[i].dist[7] + ); } W_fclose(h); } +// +// Some waypoint slots in the global list may not have been loaded +// Look for these empty slots, and shift all waypoints down to fill them +// This also fixes waypoint reference links +// +void cleanup_waypoints() { + int new_n_waypoints = 0; + + for(int i = 0; i < MAX_WAYPOINTS; i++) { + // If waypoint slot is used, count it + if(waypoints[i].used) { + new_n_waypoints += 1; + } + // If waypoint slot is unused... + else { + // Update all waypoint link references greater than this waypoint slot index down one + for(int j = 0; j < MAX_WAYPOINTS; j++) { + if(waypoints[j].used) { + for(int k = 0; k < 8; k++) { + if(waypoints[j].target[k] > i) { + waypoints[j].target[k] -= 1; + } + } + } + } + + // Move all waypoints after this down one slot: + for(int j = i; j < MAX_WAYPOINTS - 1; j++) { + memcpy(&(waypoints[j]), &(waypoints[j+1]), sizeof(waypoint_ai)); + } + // Mark waypoint slot at the end of the list as unused + waypoints[MAX_WAYPOINTS-1].used = 0; + + // Count how many used waypoint slots are to the right of index `i` + int n_remaining_waypoints = 0; + for(int j = i; j < MAX_WAYPOINTS - 1; j++) { + if(waypoints[j].used) { + n_remaining_waypoints += 1; + } + } + // If no remaining used waypoint slots, stop + if(n_remaining_waypoints == 0) { + break; + } + + // Search this index again + i -= 1; + } + } + + n_waypoints = new_n_waypoints; +} + void Load_Waypoint () { char temp[64]; - int i, p, s; + int p, s; vec3_t d; int h = 0; + // --------------------------------------- + // Clear the structs + // --------------------------------------- + n_waypoints = 0; + for (int 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; + } + } + + for (int i = 0; i < MAX_EDICTS; i++) { + closest_waypoints[i] = -1; + } + // --------------------------------------- + h = W_fopen(); w_string_temp = Z_Malloc(128); - if (h == -1) - { + if (h == -1) { Con_DPrintf("No waypoint file (%s/maps/%s.way) found, trying beta format..\n", com_gamedir, sv.name); Load_Waypoint_NZPBETA(); + cleanup_waypoints(); return; } - 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; - } - } - for (i = 0; i < MAX_EDICTS; i++) - { - closest_waypoints[i] = -1; - } - - i = 0; + int i; + // Keep track of the waypoint with the highest index we've loaded + int max_waypoint_idx = -1; + int n_waypoints_parsed = 0; Con_DPrintf("Loading waypoints\n"); - while (1) - { - if (strncmp(W_fgets (h), "Waypoint", 8)) - { + while (1) { + if (strncmp(W_fgets (h), "Waypoint", 8)) { Con_DPrintf("Last waypoint\n"); break; - } - else - { + } else { W_fgets (h); W_stov (W_substring (W_fgets (h), 9, 20), d); @@ -1822,19 +1899,23 @@ void Load_Waypoint () i = atoi (temp); - if (i >= MAX_WAYPOINTS) + 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; + n_waypoints_parsed += 1; + if(i > max_waypoint_idx) { + max_waypoint_idx = i; + } VectorCopy (d, waypoints[i].origin); strcpy(waypoints[i].special, W_substring (W_fgets (h), 10, 20)); - if (waypoints[i].special[0]) + if (waypoints[i].special[0]) { waypoints[i].open = 0; - else + } else { waypoints[i].open = 1; + } // 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. @@ -1844,7 +1925,6 @@ void Load_Waypoint () 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++; } } @@ -1853,65 +1933,52 @@ void Load_Waypoint () W_fgets (h); waypoints[i].used = 1; - - Con_DPrintf("Waypoint (%i) id: %i, tag: %s, open: %i, target: %i, target2: %i, target3: %i, target4: %i, target5: %i, target6: %i, target7: %i, target8: %i\n", - i, - waypoints[i].id, - waypoints[i].special, - waypoints[i].open, - waypoints[i].target[0], - waypoints[i].target[1], - waypoints[i].target[2], - waypoints[i].target[3], - waypoints[i].target[4], - waypoints[i].target[5], - waypoints[i].target[6], - waypoints[i].target[7]); + Con_DPrintf("Waypoint (%i), tag: %s, open: %i, target1: %i, target2: %i, target3: %i, target4: %i, target5: %i, target6: %i, target7: %i, target8: %i\n", + i, + waypoints[i].special, + waypoints[i].open, + waypoints[i].target[0], + waypoints[i].target[1], + waypoints[i].target[2], + waypoints[i].target[3], + waypoints[i].target[4], + waypoints[i].target[5], + waypoints[i].target[6], + waypoints[i].target[7] + ); } } - Con_DPrintf("Total waypoints: %i\n", i); - 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++) - { - if (waypoints[i].target[p] < 0) break; - - for (s = 0; s < MAX_WAYPOINTS; s++) - { - if (waypoints[i].target[p] == s) - { - waypoints[i].dist[p] = VecLength2(waypoints[s].origin, waypoints[i].origin); - break; - } + Con_DPrintf("Total waypoints: %i, num parsed: %i\n", max_waypoint_idx, n_waypoints_parsed); + // Store in global `n_waypoints` + n_waypoints = max_waypoint_idx; + // Cache distance between waypoints + for(i = 0; i < MAX_WAYPOINTS; i++) { + for(p = 0; p < 8; p++) { + if(waypoints[i].target[p] < 0) { + continue; } + float dist = VecLength2(waypoints[s].origin, waypoints[i].origin); + waypoints[i].dist[p] = dist; } - Con_DPrintf("Waypoint (%i)\n target: %i (%i, %f),\n target2: %i (%i, %f),\n target3: %i (%i, %f),\n target4: %i (%i, %f),\n target5: %i (%i, %f),\n target6: %i (%i, %f),\n target7: %i (%i, %f),\n 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)\n target1: (%i, %f),\n target2: (%i, %f),\n target3: (%i, %f),\n target4: (%i, %f),\n target5: (%i, %f),\n target6: (%i, %f),\n target7: (%i, %f),\n target8: (%i, %f)\n", + i, + waypoints[i].target[0], waypoints[i].dist[0], + waypoints[i].target[1], waypoints[i].dist[1], + waypoints[i].target[2], waypoints[i].dist[2], + waypoints[i].target[3], waypoints[i].dist[3], + waypoints[i].target[4], waypoints[i].dist[4], + waypoints[i].target[5], waypoints[i].dist[5], + waypoints[i].target[6], waypoints[i].dist[6], + waypoints[i].target[7], waypoints[i].dist[7] + ); } W_fclose(h); //Z_Free (w_string_temp); + cleanup_waypoints(); } + +// Util for qsort +int argsort_comparator(const void *lhs, const void *rhs) { + return ((argsort_entry_t*)lhs)->value - ((argsort_entry_t*)rhs)->value; +} +