1// Copyright 2006 Google Inc. All Rights Reserved.
2
3#include "s2polygonbuilder.h"
4
5#include <algorithm>
6using std::min;
7using std::max;
8using std::swap;
9using std::reverse;
10
11#include <hash_map>
12using __gnu_cxx::hash_map;
13
14#include <hash_set>
15using __gnu_cxx::hash_set;
16
17#include <iomanip>
18using std::setprecision;
19
20#include <iostream>
21using std::ostream;
22using std::cout;
23using std::endl;
24
25#include <map>
26using std::map;
27using std::multimap;
28
29#include <set>
30using std::set;
31using std::multiset;
32
33#include <vector>
34using std::vector;
35
36
37#include "base/logging.h"
38#include "base/macros.h"
39#include "base/scoped_ptr.h"
40#include "s2.h"
41#include "s2cellid.h"
42#include "s2polygon.h"
43#include "util/math/matrix3x3-inl.h"
44
45void S2PolygonBuilderOptions::set_undirected_edges(bool undirected_edges) {
46 undirected_edges_ = undirected_edges;
47}
48
49void S2PolygonBuilderOptions::set_xor_edges(bool xor_edges) {
50 xor_edges_ = xor_edges;
51}
52
53void S2PolygonBuilderOptions::set_validate(bool validate) {
54 validate_ = validate;
55}
56
57void S2PolygonBuilderOptions::set_vertex_merge_radius(S1Angle const& angle) {
58 vertex_merge_radius_ = angle;
59}
60
61void S2PolygonBuilderOptions::set_edge_splice_fraction(double fraction) {
62 CHECK(fraction < sqrt(3) / 2);
63 edge_splice_fraction_ = fraction;
64}
65
66S2PolygonBuilder::S2PolygonBuilder(S2PolygonBuilderOptions const& options)
67 : options_(options), edges_(new EdgeSet) {
68}
69
70S2PolygonBuilder::~S2PolygonBuilder() {
71}
72
73bool S2PolygonBuilder::HasEdge(S2Point const& v0, S2Point const& v1) {
74 EdgeSet::const_iterator candidates = edges_->find(v0);
75 return (candidates != edges_->end() &&
76 candidates->second.find(v1) != candidates->second.end());
77}
78
79bool S2PolygonBuilder::AddEdge(S2Point const& v0, S2Point const& v1) {
80 // If xor_edges is true, we look for an existing edge in the opposite
81 // direction. We either delete that edge or insert a new one.
82
83 if (v0 == v1) return false;
84 if (options_.xor_edges() && HasEdge(v1, v0)) {
85 EraseEdge(v1, v0);
86 return false;
87 }
88 if (edges_->find(v0) == edges_->end()) starting_vertices_.push_back(v0);
89 (*edges_)[v0].insert(v1);
90 if (options_.undirected_edges()) {
91 if (edges_->find(v1) == edges_->end()) starting_vertices_.push_back(v1);
92 (*edges_)[v1].insert(v0);
93 }
94 return true;
95}
96
97void S2PolygonBuilder::AddLoop(S2Loop const* loop) {
98 int sign = loop->sign();
99 for (int i = loop->num_vertices(); i > 0; --i) {
100 // Vertex indices need to be in the range [0, 2*num_vertices()-1].
101 AddEdge(loop->vertex(i), loop->vertex(i + sign));
102 }
103}
104
105void S2PolygonBuilder::AddPolygon(S2Polygon const* polygon) {
106 for (int i = 0; i < polygon->num_loops(); ++i) {
107 AddLoop(polygon->loop(i));
108 }
109}
110
111void S2PolygonBuilder::EraseEdge(S2Point const& v0, S2Point const& v1) {
112 // Note that there may be more than one copy of an edge if we are not XORing
113 // them, so a VertexSet is a multiset.
114
115 VertexSet* vset = &(*edges_)[v0];
116 DCHECK(vset->find(v1) != vset->end());
117 vset->erase(vset->find(v1));
118 if (vset->empty()) edges_->erase(v0);
119
120 if (options_.undirected_edges()) {
121 vset = &(*edges_)[v1];
122 DCHECK(vset->find(v0) != vset->end());
123 vset->erase(vset->find(v0));
124 if (vset->empty()) edges_->erase(v1);
125 }
126}
127
128void S2PolygonBuilder::set_debug_matrix(Matrix3x3_d const& m) {
129 debug_matrix_.reset(new Matrix3x3_d(m));
130}
131
132void S2PolygonBuilder::DumpVertex(S2Point const& v) const {
133 if (debug_matrix_.get()) {
134 // For orthonormal matrices, Inverse() == Transpose().
135 cout << S2LatLng(debug_matrix_->Transpose() * v);
136 } else {
137 cout << setprecision(17) << v << setprecision(6);
138 }
139}
140
141void S2PolygonBuilder::DumpEdges(S2Point const& v0) const {
142 DumpVertex(v0);
143 cout << ":\n";
144 EdgeSet::const_iterator candidates = edges_->find(v0);
145 if (candidates != edges_->end()) {
146 VertexSet const& vset = candidates->second;
147 for (VertexSet::const_iterator i = vset.begin(); i != vset.end(); ++i) {
148 cout << " ";
149 DumpVertex(*i);
150 cout << "\n";
151 }
152 }
153}
154
155void S2PolygonBuilder::Dump() const {
156 for (EdgeSet::const_iterator i = edges_->begin(); i != edges_->end(); ++i) {
157 DumpEdges(i->first);
158 }
159}
160
161void S2PolygonBuilder::EraseLoop(S2Point const* v, int n) {
162 for (int i = n - 1, j = 0; j < n; i = j++) {
163 EraseEdge(v[i], v[j]);
164 }
165}
166
167void S2PolygonBuilder::RejectLoop(S2Point const* v, int n,
168 EdgeList* unused_edges) {
169 for (int i = n - 1, j = 0; j < n; i = j++) {
170 unused_edges->push_back(make_pair(v[i], v[j]));
171 }
172}
173
174S2Loop* S2PolygonBuilder::AssembleLoop(S2Point const& v0, S2Point const& v1,
175 EdgeList* unused_edges) {
176 // We start at the given edge and assemble a loop taking left turns
177 // whenever possible. We stop the loop as soon as we encounter any
178 // vertex that we have seen before *except* for the first vertex (v0).
179 // This ensures that only CCW loops are constructed when possible.
180
181 vector<S2Point> path; // The path so far.
182 hash_map<S2Point, int> index; // Maps a vertex to its index in "path".
183 path.push_back(v0);
184 path.push_back(v1);
185 index[v1] = 1;
186 while (path.size() >= 2) {
187 // Note that "v0" and "v1" become invalid if "path" is modified.
188 S2Point const& v0 = path.end()[-2];
189 S2Point const& v1 = path.end()[-1];
190 S2Point v2;
191 bool v2_found = false;
192 EdgeSet::const_iterator candidates = edges_->find(v1);
193 if (candidates != edges_->end()) {
194 VertexSet const& vset = candidates->second;
195 for (VertexSet::const_iterator i = vset.begin(); i != vset.end(); ++i) {
196 // We prefer the leftmost outgoing edge, ignoring any reverse edges.
197 if (*i == v0) continue;
198 if (!v2_found || S2::OrderedCCW(v0, v2, *i, v1)) { v2 = *i; }
199 v2_found = true;
200 }
201 }
202 if (!v2_found) {
203 // We've hit a dead end. Remove this edge and backtrack.
204 unused_edges->push_back(make_pair(v0, v1));
205 EraseEdge(v0, v1);
206 index.erase(v1);
207 path.pop_back();
208 } else if (index.insert(make_pair(v2, path.size())).second) {
209 // This is the first time we've visited this vertex.
210 path.push_back(v2);
211 } else {
212 // We've completed a loop. Throw away any initial vertices that
213 // are not part of the loop.
214 path.erase(path.begin(), path.begin() + index[v2]);
215
216 // In the case of undirected edges, we may have assembled a clockwise
217 // loop while trying to assemble a CCW loop. To fix this, we assemble
218 // a new loop starting with an arbitrary edge in the reverse direction.
219 // This is guaranteed to assemble a loop that is interior to the previous
220 // one and will therefore eventually terminate.
221
222 S2Loop* loop = new S2Loop(path);
223 if (options_.validate() && !loop->IsValid()) {
224 // We've constructed a loop that crosses itself, which can only
225 // happen if there is bad input data. Throw away the whole loop.
226 RejectLoop(&path[0], path.size(), unused_edges);
227 EraseLoop(&path[0], path.size());
228 delete loop;
229 return NULL;
230 }
231
232 if (options_.undirected_edges() && !loop->IsNormalized()) {
233 scoped_ptr<S2Loop> deleter(loop); // XXX for debugging
234 return AssembleLoop(path[1], path[0], unused_edges);
235 }
236 return loop;
237 }
238 }
239 return NULL;
240}
241
242class S2PolygonBuilder::PointIndex {
243 // A PointIndex is a cheap spatial index to help us find mergeable
244 // vertices. Given a set of points, it can efficiently find all of the
245 // points within a given search radius of an arbitrary query location.
246 // It is essentially just a hash map from cell ids at a given fixed level to
247 // the set of points contained by that cell id.
248 //
249 // This class is not suitable for general use because it only supports
250 // fixed-radius queries and has various special-purpose operations to avoid
251 // the need for additional data structures.
252
253 private:
254 typedef multimap<S2CellId, S2Point> Map;
255 Map map_;
256
257 double vertex_radius_;
258 double edge_fraction_;
259 int level_;
260 vector<S2CellId> ids_; // Allocated here for efficiency.
261
262 public:
263 PointIndex(double vertex_radius, double edge_fraction)
264 : vertex_radius_(vertex_radius),
265 edge_fraction_(edge_fraction),
266 // We compute an S2CellId level such that the vertex neighbors at that
267 // level of any point A are a covering for spherical cap (i.e. "disc")
268 // of the given search radius centered at A. This requires that the
269 // minimum cell width at that level must be twice the search radius.
270 level_(min(S2::kMinWidth.GetMaxLevel(2 * vertex_radius),
271 S2CellId::kMaxLevel - 1)) {
272 // We insert a sentinel so that we don't need to test for map_.end().
273 map_.insert(make_pair(S2CellId::Sentinel(), S2Point()));
274 }
275
276 void Insert(S2Point const& p) {
277 S2CellId::FromPoint(p).AppendVertexNeighbors(level_, &ids_);
278 for (int i = ids_.size(); --i >= 0; ) {
279 map_.insert(make_pair(ids_[i], p));
280 }
281 ids_.clear();
282 }
283
284 void Erase(S2Point const& p) {
285 S2CellId::FromPoint(p).AppendVertexNeighbors(level_, &ids_);
286 for (int i = ids_.size(); --i >= 0; ) {
287 Map::iterator j = map_.lower_bound(ids_[i]);
288 for (; j->second != p; ++j) {
289 DCHECK_EQ(ids_[i], j->first);
290 }
291 map_.erase(j);
292 }
293 ids_.clear();
294 }
295
296 void QueryCap(S2Point const& axis, vector<S2Point>* output) {
297 // Return the set the points whose distance to "axis" is less than
298 // vertex_radius_.
299
300 output->clear();
301 S2CellId id = S2CellId::FromPoint(axis).parent(level_);
302 for (Map::const_iterator i = map_.lower_bound(id); i->first == id; ++i) {
303 S2Point const& p = i->second;
304 if (axis.Angle(p) < vertex_radius_) {
305 output->push_back(p);
306 }
307 }
308 }
309
310 bool FindNearbyPoint(S2Point const& v0, S2Point const& v1,
311 S2Point* nearby) {
312 // Return a point whose distance from the edge (v0,v1) is less than
313 // vertex_radius_, and which is not equal to v0 or v1. The current
314 // implementation returns the closest such point.
315 //
316 // Strategy: we compute a very cheap covering by approximating the edge as
317 // two spherical caps, one around each endpoint, and then computing a
318 // 4-cell covering of each one. We could improve the quality of the
319 // covering by using some intermediate points along the edge as well.
320
321 double length = v0.Angle(v1);
322 S2Point normal = S2::RobustCrossProd(v0, v1);
323 int level = min(level_, S2::kMinWidth.GetMaxLevel(length));
324 S2CellId::FromPoint(v0).AppendVertexNeighbors(level, &ids_);
325 S2CellId::FromPoint(v1).AppendVertexNeighbors(level, &ids_);
326
327 // Sort the cell ids so that we can skip duplicates in the loop below.
328 sort(ids_.begin(), ids_.end());
329
330 double best_dist = 2 * vertex_radius_;
331 for (int i = ids_.size(); --i >= 0; ) {
332 if (i > 0 && ids_[i-1] == ids_[i]) continue; // Skip duplicates.
333
334 S2CellId const& max_id = ids_[i].range_max();
335 for (Map::const_iterator j = map_.lower_bound(ids_[i].range_min());
336 j->first <= max_id; ++j) {
337 S2Point const& p = j->second;
338 if (p == v0 || p == v1) continue;
339 double dist = S2EdgeUtil::GetDistance(p, v0, v1, normal).radians();
340 if (dist < best_dist) {
341 best_dist = dist;
342 *nearby = p;
343 }
344 }
345 }
346 ids_.clear();
347 return (best_dist < edge_fraction_ * vertex_radius_);
348 }
349
350 private:
351 DISALLOW_EVIL_CONSTRUCTORS(PointIndex);
352};
353
354void S2PolygonBuilder::BuildMergeMap(PointIndex* index, MergeMap* merge_map) {
355 // The overall strategy is to start from each vertex and grow a maximal
356 // cluster of mergeable vertices. In graph theoretic terms, we find the
357 // connected components of the undirected graph whose edges connect pairs of
358 // vertices that are separated by at most vertex_merge_radius().
359 //
360 // We then choose a single representative vertex for each cluster, and
361 // update "merge_map" appropriately. We choose an arbitrary existing
362 // vertex rather than computing the centroid of all the vertices to avoid
363 // creating new vertex pairs that need to be merged. (We guarantee that all
364 // vertex pairs are separated by at least the merge radius in the output.)
365
366 // First, we build the set of all the distinct vertices in the input.
367 // We need to include the source and destination of every edge.
368 hash_set<S2Point> vertices;
369 for (EdgeSet::const_iterator i = edges_->begin(); i != edges_->end(); ++i) {
370 vertices.insert(i->first);
371 VertexSet const& vset = i->second;
372 for (VertexSet::const_iterator j = vset.begin(); j != vset.end(); ++j)
373 vertices.insert(*j);
374 }
375
376 // Build a spatial index containing all the distinct vertices.
377 for (hash_set<S2Point>::const_iterator i = vertices.begin();
378 i != vertices.end(); ++i) {
379 index->Insert(*i);
380 }
381
382 // Next, we loop through all the vertices and attempt to grow a maximial
383 // mergeable group starting from each vertex.
384 vector<S2Point> frontier, mergeable;
385 for (hash_set<S2Point>::const_iterator vstart = vertices.begin();
386 vstart != vertices.end(); ++vstart) {
387 // Skip any vertices that have already been merged with another vertex.
388 if (merge_map->find(*vstart) != merge_map->end()) continue;
389
390 // Grow a maximal mergeable component starting from "vstart", the
391 // canonical representative of the mergeable group.
392 frontier.push_back(*vstart);
393 while (!frontier.empty()) {
394 index->QueryCap(frontier.back(), &mergeable);
395 frontier.pop_back(); // Do this before entering the loop below.
396 for (int j = mergeable.size(); --j >= 0; ) {
397 S2Point const& v1 = mergeable[j];
398 if (v1 != *vstart) {
399 // Erase from the index any vertices that will be merged. This
400 // ensures that we won't try to merge the same vertex twice.
401 index->Erase(v1);
402 frontier.push_back(v1);
403 (*merge_map)[v1] = *vstart;
404 }
405 }
406 }
407 }
408}
409
410void S2PolygonBuilder::MoveVertices(MergeMap const& merge_map) {
411 if (merge_map.empty()) return;
412
413 // We need to copy the set of edges affected by the move, since
414 // edges_ could be reallocated when we start modifying it.
415 vector<pair<S2Point, S2Point> > edges;
416 for (EdgeSet::const_iterator i = edges_->begin(); i != edges_->end(); ++i) {
417 S2Point const& v0 = i->first;
418 VertexSet const& vset = i->second;
419 for (VertexSet::const_iterator j = vset.begin(); j != vset.end(); ++j) {
420 S2Point const& v1 = *j;
421 if (merge_map.find(v0) != merge_map.end() ||
422 merge_map.find(v1) != merge_map.end()) {
423 // We only need to modify one copy of each undirected edge.
424 if (!options_.undirected_edges() || v0 < v1) {
425 edges.push_back(make_pair(v0, v1));
426 }
427 }
428 }
429 }
430
431 // Now erase all the old edges, and add all the new edges. This will
432 // automatically take care of any XORing that needs to be done, because
433 // EraseEdge also erases the sibling of undirected edges.
434 for (int i = 0; i < edges.size(); ++i) {
435 S2Point v0 = edges[i].first;
436 S2Point v1 = edges[i].second;
437 EraseEdge(v0, v1);
438 MergeMap::const_iterator new0 = merge_map.find(v0);
439 if (new0 != merge_map.end()) v0 = new0->second;
440 MergeMap::const_iterator new1 = merge_map.find(v1);
441 if (new1 != merge_map.end()) v1 = new1->second;
442 AddEdge(v0, v1);
443 }
444}
445
446void S2PolygonBuilder::SpliceEdges(PointIndex* index) {
447 // We keep a stack of unprocessed edges. Initially all edges are
448 // pushed onto the stack.
449 vector<pair<S2Point, S2Point> > edges;
450 for (EdgeSet::const_iterator i = edges_->begin(); i != edges_->end(); ++i) {
451 S2Point const& v0 = i->first;
452 VertexSet const& vset = i->second;
453 for (VertexSet::const_iterator j = vset.begin(); j != vset.end(); ++j) {
454 S2Point const& v1 = *j;
455 // We only need to modify one copy of each undirected edge.
456 if (!options_.undirected_edges() || v0 < v1) {
457 edges.push_back(make_pair(v0, v1));
458 }
459 }
460 }
461
462 // For each edge, we check whether there are any nearby vertices that should
463 // be spliced into it. If there are, we choose one such vertex, split
464 // the edge into two pieces, and iterate on each piece.
465 while (!edges.empty()) {
466 S2Point v0 = edges.back().first;
467 S2Point v1 = edges.back().second;
468 edges.pop_back(); // Do this before pushing new edges.
469
470 // If we are xoring, edges may be erased before we get to them.
471 if (options_.xor_edges() && !HasEdge(v0, v1)) continue;
472
473 S2Point vmid;
474 if (!index->FindNearbyPoint(v0, v1, &vmid)) continue;
475
476 EraseEdge(v0, v1);
477 if (AddEdge(v0, vmid)) edges.push_back(make_pair(v0, vmid));
478 if (AddEdge(vmid, v1)) edges.push_back(make_pair(vmid, v1));
479 }
480}
481
482bool S2PolygonBuilder::AssembleLoops(vector<S2Loop*>* loops,
483 EdgeList* unused_edges) {
484 if (options_.vertex_merge_radius().radians() > 0) {
485 PointIndex index(options_.vertex_merge_radius().radians(),
486 options_.edge_splice_fraction());
487 MergeMap merge_map;
488 BuildMergeMap(&index, &merge_map);
489 MoveVertices(merge_map);
490 if (options_.edge_splice_fraction() > 0) {
491 SpliceEdges(&index);
492 }
493 }
494
495 EdgeList dummy_unused_edges;
496 if (unused_edges == NULL) unused_edges = &dummy_unused_edges;
497
498 // We repeatedly choose an edge and attempt to assemble a loop
499 // starting from that edge. (This is always possible unless the
500 // input includes extra edges that are not part of any loop.) To
501 // maintain a consistent scanning order over edges_ between
502 // different machine architectures (e.g. 'clovertown' vs. 'opteron'),
503 // we follow the order they were added to the builder.
504 unused_edges->clear();
505 for (int i = 0; i < starting_vertices_.size(); ) {
506 S2Point const& v0 = starting_vertices_[i];
507 EdgeSet::const_iterator candidates = edges_->find(v0);
508 if (candidates == edges_->end()) {
509 ++i;
510 continue;
511 }
512 // NOTE(user): If we have such two S2Points a, b that:
513 //
514 // a.x = b.x, a.y = b.y and
515 // -- a.z = b.z if CPU is Intel
516 // -- a.z <> b.z if CPU is AMD
517 //
518 // then the order of points picked up as v1 on the following line
519 // can be inconsistent between different machine architectures.
520 //
521 // As of b/3088321 and of cl/17847332, it's not clear if such
522 // input really exists in our input and probably it's O.K. not to
523 // address it in favor of the speed.
524 S2Point const& v1 = *(candidates->second.begin());
525 S2Loop* loop = AssembleLoop(v0, v1, unused_edges);
526 if (loop != NULL) {
527 loops->push_back(loop);
528 EraseLoop(&loop->vertex(0), loop->num_vertices());
529 }
530 }
531 return unused_edges->empty();
532}
533
534bool S2PolygonBuilder::AssemblePolygon(S2Polygon* polygon,
535 EdgeList* unused_edges) {
536 vector<S2Loop*> loops;
537 bool success = AssembleLoops(&loops, unused_edges);
538
539 // If edges are undirected, then all loops are already CCW. Otherwise we
540 // need to make sure the loops are normalized.
541 if (!options_.undirected_edges()) {
542 for (int i = 0; i < loops.size(); ++i) {
543 loops[i]->Normalize();
544 }
545 }
546 if (options_.validate() && !S2Polygon::IsValid(loops)) {
547 if (unused_edges != NULL) {
548 for (int i = 0; i < loops.size(); ++i) {
549 RejectLoop(&loops[i]->vertex(0), loops[i]->num_vertices(),
550 unused_edges);
551 }
552 }
553
554 for (int i = 0; i < loops.size(); ++i) {
555 delete loops[i];
556 }
557 loops.clear();
558 return false;
559 }
560 polygon->Init(&loops);
561 return success;
562}
563