snake.cpp 33.4 KB
Newer Older
Valentin Platzgummer's avatar
Valentin Platzgummer committed
1
#include <algorithm>
Valentin Platzgummer's avatar
Valentin Platzgummer committed
2 3 4 5
#include <iostream>

#include "snake.h"

Valentin Platzgummer's avatar
Valentin Platzgummer committed
6
#include <mapbox/geometry.hpp>
7
#include <mapbox/polylabel.hpp>
Valentin Platzgummer's avatar
Valentin Platzgummer committed
8 9 10

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
11 12
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
Valentin Platzgummer's avatar
Valentin Platzgummer committed
13

Valentin Platzgummer's avatar
Valentin Platzgummer committed
14 15 16 17 18 19 20 21
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000

#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"

Valentin Platzgummer's avatar
Valentin Platzgummer committed
22 23
using namespace operations_research;

24
#ifndef NDEBUG
Valentin Platzgummer's avatar
Valentin Platzgummer committed
25
//#define SNAKE_SHOW_TIME
26 27
#endif

Valentin Platzgummer's avatar
Valentin Platzgummer committed
28 29 30
namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;

Valentin Platzgummer's avatar
Valentin Platzgummer committed
31 32
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian)

Valentin Platzgummer's avatar
Valentin Platzgummer committed
33
namespace snake {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
34 35 36 37
//=========================================================================
// Geometry stuff.
//=========================================================================

38 39 40
void polygonCenter(const BoostPolygon &polygon, BoostPoint &center) {
  using namespace mapbox;
  if (polygon.outer().empty())
Valentin Platzgummer's avatar
Valentin Platzgummer committed
41
    return;
42 43 44 45 46
  geometry::polygon<double> p;
  geometry::linear_ring<double> lr1;
  for (size_t i = 0; i < polygon.outer().size(); ++i) {
    geometry::point<double> vertex(polygon.outer()[i].get<0>(),
                                   polygon.outer()[i].get<1>());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
47
    lr1.push_back(vertex);
48 49 50
  }
  p.push_back(lr1);
  geometry::point<double> c = polylabel(p);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
51

52 53
  center.set<0>(c.x);
  center.set<1>(c.y);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
54 55
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
56
bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97
  /*
  Find the minimum-area bounding box of a set of 2D points

  The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
  The first and last points points must be the same, making a closed polygon.
  This program finds the rotation angles of each edge of the convex polygon,
  then tests the area of a bounding box aligned with the unique angles in
  90 degrees of the 1st Quadrant.
  Returns the

  Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
  Results verified using Matlab

  Copyright (c) 2013, David Butterworth, University of Queensland
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:

      * Redistributions of source code must retain the above copyright
        notice, this list of conditions and the following disclaimer.
      * Redistributions in binary form must reproduce the above copyright
        notice, this list of conditions and the following disclaimer in the
        documentation and/or other materials provided with the distribution.
      * Neither the name of the Willow Garage, Inc. nor the names of its
        contributors may be used to endorse or promote products derived from
        this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.
  */

Valentin Platzgummer's avatar
Valentin Platzgummer committed
98 99
  if (polygon.outer().empty() || polygon.outer().size() < 3)
    return false;
100 101 102 103 104 105 106
  BoostPolygon convex_hull;
  bg::convex_hull(polygon, convex_hull);

  // cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;

  //# Compute edges (x2-x1,y2-y1)
  std::vector<BoostPoint> edges;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
107
  const auto &convex_hull_outer = convex_hull.outer();
108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201
  for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) {
    BoostPoint p1 = convex_hull_outer.at(i);
    BoostPoint p2 = convex_hull_outer.at(i + 1);
    double edge_x = p2.get<0>() - p1.get<0>();
    double edge_y = p2.get<1>() - p1.get<1>();
    edges.push_back(BoostPoint{edge_x, edge_y});
  }

  //    cout << "Edges: ";
  //    for (auto e : edges)
  //        cout << e.get<0>() << " " << e.get<1>() << ",";
  //    cout << endl;

