X-Git-Url: http://de.git.xonotic.org/?p=xonotic%2Fxonotic-data.pk3dir.git;a=blobdiff_plain;f=qcsrc%2Fserver%2Fbot%2Fdefault%2Fnavigation.qc;h=86f2feebdf718b4c36e4fcb0d578ebb8279fe4f9;hp=fc97931541bc3bcdf5d8401ae7612b9e1b6da866;hb=99f5aae787d0e46852e2340594c5cb4805c0f3f3;hpb=63841ddfb9349787f6380b9e9e2606e6d7ed9eb6

diff --git a/qcsrc/server/bot/default/navigation.qc b/qcsrc/server/bot/default/navigation.qc
index fc9793154..86f2feebd 100644
--- a/qcsrc/server/bot/default/navigation.qc
+++ b/qcsrc/server/bot/default/navigation.qc
@@ -1,5 +1,7 @@
 #include "navigation.qh"
 
+#include <server/defs.qh>
+#include <server/miscfunctions.qh>
 #include "cvars.qh"
 
 #include "bot.qh"
@@ -37,49 +39,94 @@ void navigation_dynamicgoal_unset(entity this)
 	this.nearestwaypointtimeout = -1;
 }
 
-// rough simulation of walking from one point to another to test if a path
-// can be traveled, used for waypoint linking and havocbot
+bool navigation_check_submerged_state(entity ent, vector pos)
+{
+	bool submerged;
+	if(IS_PLAYER(ent))
+		submerged = (ent.waterlevel == WATERLEVEL_SUBMERGED);
+	else if(ent.nav_submerged_state != SUBMERGED_UNDEFINED)
+		submerged = (ent.nav_submerged_state == SUBMERGED_YES);
+	else
+	{
+		submerged = SUBMERGED(pos);
+		// NOTE: SUBMERGED check of box waypoint origin may fail even if origin
+		//  is actually submerged because often they are inside some solid.
+		//  That's why submerged state is saved now that we know current pos is
+		//  not stuck in solid (previous tracewalk call to this pos was successfully)
+		if(!ent.navigation_dynamicgoal)
+			ent.nav_submerged_state = (submerged) ? SUBMERGED_YES : SUBMERGED_NO;
+	}
+	return submerged;
+}
 
-bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float movemode)
+bool navigation_checkladders(entity e, vector org, vector m1, vector m2, vector end, vector end2, int movemode)
 {
-	vector org;
-	vector move;
-	vector dir;
-	float dist;
-	float totaldist;
-	float stepdist;
-	float ignorehazards;
-	float swimming;
-	entity tw_ladder = NULL;
+	IL_EACH(g_ladders, it.classname == "func_ladder",
+	{
+		if(it.bot_pickup)
+		if(boxesoverlap(org + m1 + '-1 -1 -1', org + m2 + '1 1 1', it.absmin, it.absmax))
+		if(boxesoverlap(end, end2, it.absmin + vec2(m1) + '-1 -1 0', it.absmax + vec2(m2) + '1 1 0'))
+		{
+			vector top = org;
+			top.z = it.absmax.z + (PL_MAX_CONST.z - PL_MIN_CONST.z);
+			tracebox(org, m1, m2, top, movemode, e);
+			if(trace_fraction == 1)
+				return true;
+		}
+	});
+	return false;
+}
 
