osrm-backend/Contractor/SearchEngine.h

/*
    open source routing machine
    Copyright (C) Dennis Luxen, others 2010

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU AFFERO General Public License as published by
the Free Software Foundation; either version 3 of the License, or
any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
or see http://www.gnu.org/licenses/agpl.txt.
 */

#ifndef SEARCHENGINE_H_
#define SEARCHENGINE_H_

#include <climits>
#include <deque>

#include "BinaryHeap.h"
#include "../DataStructures/PhantomNodes.h"
#include "../typedefs.h"

struct _HeapData {
    NodeID parent;
    bool stalled;
    _HeapData( NodeID p ) : parent(p) , stalled(false) { }
};

typedef BinaryHeap< NodeID, int, int, _HeapData, DenseStorage< NodeID, unsigned > > _Heap;

template<typename EdgeData, typename GraphT, typename NodeHelperT = NodeInformationHelpDesk>
class SearchEngine {
private:
    const GraphT * _graph;
    inline double absDouble(double input) { if(input < 0) return input*(-1); else return input;}
public:
    SearchEngine(GraphT * g, NodeHelperT * nh) : _graph(g), nodeHelpDesk(nh) {}
    ~SearchEngine() {}

    inline const void getNodeInfo(NodeID id, _Coordinate& result) const
    {
        result.lat = nodeHelpDesk->getLatitudeOfNode(id);
        result.lon = nodeHelpDesk->getLongitudeOfNode(id);
    }

    unsigned int numberOfNodes() const
    {
        return nodeHelpDesk->getNumberOfNodes();
    }