  // Calculate unique edge angles  atan2(y/x)
  double angle_scale = 1e3;
  std::set<long> angles_long;
  for (auto vertex : edges) {
    double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2);
    angle =
        angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers
    angles_long.insert(long(round(angle * angle_scale)));
  }
  std::vector<double> edge_angles;
  for (auto a : angles_long)
    edge_angles.push_back(double(a) / angle_scale);

  //    cout << "Unique angles: ";
  //    for (auto e : edge_angles)
  //        cout << e*180/M_PI << ",";
  //    cout << endl;

  double min_area = std::numeric_limits<double>::infinity();
  // Test each angle to find bounding box with smallest area
  // print "Testing", len(edge_angles), "possible rotations for bounding box...
  // \n"
  for (double angle : edge_angles) {

    trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle * 180 /
                                                               M_PI);
    BoostPolygon hull_rotated;
    bg::transform(convex_hull, hull_rotated, rotate);
    // cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated)
    // << endl;

    bg::model::box<BoostPoint> box;
    bg::envelope(hull_rotated, box);
    //        cout << "Bounding box: " <<
    //        bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;

    //# print "Rotated hull points are \n", rot_points
    BoostPoint min_corner = box.min_corner();
    BoostPoint max_corner = box.max_corner();
    double min_x = min_corner.get<0>();
    double max_x = max_corner.get<0>();
    double min_y = min_corner.get<1>();
    double max_y = max_corner.get<1>();
    //        cout << "min_x: " << min_x << endl;
    //        cout << "max_x: " << max_x << endl;
    //        cout << "min_y: " << min_y << endl;
    //        cout << "max_y: " << max_y << endl;

    // Calculate height/width/area of this bounding rectangle
    double width = max_x - min_x;
    double height = max_y - min_y;
    double area = width * height;
    //        cout << "Width: " << width << endl;
    //        cout << "Height: " << height << endl;
    //        cout << "area: " << area << endl;
    //        cout << "angle: " << angle*180/M_PI << endl;

    // Store the smallest rect found first (a simple convex hull might have 2
    // answers with same area)
    if (area < min_area) {
      min_area = area;
      minBBox.angle = angle;
      minBBox.width = width;
      minBBox.height = height;

      minBBox.corners.clear();
      minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
      minBBox.corners.outer().push_back(BoostPoint{min_x, max_y});
      minBBox.corners.outer().push_back(BoostPoint{max_x, max_y});
      minBBox.corners.outer().push_back(BoostPoint{max_x, min_y});
      minBBox.corners.outer().push_back(BoostPoint{min_x, min_y});
    }
    // cout << endl << endl;
  }

  // Transform corners of minimal bounding box.
  trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-minBBox.angle *
                                                             180 / M_PI);
  BoostPolygon rotated_polygon;
  bg::transform(minBBox.corners, rotated_polygon, rotate);
  minBBox.corners = rotated_polygon;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
202 203

  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
204 205
}

206 207 208 209 210 211 212
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
                   double offset) {
  bg::strategy::buffer::distance_symmetric<double> distance_strategy(offset);
  bg::strategy::buffer::join_miter join_strategy(3);
  bg::strategy::buffer::end_flat end_strategy;
  bg::strategy::buffer::point_square point_strategy;
  bg::strategy::buffer::side_straight side_strategy;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
213

214
  bg::model::multi_polygon<BoostPolygon> result;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
215

216 217
  bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy,
             end_strategy, point_strategy);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
218

219 220
  if (result.size() > 0)
    polygonOffset = result[0];
Valentin Platzgummer's avatar
Valentin Platzgummer committed
221 222
}

