← Back to branch summary

~valhalla-routing/+junk/valhalla_2.1.9-0ubuntu1

« back to all changes in this revision

Viewing changes to libvalhalla/src/midgard/util.cc

  • Committer: valhalla
  • Date: 2017-04-24 20:20:53 UTC
  • Revision ID: valhalla@mapzen.com-20170424202053-7o69b9nwx9ee0tw3
Packaging for 2.1.9-0ubuntu1.

Show diffs side-by-side

added added

removed removed

Lines of Context:
libvalhalla/src/midgard/util.cc
 
1
#include "valhalla/midgard/util.h"
 
2
#include "valhalla/midgard/constants.h"
 
3
#include "valhalla/midgard/point2.h"
 
4
#include "valhalla/midgard/distanceapproximator.h"
 
5
 
 
6
#include <cstdint>
 
7
#include <stdlib.h>
 
8
#include <sstream>
 
9
#include <fstream>
 
10
#include <algorithm>
 
11
#include <vector>
 
12
#include <list>
 
13
#include <sys/stat.h>
 
14
 
 
15
namespace {
 
16
 
 
17
constexpr double RAD_PER_METER  = 1.0 / 6378160.187;
 
18
constexpr double RAD_PER_DEG = valhalla::midgard::kPiDouble / 180.0;
 
19
constexpr double DEG_PER_RAD = 180.0 / valhalla::midgard::kPiDouble;
 
20
 
 
21
}
 
