MREdgePathsBuilder.h

Go to the documentation of this file.

#pragma once
 
#include "MREdgePaths.h"
#include "MRBitSet.h"
#include "MRMesh.h"
#include "MRRingIterator.h"
#include "MRphmap.h"
#include "MRMacros.h"
#include <queue>
 
namespace MR
{
 
 
struct VertPathInfo
{
    // edge from this vertex to its predecessor in the forest
    EdgeId back;
    // best summed metric to reach this vertex
    float metric = FLT_MAX;
 
    bool isStart() const { return !back.valid(); }
};
 
// ParallelHashMap here was significantly slower (especially in debug version of tunnel detection),
// probably because of more allocations
using VertPathInfoMap = HashMap<VertId, VertPathInfo>;
 
template<class MetricToPenalty>
class EdgePathsBuilderT
{
public:
    EdgePathsBuilderT( const MeshTopology & topology, const EdgeMetric & metric );
 
    explicit EdgePathsBuilderT( const MeshTopology & topology );
 
    void reset( const EdgeMetric & metric );
 
    bool addStart( VertId startVert, float startMetric );
 
    struct ReachedVert
    {
        VertId v;
 
        EdgeId backward;
 
        float penalty = FLT_MAX;
 
        float metric = FLT_MAX;
    };
 
    ReachedVert reachNext();
 
    bool addOrgRingSteps( const ReachedVert & rv );
 
    ReachedVert growOneEdge();
 
public:
    bool done() const { return nextSteps_.empty(); }
 
    float doneDistance() const { return nextSteps_.empty() ? FLT_MAX : nextSteps_.top().penalty; }
 
    const VertPathInfoMap & vertPathInfoMap() const { return vertPathInfoMap_; }
 
    const VertPathInfo * getVertInfo( VertId v ) const;
 
    EdgePath getPathBack( VertId backpathStart ) const;
 
    VertId trackPathBack( VertId backpathStart, EdgePath* res = nullptr ) const;
 
protected:
    MR_NO_UNIQUE_ADDRESS MetricToPenalty metricToPenalty_;
 
private:
    const MeshTopology & topology_;
    EdgeMetric metric_;
    VertPathInfoMap vertPathInfoMap_;
 
    struct CandidateVert
    {
        VertId v;
        float penalty = FLT_MAX;
 
        friend bool operator <( const CandidateVert & a, const CandidateVert & b )
        {
            return a.penalty > b.penalty;
        }
    };
    std::priority_queue<CandidateVert> nextSteps_;
 