223 224 225
void graphFromPolygon(const BoostPolygon &polygon,
                      const BoostLineString &vertices, Matrix<double> &graph) {
  size_t n = graph.getN();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
226

227 228 229 230 231
  for (size_t i = 0; i < n; ++i) {
    BoostPoint v1 = vertices[i];
    for (size_t j = i + 1; j < n; ++j) {
      BoostPoint v2 = vertices[j];
      BoostLineString path{v1, v2};
Valentin Platzgummer's avatar
Valentin Platzgummer committed
232

233 234 235 236 237
      double distance = 0;
      if (!bg::within(path, polygon))
        distance = std::numeric_limits<double>::infinity();
      else
        distance = bg::length(path);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
238

239 240 241 242
      graph.set(i, j, distance);
      graph.set(j, i, distance);
    }
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
243 244
}

245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321
bool dijkstraAlgorithm(
    const size_t numElements, size_t startIndex, size_t endIndex,
    std::vector<size_t> &elementPath,
    std::function<double(const size_t, const size_t)> distanceDij) {
  if (startIndex >= numElements || endIndex >= numElements ||
      endIndex == startIndex) {
    return false;
  }
  // Node struct
  // predecessorIndex is the index of the predecessor node
  // (nodeList[predecessorIndex]) distance is the distance between the node and
  // the start node node number is stored by the position in nodeList
  struct Node {
    int predecessorIndex = -1;
    double distance = std::numeric_limits<double>::infinity();
  };

  // The list with all Nodes (elements)
  std::vector<Node> nodeList(numElements);
  // This list will be initalized with indices referring to the elements of
  // nodeList. Elements will be successively remove during the execution of the
  // Dijkstra Algorithm.
  std::vector<size_t> workingSet(numElements);

  // append elements to node list
  for (size_t i = 0; i < numElements; ++i)
    workingSet[i] = i;

  nodeList[startIndex].distance = 0;

  // Dijkstra Algorithm
  // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
  while (workingSet.size() > 0) {
    // serach Node with minimal distance
    double minDist = std::numeric_limits<double>::infinity();
    int minDistIndex_WS = -1; // WS = workinSet
    for (size_t i = 0; i < workingSet.size(); ++i) {
      const int nodeIndex = workingSet.at(i);
      const double dist = nodeList.at(nodeIndex).distance;
      if (dist < minDist) {
        minDist = dist;
        minDistIndex_WS = i;
      }
    }
    if (minDistIndex_WS == -1)
      return false;

    size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList
    workingSet.erase(workingSet.begin() + minDistIndex_WS);
    if (indexU_NL == endIndex) // shortest path found
      break;

    const double distanceU = nodeList.at(indexU_NL).distance;
    // update distance
    for (size_t i = 0; i < workingSet.size(); ++i) {
      int indexV_NL = workingSet[i]; // NL = nodeList
      Node *v = &nodeList[indexV_NL];
      double dist = distanceDij(indexU_NL, indexV_NL);
      // is ther an alternative path which is shorter?
      double alternative = distanceU + dist;
      if (alternative < v->distance) {
        v->distance = alternative;
        v->predecessorIndex = indexU_NL;
      }
    }
  }
  // end Djikstra Algorithm

  // reverse assemble path
  int e = endIndex;
  while (1) {
    if (e == -1) {
      if (elementPath[0] == startIndex) // check if starting point was reached
        break;
      return false;
    }
    elementPath.insert(elementPath.begin(), e);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
322

323 324 325 326
    // Update Node
    e = nodeList[e].predecessorIndex;
  }
  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
327
}
Valentin Platzgummer's avatar
Valentin Platzgummer committed
328

329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363
void toDistanceMatrix(Matrix<double> &graph) {
  size_t n = graph.getN();

  auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); };

  std::vector<size_t> path;
  for (size_t i = 0; i < n; ++i) {
    for (size_t j = i + 1; j < n; ++j) {
      double d = graph.get(i, j);
      if (!std::isinf(d))
        continue;
      path.clear();
      bool ret = dijkstraAlgorithm(n, i, j, path, distance);
      assert(ret);
      (void)ret;
      //            cout << "(" << i << "," << j << ") d: " << d << endl;
      //            cout << "Path size: " << path.size() << endl;
      //            for (auto idx : path)
      //                cout << idx << " ";
      //            cout << endl;

      d = 0;
      for (long k = 0; k < long(path.size()) - 1; ++k) {
        size_t idx0 = path[k];
        size_t idx1 = path[k + 1];
        double d0 = graph.get(idx0, idx1);
        assert(std::isinf(d0) == false);
        d += d0;
      }

      graph.set(i, j, d);
      graph.set(j, i, d);
    }
  }
}
Valentin Platzgummer's avatar
Valentin Platzgummer committed
364

365 366 367 368 369 370 371 372 373 374 375 376 377
void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
                           size_t endIndex, std::vector<size_t> &pathIdx) {

  if (!std::isinf(graph.get(startIndex, endIndex))) {
    pathIdx.push_back(startIndex);
    pathIdx.push_back(endIndex);
  } else {
    auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); };
    bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx,
                                 distance);
    assert(ret);
    (void)ret;
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
378 379
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
380 381 382 383 384
bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth,
           Area minTileArea, std::vector<BoostPolygon> &tiles,
           BoundingBox &bbox, string &errorString) {
  if (area.outer().empty() || area.outer().size() < 4) {
    errorString = "Area has to few vertices.";
385
    return false;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
386
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
387

Valentin Platzgummer's avatar
Valentin Platzgummer committed
388 389
  if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter ||
      minTileArea < 0 * bu::si::meter * bu::si::meter) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
390
    std::stringstream ss;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
391 392
    ss << "Parameters tileWidth (" << tileWidth << "), tileHeight ("
       << tileHeight << "), minTileArea (" << minTileArea
Valentin Platzgummer's avatar
Valentin Platzgummer committed
393 394
       << ") must be positive.";
    errorString = ss.str();
395 396 397
    return false;
  }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
398 399 400 401
  if (bbox.corners.outer().size() != 5) {
    bbox.corners.clear();
    minimalBoundingBox(area, bbox);
  }
