1 |
/*
|
2 |
* This file is a part of Poly2Tri-C
|
3 |
* (c) Barak Itkin <lightningismyname@gmail.com>
|
4 |
* http://code.google.com/p/poly2tri-c/
|
5 |
*
|
6 |
* All rights reserved.
|
7 |
*
|
8 |
* Redistribution and use in source and binary forms, with or without modification,
|
9 |
* are permitted provided that the following conditions are met:
|
10 |
*
|
11 |
* * Redistributions of source code must retain the above copyright notice,
|
12 |
* this list of conditions and the following disclaimer.
|
13 |
* * Redistributions in binary form must reproduce the above copyright notice,
|
14 |
* this list of conditions and the following disclaimer in the documentation
|
15 |
* and/or other materials provided with the distribution.
|
16 |
* * Neither the name of Poly2Tri nor the names of its contributors may be
|
17 |
* used to endorse or promote products derived from this software without specific
|
18 |
* prior written permission.
|
19 |
*
|
20 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
21 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
22 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
23 |
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
24 |
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
25 |
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
26 |
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
27 |
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
28 |
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
29 |
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
30 |
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
31 |
*/
|
32 |
|
33 |
#include <math.h>
|
34 |
#include <glib.h>
|
35 |
#include "rmath.h"
|
36 |
|
37 |
gdouble
|
38 |
p2tr_math_length_sq (gdouble x1, gdouble y1,
|
39 |
gdouble x2, gdouble y2)
|
40 |
{
|
41 |
return P2TR_VECTOR2_DISTANCE_SQ2 (x1, y1, x2, y2);
|
42 |
}
|
43 |
|
44 |
gdouble
|
45 |
p2tr_math_length_sq2 (const P2trVector2 *pt1,
|
46 |
const P2trVector2 *pt2)
|
47 |
{
|
48 |
return p2tr_math_length_sq (pt1->x, pt1->y, pt2->x, pt2->y);
|
49 |
}
|
50 |
|
51 |
static inline gdouble
|
52 |
p2tr_matrix_det2 (gdouble a00, gdouble a01,
|
53 |
gdouble a10, gdouble a11)
|
54 |
{
|
55 |
return a00 * a11 - a10 * a01;
|
56 |
}
|
57 |
|
58 |
static inline gdouble
|
59 |
p2tr_matrix_det3 (gdouble a00, gdouble a01, gdouble a02,
|
60 |
gdouble a10, gdouble a11, gdouble a12,
|
61 |
gdouble a20, gdouble a21, gdouble a22)
|
62 |
{
|
63 |
return
|
64 |
+ a00 * (a11 * a22 - a21 * a12)
|
65 |
- a01 * (a10 * a22 - a20 * a12)
|
66 |
+ a02 * (a10 * a21 - a20 * a11);
|
67 |
}
|
68 |
|
69 |
static inline gdouble
|
70 |
p2tr_matrix_det4 (gdouble a00, gdouble a01, gdouble a02, gdouble a03,
|
71 |
gdouble a10, gdouble a11, gdouble a12, gdouble a13,
|
72 |
gdouble a20, gdouble a21, gdouble a22, gdouble a23,
|
73 |
gdouble a30, gdouble a31, gdouble a32, gdouble a33)
|
74 |
{
|
75 |
return
|
76 |
+ a00 * p2tr_matrix_det3 (a11, a12, a13,
|
77 |
a21, a22, a23,
|
78 |
a31, a32, a33)
|
79 |
- a01 * p2tr_matrix_det3 (a10, a12, a13,
|
80 |
a20, a22, a23,
|
81 |
a30, a32, a33)
|
82 |
+ a02 * p2tr_matrix_det3 (a10, a11, a13,
|
83 |
a20, a21, a23,
|
84 |
a30, a31, a33)
|
85 |
- a03 * p2tr_matrix_det3 (a10, a11, a12,
|
86 |
a20, a21, a22,
|
87 |
a30, a31, a32);
|
88 |
}
|
89 |
|
90 |
void
|
91 |
p2tr_math_triangle_circumcircle (const P2trVector2 *A,
|
92 |
const P2trVector2 *B,
|
93 |
const P2trVector2 *C,
|
94 |
P2trCircle *circle)
|
95 |
{
|
96 |
/* | Ax Bx Cx |
|
97 |
* D = + | Ay By Cy | * 2
|
98 |
* | +1 +1 +1 |
|
99 |
*
|
100 |
* | Asq Bsq Csq |
|
101 |
* X = + | Ay By Cy | / D
|
102 |
* | 1 1 1 |
|
103 |
*
|
104 |
* | Asq Bsq Csq |
|
105 |
* Y = - | Ax Bx Cx | / D
|
106 |
* | 1 1 1 |
|
107 |
*/
|
108 |
gdouble Asq = P2TR_VECTOR2_LEN_SQ (A);
|
109 |
gdouble Bsq = P2TR_VECTOR2_LEN_SQ (B);
|
110 |
gdouble Csq = P2TR_VECTOR2_LEN_SQ (C);
|
111 |
|
112 |
gdouble invD = 1 / (2 * p2tr_matrix_det3 (
|
113 |
A->x, B->x, C->x,
|
114 |
A->y, B->y, C->y,
|
115 |
1, 1, 1));
|
116 |
|
117 |
circle->center.x = + p2tr_matrix_det3 (
|
118 |
Asq, Bsq, Csq,
|
119 |
A->y, B->y, C->y,
|
120 |
1, 1, 1) * invD;
|
121 |
|
122 |
circle->center.y = - p2tr_matrix_det3 (
|
123 |
Asq, Bsq, Csq,
|
124 |
A->x, B->x, C->x,
|
125 |
1, 1, 1) * invD;
|
126 |
|
127 |
circle->radius = sqrt (P2TR_VECTOR2_DISTANCE_SQ (A, &circle->center));
|
128 |
}
|
129 |
|
130 |
/* The point in triangle test which is implemented below is based on the
|
131 |
* algorithm which appears on:
|
132 |
*
|
133 |
* http://www.blackpawn.com/texts/pointinpoly/default.html
|
134 |
*/
|
135 |
void
|
136 |
p2tr_math_triangle_barcycentric (const P2trVector2 *A,
|
137 |
const P2trVector2 *B,
|
138 |
const P2trVector2 *C,
|
139 |
const P2trVector2 *P,
|
140 |
gdouble *u,
|
141 |
gdouble *v)
|
142 |
{
|
143 |
gdouble dot00, dot01, dot02, dot11, dot12, invDenom;
|
144 |
|
145 |
/* Compute the vectors offsetted so that A is the origin */
|
146 |
P2trVector2 v0, v1, v2;
|
147 |
p2tr_vector2_sub(C, A, &v0);
|
148 |
p2tr_vector2_sub(B, A, &v1);
|
149 |
p2tr_vector2_sub(P, A, &v2);
|
150 |
|
151 |
/* Compute dot products */
|
152 |
dot00 = P2TR_VECTOR2_DOT(&v0, &v0);
|
153 |
dot01 = P2TR_VECTOR2_DOT(&v0, &v1);
|
154 |
dot02 = P2TR_VECTOR2_DOT(&v0, &v2);
|
155 |
dot11 = P2TR_VECTOR2_DOT(&v1, &v1);
|
156 |
dot12 = P2TR_VECTOR2_DOT(&v1, &v2);
|
157 |
|
158 |
/* Compute barycentric coordinates */
|
159 |
invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
|
160 |
*u = (dot11 * dot02 - dot01 * dot12) * invDenom;
|
161 |
*v = (dot00 * dot12 - dot01 * dot02) * invDenom;
|
162 |
}
|
163 |
|
164 |
#define INTRIANGLE_EPSILON 0e-9
|
165 |
|
166 |
P2trInTriangle
|
167 |
p2tr_math_intriangle (const P2trVector2 *A,
|
168 |
const P2trVector2 *B,
|
169 |
const P2trVector2 *C,
|
170 |
const P2trVector2 *P)
|
171 |
{
|
172 |
gdouble u, v;
|
173 |
return p2tr_math_intriangle2 (A, B, C, P, &u, &v);
|
174 |
}
|
175 |
|
176 |
P2trInTriangle
|
177 |
p2tr_math_intriangle2 (const P2trVector2 *A,
|
178 |
const P2trVector2 *B,
|
179 |
const P2trVector2 *C,
|
180 |
const P2trVector2 *P,
|
181 |
gdouble *u,
|
182 |
gdouble *v)
|
183 |
{
|
184 |
p2tr_math_triangle_barcycentric (A, B, C, P, u, v);
|
185 |
|
186 |
/* Check if point is in triangle - i.e. whether (u + v) < 1 and both
|
187 |
* u and v are positive */
|
188 |
if ((*u >= INTRIANGLE_EPSILON) && (*v >= INTRIANGLE_EPSILON) && (*u + *v < 1 - INTRIANGLE_EPSILON))
|
189 |
return P2TR_INTRIANGLE_IN;
|
190 |
else if ((*u >= -INTRIANGLE_EPSILON) && (*v >= -INTRIANGLE_EPSILON) && (*u + *v <= 1 + INTRIANGLE_EPSILON))
|
191 |
return P2TR_INTRIANGLE_ON;
|
192 |
else
|
193 |
return P2TR_INTRIANGLE_OUT;
|
194 |
}
|
195 |
|
196 |
/* The point in triangle circumcircle test, and the 3-point orientation
|
197 |
* test, are both based on the work of Jonathan Richard Shewchuk. The
|
198 |
* technique used here is described in his paper "Adaptive Precision
|
199 |
* Floating-Point Arithmetic and Fast Robust Geometric Predicates"
|
200 |
*/
|
201 |
|
202 |
#define ORIENT2D_EPSILON 1e-9
|
203 |
|
204 |
P2trOrientation p2tr_math_orient2d (const P2trVector2 *A,
|
205 |
const P2trVector2 *B,
|
206 |
const P2trVector2 *C)
|
207 |
{
|
208 |
/* We are trying to compute this determinant:
|
209 |
* |Ax Ay 1|
|
210 |
* |Bx By 1|
|
211 |
* |Cx Cy 1|
|
212 |
*/
|
213 |
gdouble result = p2tr_matrix_det3 (
|
214 |
A->x, A->y, 1,
|
215 |
B->x, B->y, 1,
|
216 |
C->x, C->y, 1
|
217 |
);
|
218 |
|
219 |
if (result > ORIENT2D_EPSILON)
|
220 |
return P2TR_ORIENTATION_CCW;
|
221 |
else if (result < -ORIENT2D_EPSILON)
|
222 |
return P2TR_ORIENTATION_CW;
|
223 |
else
|
224 |
return P2TR_ORIENTATION_LINEAR;
|
225 |
}
|
226 |
|
227 |
#define INCIRCLE_EPSILON 1e-9
|
228 |
|
229 |
/* Points must be given in CCW order!!!!! */
|
230 |
P2trInCircle
|
231 |
p2tr_math_incircle (const P2trVector2 *A,
|
232 |
const P2trVector2 *B,
|
233 |
const P2trVector2 *C,
|
234 |
const P2trVector2 *D)
|
235 |
{
|
236 |
/* We are trying to compute this determinant:
|
237 |
* |Ax Ay Ax^2+Ay^2 1|
|
238 |
* |Bx By Bx^2+By^2 1|
|
239 |
* |Cx Cy Cx^2+Cy^2 1|
|
240 |
* |Dx Dy Dx^2+Dy^2 1|
|
241 |
*/
|
242 |
gdouble result = p2tr_matrix_det4 (
|
243 |
A->x, A->y, P2TR_VECTOR2_LEN_SQ(A), 1,
|
244 |
B->x, B->y, P2TR_VECTOR2_LEN_SQ(B), 1,
|
245 |
C->x, C->y, P2TR_VECTOR2_LEN_SQ(C), 1,
|
246 |
D->x, D->y, P2TR_VECTOR2_LEN_SQ(D), 1
|
247 |
);
|
248 |
|
249 |
if (result > INCIRCLE_EPSILON)
|
250 |
return P2TR_INCIRCLE_IN;
|
251 |
else if (result < INCIRCLE_EPSILON)
|
252 |
return P2TR_INCIRCLE_OUT;
|
253 |
else
|
254 |
return P2TR_INCIRCLE_ON;
|
255 |
}
|
256 |
|
257 |
/* The point inside diametral-circle test and the point inside diametral
|
258 |
* lens test, are both based on the work of Jonathan Richard Shewchuk.
|
259 |
* The techniques used here are partially described in his paper
|
260 |
* "Delaunay Refinement Algorithms for Triangular Mesh Generation".
|
261 |
*
|
262 |
* W is inside the diametral circle (lens) of the line XY if and only if
|
263 |
* the angle XWY is larger than 90 (120) degrees. We know how to compute
|
264 |
* the cosine of that angle very easily like so:
|
265 |
*
|
266 |
* cos XWY = (WX * WY) / (|WX| * |WY|)
|
267 |
*
|
268 |
* Since XWY is larger than 90 (120) degrees, cos XWY <= 0 (-0.5) so:
|
269 |
*
|
270 |
* Diametral Circle | Diametral Lens
|
271 |
* -------------------------------+-----------------------------------
|
272 |
* 0 >= (WX * WY) / (|WX| * |WY|) | - 0.5 >= (WX * WY) / (|WX| * |WY|)
|
273 |
* 0 >= WX * WY | - 0.5 * |WX| * |WY| >= WX * WY
|
274 |
*/
|
275 |
|
276 |
gboolean
|
277 |
p2tr_math_diametral_circle_contains (const P2trVector2 *X,
|
278 |
const P2trVector2 *Y,
|
279 |
const P2trVector2 *W)
|
280 |
{
|
281 |
P2trVector2 WX, WY;
|
282 |
|
283 |
p2tr_vector2_sub(X, W, &WX);
|
284 |
p2tr_vector2_sub(Y, W, &WY);
|
285 |
|
286 |
return P2TR_VECTOR2_DOT(&WX, &WY) <= 0;
|
287 |
}
|
288 |
|
289 |
gboolean
|
290 |
p2tr_math_diametral_lens_contains (const P2trVector2 *X,
|
291 |
const P2trVector2 *Y,
|
292 |
const P2trVector2 *W)
|
293 |
{
|
294 |
P2trVector2 WX, WY;
|
295 |
|
296 |
p2tr_vector2_sub(X, W, &WX);
|
297 |
p2tr_vector2_sub(Y, W, &WY);
|
298 |
|
299 |
return P2TR_VECTOR2_DOT(&WX, &WY)
|
300 |
<= 0.5 * p2tr_vector2_norm(&WX) * p2tr_vector2_norm(&WY);
|
301 |
}
|