1// Copyright 2005 Google Inc. All Rights Reserved.
2
3#include <set>
4using std::set;
5using std::multiset;
6
7#include <vector>
8using std::vector;
9
10// #include "base/commandlineflags.h"
11#include "base/logging.h"
12#include "util/math/matrix3x3-inl.h"
13#include "s2polyline.h"
14
15#include "util/coding/coder.h"
16#include "s2cap.h"
17#include "s2cell.h"
18#include "s2latlng.h"
19#include "s2edgeutil.h"
20
21// DECLARE_bool(s2debug); // defined in s2.cc
22
23static const unsigned char kCurrentEncodingVersionNumber = 1;
24
25S2Polyline::S2Polyline()
26 : num_vertices_(0),
27 vertices_(NULL) {
28}
29
30S2Polyline::S2Polyline(vector<S2Point> const& vertices)
31 : num_vertices_(0),
32 vertices_(NULL) {
33 Init(vertices);
34}
35
36S2Polyline::S2Polyline(vector<S2LatLng> const& vertices)
37 : num_vertices_(0),
38 vertices_(NULL) {
39 Init(vertices);
40}
41
42S2Polyline::~S2Polyline() {
43 delete[] vertices_;
44}
45
46void S2Polyline::Init(vector<S2Point> const& vertices) {
47 if (S2::debug) CHECK(IsValid(vertices));
48
49 delete[] vertices_;
50 num_vertices_ = vertices.size();
51 vertices_ = new S2Point[num_vertices_];
52 // Check (num_vertices_ > 0) to avoid invalid reference to vertices[0].
53 if (num_vertices_ > 0) {
54 memcpy(vertices_, &vertices[0], num_vertices_ * sizeof(vertices_[0]));
55 }
56}
57
58void S2Polyline::Init(vector<S2LatLng> const& vertices) {
59 delete[] vertices_;
60 num_vertices_ = vertices.size();
61 vertices_ = new S2Point[num_vertices_];
62 for (int i = 0; i < num_vertices_; ++i) {
63 vertices_[i] = vertices[i].ToPoint();
64 }
65 // if (FLAGS_s2debug) {
66 if (S2::debug) {
67 vector<S2Point> vertex_vector(vertices_, vertices_ + num_vertices_);
68 CHECK(IsValid(vertex_vector));
69 }
70}
71
72bool S2Polyline::IsValid(vector<S2Point> const& v) {
73 // All vertices must be unit length.
74 int n = v.size();
75 for (int i = 0; i < n; ++i) {
76 if (!S2::IsUnitLength(v[i])) {
77 LOG(INFO) << "Vertex " << i << " is not unit length";
78 return false;
79 }
80 }
81
82 // Adjacent vertices must not be identical or antipodal.
83 for (int i = 1; i < n; ++i) {
84 if (v[i-1] == v[i] || v[i-1] == -v[i]) {
85 LOG(INFO) << "Vertices " << (i - 1) << " and " << i
86 << " are identical or antipodal";
87 return false;
88 }
89 }
90 return true;
91}
92
93
94S2Polyline::S2Polyline(S2Polyline const* src)
95 : num_vertices_(src->num_vertices_),
96 vertices_(new S2Point[num_vertices_]) {
97 memcpy(vertices_, src->vertices_, num_vertices_ * sizeof(vertices_[0]));
98}
99
100S2Polyline* S2Polyline::Clone() const {
101 return new S2Polyline(this);
102}
103
104S1Angle S2Polyline::GetLength() const {
105 S1Angle length;
106 for (int i = 1; i < num_vertices(); ++i) {
107 length += S1Angle(vertex(i-1), vertex(i));
108 }
109 return length;
110}
111
112S2Point S2Polyline::GetCentroid() const {
113 S2Point centroid;
114 for (int i = 1; i < num_vertices(); ++i) {
115 // The centroid (multiplied by length) is a vector toward the midpoint
116 // of the edge, whose length is twice the sin of half the angle between
117 // the two vertices. Defining theta to be this angle, we have:
118 S2Point vsum = vertex(i-1) + vertex(i); // Length == 2*cos(theta)
119 S2Point vdiff = vertex(i-1) - vertex(i); // Length == 2*sin(theta)
120 double cos2 = vsum.Norm2();
121 double sin2 = vdiff.Norm2();
122 DCHECK_GT(cos2, 0); // Otherwise edge is undefined, and result is NaN.
123 centroid += sqrt(sin2 / cos2) * vsum; // Length == 2*sin(theta)
124 }
125 return centroid;
126}
127
128S2Point S2Polyline::GetSuffix(double fraction, int* next_vertex) const {
129 DCHECK_GT(num_vertices(), 0);
130 // We intentionally let the (fraction >= 1) case fall through, since
131 // we need to handle it in the loop below in any case because of
132 // possible roundoff errors.
133 if (fraction <= 0) {
134 *next_vertex = 1;
135 return vertex(0);
136 }
137 S1Angle length_sum;
138 for (int i = 1; i < num_vertices(); ++i) {
139 length_sum += S1Angle(vertex(i-1), vertex(i));
140 }
141 S1Angle target = fraction * length_sum;
142 for (int i = 1; i < num_vertices(); ++i) {
143 S1Angle length(vertex(i-1), vertex(i));
144 if (target < length) {
145 // This interpolates with respect to arc length rather than
146 // straight-line distance, and produces a unit-length result.
147 S2Point result = S2EdgeUtil::InterpolateAtDistance(target, vertex(i-1),
148 vertex(i), length);
149 // It is possible that (result == vertex(i)) due to rounding errors.
150 *next_vertex = (result == vertex(i)) ? (i + 1) : i;
151 return result;
152 }
153 target -= length;
154 }
155 *next_vertex = num_vertices();
156 return vertex(num_vertices() - 1);
157}
158
159S2Point S2Polyline::Interpolate(double fraction) const {
160 int next_vertex;
161 return GetSuffix(fraction, &next_vertex);
162}
163
164double S2Polyline::UnInterpolate(S2Point const& point, int next_vertex) const {
165 DCHECK_GT(num_vertices(), 0);
166 if (num_vertices() < 2) {
167 return 0;
168 }
169 S1Angle length_sum;
170 for (int i = 1; i < next_vertex; ++i) {
171 length_sum += S1Angle(vertex(i-1), vertex(i));
172 }
173 S1Angle length_to_point = length_sum + S1Angle(vertex(next_vertex-1), point);
174 for (int i = next_vertex; i < num_vertices(); ++i) {
175 length_sum += S1Angle(vertex(i-1), vertex(i));
176 }
177 // The ratio can be greater than 1.0 due to rounding errors or because the
178 // point is not exactly on the polyline.
179 return min(1.0, length_to_point / length_sum);
180}
181
182S2Point S2Polyline::Project(S2Point const& point, int* next_vertex) const {
183 DCHECK_GT(num_vertices(), 0);
184
185 if (num_vertices() == 1) {
186 // If there is only one vertex, it is always closest to any given point.
187 *next_vertex = 1;
188 return vertex(0);
189 }
190
191 // Initial value larger than any possible distance on the unit sphere.
192 S1Angle min_distance = S1Angle::Radians(10);
193 int min_index = -1;
194
195 // Find the line segment in the polyline that is closest to the point given.
196 for (int i = 1; i < num_vertices(); ++i) {
197 S1Angle distance_to_segment = S2EdgeUtil::GetDistance(point, vertex(i-1),
198 vertex(i));
199 if (distance_to_segment < min_distance) {
200 min_distance = distance_to_segment;
201 min_index = i;
202 }
203 }
204 DCHECK_NE(min_index, -1);
205
206 // Compute the point on the segment found that is closest to the point given.
207 S2Point closest_point = S2EdgeUtil::GetClosestPoint(
208 point, vertex(min_index-1), vertex(min_index));
209
210 *next_vertex = min_index + (closest_point == vertex(min_index) ? 1 : 0);
211 return closest_point;
212}
213
214bool S2Polyline::IsOnRight(S2Point const& point) const {
215 DCHECK_GE(num_vertices(), 2);
216
217 int next_vertex;
218 S2Point closest_point = Project(point, &next_vertex);
219
220 DCHECK_GE(next_vertex, 1);
221 DCHECK_LE(next_vertex, num_vertices());
222
223 // If the closest point C is an interior vertex of the polyline, let B and D
224 // be the previous and next vertices. The given point P is on the right of
225 // the polyline (locally) if B, P, D are ordered CCW around vertex C.
226 if (closest_point == vertex(next_vertex-1) && next_vertex > 1 &&
227 next_vertex < num_vertices()) {
228 if (point == vertex(next_vertex-1))
229 return false; // Polyline vertices are not on the RHS.
230 return S2::OrderedCCW(vertex(next_vertex-2), point, vertex(next_vertex),
231 vertex(next_vertex-1));
232 }
233
234 // Otherwise, the closest point C is incident to exactly one polyline edge.
235 // We test the point P against that edge.
236 if (next_vertex == num_vertices())
237 --next_vertex;
238
239 return S2::RobustCCW(point, vertex(next_vertex), vertex(next_vertex-1)) > 0;
240}
241
242bool S2Polyline::Intersects(S2Polyline const* line) const {
243 if (num_vertices() <= 0 || line->num_vertices() <= 0) {
244 return false;
245 }
246
247 if (!GetRectBound().Intersects(line->GetRectBound())) {
248 return false;
249 }
250
251 // TODO(user) look into S2EdgeIndex to make this near linear in performance.
252 for (int i = 1; i < num_vertices(); ++i) {
253 S2EdgeUtil::EdgeCrosser crosser(
254 &vertex(i - 1), &vertex(i), &line->vertex(0));
255 for (int j = 1; j < line->num_vertices(); ++j) {
256 if (crosser.RobustCrossing(&line->vertex(j)) >= 0) {
257 return true;
258 }
259 }
260 }
261 return false;
262}
263
264void S2Polyline::Reverse() {
265 reverse(vertices_, vertices_ + num_vertices_);
266}
267
268S2LatLngRect S2Polyline::GetRectBound() const {
269 S2EdgeUtil::RectBounder bounder;
270 for (int i = 0; i < num_vertices(); ++i) {
271 bounder.AddPoint(&vertex(i));
272 }
273 return bounder.GetBound();
274}
275
276S2Cap S2Polyline::GetCapBound() const {
277 return GetRectBound().GetCapBound();
278}
279
280bool S2Polyline::MayIntersect(S2Cell const& cell) const {
281 if (num_vertices() == 0) return false;
282
283 // We only need to check whether the cell contains vertex 0 for correctness,
284 // but these tests are cheap compared to edge crossings so we might as well
285 // check all the vertices.
286 for (int i = 0; i < num_vertices(); ++i) {
287 if (cell.Contains(vertex(i))) return true;
288 }
289 S2Point cell_vertices[4];
290 for (int i = 0; i < 4; ++i) {
291 cell_vertices[i] = cell.GetVertex(i);
292 }
293 for (int j = 0; j < 4; ++j) {
294 S2EdgeUtil::EdgeCrosser crosser(&cell_vertices[j], &cell_vertices[(j+1)&3],
295 &vertex(0));
296 for (int i = 1; i < num_vertices(); ++i) {
297 if (crosser.RobustCrossing(&vertex(i)) >= 0) {
298 // There is a proper crossing, or two vertices were the same.
299 return true;
300 }
301 }
302 }
303 return false;
304}
305
306void S2Polyline::Encode(Encoder* const encoder) const {
307 encoder->Ensure(num_vertices_ * sizeof(*vertices_) + 10); // sufficient
308
309 encoder->put8(kCurrentEncodingVersionNumber);
310 encoder->put32(num_vertices_);
311 encoder->putn(vertices_, sizeof(*vertices_) * num_vertices_);
312
313 DCHECK_GE(encoder->avail(), 0);
314}
315
316bool S2Polyline::Decode(Decoder* const decoder) {
317 unsigned char version = decoder->get8();
318 if (version > kCurrentEncodingVersionNumber) return false;
319
320 num_vertices_ = decoder->get32();
321 delete[] vertices_;
322 vertices_ = new S2Point[num_vertices_];
323 decoder->getn(vertices_, num_vertices_ * sizeof(*vertices_));
324
325 // if (FLAGS_s2debug) {
326 if (S2::debug) {
327 vector<S2Point> vertex_vector(vertices_, vertices_ + num_vertices_);
328 CHECK(IsValid(vertex_vector));
329 }
330
331 return decoder->avail() >= 0;
332}
333
334namespace {
335
336// Given a polyline, a tolerance distance, and a start index, this function
337// returns the maximal end index such that the line segment between these two
338// vertices passes within "tolerance" of all interior vertices, in order.
339int FindEndVertex(S2Polyline const& polyline,
340 S1Angle const& tolerance, int index) {
341 DCHECK_GE(tolerance.radians(), 0);
342 DCHECK_LT((index + 1), polyline.num_vertices());
343
344 // The basic idea is to keep track of the "pie wedge" of angles from the
345 // starting vertex such that a ray from the starting vertex at that angle
346 // will pass through the discs of radius "tolerance" centered around all
347 // vertices processed so far.
348
349 // First we define a "coordinate frame" for the tangent and normal spaces
350 // at the starting vertex. Essentially this means picking three
351 // orthonormal vectors X,Y,Z such that X and Y span the tangent plane at
352 // the starting vertex, and Z is "up". We use the coordinate frame to
353 // define a mapping from 3D direction vectors to a one-dimensional "ray
354 // angle" in the range (-Pi, Pi]. The angle of a direction vector is
355 // computed by transforming it into the X,Y,Z basis, and then calculating
356 // atan2(y,x). This mapping allows us to represent a wedge of angles as a
357 // 1D interval. Since the interval wraps around, we represent it as an
358 // S1Interval, i.e. an interval on the unit circle.
359 Matrix3x3_d frame;
360 S2Point const& origin = polyline.vertex(index);
361 S2::GetFrame(origin, &frame);
362
363 // As we go along, we keep track of the current wedge of angles and the
364 // distance to the last vertex (which must be non-decreasing).
365 S1Interval current_wedge = S1Interval::Full();
366 double last_distance = 0;
367
368 for (++index; index < polyline.num_vertices(); ++index) {
369 S2Point const& candidate = polyline.vertex(index);
370 double distance = origin.Angle(candidate);
371
372 // We don't allow simplification to create edges longer than 90 degrees,
373 // to avoid numeric instability as lengths approach 180 degrees. (We do
374 // need to allow for original edges longer than 90 degrees, though.)
375 if (distance > M_PI/2 && last_distance > 0) break;
376
377 // Vertices must be in increasing order along the ray, except for the
378 // initial disc around the origin.
379 if (distance < last_distance && last_distance > tolerance.radians()) break;
380 last_distance = distance;
381
382 // Points that are within the tolerance distance of the origin do not
383 // constrain the ray direction, so we can ignore them.
384 if (distance <= tolerance.radians()) continue;
385
386 // If the current wedge of angles does not contain the angle to this
387 // vertex, then stop right now. Note that the wedge of possible ray
388 // angles is not necessarily empty yet, but we can't continue unless we
389 // are willing to backtrack to the last vertex that was contained within
390 // the wedge (since we don't create new vertices). This would be more
391 // complicated and also make the worst-case running time more than linear.
392 S2Point direction = S2::ToFrame(frame, candidate);
393 double center = atan2(direction.y(), direction.x());
394 if (!current_wedge.Contains(center)) break;
395
396 // To determine how this vertex constrains the possible ray angles,
397 // consider the triangle ABC where A is the origin, B is the candidate
398 // vertex, and C is one of the two tangent points between A and the
399 // spherical cap of radius "tolerance" centered at B. Then from the
400 // spherical law of sines, sin(a)/sin(A) = sin(c)/sin(C), where "a" and
401 // "c" are the lengths of the edges opposite A and C. In our case C is a
402 // 90 degree angle, therefore A = asin(sin(a) / sin(c)). Angle A is the
403 // half-angle of the allowable wedge.
404
405 double half_angle = asin(sin(tolerance.radians()) / sin(distance));
406 S1Interval target = S1Interval::FromPoint(center).Expanded(half_angle);
407 current_wedge = current_wedge.Intersection(target);
408 DCHECK(!current_wedge.is_empty());
409 }
410 // We break out of the loop when we reach a vertex index that can't be
411 // included in the line segment, so back up by one vertex.
412 return index - 1;
413}
414}
415
416void S2Polyline::SubsampleVertices(S1Angle const& tolerance,
417 vector<int>* indices) const {
418 indices->clear();
419 if (num_vertices() == 0) return;
420
421 indices->push_back(0);
422 S1Angle clamped_tolerance = max(tolerance, S1Angle::Radians(0));
423 for (int index = 0; index + 1 < num_vertices(); ) {
424 int next_index = FindEndVertex(*this, clamped_tolerance, index);
425 // Don't create duplicate adjacent vertices.
426 if (vertex(next_index) != vertex(index)) {
427 indices->push_back(next_index);
428 }
429 index = next_index;
430 }
431}
432
433bool S2Polyline::ApproxEquals(S2Polyline const* b, double max_error) const {
434 if (num_vertices() != b->num_vertices()) return false;
435 for (int offset = 0; offset < num_vertices(); ++offset) {
436 if (!S2::ApproxEquals(vertex(offset), b->vertex(offset), max_error)) {
437 return false;
438 }
439 }
440 return true;
441}
442
443namespace {
444// Return the first i > "index" such that the ith vertex of "pline" is not at
445// the same point as the "index"th vertex. Returns pline.num_vertices() if
446// there is no such value of i.
447inline int NextDistinctVertex(S2Polyline const& pline, int index) {
448 S2Point const& initial = pline.vertex(index);
449 do {
450 ++index;
451 } while (index < pline.num_vertices() && pline.vertex(index) == initial);
452 return index;
453}
454
455// This struct represents a search state in the NearlyCoversPolyline algorithm
456// below. See the description of the algorithm for details.
457struct SearchState {
458 inline SearchState(int i_val, int j_val, bool i_in_progress_val)
459 : i(i_val), j(j_val), i_in_progress(i_in_progress_val) {}
460
461 int i;
462 int j;
463 bool i_in_progress;
464};
465} // namespace
466
467namespace std {
468template<>
469struct less<SearchState> {
470 // This operator is needed for storing SearchStates in a set. The ordering
471 // chosen has no special meaning.
472 inline bool operator()(SearchState const& lhs, SearchState const& rhs) const {
473 if (lhs.i < rhs.i) return true;
474 if (lhs.i > rhs.i) return false;
475 if (lhs.j < rhs.j) return true;
476 if (lhs.j > rhs.j) return false;
477 return !lhs.i_in_progress && rhs.i_in_progress;
478 }
479};
480} // namespace std
481
482bool S2Polyline::NearlyCoversPolyline(S2Polyline const& covered,
483 S1Angle const& max_error) const {
484 // NOTE: This algorithm is described assuming that adjacent vertices in a
485 // polyline are never at the same point. That is, the ith and i+1th vertices
486 // of a polyline are never at the same point in space. The implementation
487 // does not make this assumption.
488
489 // DEFINITIONS:
490 // - edge "i" of a polyline is the edge from the ith to i+1th vertex.
491 // - covered_j is a polyline consisting of edges 0 through j of "covered."
492 // - this_i is a polyline consisting of edges 0 through i of this polyline.
493 //
494 // A search state is represented as an (int, int, bool) tuple, (i, j,
495 // i_in_progress). Using the "drive a car" analogy from the header comment, a
496 // search state signifies that you can drive one car along "covered" from its
497 // first vertex through a point on its jth edge, and another car along this
498 // polyline from some point on or before its ith edge to a to a point on its
499 // ith edge, such that no car ever goes backward, and the cars are always
500 // within "max_error" of each other. If i_in_progress is true, it means that
501 // you can definitely drive along "covered" through the jth vertex (beginning
502 // of the jth edge). Otherwise, you can definitely drive along "covered"
503 // through the point on the jth edge of "covered" closest to the ith vertex of
504 // this polyline.
505 //
506 // The algorithm begins by finding all edges of this polyline that are within
507 // "max_error" of the first vertex of "covered," and adding search states
508 // representing all of these possible starting states to the stack of
509 // "pending" states.
510 //
511 // The algorithm proceeds by popping the next pending state,
512 // (i,j,i_in_progress), off of the stack. First it checks to see if that
513 // state represents finding a valid covering of "covered" and returns true if
514 // so. Next, if the state represents reaching the end of this polyline
515 // without finding a successful covering, the algorithm moves on to the next
516 // state in the stack. Otherwise, if state (i+1,j,false) is valid, it is
517 // added to the stack of pending states. Same for state (i,j+1,true).
518 //
519 // We need the stack because when "i" and "j" can both be incremented,
520 // sometimes only one choice leads to a solution. We use a set to keep track
521 // of visited states to avoid duplicating work. With the set, the worst-case
522 // number of states examined is O(n+m) where n = this->num_vertices() and m =
523 // covered.num_vertices(). Without it, the amount of work could be as high as
524 // O((n*m)^2). Using set, the running time is O((n*m) log (n*m)).
525 //
526 // TODO(user): Benchmark this, and see if the set is worth it.
527 vector<SearchState> pending;
528 set<SearchState> done;
529
530 // Find all possible starting states.
531 for (int i = 0, next_i = NextDistinctVertex(*this, 0);
532 next_i < this->num_vertices();
533 i = next_i, next_i = NextDistinctVertex(*this, next_i)) {
534 S2Point closest_point = S2EdgeUtil::GetClosestPoint(
535 covered.vertex(0), this->vertex(i), this->vertex(next_i));
536 if (closest_point != this->vertex(next_i) &&
537 S1Angle(closest_point, covered.vertex(0)) <= max_error) {
538 pending.push_back(SearchState(i, 0, true));
539 }
540 }
541
542 while (!pending.empty()) {
543 SearchState const state = pending.back();
544 pending.pop_back();
545 if (!done.insert(state).second) continue;
546
547 int const next_i = NextDistinctVertex(*this, state.i);
548 int const next_j = NextDistinctVertex(covered, state.j);
549 if (next_j == covered.num_vertices()) {
550 return true;
551 } else if (next_i == this->num_vertices()) {
552 continue;
553 }
554
555 S2Point i_begin, j_begin;
556 if (state.i_in_progress) {
557 j_begin = covered.vertex(state.j);
558 i_begin = S2EdgeUtil::GetClosestPoint(
559 j_begin, this->vertex(state.i), this->vertex(next_i));
560 } else {
561 i_begin = this->vertex(state.i);
562 j_begin = S2EdgeUtil::GetClosestPoint(
563 i_begin, covered.vertex(state.j), covered.vertex(next_j));
564 }
565
566 if (S2EdgeUtil::IsEdgeBNearEdgeA(j_begin, covered.vertex(next_j),
567 i_begin, this->vertex(next_i),
568 max_error)) {
569 pending.push_back(SearchState(next_i, state.j, false));
570 }
571 if (S2EdgeUtil::IsEdgeBNearEdgeA(i_begin, this->vertex(next_i),
572 j_begin, covered.vertex(next_j),
573 max_error)) {
574 pending.push_back(SearchState(state.i, next_j, true));
575 }
576 }
577 return false;
578}
579