402

Valentin Platzgummer's avatar
Valentin Platzgummer committed
403 404 405 406 407
  if (bbox.corners.outer().size() < 5)
    return false;
  double bboxWidth = bbox.width;
  double bboxHeight = bbox.height;
  BoostPoint origin = bbox.corners.outer()[0];
408 409 410 411

  // cout << "Origin: " << origin[0] << " " << origin[1] << endl;
  // Transform _mArea polygon to bounding box coordinate system.
  trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(
Valentin Platzgummer's avatar
Valentin Platzgummer committed
412
      bbox.angle * 180 / M_PI);
413 414 415 416
  trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
                                                       -origin.get<1>());
  BoostPolygon translated_polygon;
  BoostPolygon rotated_polygon;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
417
  boost::geometry::transform(area, translated_polygon, translate);
418 419 420 421
  boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
  bg::correct(rotated_polygon);
  // cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;

Valentin Platzgummer's avatar
Valentin Platzgummer committed
422 423
  size_t iMax = ceil(bboxWidth / tileWidth.value());
  size_t jMax = ceil(bboxHeight / tileHeight.value());
424 425 426

  if (iMax < 1 || jMax < 1) {
    std::stringstream ss;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
427
    ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight
Valentin Platzgummer's avatar
Valentin Platzgummer committed
428
       << ") to large for measurement area.";
429 430 431 432 433
    errorString = ss.str();
    return false;
  }

  trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
Valentin Platzgummer's avatar
Valentin Platzgummer committed
434
      -bbox.angle * 180 / M_PI);
435 436 437
  trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
                                                            origin.get<1>());
  for (size_t i = 0; i < iMax; ++i) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
438 439
    double x_min = tileWidth.value() * i;
    double x_max = x_min + tileWidth.value();
440
    for (size_t j = 0; j < jMax; ++j) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
441 442
      double y_min = tileHeight.value() * j;
      double y_max = y_min + tileHeight.value();
443 444 445 446 447 448 449 450 451 452 453 454 455 456

      BoostPolygon tile_unclipped;
      tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
      tile_unclipped.outer().push_back(BoostPoint{x_min, y_max});
      tile_unclipped.outer().push_back(BoostPoint{x_max, y_max});
      tile_unclipped.outer().push_back(BoostPoint{x_max, y_min});
      tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});

      std::deque<BoostPolygon> boost_tiles;
      if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
                                         boost_tiles))
        continue;

      for (BoostPolygon t : boost_tiles) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
457
        if (bg::area(t) > minTileArea.value()) {
458 459 460 461 462 463 464 465
          // Transform boost_tile to world coordinate system.
          BoostPolygon rotated_tile;
          BoostPolygon translated_tile;
          boost::geometry::transform(t, rotated_tile, rotate_back);
          boost::geometry::transform(rotated_tile, translated_tile,
                                     translate_back);

          // Store tile and calculate center point.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
466
          tiles.push_back(translated_tile);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
467
        }
468
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
469
    }
470
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
471

Valentin Platzgummer's avatar
Valentin Platzgummer committed
472
  if (tiles.size() < 1) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
473
    std::stringstream ss;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
474
    ss << "No tiles calculated. Is the minTileArea (" << minTileArea
Valentin Platzgummer's avatar
Valentin Platzgummer committed
475 476
       << ") parameter large enough?";
    errorString = ss.str();
477 478
    return false;
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
479

480
  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
