CCHMetricFamily.h

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2001-2026 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
// A family of per-vehicle-type CCH metrics over one shared CCHGraph -- the
// MetricProvider machinery behind CCHRouter, shared between the simulation
// and duarouter the same way CCHGraph shares the topology.
//
// Keying by the vehicle TYPE (rather than the class) makes everything the
// effort function reads from the vehicle exact per metric: the type's
// maximum speed, per-class edge speed restrictions, routing preferences,
// the bicycle speed table and one frozen weights.random-factor realization;
// only individual speed-factor draws within one type share a metric (the
// same approximation the CH family makes with its per-class prototype
// vehicles, but at finer granularity).
//
// Two modes:
//
// STATIC -- weights never change while a (type, weight period) pair is in
//   use. Metrics are customized lazily on the first query of a pair under a
//   mutex and cached (duarouter, marouter and the simulation's free-flow
//   routers). When the efforts do change between queries -- a runtime
//   permission flip, marouter's travel times after an assignment iteration
//   -- the host flags it (flagStale) and every cached metric is refilled
//   and re-customized on the next query.
//
// LIVE -- weights track an adaptive speed table (the rerouting device).
//   Metrics are double-buffered: worker threads acquire-load the published
//   front while the owner refills and customizes the back at its barrier
//   and flips, so queries never observe a metric mid-customization. The
//   customize is sparse (RoutingKit partial customization over the edges
//   whose effort actually moved, behind a configurable deadband) with
//   wholesale refills after permission flips. Staleness is bounded to one
//   barrier -- CH's weightPeriod semantics: once any metric exists,
//   customize(now) must run at every barrier where an effort moved (see
//   atBarrier), and an epoch pair guards the bound as an invariant.
//
// The family is intentionally ignorant of WHEN to customize -- only the
// owner knows when no query is in flight (the simulation's adaptation
// barrier, duarouter's mutex) -- and of HOW reference vehicles are built
// (injected factory; V construction is host-specific).
/****************************************************************************/
#pragma once
#include <config.h>
 
#include <atomic>
#include <cmath>
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <utility>
#include <vector>
#include <utils/common/SUMOTime.h>
#include <utils/common/SUMOVehicleClass.h>
#include <utils/router/CCHGraph.h>
 
 
// ===========================================================================
// class definitions
// ===========================================================================
template<class E, class V, class K>
class CCHMetricFamily {
public:
    typedef CCHGraph<E, V> Graph;
    typedef typename Graph::EffortOperation EffortOperation;
    typedef const RoutingKit::CustomizableContractionHierarchyMetric* MetricPtr;
    typedef V* (*RefVehicleFactory)(const K*, int);
    typedef void (*WeightPatch)(const Graph*, const V*, std::vector<unsigned>&);
 
    CCHMetricFamily(const Graph* graph, EffortOperation fillEffort,
                    SUMOTime begin, SUMOTime weightPeriod,
                    RefVehicleFactory factory, WeightPatch patch) :
        myGraph(graph), myFillEffort(fillEffort), myGateEffort(nullptr),
        myFactory(factory), myPatch(patch), myLive(false),
        myBegin(begin), myWeightPeriod(weightPeriod),
        myUpdateFactor(1.), myUpdateConstant(0.) {
    }
 
    CCHMetricFamily(const Graph* graph, EffortOperation fillEffort,
                    EffortOperation gateEffort, double updateFactor,
                    double updateConstant, RefVehicleFactory factory,
                    int ensembleK = 1) :
        myGraph(graph), myFillEffort(fillEffort), myGateEffort(gateEffort),
        myFactory(factory), myPatch(nullptr), myLive(true),
        myBegin(0), myWeightPeriod(SUMOTime_MAX),
        myUpdateFactor(updateFactor), myUpdateConstant(updateConstant),
        myEnsembleK(ensembleK > 1 ? ensembleK : 1) {
        const unsigned space = myGraph->edgeIdSpace();
        for (int i = 0; i < 2; i++) {
            // NaN = "never applied": the first pending occurrence always
            // passes the deadband and primes the entry
            myAppliedEffort[i].assign(space, std::numeric_limits<double>::quiet_NaN());
            myPendingFlag[i].assign(space, 0);
        }
    }
 
