]> de.git.xonotic.org Git - xonotic/xonotic-data.pk3dir.git/blobdiff - qcsrc/server/bot/default/navigation.qc
Merge branch 'master' into terencehill/bot_waypoints
[xonotic/xonotic-data.pk3dir.git] / qcsrc / server / bot / default / navigation.qc
index c93e826152ab68501d345e0f66901175690c8d6a..e450da01f369ee6e862acf77e7639348004ab59f 100644 (file)
@@ -37,13 +37,33 @@ void navigation_dynamicgoal_unset(entity this)
        this.nearestwaypointtimeout = -1;
 }
 
-bool navigation_checkladders(entity e, vector org, vector move, vector m1, vector m2, vector end, vector end2, int movemode)
+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 navigation_checkladders(entity e, vector org, vector m1, vector m2, vector end, vector end2, int movemode)
 {
        IL_EACH(g_ladders, it.classname == "func_ladder",
        {
                if(it.bot_pickup)
-               if(boxesoverlap(move + m1 + '-1 -1 -1', move + m2 + '1 1 1', it.absmin, it.absmax))
-               if(boxesoverlap(end, end2, it.absmin + (m1 - eZ * m1.z - '1 1 0'), it.absmax + (m2 - eZ * m2.z + '1 1 0')))
+               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);
@@ -55,6 +75,31 @@ bool navigation_checkladders(entity e, vector org, vector move, vector m1, vecto
        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]
@@ -67,27 +112,19 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float e
        }
 
        vector org = start;
-       vector dir = end - start;
-       dir.z = 0;
-       float dist = vlen(dir);
-       dir = normalize(dir);
+       vector flatdir = end - start;
+       flatdir.z = 0;
+       float flatdist = vlen(flatdir);
+       flatdir = normalize(flatdir);
        float stepdist = 32;
        bool ignorehazards = false;
-       bool swimming = 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)
        {
@@ -102,89 +139,298 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float e
        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
-       for (;;)
+       while (true)
        {
-               if (boxesoverlap(end, end2, 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))
+                                       RESURFACE_LIMITED(trace_endpos, end.z);
+                               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;
+                       }
+
+                       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;
+                               //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
                {
-                       vector water_end = end;
-                       water_end.z = bound(end.z, org.z, end2.z);
-                       vector water_dir = normalize(water_end - org);
-                       vector move = org + water_dir * stepdist;
-                       tracebox(org, m1, m2, move, 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 - water_dir * (stepdist / 2);
-                               for (; org.z < end2.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");
                                }
 
-                               if(navigation_checkladders(e, org, move, m1, m2, end, end2, movemode))
+                               //succesful stepswim
+
+                               if (flatdist <= 0)
                                {
-                                       if(autocvar_bot_debug_tracewalk)
-                                               debugnodestatus(trace_endpos, DEBUG_NODE_SUCCESS);
+                                       org = trace_endpos;
+                                       continue;
+                               }
 
-                                       //print("tracewalk: ", vtos(start), " can reach ", vtos(end), "\n");
-                                       return true;
+                               if (org.z <= move.z) // going horiz.
+                               {
+                                       tracebox(trace_endpos, m1, m2, move, movemode, e);
+                                       org = trace_endpos;
+                                       nav_action = NAV_WALK;
+                                       continue;
                                }
-                               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 stepswimmed = false;
+
+                               // hit something
+                               if (trace_fraction < 1)
+                               {
+                                       // stepswim
+                                       vector stepswim_move = move + stepheightvec;
+                                       if (flatdist > 0 && stepswim_move.z > end2.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;
+                                       }
+
+                                       stepswimmed = true;
+                               }
+
+                               if (!WETFEET(trace_endpos))
+                               {
+                                       tracebox(trace_endpos, m1, m2, trace_endpos - eZ * (stepdist + (m2.z - m1.z)), movemode, e);
+                                       // if stepswimmed we'll land on the obstacle, avoid the SUBMERGED check
+                                       if (!stepswimmed && 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)
                {
-                       vector move = org + dir * stepdist;
+                       // walk
                        tracebox(org, m1, m2, move, movemode, e);
 
                        if(autocvar_bot_debug_tracewalk)
@@ -197,14 +443,34 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float e
                                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
                                        {
-                                               if(navigation_checkladders(e, trace_endpos - dir * (stepdist / 2),
-                                                       move + jumpheight_vec, m1, m2, end, end2, movemode))
+                                               // 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)
-                                                               debugnodestatus(trace_endpos, DEBUG_NODE_SUCCESS);
+                                                       {
+                                                               debugnode(e, v);
+                                                               debugnodestatus(v, DEBUG_NODE_SUCCESS);
+                                                       }
 
                                                        //print("tracewalk: ", vtos(start), " can reach ", vtos(end), "\n");
                                                        return true;
@@ -221,10 +487,11 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float e
                                                        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;
                                                        }
+                                                       flatdist = vlen(vec2(end - move));
                                                }
                                                else
                                                {
@@ -252,18 +519,43 @@ bool tracewalk(entity e, vector start, vector m1, vector m2, vector end, float e
                        // (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(org.z > move.z - 1 || !SUBMERGED(org))
+                       {
+                               nav_action = NAV_WALK;
+                               continue;
+                       }
+
+                       // ended up submerged while walking
+                       if(autocvar_bot_debug_tracewalk)
+                               debugnode(e, org);
+
+                       RESURFACE_LIMITED(org, move.z);
+                       nav_action = NAV_SWIM_ONWATER;
+                       continue;
                }
        }
 
@@ -415,11 +707,12 @@ 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, 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)
@@ -431,7 +724,7 @@ float navigation_waypoint_will_link(vector v, vector org, entity ent, float walk
                        }
                        else
                        {
-                               if (tracewalk(ent, org, PL_MIN_CONST, PL_MAX_CONST, v, 0, bot_navigation_movemode))
+                               if (tracewalk(ent, org, PL_MIN_CONST, PL_MAX_CONST, v2, v2_height, bot_navigation_movemode))
                                        return true;
                        }
                }
@@ -461,7 +754,8 @@ entity navigation_findnearestwaypoint_withdist_except(entity ent, float walkfrom
                te_plasmaburn(org);
 
        entity best = NULL;
-       vector v;
+       vector v, v2;
+       float v2_height;
 
        if(!autocvar_g_waypointeditor && !ent.navigation_dynamicgoal)
        {
@@ -471,17 +765,8 @@ entity navigation_findnearestwaypoint_withdist_except(entity ent, float walkfrom
                        if(walkfromwp && (it.wpflags & WAYPOINTFLAG_NORELINK))
                                continue;
 
-                       if(it.wpisbox)
-                       {
-                               vector wm1 = it.absmin;
-                               vector wm2 = it.absmax;
-                               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);
-                       }
-                       else
-                               v = it.origin;
-                       if(navigation_waypoint_will_link(v, org, ent, walkfromwp, 1050))
+                       SET_TRACEWALK_DESTCOORDS_2(it, org, v, v2, v2_height);
+                       if(navigation_waypoint_will_link(v, org, ent, v2, v2_height, walkfromwp, 1050))
                                navigation_item_addlink(it, ent);
                });
        }
@@ -491,18 +776,8 @@ entity navigation_findnearestwaypoint_withdist_except(entity ent, float walkfrom
        {
                if(walkfromwp && (it.wpflags & WAYPOINTFLAG_NORELINK))
                        continue;
-
-               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);
-               }
-               else
-                       v = it.origin;
-               if(navigation_waypoint_will_link(v, org, ent, walkfromwp, bestdist))
+               SET_TRACEWALK_DESTCOORDS_2(it, org, v, v2, v2_height);
+               if(navigation_waypoint_will_link(v, org, ent, v2, v2_height, walkfromwp, bestdist))
                {
                        bestdist = vlen(v - org);
                        best = it;
@@ -525,27 +800,14 @@ 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;
+       vector v;
        //navigation_testtracewalk = true;
        int c = 0;
        float v_height;
        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 = m1.z;
-                       v_height = m2.z - m1.z;
-               }
-               else
-               {
-                       v = it.origin;
-                       v_height = 0;
-               }
+               SET_TRACEWALK_DESTCOORDS(it, this.origin, v, v_height);
+
                vector diff = v - this.origin;
                diff.z = max(0, diff.z);
                if(vdist(diff, <, maxdist))
@@ -554,7 +816,7 @@ float navigation_markroutes_nearestwaypoints(entity this, float maxdist)
                        if (tracewalk(this, this.origin, this.mins, this.maxs, v, v_height, bot_navigation_movemode))
                        {
                                it.wpnearestpoint = v;
-                               it.wpcost = waypoint_gettravelcost(this.origin, v) + it.dmg;
+                               it.wpcost = waypoint_gettravelcost(this.origin, v, this, it) + it.dmg;
                                it.wpfire = 1;
                                it.enemy = NULL;
                                c = c + 1;
@@ -568,13 +830,12 @@ 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 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);
@@ -584,7 +845,7 @@ void navigation_markroutes_checkwaypoint(entity w, entity wp, float cost, vector
        if (w.wpflags & WAYPOINTFLAG_TELEPORT)
                cost += w.wp00mincost; // assuming teleport has exactly one destination
        else
-               cost += waypoint_gettravelcost(p, v);
+               cost += waypoint_gettravelcost(p, v, w, wp);
        if (wp.wpcost > cost)
        {
                wp.wpcost = cost;
@@ -945,7 +1206,7 @@ void navigation_routerating(entity this, entity e, float f, float rangebias)
        if (nwp.wpcost < 10000000)
        {
                //te_wizspike(nwp.wpnearestpoint);
-               float cost = nwp.wpcost + waypoint_gettravelcost(nwp.wpnearestpoint, goal_org);
+               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), ")");
@@ -1088,7 +1349,7 @@ void navigation_poptouchedgoals(entity this)
        // Loose goal touching check when running
        if(this.aistatus & AI_STATUS_RUNNING)
        if(this.goalcurrent.classname=="waypoint")
-       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))
                {
@@ -1170,11 +1431,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.absmin;
                m2 = it.absmax;
+               wp_cur = it;
                IL_EACH(g_bot_dodge, it.bot_dodge,
                {
                        v = it.origin;
@@ -1182,7 +1445,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 = waypoint_getlinearcost(it.bot_dodgerating) - waypoint_gettravelcost(o, v);
+                       d = waypoint_getlinearcost(it.bot_dodgerating) - waypoint_gettravelcost(o, v, it, wp_cur);
                        if (d > 0)
                        {
                                traceline(o, v, true, NULL);