Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Factorio TrainPathFinder
#include <Entity/Rail.hpp>
#include <Entity/RailSignalBase.hpp>
#include <Rail/RailBlock.hpp>
#include <Rail/RailPath.hpp>
#include <Rail/TrainPathFinder.hpp>
#include <Rail/RailSegment.hpp>
#include <Rail/RailUtil.hpp>
#include <Rail/Train.hpp>
#include <Log.hpp>
TrainPathFinderConstants TrainPathFinder::constants;
TrainPathFinderNode::TrainPathFinderNode(RailSegment* segment,
RailDirection enterDirection,
const TrainPathFinderNode* cameFrom,
double costFromStart,
uint32_t blockDistanceFromStart,
bool setSeen,
InChainSignalSection inChainSignalSection)
: cameFrom(cameFrom)
, segment(segment)
, costFromStart(costFromStart)
, blockDistanceFromStart(blockDistanceFromStart)
, enterDirection(enterDirection)
, inChainSignalSection(inChainSignalSection)
{
if (setSeen && this->segment)
this->segment->setSeen(this->enterDirection, inChainSignalSection, this);
}
TrainPathFinderNode::~TrainPathFinderNode()
{
if (this->segment)
{
this->segment->seenForwards = nullptr;
this->segment->seenBackwards = nullptr;
this->segment->seenForwardsInChainSequence = nullptr;
this->segment->seenBackwardsInChainSequence = nullptr;
}
}
void TrainPathFinder::findPath(Request& request)
{
uint32_t endsFound = 0;
if (request.type == TrainPathRequestType::GetAccessibleStateOfAllEnds)
request.endAccessabilities.reset(new std::vector<char>(request.toEnds.size()));
Data pfData;
{
RailEnd* bestResultEnd = nullptr;
double bestResultDistance = 0;
for (RailEnd& toEnd : request.toEnds)
{
// straight path to the front
if (request.allowPathWithinSegmentFront &&
request.fromEndFront.rail &&
toEnd.rail->segment == request.fromEndFront.rail->segment)
{
double currentDistance = 0;
if (TrainPathFinder::findPathWithinSegment(request.fromEndFront, toEnd, currentDistance))
{
if (bestResultEnd == nullptr || currentDistance < bestResultDistance)
{
bestResultEnd = &toEnd;
bestResultDistance = currentDistance;
}
}
}
// straight path to the back
if (request.allowPathWithinSegmentBack &&
request.fromEndBack.rail &&
toEnd.rail->segment == request.fromEndBack.rail->segment)
{
double currentDistance = 0;
if (TrainPathFinder::findPathWithinSegment(request.fromEndBack, toEnd, currentDistance))
{
if (bestResultEnd == nullptr || currentDistance < bestResultDistance)
{
bestResultEnd = &toEnd;
bestResultDistance = currentDistance;
}
}
}
}
if (bestResultEnd && TrainPathFinder::constructPath(request, *bestResultEnd, nullptr, endsFound))
return;
}
// init starting nodes
TrainPathFinderNode* startNodeBack = nullptr;
TrainPathFinderNode* startNodeFront = nullptr;
if (request.fromEndFront.rail)
{
bool isCycle;
RailDirection leaveSegmentDirection = TrainPathFinder::calcLeaveSegmentDirection(request.fromEndFront.rail,
request.fromEndFront.direction,
isCycle).opposite();
if (!isCycle || request.allowPathWithinSegmentFront == false)
{
RailSegment* segment = request.fromEndFront.rail->segment;
// not setting seen in the first segment, so we can pathfind to the same segment we started in, lets see how many things will it break
startNodeFront = pfData.createNode(segment, leaveSegmentDirection, nullptr, 0, 0, false /* setSeen */, request.inChainSignalSection);
pfData.sortInNode(startNodeFront);
}
}
if (request.fromEndBack.rail)
{
bool isCycle;
RailDirection leaveSegmentDirection = TrainPathFinder::calcLeaveSegmentDirection(request.fromEndBack.rail,
request.fromEndBack.direction,
isCycle).opposite();
if (!isCycle || request.allowPathWithinSegmentBack == false)
{
RailSegment* segment = request.fromEndBack.rail->segment;
// not setting seen in the first segment, so we can pathfind to the same segment we started in, lets see how many things will it break
startNodeBack = pfData.createNode(segment, leaveSegmentDirection, nullptr, 0, 0, false /* setSeen */, request.inChainSignalSection);
pfData.sortInNode(startNodeBack);
}
}
// main path finding logic
while (!pfData.open.empty())
{
TrainPathFinderNode* currentNode = &pfData.open.front();
currentNode->unlink();
// found path check
for (const RailEnd& toEnd : request.toEnds)
if (toEnd.rail->segment == currentNode->segment &&
currentNode != startNodeFront &&
currentNode != startNodeBack &&
(request.allowPathWithinSegmentFront || currentNode->cameFrom != nullptr) &&
TrainPathFinder::findPathWithinSegment(currentNode->segment, currentNode->enterDirection, toEnd) &&
TrainPathFinder::constructPath(request, toEnd, currentNode, endsFound))
return;
// expand neighbors
RailSegment* currentSegment = currentNode->segment;
for (const RailDirection& currentLeaveDirection : RailUtil::railDirectionValues)
{
InChainSignalSection inChainSignalSection = currentNode->inChainSignalSection;
if (currentLeaveDirection != currentNode->enterDirection.opposite() ||
!currentSegment->canEverLeaveSegment(currentLeaveDirection, &inChainSignalSection, request.train))
continue;
for (RailSegment* neighbor : currentSegment->getSegments(currentLeaveDirection))
{
RailDirection enterNeighborDirection = neighbor->getDirectionToSegment(currentSegment, currentLeaveDirection);
// one way segment
if (!neighbor->canEverEnterSegment(enterNeighborDirection, &inChainSignalSection, request.train))
continue;
TrainPathFinderNode* seenNode = nullptr;
if (enterNeighborDirection == RailDirection::Enum::Front)
if (inChainSignalSection == InChainSignalSection::False)
{
if (neighbor->seenForwards)
if (neighbor->seenForwards->is_linked())
seenNode = neighbor->seenForwards;
else
continue;
}
else
{
if (neighbor->seenForwardsInChainSequence)
if (neighbor->seenForwardsInChainSequence->is_linked())
seenNode = neighbor->seenForwardsInChainSequence;
else
continue;
}
else
if (inChainSignalSection == InChainSignalSection::False)
{
if (neighbor->seenBackwards)
if (neighbor->seenBackwards->is_linked())
seenNode = neighbor->seenBackwards;
else
continue;
}
else
{
if (neighbor->seenBackwardsInChainSequence)
if (neighbor->seenBackwardsInChainSequence->is_linked())
seenNode = neighbor->seenBackwardsInChainSequence;
else
continue;
}
double costFromStart = currentNode->costFromStart + currentSegment->getLength();
RailSignalBase* neighborInSignal = neighbor->getSignalIn(enterNeighborDirection);
RailSignalBase* outSignal = currentSegment->getSignalOut(currentLeaveDirection);
if ((outSignal && outSignal->getReservedByCircuitNetwork())
|| (neighborInSignal && neighborInSignal->getReservedByCircuitNetwork()))
costFromStart += TrainPathFinder::constants.signalReservedByCircuitNetworkPenalty;
if (!neighbor->getBlock()->isFree(request.train))
costFromStart += (2 * neighbor->getLength()) / (currentNode->blockDistanceFromStart + 1);
if (currentSegment->frontStop)
costFromStart += TrainPathFinder::constants.trainStopPenalty;
if (currentSegment->backStop)
costFromStart += TrainPathFinder::constants.trainStopPenalty;
// Add some cost to the path depending on what is the train doing in the block
if (neighbor->getBlock() != currentSegment->getBlock())
{
Train* neighbourTrain = neighbor->getBlock()->getTrain();
if (neighbourTrain && neighbourTrain != request.train)
switch (neighbourTrain->getState())
{
case TrainState::WAIT_STATION:
costFromStart += TrainPathFinder::constants.trainInStationPenalty;
if (neighbourTrain->getTicksInStation() == uint32_t(-1))
{
costFromStart += TrainPathFinder::constants.trainInStationWithNoOtherValidStopsInSchedule;
break;
}
break;
case TrainState::ARRIVE_STATION:
costFromStart += TrainPathFinder::constants.trainArrivingToStationPenalty;
break;
case TrainState::MANUAL_CONTROL:
if (neighbourTrain->getSpeed() == 0)
if (neighbourTrain->hasPassenger())
costFromStart += TrainPathFinder::constants.stoppedManuallyControlledTrainPenalty;
else
// The train is in manual mode with no passenger - it's not going to move any time soon.
costFromStart += TrainPathFinder::constants.stoppedManuallyControlledTrainWithoutPassengerPenalty;
break;
case TrainState::ARRIVE_SIGNAL:
costFromStart += TrainPathFinder::constants.trainArrivingToSignalPenalty;
break;
case TrainState::WAIT_SIGNAL:
costFromStart += neighbourTrain->getTicksWaitingAtSignal() * TrainPathFinder::constants.trainWaitingAtSignalTickMultiplierPenalty +
TrainPathFinder::constants.trainWaitingAtSignalPenalty;
break;
case TrainState::NO_PATH:
costFromStart += TrainPathFinder::constants.trainWithNoPathPenalty;
break;
default: break;
}
}
if (seenNode)
{
assert(seenNode->is_linked());
if (seenNode->costFromStart > costFromStart)
{
seenNode->costFromStart = costFromStart;
seenNode->cameFrom = currentNode;
seenNode->unlink();
pfData.sortInNode(seenNode);
}
}
else
{
uint32_t newBlockDistanceFromStart = currentNode->blockDistanceFromStart +
(currentNode->segment->getBlock() == neighbor->getBlock() ? 0 : 1);
TrainPathFinderNode* newNode = pfData.createNode(neighbor,
enterNeighborDirection,
currentNode,
costFromStart,
newBlockDistanceFromStart,
true /* setSeen */,
inChainSignalSection);
pfData.sortInNode(newNode);
}
}
}
}
// no path
}
bool TrainPathFinder::constructPath(Request& request, const RailEnd& toEnd, const TrainPathFinderNode* lastNode, uint32_t& endsFound)
{
if (request.type == TrainPathRequestType::GetAccessibleStateOfAllEnds)
if (!(*request.endAccessabilities)[&toEnd - &request.toEnds[0]])
{
(*request.endAccessabilities)[&toEnd - &request.toEnds[0]] = true;
++endsFound;
return endsFound == request.endAccessabilities->size();
}
else
return false;
if (request.type == TrainPathRequestType::GetAccessibleStateOfAnyEnd)
{
request.someEndAccessible = true;
return true;
}
RailPathCollector pathCollector;
// when we encounter first rail from the opposite direction we are finished
RailEnd stopFront(request.fromEndFront.rail, request.fromEndFront.direction.opposite());
RailEnd stopBack(request.fromEndBack.rail, request.fromEndBack.direction.opposite());
bool stoppedAtFront;
// we go through the whole path backwards and in the end we will reverse it
// the reason for going backwards is that the nodes can be tracked from the end to the beginning only
// add rails from the last node
if (TrainPathFinder::addRailsToPath(toEnd.rail,
toEnd.direction.opposite(),
stopFront,
stopBack,
pathCollector,
stoppedAtFront,
lastNode == nullptr || lastNode->cameFrom == nullptr))
{
if (request.type == TrainPathRequestType::SimpleRailPath)
request.simpleRailPathResult.reset(pathCollector.convertToSimplePath(stoppedAtFront));
else
request.railPathResult.reset(pathCollector.convertToPath(stoppedAtFront));
return true;
}
// add rails from the intermediate segments
assert(lastNode);
const TrainPathFinderNode* node = lastNode->cameFrom;
while (node != nullptr)
{
// start from the rail at the end of the segment where we left the segment
Rail* lastRail = node->segment->getEndRail(node->enterDirection.opposite());
RailDirection goBackDirection = RailSegment::getEndRailDirectionTo(lastRail, node->enterDirection.opposite()).opposite();
if (TrainPathFinder::addRailsToPath(lastRail,
goBackDirection,
stopFront,
stopBack,
pathCollector,
stoppedAtFront,
node->cameFrom == nullptr))
{
if (request.type == TrainPathRequestType::SimpleRailPath)
request.simpleRailPathResult.reset(pathCollector.convertToSimplePath(stoppedAtFront));
else
request.railPathResult.reset(pathCollector.convertToPath(stoppedAtFront));
return true;
}
node = node->cameFrom;
}
LOG_AND_ABORT("Failed to construt path.");
}
bool TrainPathFinder::addRailsToPath(Rail* rail,
RailDirection direction,
const RailEnd& stopEndFront,
const RailEnd& stopEndBack,
RailPathCollector& pathCollector,
bool& stoppedAtFront,
bool isLastSegment)
{
Rail::PlainIterator it = Rail::PlainIterator(rail, direction);
while (!it.ended() &&
it.rail->segment == rail->segment &&
(!isLastSegment || it.rail != stopEndFront.rail || it.direction != stopEndFront.direction) &&
(!isLastSegment || it.rail != stopEndBack.rail || it.direction != stopEndBack.direction))
{
pathCollector.append(it.rail);
++it;
}
if (!it.ended() && it.rail == stopEndFront.rail && it.direction == stopEndFront.direction)
{
pathCollector.append(it.rail);
stoppedAtFront = true;
return true;
}
if (!it.ended() && it.rail == stopEndBack.rail && it.direction == stopEndBack.direction)
{
pathCollector.append(it.rail);
stoppedAtFront = false;
return true;
}
return false;
}
RailDirection TrainPathFinder::calcLeaveSegmentDirection(Rail* rail,
RailDirection direction,
bool& isCycle)
{
Rail* last = rail;
Rail* firstEdgeRail = nullptr;
if (rail->segment->isEndRail(rail))
firstEdgeRail = rail;
Rail::PlainIterator it = ++Rail::PlainIterator(rail, direction);
while (!it.ended() &&
it.rail->segment == rail->segment)
{
last = it.rail;
++it;
// this covering a very special case, where we have a cycle with one extra junction
if (it.rail && it.rail->connection(it.direction.opposite()).getConnectionCount() != 1)
{
isCycle = false;
return last->segment->getSegmentEndDirection(last, direction);
}
if (firstEdgeRail == nullptr && rail->segment->isEndRail(last))
firstEdgeRail = last;
}
isCycle = it.isCycle();
assert(last->segment->isEndRail(last) || isCycle);
// when is a cycle the return value is not used anyway
if (isCycle)
return firstEdgeRail == rail->segment->getFrontRail() ? RailDirection::Front : RailDirection::Back;
return last->segment->getSegmentEndDirection(last, direction);
}
bool TrainPathFinder::findPathWithinSegment(const RailEnd& fromEnd, const RailEnd& toEnd, double& distance)
{
// simple case when the toRail is at the end
Rail::PlainIterator it = Rail::PlainIterator(fromEnd.rail, fromEnd.direction);
while (!it.ended() &&
it.rail->segment == fromEnd.rail->segment &&
(it.rail != toEnd.rail || it.direction != toEnd.direction))
{
distance += it.rail->length();
++it;
// wee need to avoid intersections even when it is the same segment, as the rails can be connected by a loop from the other side.
// the lack of intersection from the last rail to this one is already guarded by the Rail::PlainIterator::operator++,
// but there can be intersection that merges the two rails.
if (it.rail && it.rail->connection(it.direction.opposite()).getConnectionCount() != 1)
return false;
}
return (!it.ended() && it.rail == toEnd.rail && it.direction == toEnd.direction);
}
bool TrainPathFinder::findPathWithinSegment(RailSegment* segment, RailDirection enterDirection, const RailEnd& toEnd)
{
assert(segment == toEnd.rail->segment);
Rail* fromRail = segment->getEndRail(enterDirection);
RailDirection fromDirection = RailSegment::getEndRailDirectionTo(fromRail, enterDirection).opposite();
double distance = 0;
return TrainPathFinder::findPathWithinSegment(RailEnd(fromRail, fromDirection), toEnd, distance);
}
void TrainPathFinder::Data::sortInNode(TrainPathFinderNode* node)
{
// Special case 1
if (this->open.empty())
{
this->open.push_back(*node);
return;
}
auto comp = [](const TrainPathFinderNode& a, const TrainPathFinderNode& b)
{
if (a.costFromStart != b.costFromStart)
return a.costFromStart < b.costFromStart;
assert(a.segment && b.segment);
if (a.segment == b.segment)
return a.enterDirection == RailDirection::Front && b.enterDirection == RailDirection::Back;
return a.segment->getID() < b.segment->getID();
};
// Special case 2: < then front
if (comp(*node, this->open.front()))
{
this->open.push_front(*node);
return;
}
// Special case 3: > then back
if (!comp(*node, this->open.back()))
{
this->open.push_back(*node);
return;
}
// Find position and insert
auto it = this->open.begin();
++it; // Already checked not to to be < begin
auto end = this->open.end();
for (; it != end; ++it)
if (comp(*node, *it))
{
this->open.insert(it, *node);
return;
}
this->open.push_back(*node);
}
TrainPathFinder::Request::Request(TrainPathRequestType type,
const RailEnd& fromFront,
const RailEnd& fromBack,
std::vector<RailEnd>&& toEnds,
Train* train,
InChainSignalSection inChainSignalSection)
: fromEndFront(fromFront)
, fromEndBack(fromBack)
, toEnds(std::move(toEnds))
, train(train)
, type(type)
, inChainSignalSection(inChainSignalSection)
{}
TrainPathFinder::Request::Request(TrainPathRequestType type, Train* from, std::vector<RailEnd>&& toEnds, InChainSignalSection inChainSignalSection)
: fromEndFront(nullptr, RailDirection::Front) // these are assigned in the end
, fromEndBack(nullptr, RailDirection::Front)
, toEnds(std::move(toEnds))
, train(from)
, type(type)
, inChainSignalSection(inChainSignalSection)
{
Train::TrainEnd frontEnd = this->train->getFrontEnd();
Train::TrainEnd backEnd = this->train->getBackEnd();
// the path is searched only in the direction of movement (or in both when stopped)
Rail* frontRail = this->train->getSpeed() >= 0 && !this->train->getFrontMovers().empty() ? frontEnd.getRail() : nullptr;
Rail* backRail = this->train->getSpeed() <= 0 && !this->train->getBackMovers().empty() ? backEnd.getRail() : nullptr;
// for pathfinding the from direction is the outgoing direction from the rail
this->fromEndFront = RailEnd(frontRail, frontEnd.getFromDirection().opposite());
this->fromEndBack = RailEnd(backRail, backEnd.getFromDirection());
}
TrainPathFinder::Request::~Request() = default;
#pragma once
#include <Rail/InChainSignalSection.hpp>
#include <Rail/RailEnd.hpp>
#include <Rail/TrainPathFinderConstants.hpp>
#include <Rail/TrainPathRequestType.hpp>
#include <Util/TypeAwareMemoryPool.hpp>
#include <Util/Container/IntrusiveList.hpp>
class RailPathCollector;
class RailPath;
class SimpleRailPath;
class RailSegment;
class Train;
class TrainPathFinderNode : public IntrusiveListHook<TrainPathFinderNode>
{
public:
TrainPathFinderNode() = default;
TrainPathFinderNode(RailSegment* segment,
RailDirection enterDirection,
const TrainPathFinderNode* cameFrom,
double costFromStart,
uint32_t blockDistanceFromStart,
bool setSeen,
InChainSignalSection inChainSignalSection);
~TrainPathFinderNode();
const TrainPathFinderNode* cameFrom = nullptr;
/** This is the end at which we enter the segment. */
RailSegment* segment = nullptr;
double costFromStart = 0;
// how many blocks are there between this node and start node
uint32_t blockDistanceFromStart = 0;
RailDirection enterDirection;
InChainSignalSection inChainSignalSection = InChainSignalSection::False;
};
class TrainPathFinder
{
public:
class Request
{
public:
Request(TrainPathRequestType type,
const RailEnd& fromFront,
const RailEnd& fromBack,
std::vector<RailEnd>&& toEnds,
Train* train,
InChainSignalSection inChainSignalSection);
Request(TrainPathRequestType type, Train* from, std::vector<RailEnd>&& toEnds, InChainSignalSection inChainSignalSection);
~Request();
/** The are two possible starts because we can go from front or from the back of the train.
For the start direction is the outgoing direction. */
RailEnd fromEndFront;
RailEnd fromEndBack;
std::vector<RailEnd> toEnds; // For the goal direction is the outgoing direction as well.
Train* train;
TrainPathRequestType type;
// When true, the resulting path mustn't encounter closed signal until it reaches the chain signal sequence exit
InChainSignalSection inChainSignalSection = InChainSignalSection::False;
bool allowPathWithinSegmentFront = true; // the target stop is already on my rail, but the train is too far to be aligned,
// so I need to find a path around to re-approach the station
bool allowPathWithinSegmentBack = true;
std::unique_ptr<RailPath> railPathResult; // related to TrainPathRequestType::Normal
std::unique_ptr<SimpleRailPath> simpleRailPathResult; // related to TrainPathRequestType::SimpleRailPath
std::unique_ptr<std::vector<char>> endAccessabilities; //related to TrainPathRequestType::GetAccessibleStateOfAllEnds
bool someEndAccessible = false; // related to TrainPathRequestType::GetAccessibleStateOfAnyEnd
};
static void findPath(Request& request); // stores the result(s) into the request class depending on what is requested
private:
using NodeQueue = IntrusiveList<TrainPathFinderNode>;
class Data
{
public:
Data() = default;
Data(const Data&) = delete;
Data(Data&&) = delete;
Data& operator=(const Data&) = delete;
Data& operator=(Data&&) = delete;
~Data() = default;
template<typename ... Ts>
TrainPathFinderNode* createNode(Ts&& ... values)
{
return new (this->nodeProvider.allocate())TrainPathFinderNode(std::forward<Ts>(values) ...);
}
void sortInNode(TrainPathFinderNode* node);
NodeQueue open;
private:
TypeAwareMemoryPool<TrainPathFinderNode, 24, BlockExpandMode::DuringAllocate> nodeProvider;
};
/** Searches for a path within the segment. */
static bool findPathWithinSegment(const RailEnd& fromEnd, const RailEnd& toEnd, double& distance);
/** Wrapper around previous function version. Transforms segmentStartEnd to rail and it inner direction. */
static bool findPathWithinSegment(RailSegment* segment, RailDirection enterDirection, const RailEnd& toEnd);
static bool constructPath(Request& request, const RailEnd& toEnd, const TrainPathFinderNode* lastNode, uint32_t& endsFound);
/** Helper function for path construction.
* @return true if hit the stopRail from the stopDirection */
static bool addRailsToPath(Rail* rail,
RailDirection direction,
const RailEnd& stopFront,
const RailEnd& stopBack,
RailPathCollector& pathCollector,
/** Whether we hit the stopRailFront or StopRailBack.*/
bool& stoppedAtFront,
bool isLastSegment);
/** If we start on rail and follow the direction at what segment direction will we leave the rail segment. */
static RailDirection calcLeaveSegmentDirection(Rail* rail, RailDirection direction, bool& isCycle);
public:
static TrainPathFinderConstants constants; // these are copied from UtilityConstants on load for faster access
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.