    bool addNextStep_( const VertPathInfo & c );
};
 
struct TrivialMetricToPenalty
{
    float operator()( float metric, VertId ) const { return metric; }
};
 
using EdgePathsBuilder = EdgePathsBuilderT<TrivialMetricToPenalty>;
 
template<class MetricToPenalty>
EdgePathsBuilderT<MetricToPenalty>::EdgePathsBuilderT( const MeshTopology & topology, const EdgeMetric & metric )
    : topology_( topology )
    , metric_( metric )
{
}
 
template<class MetricToPenalty>
EdgePathsBuilderT<MetricToPenalty>::EdgePathsBuilderT( const MeshTopology & topology )
    : topology_( topology )
{
}
 
template<class MetricToPenalty>
void EdgePathsBuilderT<MetricToPenalty>::reset( const EdgeMetric & metric )
{
    metric_ = metric;
    vertPathInfoMap_.clear();
    while ( !nextSteps_.empty() )
        nextSteps_.pop();
}
 
template<class MetricToPenalty>
bool EdgePathsBuilderT<MetricToPenalty>::addStart( VertId startVert, float startMetric )
{
    auto & vi = vertPathInfoMap_[startVert];
    if ( vi.metric > startMetric )
    {
        vi = { EdgeId{}, startMetric };
        nextSteps_.push( CandidateVert{ startVert, metricToPenalty_( startMetric, startVert ) } );
        return true;
    }
    return false;
}
 
template<class MetricToPenalty>
const VertPathInfo * EdgePathsBuilderT<MetricToPenalty>::getVertInfo( VertId v ) const
{
    auto it = vertPathInfoMap_.find( v );
    return ( it != vertPathInfoMap_.end() ) ? &it->second : nullptr;
}
 
template<class MetricToPenalty>
EdgePath EdgePathsBuilderT<MetricToPenalty>::getPathBack( VertId v ) const
{
    EdgePath res;
    trackPathBack( v, &res );
    return res;
}
 
template<class MetricToPenalty>
VertId EdgePathsBuilderT<MetricToPenalty>::trackPathBack( VertId v, EdgePath* res ) const
{
    for (;;)
    {
        auto it = vertPathInfoMap_.find( v );
        if ( it == vertPathInfoMap_.end() )
        {
            assert( false );
            break;
        }
        auto & vi = it->second;
        if ( vi.isStart() )
            break;
        if ( res )
            res->push_back( vi.back );
        v = topology_.dest( vi.back );
    }
    return v;
}
 
template<class MetricToPenalty>
bool EdgePathsBuilderT<MetricToPenalty>::addNextStep_( const VertPathInfo & c )
{
    if ( !( c.metric < FLT_MAX ) )
        return false; // maximal or infinity metric means that this path shall be skipped
    const auto vert = topology_.org( c.back );
    auto & vi = vertPathInfoMap_[vert];
    if ( vi.metric > c.metric )
    {
        vi = c;
        nextSteps_.push( CandidateVert{ vert, metricToPenalty_( c.metric, vert ) } );
        return true;
    }
    return false;
}
 
template<class MetricToPenalty>
bool EdgePathsBuilderT<MetricToPenalty>::addOrgRingSteps( const ReachedVert & rv )
{
    bool aNextStepAdded = false;
    if ( !rv.v )
    {
        assert( !rv.backward );
        return aNextStepAdded;
    }
    const float orgMetric = rv.metric;
    const EdgeId back = rv.backward ? rv.backward : topology_.edgeWithOrg( rv.v );
    for ( EdgeId e : orgRing( topology_, back ) )
    {
        VertPathInfo c;
        c.back = e.sym();
        c.metric = orgMetric + metric_( e );
        aNextStepAdded = addNextStep_( c ) || aNextStepAdded;
    }
    return aNextStepAdded;
}
 
template<class MetricToPenalty>
auto EdgePathsBuilderT<MetricToPenalty>::reachNext() -> ReachedVert
{
    while ( !nextSteps_.empty() )
    {
        const auto c = nextSteps_.top();
        nextSteps_.pop();
        auto & vi = vertPathInfoMap_[c.v];
        if ( metricToPenalty_( vi.metric, c.v ) < c.penalty )
        {
            // shorter path to the vertex was found
            continue;
        }
        assert( metricToPenalty_( vi.metric, c.v ) == c.penalty );
        return { .v = c.v, .backward = vi.back, .penalty = c.penalty, .metric = vi.metric };
    }
    return {};
}
 
template<class MetricToPenalty>
auto EdgePathsBuilderT<MetricToPenalty>::growOneEdge() -> ReachedVert
{
    auto res = reachNext();
    addOrgRingSteps( res );
    return res;
}
 
struct MetricToAStarPenalty
{
    const VertCoords * points = nullptr;
    Vector3f target;
    float operator()( float metric, VertId v ) const
    {
        return metric + ( (*points)[v] - target ).length();
    }
};
 
class EdgePathsAStarBuilder: public EdgePathsBuilderT<MetricToAStarPenalty>
{
public:
    EdgePathsAStarBuilder( const Mesh & mesh, VertId target, VertId start ) :
        EdgePathsBuilderT( mesh.topology, edgeLengthMetric( mesh ) )
    {
        metricToPenalty_.points = &mesh.points;
        metricToPenalty_.target = mesh.points[target];
        addStart( start, 0 );
    }
    EdgePathsAStarBuilder( const Mesh & mesh, const MeshTriPoint & target, const MeshTriPoint & start ) :
        EdgePathsBuilderT( mesh.topology, edgeLengthMetric( mesh ) )
    {
        metricToPenalty_.points = &mesh.points;
        metricToPenalty_.target = mesh.triPoint( target );
        const auto startPt = mesh.triPoint( start );
        mesh.topology.forEachVertex( start, [&]( VertId v )
        {
            addStart( v, ( mesh.points[v] - startPt ).length() );
        } );
    }
};
 
 
} // namespace MR