寻路，判断是否连通

static double MAX_SLOPE = 10000.0;
static double MIN_SLOPE = 0.00001;

void get_y_crossing( double slope, aoi_cell_pos& start_cell, aoi_cell_pos& end_cell, std::set& ret )
{
    double up  = MAX_SLOPE;
    double low = -MAX_SLOPE;
    if( slope >= up || slope <= low )
        return;
    int steps = abs( end_cell.m_cell_x - start_cell.m_cell_x ) + 1;
    int step  = end_cell.m_cell_x < start_cell.m_cell_x ? -1 : 1;
    uint16_t cell_x = start_cell.m_cell_x;
    for( int i = 0; i < steps; i++ )
    {
        if( cell_x )
        {
            double x = cell_x * CELL_SIDE_LEN;
            double y = ( ( x - start_cell.m_cell_x ) * slope ) + start_cell.m_cell_y;
            aoi_point point( x, y );
            ret.insert(point);
        }
        cell_x += step;
    }
}

void get_x_crossing( double slope, aoi_cell_pos& start_cell, aoi_cell_pos& end_cell, std::set& ret )
{
    double up  = MIN_SLOPE;
    double low = -MIN_SLOPE;
    if( slope <= up && slope >= low )
        return;
    int steps = abs( end_cell.m_cell_y - start_cell.m_cell_y ) + 1;
    int step  = end_cell.m_cell_y < start_cell.m_cell_y ? -1 : 1;
    uint16_t cell_y = start_cell.m_cell_y;
    for( int i = 0; i < steps; i++ )
    {
        if( cell_y )
        {
            double y = cell_y * CELL_SIDE_LEN;
            double x = ( ( y - start_cell.m_cell_y ) / slope ) + start_cell.m_cell_x;
            aoi_point point( x, y );
            ret.insert(point);
        }
        cell_y += step;
    }
}

bool aoi_scene::is_through( const aoi_pos& start_pos, const aoi_pos& end_pos )
{
    /// 先检查缓存
    aoi_cell_pos start_cell_pos( start_pos.m_x, start_pos.m_y );
    aoi_cell_pos end_cell_pos( end_pos.m_x, end_pos.m_y );

    /// 终点格子不可达，则直接返回false
    if( !cell_is_passable( end_cell_pos ) )
    {
        SL_DEBUG( "[aoi_scene::is_through]end cell is not passable! " << "cell_x: " << end_cell_pos.m_cell_x << " cell_y: " << end_cell_pos.m_cell_y << "\n" );
        return false;
    }

    /// 获取与所有x,y轴的交叉点
    double slope = 0;
    if( end_pos.m_x != start_pos.m_x )
        slope = double( end_pos.m_y - start_pos.m_y ) / double( end_pos.m_x - start_pos.m_x );
    else
        slope = double( end_pos.m_y - start_pos.m_y ) / MIN_SLOPE;

    std::set all_crossing;
    get_y_crossing( slope, start_cell_pos, end_cell_pos, all_crossing);
    get_x_crossing( slope, start_cell_pos, end_cell_pos, all_crossing );

    /// 不检查起始点
    aoi_point start_point_key( start_cell_pos.m_cell_x, start_cell_pos.m_cell_y );
    std::set::iterator dit = all_crossing.find( start_point_key );
    if( dit != all_crossing.end() )
        all_crossing.erase( dit );

    /// 检验每个交叉点的格子是否可达
    for( std::set::iterator it = all_crossing.begin(); it != all_crossing.end(); ++it )
    {
        aoi_cell_pos cell( (*it).m_x, (*it).m_y );
        SL_DEBUG( "[aoi_scene::is_through]current point: " << " x: " << (*it).m_x << " y: " << (*it).m_y << "\n" );
        if( !cell_is_passable(cell) )
        {
            SL_DEBUG( "[aoi_scene::is_through]point is not passable! " << "point_x: " << (*it).m_x << " point_y: " << (*it).m_y << "\n" );
            SL_DEBUG( "[aoi_scene::is_through]cell is not passable! " << "cell_x: " << cell.m_cell_x << " cell_y: " << cell.m_cell_y << "\n" );
            return false;
        }
    }

    return true;
}

 

转载于:https://www.cnblogs.com/forlove/archive/2013/01/06/2847464.html