|
6 | 6 | namespace lidR
|
7 | 7 | {
|
8 | 8 |
|
9 |
| -#define EPSILON 2e-8 |
| 9 | +#define EPSILON 1e-8 |
10 | 10 | #define XYINF 99999999999;
|
11 | 11 | #define ZINF 2147483640;
|
12 | 12 | #define MAX(a,b,c) ((a>b)?((a>c)?a:c):((b>c)?b:c));
|
@@ -207,27 +207,31 @@ inline Triangle::Triangle(Point& A, Point& B, Point& C)
|
207 | 207 | }
|
208 | 208 |
|
209 | 209 | template<typename T>
|
210 |
| -bool Triangle::contains(const T& p) |
| 210 | +bool Triangle::contains(const T& P) |
211 | 211 | {
|
212 |
| - if (p.x < xmin - EPSILON || p.x > xmax + EPSILON || p.y < ymin - EPSILON || p.y > ymax + EPSILON) |
| 212 | + if (P.x < xmin - EPSILON || P.x > xmax + EPSILON || P.y < ymin - EPSILON || P.y > ymax + EPSILON) |
213 | 213 | return false;
|
214 | 214 |
|
215 |
| - double denominator = (A.x*(B.y - C.y) + A.y*(C.x - B.x) + B.x*C.y - B.y*C.x); |
216 |
| - double t1 = (p.x*(C.y - A.y) + p.y*(A.x - C.x) - A.x*C.y + A.y*C.x) / denominator; |
217 |
| - double t2 = (p.x*(B.y - A.y) + p.y*(A.x - B.x) - A.x*B.y + A.y*B.x) / -denominator; |
| 215 | + // move to (0,0) to gain arithmetic precision |
| 216 | + double x_offset = xmin; |
| 217 | + double y_offset = ymin; |
| 218 | + PointXY a(A.x - x_offset, A.y - y_offset); |
| 219 | + PointXY b(B.x - x_offset, B.y - y_offset); |
| 220 | + PointXY c(C.x - x_offset, C.y - y_offset); |
| 221 | + PointXY p(P.x - x_offset, P.y - y_offset); |
| 222 | + |
| 223 | + double denominator = (a.x*(b.y - c.y) + a.y*(c.x - b.x) + b.x*c.y - b.y*c.x); |
| 224 | + double t1 = (p.x*(c.y - a.y) + p.y*(a.x - c.x) - a.x*c.y + a.y*c.x) / denominator; |
| 225 | + double t2 = (p.x*(b.y - a.y) + p.y*(a.x - b.x) - a.x*b.y + a.y*b.x) / -denominator; |
218 | 226 | double s = t1 + t2;
|
219 | 227 |
|
220 | 228 | if (0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1 && s <= 1)
|
221 | 229 | return true;
|
222 | 230 |
|
223 | 231 | // see http://totologic.blogspot.com/2014/01/accurate-point-in-triangle-test.html
|
224 |
| - |
225 |
| - if (distanceSquarePointToSegment(A, B, p) <= EPSILON) |
226 |
| - return true; |
227 |
| - if (distanceSquarePointToSegment(B, C, p) <= EPSILON) |
228 |
| - return true; |
229 |
| - if (distanceSquarePointToSegment(C, A, p) <= EPSILON) |
230 |
| - return true; |
| 232 | + if (distanceSquarePointToSegment(a, b, p) <= EPSILON) return true; |
| 233 | + if (distanceSquarePointToSegment(b, c, p) <= EPSILON) return true; |
| 234 | + if (distanceSquarePointToSegment(c, a, p) <= EPSILON) return true; |
231 | 235 |
|
232 | 236 | return false;
|
233 | 237 | }
|
|
0 commit comments