Debian Codegraph @

src/atlas/interpolation/element/Quad2D.cc

raw
1 /*
2 * (C) Crown Copyright 2021 Met Office
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 */
7
8 #include <cmath>
9 #include <iostream>
10
11 #include "atlas/interpolation/Vector2D.h"
12 #include "atlas/interpolation/element/Quad2D.h"
13 #include "atlas/interpolation/element/Triag2D.h"
14 #include "atlas/runtime/Log.h"
15
16 namespace atlas {
17 namespace interpolation {
18 namespace element {
19
20 //----------------------------------------------------------------------------------------------------------------------
21
22 method::Intersect Quad2D::intersects(const Point2& r, double edgeEpsilon, double epsilon) const {
23 method::Intersect isect; // intersection is false
24
25 /* Split quadrilateral into two triangles, points are labelled counter-clockwise.
26 * v01-----------v11
27 * / " . `.
28 * / " . `.
29 * / " .`.
30 * v00-------------------v10
31 *
32 */
33
34 Triag2D T013(v00, v10, v01);
35 isect = T013.intersects(r, edgeEpsilon, epsilon);
36 if (isect) {
37 return isect;
38 }
39
40 Triag2D T231(v11, v01, v10);
41 isect = T231.intersects(r, edgeEpsilon, epsilon);
42 if (isect) {
43 isect.u = 1 - isect.u;
44 isect.v = 1 - isect.v;
45 return isect;
46 }
47
48 return isect.fail();
49 }
50
51 method::Intersect Quad2D::localRemap(const Point2& p, double edgeEpsilon, double epsilon) const {
52 method::Intersect isect;
53
54 // get area of quad.
55 double quadArea = area();
56
57 // Epsilon is compared against areas. Scale value accordingly.
58 double areaEpsilon = epsilon * quadArea;
59
60 // work out if point is within the polygon
61 if (!inQuadrilateral({p.x(), p.y()}, areaEpsilon)) {
62 return isect.fail();
63 }
64
65 auto solve_weight = [&](double a, double b, double c, double& weight) -> bool {
66 auto checkWeight = [](double weight, double tol) -> bool { return ((weight > -tol) && (weight < (1. + tol))); };
67
68 // Quadratic equation ax^2 + bx + c = 0.
69 if (std::abs(a) >= areaEpsilon) {
70 // Solve numerically stable form of quadratic formula:
71 // x1 = (-b - sign(b) sqrt(b^2 - 4ac)) / 2a
72 // x2 = c / ax1
73
74 // Kahan's algorithm for accurate difference of products.
75 const auto prodDiff = [](double a, double b, double c, double d) {
76 // return ab - cd
77 const double w = d * c;
78 const double e = std::fma(-d, c, w);
79 const double f = std::fma(a, b, -w);
80 return f + e;
81 };
82
83 double discriminant = prodDiff(b, b, 4. * a, c);
84
85 if (discriminant > -areaEpsilon * 2. * quadArea) {
86 // Solution is real.
87
88 double sqrtDiscriminant = std::sqrt(std::max(0., discriminant));
89
90 // "Classic" solution to quadratic formula with no cancelation
91 // on numerator.
92 weight = (-b - std::copysign(sqrtDiscriminant, b)) / (2. * a);
93 if (checkWeight(weight, edgeEpsilon)) {
94 return true;
95 }
96
97 // Use Vieta's formula x1 * x2 = c / a;
98 weight = c / (a * weight);
99 return checkWeight(weight, edgeEpsilon);
100 }
101 }
102 else if (std::abs(b) >= areaEpsilon) {
103 // Linear case bx + c = 0.
104 weight = -c / b;
105 return checkWeight(weight, edgeEpsilon);
106 }
107
108 // No real solutions to equation.
109 return false;
110 };
111
112 // solve for u and v where:
113 // w1 = ( 1 - u ) * ( 1 - v )
114 // w2 = u * ( 1 - v )
115 // w3 = u * v
116 // w4 = ( 1 - u ) * v
117
118 Vector2D point(p.x(), p.y());
119 Vector2D vA = v00 - point;
120 Vector2D vB = v10 - v00;
121 Vector2D vC = v01 - v00;
122 Vector2D vD = v00 - v10 - v01 + v11;
123
124 // solve for v
125 double a = cross2d(vC, vD);
126 double b = cross2d(vC, vB) + cross2d(vA, vD);
127 double c = cross2d(vA, vB);
128
129 if (!solve_weight(a, b, c, isect.v)) {
130 return isect.fail();
131 }
132
133 // solve for u
134 a = cross2d(vB, vD);
135 b = cross2d(vB, vC) + cross2d(vA, vD);
136 c = cross2d(vA, vC);
137
138 if (!solve_weight(a, b, c, isect.u)) {
139 return isect.fail();
140 }
141
142 return isect.success();
143 }
144
145 bool Quad2D::validate() const {
146 // normal for sub-triangle T231
147
148 Vector2D E23 = v01 - v11;
149 Vector2D E21 = v10 - v11;
150
151 double N231 = cross2d(E23, E21);
152
153 // normal for sub-triangle T013
154
155 Vector2D E01 = v10 - v00;
156 Vector2D E03 = v01 - v00;
157
158 double N013 = cross2d(E01, E03);
159
160 // normal for sub-triangle T120
161
162 Vector2D E12 = -E21;
163 Vector2D E10 = -E01;
164
165 double N120 = cross2d(E12, E10);
166
167 // normal for sub-triangle T302
168
169 Vector2D E30 = -E03;
170 Vector2D E32 = -E23;
171
172 double N302 = cross2d(E30, E32);
173
174 // all normals must point same way
175
176 double dot02 = N231 * N013;
177 double dot23 = N013 * N120;
178 double dot31 = N120 * N302;
179 double dot10 = N302 * N231;
180
181 // all normals must point same way
182 bool is_inside = ((dot02 >= 0. && dot23 >= 0. && dot31 >= 0. && dot10 >= 0.) ||
183 (dot02 <= 0. && dot23 <= 0. && dot31 <= 0. && dot10 <= 0.));
184 return is_inside;
185 }
186
187 double Quad2D::area() const {
188 return std::abs(0.5 * cross2d((v01 - v00), (v11 - v00))) + std::abs(0.5 * cross2d((v10 - v11), (v01 - v11)));
189 }
190
191 bool Quad2D::inQuadrilateral(const Vector2D& p, double tolerance) const {
192 // point p must be on the inside of all quad edges to be inside the quad.
193 return cross2d(p - v00, p - v10) > -tolerance && cross2d(p - v10, p - v11) > -tolerance &&
194 cross2d(p - v11, p - v01) > -tolerance && cross2d(p - v01, p - v00) > -tolerance;
195 }
196
197 void Quad2D::print(std::ostream& s) const {
198 auto printVector2D = [&s](const Vector2D& v) { s << "[" << v[0] << "," << v[1] << "]"; };
199 s << "Quad2D[";
200 printVector2D(v00);
201 s << ", ";
202 printVector2D(v10);
203 s << ", ";
204 printVector2D(v11);
205 s << ", ";
206 printVector2D(v01);
207 s << "]";
208 }
209
210
211 //----------------------------------------------------------------------------------------------------------------------
212
213 } // namespace element
214 } // namespace interpolation
215 } // namespace atlas