    ~CCHMetricFamily() {
        for (LiveMetric* c : myLiveMetrics) {
            delete c->refVehicle;
            delete c;
        }
        for (auto& item : myStaticMetrics) {
            delete item.second.refVehicle;
        }
    }
 
 
    MetricPtr get(const K* key, SUMOVehicleClass vClass, SUMOTime time, const V* veh) {
        std::lock_guard<std::mutex> lock(myStaticLock);
        if (myStale) {
            for (auto& item : myStaticMetrics) {
                StaticMetric& sm = item.second;
                fillStatic(sm, item.first.second, time, sm.refVehicle != nullptr ? sm.refVehicle : veh, veh);
                sm.metric->customize();
            }
            myStale = false;
        }
        const int period = periodOf(time);
        const std::pair<const K*, int> mapKey(key, period);
        auto it = myStaticMetrics.find(mapKey);
        if (it == myStaticMetrics.end()) {
            StaticMetric& sm = myStaticMetrics[mapKey];
            sm.vClass = vClass;
            const V* ref = veh;
            if (myFactory != nullptr) {
                sm.refVehicle = myFactory(key, 0);
                ref = sm.refVehicle;
            }
            fillStatic(sm, period, time, ref, veh);
            sm.metric.reset(new RoutingKit::CustomizableContractionHierarchyMetric(
                                myGraph->cch(), sm.weights));
            sm.metric->customize();
            return sm.metric.get();
        }
        return it->second.metric.get();
    }
 
    SUMOTime periodEnd(SUMOTime time) const {
        if (myWeightPeriod <= 0 || myWeightPeriod == SUMOTime_MAX) {
            return SUMOTime_MAX;
        }
        return myBegin + (periodOf(time) + 1) * myWeightPeriod;
    }
 
    void flagStale() {
        std::lock_guard<std::mutex> lock(myStaticLock);
        myStale = true;
    }
 
    void flagPermissionsStale() {
        flagStale();
    }
 
 
    void seedKey(const K* key) {
        if (myByKey.count(key) == 0) {
            std::vector<LiveMetric*>& slots = myByKey[key];
            for (int k = 0; k < myEnsembleK; k++) {
                slots.push_back(buildLiveMetric(key, k));
            }
        }
    }
 
    void markDirty(const E* e) {
        const int id = e->getNumericalID();
        if (id < 0 || id >= (int)myPendingFlag[0].size()) {
            return;
        }
        for (int i = 0; i < 2; i++) {
            if (!myPendingFlag[i][id]) {
                myPendingFlag[i][id] = 1;
                myPendingList[i].push_back(e);
            }
        }
    }
 
    void invalidateEdge(const E* e) {
        const int id = e->getNumericalID();
        if (id < 0 || id >= (int)myPendingFlag[0].size()) {
            return;
        }
        markDirty(e);
        // NaN sentinel: the deadband always passes, so the flip reaches the
        // metric at the next barrier even though the speed table did not move
        myAppliedEffort[0][id] = std::numeric_limits<double>::quiet_NaN();
        myAppliedEffort[1][id] = std::numeric_limits<double>::quiet_NaN();
        myFullFillsPending = 2;
        myMetricStale.store(true, std::memory_order_release);
    }
 
    void atBarrier(double now, bool effortsMoved) {
        if (effortsMoved) {
            mySpeedEpoch.fetch_add(1, std::memory_order_release);
        }
        if (myQueried.exchange(false, std::memory_order_acq_rel)
                || myMetricStale.load(std::memory_order_acquire)
                || (effortsMoved && !myLiveMetrics.empty())) {
            customize(now);
        }
    }
 
