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
p2tr_math_length_sq(gdouble x1,gdouble y1,gdouble x2,gdouble y2)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
p2tr_math_length_sq2(const P2trVector2 * pt1,const P2trVector2 * pt2)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
p2tr_matrix_det2(gdouble a00,gdouble a01,gdouble a10,gdouble a11)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
p2tr_matrix_det3(gdouble a00,gdouble a01,gdouble a02,gdouble a10,gdouble a11,gdouble a12,gdouble a20,gdouble a21,gdouble a22)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
p2tr_matrix_det4(gdouble a00,gdouble a01,gdouble a02,gdouble a03,gdouble a10,gdouble a11,gdouble a12,gdouble a13,gdouble a20,gdouble a21,gdouble a22,gdouble a23,gdouble a30,gdouble a31,gdouble a32,gdouble a33)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
p2tr_math_triangle_circumcircle(const P2trVector2 * A,const P2trVector2 * B,const P2trVector2 * C,P2trCircle * circle)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
p2tr_math_triangle_barcycentric(const P2trVector2 * A,const P2trVector2 * B,const P2trVector2 * C,const P2trVector2 * P,gdouble * u,gdouble * v)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
p2tr_math_intriangle(const P2trVector2 * A,const P2trVector2 * B,const P2trVector2 * C,const P2trVector2 * P)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
p2tr_math_intriangle2(const P2trVector2 * A,const P2trVector2 * B,const P2trVector2 * C,const P2trVector2 * P,gdouble * u,gdouble * v)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
p2tr_math_orient2d(const P2trVector2 * A,const P2trVector2 * B,const P2trVector2 * C)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
p2tr_math_incircle(const P2trVector2 * A,const P2trVector2 * B,const P2trVector2 * C,const P2trVector2 * D)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
p2tr_math_diametral_circle_contains(const P2trVector2 * X,const P2trVector2 * Y,const P2trVector2 * W)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
p2tr_math_diametral_lens_contains(const P2trVector2 * X,const P2trVector2 * Y,const P2trVector2 * W)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 }
302