Index: source/simulation2/components/CCmpUnitMotion.cpp
===================================================================
--- source/simulation2/components/CCmpUnitMotion.cpp
+++ source/simulation2/components/CCmpUnitMotion.cpp
@@ -1011,6 +1011,12 @@
 
 		if (m_PathState == PATHSTATE_FOLLOWING)
 		{
+			// If we're following a path and can see the next long waypoint, head towards that point instead.
+			const Grid<NavcellData>& passGrid = cmpPathfinder->GetPassabilityGrid();
+			std::vector<Waypoint>& w = m_LongPath.m_Waypoints;
+			while (w.size() > 1 && Pathfinding::CheckLineMovement(pos.X, pos.Y, w[w.size()-2].x, w[w.size()-2].z, m_PassClass, passGrid))
+				w.pop_back();
+
 			// If we're not currently computing any new paths:
 			if (m_LongPath.m_Waypoints.empty() && m_ShortPath.m_Waypoints.empty())
 			{
Index: source/simulation2/helpers/LongPathfinder.cpp
===================================================================
--- source/simulation2/helpers/LongPathfinder.cpp
+++ source/simulation2/helpers/LongPathfinder.cpp
@@ -968,8 +968,19 @@
 		PathfindTile& n = state.tiles->get(ip, jp);
 		entity_pos_t x, z;
 		Pathfinding::NavcellCenter(ip, jp, x, z);
+#if ACCEPT_DIAGONAL_GAPS
 		path.m_Waypoints.emplace_back(Waypoint{ x, z });
-
+#else
+		// Remove redundant diagonal waypoints
+		if (path.m_Waypoints.size() > 1 &&
+		    path.m_Waypoints.back().x != x &&
+		    path.m_Waypoints.back().z != z &&
+		    path.m_Waypoints.back().x != path.m_Waypoints[path.m_Waypoints.size()-2].x &&
+		    path.m_Waypoints.back().z != path.m_Waypoints[path.m_Waypoints.size()-2].z)
+			path.m_Waypoints.back() = Waypoint{ x, z };
+		else
+			path.m_Waypoints.emplace_back(Waypoint{ x, z });
+#endif
 		// Follow the predecessor link
 		ip = n.GetPredI(ip);
 		jp = n.GetPredJ(jp);
@@ -979,7 +990,10 @@
 	if (!path.m_Waypoints.empty())
 		path.m_Waypoints.front() = { state.goal.x, state.goal.z };
 
-	ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0);
+	// If we can see the next long waypoint from our starting position, head towards that point instead
+	while (path.m_Waypoints.size() > 1 && Pathfinding::CheckLineMovement(x0, z0,
+	    path.m_Waypoints[path.m_Waypoints.size()-2].x, path.m_Waypoints[path.m_Waypoints.size()-2].z, passClass, *m_Grid))
+		path.m_Waypoints.pop_back();
 
 	// Save this grid for debug display
 	delete m_DebugGrid;
@@ -988,58 +1002,6 @@
 	m_DebugGoal = state.goal;
 }
 
-void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0)
-{
-	if (path.m_Waypoints.empty())
-		return;
-
-	if (maxDist > fixed::Zero())
-	{
-		CFixedVector2D start(x0, z0);
-		CFixedVector2D first(path.m_Waypoints.back().x, path.m_Waypoints.back().z);
-		CFixedVector2D offset = first - start;
-		if (offset.CompareLength(maxDist) > 0)
-		{
-			offset.Normalize(maxDist);
-			path.m_Waypoints.emplace_back(Waypoint{ (start + offset).X, (start + offset).Y });
-		}
-	}
-
-	if (path.m_Waypoints.size() < 2)
-		return;
-
-	std::vector<Waypoint>& waypoints = path.m_Waypoints;
-	std::vector<Waypoint> newWaypoints;
-
-	CFixedVector2D prev(waypoints.front().x, waypoints.front().z);
-	newWaypoints.push_back(waypoints.front());
-	for (size_t k = 1; k < waypoints.size() - 1; ++k)
-	{
-		CFixedVector2D ahead(waypoints[k + 1].x, waypoints[k + 1].z);
-		CFixedVector2D curr(waypoints[k].x, waypoints[k].z);
-
-		if (maxDist > fixed::Zero() && (curr - prev).CompareLength(maxDist) > 0)
-		{
-			// We are too far away from the previous waypoint, so create one in
-			// between and continue with the improvement of the path
-			prev = prev + (curr - prev) / 2;
-			newWaypoints.emplace_back(Waypoint{ prev.X, prev.Y });
-		}
-
-		// If we're mostly straight, don't even bother.
-		if ((ahead - curr).Perpendicular().Dot(curr - prev).Absolute() <= fixed::Epsilon() * 100)
-			continue;
-
-		if (!Pathfinding::CheckLineMovement(prev.X, prev.Y, ahead.X, ahead.Y, passClass, *m_Grid))
-		{
-			prev = CFixedVector2D(waypoints[k].x, waypoints[k].z);
-			newWaypoints.push_back(waypoints[k]);
-		}
-	}
-	newWaypoints.push_back(waypoints.back());
-	path.m_Waypoints.swap(newWaypoints);
-}
-
 void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) const
 {
 	steps = m_DebugSteps;