| 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 |