481 482
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
483 484 485
bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea,
                const BoostPolygon &corridor, BoostPolygon &jArea,
                std::string &errorString) {
486
  // Measurement area and service area overlapping?
Valentin Platzgummer's avatar
Valentin Platzgummer committed
487 488
  bool overlapingSerMeas = bg::intersects(mArea, sArea) ? true : false;
  bool corridorValid = corridor.outer().size() > 0 ? true : false;
489 490 491 492 493

  // Check if corridor is connecting measurement area and service area.
  bool corridor_is_connection = false;
  if (corridorValid) {
    // Corridor overlaping with measurement area?
Valentin Platzgummer's avatar
Valentin Platzgummer committed
494
    if (bg::intersects(corridor, mArea)) {
495
      // Corridor overlaping with service area?
Valentin Platzgummer's avatar
Valentin Platzgummer committed
496
      if (bg::intersects(corridor, sArea)) {
497 498
        corridor_is_connection = true;
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
499
    }
500 501 502 503
  }

  // Are areas joinable?
  std::deque<BoostPolygon> sol;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
504
  BoostPolygon partialArea = mArea;
505 506
  if (overlapingSerMeas) {
    if (corridor_is_connection) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
507
      bg::union_(partialArea, corridor, sol);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
508
    }
509
  } else if (corridor_is_connection) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
510
    bg::union_(partialArea, corridor, sol);
511
  } else {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
512 513 514 515 516 517
    std::stringstream ss;
    auto printPoint = [&ss](const BoostPoint &p) {
      ss << " (" << p.get<0>() << ", " << p.get<1>() << ")";
    };
    ss << "Areas are not overlapping." << std::endl;
    ss << "Measurement area:";
Valentin Platzgummer's avatar
Valentin Platzgummer committed
518
    bg::for_each_point(mArea, printPoint);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
519 520
    ss << std::endl;
    ss << "Service area:";
Valentin Platzgummer's avatar
Valentin Platzgummer committed
521
    bg::for_each_point(sArea, printPoint);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
522 523
    ss << std::endl;
    ss << "Corridor:";
Valentin Platzgummer's avatar
Valentin Platzgummer committed
524
    bg::for_each_point(corridor, printPoint);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
525 526
    ss << std::endl;
    errorString = ss.str();
527 528 529 530 531 532 533 534 535
    return false;
  }

  if (sol.size() > 0) {
    partialArea = sol[0];
    sol.clear();
  }

  // Join areas.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
536
  bg::union_(partialArea, sArea, sol);
537 538

  if (sol.size() > 0) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
539
    jArea = sol[0];
540
  } else {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
541 542 543 544 545 546 547 548 549 550 551 552 553 554 555
    std::stringstream ss;
    auto printPoint = [&ss](const BoostPoint &p) {
      ss << " (" << p.get<0>() << ", " << p.get<1>() << ")";
    };
    ss << "Areas not joinable." << std::endl;
    ss << "Measurement area:";
    bg::for_each_point(mArea, printPoint);
    ss << std::endl;
    ss << "Service area:";
    bg::for_each_point(sArea, printPoint);
    ss << std::endl;
    ss << "Corridor:";
    bg::for_each_point(corridor, printPoint);
    ss << std::endl;
    errorString = ss.str();
556 557 558 559
    return false;
  }

  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
560 561
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
562 563
bool joinedArea(const std::vector<BoostPolygon *> &areas,
                BoostPolygon &joinedArea) {
564 565
  if (areas.size() < 1)
    return false;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
566
  joinedArea = *areas[0];
567 568 569 570 571 572 573 574
  std::deque<std::size_t> idxList;
  for (size_t i = 1; i < areas.size(); ++i)
    idxList.push_back(i);

  std::deque<BoostPolygon> sol;
  while (idxList.size() > 0) {
    bool success = false;
    for (auto it = idxList.begin(); it != idxList.end(); ++it) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
575
      bg::union_(joinedArea, *areas[*it], sol);
576 577 578 579 580 581 582
      if (sol.size() > 0) {
        joinedArea = sol[0];
        sol.clear();
        idxList.erase(it);
        success = true;
        break;
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
583
    }
584 585 586
    if (!success)
      return false;
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
587

588
  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
589 590
}

591
BoundingBox::BoundingBox() : width(0), height(0), angle(0) {}
Valentin Platzgummer's avatar
Valentin Platzgummer committed
592

593 594 595 596 597
void BoundingBox::clear() {
  width = 0;
  height = 0;
  angle = 0;
  corners.clear();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
598 599
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
600 601 602 603 604
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
                           const BoostPolygon &mArea,
                           const std::vector<BoostPolygon> &tiles,
                           const Progress &p, Transects &t,
                           string &errorString) {
605 606
  // Rotate measurement area by angle and calculate bounding box.
  BoostPolygon mAreaRotated;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
607
  trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630
                                                             180 / M_PI);
  bg::transform(mArea, mAreaRotated, rotate);

  BoostBox box;
  boost::geometry::envelope(mAreaRotated, box);
  double x0 = box.min_corner().get<0>();
  double y0 = box.min_corner().get<1>();
  double x1 = box.max_corner().get<0>();
  double y1 = box.max_corner().get<1>();

  // Generate transects and convert them to clipper path.
  size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects
  vector<ClipperLib::Path> transectsClipper;
  transectsClipper.reserve(num_t);
  for (size_t i = 0; i < num_t; ++i) {
    // calculate transect
    BoostPoint v1{x0, y0 + i * distance.value()};
    BoostPoint v2{x1, y0 + i * distance.value()};
    BoostLineString transect;
    transect.push_back(v1);
    transect.push_back(v2);
    // transform back
    BoostLineString temp_transect;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
631 632
    trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
        -angle.value() * 180 / M_PI);
633
    bg::transform(transect, temp_transect, rotate_back);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
634 635 636 637 638 639 640 641 642
    // to clipper
    ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
                                temp_transect[0].get<0>() * CLIPPER_SCALE),
                            static_cast<ClipperLib::cInt>(
                                temp_transect[0].get<1>() * CLIPPER_SCALE)};
    ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
                                temp_transect[1].get<0>() * CLIPPER_SCALE),
                            static_cast<ClipperLib::cInt>(
                                temp_transect[1].get<1>() * CLIPPER_SCALE)};