    unsigned int ComputeRoute(PhantomNodes * phantomNodes, vector<NodeID> * path, _Coordinate& startCoord, _Coordinate& targetCoord)
    {
        bool onSameEdge = false;
        bool onSameEdgeReversed = false;
        _Heap * _forwardHeap = new _Heap(nodeHelpDesk->getNumberOfNodes());
        _Heap * _backwardHeap = new _Heap(nodeHelpDesk->getNumberOfNodes());
        NodeID middle = ( NodeID ) 0;
        unsigned int _upperbound = std::numeric_limits<unsigned int>::max();

        if(phantomNodes->startNode1 == UINT_MAX || phantomNodes->startNode2 == UINT_MAX)
            return _upperbound;

        if( (phantomNodes->startNode1 == phantomNodes->targetNode1 && phantomNodes->startNode2 == phantomNodes->targetNode2 ) ||
                (phantomNodes->startNode1 == phantomNodes->targetNode2 && phantomNodes->startNode2 == phantomNodes->targetNode1 ) )
        {
            bool reverse = false;
            EdgeID currentEdge = _graph->FindEdge( phantomNodes->startNode1, phantomNodes->startNode2 );
            if(currentEdge == UINT_MAX){
                currentEdge = _graph->FindEdge( phantomNodes->startNode2, phantomNodes->startNode1 );
                reverse = true;
            }

            if(phantomNodes->startRatio < phantomNodes->targetRatio && _graph->GetEdgeData(currentEdge).forward) {
                onSameEdge = true;
                _upperbound = 10 * ApproximateDistance(phantomNodes->startCoord.lat, phantomNodes->startCoord.lon, phantomNodes->targetCoord.lat, phantomNodes->targetCoord.lon);
            } else if(phantomNodes->startRatio > phantomNodes->targetRatio && _graph->GetEdgeData(currentEdge).backward && !reverse)
            {
                onSameEdge = true;
                _upperbound = 10 * ApproximateDistance(phantomNodes->startCoord.lat, phantomNodes->startCoord.lon, phantomNodes->targetCoord.lat, phantomNodes->targetCoord.lon);
            } else if(phantomNodes->startRatio < phantomNodes->targetRatio && _graph->GetEdgeData(currentEdge).backward) {
                onSameEdge = true;
                _upperbound = 10 * ApproximateDistance(phantomNodes->startCoord.lat, phantomNodes->startCoord.lon, phantomNodes->targetCoord.lat, phantomNodes->targetCoord.lon);
            } else if(phantomNodes->startRatio > phantomNodes->targetRatio) {
                onSameEdgeReversed = true;

                _Coordinate result;
                getNodeInfo(phantomNodes->startNode1, result);
                getNodeInfo(phantomNodes->startNode2, result);

                EdgeWeight w = _graph->GetEdgeData( currentEdge ).distance;
                _forwardHeap->Insert(phantomNodes->startNode2, absDouble(  w*phantomNodes->startRatio), phantomNodes->startNode2);
                _backwardHeap->Insert(phantomNodes->startNode1, absDouble(  w-w*phantomNodes->targetRatio), phantomNodes->startNode1);
            }

        } else if(phantomNodes->startNode1 != UINT_MAX)
        {
            bool reverse = false;
            EdgeID edge = _graph->FindEdge( phantomNodes->startNode1, phantomNodes->startNode2);
            if(edge == UINT_MAX){
                edge = _graph->FindEdge( phantomNodes->startNode2, phantomNodes->startNode1 );
                reverse = true;
            }
            EdgeWeight w = _graph->GetEdgeData( edge ).distance;
            if( (_graph->GetEdgeData( edge ).backward && !reverse) || (_graph->GetEdgeData( edge ).forward && reverse) )
                _forwardHeap->Insert(phantomNodes->startNode1, absDouble(  w*phantomNodes->startRatio), phantomNodes->startNode1);
            if( (_graph->GetEdgeData( edge ).backward && reverse) || (_graph->GetEdgeData( edge ).forward && !reverse) )
                _forwardHeap->Insert(phantomNodes->startNode2, absDouble(w-w*phantomNodes->startRatio), phantomNodes->startNode2);
        }
        if(phantomNodes->targetNode1 != UINT_MAX && !onSameEdgeReversed)
        {
            bool reverse = false;
            EdgeID edge = _graph->FindEdge( phantomNodes->targetNode1, phantomNodes->targetNode2);
            if(edge == UINT_MAX){
                edge = _graph->FindEdge( phantomNodes->targetNode2, phantomNodes->targetNode1 );
                reverse = true;
            }
            EdgeWeight w = _graph->GetEdgeData( edge ).distance;
            if( (_graph->GetEdgeData( edge ).backward && !reverse) || (_graph->GetEdgeData( edge ).forward && reverse) )
                _backwardHeap->Insert(phantomNodes->targetNode2, absDouble(  w*phantomNodes->targetRatio), phantomNodes->targetNode2);
            if( (_graph->GetEdgeData( edge ).backward && reverse) || (_graph->GetEdgeData( edge ).forward && !reverse) )
                _backwardHeap->Insert(phantomNodes->targetNode1, absDouble(w-w*phantomNodes->startRatio), phantomNodes->targetNode1);
        }

        NodeID sourceHeapNode = 0;
        NodeID targetHeapNode = 0;
        if(onSameEdgeReversed) {
            sourceHeapNode = _forwardHeap->Min();
            targetHeapNode = _backwardHeap->Min();
        }
        while(_forwardHeap->Size() + _backwardHeap->Size() > 0)
        {
            if ( _forwardHeap->Size() > 0 ) {
                _RoutingStep( _forwardHeap, _backwardHeap, true, &middle, &_upperbound );
            }
            if ( _backwardHeap->Size() > 0 ) {
                _RoutingStep( _backwardHeap, _forwardHeap, false, &middle, &_upperbound );
            }
        }

        if ( _upperbound == std::numeric_limits< unsigned int >::max() || onSameEdge )
        {
            delete _forwardHeap;
            delete _backwardHeap;
            return _upperbound;
        }

        NodeID pathNode = middle;
        deque< NodeID > packedPath;

        while ( onSameEdgeReversed ? pathNode != sourceHeapNode : pathNode != phantomNodes->startNode1 && pathNode != phantomNodes->startNode2 ) {
            pathNode = _forwardHeap->GetData( pathNode ).parent;
            packedPath.push_front( pathNode );
        }

        packedPath.push_back( middle );
        pathNode = middle;

        while ( onSameEdgeReversed ? pathNode != targetHeapNode : pathNode != phantomNodes->targetNode2 && pathNode != phantomNodes->targetNode1 ){
            pathNode = _backwardHeap->GetData( pathNode ).parent;
            packedPath.push_back( pathNode );
        }

        // push start node explicitely
        path->push_back(packedPath[0]);
        //        if(packedPath[0] != packedPath[1])
        {
            for(deque<NodeID>::size_type i = 0; i < packedPath.size()-1; i++)
            {
                _UnpackEdge(packedPath[i], packedPath[i+1], path);
            }
        }

        packedPath.clear();
        delete _forwardHeap;
        delete _backwardHeap;

        return _upperbound/10;
    }

