From 0ef1a7f7dc21f86cb9b1816baea5fc823d98c097 Mon Sep 17 00:00:00 2001 From: Barak Itkin Date: Sat, 21 Apr 2012 00:49:34 +0300 Subject: [PATCH] Add more necessary math functions --- refine/math.c | 206 +++++++++++++++++++++++++++++++++++++++++++++++ refine/math.h | 50 ++++++++++++ refine/utils.h | 12 +-- refine/vector2.c | 22 ++--- refine/vector2.h | 10 +-- 5 files changed, 278 insertions(+), 22 deletions(-) create mode 100644 refine/math.c create mode 100644 refine/math.h diff --git a/refine/math.c b/refine/math.c new file mode 100644 index 0000000..1c26159 --- /dev/null +++ b/refine/math.c @@ -0,0 +1,206 @@ +#include +#include "math.h" + +#define EDGE_LEN_SQ(X,Y) ((X) * (X) + (Y) * (Y)) +#define VECTOR2_LEN_SQ(V) (EDGE_LEN_SQ((V)->x, (V)->y)) +#define VECTOR2_DOT(V1,V2) ((V1)->x * (V2)->x + (V1)->y * (V2)->y) + +gdouble +p2tr_math_length_sq (gdouble x1, gdouble y1, + gdouble x2, gdouble y2) +{ + return EDGE_LEN_SQ (x1 - x2, y1 - y2); +} + +gdouble +p2tr_math_length_sq2 (const P2trVector2 *pt1, + const P2trVector2 *pt2) +{ + return p2tr_math_length_sq (pt1->x, pt1->y, pt2->x, pt2->y); +} + +static inline gdouble +p2tr_matrix_det2 (gdouble a00, gdouble a01, + gdouble a10, gdouble a11) +{ + return a00 * a11 - a10 * a01; +} + +static inline gdouble +p2tr_matrix_det3 (gdouble a00, gdouble a01, gdouble a02, + gdouble a10, gdouble a11, gdouble a12, + gdouble a20, gdouble a21, gdouble a22) +{ + return + + a00 * (a11 * a22 - a21 * a12) + - a01 * (a10 * a22 - a20 * a12) + + a02 * (a10 * a21 - a20 * a11); +} + +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) +{ + return + + a00 * p2tr_matrix_det3 (a11, a12, a13, + a21, a22, a23, + a31, a32, a33) + - a01 * p2tr_matrix_det3 (a10, a12, a13, + a20, a22, a23, + a30, a32, a33) + + a02 * p2tr_matrix_det3 (a10, a11, a13, + a20, a21, a23, + a30, a31, a33) + - a03 * p2tr_matrix_det3 (a10, a11, a12, + a20, a21, a22, + a30, a31, a32); +} + +/* The point in triangle test which is implemented below is based on the + * algorithm which appears on: + * + * http://www.blackpawn.com/texts/pointinpoly/default.html + */ +#define INTRIANGLE_EPSILON 0e-9 + +P2trInTriangle +p2tr_math_intriangle(const P2trVector2 *A, + const P2trVector2 *B, + const P2trVector2 *C, + const P2trVector2 *P) +{ + /* Compute the vectors offsetted so that A is the origin */ + P2trVector2 v0, v1, v2; + p2tr_vector2_sub(C, A, &v0); + p2tr_vector2_sub(B, A, &v1); + p2tr_vector2_sub(P, A, &v2); + + /* Compute dot products */ + double dot00 = VECTOR2_DOT(&v0, &v0); + double dot01 = VECTOR2_DOT(&v0, &v1); + double dot02 = VECTOR2_DOT(&v0, &v2); + double dot11 = VECTOR2_DOT(&v1, &v1); + double dot12 = VECTOR2_DOT(&v1, &v2); + + /* Compute barycentric coordinates */ + double invDenom = 1 / (dot00 * dot11 - dot01 * dot01); + double u = (dot11 * dot02 - dot01 * dot12) * invDenom; + double v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + /* Check if point is in triangle - i.e. whether (u + v) < 1 and both + * u and v are positive */ + if ((u >= INTRIANGLE_EPSILON) && (v >= INTRIANGLE_EPSILON) && (u + v < 1 - INTRIANGLE_EPSILON)) + return P2TR_INTRIANGLE_IN; + else if ((u >= -INTRIANGLE_EPSILON) && (v >= -INTRIANGLE_EPSILON) && (u + v <= 1 + INTRIANGLE_EPSILON)) + return P2TR_INTRIANGLE_ON; + else + return P2TR_INTRIANGLE_OUT; +} + +/* The point in triangle circumcircle test, and the 3-point orientation + * test, are both based on the work of Jonathan Richard Shewchuk. The + * technique used here is described in his paper "Adaptive Precision + * Floating-Point Arithmetic and Fast Robust Geometric Predicates" + */ + +#define ORIENT2D_EPSILON 1e-9 + +P2trOrientation p2tr_math_orient2d (const P2trVector2 *A, + const P2trVector2 *B, + const P2trVector2 *C) +{ + /* We are trying to compute this determinant: + * |Ax Ay 1| + * |Bx By 1| + * |Cx Cy 1| + */ + gdouble result = p2tr_matrix_det3 ( + A->x, A->y, 1, + B->x, B->y, 1, + C->x, C->y, 1 + ); + + if (result > ORIENT2D_EPSILON) + return P2TR_ORIENTATION_CCW; + else if (result < ORIENT2D_EPSILON) + return P2TR_ORIENTATION_CW; + else + return P2TR_ORIENTATION_LINEAR; +} + +#define INCIRCLE_EPSILON 1e-9 + +P2trInCircle +p2tr_math_incircle (const P2trVector2 *A, + const P2trVector2 *B, + const P2trVector2 *C, + const P2trVector2 *D) +{ + /* We are trying to compute this determinant: + * |Ax Ay Ax^2+Ay^2 1| + * |Bx By Bx^2+By^2 1| + * |Cx Cy Cx^2+Cy^2 1| + * |Dx Dy Dx^2+Dy^2 1| + */ + gdouble result = p2tr_matrix_det4 ( + A->x, A->y, VECTOR2_LEN_SQ(A), 1, + B->x, B->y, VECTOR2_LEN_SQ(B), 1, + C->x, C->y, VECTOR2_LEN_SQ(C), 1, + D->x, D->y, VECTOR2_LEN_SQ(D), 1 + ); + + if (result > INCIRCLE_EPSILON) + return P2TR_INCIRCLE_IN; + else if (result < INCIRCLE_EPSILON) + return P2TR_INCIRCLE_OUT; + else + return P2TR_INCIRCLE_ON; +} + +/* The point inside diametral-circle test and the point inside diametral + * lens test, are both based on the work of Jonathan Richard Shewchuk. + * The techniques used here are partially described in his paper + * "Delaunay Refinement Algorithms for Triangular Mesh Generation". + * + * W is inside the diametral circle (lens) of the line XY if and only if + * the angle XWY is larger than 90 (120) degrees. We know how to compute + * the cosine of that angle very easily like so: + * + * cos XWY = (WX * WY) / (|WX| * |WY|) + * + * Since XWY is larger than 90 (120) degrees, cos XWY <= 0 (-0.5) so: + * + * Diametral Circle | Diametral Lens + * -------------------------------+----------------------------------- + * 0 >= (WX * WY) / (|WX| * |WY|) | - 0.5 >= (WX * WY) / (|WX| * |WY|) + * 0 >= WX * WY | - 0.5 * |WX| * |WY| >= WX * WY + */ + +gboolean +p2tr_math_diametral_circle_contains (const P2trVector2 *X, + const P2trVector2 *Y, + const P2trVector2 *W) +{ + P2trVector2 WX, WY; + + p2tr_vector2_sub(X, W, &WX); + p2tr_vector2_sub(Y, W, &WY); + + return VECTOR2_DOT(&WX, &WY) <= 0; +} + +gboolean +p2tr_math_diametral_lens_contains (const P2trVector2 *X, + const P2trVector2 *Y, + const P2trVector2 *W) +{ + P2trVector2 WX, WY; + + p2tr_vector2_sub(X, W, &WX); + p2tr_vector2_sub(Y, W, &WY); + + return VECTOR2_DOT(&WX, &WY) + <= 0.5 * p2tr_vector2_norm(&WX) * p2tr_vector2_norm(&WY); +} diff --git a/refine/math.h b/refine/math.h new file mode 100644 index 0000000..266aaa9 --- /dev/null +++ b/refine/math.h @@ -0,0 +1,50 @@ +#ifndef __P2TC_REFINE_MATH_H__ +#define __P2TC_REFINE_MATH_H__ + +#include +#include "vector2.h" + +gdouble p2tr_math_length_sq (gdouble x1, + gdouble y1, + gdouble x2, + gdouble y2); + +gdouble p2tr_math_length_sq2 (const P2trVector2 *pt1, + const P2trVector2 *pt2); + +typedef enum +{ + P2TR_INTRIANGLE_OUT = -1, + P2TR_INTRIANGLE_ON = 0, + P2TR_INTRIANGLE_IN = 1 +} P2trInTriangle; + +P2trInTriangle p2tr_math_intriangle (const P2trVector2 *A, + const P2trVector2 *B, + const P2trVector2 *C, + const P2trVector2 *P); + +typedef enum +{ + P2TR_ORIENTATION_CW = -1, + P2TR_ORIENTATION_LINEAR = 0, + P2TR_ORIENTATION_CCW = 1 +} P2trOrientation; + +P2trOrientation p2tr_math_orient2d (const P2trVector2 *A, + const P2trVector2 *B, + const P2trVector2 *C); + +typedef enum +{ + P2TR_INCIRCLE_IN, + P2TR_INCIRCLE_ON, + P2TR_INCIRCLE_OUT +} P2trInCircle; + +P2trInCircle p2tr_math_incircle (const P2trVector2 *A, + const P2trVector2 *B, + const P2trVector2 *C, + const P2trVector2 *D); + +#endif \ No newline at end of file diff --git a/refine/utils.h b/refine/utils.h index a846155..b98931b 100755 --- a/refine/utils.h +++ b/refine/utils.h @@ -33,10 +33,10 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef __POLY2TRI_C_REFINE_UTILS_H__ -#define __POLY2TRI_C_REFINE_UTILS_H__ +#ifndef __P2TC_REFINE_UTILS_H__ +#define __P2TC_REFINE_UTILS_H__ -#ifdef __cplusplus +#ifdef __cplusplus extern "C" { #endif @@ -47,7 +47,7 @@ extern "C" * http://developer.gnome.org/glib/2.29/glib-Hash-Tables.html */ - typedef GHashTable P2tRHashSet; + typedef GHashTable P2trHashSet; typedef GHashTableIter P2trHashSetIter; #define p2tr_hash_set_set_new(hash_func, equal_func, destroy) g_hash_table_new_full ((hash_func), (equal_func), (destroy),NULL) @@ -64,8 +64,8 @@ extern "C" #define foreach(iter,list) for ((iter) = (list); (iter) != NULL; (iter) = (iter)->next) -#ifdef __cplusplus +#ifdef __cplusplus } #endif -#endif /* UTILS_H */ +#endif \ No newline at end of file diff --git a/refine/vector2.c b/refine/vector2.c index 8a19e8d..fde20b6 100644 --- a/refine/vector2.c +++ b/refine/vector2.c @@ -1,17 +1,17 @@ #include #include -#include "basic-geometry.h" +#include "vector2.h" gdouble -p2tr_vector2_dot (P2trVector2 *a, - P2trVector2 *b) +p2tr_vector2_dot (const P2trVector2 *a, + const P2trVector2 *b) { return a->x * b->x + a->y * b->y; } gboolean -p2tr_vector2_is_same (P2trVector2 *a, - P2trVector2 *b) +p2tr_vector2_is_same (const P2trVector2 *a, + const P2trVector2 *b) { if (a == NULL || b == NULL) return ! ((a == NULL) ^ (b == NULL)); @@ -20,23 +20,23 @@ p2tr_vector2_is_same (P2trVector2 *a, } void -p2tr_vector2_sub (P2trVector2 *a, - P2trVector2 *b, - P2trVector2 *dest) +p2tr_vector2_sub (const P2trVector2 *a, + const P2trVector2 *b, + P2trVector2 *dest) { dest->x = a->x - b->x; dest->y = a->y - b->y; } gdouble -p2tr_vector2_norm (P2trVector2 *v) +p2tr_vector2_norm (const P2trVector2 *v) { return sqrt (v->x * v->x + v->y * v->y); } void -p2tr_vector2_copy (P2trVector2 *dest, - P2trVector2 *src) +p2tr_vector2_copy (P2trVector2 *dest, + const P2trVector2 *src) { dest->x = src->x; dest->y = src->y; diff --git a/refine/vector2.h b/refine/vector2.h index 9d6a58c..36f3945 100644 --- a/refine/vector2.h +++ b/refine/vector2.h @@ -8,10 +8,10 @@ typedef struct { gdouble y; } P2trVector2; -gdouble p2tr_vector2_dot (P2trVector2 *a, P2trVector2 *b); -gboolean p2tr_vector2_is_same (P2trVector2 *a, P2trVector2 *b); -void p2tr_vector2_sub (P2trVector2 *a, P2trVector2 *b, P2trVector2 *dest); -gdouble p2tr_vector2_norm (P2trVector2 *v); -void p2tr_vector2_copy (P2trVector2 *dest, P2trVector2 *src); +gdouble p2tr_vector2_dot (const P2trVector2 *a, const P2trVector2 *b); +gboolean p2tr_vector2_is_same (const P2trVector2 *a, const P2trVector2 *b); +void p2tr_vector2_sub (const P2trVector2 *a, const P2trVector2 *b, P2trVector2 *dest); +gdouble p2tr_vector2_norm (const P2trVector2 *v); +void p2tr_vector2_copy (P2trVector2 *dest, const P2trVector2 *src); #endif \ No newline at end of file -- 2.50.1