Debian Codegraph @

src/atlas/util/ConvexSphericalPolygon.cc

raw
1 /*
2 * (C) Copyright 2013 ECMWF.
3 *
4 * This software is licensed under the terms of the Apache Licence Version 2.0
5 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
6 * In applying this licence, ECMWF does not waive the privileges and immunities
7 * granted to it by virtue of its status as an intergovernmental organisation
8 * nor does it submit to any jurisdiction.
9 */
10
11 #include <iomanip>
12 #include <iostream>
13
14 #include "atlas/util/Point.h"
15 #include "eckit/geometry/Sphere.h"
16 #include "eckit/types/FloatCompare.h"
17
18 #include "atlas/runtime/Exception.h"
19 #include "atlas/runtime/Log.h"
20 #include "atlas/util/ConvexSphericalPolygon.h"
21 #include "atlas/util/CoordinateEnums.h"
22 #include "atlas/util/NormaliseLongitude.h"
23 #include "atlas/library/FloatingPointExceptions.h"
24
25 namespace atlas {
26 namespace util {
27
28 using GreatCircleSegment = ConvexSphericalPolygon::GreatCircleSegment;
29
30 namespace {
31
32 constexpr double EPS = std::numeric_limits<double>::epsilon();
33 constexpr double EPS2 = EPS * EPS;
34
35 inline double distance2(const PointXYZ& p1, const PointXYZ& p2) {
36 double dx = p2[0] - p1[0];
37 double dy = p2[1] - p1[1];
38 double dz = p2[2] - p1[2];
39 return dx * dx + dy * dy + dz * dz;
40 }
41
42 inline double dot(const PointXYZ& p1, const PointXYZ& p2) {
43 return p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2];
44 }
45
46 inline bool approx_eq(const double& v1, const double& v2) {
47 return std::abs(v1 - v2) <= EPS;
48 }
49
50 inline bool approx_eq(const PointXYZ& v1, const PointXYZ& v2) {
51 //return approx_eq( v1[0], v2[0], t ) && approx_eq( v1[1], v2[1], t ) && approx_eq( v1[2], v2[2], t );
52 return distance2(v1, v2) <= EPS2;
53 }
54
55 void lonlat2xyz(const PointLonLat& lonlat, PointXYZ& xyz) {
56 eckit::geometry::Sphere::convertSphericalToCartesian(1., lonlat, xyz);
57 }
58
59 void xyz2lonlat(const PointXYZ& xyz, PointLonLat& lonlat) {
60 eckit::geometry::Sphere::convertCartesianToSpherical(1., xyz, lonlat);
61 }
62
63 #if DEBUG_OUTPUT
64 PointLonLat xyz2lonlat(const PointXYZ& xyz) {
65 PointLonLat lonlat;
66 eckit::geometry::Sphere::convertCartesianToSpherical(1., xyz, lonlat);
67 return lonlat;
68 }
69 #endif
70
71 } // namespace
72
73 //------------------------------------------------------------------------------------------------------
74
75 bool ConvexSphericalPolygon::fpe_ = true;
76
77 ConvexSphericalPolygon::ConvexSphericalPolygon(const PointLonLat points[], size_t size): size_{size} {
78 ATLAS_ASSERT(size_ > 2, "Polygon must have at least 3 points");
79 ATLAS_ASSERT(size_ < MAX_SIZE, "Number of polygon points exceeds compile time MAX_SIZE");
80 lonlat2xyz(points[0], sph_coords_[0]);
81 size_t isp = 1;
82 for (size_t i = 1; i < size_ - 1; ++i) {
83 lonlat2xyz(points[i], sph_coords_[isp]);
84 // Log::info() << " d : " << PointXYZ::distance(sph_coords_[isp], sph_coords_[isp - 1]) << std::endl;
85 if (approx_eq(sph_coords_[isp], sph_coords_[isp - 1])) {
86 continue;
87 }
88 ++isp;
89 }
90 lonlat2xyz(points[size_ - 1], sph_coords_[isp]);
91 if (approx_eq(sph_coords_[isp], sph_coords_[0]) or approx_eq(sph_coords_[isp], sph_coords_[isp - 1])) {
92 }
93 else {
94 ++isp;
95 }
96 size_ = isp;
97 validate();
98
99 if (not valid_) {
100 invalidate_this_polygon();
101 }
102 }
103
104 ConvexSphericalPolygon::ConvexSphericalPolygon(const PointXYZ points[], size_t size): size_{size} {
105 ATLAS_ASSERT(size_ > 2, "Polygon must have at least 3 points");
106 ATLAS_ASSERT(size_ < MAX_SIZE, "Number of polygon points exceeds compile time MAX_SIZE");
107 sph_coords_[0] = points[0];
108 size_t isp = 1;
109 for (size_t i = 1; i < size_ - 1; ++i) {
110 sph_coords_[isp] = points[i];
111 if (approx_eq(sph_coords_[isp], sph_coords_[isp - 1])) {
112 continue;
113 }
114 ++isp;
115 }
116 sph_coords_[isp] = points[size_ - 1];
117 if (approx_eq(sph_coords_[isp], sph_coords_[0]) or approx_eq(sph_coords_[isp], sph_coords_[isp - 1])) {
118 }
119 else {
120 ++isp;
121 }
122 size_ = isp;
123 validate();
124
125 if (not valid_) {
126 invalidate_this_polygon();
127 }
128 }
129
130 void ConvexSphericalPolygon::compute_centroid_and_area() const {
131 const auto triangles = triangulate();
132 area_ = triangles.area();
133 computed_area_ = true;
134
135 // Compute centroid based on triangles rather than on vertices of polygon
136 centroid_[0] = 0.;
137 centroid_[1] = 0.;
138 centroid_[2] = 0.;
139 if (area_ > 0.) {
140 for (auto& triangle : triangles) {
141 for (size_t i = 0; i < 3; ++i) {
142 centroid_[i] += triangle.centroid[i] * triangle.area;
143 }
144 }
145 centroid_ /= PointXYZ::norm(centroid_);
146 }
147
148 // centroid_ = PointXYZ::zero();
149 // for (int i = 0; i < size_; ++i) {
150 // centroid_ = centroid_ + sph_coords_[i];
151 // }
152 // PointXYZ::normalise(centroid_);
153 computed_centroid_ = true;
154 }
155
156 // cf. M. Floater, “Generalized barycentric coordinates and applications” Acta Numerica, p. 001, 2016.
157 int ConvexSphericalPolygon::compute_vertex_weights(const PointXYZ& candidatePoint, double vertex_weights[],
158 size_t vertex_weights_size) {
159 ATLAS_ASSERT(vertex_weights_size == size(), "vertex_weights container size does not match number of vertices");
160 ATLAS_ASSERT(edge_normals().size() == this->size(),
161 "Incorrect number of edge normals computed - should equal number of polygon edges.");
162
163 std::array<double, MAX_SIZE> greatCircleProducts = {0};
164
165 for (size_t i = 0; i < size_; ++i) {
166 GreatCircleSegment segment = GreatCircleSegment(sph_coords_[i], sph_coords_[(i + 1) % size_]);
167 greatCircleProducts[i] = dot(edge_normals_[i], candidatePoint);
168
169 if (!segment.inLeftHemisphere(candidatePoint, -5 * EPS)) {
170 return 0;
171 }
172 }
173
174 // TODO - set vertex_weights to all zeros
175
176 for (size_t v = 0; v < size_; ++v) {
177 if (PointXYZ::distance2(candidatePoint, sph_coords_[v]) < 25 * EPS2) {
178 for (size_t vv = 0; vv < size_; ++vv) {
179 vertex_weights[vv] = 0;
180 }
181 vertex_weights[v] = 1.;
182 return 1;
183 }
184 }
185
186 std::array<PointXYZ, MAX_SIZE> spokeNormals;
187 std::array<double, MAX_SIZE> spokeNorms;
188 double denominator = 0.;
189
190 for (size_t v = 0; v < size_; ++v) {
191 spokeNormals[v] = PointXYZ::cross(sph_coords_[v], candidatePoint);
192 spokeNorms[v] = PointXYZ::norm(spokeNormals[v]);
193 }
194
195 for (size_t w = 0; w < size_; ++w) {
196 int pw = previous(w);
197 int nw = next(w);
198 double product = 1.;
199
200 for (size_t i = 0; i < size_; ++i) {
201 if ((i != w) && (i != pw)) {
202 product *= greatCircleProducts[i];
203 }
204 }
205
206 double leftSideAngle = spokeNorms[nw] - dot(spokeNormals[nw], spokeNormals[w]) / spokeNorms[w];
207 double rightSideAngle = spokeNorms[pw] - dot(spokeNormals[pw], spokeNormals[w]) / spokeNorms[w];
208
209 vertex_weights[w] =
210 (greatCircleProducts[pw] * (leftSideAngle) + greatCircleProducts[w] * (rightSideAngle)) * product;
211 denominator += vertex_weights[w] * dot(sph_coords_[w], candidatePoint);
212 }
213
214 for (size_t w = 0; w < size_; ++w) {
215 vertex_weights[w] /= denominator;
216 }
217 return 1;
218 }
219
220 void ConvexSphericalPolygon::validate() {
221 valid_ = size_ > 2;
222 if (valid_) {
223 for (int i = 0; i < size(); i++) {
224 int ni = next(i);
225 int nni = next(ni);
226 const PointXYZ& P = sph_coords_[i];
227 const PointXYZ& nextP = sph_coords_[ni];
228 if( std::abs(dot(P, P) - 1.) >= 10 * EPS ) {
229 valid_ = false;
230 break;
231 }
232 ATLAS_ASSERT(not approx_eq(P, PointXYZ::mul(nextP, -1.)));
233 if (not GreatCircleSegment{P, nextP}.inLeftHemisphere(sph_coords_[nni], -0.5 * EPS)) {
234 valid_ = false;
235 break;
236 }
237 }
238 }
239 }
240
241 void ConvexSphericalPolygon::compute_edge_normals() const {
242 if (valid_) {
243 std::vector<PointXYZ>().swap(edge_normals_);
244 edge_normals_.reserve(size_);
245
246 for (int i = 0; i < size_; i++) {
247 int ni = next(i);
248 const PointXYZ& P = sph_coords_[i];
249 const PointXYZ& nextP = sph_coords_[ni];
250 edge_normals_.emplace_back(GreatCircleSegment{P, nextP}.cross());
251 }
252 computed_edge_normals_ = true;
253 }
254 }
255
256
257 bool ConvexSphericalPolygon::equals(const ConvexSphericalPolygon& plg, const double deg_prec) const {
258 if (size_ == 0 and plg.size_ == 0) {
259 return true;
260 }
261 if (not valid_) {
262 Log::info() << " ConvexSphericalPolygon::equals : this polygon is not valid\n";
263 return false;
264 }
265 if (not plg.valid_) {
266 Log::info() << " ConvexSphericalPolygon::equals : other polygon passed as argument is not valid\n";
267 return false;
268 }
269 if (size_ != plg.size()) {
270 Log::info() << " ConvexSphericalPolygon::equals : incompatible sizes: " << size_ << " != " << plg.size() << "\n";
271 return false;
272 }
273 int offset = 0;
274 const double le = 2. * std::sin(M_PI * deg_prec / 360.);
275 const double le2 = le * le;
276 for (; offset < size_; ++offset) {
277 if (distance2(plg.sph_coords_[0], sph_coords_[offset]) < le2) {
278 break;
279 }
280 }
281 if (offset == size_) {
282 Log::info() << "ConvexSphericalPolygon::equals : no point equal\n";
283 return false;
284 }
285
286 for (int j = 0; j < size_; j++) {
287 int idx = (offset + j) % size_;
288 auto dist2 = distance2(plg.sph_coords_[j], sph_coords_[idx]);
289 if (dist2 > le2) {
290 Log::info() << " ConvexSphericalPolygon::equals : point distance " << std::sqrt(dist2) << " < " << le2 << "(= "<< deg_prec << " deg)" << "\n";
291 return false;
292 }
293 }
294 return true;
295 }
296
297 // cf. Folke Eriksson, "On the Measure of Solid Angles", Mathematics Magazine, Vol. 63, No. 3, pp. 184-187 (1990)
298 ConvexSphericalPolygon::SubTriangles ConvexSphericalPolygon::triangulate() const {
299 SubTriangles triangles;
300 if (size_ < 3) {
301 return triangles;
302 }
303 size_t itri{0};
304 const PointXYZ& a = sph_coords_[0];
305 for (size_t i = 1; i < size_ - 1; i++) {
306 const PointXYZ& b = sph_coords_[i];
307 const PointXYZ& c = sph_coords_[i + 1];
308 triangles[itri].centroid = PointXYZ::normalize(a + b + c);
309 // if (PointXYZ::distance(a, b) + PointXYZ::distance(b, c) < 1e-10) {
310 // triangles[itri].area = 0.5 * PointXYZ::norm(PointXYZ::cross(b - a, c - b));
311 // }
312 // else {
313 auto abc = dot(a, b) + dot(b, c) + dot(c, a);
314 auto a_bc = dot(a, PointXYZ::cross(b, c));
315 triangles[itri].area = 2. * std::atan(std::abs(a_bc) / (1. + abc));
316 // }
317 ++itri;
318 }
319 triangles.size() = itri;
320 return triangles;
321
322 }
323
324
325 double ConvexSphericalPolygon::SubTriangles::area() const {
326 double area = 0.;
327 for (auto& triangle : *this) {
328 area += triangle.area;
329 }
330 return area;
331 }
332
333
334 template <class ClippedCoords>
335 void ConvexSphericalPolygon::clip(const GreatCircleSegment& great_circle, ClippedCoords& clipped_sph_coords, const double& pointsSameEPS, std::ostream* out) {
336 size_t clipped_sph_coords_size{0};
337 #if DEBUG_OUTPUT
338 int add_point_num = 0;
339 ATLAS_ASSERT(valid_);
340 ATLAS_ASSERT(not approx_eq(great_circle.first(), great_circle.second()));
341 #endif
342 #if ATLAS_BUILD_TYPE_DEBUG
343 ATLAS_ASSERT(valid_);
344 ATLAS_ASSERT(not approx_eq(great_circle.first(), great_circle.second()));
345 #endif
346 bool first_in = great_circle.inLeftHemisphere(sph_coords_[0], -1.5 * EPS, out);
347 for (int i = 0; i < size_; i++) {
348 int in = (i+1) % size_;
349 bool second_in = great_circle.inLeftHemisphere(sph_coords_[in], -1.5 * EPS, out);
350 #if DEBUG_OUTPUT
351 if (out) {
352 out->precision(18);
353 *out << " ** first: " << xyz2lonlat(sph_coords_[i]) << ", in ? " << first_in << std::endl;
354 *out << " second: " << xyz2lonlat(sph_coords_[in]) << ", in ? " << second_in << std::endl;
355 }
356 #endif
357 if (first_in and second_in) {
358 clipped_sph_coords[clipped_sph_coords_size++] = sph_coords_[in];
359 #if DEBUG_OUTPUT
360 if (out) {
361 *out << " ((" << ++add_point_num << ")) both_in add second: " << xyz2lonlat(sph_coords_[in]) << std::endl;
362 }
363 #endif
364 }
365 else if (not first_in and not second_in) {
366 // continue to update first_in
367 }
368 else {
369 const GreatCircleSegment segment(sph_coords_[i], sph_coords_[in]);
370 PointXYZ ip = great_circle.intersect(segment, out, pointsSameEPS);
371 #if DEBUG_OUTPUT
372 if (out) {
373 *out << " ip : " << xyz2lonlat(ip) << std::endl;
374 }
375 #endif
376 if (ip[0] == 1 and ip[1] == 1 and ip[2] == 1) {
377 // consider the segments parallel
378 #if DEBUG_OUTPUT
379 if (out) {
380 *out << " ((" << ++add_point_num << ")) ip=(1,1,1) add second: "
381 << xyz2lonlat(sph_coords_[in]) << std::endl;
382 }
383 #endif
384 clipped_sph_coords[clipped_sph_coords_size++] = sph_coords_[in];
385 first_in = second_in;
386 continue;
387 }
388 if (second_in) {
389 int inn = (in+1) % size_;
390 const GreatCircleSegment segment_n(sph_coords_[in], sph_coords_[inn]);
391 if (segment.inLeftHemisphere(ip, -1.5 * EPS, out) and
392 segment_n.inLeftHemisphere(ip, -1.5 * EPS, out) and
393 (distance2(ip, sph_coords_[in]) > pointsSameEPS * pointsSameEPS)) {
394 clipped_sph_coords[clipped_sph_coords_size++] = ip;
395 #if DEBUG_OUTPUT
396 if (out) {
397 *out << " ((" << ++add_point_num << ")) second_in add ip: " << xyz2lonlat(ip) << std::endl;
398 }
399 #endif
400 }
401 clipped_sph_coords[clipped_sph_coords_size++] = sph_coords_[in];
402 #if DEBUG_OUTPUT
403 if (out) {
404 *out << " ((" << ++add_point_num << ")) second_in add second: " << xyz2lonlat(sph_coords_[in]) << std::endl;
405 }
406 #endif
407 }
408 else {
409 if (distance2(ip, sph_coords_[i]) > pointsSameEPS * pointsSameEPS) {
410 clipped_sph_coords[clipped_sph_coords_size++] = ip;
411 #if DEBUG_OUTPUT
412 if (out) {
413 *out << " ((" << ++add_point_num << ")) first_in add ip: " << xyz2lonlat(ip) << std::endl;
414 }
415 #endif
416 }
417 }
418 }
419 first_in = second_in;
420 }
421
422 // Update polygon
423 {
424 size_ = clipped_sph_coords_size;
425 if (size_ < 3) {
426 invalidate_this_polygon();
427 }
428 else {
429 for (size_t i = 0; i < size_; i++) {
430 sph_coords_[i] = clipped_sph_coords[i];
431 }
432 }
433 }
434 }
435
436 // intersect a polygon with this polygon
437 // @param[in] pol clipping polygon
438 // @param[out] intersecting polygon
439 ConvexSphericalPolygon ConvexSphericalPolygon::intersect(const ConvexSphericalPolygon& plg, std::ostream* out, double pointsSameEPS) const {
440
441 bool fpe_disabled = fpe_ ? atlas::library::disable_floating_point_exception(FE_INVALID) : false;
442 auto restore_fpe = [fpe_disabled] {
443 if (fpe_disabled) {
444 atlas::library::enable_floating_point_exception(FE_INVALID);
445 }
446 };
447
448 ConvexSphericalPolygon intersection;
449 const ConvexSphericalPolygon* intersector;
450
451 bool this_intersector = (area() > plg.area());
452 if (approx_eq(area(), plg.area())) {
453 PointXYZ dc = centroid() - plg.centroid();
454 if (dc[0] > 0. or (dc[0] == 0. and (dc[1] > 0. or (dc[1] == 0. and dc[2] > 0.)))) {
455 this_intersector = true;
456 }
457 }
458 if (this_intersector) {
459 intersector = this;
460 intersection = plg;
461 }
462 else {
463 intersector = &plg;
464 intersection = *this;
465 }
466
467 #if DEBUG_OUTPUT
468 std::string intor_id = "P1";
469 std::string inted_id = "P2";
470 if (! this_intersector) {
471 intor_id = "P2";
472 inted_id = "P1";
473 }
474 if (out) {
475 *out << inted_id << " : ";
476 print(*out);
477 *out << std::endl << intor_id << " : ";
478 plg.print(*out);
479 *out << std::endl;
480 }
481 #endif
482 if (intersection.valid_) {
483 std::array<PointXYZ, ConvexSphericalPolygon::MAX_SIZE> clipped_sph_coords;
484 for (size_t i = 0; i < intersector->size_; i++) {
485 const PointXYZ& s1 = intersector->sph_coords_[i];
486 const PointXYZ& s2 = intersector->sph_coords_[(i != intersector->size_ - 1) ? i + 1 : 0];
487 #if DEBUG_OUTPUT
488 if (out) {
489 *out << std::endl << "Clip with [" << intor_id << "_" << i << ", " << intor_id << "_"
490 << (i+1) % intersector->size_ << "]" << std::endl;
491 }
492 #endif
493 intersection.clip(GreatCircleSegment(s1, s2), clipped_sph_coords, pointsSameEPS, out);
494 if (not intersection.valid_) {
495 restore_fpe();
496 return intersection;
497 }
498 }
499 }
500 intersection.computed_area_ = false;
501 intersection.computed_radius_ = false;
502 intersection.computed_centroid_ = false;
503 restore_fpe();
504 return intersection;
505 }
506
507 void ConvexSphericalPolygon::print(std::ostream& out) const {
508 out << "{";
509 for (size_t i = 0; i < size(); ++i) {
510 if (i > 0) {
511 out << ",";
512 }
513 PointLonLat ip_ll;
514 xyz2lonlat(sph_coords_[i], ip_ll);
515 out << ip_ll;
516 }
517 out << "}";
518 }
519
520 std::string ConvexSphericalPolygon::json(int precision) const {
521 std::stringstream ss;
522 if( precision ) {
523 ss.precision(16);
524 }
525 ss << "[";
526 for (size_t i = 0; i < size(); ++i) {
527 if (i > 0) {
528 ss << ",";
529 }
530 PointLonLat ip_ll;
531 xyz2lonlat(sph_coords_[i], ip_ll);
532 ss << "[" << ip_ll.lon() << "," << ip_ll.lat() << "]";
533 }
534 ss << "]";
535 return ss.str();
536 }
537
538
539 double ConvexSphericalPolygon::compute_radius() const {
540 double radius{0.};
541 if (valid_) {
542 if (not computed_centroid_) {
543 compute_centroid_and_area();
544 }
545 for (size_t i = 0; i < size_; ++i) {
546 radius = std::max(radius, PointXYZ::distance(sph_coords_[i], centroid_));
547 }
548 }
549 return radius;
550 }
551
552
553 // 'this' great circle's intersection with the segment 'p': [p.first(), p.second())
554 PointXYZ ConvexSphericalPolygon::GreatCircleSegment::intersect(const GreatCircleSegment& p, std::ostream* out, double pointsSameEPS) const {
555 const auto& s = *this;
556 PointXYZ sp = PointXYZ::cross(s.cross(), p.cross());
557
558 double sp_norm = PointXYZ::norm(sp);
559 bool gcircles_distinct = (sp_norm > EPS);
560 #if DEBUG_OUTPUT
561 if (out) {
562 *out << " Great circles distinct ? " << sp_norm << " > " << EPS << " ? "
563 << gcircles_distinct << std::endl;
564 }
565 #endif
566 if (gcircles_distinct) {
567 sp /= sp_norm;
568 auto sp_2 = sp * -1.;
569 double d1 = std::max(distance2(sp, p.first()), distance2(sp, p.second()));
570 double d2 = std::max(distance2(sp_2, p.first()), distance2(sp_2, p.second()));
571 if (d1 < d2) {
572 return sp;
573 }
574 else {
575 return sp_2;
576 }
577 }
578 else {
579 return PointXYZ(1, 1, 1);
580 }
581 }
582
583
584 //------------------------------------------------------------------------------------------------------
585
586 } // namespace util
587 } // namespace atlas