643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670
    ClipperLib::Path path{c1, c2};
    transectsClipper.push_back(path);
  }

  if (transectsClipper.size() == 0) {
    std::stringstream ss;
    ss << "Not able to generate transects. Parameter: distance = " << distance
       << std::endl;
    errorString = ss.str();
    return false;
  }

  // Convert measurement area to clipper path.
  ClipperLib::Path mAreaClipper;
  for (auto vertex : mArea.outer()) {
    mAreaClipper.push_back(ClipperLib::IntPoint{
        static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
        static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
  }

  // Perform clipping.
  // Clip transects to measurement area.
  ClipperLib::Clipper clipper;
  clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
  clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
  ClipperLib::PolyTree clippedTransecs;
  clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
                  ClipperLib::pftNonZero, ClipperLib::pftNonZero);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
671
  const auto *transects = &clippedTransecs;
672

Valentin Platzgummer's avatar
Valentin Platzgummer committed
673
  bool ignoreProgress = p.size() != tiles.size();
674 675 676 677 678 679 680 681 682 683
  ClipperLib::PolyTree clippedTransecs2;
  if (!ignoreProgress) {
    // Calculate processed tiles (_progress[i] == 100) and subtract them from
    // measurement area.
    size_t numTiles = p.size();
    vector<BoostPolygon> processedTiles;
    for (size_t i = 0; i < numTiles; ++i) {
      if (p[i] == 100) {
        processedTiles.push_back(tiles[i]);
      }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
684
    }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
685

686 687
    if (processedTiles.size() != numTiles) {
      vector<ClipperLib::Path> processedTilesClipper;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
688
      for (const auto &t : processedTiles) {
689
        ClipperLib::Path path;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
690
        for (const auto &vertex : t.outer()) {
691 692 693
          path.push_back(ClipperLib::IntPoint{
              static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
              static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
Valentin Platzgummer's avatar
Valentin Platzgummer committed
694
        }
695 696 697 698 699
        processedTilesClipper.push_back(path);
      }

      // Subtract holes (tiles with measurement_progress == 100) from transects.
      clipper.Clear();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
700
      for (const auto &child : clippedTransecs.Childs) {
701
        clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
702
      }
703 704 705
      clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
      clipper.Execute(ClipperLib::ctDifference, clippedTransecs2,
                      ClipperLib::pftNonZero, ClipperLib::pftNonZero);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
706 707 708 709
      transects = &clippedTransecs2;
    } else {
      // All tiles processed (t.size() not changed).
      return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
710
    }
711 712 713
  }

  // Extract transects from  PolyTree and convert them to BoostLineString
Valentin Platzgummer's avatar
Valentin Platzgummer committed
714 715
  for (const auto &child : transects->Childs) {
    const auto &clipperTransect = child->Contour;
716 717 718 719 720 721
    BoostPoint v1{static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
                  static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
    BoostPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
                  static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};

    BoostLineString transect{v1, v2};
Valentin Platzgummer's avatar
Valentin Platzgummer committed
722
    if (bg::length(transect) >= minLength.value()) {
723
      t.push_back(transect);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
724
    }
725 726 727 728 729 730 731 732 733 734
  }

  if (t.size() == 0) {
    std::stringstream ss;
    ss << "Not able to generate transects. Parameter: minLength = " << minLength
       << std::endl;
    errorString = ss.str();
    return false;
  }
  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
735 736
}

737 738 739 740
struct RoutingDataModel {
  Matrix<int64_t> distanceMatrix;
  long numVehicles;
  RoutingIndexManager::NodeIndex depot;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
741 742
};

Valentin Platzgummer's avatar
Valentin Platzgummer committed
743
void generateRoutingModel(const BoostLineString &vertices,
744 745
                          const BoostPolygon &polygonOffset, size_t n0,
                          RoutingDataModel &dataModel, Matrix<double> &graph) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
746

Valentin Platzgummer's avatar
Valentin Platzgummer committed
747
#ifdef SNAKE_SHOW_TIME
748
  auto start = std::chrono::high_resolution_clock::now();
749
#endif
750
  graphFromPolygon(polygonOffset, vertices, graph);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
751
#ifdef SNAKE_SHOW_TIME
752 753 754 755
  auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::high_resolution_clock::now() - start);
  cout << "Execution time graphFromPolygon(): " << delta.count() << " ms"
       << endl;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
756
#endif
757
  Matrix<double> distanceMatrix(graph);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
758
#ifdef SNAKE_SHOW_TIME
759
  start = std::chrono::high_resolution_clock::now();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
760
#endif
761
  toDistanceMatrix(distanceMatrix);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
762
#ifdef SNAKE_SHOW_TIME
763 764 765 766
  delta = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::high_resolution_clock::now() - start);
  cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms"
       << endl;