22
 
 
23
namespace valhalla {
 
24
namespace midgard {
 
25
 
 
26
int GetTime(const float length, const float speed) {
 
27
  if (speed > 0.0f)
 
28
    return (int)(length / (speed * kHourPerSec) + 0.5f);
 
29
  return 0;
 
30
}
 
31
 
 
32
uint32_t GetTurnDegree(const uint32_t from_heading, const uint32_t to_heading) {
 
33
  return (((to_heading - from_heading) + 360) % 360);
 
34
}
 
35
 
 
36
float rand01() {
 
37
  return (float)rand() / (float)RAND_MAX;
 
38
}
 
39
 
 
40
float FastInvSqrt(float x) {
 
41
  float xhalf = 0.5f * x;
 
42
  int i = *(int*)&x;                 // get bits for floating value
 
43
  i = 0x5f3759df - (i >> 1);         // give initial guess y0
 
44
  x = *(float*)&i;                   // convert bits back to float
 
45
  return x * (1.5f - xhalf * x * x); // newton step
 
46
  // x *= 1.5f - xhalf*x*x;          // repeating step increases accuracy
 
47
}
 
48
 
 
49
// Trim the front of a polyline (represented as a list or vector of Point2).
 
50
// Returns the trimmed portion of the polyline. The supplied polyline is
 
51
// altered (the trimmed part is removed).
 
52
template <class container_t>
 
53
container_t trim_front(container_t& pts, const float dist) {
 
54
  // Return if less than 2 points
 
55
  if (pts.size() < 2) {
 
56
    return {};
 
57
  }
 
58
 
 
59
  // Walk the polyline and accumulate length until it exceeds dist
 
60
  container_t result;
 
61
  result.push_back(pts.front());
 
62
  double d = 0.0f;
 
63
  for (auto p1 = pts.begin(), p2 = std::next(pts.begin()); p2 != pts.end(); ++p1, ++p2) {
 
64
    double segdist = p1->Distance(*p2);
 
65
    if ((d + segdist) > dist) {
 
66
      double frac = (dist - d) / segdist;
 
67
      auto midpoint = p1->AffineCombination((1.0-frac), frac, *p2);
 
68
      result.push_back(midpoint);
 
69
 
 
70
      // Remove used part of polyline
 
71
      pts.erase(pts.begin(), p1);
 
72
      pts.front() = midpoint;
 
73
      return result;
 
74
    } else {
 
75
      d += segdist;
 
76
      result.push_back(*p2);
 
77
    }
 
78
  }
 
79
 
 
80
  // Used all of the polyline without exceeding dist
 
81
  pts.clear();
 
82
  return result;
 
83
}
 
84
 
 
85
// Explicit instantiations
 
86
template std::vector<PointLL> trim_front<std::vector<PointLL> >(std::vector<PointLL>&, const float);
 
87
template std::vector<Point2> trim_front<std::vector<Point2> >(std::vector<Point2>&, const float);
 
88
template std::list<PointLL> trim_front<std::list<PointLL> >(std::list<PointLL>&, const float);
 
89
template std::list<Point2> trim_front<std::list<Point2> >(std::list<Point2>&, const float);
 
90
 
 
91
memory_status::memory_status(const std::unordered_set<std::string> interest){
 
92
  //grab the vm stats from the file
 
93
  std::ifstream file("/proc/self/status");
 
94
  std::string line;
 
95
  while(std::getline(file, line)){
 
96
    //did we find a memory metric
 
97
    if(line.find_first_of("Vm") == 0){
 
98
      //grab the name of it and see if we care about it
 
99
      std::string name = line.substr(0, line.find_first_of(':'));
 
100
      if(interest.size() > 0 && interest.find(name) == interest.end())
 
101
        continue;
 
102
      //try to get the number of bytes
 
103
      std::remove_if(line.begin(), line.end(), [](const char c) {return !std::isdigit(c);});
 
104
      if(line.size() == 0)
 
105
        continue;
 
106
      auto bytes = std::stod(line) * 1024.0;
 
107
      //get the units and scale
 
108
      std::pair<double, std::string> metric = std::make_pair(bytes, "b");
 
109
      for(auto unit : { "B", "KB", "MB", "GB" }){
 
110
        metric.second = unit;
 
111
        if (metric.first > 1024.0)
 
112
          metric.first /= 1024.0;
 
113
        else
 
114
          break;
 
115
      }
 
116
      metrics.emplace(std::piecewise_construct, std::forward_as_tuple(name), std::forward_as_tuple(metric));
 
117
    }
 
118
    line.clear();
 
119
  }
 
120
}
 
121
 
 
122
bool memory_status::supported() {
 
123
  struct stat s;
 
124
  return stat("/proc/self/status", &s) == 0;
 
125
}
 
126
 
 
127
std::ostream& operator<<(std::ostream& stream, const memory_status& s){
 
128
  for(const auto& metric : s.metrics)
 
129
    stream << metric.first << ": " << metric.second.first << metric.second.second << std::endl;
 
130
  return stream;
 
131
}
 
132
 
 
133
/* This method makes use of several computations explained and demonstrated at:
 
134
 * http://williams.best.vwh.net/avform.htm *
 
135
 * We humbly bow to you sir!
 
136
 */
 
137
template <class container_t>
 
138
container_t resample_spherical_polyline(const container_t& polyline, double resolution, bool preserve) {
 
139
  if(polyline.size() == 0)
 
140
    return {};
 
141
 
 
142
  //for each point
 
143
  container_t resampled = {polyline.front()};
 
144
  resolution *= RAD_PER_METER;
 
145
  double remaining = resolution;
 
146
  PointLL last = resampled.back();
 
147
  for(auto p = std::next(polyline.cbegin()); p != polyline.cend(); ++p) {
 
148
    //radians
 
149
    auto lon2 = p->first * -RAD_PER_DEG;
 
150
    auto lat2 = p->second * RAD_PER_DEG;
 
151
    //how much do we have left on this segment from where we are (in great arc radians)
 
152
    //double d = 2.0 * asin(sqrt(pow(sin((resampled.back().second * RAD_PER_DEG - lat2) / 2.0), 2.0) + cos(resampled.back().second * RAD_PER_DEG) * cos(lat2) *pow(sin((resampled.back().first * -RAD_PER_DEG - lon2) / 2.0), 2.0)));
 
153
    auto d = acos(
 
154
      sin(last.second * RAD_PER_DEG) * sin(lat2) +
 
155
      cos(last.second * RAD_PER_DEG) * cos(lat2) *
 
156
      cos(last.first * -RAD_PER_DEG - lon2)
 
157
    );
 
158
    //keep placing points while we can fit them
 
159
    while(d > remaining) {
 
160
      //some precomputed stuff
 
161
      auto lon1 = last.first * -RAD_PER_DEG;
 
162
      auto lat1 = last.second * RAD_PER_DEG;
 
163
      auto sd = sin(d);
 
164
      auto a = sin(d - remaining) / sd;
 
165
      auto acs1 = a * cos(lat1);
 
166
      auto b = sin(remaining) / sd;
 
167
      auto bcs2 = b * cos(lat2);
 
168
      //find the interpolated point along the arc
 
169
      auto x = acs1 * cos(lon1) + bcs2 * cos(lon2);
 
170
      auto y = acs1 * sin(lon1) + bcs2 * sin(lon2);
 
171
      auto z = a * sin(lat1) + b * sin(lat2);
 
172
      last.first = atan2(y, x) * -DEG_PER_RAD;
 
173
      last.second = atan2(z, sqrt(x * x + y * y)) * DEG_PER_RAD;
 
174
      resampled.push_back(last);
 
175
      //we just consumed a bit
 
176
      d -= remaining;
 
177
      //we need another bit
 
178
      remaining = resolution;
 
179
    }
 
180
    //we're going to the next point so consume whatever's left
 
181
    remaining -= d;
 
182
    last = *p;
 
183
    if(preserve)
 
184
      resampled.push_back(last);
 
185
  }
 
186
 
 
187
  //TODO: do we want to let them know remaining?
 
188
 
 
189
  //hand it back
 
190
  return resampled;
 
191
}
 
192
 
 
193
//explicit instantiations
 
194
template std::vector<PointLL> resample_spherical_polyline<std::vector<PointLL> >(const std::vector<PointLL>&, double, bool);
 
195
template std::vector<Point2> resample_spherical_polyline<std::vector<Point2> >(const std::vector<Point2>&, double, bool);
 
196
template std::list<PointLL> resample_spherical_polyline<std::list<PointLL> >(const std::list<PointLL>&, double, bool);
 
197
template std::list<Point2> resample_spherical_polyline<std::list<Point2> >(const std::list<Point2>&, double, bool);
 
198
 
 
199
//Return the intersection of two infinite lines if any
 
200
template <class coord_t>
 
201
bool intersect(const coord_t& u, const coord_t& v, const coord_t& a, const coord_t& b, coord_t& i) {
 
202
  auto uv_xd = u.first - v.first;
 
203
  auto uv_yd = u.second - v.second;
 
204
  auto ab_xd = a.first - b.first;
 
205
  auto ab_yd = a.second - b.second;
 
206
  auto d_cross = uv_xd*ab_yd - ab_xd*uv_yd;
 
207
  //parallel or very close to it
 
208
  if(std::abs(d_cross) < 1e-5)
 
209
    return false;
 
210
  auto uv_cross = u.first*v.second - u.second*v.first;
 
211
  auto ab_cross = a.first*b.second - a.second*b.first;
 
212
  i.first  = (uv_cross*ab_xd - uv_xd*ab_cross) / d_cross;
 
213
  i.second = (uv_cross*ab_yd - uv_yd*ab_cross) / d_cross;
 
214
  return true;
 
215
}
 
216
template bool intersect<PointLL>(const PointLL& u, const PointLL& v, const PointLL& a, const PointLL& b, PointLL& i);
 
217
template bool intersect<Point2>(const Point2& u, const Point2& v, const Point2& a, const Point2& b, Point2& i);
 
218
 
 
219
//Return the intercept of the line passing through uv with the horizontal line defined by y
 
220
template <class coord_t>
 
221
typename coord_t::first_type y_intercept(const coord_t& u, const coord_t& v, const typename coord_t::second_type y) {
 
222
  if(std::abs(u.first - v.first) < 1e-5)
 
223
    return u.first;
 
224
  if(std::abs(u.second - u.second) < 1e-5)
 
225
    return NAN;
 
226
  auto m = (v.second - u.second) / (v.first - u.first);
 
227
  auto b = u.second - (u.first * m);
 
228
  return (y - b) / m;
 
229
}
 
230
template PointLL::first_type y_intercept<PointLL>(const PointLL& u, const PointLL& v, const PointLL::first_type y);
 
231
template Point2::first_type y_intercept<Point2>(const Point2& u, const Point2& v, const Point2::first_type y);
 
232
 
 
233
//Return the intercept of the line passing through uv with the vertical line defined by x
 
234
template <class coord_t>
 
235
typename coord_t::first_type x_intercept(const coord_t& u, const coord_t& v, const typename coord_t::second_type x) {
 
236
  if(std::abs(u.second - v.second) < 1e-5)
 
237
    return u.second;
 
238
  if(std::abs(u.first - v.first) < 1e-5)
 
239
    return NAN;
 
240
  auto m = (v.second - u.second) / (v.first - u.first);
 
241
  auto b = u.second - (u.first * m);
 
242
  return x * m + b;
 
243
}
 
244
template PointLL::second_type x_intercept<PointLL>(const PointLL& u, const PointLL& v, const PointLL::second_type x);
 
245
template Point2::second_type x_intercept<Point2>(const Point2& u, const Point2& v, const Point2::second_type x);
 
246
 
 
247
template <class container_t>
 
248
float polygon_area(const container_t& polygon) {
 
249
  typename container_t::value_type::first_type area = polygon.back() == polygon.front() ? 0.f :
 
250
    (polygon.back().first + polygon.front().first)*(polygon.back().second + polygon.front().second);
 
251
  for(auto p1 = polygon.cbegin(), p2 = std::next(polygon.cbegin()); p2 != polygon.cend(); ++p1, ++p2)
 
252
    area += (p1->first + p2->first)*(p1->second + p2->second);
 
253
  return area*.5;
 
254
}
 
255
 
 
256
template PointLL::first_type polygon_area(const std::list<PointLL>&);
 
257
template PointLL::first_type polygon_area(const std::vector<PointLL>&);
 
258
template Point2::first_type polygon_area(const std::list<Point2>&);
 
259
template Point2::first_type polygon_area(const std::vector<Point2>&);
 
260
 
 
261
}
 
262
}