寻路，判断是否连通

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<aoi_point>& 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<aoi_point>& 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<aoi_point> 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<aoi_point>::iterator dit = all_crossing.find( start_point_key );

    if( dit != all_crossing.end() )

        all_crossing.erase( dit );



    /// 检验每个交叉点的格子是否可达

    for( std::set<aoi_point>::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;

}