767
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
768

769 770 771 772 773 774 775 776
  dataModel.distanceMatrix.setDimension(n0, n0);
  for (size_t i = 0; i < n0; ++i) {
    dataModel.distanceMatrix.set(i, i, 0);
    for (size_t j = i + 1; j < n0; ++j) {
      dataModel.distanceMatrix.set(
          i, j, int64_t(distanceMatrix.get(i, j) * CLIPPER_SCALE));
      dataModel.distanceMatrix.set(
          j, i, int64_t(distanceMatrix.get(i, j) * CLIPPER_SCALE));
Valentin Platzgummer's avatar
Valentin Platzgummer committed
777
    }
778 779 780
  }
  dataModel.numVehicles = 1;
  dataModel.depot = 0;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
781 782
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
783
bool route(const BoostPolygon &area, const Transects &transects,
Valentin Platzgummer's avatar
Valentin Platzgummer committed
784 785
           std::vector<TransectInfo> &transectInfo, Route &route,
           string &errorString) {
786 787 788 789 790 791 792 793
  //=======================================
  // Route Transects using Google or-tools.
  //=======================================

  // Create vertex list;
  BoostLineString vertices;
  size_t n0 = 0;
  for (const auto &t : transects) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
794
    n0 += std::min<std::size_t>(t.size(), 2);
795 796
  }
  vertices.reserve(n0);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
797

Valentin Platzgummer's avatar
Valentin Platzgummer committed
798 799
  struct LocalInfo {
    LocalInfo(size_t n, bool f) : index(n), front(f) {}
800 801 802
    size_t index;
    bool front;
  };
Valentin Platzgummer's avatar
Valentin Platzgummer committed
803
  std::vector<LocalInfo> localTransectInfo;
804 805 806
  for (size_t i = 0; i < transects.size(); ++i) {
    const auto &t = transects[i];
    vertices.push_back(t.front());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
807
    localTransectInfo.push_back(LocalInfo{i, true});
808 809
    if (t.size() >= 2) {
      vertices.push_back(t.back());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
810
      localTransectInfo.push_back(LocalInfo{i, false});
Valentin Platzgummer's avatar
Valentin Platzgummer committed
811
    }
812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827
  }

  for (long i = 0; i < long(area.outer().size()) - 1; ++i) {
    vertices.push_back(area.outer()[i]);
  }
  for (auto &ring : area.inners()) {
    for (auto &vertex : ring)
      vertices.push_back(vertex);
  }
  size_t n1 = vertices.size();
  // Generate routing model.
  RoutingDataModel dataModel;
  Matrix<double> connectionGraph(n1, n1);
  // Offset joined area.
  BoostPolygon areaOffset;
  offsetPolygon(area, areaOffset, detail::offsetConstant);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
828 829
#ifdef SNAKE_SHOW_TIME
  auto start = std::chrono::high_resolution_clock::now();
830
#endif
831
  generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
832 833
#ifdef SNAKE_SHOW_TIME
  auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
834 835 836
      std::chrono::high_resolution_clock::now() - start);
  cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms"
       << endl;
837
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
838

839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858
  // Create Routing Index Manager.
  RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
                              dataModel.numVehicles, dataModel.depot);
  // Create Routing Model.
  RoutingModel routing(manager);

  // Create and register a transit callback.
  const int transitCallbackIndex = routing.RegisterTransitCallback(
      [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
        // Convert from routing variable Index to distance matrix NodeIndex.
        auto from_node = manager.IndexToNode(from_index).value();
        auto to_node = manager.IndexToNode(to_index).value();
        return dataModel.distanceMatrix.get(from_node, to_node);
      });

  // Define cost of each arc.
  routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);

  // Define Constraints (this constraints have a huge impact on the
  // solving time, improvments could be done, e.g. SearchFilter).
