Skip to content

Commit 367fb69

Browse files
GrufoonyGrufoony
committed
Simplify m_nextStreetId function
1 parent cc93be5 commit 367fb69

File tree

1 file changed

+77
-112
lines changed

1 file changed

+77
-112
lines changed

‎src/dsf/mobility/RoadDynamics.hpp‎

Lines changed: 77 additions & 112 deletions
Original file line numberDiff line numberDiff line change
@@ -494,144 +494,109 @@ namespace dsf::mobility {
494494
requires(is_numeric_v<delay_t>)
495495
std::optional<Id> RoadDynamics<delay_t>::m_nextStreetId(
496496
const std::unique_ptr<Agent>& pAgent, const std::unique_ptr<RoadJunction>& pNode) {
497-
// Get outgoing edges directly - avoid storing targets separately
497+
spdlog::trace("Computing m_nextStreetId for {}", *pAgent);
498498
auto const& outgoingEdges = pNode->outgoingEdges();
499+
500+
// Handle single outgoing edge case
499501
if (outgoingEdges.size() == 1) {
502+
auto const& pStreetOut{this->graph().edge(outgoingEdges[0])};
503+
if (!pNode->isRoundabout() && pStreetOut->target() == pStreetOut->source()) {
504+
return std::nullopt;
505+
}
500506
return outgoingEdges[0];
501507
}
502-
if (pAgent->isRandom()) { // Try to use street transition probabilities
503-
spdlog::trace("Computing m_nextStreetId for {}", *pAgent);
504-
if (pAgent->streetId().has_value()) {
505-
auto const& pStreetCurrent{this->graph().edge(pAgent->streetId().value())};
506-
auto const speedCurrent{pStreetCurrent->maxSpeed() /**
507-
this->m_speedFactor(pStreetCurrent->density())*/};
508-
double cumulativeProbability = 0.0;
509-
std::unordered_map<Id, double> transitionProbabilities;
510-
for (const auto outEdgeId : outgoingEdges) {
511-
auto const& pStreetOut{this->graph().edge(outEdgeId)};
512-
auto const speed{pStreetOut->maxSpeed() /**
513-
this->m_speedFactor(pStreetOut->density())*/};
514-
double probability = speed * speedCurrent;
515-
if (pStreetOut->target() == pStreetCurrent->source()) {
516-
if (pNode->isRoundabout()) {
517-
probability *= U_TURN_PENALTY_FACTOR; // Discourage U-TURNS
518-
} else {
519-
continue; // No U-TURNS
520-
}
521-
}
522-
transitionProbabilities[pStreetOut->id()] = probability;
523-
cumulativeProbability += probability;
524-
}
525-
std::uniform_real_distribution<double> uniformDist{0., cumulativeProbability};
526-
auto const randValue = uniformDist(this->m_generator);
527-
cumulativeProbability = 0.0;
528-
for (const auto& [targetStreetId, probability] : transitionProbabilities) {
529-
cumulativeProbability += probability;
530-
if (randValue <= cumulativeProbability) {
531-
return targetStreetId;
532-
}
533-
}
534-
// auto const& transitionProbabilities = pStreetCurrent->transitionProbabilities();
535-
// if (!transitionProbabilities.empty()) {
536-
// std::uniform_real_distribution<double> uniformDist{0., 1.};
537-
// auto const randValue = uniformDist(this->m_generator);
538-
// double cumulativeProbability = 0.0;
539-
// for (const auto& [targetStreetId, probability] : transitionProbabilities) {
540-
// cumulativeProbability += probability;
541-
// if (randValue <= cumulativeProbability) {
542-
// return targetStreetId;
543-
// }
544-
// }
545-
// }
546-
}
547-
}
548-
std::vector<Id> possibleEdgeIds;
549-
possibleEdgeIds.reserve(outgoingEdges.size()); // Pre-allocate to avoid reallocations
550-
551-
// Build forbidden target nodes set efficiently
552-
std::unordered_set<Id> forbiddenTargetNodes;
508+
509+
// Get current street information
510+
std::optional<Id> previousNodeId = std::nullopt;
511+
std::set<Id> forbiddenTurns;
512+
double speedCurrent = 1.0;
553513
if (pAgent->streetId().has_value()) {
554-
auto const& pStreet{this->graph().edge(pAgent->streetId().value())};
555-
const auto& forbiddenTurns = pStreet->forbiddenTurns();
556-
forbiddenTargetNodes.insert(forbiddenTurns.begin(), forbiddenTurns.end());
557-
558-
// Avoid U-TURNS, if possible
559-
if (!(pNode->isRoundabout()) &&
560-
(outgoingEdges.size() > forbiddenTurns.size() + 1)) {
561-
auto const& pOppositeStreet{
562-
this->graph().street(pStreet->target(), pStreet->source())};
563-
if (pOppositeStreet) {
564-
forbiddenTargetNodes.insert(pOppositeStreet->get()->id());
565-
}
566-
}
514+
auto const& pStreetCurrent{this->graph().edge(pAgent->streetId().value())};
515+
previousNodeId = pStreetCurrent->source();
516+
forbiddenTurns = pStreetCurrent->forbiddenTurns();
517+
speedCurrent = pStreetCurrent->maxSpeed();
567518
}
568519
569-
// Log forbidden turns if any
570-
if (!forbiddenTargetNodes.empty()) {
571-
spdlog::debug("Excluding {} forbidden turns", forbiddenTargetNodes.size());
520+
// Get path targets for non-random agents
521+
std::vector<Id> pathTargets;
522+
if (!pAgent->isRandom()) {
523+
auto const it{m_itineraries.find(pAgent->itineraryId())};
524+
if (it == m_itineraries.cend() || pNode->id() == it->second->destination()) {
525+
return std::nullopt;
526+
}
527+
try {
528+
pathTargets = it->second->path().at(pNode->id());
529+
} catch (const std::out_of_range&) {
530+
throw std::runtime_error(std::format("No path found for itinerary {} at node {}",
531+
pAgent->itineraryId(),
532+
pNode->id()));
533+
}
572534
}
573535
574-
// For non-random agents, get allowed targets from itinerary
575-
std::unordered_set<Id> allowedTargets;
576-
bool hasItineraryConstraints = false;
536+
// Calculate transition probabilities for all valid outgoing edges
537+
std::unordered_map<Id, double> transitionProbabilities;
538+
double cumulativeProbability = 0.0;
577539
578-
if (!this->itineraries().empty()) {
579-
std::uniform_real_distribution<double> uniformDist{0., 1.};
580-
if (!(m_errorProbability.has_value() &&
581-
uniformDist(this->m_generator) < m_errorProbability)) {
582-
const auto& it = this->itineraries().at(pAgent->itineraryId());
583-
if (it->destination() != pNode->id()) {
584-
try {
585-
const auto pathTargets = it->path().at(pNode->id());
586-
allowedTargets.insert(pathTargets.begin(), pathTargets.end());
587-
hasItineraryConstraints = true;
540+
for (const auto outEdgeId : outgoingEdges) {
541+
if (forbiddenTurns.contains(outEdgeId)) {
542+
continue;
543+
}
588544
589-
// Remove forbidden nodes from allowed targets
590-
for (const auto& forbiddenNodeId : forbiddenTargetNodes) {
591-
allowedTargets.erase(forbiddenNodeId);
592-
}
593-
// Catch unordered_map::at exceptions
594-
} catch (const std::out_of_range&) {
595-
throw std::runtime_error(std::format(
596-
"No path from {} to destination {}", *pNode, it->destination()));
597-
}
545+
auto const& pStreetOut{this->graph().edge(outEdgeId)};
546+
547+
// Check if this is a valid path target for non-random agents
548+
bool bIsPathTarget = true;
549+
if (!pathTargets.empty()) {
550+
bIsPathTarget =
551+
std::find(pathTargets.cbegin(), pathTargets.cend(), pStreetOut->target()) !=
552+
pathTargets.cend();
553+
if (!this->m_errorProbability.has_value() && !bIsPathTarget) {
554+
continue;
598555
}
599556
}
600-
}
601557
602-
// Single pass through outgoing edges with efficient filtering
603-
for (const auto outEdgeId : outgoingEdges) {
604-
const Id targetNode = this->graph().edge(outEdgeId)->target();
558+
// Calculate base probability
559+
auto const speed{pStreetOut->maxSpeed()};
560+
double probability = speed * speedCurrent;
605561
606-
// Skip if target is forbidden
607-
if (forbiddenTargetNodes.count(targetNode)) {
608-
continue;
562+
// Apply error probability for non-random agents
563+
if (this->m_errorProbability.has_value() && pathTargets.empty()) {
564+
probability *=
565+
(bIsPathTarget
566+
? (1. - this->m_errorProbability.value())
567+
: this->m_errorProbability.value() /
568+
static_cast<double>(outgoingEdges.size() - pathTargets.size()));
609569
}
610570
611-
// For non-random agents with itinerary constraints
612-
if (hasItineraryConstraints) {
613-
if (allowedTargets.count(targetNode)) {
614-
possibleEdgeIds.push_back(outEdgeId);
571+
// Handle U-turns
572+
if (previousNodeId.has_value() && pStreetOut->target() == previousNodeId.value()) {
573+
if (pNode->isRoundabout()) {
574+
probability *= U_TURN_PENALTY_FACTOR;
575+
} else {
576+
continue; // No U-turns allowed
615577
}
616-
} else {
617-
// For random agents or when no itinerary constraints apply
618-
possibleEdgeIds.push_back(outEdgeId);
619578
}
579+
580+
transitionProbabilities[pStreetOut->id()] = probability;
581+
cumulativeProbability += probability;
620582
}
621583
622-
if (possibleEdgeIds.empty()) {
584+
// Select street based on weighted probabilities
585+
if (transitionProbabilities.empty()) {
623586
return std::nullopt;
624-
// throw std::runtime_error(
625-
// std::format("No possible moves for agent {} at node {}.", *pAgent, nodeId));
626587
}
627588
628-
if (possibleEdgeIds.size() == 1) {
629-
return possibleEdgeIds[0];
589+
std::uniform_real_distribution<double> uniformDist{0., cumulativeProbability};
590+
auto const randValue = uniformDist(this->m_generator);
591+
double accumulated = 0.0;
592+
for (const auto& [targetStreetId, probability] : transitionProbabilities) {
593+
accumulated += probability;
594+
if (randValue <= accumulated) {
595+
return targetStreetId;
596+
}
630597
}
631598
632-
std::uniform_int_distribution<Size> moveDist{
633-
0, static_cast<Size>(possibleEdgeIds.size() - 1)};
634-
return possibleEdgeIds[moveDist(this->m_generator)];
599+
return std::nullopt;
635600
}
636601
637602
template <typename delay_t>

0 commit comments

Comments
 (0)