SaraP
209 lines
10 KiB
C
209 lines
10 KiB
C
/*****************************************************************************/
|
|
/* */
|
|
/* Copyright (C) 1999-2023 M. Held */
|
|
/* */
|
|
/* This code is not in the public domain. All rights reserved! Please make */
|
|
/* sure to read the full copyright statement contained in "README.txt" or in */
|
|
/* the "main" file of this code, such as "main.cc". */
|
|
/* */
|
|
/*****************************************************************************/
|
|
/* */
|
|
/* Written by: Martin Held */
|
|
/* */
|
|
/* E-Mail: held@cs.sbg.ac.at */
|
|
/* Fax Mail: (+43 662) 8044-611 */
|
|
/* Voice Mail: (+43 662) 8044-6304 */
|
|
/* Snail Mail: Martin Held */
|
|
/* FB Informatik */
|
|
/* Universitaet Salzburg */
|
|
/* A-5020 Salzburg, Austria */
|
|
/* */
|
|
/*****************************************************************************/
|
|
|
|
#ifndef VRONI_NUMERICS_H
|
|
#define VRONI_NUMERICS_H
|
|
|
|
|
|
#define Det2D(u, v, w) \
|
|
(((u).x - (v).x) * ((v).y - (w).y) + ((v).y - (u).y) * ((v).x - (w).x))
|
|
|
|
|
|
/* */
|
|
/* this macro solves the equation p + s * q = u + t * v */
|
|
/* for the parameter s, with p and u being points and q and v */
|
|
/* being direction vectors of the lines through p and u. */
|
|
/* if the equation cannot be solved (since the two lines are parallel) then */
|
|
/* exists = 0; if the solution is not unique then exists = 2; and */
|
|
/* exists = 1, otherwise. also, w := p + xy[0] * q. */
|
|
/* */
|
|
/* A[2][2], B[2], xy[2], I0, I1, J0, J1 are used internally within this */
|
|
/* macro. */
|
|
/* */
|
|
#define LineLineIntersection(A, B, xy, I0, J0, I1, J1, p, q, u, v, w, exists) \
|
|
{\
|
|
A[0][0] = (q).x; \
|
|
A[1][0] = (q).y; \
|
|
A[0][1] = - (v).x; \
|
|
A[1][1] = - (v).y; \
|
|
B[0] = (u).x - (p).x; \
|
|
B[1] = (u).y - (p).y; \
|
|
LinearEqnSolver_2x2(A, B, xy, exists, I0, J0, I1, J1); \
|
|
if (exists == 1) { \
|
|
(w).x = (p).x + xy[0] * (q).x; \
|
|
(w).y = (p).y + xy[0] * (q).y; \
|
|
}\
|
|
}
|
|
|
|
|
|
|
|
/* */
|
|
/* P ... query point */
|
|
/* U ... start point of segment */
|
|
/* A, B, C ... normalized line coefficients, where (B,-A) is the unit */
|
|
/* direction vector of the line segment */
|
|
/* L ... length of the line segment */
|
|
/* */
|
|
#define IsInSegConeStrict(U, P, A, B, L) \
|
|
(numerics_h_local = ((P).x - (U).x) * (B) - ((P).y - (U).y) * (A), \
|
|
((numerics_h_local < (L)) ? ((numerics_h_local <= 0.0) ? -1 : 0) : 1))
|
|
|
|
|
|
#define IsInSegCone(U, P, A, B, L) \
|
|
(numerics_h_local = ((P).x - (U).x) * (B) - ((P).y - (U).y) * (A), \
|
|
((numerics_h_local <= ((L) + ZERO)) ? ((numerics_h_local < -ZERO) ? -1 : 0) : 1))
|
|
|
|
|
|
#define IsInSegConeZero(U, P, A, B, L, eps) \
|
|
(numerics_h_local = ((P).x - (U).x) * (B) - ((P).y - (U).y) * (A), \
|
|
((numerics_h_local <= ((L) + (eps))) ? ((numerics_h_local < -(eps)) ? -1 : 0) : 1))
|
|
|
|
|
|
#define PntSegDist(U, P, A, B, C, L, eps) \
|
|
((IsInSegConeZero(U, P, A, B, L, eps) == 0) ? \
|
|
(PntLineDist(A, B, C, P)) : LARGE)
|
|
|
|
|
|
#define AbsPntSegDist(U, P, A, B, C, L, eps) \
|
|
((IsInSegConeZero(U, P, A, B, L, eps) == 0) ? \
|
|
(AbsPntLineDist(A, B, C, P)) : LARGE)
|
|
|
|
|
|
#define xParabola(A, a, b, sign, dist, t, k1, k2, r, x) \
|
|
{ \
|
|
numerics_h_det = r * r + 2.0 * t * (r * k1 - dist * k2) - dist * dist; \
|
|
if (numerics_h_det <= 0.0) { \
|
|
(x) = A - a * t * k2; \
|
|
} \
|
|
else { \
|
|
(x) = A - a * t * k2 + sign * b * sqrt(numerics_h_det); \
|
|
} \
|
|
}
|
|
|
|
#define yParabola(B, a, b, sign, dist, t, k1, k2, r, y) \
|
|
{ \
|
|
numerics_h_det = r * r + 2.0 * t * (r * k1 - dist * k2) - dist * dist; \
|
|
if (numerics_h_det <= 0.0) { \
|
|
(y) = B - b * t * k2; \
|
|
} \
|
|
else { \
|
|
(y) = B - b * t * k2 - sign * a * sqrt(numerics_h_det); \
|
|
} \
|
|
}
|
|
|
|
#define xHyperEll(A, C, sign, dist, dy, t, k1, k2, r1, r2, r1t, r2t, ht, x) \
|
|
{ \
|
|
r1t = r1 + k1 * t; \
|
|
r2t = r2 + k2 * t; \
|
|
ht = (r2t * r2t - r1t * r1t - dist * dist) / (2.0 * dist); \
|
|
numerics_h_det = r1t * r1t - ht * ht; \
|
|
if (numerics_h_det <= 0.0) { \
|
|
(x) = A - C * t; \
|
|
} \
|
|
else { \
|
|
(x) = A - C * t + sign * dy * sqrt(numerics_h_det); \
|
|
} \
|
|
}
|
|
|
|
|
|
#define yHyperEll(B, D, sign, dist, dx, t, k1, k2, r1, r2, r1t, r2t, ht, y) \
|
|
{ \
|
|
r1t = r1 + k1 * t; \
|
|
r2t = r2 + k2 * t; \
|
|
ht = (r2t * r2t - r1t * r1t - dist * dist) / (2.0 * dist); \
|
|
numerics_h_det = r1t * r1t - ht * ht; \
|
|
if (numerics_h_det <= 0.0) { \
|
|
(y) = B - D * t; \
|
|
} \
|
|
else { \
|
|
(y) = B - D * t - sign * dx * sqrt(numerics_h_det); \
|
|
} \
|
|
}
|
|
|
|
|
|
|
|
/* */
|
|
/* A, B, C ... normalized coefficients of the equation of the arc's chord */
|
|
/* such that the arc's center is to the left of the chord */
|
|
/* P ... query point */
|
|
/* */
|
|
/* note: we assume that the arc is oriented CCW, and that a point-on-circle */
|
|
/* test has been performed! */
|
|
/* */
|
|
#define IsOnArc(A, B, C, P) \
|
|
(PntLineDist(A, B, C, P) <= ZERO)
|
|
|
|
|
|
#define IsOnArcStrict(A, B, C, P) \
|
|
(PntLineDist(A, B, C, P) < 0.0)
|
|
|
|
|
|
#define IsOnArcZero(A, B, C, P, eps) \
|
|
(PntLineDist(A, B, C, P) <= (eps))
|
|
|
|
|
|
/* */
|
|
/* VS ... unit outwards normal vector of arc at start point */
|
|
/* VE ... unit outwards normal vector of arc at end point */
|
|
/* CP ... vector from arc's center to query point */
|
|
/* */
|
|
/* note: (1) we assume that the arc is oriented CCW, and */
|
|
/* that it spans less than 180 degrees! */
|
|
/* (2) if a point is inside a circle then its distance to the circle */
|
|
/* is negative. */
|
|
/* */
|
|
#define IsInArcCone(VS, VE, CP) \
|
|
((((VS).x * (CP).y - (VS).y * (CP).x) >= -ZERO) ? \
|
|
((((VE).y * (CP).x - (VE).x * (CP).y) >= -ZERO) ? 0 : -1) : 1)
|
|
|
|
|
|
#define IsInArcConeZero(VS, VE, CP, eps) \
|
|
((((VS).x * (CP).y - (VS).y * (CP).x) >= -(eps)) ? \
|
|
((((VE).y * (CP).x - (VE).x * (CP).y) >= -(eps)) ? 0 : -1) : 1)
|
|
|
|
|
|
#define IsInArcConeStrict(VS, VE, CP) \
|
|
((((VS).x * (CP).y - (VS).y * (CP).x) > 0.0) ? \
|
|
((((VE).y * (CP).x - (VE).x * (CP).y) > 0.0) ? 0 : -1) : 1)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#define PntArcDist(VS, VE, CP, R, eps) \
|
|
((((VS).x * (CP).y - (VS).y * (CP).x) >= -(eps)) ? \
|
|
((((VE).y * (CP).x - (VE).x * (CP).y) >= -(eps)) ? (VecLen(CP) - R) \
|
|
: LARGE) : LARGE)
|
|
|
|
|
|
#define AbsPntArcDist(VS, VE, CP, R, Z) \
|
|
((((VS).x * (CP).y - (VS).y * (CP).x) >= -(Z)) ? \
|
|
((((VE).y * (CP).x - (VE).x * (CP).y) >= -(Z)) ? \
|
|
(numerics_h_local = VecLen(CP) - R, \
|
|
((numerics_h_local < 0.0) ? -numerics_h_local : numerics_h_local)) : \
|
|
LARGE) : LARGE)
|
|
|
|
|
|
#endif
|
|
|