Pathfinding.cpp

// A pathfinding algorithm with a novel change to the path simplification algorithm
// to make for more believable crowd simulations.

// While traditional methods for pathfinding on navigation meshes work great for
// individual agents, they have problems with crowds. Since all agents try to
// follow the same shortest path from one point to another, agents end up
// in traffic jams, funneling towards single points and in single file lines.

// My algorithm returns a simplified set of triangle edges to walk through instead of a
// set of simplified single points. Thus, allowing for agents to spread out and
// move more naturally in crowds.



// Perform A* pathfinding algorithm from a starting triangle to a goal position
// on the NavMesh. Returns true if a path was found and sets each triangle's
// parent to be the next triangle in the path.
bool NavMesh::AStar(const NavTri &startTri, const v3 &goalPosition, NavTri **endTri) {
	// Find the NavTri containing the goal position. If no triangle contain the
	// goal, no path can be obtained.
	NavTri *goalTri = FindContainingNavTri(goalPosition);
	if (!goalTri)
		return false;

	*endTri = goalTri;

	StartNewQuery();
	v2 goalPos = V2(goalPosition.x, goalPosition.z);

	// Priority queue for open set of nodes to explore.
	Heap<NavTri *> heap(512, tm, [](NavTri *a, NavTri *b) {
		return a->GetF() < b->GetF();
	});

	// Initialize starting triangle and add to open set
	NavTri *curTri = &GetNavTri(startTri.GetID());
	curTri->SetQueryID(GetQueryID());
	curTri->SetG(0.0f);
	curTri->SetF(Length(curTri->GetCenter() - goalPos));
	curTri->SetParent(curTri);

	heap.Push(curTri);

	// A* search loop
	while (heap.Count() > 0 && heap.Count() < heap.MaxCount()) {
		curTri = heap.Pop();
		if (curTri == goalTri) {
			return true; // Path to goal found
		}

		// Explore neighbors of the current triangle
		for (u32 i = 0; i < 3; i++) {
			const u16 neighborID = curTri->GetNeighborID(i);
			if (neighborID == 65535) // No neighbor
				continue;

			NavTri *nextTri = &m_navTris[neighborID];
			const f32 newG = curTri->GetG() + Length(curTri->GetCenter() - nextTri->GetCenter());

			// Skip if already visited and has a better path
			if (nextTri->GetQueryID() == m_queryID && newG >= nextTri->GetG())
				continue;

			// Update information for the next triangle and add to open set
			nextTri->SetQueryID(GetQueryID());
			nextTri->SetParent(curTri);
			nextTri->SetG(newG);
			nextTri->SetF(newG + Length(nextTri->GetCenter() - goalPos));
			heap.Push(nextTri);
		}
	}

	return false;
}


// The core of the algorithm change, a novel observation to use a two points
// forming a line segment instead of a single point to test if points have a clear
// line of movement to eachother and therefore removed, simplifying the path.

// Checks if the new line segment from newLeft to newRight crosses outside the
// sides of the channel formed from leftApex to left and rightApex to right.
bool NavAgent::ValidChannel(const v2 &leftApex, const v2 &rightApex, const v2 &left, const v2 &right, const v2 &newLeft, const v2 &newRight) const {
	return TriArea2(leftApex, left, newLeft) < EPSILON &&
	       TriArea2(rightApex, right, newRight) > -EPSILON &&
	       TriArea2(leftApex, left, newRight) < EPSILON &&
	       TriArea2(rightApex, right, newLeft) > -EPSILON &&
	       TriArea2(leftApex, rightApex, newLeft) > -EPSILON &&
	       TriArea2(leftApex, rightApex, newRight) > -EPSILON;
}

// Find a path from the agent's current position to the specified goal position.
bool NavAgent::FindPath(const v3 &goalPosition) {
	m_pathLength = 0;

	// Find the end triangle of the path using A* algorithm.
	NavTri *endTri = nullptr;
	if (!m_navMesh->AStar(*m_navTri, goalPosition, &endTri))
		return false;

	// Convert the goal position to a 2D position.
	const v2 goalPos = V2(goalPosition.x, goalPosition.z);

	// Set the final portal to the goal position and with an edge length of zero.
	m_pathPortals[0] = {goalPos, V2(0.0f, 0.0f)};
	i32 pathLength = 1;

	// Initialize apex points and left/right points that form the visable channel.
	v2 leftApex = goalPos;
	v2 rightApex = goalPos;
	v2 left;
	v2 right;
	NavTri *curTri = endTri->GetParent();
	Assert(curTri->GetSharedVerts(endTri->GetID(), &left, &right));

	// Build the path from end to start.
	while (curTri->GetParent() != curTri) {
		// Make sure the path fill fit in the array.
		if (pathLength + 1 >= MAX_PATH_LENGTH)
			return 0;

		// Get the next triangle in the list to get from the goal to the start.
		NavTri *parent = curTri->GetParent();
		v2 newLeft, newRight;

		Assert(parent->GetSharedVerts(curTri->GetID(), &newLeft, &newRight));

		// If the new edge isn't contained in the current channel, add the last
		// edge to the list of portals and set it as the new apex.
		if (!ValidChannel(leftApex, rightApex, left, right, newLeft, newRight)) {
			const v2 e = right - left;
			m_pathPortals[pathLength++] = {left, e};

			leftApex = left;
			rightApex = right;
		}

		left = newLeft;
		right = newRight;
		curTri = parent;
	}

	// Check i fthe starting position is contained without the current channel,
	// if not, add the last edge to the list of portals.
	const v2 startPos = V2(m_position.x, m_position.z);
	if (!ValidChannel(leftApex, rightApex, left, right, startPos, startPos)) {
		const v2 e = right - left;
		m_pathPortals[pathLength++] = {left, e};
	}

	// Add the starting position as the last portal point.
	m_pathPortals[pathLength] = {startPos, V2(0.0f, 0.0f)};
	m_pathLength = pathLength;

	return true;
}