Skip to content

Commit 7afeff6

Browse files
GrufoonyGrufoony
authored
Merge pull request #370 from physycom/reworkNextStreetId
Rework next streetid computation
2 parents 2fac69f + b8a9740 commit 7afeff6

File tree

8 files changed

+110
-157
lines changed

8 files changed

+110
-157
lines changed

‎src/dsf/dsf.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
static constexpr uint8_t DSF_VERSION_MAJOR = 4;
88
static constexpr uint8_t DSF_VERSION_MINOR = 4;
9-
static constexpr uint8_t DSF_VERSION_PATCH = 3;
9+
static constexpr uint8_t DSF_VERSION_PATCH = 4;
1010

1111
static auto const DSF_VERSION =
1212
std::format("{}.{}.{}", DSF_VERSION_MAJOR, DSF_VERSION_MINOR, DSF_VERSION_PATCH);

‎src/dsf/mobility/Road.cpp‎

Lines changed: 0 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -101,13 +101,4 @@ namespace dsf::mobility {
101101
}
102102
return Direction::LEFT;
103103
}
104-
105-
double Road::length() const { return m_length; }
106-
double Road::maxSpeed() const { return m_maxSpeed; }
107-
int Road::nLanes() const { return m_nLanes; }
108-
int Road::capacity() const { return m_capacity; }
109-
double Road::transportCapacity() const { return m_transportCapacity; }
110-
std::string Road::name() const { return m_name; }
111-
int Road::priority() const { return m_priority; }
112-
std::set<Id> const& Road::forbiddenTurns() const { return m_forbiddenTurns; }
113104
}; // namespace dsf::mobility