859 860 861
#ifdef SNAKE_DEBUG
  std::cout << "snake::route(): Constraints:" << std::endl;
#endif
862 863 864 865
  Solver *solver = routing.solver();
  size_t index = 0;
  for (size_t i = 0; i < transects.size(); ++i) {
    const auto &t = transects[i];
866 867 868
#ifdef SNAKE_DEBUG
    std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl;
#endif
869 870 871 872 873 874
    if (t.size() >= 2) {
      auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
      auto idx1 =
          manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1));
      auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
      auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
875
      auto c = solver->MakeNonEquality(cond0, cond1);
876
      solver->AddConstraint(c);
877 878 879 880
#ifdef SNAKE_DEBUG
      std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next("
                << index + 1 << ") == " << index << " )" << std::endl;
#endif
881 882 883
      index += 2;
    } else {
      index += 1;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
884
    }
885 886 887 888 889 890 891 892 893 894 895 896
  }

  // Setting first solution heuristic.
  RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
  searchParameters.set_first_solution_strategy(
      FirstSolutionStrategy::PATH_CHEAPEST_ARC);
  google::protobuf::Duration *tMax =
      new google::protobuf::Duration(); // seconds
  tMax->set_seconds(10);
  searchParameters.set_allocated_time_limit(tMax);

  // Solve the problem.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
897
#ifdef SNAKE_SHOW_TIME
898
  start = std::chrono::high_resolution_clock::now();
899
#endif
900
  const Assignment *solution = routing.SolveWithParameters(searchParameters);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
901
#ifdef SNAKE_SHOW_TIME
902 903 904 905
  delta = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::high_resolution_clock::now() - start);
  cout << "Execution time routing.SolveWithParameters(): " << delta.count()
       << " ms" << endl;
906
#endif
Valentin Platzgummer's avatar
Valentin Platzgummer committed
907

908 909 910 911 912 913 914 915 916 917 918
  if (!solution || solution->Size() <= 1) {
    errorString = "Not able to solve the routing problem.";
    return false;
  }

  // Extract index list from solution.
  index = routing.Start(0);
  std::vector<size_t> route_idx;
  route_idx.push_back(manager.IndexToNode(index).value());
  while (!routing.IsEnd(index)) {
    index = solution->Value(routing.NextVar(index));
Valentin Platzgummer's avatar
Valentin Platzgummer committed
919
    route_idx.push_back(manager.IndexToNode(index).value());
920
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
921

922 923 924
  // Helper Lambda.
  auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
                                std::vector<BoostPoint> &path) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
925
    for (auto idx : idxArray)
926 927 928 929 930 931 932
      path.push_back(vertices[idx]);
  };

  // Construct route.
  for (size_t i = 0; i < route_idx.size() - 1; ++i) {
    size_t idx0 = route_idx[i];
    size_t idx1 = route_idx[i + 1];
Valentin Platzgummer's avatar
Valentin Platzgummer committed
933 934
    const auto &info1 = localTransectInfo[idx0];
    const auto &info2 = localTransectInfo[idx1];
935
    if (info1.index == info2.index) { // same transect?
Valentin Platzgummer's avatar
Valentin Platzgummer committed
936 937 938 939
      TransectInfo trInfo(info1.index,
                          !info1.front ? true : false /*transect reversed?*/);
      transectInfo.push_back(trInfo);
      if (!info1.front) { // transect reversal needed?
940 941 942 943 944 945 946 947
        BoostLineString reversedTransect;
        const auto &t = transects[info1.index];
        for (auto it = t.end() - 1; it >= t.begin(); --it) {
          reversedTransect.push_back(*it);
        }
        for (auto it = reversedTransect.begin();
             it < reversedTransect.end() - 1; ++it) {
          route.push_back(*it);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
948
        }
949 950 951 952 953 954 955 956 957 958 959 960 961
      } else {
        const auto &t = transects[info1.index];
        for (auto it = t.begin(); it < t.end() - 1; ++it) {
          route.push_back(*it);
        }
      }
    } else {
      std::vector<size_t> idxList;
      shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
      if (i != route_idx.size() - 2) {
        idxList.pop_back();
      }
      idx2Vertex(idxList, route);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
962
    }
963
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
964
  return true;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
965 966
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
967
} // namespace snake
968 969 970 971 972 973 974 975 976 977

bool boost::geometry::model::operator==(snake::BoostPoint p1,
                                        snake::BoostPoint p2) {
  return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}

bool boost::geometry::model::operator!=(snake::BoostPoint p1,
                                        snake::BoostPoint p2) {
  return !(p1 == p2);
}