Merge pull request #24 from shpuld/feat/faster-pathfinding

source/mathlib.h

@@ -89,6 +89,8 @@ extern	int nanmask;
 
 #define VectorNormalizeFast( v ){float	ilength = (float)rsqrt(DotProduct(v,v));v[0] *= ilength;v[1] *= ilength;v[2] *= ilength; }
 
+#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]))
+
 typedef float matrix3x4[3][4];
 typedef float matrix3x3[3][3];
 

source/pr_cmds.c

@@ -1602,7 +1602,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);
 }
 
 int proces_list[MAX_WAYPOINTS];
@@ -1641,6 +1641,8 @@ 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)
 				{
@@ -1658,11 +1660,13 @@ 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--)
 	{
 		//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.
@@ -1704,8 +1708,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)
 			{
@@ -1755,6 +1759,7 @@ Get_Waypoint_Near
 vector Get_Waypoint_Near (entity)
 =================
 */
+
 void Get_Waypoint_Near (void)
 {
 	float best_dist;
@@ -1773,13 +1778,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);
+			if(dist < best_dist)
+			{
+				trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, ent);
 
 				//Con_DPrintf("Waypoint: %i, distance: %f, fraction: %f\n", i, dist, trace.fraction);
-			if (trace.fraction >= 1)
+				if (trace.fraction >= 1)
-			{
-				if(dist < best_dist)
 				{
 					best_dist = dist;
 					best = i;
@@ -1861,66 +1866,116 @@ float Do_Pathfind (entity zombie, entity target)
 =================
 */
 // #define MEASURE_PF_PERF
+float max_waypoint_distance = 750;
+short closest_waypoints[MAX_EDICTS]; 
+
 void Do_Pathfind (void)
 {
-	int i, s;
-	trace_t   trace;
-	edict_t   *ent;
-	edict_t   *zombie;
-	int			entnum;
-
 	#ifdef MEASURE_PF_PERF
 	u64 t1, t2;
 	sceRtcGetCurrentTick(&t1);
 	#endif
 
-	entnum = G_EDICTNUM(OFS_PARM0);
+	int i, s;
+	trace_t   trace;
 
 	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);
 
-	float best_dist_z = 1000000000;
+	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 = 0;
+	int best_z = -1;
-	float best_dist_e = 1000000000;
+	float best_dist_e = max_waypoint_distance * max_waypoint_distance;
 	float dist_e = 0;
-	int best_e = 0;
+	int best_e = -1;
 
-	for (i = 0; i < MAX_WAYPOINTS; i++)
+	int prevclosest = closest_waypoints[zombie_entnum];
-	{
+	if (prevclosest >= 0) {
-		if (waypoints[i].used && waypoints[i].open)
+		trace = SV_Move (zombie->v.origin, vec3_origin, vec3_origin, waypoints[prevclosest].origin, 1, zombie);
-		{
+		if (trace.fraction >= 1) {
-			dist_z = VecLength2(waypoints[i].origin, zombie->v.origin);
+			dist_z = VectorDistanceSquared(waypoints[prevclosest].origin, zombie->v.origin);
-			if (dist_z < best_dist_z)
+			best_dist_z = dist_z;
-			{
+			best_z = prevclosest;
-				trace = SV_Move (zombie->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, zombie);
+		} else {
-				if (trace.fraction >= 1)
+			for (s = 0; s < 8; s++) {
-				{
+				int neighbor = waypoints[prevclosest].target_id[s];
-					best_dist_z = dist_z;
+				if (neighbor < 0) break;
-					best_z = i;
-				}
-			}
 
-			dist_e = VecLength2(waypoints[i].origin, ent->v.origin);
+				dist_z = VectorDistanceSquared(waypoints[neighbor].origin, zombie->v.origin);
-			if (dist_e < best_dist_e)
+				if (dist_z < best_dist_z) {
-			{
+					trace = SV_Move (zombie->v.origin, vec3_origin, vec3_origin, waypoints[neighbor].origin, 1, zombie);
-				trace = SV_Move (ent->v.origin, vec3_origin, vec3_origin, waypoints[i].origin, 1, ent);
+					if (trace.fraction >= 1) {
-				if (trace.fraction >= 1)
+						best_dist_z = dist_z;
-				{
+						best_z = neighbor;
-					best_dist_e = dist_e;
+						break;
-					best_e = i;
+					}
 				}
 			}
 		}
 	}
 
+	// 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) {
+				best_dist_z = dist_z;
+				best_z = i;
+			}
+		}
+
+		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 (entnum == zombie_list[i].zombienum)
+			if (zombie_entnum == zombie_list[i].zombienum)
 			{
 				for (s = 0; s < MAX_WAYPOINTS; s++)
 				{
@@ -1934,7 +1989,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];

source/pr_edict.c

@@ -156,6 +156,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;

source/quakedef.h

@@ -37,6 +37,7 @@ Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 #include <stdio.h>
 #include <stdlib.h>
 #include <setjmp.h>
+#include <ctype.h>
 //#include <assert.h> // For QMB assert
 
 
@@ -353,6 +354,7 @@ typedef struct
 } waypoint_ai;
 
 extern waypoint_ai waypoints[MAX_WAYPOINTS];
+extern short closest_waypoints[MAX_EDICTS];
 
 
 // thread structs

source/sv_main.c

@@ -1387,12 +1387,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)
 	{
@@ -1403,8 +1413,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);
@@ -1412,7 +1420,12 @@ 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));
@@ -1421,36 +1434,19 @@ void Load_Waypoint ()
 				waypoints[i].open = 0;
 			else
 				waypoints[i].open = 1;
-
+			
-			strcpy(temp, W_substring (W_fgets (h), 9, 20));
+			// Note: this block makes sure that empty/invalid neighbors are always packed to the end
-			waypoints[i].target[0] = atoi (temp);
+			// In other words, when iterating from start, first empty means rest are empty too.
-
+			int slot = 0;
-			strcpy(temp, W_substring (W_fgets (h), 10, 20));
+			for (int t = 0; t < 8; t++) {
-			waypoints[i].target[1] = atoi (temp);
+				int start = t == 0 ? 9 : 10;
-
+				strcpy(temp, W_substring (W_fgets (h), start, 20));
-
+				if (isdigit(temp[0])) {
-			strcpy(temp, W_substring (W_fgets (h), 10, 20));
+					waypoints[i].target[slot] = atoi (temp);
-			waypoints[i].target[2] = atoi (temp);
+					waypoints[i].target_id[slot] = waypoints[i].target[slot];
-
+					slot++;
-
+				}
-			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);
 
 			W_fgets (h);
 			W_fgets (h);
@@ -1473,48 +1469,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 (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)
+				if (waypoints[i].target[p] == s)
-					Sys_Error ("Waypoint (%i) without a target!\n", s);
-				if (waypoints[i].target[p] == waypoints[s].id)
 				{
 					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",
+		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].id,
-			waypoints[i].target[0],
+		waypoints[i].target[0],
-			waypoints[i].target_id[0],
+		waypoints[i].target_id[0],
-			waypoints[i].dist[0],
+		waypoints[i].dist[0],
-			waypoints[i].target[1],
+		waypoints[i].target[1],
-			waypoints[i].target_id[1],
+		waypoints[i].target_id[1],
-			waypoints[i].dist[1],
+		waypoints[i].dist[1],
-			waypoints[i].target[2],
+		waypoints[i].target[2],
-			waypoints[i].target_id[2],
+		waypoints[i].target_id[2],
-			waypoints[i].dist[2],
+		waypoints[i].dist[2],
-			waypoints[i].target[3],
+		waypoints[i].target[3],
-			waypoints[i].target_id[3],
+		waypoints[i].target_id[3],
-			waypoints[i].dist[3],
+		waypoints[i].dist[3],
-			waypoints[i].target[4],
+		waypoints[i].target[4],
-			waypoints[i].target_id[4],
+		waypoints[i].target_id[4],
-			waypoints[i].dist[4],
+		waypoints[i].dist[4],
-			waypoints[i].target[5],
+		waypoints[i].target[5],
-			waypoints[i].target_id[5],
+		waypoints[i].target_id[5],
-			waypoints[i].dist[5],
+		waypoints[i].dist[5],
-			waypoints[i].target[6],
+		waypoints[i].target[6],
-			waypoints[i].target_id[6],
+		waypoints[i].target_id[6],
-			waypoints[i].dist[6],
+		waypoints[i].dist[6],
-			waypoints[i].target[7],
+		waypoints[i].target[7],
-			waypoints[i].target_id[7],
+		waypoints[i].target_id[7],
-			waypoints[i].dist[7]);
+		waypoints[i].dist[7]);
 	}
 	W_fclose(h);
 	//Z_Free (w_string_temp);