‎src/dsf/mobility/Road.hpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -71,30 +71,30 @@ namespace dsf::mobility {
7171

7272
/// @brief Get the length, in meters
7373
/// @return double The length, in meters
74-
double length() const;
74+
inline auto length() const noexcept { return m_length; }
7575
/// @brief Get the maximum speed, in meters per second
7676
/// @return double The maximum speed, in meters per second
77-
double maxSpeed() const;
77+
inline auto maxSpeed() const noexcept { return m_maxSpeed; }
7878
/// @brief Get the number of lanes
7979
/// @return int The number of lanes
80-
int nLanes() const;
80+
inline auto nLanes() const noexcept { return m_nLanes; }
8181
/// @brief Get the road's capacity, in number of agents
8282
/// @return int The road's capacity, in number of agents
83-
int capacity() const;
83+
inline auto capacity() const noexcept { return m_capacity; }
8484
/// @brief Get the road's transport capacity, in number of agents
8585
/// @return double The road's transport capacity, in number of agents
86-
double transportCapacity() const;
86+
inline auto transportCapacity() const noexcept { return m_transportCapacity; }
8787
/// @brief Get the name
8888
/// @return std::string The name
89-
std::string name() const;
89+
inline auto const& name() const noexcept { return m_name; }
9090
/// @brief Get the priority
9191
/// @return int The priority
92-
int priority() const;
92+
inline auto priority() const noexcept { return m_priority; }
9393
/// @brief Get the road's forbidden turns
9494
/// @return std::set<Id> The road's forbidden turns
9595
/// @details The forbidden turns are the road ids that are not allowed to be used by the agents
9696
/// when they are on the road.
97-
std::set<Id> const& forbiddenTurns() const;
97+
inline auto const& forbiddenTurns() const noexcept { return m_forbiddenTurns; }
9898
/// @brief Get the road's turn direction given the previous road angle
9999
/// @param previousStreetAngle The angle of the previous road
100100
/// @return Direction The turn direction

‎src/dsf/mobility/RoadDynamics.hpp‎

Lines changed: 93 additions & 130 deletions
Original file line numberDiff line numberDiff line change
@@ -80,13 +80,11 @@ namespace dsf::mobility {
8080
void m_updatePath(std::unique_ptr<Itinerary> const& pItinerary);
8181

8282
/// @brief Get the next street id
83-
/// @param agentId The id of the agent
84-
/// @param NodeId The id of the node
85-
/// @param streetId The id of the incoming street
83+
/// @param pAgent A std::unique_ptr to the agent
84+
/// @param pNode A std::unique_ptr to the current node
8685
/// @return Id The id of the randomly selected next street
87-
std::optional<Id> m_nextStreetId(std::unique_ptr<Agent> const& pAgent,
88-
Id NodeId,
89-
std::optional<Id> streetId = std::nullopt);
86+
std::optional<Id> m_nextStreetId(const std::unique_ptr<Agent>& pAgent,
87+
const std::unique_ptr<RoadJunction>& pNode);
9088
/// @brief Evolve a street
9189
/// @param pStreet A std::unique_ptr to the street
9290
/// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination
@@ -454,7 +452,7 @@ namespace dsf::mobility {
454452
m_travelDTs.push_back({pAgent->distance(),
455453
static_cast<double>(this->time_step() - pAgent->spawnTime())});
456454
--m_nAgents;
457-
return std::move(pAgent);
455+
return pAgent;
458456
}
459457
460458
template <typename delay_t>
@@ -495,146 +493,108 @@ namespace dsf::mobility {
495493
template <typename delay_t>
496494
requires(is_numeric_v<delay_t>)
497495
std::optional<Id> RoadDynamics<delay_t>::m_nextStreetId(
498-
std::unique_ptr<Agent> const& pAgent, Id nodeId, std::optional<Id> streetId) {
499-
// Get outgoing edges directly - avoid storing targets separately
500-
auto const& pNode{this->graph().node(nodeId)};
496+
const std::unique_ptr<Agent>& pAgent, const std::unique_ptr<RoadJunction>& pNode) {
497+
spdlog::trace("Computing m_nextStreetId for {}", *pAgent);
501498
auto const& outgoingEdges = pNode->outgoingEdges();
502-
if (outgoingEdges.size() == 1) {
503-
return outgoingEdges[0];
504-
}
505-
if (pAgent->isRandom()) { // Try to use street transition probabilities
506-
spdlog::trace("Computing m_nextStreetId for {}", *pAgent);
507-
if (streetId.has_value()) {
508-
auto const& pStreetCurrent{this->graph().edge(streetId.value())};
509-
auto const speedCurrent{pStreetCurrent->maxSpeed() /**
510-
this->m_speedFactor(pStreetCurrent->density())*/};
511-
double cumulativeProbability = 0.0;
512-
std::unordered_map<Id, double> transitionProbabilities;
513-
for (const auto outEdgeId : outgoingEdges) {
514-
auto const& pStreetOut{this->graph().edge(outEdgeId)};
515-
auto const speed{pStreetOut->maxSpeed() /**
516-
this->m_speedFactor(pStreetOut->density())*/};
517-
double probability = speed * speedCurrent;
518-
if (pStreetOut->target() == pStreetCurrent->source()) {
519-
if (pNode->isRoundabout()) {
520-
probability *= U_TURN_PENALTY_FACTOR; // Discourage U-TURNS
521-
} else {
522-
continue; // No U-TURNS
523-
}
524-
}
525-
transitionProbabilities[pStreetOut->id()] = probability;
526-
cumulativeProbability += probability;
527-
}
528-
std::uniform_real_distribution<double> uniformDist{0., cumulativeProbability};
529-
auto const randValue = uniformDist(this->m_generator);
530-
cumulativeProbability = 0.0;
531-
for (const auto& [targetStreetId, probability] : transitionProbabilities) {
532-
cumulativeProbability += probability;
533-
if (randValue <= cumulativeProbability) {
534-
return targetStreetId;
535-
}
536-
}
537-
// auto const& transitionProbabilities = pStreetCurrent->transitionProbabilities();
538-
// if (!transitionProbabilities.empty()) {
539-
// std::uniform_real_distribution<double> uniformDist{0., 1.};
540-
// auto const randValue = uniformDist(this->m_generator);
541-
// double cumulativeProbability = 0.0;
542-
// for (const auto& [targetStreetId, probability] : transitionProbabilities) {
543-
// cumulativeProbability += probability;
544-
// if (randValue <= cumulativeProbability) {
545-
// return targetStreetId;
546-
// }
547-
// }
548-
// }
549-
}
550-
}
551-
std::vector<Id> possibleEdgeIds;
552-
possibleEdgeIds.reserve(outgoingEdges.size()); // Pre-allocate to avoid reallocations
553-
554-
// Build forbidden target nodes set efficiently
555-
std::unordered_set<Id> forbiddenTargetNodes;
556-
if (streetId.has_value()) {
557-
auto const& pStreet{this->graph().edge(*streetId)};
558-
const auto& forbiddenTurns = pStreet->forbiddenTurns();
559-
forbiddenTargetNodes.insert(forbiddenTurns.begin(), forbiddenTurns.end());
560-
561-
// Avoid U-TURNS, if possible
562-
if (!(this->graph().node(nodeId)->isRoundabout()) &&
563-
(outgoingEdges.size() > forbiddenTurns.size() + 1)) {
564-
auto const& pOppositeStreet{
565-
this->graph().street(pStreet->target(), pStreet->source())};
566-
if (pOppositeStreet) {
567-
forbiddenTargetNodes.insert(pOppositeStreet->get()->id());
499+
500+
// Get current street information
501+
std::optional<Id> previousNodeId = std::nullopt;
502+
std::set<Id> forbiddenTurns;
503+
double speedCurrent = 1.0;
504+
if (pAgent->streetId().has_value()) {
505+
auto const& pStreetCurrent{this->graph().edge(pAgent->streetId().value())};
506+
previousNodeId = pStreetCurrent->source();
507+
forbiddenTurns = pStreetCurrent->forbiddenTurns();
508+
speedCurrent = pStreetCurrent->maxSpeed();
509+
}
510+
511+
// Get path targets for non-random agents
512+
std::vector<Id> pathTargets;
513+
if (!pAgent->isRandom()) {
514+
try {
515+
pathTargets = m_itineraries.at(pAgent->itineraryId())->path().at(pNode->id());
516+
} catch (const std::out_of_range&) {
517+
if (!m_itineraries.contains(pAgent->itineraryId())) {
518+
throw std::runtime_error(
519+
std::format("No itinerary found with id {}", pAgent->itineraryId()));
568520
}
521+
throw std::runtime_error(std::format(
522+
"No path found for itinerary {} at node {}. To solve this error, consider "
523+
"using ODs extracted from a fully-connected subnetwork of your whole road "
524+
"network or, alternatively, set an error probability.",
525+
pAgent->itineraryId(),
526+
pNode->id()));
569527
}
570528
}
571529
572-
// Log forbidden turns if any
573-
if (!forbiddenTargetNodes.empty()) {
574-
spdlog::debug("Excluding {} forbidden turns", forbiddenTargetNodes.size());
575-
}
530+
// Calculate transition probabilities for all valid outgoing edges
531+
std::unordered_map<Id, double> transitionProbabilities;
532+
double cumulativeProbability = 0.0;
576533
577-
// For non-random agents, get allowed targets from itinerary
578-
std::unordered_set<Id> allowedTargets;
579-
bool hasItineraryConstraints = false;
534+
for (const auto outEdgeId : outgoingEdges) {
535+
if (forbiddenTurns.contains(outEdgeId)) {
536+
continue;
537+
}
580538
581-
if (!this->itineraries().empty()) {
582-
std::uniform_real_distribution<double> uniformDist{0., 1.};
583-
if (!(m_errorProbability.has_value() &&
584-
uniformDist(this->m_generator) < m_errorProbability)) {
585-
const auto& it = this->itineraries().at(pAgent->itineraryId());
586-
if (it->destination() != nodeId) {
587-
try {
588-
const auto pathTargets = it->path().at(nodeId);
589-
allowedTargets.insert(pathTargets.begin(), pathTargets.end());
590-
hasItineraryConstraints = true;
539+
auto const& pStreetOut{this->graph().edge(outEdgeId)};
591540
592-
// Remove forbidden nodes from allowed targets
593-
for (const auto& forbiddenNodeId : forbiddenTargetNodes) {
594-
allowedTargets.erase(forbiddenNodeId);
595-
}
596-
// Catch unordered_map::at exceptions
597-
} catch (const std::out_of_range&) {
598-
throw std::runtime_error(std::format(
599-
"No path from node {} to destination {}", nodeId, it->destination()));
600-
}
541+
// Check if this is a valid path target for non-random agents
542+
bool bIsPathTarget = true;
543+
if (!pathTargets.empty()) {
544+
bIsPathTarget =
545+
std::find(pathTargets.cbegin(), pathTargets.cend(), pStreetOut->target()) !=
546+
pathTargets.cend();
547+
if (!this->m_errorProbability.has_value() && !bIsPathTarget) {
548+
continue;
601549
}
602550
}
603-
}
604551
605-
// Single pass through outgoing edges with efficient filtering
606-
for (const auto outEdgeId : outgoingEdges) {
607-
const Id targetNode = this->graph().edge(outEdgeId)->target();
552+
// Calculate base probability
553+
auto const speedNext{pStreetOut->maxSpeed()};
554+
double probability = speedCurrent * speedNext;
608555
609-
// Skip if target is forbidden
610-
if (forbiddenTargetNodes.count(targetNode)) {
611-
continue;
556+
// Apply error probability for non-random agents
557+
if (this->m_errorProbability.has_value() && !pathTargets.empty()) {
558+
probability *=
559+
(bIsPathTarget
560+
? (1. - this->m_errorProbability.value())
561+
: this->m_errorProbability.value() /
562+
static_cast<double>(outgoingEdges.size() - pathTargets.size()));
612563
}
613564
614-
// For non-random agents with itinerary constraints
615-
if (hasItineraryConstraints) {
616-
if (allowedTargets.count(targetNode)) {
617-
possibleEdgeIds.push_back(outEdgeId);
565+
// Handle U-turns
566+
if (previousNodeId.has_value() && pStreetOut->target() == previousNodeId.value()) {
567+
if (pNode->isRoundabout()) {
568+
probability *= U_TURN_PENALTY_FACTOR;
569+
} else {
570+
continue; // No U-turns allowed
618571
}
619-
} else {
620-
// For random agents or when no itinerary constraints apply
621-
possibleEdgeIds.push_back(outEdgeId);
622572
}
573+
574+
transitionProbabilities[pStreetOut->id()] = probability;
575+
cumulativeProbability += probability;
623576
}
624577
625-
if (possibleEdgeIds.empty()) {
578+
// Select street based on weighted probabilities
579+
if (transitionProbabilities.empty()) {
580+
spdlog::trace("No valid transitions found for {} at {}", *pAgent, *pNode);
626581
return std::nullopt;
627-
// throw std::runtime_error(
628-
// std::format("No possible moves for agent {} at node {}.", *pAgent, nodeId));
629582
}
630-
631-
if (possibleEdgeIds.size() == 1) {
632-
return possibleEdgeIds[0];
583+
if (transitionProbabilities.size() == 1) {
584+
return transitionProbabilities.cbegin()->first;
633585
}
634586
635-
std::uniform_int_distribution<Size> moveDist{
636-
0, static_cast<Size>(possibleEdgeIds.size() - 1)};
637-
return possibleEdgeIds[moveDist(this->m_generator)];
587+
std::uniform_real_distribution<double> uniformDist{0., cumulativeProbability};
588+
auto const randValue = uniformDist(this->m_generator);
589+
double accumulated = 0.0;
590+
for (const auto& [targetStreetId, probability] : transitionProbabilities) {
591+
accumulated += probability;
592+
if (randValue < accumulated) {
593+
return targetStreetId;
594+
}
595+
}
596+
// Return last one as fallback
597+
return std::prev(transitionProbabilities.cend())->first;
638598
}
639599
640600
template <typename delay_t>
@@ -667,7 +627,7 @@ namespace dsf::mobility {
667627
continue;
668628
}
669629
auto const nextStreetId =
670-
this->m_nextStreetId(pAgent, pStreet->target(), pStreet->id());
630+
this->m_nextStreetId(pAgent, this->graph().node(pStreet->target()));
671631
if (!nextStreetId.has_value()) {
672632
spdlog::debug(
673633
"No next street found for agent {} at node {}", *pAgent, pStreet->target());
@@ -767,7 +727,7 @@ namespace dsf::mobility {
767727
timeTolerance,
768728
timeDiff);
769729
// Kill the agent
770-
this->m_killAgent(std::move(pStreet->dequeue(queueIndex)));
730+
this->m_killAgent(pStreet->dequeue(queueIndex));
771731
continue;
772732
}
773733
}
@@ -926,7 +886,7 @@ namespace dsf::mobility {
926886
}
927887
}
928888
if (bArrived) {
929-
auto pAgent = this->m_killAgent(std::move(pStreet->dequeue(queueIndex)));
889+
auto pAgent = this->m_killAgent(pStreet->dequeue(queueIndex));
930890
if (reinsert_agents) {
931891
// reset Agent's values
932892
pAgent->reset(this->time_step());
@@ -1055,6 +1015,10 @@ namespace dsf::mobility {
10551015
template <typename delay_t>
10561016
requires(is_numeric_v<delay_t>)
10571017
void RoadDynamics<delay_t>::m_evolveAgents() {
1018+
if (m_agents.empty()) {
1019+
spdlog::trace("No agents to process.");
1020+
return;
1021+
}
10581022
std::uniform_int_distribution<Id> nodeDist{
10591023
0, static_cast<Id>(this->graph().nNodes() - 1)};
10601024
spdlog::debug("Processing {} agents", m_agents.size());
@@ -1073,8 +1037,7 @@ namespace dsf::mobility {
10731037
}
10741038
if (!pAgent->nextStreetId().has_value()) {
10751039
spdlog::debug("No next street id, generating a random one");
1076-
auto const nextStreetId{
1077-
this->m_nextStreetId(pAgent, pSourceNode->id(), pAgent->streetId())};
1040+
auto const nextStreetId{this->m_nextStreetId(pAgent, pSourceNode)};
10781041
if (!nextStreetId.has_value()) {
10791042
spdlog::debug(
10801043
"No next street found for agent {} at node {}", *pAgent, pSourceNode->id());

‎src/dsf/mobility/RoadJunction.cpp‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,4 @@ namespace dsf::mobility {
1919
double RoadJunction::transportCapacity() const { return m_transportCapacity; }
2020
double RoadJunction::density() const { return 0.; }
2121
bool RoadJunction::isFull() const { return true; }
22-
23-
bool RoadJunction::isIntersection() const noexcept { return false; }
24-
bool RoadJunction::isTrafficLight() const noexcept { return false; }
25-
bool RoadJunction::isRoundabout() const noexcept { return false; }
2622
} // namespace dsf::mobility

‎src/dsf/mobility/RoadJunction.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,9 @@ namespace dsf::mobility {
4242
virtual double density() const;
4343
virtual bool isFull() const;
4444

45-
virtual bool isIntersection() const noexcept;
46-
virtual bool isTrafficLight() const noexcept;
47-
virtual bool isRoundabout() const noexcept;
45+
virtual constexpr bool isIntersection() const noexcept { return false; }
46+
virtual constexpr bool isTrafficLight() const noexcept { return false; }
47+
virtual constexpr bool isRoundabout() const noexcept { return false; }
4848
};
4949
} // namespace dsf::mobility
5050

0 commit comments

Comments
 (0)