SaraP
1038 lines
38 KiB
C++
1038 lines
38 KiB
C++
/*****************************************************************************/
|
|
/* */
|
|
/* Copyright (C) 1999-2025 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 */
|
|
/* */
|
|
/*****************************************************************************/
|
|
|
|
/* */
|
|
/* get standard libraries */
|
|
/* */
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <float.h>
|
|
|
|
/* */
|
|
/* get my header files */
|
|
/* */
|
|
#include "fpkernel.h"
|
|
#include "vronivector.h"
|
|
#include "vroni_object.h"
|
|
#include "numerics.h"
|
|
#include "roots.h"
|
|
|
|
|
|
|
|
#ifdef WITH_MPFRBACKEND
|
|
|
|
double SMALL = 0.0078125;
|
|
double INVERSE_SMALL = 128.0;
|
|
double ZERO_IO = 1.0e-5;
|
|
double ZERO_MAX = 1.0e-5;
|
|
double ZERO = 1.0e-13;
|
|
double ZERO2 = 1.0e-26;
|
|
double TINY = 1.0e-14;
|
|
double THESHOLD = 1.0e-6;
|
|
|
|
//"Constants"
|
|
double M_1_3 = 0.33333333333333333333;
|
|
double M_180_PI = 57.2957795130823208767;
|
|
double M_PI_180 = 0.01745329251994329576;
|
|
#undef M_SQRT2
|
|
double M_SQRT2 = 1.41421356237309504880;
|
|
#undef M_PI_4
|
|
double M_PI_4 = 0.78539816339744830962;
|
|
#undef M_PI_2
|
|
double M_PI_2 = 1.57079632679489661923;
|
|
#undef M_PI
|
|
double M_PI = 3.14159265358979323846;
|
|
#undef M_2PI
|
|
double M_2PI = 6.28318530717958623199;
|
|
|
|
|
|
void vroniObject::set_mpfrbackend_prec(mpfr_prec_t mpfr_prec, vr_bool verbose)
|
|
{
|
|
mpfr_set_default_prec(mpfr_prec);
|
|
|
|
M_PI.set_precision(mpfr_prec);
|
|
M_PI.set_pi();
|
|
|
|
M_2PI.set_precision(mpfr_prec);
|
|
M_2PI = M_PI * 2;
|
|
|
|
M_PI_2.set_precision(mpfr_prec);
|
|
M_PI_2 = M_PI / 2;
|
|
|
|
M_PI_4.set_precision(mpfr_prec);
|
|
M_PI_4 = M_PI / 4;
|
|
|
|
M_PI_180.set_precision(mpfr_prec);
|
|
M_PI_180 = M_PI / 180;
|
|
|
|
M_180_PI.set_precision(mpfr_prec);
|
|
M_180_PI = 180 / M_PI;
|
|
|
|
M_1_3.set_precision(mpfr_prec);
|
|
M_1_3 = double(1) / 3;
|
|
|
|
M_SQRT2.set_precision(mpfr_prec);
|
|
M_SQRT2 = sqrt(double(2));
|
|
|
|
|
|
/* Set appropriate values for SMALL, etc...
|
|
|
|
In the pattern pow(double(2), double(const * mpfr_prec / 53)),
|
|
const is calculated as log_2(plain_zero) where plain_zero is the value
|
|
from the double precision version of VRONI */
|
|
|
|
|
|
SMALL.set_precision(mpfr_prec);
|
|
SMALL = pow(double(2), double(-7.0 * mpfr_prec / 53));
|
|
|
|
INVERSE_SMALL.set_precision(mpfr_prec);
|
|
INVERSE_SMALL = 1 / SMALL;
|
|
|
|
ZERO_MAX.set_precision(mpfr_prec);
|
|
ZERO_MAX = pow(double(2), double(-26.57 * mpfr_prec / 53));
|
|
|
|
ZERO.set_precision(mpfr_prec);
|
|
ZERO = pow(double(2), double(-43.19 * mpfr_prec / 53));
|
|
|
|
ZERO2.set_precision(mpfr_prec);
|
|
ZERO2 = ZERO * ZERO;
|
|
|
|
TINY.set_precision(mpfr_prec);
|
|
TINY = pow(double(2), double(-46.50 * mpfr_prec / 53));
|
|
|
|
if(verbose) {
|
|
FP_printf("Setting SMALL to %e\n", FP_PRNTARG(SMALL));
|
|
FP_printf("Setting INVERSE_SMALL to %e\n", FP_PRNTARG(INVERSE_SMALL));
|
|
FP_printf("Setting ZERO_MAX to %e\n", FP_PRNTARG(ZERO_MAX));
|
|
FP_printf("Setting ZERO to %e\n", FP_PRNTARG(ZERO));
|
|
FP_printf("Setting ZERO2 to %e\n", FP_PRNTARG(ZERO2));
|
|
FP_printf("Setting TINY to %e\n", FP_PRNTARG(TINY));
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
#define COUNTER_MAGIC 100
|
|
|
|
|
|
#define AssignAndReturnSolution(TQ, Q, T, P) \
|
|
{\
|
|
TQ = T; \
|
|
Q = P; \
|
|
return true; \
|
|
}
|
|
|
|
|
|
|
|
#define AssignBetterSolution(D, DQ, T, TQ) \
|
|
{ \
|
|
D = Abs(D); \
|
|
if (D < DQ) { \
|
|
DQ = D; \
|
|
TQ = T; \
|
|
} \
|
|
}
|
|
|
|
|
|
|
|
double vroniObject::ComputeSiteDistance(int i, t_site ti, const coord & p)
|
|
{
|
|
double dist = LARGE, lgth;
|
|
coord q, ns, ne, ca, v;
|
|
double a, b,c;
|
|
|
|
switch(ti) {
|
|
case PNT:
|
|
q = GetPntCoords(i);
|
|
dist = PntPntDist(p, q);
|
|
break;
|
|
case SEG:
|
|
q = GetSegStartCoord(i);
|
|
lgth = GetSegLgth(i);
|
|
GetSegEqnData(i, &a, &b, &c);
|
|
switch(IsInSegConeZero(q, p, a, b, lgth, ZERO_MAX)) {
|
|
case -1:
|
|
dist = LARGE;
|
|
break;
|
|
case 0:
|
|
dist = a * p.x + b * p.y +c;
|
|
dist = Abs(dist);
|
|
break;
|
|
case 1:
|
|
q = GetSegEndCoord(i);
|
|
dist = LARGE;
|
|
break;
|
|
}
|
|
break;
|
|
case ARC:
|
|
ns = GetArcStartNormal(i);
|
|
ne = GetArcEndNormal(i);
|
|
ca = GetArcCenter(i);
|
|
v = VecSub(p, ca);
|
|
switch(IsInArcConeZero(ns, ne, v, ZERO_MAX)) {
|
|
case -1:
|
|
q = GetArcStartCoord(i);
|
|
dist = PntPntDist(p, q);
|
|
dist = LARGE;
|
|
break;
|
|
case 0:
|
|
dist = VecLen(v) - GetArcRadius(i);
|
|
dist = Abs(dist);
|
|
break;
|
|
case 1:
|
|
q = GetArcEndCoord(i);
|
|
dist = PntPntDist(p, q);
|
|
dist = LARGE;
|
|
break;
|
|
}
|
|
break;
|
|
default:
|
|
assert(0);
|
|
}
|
|
|
|
return dist;
|
|
}
|
|
|
|
|
|
|
|
|
|
/* */
|
|
/* computes the bisector Q + lambda * V of the supporting lines of the */
|
|
/* segments segs[i],segs[j]. the correct bisector is chosen by means of */
|
|
/* the normal vectors (ai,bi) and (aj,bj). this functions returns false */
|
|
/* if it cannot compute a bisector due to numerical problems. note that the */
|
|
/* parameterization does not reflect the distance to the sites! */
|
|
/* */
|
|
vr_bool vroniObject::SegSegBisector(int i, int j, double_arg ai, double_arg bi,
|
|
double_arg aj, double_arg bj,
|
|
double_arg cj, coord *Q, coord *V)
|
|
{
|
|
coord A, B, C, U, W, D, E, F;
|
|
double AA[2][2], BB[2], xy[2];
|
|
int exists, I0, I1, J0, J1;
|
|
int ms, me;
|
|
double lambda, delta, dist, dist_c, dist_d, t_s, t_e, dist_a, dist_b;
|
|
int counter = 0, sign_s, sign_e;
|
|
|
|
#ifdef TRACE
|
|
if (center) {
|
|
printf("SegSegBisector - ai = %24.20f, bi = %24.20f\n", ai, bi);
|
|
printf("SegSegBisector - aj = %24.20f, bj = %24.20f\n", aj, bj);
|
|
}
|
|
#endif
|
|
|
|
delta = 1.0 + ai * aj + bi * bj;
|
|
if (eq(delta, SMALL)) {
|
|
/* */
|
|
/* the lines are (nearly) parallel and their normal vectors point */
|
|
/* into opposing directions. */
|
|
/* */
|
|
*V = MakeVec( (bi - bj) / 2.0, (aj - ai) / 2.0);
|
|
#ifdef TRACE
|
|
if (center) {
|
|
printf("SegSegBisector - nearly parallel, opposing normals\n");
|
|
printf("SegSegBisector - direction vector V: (%24.20f, %24.20f)\n",
|
|
V->x, V->y);
|
|
}
|
|
#endif
|
|
}
|
|
else {
|
|
*V = MakeVec( (ai + aj) / 2.0, (bi + bj) / 2.0);
|
|
#ifdef TRACE
|
|
if (center) {
|
|
printf("SegSegBisector - not opposing normals\n");
|
|
printf("SegSegBisector - direction vector V: (%24.20f, %24.20f)\n",
|
|
V->x, V->y);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
/* */
|
|
/* check whether Q is trivially obtained by using the common end point of */
|
|
/* of the two segments. */
|
|
/* */
|
|
ms = Get1stVtx(i, SEG);
|
|
me = Get2ndVtx(i, SEG);
|
|
|
|
if (IsSegStartPnt(j, ms) || IsSegEndPnt(j, ms)) {
|
|
*Q = GetSegStartCoord(i);
|
|
#ifdef TRACE
|
|
if (center) {
|
|
printf("SegSegBisector - start point Q; (%24.20f, %24.20f)\n",
|
|
Q->x, Q->y);
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
else if (IsSegStartPnt(j, me) || IsSegEndPnt(j, me)) {
|
|
*Q = GetSegEndCoord(i);
|
|
#ifdef TRACE
|
|
if (center) {
|
|
printf("SegSegBisector start point Q; (%24.20f, %24.20f)\n",
|
|
Q->x, Q->y);
|
|
}
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
if (eq(delta, SMALL)) {
|
|
/* */
|
|
/* the lines are (nearly) parallel and their normal vectors point */
|
|
/* into opposing directions. we compute a point Q on the bisector */
|
|
/* between both lines by computing the center of those two lines and a */
|
|
/* third line that is perpendicular to the first line. */
|
|
/* */
|
|
#ifdef TRACE
|
|
if (center)
|
|
printf("SegSegBisector - nearly parallel, opposing normals\n");
|
|
#endif
|
|
A = GetSegStartCoord(i);
|
|
lambda = - PntLineDist(aj, bj, cj, A) / (delta - 1.0);
|
|
W = MakeVec( ai, bi);
|
|
B = RayPnt(A, W, lambda);
|
|
U = MakeVec( ai + bi, bi - ai);
|
|
W = MakeVec( aj + bi, bj - ai);
|
|
LineLineIntersection(AA, BB, xy, I0, J0, I1, J1, A, U, B, W, *Q,
|
|
exists);
|
|
if (exists != 1) {
|
|
/* */
|
|
/* for reasons unknown we could not compute the intersection */
|
|
/* of two lines that intersect at an angle of close to 90 deg. */
|
|
/* */
|
|
VD_Warning("SegSegBisector() - lines are parallel!");
|
|
return false;
|
|
}
|
|
#ifdef TRACE
|
|
else {
|
|
if (center)
|
|
printf("start point Q; (%24.20f, %24.20f)\n", Q->x, Q->y);
|
|
}
|
|
#endif
|
|
}
|
|
else {
|
|
delta = ai * bj - aj * bi;
|
|
if (eq(delta, SMALL)) {
|
|
/* */
|
|
/* the two lines are (nearly) collinear but do not share a */
|
|
/* common vertex. we try to compute an intersection iteratively. */
|
|
/* */
|
|
#ifdef TRACE
|
|
if (center)
|
|
printf("SegSegBisector - nearly parallel but don't share vertex\n");
|
|
#endif
|
|
W = GetSegDirection(i);
|
|
t_s = 0.0;
|
|
t_e = GetSegLgth(i);
|
|
D = GetSegStartCoord(i);
|
|
B = GetSegEndCoord(i);
|
|
dist_a = PntLineDist(aj, bj, cj, D);
|
|
dist_b = PntLineDist(aj, bj, cj, B);
|
|
sign_s = Sign(dist_a);
|
|
sign_e = Sign(dist_b);
|
|
if (sign_s == 0) {
|
|
*Q = D;
|
|
return true;
|
|
}
|
|
else if (sign_e == 0) {
|
|
*Q = B;
|
|
return true;
|
|
}
|
|
else if (sign_s == sign_e) {
|
|
while (counter < COUNTER_MAGIC) {
|
|
delta = t_e - t_s;
|
|
t_s -= delta;
|
|
t_e += delta;
|
|
A = RayPnt(D, W, t_s);
|
|
B = RayPnt(D, W, t_e);
|
|
dist_a = PntLineDist(aj, bj, cj, A);
|
|
dist_b = PntLineDist(aj, bj, cj, B);
|
|
sign_s = Sign(dist_a);
|
|
sign_e = Sign(dist_b);
|
|
if (sign_s == 0) {
|
|
*Q = A;
|
|
return true;
|
|
}
|
|
else if (sign_e == 0) {
|
|
*Q = B;
|
|
return true;
|
|
}
|
|
else if (sign_s != sign_e) {
|
|
counter = COUNTER_MAGIC;
|
|
}
|
|
++counter;
|
|
}
|
|
}
|
|
if (sign_s != sign_e) {
|
|
/* */
|
|
/* we have an enclosing parameter interval [t_s, t_e] of the */
|
|
/* intersection. let's see whether we can compute the */
|
|
/* intersection Q iteratively. */
|
|
/* */
|
|
#ifdef TRACE
|
|
if (center) {
|
|
printf("SegSegBisector - iterative solution sought\n");
|
|
printf("SegSegBisector - t_s = %24.20f, dist_a = %24.20f\n",
|
|
t_s, dist_a);
|
|
printf("SegSegBisector - t_e = %24.20f, dist_b = %24.20f\n",
|
|
t_e, dist_b);
|
|
}
|
|
|
|
#endif
|
|
delta = (t_s + t_e) / 2.0;
|
|
assert(t_s < t_e);
|
|
assert(t_s < delta);
|
|
assert(t_e > delta);
|
|
if (IterativeIntersection(SEG, SEG, aj, bj, cj, B, dist_a,
|
|
D, t_s, t_e, W, 0.0, &delta, Q))
|
|
return true;
|
|
#ifdef TRACE
|
|
if (center)
|
|
printf("SegSegBisector - iterative solution not found\n");
|
|
#endif
|
|
}
|
|
|
|
/* */
|
|
/* we compute the midpoint of their closest pair of vertices. */
|
|
/* */
|
|
A = GetSegStartCoord(i);
|
|
B = GetSegEndCoord(i);
|
|
C = GetSegStartCoord(j);
|
|
D = GetSegEndCoord(j);
|
|
dist_c = PntPntDistSq(A, C);
|
|
dist_d = PntPntDistSq(A, D);
|
|
if (dist_c < dist_d) {
|
|
F = C;
|
|
dist = dist_c;
|
|
}
|
|
else {
|
|
F = D;
|
|
dist = dist_d;
|
|
}
|
|
dist_c = PntPntDistSq(B, C);
|
|
dist_d = PntPntDistSq(B, D);
|
|
if (dist_c < dist_d) {
|
|
if (dist_c < dist) {
|
|
E = B;
|
|
F = C;
|
|
}
|
|
else {
|
|
E = A;
|
|
}
|
|
}
|
|
else {
|
|
if (dist_d < dist) {
|
|
E = B;
|
|
F = D;
|
|
}
|
|
else {
|
|
E = A;
|
|
}
|
|
}
|
|
*Q = MidPoint(E, F);
|
|
#ifdef TRACE
|
|
if (center)
|
|
printf("SegSegBisector - start point Q; (%24.20f, %24.20f)\n",
|
|
Q->x, Q->y);
|
|
#endif
|
|
}
|
|
else {
|
|
/* */
|
|
/* we compute the intersection of the two lines. */
|
|
/* */
|
|
#ifdef TRACE
|
|
if (center)
|
|
printf("SegSegBisector - normal\n");
|
|
#endif
|
|
A = GetSegStartCoord(i);
|
|
lambda = PntLineDist(aj, bj, cj, A) / delta;
|
|
W = MakeVec( bi, -ai);
|
|
*Q = RayPnt(A, W, lambda);
|
|
#ifdef TRACE
|
|
if (center)
|
|
printf("SegSegBisector - start point Q; (%24.20f, %24.20f)\n",
|
|
Q->x, Q->y);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
/* */
|
|
/* intersects the ray with direction vector v that originates at pnt p */
|
|
/* with the offset line of the line with equation a*x+b*y+c=0: */
|
|
/* */
|
|
/* */
|
|
/* p + s1*t*v and a*x + b*y + c - s2 * sgn(dist) * t = 0, */
|
|
/* */
|
|
/* where dist := (a,b)*p + c and */
|
|
/* */
|
|
/* | +1 if k1 = +1 | +1 if k2 = +1 */
|
|
/* s1 = | -1 if k1 = -1 and s2 = | -1 if k2 = -1. */
|
|
/* | +/-1 if k1 = 0 | +/-1 if k1 = 0 */
|
|
/* */
|
|
/* for every combination of s1,s2 it stores the point of intersection and */
|
|
/* the (positive) radius in the fields centers[] and radii[]. */
|
|
/* */
|
|
/* note: we assume that v is a unit vector! */
|
|
/* */
|
|
vr_bool vroniObject::IntersectRayOffsetSeg(const coord & p, const coord & v,
|
|
double_arg a, double_arg b,
|
|
double_arg c, int k1, int k2,
|
|
coord *centers, double *radii,
|
|
int *num_solutions)
|
|
{
|
|
int s1, s2, sign;
|
|
double t;
|
|
int s1_min, s2_min, s1_max, s2_max;
|
|
|
|
assert((-1 <= k1) && (k1 <= 1));
|
|
assert((-1 <= k2) && (k2 <= 1));
|
|
|
|
/* */
|
|
/* compute the signed distance of p from the line. */
|
|
/* */
|
|
const double dist = PntLineDist(a, b, c, p);
|
|
const double dot = v.x * a + v.y * b;
|
|
|
|
*num_solutions = 0;
|
|
sign = SignEps(dist, ZERO);
|
|
if (sign == 0) return false;
|
|
|
|
/* */
|
|
/* assign the sliding directions */
|
|
/* */
|
|
if (k1 == 0) {
|
|
s1_min = -1;
|
|
s1_max = 1;
|
|
}
|
|
else {
|
|
s1_min = k1;
|
|
s1_max = k1;
|
|
}
|
|
if (k2 == 0) {
|
|
s2_min = -1;
|
|
s2_max = 1;
|
|
}
|
|
else {
|
|
s2_min = k2;
|
|
s2_max = k2;
|
|
}
|
|
|
|
for (s1 = s1_min; s1 <= s1_max; s1 += 2) {
|
|
for (s2 = s2_min; s2 <= s2_max; s2 += 2) {
|
|
t = s2 * sign - s1 * dot;
|
|
if (ne(t, ZERO)) {
|
|
t = dist / t;
|
|
if (gt(t, ZERO)) {
|
|
centers[*num_solutions] = RayPnt(p, v, s1 * t);
|
|
radii[*num_solutions] = Abs(t);
|
|
++(*num_solutions);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
#define EvalObjDist(TYPE, A, B, C, P, R, Q, D) \
|
|
{ \
|
|
assert((TYPE == SEG) || (TYPE == ARC)); \
|
|
if (TYPE == SEG) { \
|
|
D = PntLineDist(A, B, C, Q); \
|
|
} \
|
|
else { \
|
|
D = PntCircleDist(P, R, Q); \
|
|
} \
|
|
}
|
|
|
|
|
|
#define EvalObjParam(TYPE, CP, VEC, RAD, PARAM, Q) \
|
|
{ \
|
|
assert((TYPE == SEG) || (TYPE == ARC)); \
|
|
if (TYPE == SEG) { \
|
|
Q = RayPnt(CP, VEC, PARAM); \
|
|
} \
|
|
else { \
|
|
Q = CirclePnt(CP, RAD, PARAM); \
|
|
} \
|
|
}
|
|
|
|
|
|
/* */
|
|
/* computes one intersection between an infinite line or full circle and a */
|
|
/* line segment or circular arc. the object types are determined by the */
|
|
/* parameters type1 and type2: SEG and/or ARC. */
|
|
/* */
|
|
/* if type1 == SEG then the first object is an infinite line represented */
|
|
/* by the equation a * x + b * y + c = 0. */
|
|
/* if type1 == ARC then the first object is a full circle with */
|
|
/* center c1 and radius r1. */
|
|
/* if type2 == SEG then the second object is a line segment defined by the */
|
|
/* start point p2, the direction vector v2, and the */
|
|
/* parameter interval [t_s, t_e]. thus, the line segment */
|
|
/* is given by p2 + t * v2, with t in [t_s, t_e]. */
|
|
/* if type2 == ARC then the second object is a circular arc defined by the */
|
|
/* center p2, radius r2, and the parameter interval */
|
|
/* [t_s, t_e] (in radians). thus, the circular arc is */
|
|
/* given by (p2.x + r2 * cos(t), p2.y + r2 * sin(t)), */
|
|
/* with t in [t_s, t_e]. */
|
|
/* */
|
|
/* note: (1) we assume that an intersection exists within [t_s, t_e]. */
|
|
/* (2) also, we assume that the points defined by t_s and t_e lie */
|
|
/* on different sides of object 1. */
|
|
/* (3) for the sake of numerical accuracy, the intersection will be */
|
|
/* computed iteratively, using binary bisection. */
|
|
/* (4) the parameter t_q is used as seed for the iteration. */
|
|
/* (5) t_s <= t_q <= t_e is assumed. */
|
|
/* */
|
|
vr_bool vroniObject::IterativeIntersection(/* object types */
|
|
t_site type1, t_site type2,
|
|
/* line 1 */
|
|
double_arg a, double_arg b,
|
|
double_arg c,
|
|
/* circle 1 */
|
|
const coord & c1, double_arg r1,
|
|
/* start point or center of obj 2 */
|
|
const coord & p2,
|
|
/* parameters of obj 2 */
|
|
double t_s, double t_e,
|
|
/* direction vector of seg 2 */
|
|
const coord & v2,
|
|
/* radius of arc 2 */
|
|
double_arg r2,
|
|
/* intersection data */
|
|
double *t_q, coord *q)
|
|
{
|
|
double t, t_z, dist, dist_s, dist_e, dist_q, dist_z;
|
|
int sign_s, sign, sign_e, sign_z, counter;
|
|
coord p;
|
|
vr_bool keep_going;
|
|
#ifdef TRACE
|
|
vr_bool debug_itint = false;
|
|
#endif
|
|
|
|
#ifdef TRACE
|
|
if (debug_itint) {
|
|
printf("\nIterativeIntersection()\n");
|
|
printf("t_s = %30.16f\n", t_s);
|
|
printf("t_e = %30.16f\n", t_e);
|
|
printf("t_q = %30.16f\n", *t_q);
|
|
}
|
|
#endif
|
|
t = *t_q;
|
|
assert(t_s <= t_e);
|
|
assert((t_s - ZERO) <= t);
|
|
assert((t_e + ZERO) >= t);
|
|
|
|
/* */
|
|
/* evaluate the distances of p(t_s), p(t_e), and p(t_q) from obj 1. */
|
|
/* */
|
|
EvalObjParam(type2, p2, v2, r2, t, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist);
|
|
sign = Sign(dist);
|
|
if (sign == 0) AssignAndReturnSolution(*t_q, *q, t, p);
|
|
|
|
EvalObjParam(type2, p2, v2, r2, t_s, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist_s);
|
|
sign_s = Sign(dist_s);
|
|
if (sign_s == 0) AssignAndReturnSolution(*t_q, *q, t_s, p);
|
|
|
|
EvalObjParam(type2, p2, v2, r2, t_e, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist_e);
|
|
sign_e = Sign(dist_e);
|
|
if (sign_e == 0) AssignAndReturnSolution(*t_q, *q, t_e, p);
|
|
|
|
dist_q = Abs(dist);
|
|
/*
|
|
AssignBetterSolution(dist_s, dist_q, t_s, *t_q);
|
|
AssignBetterSolution(dist_e, dist_q, t_e, *t_q);
|
|
*/
|
|
|
|
#ifdef TRACE
|
|
if (debug_itint) {
|
|
printf("\nIterativeIntersection()\n");
|
|
printf("t_s = %30.16f, dist_s = %30.16f\n",
|
|
t_s, dist_s);
|
|
printf("t_e = %30.16f, dist_e = %30.16f\n",
|
|
t_e, dist_e);
|
|
printf("t = %30.16f, dist = %30.16f\n",
|
|
t, dist);
|
|
printf("t_q = %30.16f, dist_q = %30.16f\n",
|
|
*t_q, dist_q);
|
|
}
|
|
#endif
|
|
|
|
if (sign_s == sign_e) {
|
|
if (sign_s == sign) {
|
|
/* */
|
|
/* both end points of the seg/arc and the approximate intersection */
|
|
/* lie on the same side of the line/circle; no chance to find a */
|
|
/* solution by means of bisection unless we find better bounds. we */
|
|
/* sample the parameter interval and hope for good luck... */
|
|
/* */
|
|
t_z = (t_e - t_s) / COUNTER_MAGIC;
|
|
t_z = VroniMax(t_z, ZERO);
|
|
t = t_s + t_z;
|
|
keep_going = (t < t_e);
|
|
while (keep_going) {
|
|
EvalObjParam(type2, p2, v2, r2, t, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist);
|
|
sign = Sign(dist);
|
|
AssignBetterSolution(dist, dist_q, t, *t_q);
|
|
if (sign != sign_s) {
|
|
keep_going = false;
|
|
}
|
|
else {
|
|
t += t_z;
|
|
keep_going = t < t_e;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (sign_s == sign) {
|
|
/* */
|
|
/* looks like we won't find a solution. we return the best */
|
|
/* "solution" found so far. */
|
|
/* */
|
|
EvalObjParam(type2, p2, v2, r2, *t_q, *q);
|
|
if (ne(dist_q, ZERO)) {
|
|
VD_Warning("IterativeIntersection() - cannot get intersection!");
|
|
troubles = true;
|
|
numerical = true;
|
|
return false;
|
|
}
|
|
else {
|
|
return true;
|
|
}
|
|
}
|
|
else {
|
|
/* */
|
|
/* ok, now we have bounds that (should) enclose a solution. */
|
|
/* */
|
|
if (sign == 0) {
|
|
AssignAndReturnSolution(*t_q, *q, t, p)
|
|
}
|
|
else if (t < *t_q) {
|
|
t_s = t;
|
|
sign_s = sign;
|
|
t_e = *t_q;
|
|
}
|
|
else {
|
|
t_s = *t_q;
|
|
sign_e = sign;
|
|
t_e = t;
|
|
}
|
|
/* */
|
|
/* let's compute a new start value for the iteration. */
|
|
/* */
|
|
t = (t_s + t_e) / 2.0;
|
|
EvalObjParam(type2, p2, v2, r2, t, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist);
|
|
sign = Sign(dist);
|
|
}
|
|
}
|
|
|
|
#ifdef TRACE
|
|
if (debug_itint) {
|
|
printf("\n\nt_s = %30.16f, dist_s = %30.16f\n", t_s, dist_s);
|
|
printf("t_e = %30.16f, dist_e = %30.16f\n", t_e, dist_e);
|
|
printf("t = %30.16f, dist = %30.16f\n", t, dist);
|
|
}
|
|
#endif
|
|
|
|
/* */
|
|
/* let's see whether we can find a better enclosing interval of the */
|
|
/* intersection */
|
|
/* */
|
|
t_z = t - ZERO;
|
|
t_z = VroniMax(t_z, t_s);
|
|
EvalObjParam(type2, p2, v2, r2, t_z, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist_z);
|
|
sign_z = Sign(dist_z);
|
|
if (sign_z == 0) AssignAndReturnSolution(*t_q, *q, t_z, p);
|
|
AssignBetterSolution(dist_z, dist_q, t_z, *t_q);
|
|
if (sign_z != sign) {
|
|
t_s = t_z;
|
|
t_e = t;
|
|
sign_s = sign_z;
|
|
sign_e = sign;
|
|
t = (t_s + t_e) / 2.0;
|
|
EvalObjParam(type2, p2, v2, r2, t, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist);
|
|
sign = Sign(dist);
|
|
}
|
|
else {
|
|
t_z = t + ZERO;
|
|
t_z = VroniMin(t_z, t_e);
|
|
EvalObjParam(type2, p2, v2, r2, t_z, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist_z);
|
|
sign_z = Sign(dist_z);
|
|
if (sign_z == 0) AssignAndReturnSolution(*t_q, *q, t_z, p);
|
|
AssignBetterSolution(dist_z, dist_q, t_z, *t_q);
|
|
if (sign_z != sign) {
|
|
t_e = t_z;
|
|
t_s = t;
|
|
sign_e = sign_z;
|
|
sign_s = sign;
|
|
t = (t_s + t_e) / 2.0;
|
|
EvalObjParam(type2, p2, v2, r2, t, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist);
|
|
sign = Sign(dist);
|
|
}
|
|
}
|
|
|
|
/* */
|
|
/* let's loop until an intersection is found -- or the existence of an */
|
|
/* intersection appears to be unlikely. */
|
|
/* */
|
|
counter = 0;
|
|
#ifdef TRACE
|
|
if (debug_itint) {
|
|
printf("t_s = %30.16f, dist_s = %30.16f\n", t_s, dist_s);
|
|
printf("t_e = %30.16f, dist_e = %30.16f\n", t_e, dist_e);
|
|
printf("t_q = %30.16f, dist_q = %30.16f\n", *t_q, dist_q);
|
|
}
|
|
#endif
|
|
|
|
while (counter < COUNTER_MAGIC) {
|
|
if (sign == sign_s) t_s = t;
|
|
else t_e = t;
|
|
t = (t_e + t_s) / 2.0;
|
|
EvalObjParam(type2, p2, v2, r2, t, p);
|
|
EvalObjDist(type1, a, b, c, c1, r1, p, dist);
|
|
sign = Sign(dist);
|
|
if (sign == 0) AssignAndReturnSolution(*t_q, *q, t, p);
|
|
AssignBetterSolution(dist, dist_q, t, *t_q);
|
|
++counter;
|
|
#ifdef TRACE
|
|
if (debug_itint) {
|
|
if ((t <= t_s) || (t >= t_e)) {
|
|
printf("t_q = %30.16f, dist_q = %30.16f\n", *t_q, dist_q);
|
|
printf("used %d iterations to compute a solution:%30.16f\n",
|
|
counter, dist_q);
|
|
}
|
|
}
|
|
#endif
|
|
if ((t <= t_s) || (t >= t_e)) counter = COUNTER_MAGIC;
|
|
}
|
|
#ifdef TRACE
|
|
if (debug_itint) {
|
|
if (!((t <= t_s) || (t >= t_e))) {
|
|
printf("t_q = %30.16f, dist_q = %30.16f\n", *t_q, dist_q);
|
|
printf("used %d iterations to compute a solution:%30.16f\n",
|
|
counter, dist_q);
|
|
}
|
|
}
|
|
#endif
|
|
if (ne(dist_q, ZERO)) {
|
|
VD_Warning("IterativeIntersection() - cannot compute intersection!");
|
|
numerical = true;
|
|
*q = p;
|
|
return false;
|
|
}
|
|
else {
|
|
AssignAndReturnSolution(*t_q, *q, t, p)
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* */
|
|
/* this function checks whether the point q is within a strip/wedge */
|
|
/* defined by lines through the points q1 and q2 which are perpendicular */
|
|
/* to the site i. it does not check, though, whether q lies on that site! */
|
|
/* it also does not check whether q1, q2 are within the cone of influcence */
|
|
/* of site i. note that q1, q2 are not allowed to coincide with c as */
|
|
/* defined in the function. */
|
|
/* it returns 0.0 if q lies in that region, a non-zero value otherwise. */
|
|
/* this value is negative if q lies on the side of q1, and positive if */
|
|
/* it lies on the side of q2. if the line through q1,q2 is perpendicular */
|
|
/* onto site i then we check whether q is one the line segment between */
|
|
/* q1 and q2. */
|
|
/* */
|
|
double vroniObject::PntSiteConeClassificator(int i, t_site ti, coord q1,
|
|
coord q2, const coord & q,
|
|
double eps)
|
|
{
|
|
coord c, v, p1, p2;
|
|
double d, d1, d2;
|
|
vr_bool more_than_pi = false, swapped = false;
|
|
|
|
if ((ti == PNT) || (ti == ARC)) {
|
|
/* */
|
|
/* c is set to the pnt coords or to the center of the arc of site i */
|
|
/* */
|
|
c = (ti == PNT) ? GetPntCoords(i) : GetArcCenter(i);
|
|
d = VecDet(c, q2, q1);
|
|
#ifdef TRACE
|
|
if ((i == 3) && (ti == 2)) printf("q1 = (%20.16f %20.16f)\n", q1.x, q1.y);
|
|
if ((i == 3) && (ti == 2)) printf("q2 = (%20.16f %20.16f)\n", q2.x, q2.y);
|
|
if ((i == 3) && (ti == 2)) printf("q = (%20.16f %20.16f)\n", q.x, q.y);
|
|
if ((i == 3) && (ti == 2)) printf("d = %20.16f\n", d);
|
|
#endif
|
|
|
|
if (eq(d, eps) && (ti == PNT)) {
|
|
p1 = VecSub(q1, c);
|
|
p2 = VecSub(q2, c);
|
|
if (VecDotProd(p1, p2) < 0.0) d = -1.0;
|
|
}
|
|
|
|
if (eq(d, eps)) {
|
|
/* */
|
|
/* the line through q1, q2 is perpendicular onto the site or some */
|
|
/* points coincide. */
|
|
/* */
|
|
d = PntPntDist(q1, q2);
|
|
if (eq(d, eps)) {
|
|
d1 = PntPntDist(q1, q);
|
|
if (eq(d1, eps)) return 0.0;
|
|
d2 = PntPntDist(q2, q);
|
|
if (eq(d2, eps)) return 0.0;
|
|
if (d1 < d2) return -d1;
|
|
else return d2;
|
|
}
|
|
}
|
|
else {
|
|
if (d < 0.0) {
|
|
if (ti == PNT) {
|
|
more_than_pi = true;
|
|
}
|
|
else {
|
|
VroniSwap(q1, q2, p1);
|
|
swapped = true;
|
|
}
|
|
}
|
|
if (more_than_pi) {
|
|
assert(PntPntDist(c, q1) > 0.0);
|
|
d1 = VecDet(c, q, q1) / PntPntDist(c, q1);
|
|
assert(PntPntDist(c, q2) > 0.0);
|
|
d2 = VecDet(c, q, q2) / PntPntDist(c, q2);
|
|
if ((d1 < -eps) && (d2 > eps)) return (d2 - d1);
|
|
else return 0.0;
|
|
}
|
|
else {
|
|
assert(PntPntDist(c, q1) > 0.0);
|
|
d1 = VecDet(c, q, q1) / PntPntDist(c, q1);
|
|
if (d1 < -eps) {
|
|
if (swapped) return -d1;
|
|
else return d1;
|
|
}
|
|
assert(PntPntDist(c, q2) > 0.0);
|
|
d2 = VecDet(c, q, q2) / PntPntDist(c, q2);
|
|
if (d2 > eps) {
|
|
if (swapped) return -d2;
|
|
else return d2;
|
|
}
|
|
else return 0.0;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
assert(ti == SEG);
|
|
/* */
|
|
/* we compute the projection of q onto the seg i and compare it to */
|
|
/* the projections of q1 and q2 onto the seg i. */
|
|
/* */
|
|
p1 = GetSegStartCoord(i);
|
|
p2 = GetSegEndCoord(i);
|
|
c = VecSub(p2, p1);
|
|
v = VecSub(q1, p1);
|
|
d1 = VecDotProd(c, v);
|
|
v = VecSub(q2, p1);
|
|
d2 = VecDotProd(c, v);
|
|
if (d2 < d1) {
|
|
VroniSwap(d1, d2, d);
|
|
swapped = true;
|
|
}
|
|
d = d2 - d1;
|
|
if (eq(d, eps)) {
|
|
/* */
|
|
/* the line through q1, q2 is perpendicular onto the site or the */
|
|
/* points q1, q2 coincide. */
|
|
/* */
|
|
d = PntPntDist(q1, q2);
|
|
if (eq(d, eps)) {
|
|
/* */
|
|
/* the points q1, q2 coincide or, at least, are very close */
|
|
/* */
|
|
v = VecSub(q, p1);
|
|
d = VecDotProd(c, v);
|
|
if (eq(d, eps)) return 0.0;
|
|
else if (swapped) return -d;
|
|
else return d;
|
|
}
|
|
}
|
|
else {
|
|
v = VecSub(q, p1);
|
|
d = VecDotProd(c, v);
|
|
/* */
|
|
/* we'd need to have d1 <= d <= d2 */
|
|
/* */
|
|
if (d < (d1 - eps)) {
|
|
if (swapped) return -d;
|
|
else return d;
|
|
}
|
|
else if (d > (d2 + eps)) {
|
|
if (swapped) return -d;
|
|
else return d;
|
|
}
|
|
else
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
/* */
|
|
/* the line through q1, q2 is perpendicular onto the site i. we have */
|
|
/* d as the (non-zero) distance between q1 and q2. */
|
|
/* */
|
|
assert(gt(d, eps));
|
|
d1 = VecDet(q1, q2, q);
|
|
c = VecSub(q2, q1);
|
|
v = VecSub(q, q1);
|
|
d2 = VecDotProd(c, v) / d;
|
|
/*
|
|
printf("\nd = %20.16f\n", d);
|
|
printf("d2 = %20.16f\n", d2);
|
|
printf("d1 = %20.16f\n", d1);
|
|
printf("dd = %20.16f\n", d1/d);
|
|
*/
|
|
|
|
/* */
|
|
/* we should have -eps < d2 < d + eps and eq(d1, eps). */
|
|
/* */
|
|
if (d2 < -eps) return d2;
|
|
else if (d2 > (d + eps)) return d2;
|
|
else if (eq(d1, ZERO_MAX)) return 0.0;
|
|
else if (d2 < 0.5) return -Abs(d1) / d;
|
|
else return Abs(d1) / d;
|
|
}
|