    unsigned int ComputeDistanceBetweenNodes(NodeID start, NodeID target)
    {
        _Heap * _forwardHeap = new _Heap(_graph->GetNumberOfNodes());
        _Heap * _backwardHeap = new _Heap(_graph->GetNumberOfNodes());
        NodeID middle = ( NodeID ) 0;
        unsigned int _upperbound = std::numeric_limits<unsigned int>::max();

        _forwardHeap->Insert(start, 0, start);
        _backwardHeap->Insert(target, 0, target);

        while(_forwardHeap->Size() + _backwardHeap->Size() > 0)
        {
            if ( _forwardHeap->Size() > 0 ) {
                _RoutingStep( _forwardHeap, _backwardHeap, true, &middle, &_upperbound );
            }
            if ( _backwardHeap->Size() > 0 ) {
                _RoutingStep( _backwardHeap, _forwardHeap, false, &middle, &_upperbound );
            }
        }
        delete _forwardHeap;
        delete _backwardHeap;
        return _upperbound;
    }

    inline unsigned int findNearestNodeForLatLon(const _Coordinate& coord, _Coordinate& result) const
    {
        nodeHelpDesk->findNearestNodeIDForLatLon( coord, result );
    }

    inline bool FindRoutingStarts(const _Coordinate start, const _Coordinate target, PhantomNodes * routingStarts)
    {
        nodeHelpDesk->FindRoutingStarts(start, target, routingStarts);
    }
private:
    NodeHelperT * nodeHelpDesk;

    void _RoutingStep(_Heap * _forwardHeap, _Heap *_backwardHeap, const bool& forwardDirection, NodeID * middle, unsigned int * _upperbound)
    {
        const NodeID node = _forwardHeap->DeleteMin();
        const unsigned int distance = _forwardHeap->GetKey( node );
        if ( _backwardHeap->WasInserted( node ) ) {
            const unsigned int newDistance = _backwardHeap->GetKey( node ) + distance;
            if ( newDistance < *_upperbound ) {
                *middle = node;
                *_upperbound = newDistance;
            }
        }
        if ( distance > *_upperbound ) {
            _forwardHeap->DeleteAll();
            return;
        }
        for ( typename GraphT::EdgeIterator edge = _graph->BeginEdges( node ); edge < _graph->EndEdges(node); edge++ ) {
            const NodeID to = _graph->GetTarget(edge);
            const int edgeWeight = _graph->GetEdgeData(edge).distance;

            assert( edgeWeight > 0 );
            const int toDistance = distance + edgeWeight;

            if(forwardDirection ? _graph->GetEdgeData(edge).forward : _graph->GetEdgeData(edge).backward )
            {
                //New Node discovered -> Add to Heap + Node Info Storage
                if ( !_forwardHeap->WasInserted( to ) )
                {
                    _forwardHeap->Insert( to, toDistance, node );
                }
                //Found a shorter Path -> Update distance
                else if ( toDistance < _forwardHeap->GetKey( to ) ) {
                    _forwardHeap->GetData( to ).parent = node;
                    _forwardHeap->DecreaseKey( to, toDistance );
                    //new parent
                }
            }
        }
    }

    bool _UnpackEdge( const NodeID source, const NodeID target, std::vector< NodeID >* path ) {
        assert(source != target);
        //find edge first.
        typename GraphT::EdgeIterator smallestEdge = SPECIAL_EDGEID;
        EdgeWeight smallestWeight = UINT_MAX;
        for(typename GraphT::EdgeIterator eit = _graph->BeginEdges(source); eit < _graph->EndEdges(source); eit++)
        {
            const EdgeWeight weight = _graph->GetEdgeData(eit).distance;
            {
                if(_graph->GetTarget(eit) == target && weight < smallestWeight && _graph->GetEdgeData(eit).forward)
                {
                    smallestEdge = eit; smallestWeight = weight;
                }
            }
        }
        if(smallestEdge == SPECIAL_EDGEID)
        {
            for(typename GraphT::EdgeIterator eit = _graph->BeginEdges(target); eit < _graph->EndEdges(target); eit++)
            {
                const EdgeWeight weight = _graph->GetEdgeData(eit).distance;
                {
                    if(_graph->GetTarget(eit) == source && weight < smallestWeight && _graph->GetEdgeData(eit).backward)
                    {
                        smallestEdge = eit; smallestWeight = weight;
                    }
                }
            }
        }

        assert(smallestWeight != SPECIAL_EDGEID); //no edge found. This should not happen at all!

        const EdgeData ed = _graph->GetEdgeData(smallestEdge);
        if(ed.shortcut)
        {//unpack
            const NodeID middle = ed.middle;
            _UnpackEdge(source, middle, path);
            _UnpackEdge(middle, target, path);
            return false;
        } else {
            assert(!ed.shortcut);
            path->push_back(target);
            return true;
        }
    }
};

#endif /* SEARCHENGINE_H_ */