Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AC_Avoidance_config.h"
      17             : 
      18             : #if AP_OAPATHPLANNER_ENABLED
      19             : 
      20             : #include "AP_OAVisGraph.h"
      21             : 
      22             : // constructor initialises expanding array to use 20 elements per chunk
      23          21 : AP_OAVisGraph::AP_OAVisGraph() :
      24          21 :     _items(20)
      25             : {
      26          21 : }
      27             : 
      28             : // add item to visiblity graph, returns true on success, false if graph is full
      29          39 : bool AP_OAVisGraph::add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm)
      30             : {
      31             :     // no more than 65k items
      32          39 :     if (_num_items == UINT16_MAX) {
      33           0 :         return false;
      34             :     }
      35             : 
      36             :     // ensure there is space in the array
      37          39 :     if (!_items.expand_to_hold(_num_items+1)) {
      38           0 :         return false;
      39             :     }
      40             : 
      41             :     // add item
      42          39 :     _items[_num_items] = {id1, id2, distance_cm};
      43          39 :     _num_items++;
      44          39 :     return true;
      45             : }
      46             : 
      47             : #endif  // AP_OAPATHPLANNER_ENABLED