寻路,判断是否连通
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; }