Try to anticipate unit collisions to improve pathing

source/simulation2/components/CCmpObstructionManager.cpp

@@ -481,6 +481,7 @@ public:
 	bool AreShapesInRange(const ObstructionSquare& source, const ObstructionSquare& target, entity_pos_t minRange, entity_pos_t maxRange, bool opposite) const override;
 
 	bool TestLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits = false) const override;
+	bool TestUnitLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits) const override;
 	bool TestStaticShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, std::vector<entity_id_t>* out) const override;
 	bool TestUnitShape(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t r, std::vector<entity_id_t>* out) const override;
 
@@ -860,8 +861,6 @@ bool CCmpObstructionManager::AreShapesInRange(const ObstructionSquare& source, c
 
 bool CCmpObstructionManager::TestLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits) const
 {
-	PROFILE("TestLine");
-
 	// Check that both end points are within the world (which means the whole line must be)
 	if (!IsInWorld(x0, z0, r) || !IsInWorld(x1, z1, r))
 		return true;
@@ -909,6 +908,39 @@ bool CCmpObstructionManager::TestLine(const IObstructionTestFilter& filter, enti
 	return false;
 }
 
+bool CCmpObstructionManager::TestUnitLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits) const
+{
+	// Check that both end points are within the world (which means the whole line must be)
+	if (!IsInWorld(x0, z0, r) || !IsInWorld(x1, z1, r))
+		return true;
+
+	CFixedVector2D posMin (std::min(x0, x1) - r, std::min(z0, z1) - r);
+	CFixedVector2D posMax (std::max(x0, x1) + r, std::max(z0, z1) + r);
+
+	// actual radius used for unit-unit collisions. If relaxClearanceForUnits, will be smaller to allow more overlap.
+	entity_pos_t unitUnitRadius = r;
+	if (relaxClearanceForUnits)
+		unitUnitRadius -= entity_pos_t::FromInt(1)/2;
+
+	std::vector<entity_id_t> unitShapes;
+	m_UnitSubdivision.GetInRange(unitShapes, posMin, posMax);
+	for (const entity_id_t& shape : unitShapes)
+	{
+		std::map<u32, UnitShape>::const_iterator it = m_UnitShapes.find(shape);
+		ENSURE(it != m_UnitShapes.end());
+
+		if (!filter.TestShape(UNIT_INDEX_TO_TAG(it->first), it->second.flags, it->second.group, INVALID_ENTITY))
+			continue;
+
+		CFixedVector2D center(it->second.x, it->second.z);
+		CFixedVector2D halfSize(it->second.clearance + unitUnitRadius, it->second.clearance + unitUnitRadius);
+		if (Geometry::TestRayAASquare(CFixedVector2D(x0, z0) - center, CFixedVector2D(x1, z1) - center, halfSize))
+			return true;
+	}
+	return false;
+}
+
+
 bool CCmpObstructionManager::TestStaticShape(const IObstructionTestFilter& filter,
 	entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h,
 	std::vector<entity_id_t>* out) const

source/simulation2/components/CCmpPathfinder.cpp

@@ -859,8 +859,6 @@ bool CCmpPathfinder::CheckMovement(const IObstructionTestFilter& filter,
 	entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r,
 	pass_class_t passClass) const
 {
-	PROFILE2_IFSPIKE("Check Movement", 0.001);
-
 	// Test against obstructions first. filter may discard pathfinding-blocking obstructions.
 	// Use more permissive version of TestLine to allow unit-unit collisions to overlap slightly.
 	// This makes movement smoother and more natural for units, overall.

source/simulation2/components/CCmpUnitMotion.h

@@ -1093,13 +1093,47 @@ void CCmpUnitMotion::PostMove(CCmpUnitMotionManager::MotionState& state, fixed d
 	else if (!state.wasObstructed && state.pos != state.initialPos)
 		m_FailedMovements = 0;
 
+	bool needPathUpdate = PathingUpdateNeeded(state.pos);
+
+	// If we're following a long-path, check if we might run into units in advance, to smoothe motion.
+	if (!needPathUpdate && !state.wasObstructed && m_LongPath.m_Waypoints.size() >= 1)
+	{
+		ICmpObstructionManager::tag_t specificIgnore;
+		if (m_MoveRequest.m_Type == MoveRequest::ENTITY)
+		{
+			CmpPtr<ICmpObstruction> cmpTargetObstruction(GetSimContext(), m_MoveRequest.m_Entity);
+			if (cmpTargetObstruction)
+				specificIgnore = cmpTargetObstruction->GetObstruction();
+		}
+		CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity());
+		if (cmpObstructionManager && cmpObstructionManager->TestUnitLine(GetObstructionFilter(specificIgnore),
+			state.pos.X, state.pos.Y, m_LongPath.m_Waypoints.back().x, m_LongPath.m_Waypoints.back().z, m_Clearance, true))
+		{
+			// We will run into something: request a new short path.
+			// If we have several waypoints left, aim for the one after next directly.
+			// (This is mostly because the obstruction might be at the waypoint, and the end is kind of treated specially).
+			// Else just path to the goal.
+			if (m_LongPath.m_Waypoints.size() > 1) {
+				fixed radius = Pathfinding::NAVCELL_SIZE * 2;
+				m_LongPath.m_Waypoints.pop_back();
+				PathGoal subgoal = { PathGoal::CIRCLE, m_LongPath.m_Waypoints.back().x, m_LongPath.m_Waypoints.back().z, radius };
+				RequestShortPath(state.pos, subgoal, false);
+			} else {
+				// If we only have one waypoint left, request a short path to the waypoint itself.
+				PathGoal goal;
+				if (ComputeGoal(goal, m_MoveRequest))
+					RequestShortPath(state.pos, goal, false);
+			}
+		}
+	}
+
 	// If we moved straight, and didn't quite finish the path, reset - we'll update it next turn if still OK.
 	if (state.wentStraight && !state.wasObstructed)
 		m_ShortPath.m_Waypoints.clear();
 
 	// We may need to recompute our path sometimes (e.g. if our target moves).
 	// Since we request paths asynchronously anyways, this does not need to be done before moving.
-	if (!state.wentStraight && PathingUpdateNeeded(state.pos))
+	if (!state.wentStraight && needPathUpdate)
 	{
 		PathGoal goal;
 		if (ComputeGoal(goal, m_MoveRequest))

source/simulation2/components/ICmpObstructionManager.h

@@ -1,4 +1,4 @@
-/* Copyright (C) 2022 Wildfire Games.
+/* Copyright (C) 2023 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -251,6 +251,8 @@ public:
 	 */
 	virtual bool TestLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits) const = 0;
 
+	virtual bool TestUnitLine(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, bool relaxClearanceForUnits) const = 0;
+
 	/**
 	 * Collision test a static square shape against the current set of shapes.
 	 * @param filter filter to restrict the shapes that are being tested against