DrawableRoad.cpp 4.24 KB
Newer Older
Martin Kröning's avatar
Martin Kröning committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#include "DrawableRoad.hpp"
#include "cairomm/context.h"
#include "cairomm/enums.h"
#include "cairomm/refptr.h"
#include <algorithm>
#include <cmath>
#include <ext/alloc_traits.h>
#include <limits>
#include <stdexcept>
#include <utility>

const double DrawableRoad::width = 30;

DrawableRoad::DrawableRoad(std::string laneThereName, std::string laneBackName,
                           std::vector<std::valarray<double>> coordinates)
    : laneThereName(std::move(laneThereName)),
      laneBackName(std::move(laneBackName)), pixelLength(0),
      coordinates(coordinates) {
  auto &lastCoordinatePair = coordinates.front();
  for (auto &coordinatePair : coordinates) {
    auto nextStep = coordinatePair - lastCoordinatePair;
    pixelLength += std::hypot(nextStep[0], nextStep[1]);
    lastCoordinatePair = coordinatePair;
  }
}

void DrawableRoad::draw(const Cairo::RefPtr<Cairo::Context> &cr) const {
  cr->save();

  cr->move_to(coordinates.front()[0], coordinates.front()[1]);
  std::for_each(++coordinates.begin(), coordinates.end(),
                [&cr](const std::valarray<double> &coordinatePair) {
                  cr->line_to(coordinatePair[0], coordinatePair[1]);
                });

  cr->set_line_join(Cairo::LINE_JOIN_ROUND);
  cr->set_line_width(30);
  cr->stroke_preserve();
  cr->set_line_width(2.56);
  cr->set_source_rgb(1, 1, 1);
  cr->stroke();

  cr->restore();
}

void DrawableRoad::getRegion(int &x, int &y, int &width, int &height) const {
  x = y = std::numeric_limits<int>::max();
  width = height = 0;
  for (auto &&coordinatePair : coordinates) {
    const auto currentX = static_cast<int>(
        std::floor(coordinatePair[0] - DrawableRoad::width / 2));
    const auto currentY = static_cast<int>(
        std::floor(coordinatePair[1] - DrawableRoad::width / 2));
    x = std::min(currentX, x);
    y = std::min(currentY, y);
    const auto currentWidth = static_cast<int>(
        std::ceil(coordinatePair[0] + DrawableRoad::width / 2 - x));
    const auto currentHeight = static_cast<int>(
        std::ceil(coordinatePair[1] + DrawableRoad::width / 2 - y));
    width = std::max(currentWidth, width);
    height = std::max(currentHeight, height);
  }
}

void DrawableRoad::getCoordinatesOf(const std::string &laneName,
                                    const double positionOnLane, double &x,
                                    double &y) const {
  if (positionOnLane < 0 || positionOnLane > 1) {
    throw std::invalid_argument("positionOnLane is invalid: " +
                                std::to_string(positionOnLane));
  }

  double currentPixelDistance = 0;
  auto currentCoordinatePair =
      laneName == laneThereName ? coordinates.front() : coordinates.back();
  auto nextCoordinatePair = currentCoordinatePair;
  const auto advanceTraversalAndFindNextStep =
      [this, &currentPixelDistance, &currentCoordinatePair,
       positionOnLane](std::valarray<double> coordinatePair) {
        auto nextStep = coordinatePair - currentCoordinatePair;
        auto nextStepLength = std::hypot(nextStep[0], nextStep[1]);
        auto nextStepIsTooFar = currentPixelDistance + nextStepLength >
                                positionOnLane * pixelLength;
        if (!nextStepIsTooFar) {
          currentPixelDistance += nextStepLength;
          currentCoordinatePair = coordinatePair;
        }
        return nextStepIsTooFar;
      };
  if (laneName == laneThereName) {
    nextCoordinatePair = *std::find_if(++coordinates.begin(), coordinates.end(),
                                       advanceTraversalAndFindNextStep);
  } else {
    nextCoordinatePair =
        *std::find_if(++coordinates.rbegin(), coordinates.rend(),
                      advanceTraversalAndFindNextStep);
  }
  const auto nextStep = nextCoordinatePair - currentCoordinatePair;
  const auto nextStepLength = std::hypot(nextStep[0], nextStep[1]);
  auto offset = std::valarray<double>({-nextStep[1], nextStep[0]});
  offset = offset / nextStepLength * 7.5;
  currentCoordinatePair +=
      (positionOnLane * pixelLength - currentPixelDistance) / nextStepLength *
      nextStep;
  currentCoordinatePair += offset;
  x = currentCoordinatePair[0];
  y = currentCoordinatePair[1];
}

bool DrawableRoad::contains(const std::string &laneName) const {
  return laneThereName == laneName || laneBackName == laneName;
}