    void customize(double now) {
        // types that queried since the last barrier without a metric
        // (streamed route files, TraCI-added types): create their state now
        {
            std::lock_guard<std::mutex> lock(myWantedLock);
            for (const K* key : myWantedKeys) {
                seedKey(key);
            }
            myWantedKeys.clear();
        }
        if (myLiveMetrics.empty()) {
            return;
        }
        // buffers flip in lockstep across types (every call processes all)
        const int backShared = 1 - myLiveMetrics.front()->frontIndex;
        // Permission flip pending: run a FULL refill on this pass's back
        // buffer (the other buffer gets its full refill on the next pass --
        // myFullFillsPending counts both down). The fills below re-prime the
        // graph's connection masks, which the flip's caller invalidated.
        bool forceFull = false;
        if (myFullFillsPending > 0) {
            myFullFillsPending--;
            forceFull = true;
        }
        // Deadband pass, once, on the type-shared gate effort: accept edges
        // whose effort moved by more than BOTH bounds since this buffer last
        // applied them; rejected edges stay pending (see the constructor doc)
        std::vector<const E*> accepted;
        if (!forceFull && myLiveMetrics.front()->metric[backShared] != nullptr) {
            std::vector<const E*> stillPending;
            for (const E* e : myPendingList[backShared]) {
                const int id = e->getNumericalID();
                const double effNow = myGateEffort(e, nullptr, now);
                const double effApplied = myAppliedEffort[backShared][id];
                bool pass = true;
                if (!std::isnan(effApplied) && effApplied > 0. && effNow > 0.) {
                    const double hi = MAX2(effNow, effApplied);
                    const double lo = MIN2(effNow, effApplied);
                    pass = (hi / lo > myUpdateFactor) && (hi - lo > myUpdateConstant);
                }
                if (pass) {
                    accepted.push_back(e);
                    myAppliedEffort[backShared][id] = effNow;
                    myPendingFlag[backShared][id] = 0;
                } else {
                    stillPending.push_back(e);
                }
            }
            myPendingList[backShared].swap(stillPending);
        }
        for (LiveMetric* c : myLiveMetrics) {
            const int back = 1 - c->frontIndex;
            // Fill from the live efforts, masking arcs the class is not
            // permitted on -- this is where per-class permissions AND active
            // closures become inf_weight. The fill reference is the type's
            // OWNED refVehicle, so the effort captures the type's extras
            // exactly (including each folded via edge inside viaChainEffort)
            // and freezes ONE random realization per metric -- the same
            // approximation CHRouterWrapper makes per hierarchy; exact
            // per-vehicle randomization remains the domain of the exact
            // routers.
            if (c->metric[back] == nullptr) {
                // first use of this buffer for THIS type only (streamed/late
                // vType): no previous state to diff against for this type,
                // but every other already-running type's sparse deadband
                // state is unaffected, so this branch must NOT trigger the
                // shared wipe below (which would reset the type-shared
                // myPendingList/myAppliedEffort for everybody just because
                // one type is new)
                myGraph->fillInputWeights(myFillEffort, c->vClass, c->refVehicle, now, c->weight[back]);
                c->metric[back] = std::make_shared<RoutingKit::CustomizableContractionHierarchyMetric>(
                                      myGraph->cch(), c->weight[back]);
                c->metric[back]->customize();  // serial; avoids OpenMP oversubscription under FOX
            } else if (forceFull) {
                // permission flip: wholesale refill in place from the
                // re-primed masks and live permissions (the metric already
                // references weight[back]), then a full customize. This is
                // the ONLY case that invalidates the type-shared deadband
                // state below, since it is the only one that actually
                // changes what "applied" means for every type at once.
                myGraph->fillInputWeights(myFillEffort, c->vClass, c->refVehicle, now, c->weight[back]);
                c->metric[back]->customize();
            } else {
                // SPARSE PATH: metric[back] is the customization of the
                // current contents of weight[back]. Recompute only the arcs
                // of accepted edges (edge->arc reverse image), write the
                // ones that moved, and propagate through affected triangles
                // only. Cost scales with traffic transitions, not with the
                // network.
                std::vector<unsigned>& applied = c->weight[back];
                c->partial->reset(myGraph->cch());
                unsigned changed = 0;
                for (const E* e : accepted) {
                    for (const unsigned a : myGraph->arcsOfEdge(e)) {
                        const unsigned newW = myGraph->computeArcWeight(a, myFillEffort, c->vClass, c->refVehicle, now);
                        if (newW != applied[a]) {
                            applied[a] = newW;
                            c->partial->update_arc(a);  // takes INPUT arc ids
                            ++changed;
                        }
                    }
                }
                if (changed > 0) {
                    c->partial->customize(*c->metric[back]);
                }
            }
            // publish lock-free: release store at the barrier, worker
            // acquire-loads on the hot path
            c->frontIndex = back;
            c->front.store(c->metric[back].get(), std::memory_order_release);
        }
        if (forceFull) {
            // a permission flip's full refill matched every arc to the live
            // efforts/masks for this buffer: type-shared deadband entries
            // are stale; NaN re-arms the first-change auto-pass. (A plain
            // per-type first-fill above does NOT reach here -- it must not
            // wipe deadband state that other, already-running types still
            // depend on.)
            for (const E* e : myPendingList[backShared]) {
                myPendingFlag[backShared][e->getNumericalID()] = 0;
            }
            myPendingList[backShared].clear();
            myAppliedEffort[backShared].assign(myAppliedEffort[backShared].size(),
                                               std::numeric_limits<double>::quiet_NaN());
        }
        // every published front now reflects the live permissions (the other
        // buffer, if still pending a full refill, is repaired before its
        // next publish) and the live efforts as of this barrier (both epoch
        // counters only move on the owner's thread at the barrier, so the
        // pair cannot tear)
        myMetricStale.store(false, std::memory_order_release);
        myPublishedEpoch.store(mySpeedEpoch.load(std::memory_order_relaxed),
                               std::memory_order_release);
    }
 
