MapMatcher.h

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2001-2024 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
/****************************************************************************/
// Utility functions for matching locations to edges (during route parsing)
/****************************************************************************/
#pragma once
#include <config.h>
 
#include <utils/geom/GeoConvHelper.h>
#include <utils/vehicle/SUMOVTypeParameter.h>
#include "NamedRTree.h"
#include "SUMOVehicleClass.h"
#include "MsgHandler.h"
 
#define JUNCTION_TAZ_MISSING_HELP "\nSet option '--junction-taz' or load a TAZ-file"
 
// ===========================================================================
// class declarations
// ===========================================================================
 
// ===========================================================================
// class definitions
// ===========================================================================
template<class E, class L, class N>
class MapMatcher {
protected:
    MapMatcher(bool matchJunctions, double matchDistance, MsgHandler* errorOutput):
        myLaneTree(nullptr),
        myMapMatchJunctions(matchJunctions),
        myMapMatchingDistance(matchDistance),
        myErrorOutput(errorOutput) {}
 
    virtual ~MapMatcher() {
        delete myLaneTree;
    }
 
    void parseGeoEdges(const PositionVector& positions, bool geo, SUMOVehicleClass vClass,
                       std::vector<const E*>& into, const std::string& rid, bool isFrom, bool& ok) {
        if (geo && !GeoConvHelper::getFinal().usingGeoProjection()) {
            WRITE_ERROR(TL("Cannot convert geo-positions because the network has no geo-reference"));
            return;
        }
        for (Position pos : positions) {
            Position orig = pos;
            if (geo) {
                GeoConvHelper::getFinal().x2cartesian_const(pos);
            }
            double dist = MIN2(10.0, myMapMatchingDistance);
            const E* best = getClosestEdge(pos, dist, vClass);
            while (best == nullptr && dist < myMapMatchingDistance) {
                dist = MIN2(dist * 2, myMapMatchingDistance);
                best = getClosestEdge(pos, dist, vClass);
            }
            if (best == nullptr) {
                myErrorOutput->inform("No edge found near position " + toString(orig, geo ? gPrecisionGeo : gPrecision) + " within the route " + rid + ".");
                ok = false;
            } else {
                if (myMapMatchJunctions) {
                    best = getJunctionTaz(pos, best, vClass, isFrom);
                    if (best != nullptr) {
                        into.push_back(best);
                    }
                } else {
                    // handle multiple via locations on the same edge without loops
                    if (positions.size() == 1 || into.empty() || into.back() != best) {
                        into.push_back(best);
                    }
                }
            }
        }
    }
 
 
    const E* getClosestEdge(const Position& pos, double distance, SUMOVehicleClass vClass) {
        NamedRTree* t = getLaneTree();
        Boundary b;
        b.add(pos);
        b.grow(distance);
        const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
        const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
        std::set<const Named*> lanes;
        Named::StoringVisitor sv(lanes);
        t->Search(cmin, cmax, sv);
        // use closest
        double minDist = std::numeric_limits<double>::max();
        const L* best = nullptr;
        for (const Named* o : lanes) {
            const L* cand = static_cast<const L*>(o);
            if (!cand->allowsVehicleClass(vClass)) {
                continue;
            }
            double dist = cand->getShape().distance2D(pos);
            if (dist < minDist) {
                minDist = dist;
                best = cand;
            }
        }
        if (best == nullptr) {
            return nullptr;
        } else {
            const E* bestEdge = &best->getEdge();
            while (bestEdge->isInternal()) {
                bestEdge = bestEdge->getSuccessors().front();
            }
            return bestEdge;
        }
    }
 
    const E* getJunctionTaz(const Position& pos, const E* closestEdge, SUMOVehicleClass vClass, bool isFrom) {
        if (closestEdge == nullptr) {
            return nullptr;
        } else {
            const N* fromJunction = closestEdge->getFromJunction();
            const N* toJunction = closestEdge->getToJunction();
            const bool fromCloser = (fromJunction->getPosition().distanceSquaredTo2D(pos) <
                                     toJunction->getPosition().distanceSquaredTo2D(pos));
            const E* fromSource = retrieveEdge(fromJunction->getID() + "-source");
            const E* fromSink = retrieveEdge(fromJunction->getID() + "-sink");
            const E* toSource = retrieveEdge(toJunction->getID() + "-source");
            const E* toSink = retrieveEdge(toJunction->getID() + "-sink");
            if (fromSource == nullptr || fromSink == nullptr) {
                myErrorOutput->inform("Junction-taz '" + fromJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
                return nullptr;
            }
            if (toSource == nullptr || toSink == nullptr) {
                myErrorOutput->inform("Junction-taz '" + toJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
                return nullptr;
            }
            const bool fromPossible = isFrom ? fromSource->getSuccessors(vClass).size() > 0 : fromSink->getPredecessors().size() > 0;
            const bool toPossible = isFrom ? toSource->getSuccessors(vClass).size() > 0 : toSink->getPredecessors().size() > 0;
            //std::cout << "getJunctionTaz pos=" << pos << " isFrom=" << isFrom << " closest=" << closestEdge->getID() << " fromPossible=" << fromPossible << " toPossible=" << toPossible << "\n";
            if (fromCloser && fromPossible) {
                // return closest if possible
                return isFrom ? fromSource : fromSink;
            } else if (!fromCloser && toPossible) {
                // return closest if possible
                return isFrom ? toSource : toSink;
            } else {
                // return possible
                if (fromPossible) {
                    return isFrom ? fromSource : fromSink;
                } else {
                    return isFrom ? toSource : toSink;
                }
            }
        }
    }
 
 
    virtual void initLaneTree(NamedRTree* tree) = 0;
 
    virtual E* retrieveEdge(const std::string& id) = 0;
 
private:
    NamedRTree* getLaneTree() {
        if (myLaneTree == nullptr) {
            myLaneTree = new NamedRTree();
            initLaneTree(myLaneTree);
        }
        return myLaneTree;
    }
 
    NamedRTree* myLaneTree;
    bool myMapMatchJunctions;
    double myMapMatchingDistance;
    MsgHandler* myErrorOutput;
 
 
private:
    MapMatcher(const MapMatcher& s) = delete;
 
    MapMatcher& operator=(const MapMatcher& s) = delete;
};