NX/VITA: Pathfinding performance improvement

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)

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.

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;
+	int i, s;
-	float dist;
-	int i, s, best, best_target;
 	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;
+	int zombie_entnum = G_EDICTNUM(OFS_PARM0);
-	dist = 0;
+	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++)
+	float best_dist_z = max_waypoint_distance * max_waypoint_distance;
-	{
+	float dist_z = 0;
-		if (waypoints[i].used && waypoints[i].open)
+	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)
+			if (trace.fraction >= 1) {
-			{
+				best_dist_z = dist_z;
-				dist = VecLength2(waypoints[i].origin, zombie->v.origin);
+				best_z = i;
-
-				if(dist < best_dist)
-				{
-					best_dist = dist;
-					best = i;
-				}
 			}
 		}
-	}
 
-	best_dist = 1000000000;
+		dist_e = VectorDistanceSquared(waypoints[i].origin, ent->v.origin);
-	dist = 0;
+		if (dist_e < best_dist_e) {
-	best_target = 0;
-	for (i = 0; i < MAX_WAYPOINTS; i++)
-	{
-		if (waypoints[i].used && waypoints[i].open)
-		{
 			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_e = dist_e;
-				dist = VecLength2(waypoints[i].origin, ent->v.origin);
+				best_e = i;
-
-				if(dist < best_dist)
-				{
-					best_dist = dist;
-					best_target = 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;

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;

source/quakedef.h

@@ -337,6 +337,9 @@ typedef struct
 #include "cdaudio.h"
 #include "glquake.h"
 
+#include <ctype.h>
+
+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 */
 

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));
+			// 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);
@@ -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)
+				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)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].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);