    MetricPtr published(const K* key, const std::string& vehID) {
        const auto it = myByKey.find(key);
        if (it == myByKey.end()) {
            // streamed-in type without a metric yet: register it for
            // creation at the next barrier and fall back
            {
                std::lock_guard<std::mutex> lock(myWantedLock);
                myWantedKeys.push_back(key);
            }
            myQueried.store(true, std::memory_order_relaxed);
            return nullptr;
        }
        // mark the metric as consumed. Test before set: an unconditional
        // store from every query would ping-pong the cache line between
        // cores; the read is shared and the store fires once per barrier
        // window (relaxed: only gates bootstrap/re-arm, never visibility)
        if (!myQueried.load(std::memory_order_relaxed)) {
            myQueried.store(true, std::memory_order_relaxed);
        }
        // A permission flip that is not yet customized in makes every
        // published metric unusable: it may still route THROUGH a
        // just-closed edge, and CCHRouter's coverage test consults the LIVE
        // permissions, which would wrongly bless the query. Route exactly
        // until the barrier repairs the metrics.
        if (myMetricStale.load(std::memory_order_acquire)) {
            return nullptr;
        }
        // Safety invariant: never serve a metric the efforts have moved away
        // from -- the exact routers see live weights and CH is bounded to
        // one weightPeriod. atBarrier re-customizes at every moved barrier
        // once metrics exist, so this cannot fire in steady state; if a gap
        // ever opens, this burst routes on the exact fallback (live weights)
        // and the queried flag set above arms the customize that closes it.
        if (myPublishedEpoch.load(std::memory_order_acquire)
                != mySpeedEpoch.load(std::memory_order_acquire)) {
            return nullptr;
        }
        // stable slot assignment: FNV-1a over the id (std::hash is
        // implementation-defined and would break cross-platform test
        // reproducibility)
        const LiveMetric* c = myEnsembleK == 1 ? it->second.front()
                              : it->second[fnv1a(vehID) % myEnsembleK];
        return c->front.load(std::memory_order_acquire);
    }
 
