SaraP
590 lines
23 KiB
C++
590 lines
23 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 "defs.h"
|
|
#include "numerics.h"
|
|
#include "roots.h"
|
|
|
|
|
|
|
|
#define Determine3Sidedness(a, b, c, old_start, old_end, dist, eps) \
|
|
{\
|
|
dist = PntLineDist(a, b, c, old_start); \
|
|
if (eq(dist, eps)) { \
|
|
dist = PntLineDist(a, b, c, old_end); \
|
|
if (eq(dist, eps)) { \
|
|
restart = true; \
|
|
} \
|
|
} \
|
|
if (dist < 0.0) { \
|
|
a = -a; \
|
|
b = -b; \
|
|
c = -c; \
|
|
} \
|
|
}
|
|
|
|
|
|
/* */
|
|
/* computes the center cntr and the radius r2 of the circle tangential */
|
|
/* to the three segments segs[i], segs[j], segs[k]. it returns false if */
|
|
/* the radius is too large to be computed numerically, or if it is obvious */
|
|
/* that problems have occurred. */
|
|
/* note that we assume that the new center lies on the same sides of */
|
|
/* segs[i], segs[j], segs[k] as the node old_start on edge e, where e */
|
|
/* is defined by segs[i], segs[j]. */
|
|
/* */
|
|
vr_bool vroniObject::SegSegSegCntr(int i, int j, int k, int e, coord *cntr,
|
|
double *r2, vr_bool *problematic, int *site)
|
|
{
|
|
int m, n, n1, n2;
|
|
double dist, ai, bi, ci, aj = 0.0, bj = 0.0, cj = 0.0;
|
|
double ak = 0.0, bk = 0.0, ck = 0.0, c, t;
|
|
coord Q, V, U, W, X, old_end, old_start;
|
|
double A[2][2], B[2], xy[2], alpha, beta, gamma, delta;
|
|
double radius1, radius2, eps;
|
|
int exists = 0, I0, I1, J0, J1, K;
|
|
vr_bool success;
|
|
vr_bool ij = false, ik = false, jk = false;
|
|
coord centers[VRONI_MAXSOL];
|
|
double radii[VRONI_MAXSOL];
|
|
int num_sol = 0, best_sol = NIL;
|
|
coord u, v;
|
|
#ifdef TRACE
|
|
double rad_min, rad_max;
|
|
#endif
|
|
int i_in, j_in, k_in;
|
|
double lgth_i, lgth_j, lgth_k;
|
|
|
|
assert(InSegsList(i));
|
|
assert(InSegsList(j));
|
|
assert(InSegsList(k));
|
|
assert(!(eq(GetSegLgth(i), ZERO)));
|
|
assert(!(eq(GetSegLgth(j), ZERO)));
|
|
assert(!(eq(GetSegLgth(k), ZERO)));
|
|
|
|
eps = ZERO;
|
|
i_in = i;
|
|
j_in = j;
|
|
k_in = k;
|
|
|
|
*site = NIL;
|
|
*problematic = false;
|
|
|
|
#ifdef TRACE
|
|
if ((i_in == 100) && (j_in == 103) && (k_in == 104)) {
|
|
printf("SegSegSegCntr - seg1=%d seg2=%d seg3=%d\n", i, j, k);
|
|
Q = GetSegStartCoord(i);
|
|
printf("start (%d) - %19.16f %19.16f\n", i, Q.x, Q.y);
|
|
Q = GetSegEndCoord(i);
|
|
printf("end (%d) - %19.16f %19.16f\n", i, Q.x, Q.y);
|
|
Q = GetSegStartCoord(j);
|
|
printf("start (%d) - %19.16f %19.16f\n", j, Q.x, Q.y);
|
|
Q = GetSegEndCoord(j);
|
|
printf("end (%d) - %19.16f %19.16f\n", j, Q.x, Q.y);
|
|
Q = GetSegStartCoord(k);
|
|
printf("start (%d) - %19.16f %19.16f\n", k, Q.x, Q.y);
|
|
Q = GetSegEndCoord(k);
|
|
printf("end (%d) - %19.16f %19.16f\n", k, Q.x, Q.y);
|
|
}
|
|
#endif
|
|
|
|
/* */
|
|
/* we sort the indices of line segments in order to guarantee that the */
|
|
/* center of these three sites is always computed consistently. */
|
|
/* */
|
|
K = k;
|
|
SortThreeNumbers(i, j, k, m);
|
|
|
|
/* */
|
|
/* check whether two segments are identical. */
|
|
/* */
|
|
if ((i == j) || (j == k)) {
|
|
VD_Dbg_Warning("SegSegSegCntr() - two identical segments!");
|
|
return false;
|
|
}
|
|
|
|
/* */
|
|
/* check whether the three segments share a common end point, or whether */
|
|
/* two of them are identical */
|
|
/* */
|
|
m = Get1stVtx(k, SEG);
|
|
if (IsSegStartPnt(i, m) || IsSegEndPnt(i, m)) {
|
|
if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
n = Get2ndVtx(k, SEG);
|
|
if (IsSegStartPnt(i, n) || IsSegEndPnt(i, n)) {
|
|
SetInvalidSites(i, SEG, k, SEG, ZERO);
|
|
restart = false;
|
|
return false; /* duplicate segs ! */
|
|
}
|
|
else if (IsSegStartPnt(j, n) || IsSegEndPnt(j, n)) {
|
|
SetInvalidSites(j, SEG, k, SEG, ZERO);
|
|
restart = false;
|
|
return false; /* duplicate segs ! */
|
|
}
|
|
else {
|
|
*cntr = GetPntCoords(m);
|
|
*r2 = 0.0;
|
|
*site = m;
|
|
return true; /* common end point */
|
|
}
|
|
}
|
|
ik = true;
|
|
}
|
|
else if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
jk = true;
|
|
}
|
|
m = Get2ndVtx(k, SEG);
|
|
if (IsSegStartPnt(i, m) || IsSegEndPnt(i, m)) {
|
|
if (ik) {
|
|
SetInvalidSites(i, SEG, k, SEG, ZERO);
|
|
restart = false;
|
|
return false; /* duplicate segs ! */
|
|
}
|
|
if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
if (jk) {
|
|
SetInvalidSites(j, SEG, k, SEG, ZERO);
|
|
restart = false;
|
|
return false; /* duplicate segs ! */
|
|
}
|
|
else {
|
|
*cntr = GetPntCoords(m);
|
|
*r2 = 0.0;
|
|
*site = m;
|
|
return true; /* common end point */
|
|
}
|
|
}
|
|
ik = true;
|
|
}
|
|
else if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
if (jk) {
|
|
SetInvalidSites(j, SEG, k, SEG, ZERO);
|
|
restart = false;
|
|
return false; /* duplicate segs ! */
|
|
}
|
|
jk = true;
|
|
}
|
|
m = Get1stVtx(i, SEG);
|
|
if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
m = Get2ndVtx(i, SEG);
|
|
if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
SetInvalidSites(i, SEG, j, SEG, ZERO);
|
|
restart = false;
|
|
return false; /* duplicate segs ! */
|
|
}
|
|
ij = true;
|
|
}
|
|
else {
|
|
m = Get2ndVtx(i, SEG);
|
|
if (IsSegStartPnt(j, m) || IsSegEndPnt(j, m)) {
|
|
ij = true;
|
|
}
|
|
}
|
|
|
|
/* */
|
|
/* get the precomputed line data. */
|
|
/* */
|
|
n1 = GetStartNode(e);
|
|
if (IsNodeDeleted(n1)) {
|
|
n2 = n1;
|
|
n1 = GetEndNode(e);
|
|
}
|
|
else {
|
|
n2 = GetEndNode(e);
|
|
}
|
|
GetNodeData(n1, &old_start, &radius1);
|
|
GetNodeData(n2, &old_end, &radius2);
|
|
#ifdef TRACE
|
|
if (radius1 < radius2) {
|
|
rad_min = radius1;
|
|
rad_max = radius2;
|
|
}
|
|
else {
|
|
rad_min = radius2;
|
|
rad_max = radius1;
|
|
}
|
|
if (center)
|
|
printf("SegSegSeg: min radius = %19.16f, max radius = %19.16f\n",
|
|
rad_min, rad_max);
|
|
#endif
|
|
|
|
lgth_i = GetSegLgth(i);
|
|
lgth_j = GetSegLgth(j);
|
|
lgth_k = GetSegLgth(k);
|
|
|
|
while (eps <= ZERO_MAX) {
|
|
|
|
num_sol = 0;
|
|
restart = false;
|
|
|
|
/* */
|
|
/* determine the correct side of each line segment */
|
|
/* */
|
|
GetSegEqnData(i, &ai, &bi, &ci);
|
|
if (K != i) {
|
|
assert(IsLftRgtSite(e, i, SEG));
|
|
Determine3Sidedness(ai, bi, ci, old_end, old_start, dist, eps);
|
|
}
|
|
else {
|
|
Determine3Sidedness(ai, bi, ci, old_start, old_end, dist, eps);
|
|
}
|
|
if (restart) {
|
|
VD_Dbg_Warning("SegSegSegCntr() - degenerate bisector!");
|
|
}
|
|
if (!restart) {
|
|
GetSegEqnData(j, &aj, &bj, &cj);
|
|
if (K != j) {
|
|
assert(IsLftRgtSite(e, j, SEG));
|
|
Determine3Sidedness(aj, bj, cj, old_end, old_start, dist, eps);
|
|
}
|
|
else {
|
|
Determine3Sidedness(aj, bj, cj, old_start, old_end, dist, eps);
|
|
}
|
|
if (restart) {
|
|
VD_Dbg_Warning("SegSegSegCntr() - degenerate bisector!");
|
|
}
|
|
}
|
|
if (!restart) {
|
|
GetSegEqnData(k, &ak, &bk, &ck);
|
|
if (K != k) {
|
|
assert(IsLftRgtSite(e, k, SEG));
|
|
Determine3Sidedness(ak, bk, ck, old_end, old_start, dist, eps);
|
|
}
|
|
else {
|
|
Determine3Sidedness(ak, bk, ck, old_start, old_end, dist, eps);
|
|
}
|
|
if (restart) {
|
|
VD_Dbg_Warning("SegSegSegCntr() - degenerate bisector!");
|
|
}
|
|
}
|
|
|
|
if (!restart) {
|
|
/* */
|
|
/* first attempt to compute that Voronoi node. */
|
|
/* */
|
|
/* let's see whether we can compute the center by intersecting the */
|
|
/* offset segments. */
|
|
/* */
|
|
A[0][0] = ai - aj;
|
|
A[1][0] = ai - ak;
|
|
A[0][1] = bi - bj;
|
|
A[1][1] = bi - bk;
|
|
B[0] = cj - ci;
|
|
B[1] = ck - ci;
|
|
LinearEqnSolver_2x2_Zero(A, B, xy, exists, I0, J0, I1, J1);
|
|
|
|
if (exists == 1) {
|
|
/* */
|
|
/* we've found a center. */
|
|
/* */
|
|
centers[num_sol].x = xy[0];
|
|
centers[num_sol].y = xy[1];
|
|
if (K == i) dist = PntLineDist(ai, bi, ci, centers[num_sol]);
|
|
else if (K == j) dist = PntLineDist(aj, bj, cj, centers[num_sol]);
|
|
else if (K == k) dist = PntLineDist(ak, bk, ck, centers[num_sol]);
|
|
radii[num_sol] = Abs(dist);
|
|
num_sol += 1;
|
|
}
|
|
|
|
/* */
|
|
/* second attempt to compute that Voronoi node. */
|
|
/* */
|
|
/* we compute the bisector between two segments, and intersect it */
|
|
/* with the offset segment of the third segment. in an attempt to */
|
|
/* minimize numerical instabilities we pay attention to the angles */
|
|
/* between the segments: we pick the pair of segments that enclose */
|
|
/* an angle which is closest to a right angle. however, if two */
|
|
/* segments share endpoints then we prefer to use those segments */
|
|
/* irrelevant of the angle enclosed. */
|
|
/* */
|
|
if (ij && ik && jk) {
|
|
alpha = ai * ak + bi * bk;
|
|
beta = ai * aj + bi * bj;
|
|
gamma = aj * ak + bj * bk;
|
|
delta = Max3(alpha, beta, gamma);
|
|
}
|
|
else if (ij) {
|
|
delta = beta = 0.0;
|
|
alpha = ai * ak + bi * bk;
|
|
alpha = Abs(alpha);
|
|
gamma = aj * ak + bj * bk;
|
|
gamma = Abs(gamma);
|
|
}
|
|
else if (ik) {
|
|
delta = alpha = 0.0;
|
|
beta = ai * aj + bi * bj;
|
|
beta = Abs(beta);
|
|
gamma = aj * ak + bj * bk;
|
|
gamma = Abs(gamma);
|
|
}
|
|
else if (jk) {
|
|
delta = gamma = 0.0;
|
|
alpha = ai * ak + bi * bk;
|
|
alpha = Abs(alpha);
|
|
beta = ai * aj + bi * bj;
|
|
beta = Abs(beta);
|
|
}
|
|
else {
|
|
alpha = ai * ak + bi * bk;
|
|
alpha = Abs(alpha);
|
|
beta = ai * aj + bi * bj;
|
|
beta = Abs(beta);
|
|
gamma = aj * ak + bj * bk;
|
|
gamma = Abs(gamma);
|
|
delta = Min3(alpha, beta, gamma);
|
|
}
|
|
|
|
if (delta == alpha)
|
|
success = SegSegBisector(i, k, ai, bi, ak, bk, ck, &Q, &V);
|
|
else if (delta == beta)
|
|
success = SegSegBisector(i, j, ai, bi, aj, bj, cj, &Q, &V);
|
|
else /* (delta == gamma) */
|
|
success = SegSegBisector(j, k, aj, bj, ak, bk, ck, &Q, &V);
|
|
|
|
if (!success) {
|
|
/* */
|
|
/* we could not compute that bisector: we use the old bisector! */
|
|
/* */
|
|
Q = old_start;
|
|
V = VecSub(old_end, old_start);
|
|
if (K == i) {
|
|
delta = gamma = 2.0;
|
|
}
|
|
else if (K == j) {
|
|
delta = alpha = 2.0;
|
|
}
|
|
else {
|
|
delta = beta = 2.0;
|
|
}
|
|
}
|
|
|
|
/* */
|
|
/* we intersect the bisector of the two segments with the offset of */
|
|
/* the third segment */
|
|
/* */
|
|
if (delta == alpha) {
|
|
if (beta < gamma) {
|
|
W.x = aj - ak;
|
|
W.y = bj - bk;
|
|
c = cj - ck;
|
|
}
|
|
else {
|
|
W.x = aj - ai;
|
|
W.y = bj - bi;
|
|
c = cj - ci;
|
|
}
|
|
}
|
|
else if (delta == beta) {
|
|
if (alpha < gamma) {
|
|
W.x = ak - ai;
|
|
W.y = bk - bi;
|
|
c = ck - ci;
|
|
}
|
|
else {
|
|
W.x = ak - aj;
|
|
W.y = bk - bj;
|
|
c = ck - cj;
|
|
}
|
|
}
|
|
else {
|
|
if (alpha < beta) {
|
|
W.x = ai - ak;
|
|
W.y = bi - bk;
|
|
c = ci - ck;
|
|
}
|
|
else {
|
|
W.x = ai - aj;
|
|
W.y = bi - bj;
|
|
c = ci - cj;
|
|
}
|
|
}
|
|
delta = - VecDotProd(W, V);
|
|
|
|
if (!eq(delta, TINY)) {
|
|
t = (c + VecDotProd(W,Q)) / delta;
|
|
centers[num_sol] = RayPnt(Q, V, t);
|
|
if (K == i) dist = PntLineDist(ai, bi, ci, centers[num_sol]);
|
|
else if (K == j) dist = PntLineDist(aj, bj, cj, centers[num_sol]);
|
|
else if (K == k) dist = PntLineDist(ak, bk, ck, centers[num_sol]);
|
|
radii[num_sol] = Abs(dist);
|
|
num_sol += 1;
|
|
}
|
|
|
|
/* */
|
|
/* third attempt to compute that Voronoi node. */
|
|
/* */
|
|
/* we will compute the bisectors of two segments and will intersect */
|
|
/* them. we first find the bisector between the segments which */
|
|
/* enclose the smallest angle. (thus, their normal vectors enclose */
|
|
/* the largest angle!) */
|
|
/* */
|
|
if ((ij && ik && jk) || (!(ij || ik || jk))) {
|
|
alpha = ai * ak + bi * bk;
|
|
beta = ai * aj + bi * bj;
|
|
gamma = aj * ak + bj * bk;
|
|
delta = Min3(alpha, beta, gamma);
|
|
}
|
|
else if (ij) {
|
|
delta = beta = -1.0;
|
|
if (ik) alpha = 1.0;
|
|
else alpha = ai * ak + bi * bk;
|
|
if (jk) gamma = 1.0;
|
|
else gamma = aj * ak + bj * bk;
|
|
}
|
|
else if (ik) {
|
|
delta = alpha = -1.0;
|
|
beta = ai * aj + bi * bj;
|
|
if (jk) gamma = 1.0;
|
|
else gamma = aj * ak + bj * bk;
|
|
}
|
|
else {
|
|
alpha = ai * ak + bi * bk;
|
|
beta = ai * aj + bi * bj;
|
|
delta = gamma = -1.0;
|
|
}
|
|
|
|
if (delta == alpha)
|
|
success = SegSegBisector(i, k, ai, bi, ak, bk, ck, &Q, &V);
|
|
else if (delta == beta)
|
|
success = SegSegBisector(i, j, ai, bi, aj, bj, cj, &Q, &V);
|
|
else
|
|
success = SegSegBisector(j, k, aj, bj, ak, bk, ck, &Q, &V);
|
|
|
|
if (success) {
|
|
/* */
|
|
/* we then find the bisector between the segments which enclose */
|
|
/* the largest angle. (thus, their normal vectors enclose the */
|
|
/* smallest angle!) */
|
|
/* */
|
|
delta = Max3(alpha, beta, gamma);
|
|
if (delta == alpha)
|
|
success = SegSegBisector(i, k, ai, bi, ak, bk, ck, &U, &W);
|
|
else if (delta == beta)
|
|
success = SegSegBisector(i, j, ai, bi, aj, bj, cj, &U, &W);
|
|
else
|
|
success = SegSegBisector(j, k, aj, bj, ak, bk, ck, &U, &W);
|
|
|
|
if (success) {
|
|
/* */
|
|
/* let's solve the 2x2 linear system and hope for the best. */
|
|
/* */
|
|
LineLineIntersection(A, B, xy, I0, J0, I1, J1, Q, V, U, W, X,
|
|
exists);
|
|
if (exists == 1) {
|
|
centers[num_sol] = X;
|
|
if (K == i) dist = PntLineDist(ai, bi, ci, X);
|
|
else if (K == j) dist = PntLineDist(aj, bj, cj, X);
|
|
else if (K == k) dist = PntLineDist(ak, bk, ck, X);
|
|
radii[num_sol] = Abs(dist);
|
|
num_sol += 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
restart = false;
|
|
|
|
/* */
|
|
/* we have up to three possible centers. we select the center that */
|
|
/* (ideally) lies within the cones of influence of the segments and on */
|
|
/* the Voronoi edge e. */
|
|
/* */
|
|
best_sol = ClassifySolutions(i_in, j_in, k_in, e, SEG, SEG, SEG,
|
|
true, true, false, centers, radii,
|
|
&num_sol, eps, problematic);
|
|
assert((0 <= best_sol) && (best_sol < num_sol));
|
|
if (!*problematic) {
|
|
*cntr = centers[best_sol];
|
|
*r2 = radii[best_sol];
|
|
return true;
|
|
}
|
|
else {
|
|
eps *= 10.0;
|
|
}
|
|
}
|
|
|
|
if (eq(lgth_i, ZERO_MAX)) {
|
|
/* */
|
|
/* this is a seg with a very small length; we replace it */
|
|
/* */
|
|
VD_Dbg_Warning("SegSegSegCntr() - seg with small length!");
|
|
ReplaceSeg(i);
|
|
restart = true;
|
|
return false;
|
|
}
|
|
else if (eq(lgth_j, ZERO_MAX)) {
|
|
/* */
|
|
/* this is a seg with a very small length; we replace it */
|
|
/* */
|
|
VD_Dbg_Warning("SegSegSegCntr() - seg with small length!");
|
|
ReplaceSeg(j);
|
|
restart = true;
|
|
return false;
|
|
}
|
|
else if (eq(lgth_k, ZERO_MAX)) {
|
|
/* */
|
|
/* this is a seg with a very small length; we replace it */
|
|
/* */
|
|
VD_Dbg_Warning("SegSegSegCntr() - seg with small length!");
|
|
ReplaceSeg(k);
|
|
restart = true;
|
|
return false;
|
|
}
|
|
else if (CheckIntersectionsLocally(i, SEG, j, SEG, k, SEG)) {
|
|
restart = true;
|
|
return false;
|
|
}
|
|
|
|
#ifdef VRONI_DBG_WARN
|
|
printf("\nCenter not computed for seg-seg-seg:\n");
|
|
Q = GetSegStartCoord(i);
|
|
V = GetSegEndCoord(i);
|
|
printf("%20.16f %20.16f %20.16f %20.16f\n", Q.x, Q.y, V.x, V.y);
|
|
Q = GetSegStartCoord(j);
|
|
V = GetSegEndCoord(j);
|
|
printf("%20.16f %20.16f %20.16f %20.16f\n", Q.x, Q.y, V.x, V.y);
|
|
Q = GetSegStartCoord(k);
|
|
V = GetSegEndCoord(k);
|
|
printf("%20.16f %20.16f %20.16f %20.16f\n", Q.x, Q.y, V.x, V.y);
|
|
#endif
|
|
|
|
*cntr = centers[best_sol];
|
|
*r2 = radii[best_sol];
|
|
restart = false;
|
|
|
|
return false;
|
|
}
|