+vector resurface_limited(vector org, float lim, vector m1)
+{
+	if (WETFEET(org + eZ * (lim - org.z)))
+		org.z = lim;
+	else
+	{
+		float RES_min_h = org.z;
+		float RES_max_h = lim;
+		do {
+			org.z = 0.5 * (RES_min_h + RES_max_h);
+			if(WETFEET(org))
+				RES_min_h = org.z;
+			else
+				RES_max_h = org.z;
+		} while (RES_max_h - RES_min_h >= 1);
+		org.z = RES_min_h;
+	}
+	return org;
+}
+#define RESURFACE_LIMITED(org, lim) org = resurface_limited(org, lim, m1)
+
+#define NAV_WALK 0
+#define NAV_SWIM_ONWATER 1
+#define NAV_SWIM_UNDERWATER 2
+
+// rough simulation of walking from one point to another to test if a path
+// can be traveled, used for waypoint linking and havocbot
+// if end_height is > 0 destination is any point in the vertical segment [end, end + end_height * eZ]
+bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float end_height, float movemode)
+{
 	if(autocvar_bot_debug_tracewalk)
 	{
 		debugresetnodes();
 		debugnode(e, start);
 	}
 
-	move = end - start;
-	move.z = 0;
-	org = start;
-	dist = totaldist = vlen(move);
-	dir = normalize(move);
-	stepdist = 32;
-	ignorehazards = false;
-	swimming = false;
+	vector org = start;
+	vector flatdir = end - start;
+	flatdir.z = 0;
+	float flatdist = vlen(flatdir);
+	flatdir = normalize(flatdir);
+	float stepdist = 32;
+	bool ignorehazards = false;
+	int nav_action;
 
 	// Analyze starting point
 	traceline(start, start, MOVE_NORMAL, e);
 	if (trace_dpstartcontents & (DPCONTENTS_SLIME | DPCONTENTS_LAVA))
 		ignorehazards = true;
-	else
-	{
-		traceline( start, start + '0 0 -65536', MOVE_NORMAL, e);
-		if (trace_dpstartcontents & (DPCONTENTS_SLIME | DPCONTENTS_LAVA))
-		{
-			ignorehazards = true;
-			swimming = true;
-		}
-	}
+
 	tracebox(start, m1, m2, start, MOVE_NOMONSTERS, e);
 	if (trace_startsolid)
 	{
@@ -91,78 +138,309 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float m
 		return false;
 	}
 
+	vector end2 = end;
+	if(end_height)
+		end2.z += end_height;
+
+	vector fixed_end = end;
+	vector move;
+
+	if (flatdist > 0 && WETFEET(org))
+	{
+		if (SUBMERGED(org))
+			nav_action = NAV_SWIM_UNDERWATER;
+		else
+		{
+			// tracebox down by player's height
+			// useful to know if water level is so low that bot can still walk
+			tracebox(org, m1, m2, org - eZ * (m2.z - m1.z), movemode, e);
+			if (SUBMERGED(trace_endpos))
+			{
+				org = trace_endpos;
+				nav_action = NAV_SWIM_UNDERWATER;
+			}
+			else
+				nav_action = NAV_WALK;
+		}
+	}
+	else
+		nav_action =  NAV_WALK;
+
 	// Movement loop
-	move = end - org;
-	for (;;)
+	while (true)
 	{
-		if (boxesoverlap(end, end, org + m1 + '-1 -1 -1', org + m2 + '1 1 1'))
+		if (flatdist <= 0)
 		{
-			// Succeeded
-			if(autocvar_bot_debug_tracewalk)
-				debugnodestatus(org, DEBUG_NODE_SUCCESS);
+			bool success = true;
+			if (org.z > end2.z + 1)
+			{
+				tracebox(org, m1, m2, end2, movemode, e);
+				org = trace_endpos;
+				if (org.z > end2.z + 1)
+					success = false;
+			}
+			else if (org.z < end.z - 1)
+			{
+				tracebox(org, m1, m2, org - jumpheight_vec, movemode, e);
+				if (SUBMERGED(trace_endpos))
+				{
+					vector v = trace_endpos;
+					tracebox(v, m1, m2, end, movemode, e);
+					if(trace_endpos.z >= end.z - 1)
+					{
+						RESURFACE_LIMITED(v, trace_endpos.z);
+						trace_endpos = v;
+					}
+				}
+				else if (trace_endpos.z > org.z - jumpheight_vec.z)
+					tracebox(trace_endpos, m1, m2, trace_endpos + jumpheight_vec, movemode, e);
+				org = trace_endpos;
+				if (org.z < end.z - 1)
+					success = false;
+			}
 
-			//print("tracewalk: ", vtos(start), " can reach ", vtos(end), "\n");
-			return true;
+			if (success)
+			{
+				// Succeeded
+				if(autocvar_bot_debug_tracewalk)
+				{
+					debugnode(e, org);
+					debugnodestatus(org, DEBUG_NODE_SUCCESS);
+				}
+
+				//print("tracewalk: ", vtos(start), " can reach ", vtos(end), "\n");
+				return true;
+			}
 		}
+
 		if(autocvar_bot_debug_tracewalk)
 			debugnode(e, org);
 
-		if (dist <= 0)
+		if (flatdist <= 0)
 			break;
-		if (stepdist > dist)
-			stepdist = dist;
-		dist = dist - stepdist;
-		traceline(org, org, MOVE_NORMAL, e);
-		if (!ignorehazards)
-		{
-			if (trace_dpstartcontents & (DPCONTENTS_SLIME | DPCONTENTS_LAVA))
-			{
-				// hazards blocking path
-				if(autocvar_bot_debug_tracewalk)
-					debugnodestatus(org, DEBUG_NODE_FAIL);
 
-				//print("tracewalk: ", vtos(start), " hits a hazard when trying to reach ", vtos(end), "\n");
-				return false;
+		if (stepdist > flatdist)
+			stepdist = flatdist;
+		if(nav_action == NAV_SWIM_UNDERWATER || (nav_action == NAV_SWIM_ONWATER && org.z > end2.z))
+		{
+			// can't use movement direction here to calculate move because of
+			// precision errors especially when direction has a high enough z value
+			//water_dir = normalize(water_end - org);
+			//move = org + water_dir * stepdist;
+			fixed_end.z = bound(end.z, org.z, end2.z);
+			if (stepdist == flatdist) {
+				move = fixed_end;
+				flatdist = 0;
+			} else {
+				move = org + (fixed_end - org) * (stepdist / flatdist);
+				flatdist = vlen(vec2(fixed_end - move));
 			}
 		}
-		if (trace_dpstartcontents & DPCONTENTS_LIQUIDSMASK)
+		else // horiz. direction
 		{
-			move = normalize(end - org);
-			tracebox(org, m1, m2, org + move * stepdist, movemode, e);
+			flatdist -= stepdist;
+			move = org + flatdir * stepdist;
+		}
 
-			if(autocvar_bot_debug_tracewalk)
-				debugnode(e, trace_endpos);
+		if(nav_action == NAV_SWIM_ONWATER)
+		{
+			tracebox(org, m1, m2, move, movemode, e); // swim
 
+			// hit something
 			if (trace_fraction < 1)
 			{
-				swimming = true;
-				org = trace_endpos + normalize(org - trace_endpos) * stepdist;
-				for (; org.z < end.z + e.maxs.z; org.z += stepdist)
+				// stepswim
+				tracebox(org + stepheightvec, m1, m2, move + stepheightvec, movemode, e);
+
+				if (trace_fraction < 1 || trace_startsolid) // can't jump obstacle out of water
 				{
-					if(autocvar_bot_debug_tracewalk)
-						debugnode(e, org);
+					org = trace_endpos;
+					if(navigation_checkladders(e, org, m1, m2, end, end2, movemode))
+					{
+						if(autocvar_bot_debug_tracewalk)
+						{
+							debugnode(e, org);
+							debugnodestatus(org, DEBUG_NODE_SUCCESS);
+						}
 
-					if(pointcontents(org) == CONTENT_EMPTY)
-						break;
-				}
+						//print("tracewalk: ", vtos(start), " can reach ", vtos(end), "\n");
+						return true;
+					}
 
-				if(pointcontents(org + '0 0 1') != CONTENT_EMPTY)
-				{
 					if(autocvar_bot_debug_tracewalk)
 						debugnodestatus(org, DEBUG_NODE_FAIL);
 
 					return false;
-					//print("tracewalk: ", vtos(start), " failed under water\n");
+					//print("tracewalk: ", vtos(start), " hit something when trying to reach ", vtos(end), "\n");
 				}
-				continue;
 
+				//succesful stepswim
+
+				if (flatdist <= 0)
+				{
+					org = trace_endpos;
+					continue;
+				}
+
+				if (org.z <= move.z) // going horiz.
+				{
+					tracebox(trace_endpos, m1, m2, move, movemode, e);
+					org = trace_endpos;
+					nav_action = NAV_WALK;
+					continue;
+				}
+			}
+
+			if (org.z <= move.z) // going horiz.
+			{
+				org = trace_endpos;
+				nav_action = NAV_SWIM_ONWATER;
 			}
-			else
+			else // going down
+			{
 				org = trace_endpos;
+				if (SUBMERGED(org))
+					nav_action = NAV_SWIM_UNDERWATER;
+				else
+					nav_action = NAV_SWIM_ONWATER;
+			}
 		}
-		else
+		else if(nav_action == NAV_SWIM_UNDERWATER)
+		{
+			if (move.z >= org.z) // swimming upwards or horiz.
+			{
+				tracebox(org, m1, m2, move, movemode, e); // swim
+
+				bool stepswum = false;
+
+				// hit something
+				if (trace_fraction < 1)
+				{
+					// stepswim
+					vector stepswim_move = move + stepheightvec;
+					if (flatdist > 0 && stepswim_move.z > end2.z + stepheightvec.z) // don't allow stepswim to go higher than destination
+						stepswim_move.z = end2.z;
+
+					tracebox(org + stepheightvec, m1, m2, stepswim_move, movemode, e);
+
+					// hit something
+					if (trace_startsolid)
+					{
+						if(autocvar_bot_debug_tracewalk)
+							debugnodestatus(org, DEBUG_NODE_FAIL);
+
+						//print("tracewalk: ", vtos(start), " hit something when trying to reach ", vtos(end), "\n");
+						return false;
+					}
+
+					if (trace_fraction < 1)
+					{
+						float org_z_prev = org.z;
+						RESURFACE_LIMITED(org, end2.z);
+						if(org.z == org_z_prev)
+						{
+							if(autocvar_bot_debug_tracewalk)
+								debugnodestatus(org, DEBUG_NODE_FAIL);
+
+							//print("tracewalk: ", vtos(start), " can't reach ", vtos(end), "\n");
+							return false;
+						}
+						if(SUBMERGED(org))
+							nav_action = NAV_SWIM_UNDERWATER;
+						else
+							nav_action = NAV_SWIM_ONWATER;
+
+						// we didn't advance horiz. in this step, flatdist decrease should be reverted
+						// but we can't do it properly right now... apply this workaround instead
+						if (flatdist <= 0)
+							flatdist = 1;
+
+						continue;
+					}
+
+					//succesful stepswim
+
+					if (flatdist <= 0)
+					{
+						org = trace_endpos;
+						continue;
+					}
+
+					stepswum = true;
+				}
+
+				if (!WETFEET(trace_endpos))
+				{
+					tracebox(trace_endpos, m1, m2, trace_endpos - eZ * (stepdist + (m2.z - m1.z)), movemode, e);
+					// if stepswum we'll land on the obstacle, avoid the SUBMERGED check
+					if (!stepswum && SUBMERGED(trace_endpos))
+					{
+						RESURFACE_LIMITED(trace_endpos, end2.z);
+						org = trace_endpos;
+						nav_action = NAV_SWIM_ONWATER;
+						continue;
+					}
+
+					// not submerged
+					org = trace_endpos;
+					nav_action = NAV_WALK;
+					continue;
+				}
+
+				// wetfeet
+				org = trace_endpos;
+				nav_action = NAV_SWIM_UNDERWATER;
+				continue;
+			}
+			else //if (move.z < org.z) // swimming downwards
+			{
+				tracebox(org, m1, m2, move, movemode, e); // swim
+
+				// hit something
+				if (trace_fraction < 1)
+				{
+					// stepswim
+					tracebox(org + stepheightvec, m1, m2, move + stepheightvec, movemode, e);
+
+					// hit something
+					if (trace_fraction < 1 || trace_startsolid) // can't jump obstacle out of water
+					{
+						if(autocvar_bot_debug_tracewalk)
+							debugnodestatus(move, DEBUG_NODE_FAIL);
+
+						//print("tracewalk: ", vtos(start), " hit something when trying to reach ", vtos(end), "\n");
+						return false;
+					}
+
+					//succesful stepswim
+
+					if (flatdist <= 0)
+					{
+						org = trace_endpos;
+						continue;
+					}
+
+					if (trace_endpos.z > org.z && !SUBMERGED(trace_endpos))
+					{
+						// stepswim caused upwards direction
+						tracebox(trace_endpos, m1, m2, trace_endpos - stepheightvec, movemode, e);
+						if (!SUBMERGED(trace_endpos))
+						{
+							org = trace_endpos;
+							nav_action = NAV_WALK;
+							continue;
+						}
+					}
+				}
+
+				org = trace_endpos;
+				nav_action = NAV_SWIM_UNDERWATER;
+				continue;
+			}
+		}
+		else if(nav_action == NAV_WALK)
 		{
-			move = dir * stepdist + org;
+			// walk
 			tracebox(org, m1, m2, move, movemode, e);
 
 			if(autocvar_bot_debug_tracewalk)
@@ -175,48 +453,55 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float m
 				tracebox(org + stepheightvec, m1, m2, move + stepheightvec, movemode, e);
 				if (trace_fraction < 1 || trace_startsolid)
 				{
-					tracebox(org + jumpstepheightvec, m1, m2, move + jumpstepheightvec, movemode, e);
-					if (trace_fraction < 1 || trace_startsolid)
+					if (trace_startsolid) // hit ceiling above org
 					{
+						// reduce stepwalk height
+						tracebox(org, m1, m2, org + stepheightvec, movemode, e);
+						tracebox(trace_endpos, m1, m2, move + eZ * (trace_endpos.z - move.z), movemode, e);
+					}
+					else //if (trace_fraction < 1)
+					{
+						tracebox(org + jumpstepheightvec, m1, m2, move + jumpstepheightvec, movemode, e);
+						if (trace_startsolid) // hit ceiling above org
+						{
+							// reduce jumpstepwalk height
+							tracebox(org, m1, m2, org + jumpstepheightvec, movemode, e);
+							tracebox(trace_endpos, m1, m2, move + eZ * (trace_endpos.z - move.z), movemode, e);
+						}
+					}
+
+					if (trace_fraction < 1)
+					{
+						vector v = trace_endpos;
+						v.z = org.z + jumpheight_vec.z;
+						if(navigation_checkladders(e, v, m1, m2, end, end2, movemode))
+						{
+							if(autocvar_bot_debug_tracewalk)
+							{
+								debugnode(e, v);
+								debugnodestatus(v, DEBUG_NODE_SUCCESS);
+							}
+
+							//print("tracewalk: ", vtos(start), " can reach ", vtos(end), "\n");
+							return true;
+						}
+
 						if(autocvar_bot_debug_tracewalk)
 							debugnodestatus(trace_endpos, DEBUG_NODE_WARNING);
 
-						IL_EACH(g_ladders, it.classname == "func_ladder",
-							{ it.solid = SOLID_BSP; });
-
 						traceline( org, move, movemode, e);
 
-						IL_EACH(g_ladders, it.classname == "func_ladder",
-							{ it.solid = SOLID_TRIGGER; });
-
 						if ( trace_ent.classname == "door_rotating" || trace_ent.classname == "door")
 						{
 							vector nextmove;
 							move = trace_endpos;
 							while(trace_ent.classname == "door_rotating" || trace_ent.classname == "door")
 							{
-								nextmove = move + (dir * stepdist);
+								nextmove = move + (flatdir * stepdist);
 								traceline( move, nextmove, movemode, e);
 								move = nextmove;
 							}
-						}
-						else if (trace_ent.classname == "func_ladder")
-						{
-							tw_ladder = trace_ent;
-							vector ladder_bottom = trace_endpos - dir * m2.x;
-							vector ladder_top = ladder_bottom;
-							ladder_top.z = trace_ent.absmax.z + (-m1.z + 1);
-							tracebox(ladder_bottom, m1, m2, ladder_top, movemode, e);
-							if (trace_fraction < 1 || trace_startsolid)
-							{
-								if(autocvar_bot_debug_tracewalk)
-									debugnodestatus(trace_endpos, DEBUG_NODE_FAIL);
-
-								return false; // failed
-							}
-							org = ladder_top + dir * m2.x;
-							move = org + dir * stepdist;
-							continue;
+							flatdist = vlen(vec2(end - move));
 						}
 						else
 						{
@@ -244,28 +529,43 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float m
 			// (this is the same logic as the Quake walkmove function used)
 			tracebox(move, m1, m2, move + '0 0 -65536', movemode, e);
 
-			// moved successfully
-			if(swimming)
+			org = trace_endpos;
+
+			if (!ignorehazards)
 			{
-				float c;
-				c = pointcontents(org + '0 0 1');
-				if (!(c == CONTENT_WATER || c == CONTENT_LAVA || c == CONTENT_SLIME))
-					swimming = false;
-				else
-					continue;
+				if (IN_LAVA(org))
+				{
+					if(autocvar_bot_debug_tracewalk)
+					{
+						debugnode(e, trace_endpos);
+						debugnodestatus(org, DEBUG_NODE_FAIL);
+					}
+
+					//print("tracewalk: ", vtos(start), " hits a hazard when trying to reach ", vtos(end), "\n");
+					return false;
+				}
 			}
 
-			org = trace_endpos;
-		}
+			if (flatdist <= 0)
+			{
+				if(move.z >= end2.z && org.z < end2.z)
+					org.z = end2.z;
+				continue;
+			}
 
-		if(tw_ladder && org.z < tw_ladder.absmax.z)
-		{
-			// stop tracewalk if destination height is lower than the top of the ladder
-			// otherwise bot can't easily figure out climbing direction
+			if(org.z > move.z - 1 || !SUBMERGED(org))
+			{
+				nav_action = NAV_WALK;
+				continue;
+			}
+
+			// ended up submerged while walking
 			if(autocvar_bot_debug_tracewalk)
-				debugnodestatus(org, DEBUG_NODE_FAIL);
+				debugnode(e, org);
 
-			return false;
+			RESURFACE_LIMITED(org, move.z);
+			nav_action = NAV_SWIM_ONWATER;
+			continue;
 		}
 	}
 
@@ -285,6 +585,9 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float m
 // completely empty the goal stack, used when deciding where to go
 void navigation_clearroute(entity this)
 {
+	this.goalcurrent_prev = this.goalcurrent;
+	this.goalcurrent_distance = 10000000;
+	this.goalcurrent_distance_time = 0;
 	//print("bot ", etos(this), " clear\n");
 	this.goalentity = NULL;
 	this.goalcurrent = NULL;
@@ -329,6 +632,9 @@ void navigation_clearroute(entity this)
 // steps to the goal, and then recalculate the path.
 void navigation_pushroute(entity this, entity e)
 {
+	this.goalcurrent_prev = this.goalcurrent;
+	this.goalcurrent_distance = 10000000;
+	this.goalcurrent_distance_time = 0;
 	//print("bot ", etos(this), " push ", etos(e), "\n");
 	if(this.goalstack31 == this.goalentity)
 		this.goalentity = NULL;
@@ -371,6 +677,9 @@ void navigation_pushroute(entity this, entity e)
 // (used when a spawnfunc_waypoint is reached)
 void navigation_poproute(entity this)
 {
+	this.goalcurrent_prev = this.goalcurrent;
+	this.goalcurrent_distance = 10000000;
+	this.goalcurrent_distance_time = 0;
 	//print("bot ", etos(this), " pop\n");
 	if(this.goalcurrent == this.goalentity)
 		this.goalentity = NULL;
@@ -408,23 +717,24 @@ void navigation_poproute(entity this)
 	this.goalstack31 = NULL;
 }
 
-float navigation_waypoint_will_link(vector v, vector org, entity ent, float walkfromwp, float bestdist)
+// walking to wp (walkfromwp == false) v2 and v2_height will be used as
+// waypoint destination coordinates instead of v (only useful for box waypoints)
+// for normal waypoints v2 == v and v2_height == 0
+float navigation_waypoint_will_link(vector v, vector org, entity ent, vector v2, float v2_height, vector o2, float o2_height, float walkfromwp, float bestdist)
 {
-	float dist;
-	dist = vlen(v - org);
-	if (bestdist > dist)
+	if (vdist(v - org, <, bestdist))
 	{
 		traceline(v, org, true, ent);
 		if (trace_fraction == 1)
 		{
 			if (walkfromwp)
 			{
-				if (tracewalk(ent, v, PL_MIN_CONST, PL_MAX_CONST, org, bot_navigation_movemode))
+				if (tracewalk(ent, v, PL_MIN_CONST, PL_MAX_CONST, v2, v2_height, bot_navigation_movemode))
 					return true;
 			}
 			else
 			{
-				if (tracewalk(ent, org, PL_MIN_CONST, PL_MAX_CONST, v, bot_navigation_movemode))
+				if (tracewalk(ent, org, PL_MIN_CONST, PL_MAX_CONST, o2, o2_height, bot_navigation_movemode))
 					return true;
 			}
 		}
@@ -435,6 +745,9 @@ float navigation_waypoint_will_link(vector v, vector org, entity ent, float walk
 // find the spawnfunc_waypoint near a dynamic goal such as a dropped weapon
 entity navigation_findnearestwaypoint_withdist_except(entity ent, float walkfromwp, float bestdist, entity except)
 {
+	if(ent.tag_entity)
+		ent = ent.tag_entity;
+
 	vector pm1 = ent.origin + ent.mins;
 	vector pm2 = ent.origin + ent.maxs;
 
@@ -445,31 +758,46 @@ entity navigation_findnearestwaypoint_withdist_except(entity ent, float walkfrom
 			return it;
 	});
 
-	vector org = ent.origin + 0.5 * (ent.mins + ent.maxs);
-	org.z = ent.origin.z + ent.mins.z - PL_MIN_CONST.z; // player height
-	// TODO possibly make other code have the same support for bboxes
-	if(ent.tag_entity)
-		org = org + ent.tag_entity.origin;
+	vector org = ent.origin;
 	if (navigation_testtracewalk)
 		te_plasmaburn(org);
 
 	entity best = NULL;
-	vector v;
+	vector v = '0 0 0', v2 = '0 0 0';
+	float v2_height = 0;
+
+	if(ent.size && !IS_PLAYER(ent))
+	{
+		org += 0.5 * (ent.mins + ent.maxs);
+		org.z = ent.origin.z + ent.mins.z - PL_MIN_CONST.z; // player height
+	}
+
+	if(!autocvar_g_waypointeditor && walkfromwp && !ent.navigation_dynamicgoal)
+	{
+		waypoint_clearlinks(ent); // initialize wpXXmincost fields
+		IL_EACH(g_waypoints, it != ent,
+		{
+			if(walkfromwp && (it.wpflags & WAYPOINTFLAG_NORELINK))
+				continue;
+
+			SET_TRACEWALK_DESTCOORDS(it, org, v2, v2_height);
+			if(vdist(v2 - org, <, 1050))
+			if(tracewalk(ent, org, PL_MIN_CONST, PL_MAX_CONST, v2, v2_height, bot_navigation_movemode))
+				navigation_item_addlink(it, ent);
+		});
+	}
 
 	// box check failed, try walk
 	IL_EACH(g_waypoints, it != ent,
 	{
-		if(it.wpisbox)
-		{
-			vector wm1 = it.origin + it.mins;
-			vector wm2 = it.origin + it.maxs;
-			v.x = bound(wm1_x, org.x, wm2_x);
-			v.y = bound(wm1_y, org.y, wm2_y);
-			v.z = bound(wm1_z, org.z, wm2_z);
-		}
+		if(walkfromwp && (it.wpflags & WAYPOINTFLAG_NORELINK))
+			continue;
+		v = it.origin;
+		if(walkfromwp)
+			SET_TRACEWALK_DESTCOORDS(ent, v, v2, v2_height);
 		else
-			v = it.origin;
-		if(navigation_waypoint_will_link(v, org, ent, walkfromwp, bestdist))
+			SET_TRACEWALK_DESTCOORDS(it, org, v2, v2_height);
+		if(navigation_waypoint_will_link(v, org, ent, v2, v2_height, v2, v2_height, walkfromwp, bestdist))
 		{
 			bestdist = vlen(v - org);
 			best = it;
@@ -492,31 +820,23 @@ entity navigation_findnearestwaypoint(entity ent, float walkfromwp)
 // finds the waypoints near the bot initiating a navigation query
 float navigation_markroutes_nearestwaypoints(entity this, float maxdist)
 {
-	vector v, m1, m2;
-//	navigation_testtracewalk = true;
+	vector v = '0 0 0';
+	//navigation_testtracewalk = true;
 	int c = 0;
+	float v_height = 0;
 	IL_EACH(g_waypoints, !it.wpconsidered,
 	{
-		if (it.wpisbox)
-		{
-			m1 = it.origin + it.mins;
-			m2 = it.origin + it.maxs;
-			v = this.origin;
-			v.x = bound(m1_x, v.x, m2_x);
-			v.y = bound(m1_y, v.y, m2_y);
-			v.z = bound(m1_z, v.z, m2_z);
-		}
-		else
-			v = it.origin;
+		SET_TRACEWALK_DESTCOORDS(it, this.origin, v, v_height);
+
 		vector diff = v - this.origin;
 		diff.z = max(0, diff.z);
 		if(vdist(diff, <, maxdist))
 		{
 			it.wpconsidered = true;
-			if (tracewalk(this, this.origin, this.mins, this.maxs, v, bot_navigation_movemode))
+			if (tracewalk(this, this.origin, this.mins, this.maxs, v, v_height, bot_navigation_movemode))
 			{
 				it.wpnearestpoint = v;
-				it.wpcost = vlen(v - this.origin) + it.dmg;
+				it.wpcost = waypoint_gettravelcost(this.origin, v, this, it) + it.dmg;
 				it.wpfire = 1;
 				it.enemy = NULL;
 				c = c + 1;
@@ -528,25 +848,27 @@ float navigation_markroutes_nearestwaypoints(entity this, float maxdist)
 }
 
 // updates a path link if a spawnfunc_waypoint link is better than the current one
-void navigation_markroutes_checkwaypoint(entity w, entity wp, float cost2, vector p)
+void navigation_markroutes_checkwaypoint(entity w, entity wp, float cost, vector p)
 {
-	vector m1;
-	vector m2;
+	vector m1, m2;
 	vector v;
 	if (wp.wpisbox)
 	{
-		m1 = wp.absmin;
-		m2 = wp.absmax;
+		m1 = wp.origin + wp.mins;
+		m2 = wp.origin + wp.maxs;
 		v.x = bound(m1_x, p.x, m2_x);
 		v.y = bound(m1_y, p.y, m2_y);
 		v.z = bound(m1_z, p.z, m2_z);
 	}
 	else
 		v = wp.origin;
-	cost2 = cost2 + vlen(v - p);
-	if (wp.wpcost > cost2)
+	if (w.wpflags & WAYPOINTFLAG_TELEPORT)
+		cost += w.wp00mincost; // assuming teleport has exactly one destination
+	else
+		cost += waypoint_gettravelcost(p, v, w, wp);
+	if (wp.wpcost > cost)
 	{
-		wp.wpcost = cost2;
+		wp.wpcost = cost;
 		wp.enemy = w;
 		wp.wpfire = 1;
 		wp.wpnearestpoint = v;
@@ -681,16 +1003,9 @@ void navigation_markroutes_inverted(entity fixed_source_waypoint)
 			cost = it.wpcost; // cost to walk from it to home
 			p = it.wpnearestpoint;
 			entity wp = it;
-			IL_EACH(g_waypoints, true,
+			IL_EACH(g_waypoints, it != wp,
 			{
-				if(wp != it.wp00) if(wp != it.wp01) if(wp != it.wp02) if(wp != it.wp03)
-				if(wp != it.wp04) if(wp != it.wp05) if(wp != it.wp06) if(wp != it.wp07)
-				if(wp != it.wp08) if(wp != it.wp09) if(wp != it.wp10) if(wp != it.wp11)
-				if(wp != it.wp12) if(wp != it.wp13) if(wp != it.wp14) if(wp != it.wp15)
-				if(wp != it.wp16) if(wp != it.wp17) if(wp != it.wp18) if(wp != it.wp19)
-				if(wp != it.wp20) if(wp != it.wp21) if(wp != it.wp22) if(wp != it.wp23)
-				if(wp != it.wp24) if(wp != it.wp25) if(wp != it.wp26) if(wp != it.wp27)
-				if(wp != it.wp28) if(wp != it.wp29) if(wp != it.wp30) if(wp != it.wp31)
+				if(!waypoint_islinked(it, wp))
 					continue;
 				cost2 = cost + it.dmg;
 				navigation_markroutes_checkwaypoint(wp, it, cost2, p);
@@ -708,6 +1023,9 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
 	if(e.blacklisted)
 		return;
 
+	rangebias = waypoint_getlinearcost(rangebias);
+	f = waypoint_getlinearcost(f);
+
 	if (IS_PLAYER(e))
 	{
 		bool rate_wps = false;
@@ -749,7 +1067,7 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
 		}
 	}
 
-	vector o = (e.absmin + e.absmax) * 0.5;
+	vector goal_org = (e.absmin + e.absmax) * 0.5;
 
 	//print("routerating ", etos(e), " = ", ftos(f), " - ", ftos(rangebias), "\n");
 
@@ -757,7 +1075,7 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
 	if(g_jetpack)
 	if(this.items & IT_JETPACK)
 	if(autocvar_bot_ai_navigation_jetpack)
-	if(vdist(this.origin - o, >, autocvar_bot_ai_navigation_jetpack_mindistance))
+	if(vdist(this.origin - goal_org, >, autocvar_bot_ai_navigation_jetpack_mindistance))
 	{
 		vector pointa, pointb;
 
@@ -768,7 +1086,7 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
 		pointa = trace_endpos - '0 0 1';
 
 		// Point B
-		traceline(o, o + '0 0 65535', MOVE_NORMAL, e);
+		traceline(goal_org, goal_org + '0 0 65535', MOVE_NORMAL, e);
 		pointb = trace_endpos - '0 0 1';
 
 		// Can I see these two points from the sky?
@@ -861,7 +1179,26 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
 		{
 			nwp = navigation_findnearestwaypoint(e, true);
 			if(nwp)
+			{
 				e.nearestwaypoint = nwp;
+
+				vector m1 = nwp.absmin, m2 = nwp.absmax;
+				m1.x = nwp.origin.x; m1.y = nwp.origin.y;
+				m2.x = nwp.origin.x; m2.y = nwp.origin.y;
+				vector ve = (e.absmin - e.absmax) * 0.5;
+				ve.x = bound(m1.x, ve.x, m2.x);
+				ve.y = bound(m1.y, ve.y, m2.y);
+				ve.z = bound(m1.z, ve.z, m2.z);
+
+				m1 = e.absmin; m2 = e.absmax;
+				m1.x = e.origin.x; m1.y = e.origin.y;
+				m2.x = e.origin.x; m2.y = e.origin.y;
+				vector vnwp = nwp.origin;
+				vnwp.x = bound(m1.x, vnwp.x, m2.x);
+				vnwp.y = bound(m1.y, vnwp.y, m2.y);
+				vnwp.z = bound(m1.z, vnwp.z, m2.z);
+				e.nearestwaypoint_dist = vlen(ve - vnwp);
+			}
 			else
 			{
 				LOG_DEBUG("FAILED to find a nearest waypoint to '", e.classname, "' #", etos(e));
@@ -889,8 +1226,9 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
 	if (nwp.wpcost < 10000000)
 	{
 		//te_wizspike(nwp.wpnearestpoint);
-		LOG_DEBUG(e.classname, " ", ftos(f), "/(1+", ftos((nwp.wpcost + vlen(e.origin - nwp.wpnearestpoint))), "/", ftos(rangebias), ") = ");
-		f = f * rangebias / (rangebias + (nwp.wpcost + vlen(o - nwp.wpnearestpoint)));
+		float cost = nwp.wpcost + waypoint_gettravelcost(nwp.wpnearestpoint, goal_org, nwp, e);
+		LOG_DEBUG(e.classname, " ", ftos(f), "/(1+", ftos(cost), "/", ftos(rangebias), ") = ");
+		f = f * rangebias / (rangebias + cost);
 		LOG_DEBUG("considering ", e.classname, " (with rating ", ftos(f), ")");
 		if (navigation_bestrating < f)
 		{
@@ -908,12 +1246,25 @@ bool navigation_routetogoal(entity this, entity e, vector startposition)
 	if (!e)
 		return false;
 
+	entity teleport_goal = NULL;
+
 	this.goalentity = e;
 
+	if(e.wpflags & WAYPOINTFLAG_TELEPORT)
+	{
+		// force teleport destination as route destination
+		teleport_goal = e;
+		navigation_pushroute(this, e.wp00);
+		this.goalentity = e.wp00;
+	}
+
 	// put the entity on the goal stack
 	//print("routetogoal ", etos(e), "\n");
 	navigation_pushroute(this, e);
 
+	if(teleport_goal)
+		e = this.goalentity;
+
 	if(e.classname == "waypoint" && !(e.wpflags & WAYPOINTFLAG_PERSONAL))
 	{
 		this.wp_goal_prev1 = this.wp_goal_prev0;
@@ -925,7 +1276,10 @@ bool navigation_routetogoal(entity this, entity e, vector startposition)
 		return true;
 
 	// if it can reach the goal there is nothing more to do
-	if (tracewalk(this, startposition, STAT(PL_MIN, this), STAT(PL_MAX, this), (e.absmin + e.absmax) * 0.5, bot_navigation_movemode))
+	vector dest = '0 0 0';
+	float dest_height = 0;
+	SET_TRACEWALK_DESTCOORDS(e, startposition, dest, dest_height);
+	if (tracewalk(this, startposition, STAT(PL_MIN, this), STAT(PL_MAX, this), dest, dest_height, bot_navigation_movemode))
 		return true;
 
 	entity nearest_wp = NULL;
@@ -935,6 +1289,8 @@ bool navigation_routetogoal(entity this, entity e, vector startposition)
 		e = e.nearestwaypoint;
 		nearest_wp = e;
 	}
+	else if(teleport_goal)
+		e = teleport_goal;
 	else
 		e = e.enemy; // we already have added it, so...
 
@@ -944,8 +1300,20 @@ bool navigation_routetogoal(entity this, entity e, vector startposition)
 	if(nearest_wp && nearest_wp.enemy)
 	{
 		// often path can be optimized by not adding the nearest waypoint
-		if(tracewalk(this, nearest_wp.enemy.origin, STAT(PL_MIN, this), STAT(PL_MAX, this), (this.goalentity.absmin + this.goalentity.absmax) * 0.5, bot_navigation_movemode))
+		if (this.goalentity.nearestwaypoint_dist < 8)
 			e = nearest_wp.enemy;
+		else
+		{
+			if (this.goalentity.navigation_dynamicgoal || autocvar_g_waypointeditor)
+			{
+				SET_TRACEWALK_DESTCOORDS(this.goalentity, nearest_wp.enemy.origin, dest, dest_height);
+				if(vdist(dest - nearest_wp.enemy.origin, <, 1050))
+				if(tracewalk(this, nearest_wp.enemy.origin, STAT(PL_MIN, this), STAT(PL_MAX, this), dest, dest_height, bot_navigation_movemode))
+					e = nearest_wp.enemy;
+			}
+			else if(navigation_item_islinked(nearest_wp.enemy, this.goalentity))
+				e = nearest_wp.enemy;
+		}
 	}
 
 	for (;;)
@@ -965,17 +1333,12 @@ bool navigation_routetogoal(entity this, entity e, vector startposition)
 // (this is how bots detect if they reached a goal)
 void navigation_poptouchedgoals(entity this)
 {
-	vector org, m1, m2;
-	org = this.origin;
-	m1 = org + this.mins;
-	m2 = org + this.maxs;
-
 	if(this.goalcurrent.wpflags & WAYPOINTFLAG_TELEPORT)
 	{
 		// make sure jumppad is really hit, don't rely on distance based checks
 		// as they may report a touch even if it didn't really happen
-		if(this.lastteleporttime>0)
-		if(time - this.lastteleporttime < ((this.goalcurrent.wpflags & WAYPOINTFLAG_PERSONAL) ? 2 : 0.15))
+		if(this.lastteleporttime > 0
+			&& time - this.lastteleporttime < ((this.goalcurrent.wpflags & WAYPOINTFLAG_PERSONAL) ? 2 : 0.15))
 		{
 			if(this.aistatus & AI_STATUS_WAYPOINT_PERSONAL_GOING)
 			if(this.goalcurrent.wpflags & WAYPOINTFLAG_PERSONAL && this.goalcurrent.owner==this)
@@ -984,31 +1347,38 @@ void navigation_poptouchedgoals(entity this)
 				this.aistatus |= AI_STATUS_WAYPOINT_PERSONAL_REACHED;
 			}
 			navigation_poproute(this);
-			return;
 		}
+		else
+			return;
 	}
 
 	// If for some reason the bot is closer to the next goal, pop the current one
 	if(this.goalstack01 && !wasfreed(this.goalstack01))
 	if(vlen2(this.goalcurrent.origin - this.origin) > vlen2(this.goalstack01.origin - this.origin))
 	if(checkpvs(this.origin + this.view_ofs, this.goalstack01))
-	if(tracewalk(this, this.origin, this.mins, this.maxs, (this.goalstack01.absmin + this.goalstack01.absmax) * 0.5, bot_navigation_movemode))
 	{
-		LOG_DEBUG("path optimized for ", this.netname, ", removed a goal from the queue");
-		navigation_poproute(this);
-		// TODO this may also be a nice idea to do "early" (e.g. by
-		// manipulating the vlen() comparisons) to shorten paths in
-		// general - this would make bots walk more "on rails" than
-		// "zigzagging" which they currently do with sufficiently
-		// random-like waypoints, and thus can make a nice bot
-		// personality property
+		vector dest = (this.goalstack01.absmin + this.goalstack01.absmax) * 0.5;
+		dest.z = this.goalstack01.absmin.z;
+		float dest_height = this.goalstack01.absmax.z - this.goalstack01.absmin.z;
+		if(tracewalk(this, this.origin, this.mins, this.maxs, dest, dest_height, bot_navigation_movemode))
+		{
+			LOG_DEBUG("path optimized for ", this.netname, ", removed a goal from the queue");
+			navigation_poproute(this);
+			if(this.goalcurrent && this.goalcurrent.wpflags & WAYPOINTFLAG_TELEPORT)
+				return;
+			// TODO this may also be a nice idea to do "early" (e.g. by
+			// manipulating the vlen() comparisons) to shorten paths in
+			// general - this would make bots walk more "on rails" than
+			// "zigzagging" which they currently do with sufficiently
+			// random-like waypoints, and thus can make a nice bot
+			// personality property
+		}
 	}
 
 	// Loose goal touching check when running
 	if(this.aistatus & AI_STATUS_RUNNING)
 	if(this.goalcurrent.classname=="waypoint")
-	if(!(this.goalcurrent.wpflags & WAYPOINTFLAG_TELEPORT))
-	if(vlen(this.velocity - eZ * this.velocity.z) >= autocvar_sv_maxspeed) // if -really- running
+	if(vdist(vec2(this.velocity), >=, autocvar_sv_maxspeed)) // if -really- running
 	{
 		if(vdist(this.origin - this.goalcurrent.origin, <, 150))
 		{
@@ -1024,6 +1394,8 @@ void navigation_poptouchedgoals(entity this)
 				}
 
 				navigation_poproute(this);
+				if(this.goalcurrent && this.goalcurrent.wpflags & WAYPOINTFLAG_TELEPORT)
+					return;
 			}
 		}
 	}
@@ -1037,10 +1409,7 @@ void navigation_poptouchedgoals(entity this)
 			gc_min = this.goalcurrent.origin - '1 1 1' * 12;
 			gc_max = this.goalcurrent.origin + '1 1 1' * 12;
 		}
-		if(!boxesoverlap(m1, m2, gc_min, gc_max))
-			break;
-
-		if((this.goalcurrent.wpflags & WAYPOINTFLAG_TELEPORT))
+		if(!boxesoverlap(this.absmin, this.absmax, gc_min, gc_max))
 			break;
 
 		// Detect personal waypoints
@@ -1052,6 +1421,8 @@ void navigation_poptouchedgoals(entity this)
 		}
 
 		navigation_poproute(this);
+		if(this.goalcurrent && this.goalcurrent.wpflags & WAYPOINTFLAG_TELEPORT)
+			return;
 	}
 }
 
@@ -1093,11 +1464,13 @@ void botframe_updatedangerousobjects(float maxupdate)
 	vector m1, m2, v, o;
 	float c, d, danger;
 	c = 0;
+	entity wp_cur;
 	IL_EACH(g_waypoints, true,
 	{
 		danger = 0;
-		m1 = it.mins;
-		m2 = it.maxs;
+		m1 = it.absmin;
+		m2 = it.absmax;
+		wp_cur = it;
 		IL_EACH(g_bot_dodge, it.bot_dodge,
 		{
 			v = it.origin;
@@ -1105,7 +1478,7 @@ void botframe_updatedangerousobjects(float maxupdate)
 			v.y = bound(m1_y, v.y, m2_y);
 			v.z = bound(m1_z, v.z, m2_z);
 			o = (it.absmin + it.absmax) * 0.5;
-			d = it.bot_dodgerating - vlen(o - v);
+			d = waypoint_getlinearcost(it.bot_dodgerating) - waypoint_gettravelcost(o, v, it, wp_cur);
 			if (d > 0)
 			{
 				traceline(o, v, true, NULL);
@@ -1143,7 +1516,10 @@ void navigation_unstuck(entity this)
 		// evaluate the next goal on the queue
 		float d = vlen2(this.origin - bot_waypoint_queue_goal.origin);
 		LOG_DEBUG(this.netname, " evaluating ", bot_waypoint_queue_goal.classname, " with distance ", ftos(d));
-		if(tracewalk(bot_waypoint_queue_goal, this.origin, STAT(PL_MIN, this), STAT(PL_MAX, this), bot_waypoint_queue_goal.origin, bot_navigation_movemode))
+		vector dest = (bot_waypoint_queue_goal.absmin + bot_waypoint_queue_goal.absmax) * 0.5;
+		dest.z = bot_waypoint_queue_goal.absmin.z;
+		float dest_height = bot_waypoint_queue_goal.absmax.z - bot_waypoint_queue_goal.absmin.z;
+		if(tracewalk(bot_waypoint_queue_goal, this.origin, STAT(PL_MIN, this), STAT(PL_MAX, this), dest, dest_height, bot_navigation_movemode))
 		{
 			if( d > bot_waypoint_queue_bestgoalrating)
 			{