    bool empty() const {
        return myLiveMetrics.empty();
    }
 
private:
    struct LiveMetric {
        std::vector<unsigned> weight[2];  // ping-pong input-weight buffers, live whole run
        std::shared_ptr<RoutingKit::CustomizableContractionHierarchyMetric> metric[2];
        std::atomic<const RoutingKit::CustomizableContractionHierarchyMetric*> front{nullptr};
        int frontIndex = 1;               // first customize uses back = 0
        SUMOVehicleClass vClass = SVC_PASSENGER;
        V* refVehicle = nullptr;
        std::shared_ptr<RoutingKit::CustomizableContractionHierarchyPartialCustomization> partial;
    };
 
    struct StaticMetric {
        std::vector<unsigned> weights;
        std::unique_ptr<RoutingKit::CustomizableContractionHierarchyMetric> metric;
        SUMOVehicleClass vClass = SVC_PASSENGER;
        V* refVehicle = nullptr;
        bool filled = false;
        std::vector<std::pair<unsigned, unsigned> > patched;
    };
 
    int periodOf(SUMOTime time) const {
        if (myWeightPeriod > 0 && myWeightPeriod != SUMOTime_MAX && time > myBegin) {
            return (int)((time - myBegin) / myWeightPeriod);
        }
        return 0;
    }
 
    void fillStatic(StaticMetric& sm, int period, SUMOTime time, const V* ref, const V* veh) {
        const double fillTime = myWeightPeriod == SUMOTime_MAX
                                ? STEPS2TIME(time) : STEPS2TIME(myBegin + period * myWeightPeriod);
        myGraph->fillInputWeights(myFillEffort, sm.vClass, ref, fillTime, sm.weights);
        if (!sm.filled) {
            sm.filled = true;
            if (myPatch != nullptr) {
                const std::vector<unsigned> unpatched(sm.weights);
                myPatch(myGraph, veh, sm.weights);
                for (unsigned a = 0; a < (unsigned)sm.weights.size(); a++) {
                    if (sm.weights[a] != unpatched[a]) {
                        sm.patched.emplace_back(a, sm.weights[a]);
                    }
                }
            }
        } else {
            for (const auto& p : sm.patched) {
                sm.weights[p.first] = p.second;
            }
        }
    }
 
    static uint64_t fnv1a(const std::string& s) {
        uint64_t h = 1469598103934665603ull;
        for (const char ch : s) {
            h = (h ^ (unsigned char)ch) * 1099511628211ull;
        }
        return h;
    }
 
    LiveMetric* buildLiveMetric(const K* key, int slot) {
        LiveMetric* c = new LiveMetric();
        c->refVehicle = myFactory(key, slot);
        c->vClass = c->refVehicle->getVClass();
        c->weight[0].resize(myGraph->arcCount());
        c->weight[1].resize(myGraph->arcCount());
        c->partial = std::make_shared<RoutingKit::CustomizableContractionHierarchyPartialCustomization>(myGraph->cch());
        myLiveMetrics.push_back(c);
        return c;
    }
 
    const Graph* myGraph;
    EffortOperation myFillEffort;
    EffortOperation myGateEffort;
    RefVehicleFactory myFactory;
    WeightPatch myPatch;
    const bool myLive;
 
    SUMOTime myBegin;
    SUMOTime myWeightPeriod;
    std::map<std::pair<const K*, int>, StaticMetric> myStaticMetrics;
    std::mutex myStaticLock;
    bool myStale = false;
 
    double myUpdateFactor;
    double myUpdateConstant;
    int myEnsembleK = 1;
    std::vector<LiveMetric*> myLiveMetrics;
    std::map<const K*, std::vector<LiveMetric*> > myByKey;
    std::vector<const K*> myWantedKeys;
    std::mutex myWantedLock;
    std::atomic<bool> myQueried{false};
    std::atomic<bool> myMetricStale{false};
    std::atomic<uint64_t> mySpeedEpoch{0};
    std::atomic<uint64_t> myPublishedEpoch{0};
    int myFullFillsPending = 0;
    std::vector<double> myAppliedEffort[2];
    std::vector<char> myPendingFlag[2];
    std::vector<const E*> myPendingList[2];
 
private:
    CCHMetricFamily(const CCHMetricFamily&) = delete;
    CCHMetricFamily& operator=(const CCHMetricFamily&) = delete;
};