SaraP
1054 lines
34 KiB
C++
1054 lines
34 KiB
C++
/*****************************************************************************/
|
|
/* */
|
|
/* Copyright (C) 2007-2025 S. Huber, 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: Stefan Huber, 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"
|
|
|
|
|
|
const double ZERO_MAX_ARC = ZERO_MAX * 100.0;
|
|
|
|
|
|
int vroniObject::ArcRoots2abc(double a, double b, double c, double* roots)
|
|
{
|
|
int num;
|
|
|
|
Roots2abc(a, b, c, roots, num);
|
|
|
|
return num;
|
|
}
|
|
|
|
|
|
//These expressions are used to determine the offset-parameter t when
|
|
//x and y are given as x=(kx1+kx2*t)/nn and y=(ky1+ky2*t)/nn. These three macros are the same
|
|
//for all Circle-XXX-XXX functions.
|
|
//kt1 + kt2*t + kt3*t^2 = 0
|
|
#define ARC_KT1(nn, kx1, kx2, ky1, ky2, a, b, r) ( kx1*kx1 + ky1*ky1 - 2.0*nn*(a*kx1+b*ky1) + nn*nn*(a*a+b*b-r*r) )
|
|
#define ARC_KT2(nn, kx1, kx2, ky1, ky2, a, b, r, k) ( -2.0*( -kx1*kx2 - ky1*ky2 + nn*( a*kx2 + b*ky2 + k*nn*r ) ) )
|
|
#define ARC_KT3(nn, kx2, ky2) ( -nn*nn + kx2*kx2 + ky2*ky2 )
|
|
|
|
|
|
|
|
|
|
|
|
//These expressions are used to determine equations for x and y of the new center:
|
|
//x = (kx1 + kx2 * t) / nn
|
|
//y = (ky1 + ky2 * t) / nn
|
|
#define ARC_NN(a,b,c,d,e,f) ( 2.0*( -d*e + b*(e-c) + a*(d-f) + c*f ) )
|
|
#define ARC_KX1(a,b,c,d,e,f,r,s,q) ( d*(q*q-e*e-f*f) + (d-f)*((a)*(a)+(b)*(b)-r*r) + f*(c*c+d*d-s*s)\
|
|
+ (b)*(e*e+f*f-q*q + s*s-c*c-d*d) )
|
|
#define ARC_KX2(a,b,c,d,e,f,r,s,q,k,l,m) ( 2.0*( m*q*((d)-(b)) + k*r*((f)-(d)) + l*s*((b)-(f)) ) )
|
|
#define ARC_KY1(a,b,c,d,e,f,r,s,q) ARC_KX1( b, a, f, e, d, c, r, q, s )
|
|
#define ARC_KY2(a,b,c,d,e,f,r,s,q,k,l,m) ARC_KX2(xx,-a,xx,-c,xx,-e,r,s,q,k,l,m)
|
|
|
|
|
|
int vroniObject::CircCircCircCenters( const coord & c1, const coord & c2,
|
|
const coord & c3,
|
|
double_arg r1, double_arg r2,
|
|
double_arg r3,
|
|
coord* centers, double* radii,
|
|
double eps )
|
|
{
|
|
int dc, i;
|
|
//Number of Delaunay circles found
|
|
int num=0;
|
|
double kx2, ky2;
|
|
double kt1, kt2, kt3;
|
|
|
|
const double nn = ARC_NN(c1.x, c1.y, c2.x, c2.y, c3.x, c3.y);
|
|
const double kx1 = ARC_KX1(c1.x, c1.y, c2.x, c2.y, c3.x, c3.y, r1, r2, r3);
|
|
const double ky1 = ARC_KY1(c1.x, c1.y, c2.x, c2.y, c3.x, c3.y, r1, r2, r3);
|
|
|
|
double roots[2];
|
|
double dl2, dl3, dr2, dr3;
|
|
int numroots;
|
|
|
|
#ifdef TRACE
|
|
printf("CircCircCirc (%20.16f, %20.16f, %20.16f) (%20.16f, %20.16f, %20.16f) (%20.16f, %20.16f, %20.16f)\n", c1.x, c1.y, r1, c2.x, c2.y, r2, c3.x, c3.y, r3);
|
|
#endif
|
|
|
|
//dc is a number 0...7. Its binary representation is used
|
|
//to create triples from {-1,1}^3. These three numbers represent
|
|
//the offset directions k, l, m of the three circles.
|
|
for (dc=0; dc<=7; dc++) {
|
|
const double k = (dc & 4) ? 1 : -1;
|
|
const double l = (dc & 2) ? 1 : -1;
|
|
const double m = (dc & 1) ? 1 : -1;
|
|
|
|
|
|
//If radius is 0 an offsetting in the interior of circle is not possible
|
|
//This is only for performance boosting, since t has to be > -ZERO anyway
|
|
if( (k==-1 && eq(r1,ZERO)) ||
|
|
(l==-1 && eq(r2,ZERO)) ||
|
|
(m==-1 && eq(r3,ZERO)))
|
|
continue;
|
|
|
|
kx2 = ARC_KX2( c1.x, c1.y, c2.x, c2.y, c3.x, c3.y, r1, r2, r3, k, l, m );
|
|
ky2 = ARC_KY2( c1.x, c1.y, c2.x, c2.y, c3.x, c3.y, r1, r2, r3, k, l, m );
|
|
|
|
//Note: if kt1 wouldn't be modified by Roots2abc, then this could be extracted from loop
|
|
kt1 = ARC_KT1(nn, kx1, kx2, ky1, ky2, c1.x, c1.y, r1);
|
|
kt2 = ARC_KT2(nn, kx1, kx2, ky1, ky2, c1.x, c1.y, r1, k);
|
|
kt3 = ARC_KT3(nn, kx2, ky2);
|
|
|
|
|
|
//printf("kx1: %e, kx2: %e, ky1: %e, ky2: %e, kt1: %e, kt2: %e, kt3: %e\n",
|
|
// kx1, kx2, ky1, ky2, kt1, kt2, kt3);
|
|
|
|
//Solve the 2nd degree polynomial equation
|
|
numroots = ArcRoots2abc(kt3, kt2, kt1, roots);
|
|
|
|
//Save all roots which have been found
|
|
for (i=0; i<numroots; i++) {
|
|
//printf("%1f:%1f:%1f: root: %e\n", k,l,m, roots[i]);
|
|
|
|
//Only add, if
|
|
// radius of found Delaunay circle is positive
|
|
// radii of offset circles are positive
|
|
if (lt(roots[i], ZERO) || lt(r1+roots[i]*k, ZERO) ||
|
|
lt(r2+roots[i]*l, ZERO) || lt(r3+roots[i]*m, ZERO))
|
|
continue;
|
|
|
|
//Too big.
|
|
if (Abs(roots[i]) > too_large)
|
|
continue;
|
|
|
|
//If not almost zero, set it to zero
|
|
if (roots[i] < 0.0) roots[i] = 0.0;
|
|
if ((r1+roots[i]*k) < 0.0) roots[i] = -r1 / k;
|
|
if ((r2+roots[i]*l) < 0.0) roots[i] = -r2 / l;
|
|
if ((r3+roots[i]*m) < 0.0) roots[i] = -r3 / m;
|
|
|
|
//The three circle-centers are collinear
|
|
//We can not insert in parameterization of x and y
|
|
//We have to calculate the intersection of two known offset-circles
|
|
//"by hand"
|
|
if (!eq(nn, eps)) {
|
|
//Insert in parameterization of x and y
|
|
centers[num].x = (kx1 + kx2*roots[i])/nn;
|
|
centers[num].y = (ky1 + ky2*roots[i])/nn;
|
|
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
|
|
if (eq(nn, ZERO_MAX_ARC)) {
|
|
//printf("collinear\n");
|
|
|
|
//Centers of two circles, which will be intersected
|
|
coord ca, cb;
|
|
double u, v;
|
|
coord left, right;
|
|
int cntint;
|
|
|
|
//Always take first center
|
|
ca = c1;
|
|
u = r1 + k*roots[i];
|
|
|
|
//Use two centers with bigger distance. Note, that c2 and
|
|
//c3 are not more than double as distant as ca and cb.
|
|
if( PntPntDist(c1,c2) > PntPntDist(c1,c3) ) {
|
|
cb = c2;
|
|
v = r2 + l*roots[i];
|
|
}
|
|
else {
|
|
cb = c3;
|
|
v = r3 + m*roots[i];
|
|
}
|
|
|
|
//Left and right intersection points;
|
|
//Intersect the two circles
|
|
cntint = IntersectTwoCircles(ca, cb, u, v, &left, &right);
|
|
|
|
if (cntint < 1)
|
|
//error or no intersection
|
|
continue;
|
|
else if (cntint == 1) {
|
|
//Just one solution, take it
|
|
centers[num] = left;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
else {
|
|
//Two solutions
|
|
//printf("Sol 1: (%e, %e) :: %e, %e\n", left.x, left.y,
|
|
// Abs( Abs(distl2 - r2) - roots[i]),
|
|
// Abs( Abs(distl3 - r3) - roots[i]) );
|
|
|
|
//printf("Sol 2: (%e, %e) :: %e, %e\n", right.x, right.y,
|
|
// Abs( Abs(distr2 - r2) - roots[i]),
|
|
// Abs( Abs(distr3 - r3) - roots[i]) );
|
|
|
|
|
|
dl2 = AbsPntCircleDist(c2, r2, left) - roots[i];
|
|
dl3 = AbsPntCircleDist(c3, r3, left) - roots[i];
|
|
dr2 = AbsPntCircleDist(c2, r2, right) - roots[i];
|
|
dr3 = AbsPntCircleDist(c3, r3, right) - roots[i];
|
|
|
|
if (le(dl2, eps) && le(dl3, eps) &&
|
|
le(dr2, eps) && le(dr3, eps)) {
|
|
//Both solutions are good!
|
|
centers[num] = left;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
|
|
centers[num] = right;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
else if ((Abs(dl2) + Abs(dl3)) < (Abs(dr2) + Abs(dr3))) {
|
|
//Take the better one...
|
|
centers[num] = left;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
else {
|
|
centers[num] = right;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
assert(num<=VRONI_MAXSOL);
|
|
|
|
//Re-compute the clearance as the mean of the distances
|
|
for (i=0; i<num; i++) {
|
|
|
|
const coord c = centers[i];
|
|
const double r = AbsPntCircleDist(c1,r1, c) + AbsPntCircleDist(c2,r2, c)
|
|
+ AbsPntCircleDist(c3,r3, c);
|
|
|
|
radii[i] = r/3.0;
|
|
}
|
|
|
|
return num;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//These expressions are used to determine equations for x and y of the new center:
|
|
//x = (kx1 + kx2 * t) / nn
|
|
//y = (ky1 + ky2 * t) / nn
|
|
#undef ARC_NN
|
|
#undef ARC_KX1
|
|
#undef ARC_KX2
|
|
#undef ARC_KY1
|
|
#undef ARC_KY2
|
|
|
|
#define ARC_NN(a,b,c,d,e,f) ( 2.0*( f*(c-a) + e*(b-d) ) )
|
|
#define ARC_KX1(a,b,c,d,e,f,g,r,s) ( (f)*(c*c+(d)*(d)-a*a-(b)*(b)+r*r-s*s) + 2*g*(b-(d) ) )
|
|
#define ARC_KX2(a,b,c,d,e,f,g,r,s,k,l,m) ( 2.0*( m*(b-d) + (f)*(k*r-l*s) ) )
|
|
#define ARC_KY1(a,b,c,d,e,f,g,r,s) ARC_KX1( b, -a, d, -c, xx, -e, g, r, s )
|
|
#define ARC_KY2(a,b,c,d,e,f,g,r,s,k,l,m) ARC_KX2( xx, c, xx, a, xx, -e, xx, r, s, k, l, m )
|
|
|
|
|
|
int vroniObject::CircCircSegCenters(const coord & c1, const coord & c2,
|
|
double_arg r1, double_arg r2,
|
|
double_arg s1a, double_arg s1b,
|
|
double_arg s1c,
|
|
coord* centers, double* radii, double eps )
|
|
{
|
|
int dc, i;
|
|
//Number of Delaunay circles found
|
|
int num=0;
|
|
double kx2, ky2;
|
|
double kt1, kt2, kt3;
|
|
const double nn = ARC_NN(c1.x, c1.y, c2.x, c2.y, s1a, s1b );
|
|
const double kx1 = ARC_KX1(c1.x, c1.y, c2.x, c2.y, s1a, s1b, s1c, r1, r2 );
|
|
const double ky1 = ARC_KY1(c1.x, c1.y, c2.x, c2.y, s1a, s1b, s1c, r1, r2 );
|
|
double roots[2];
|
|
int numroots;
|
|
|
|
|
|
|
|
//printf("CircCircSeg: (%e, %e) %e; (%e, %e) %e; %e, %e, %e \n", c1.x, c1.y, r1,
|
|
// c2.x, c2.y, r2, s1a, s1b, s1c);
|
|
|
|
|
|
//dc is a number 0...7. Its binary representation is used
|
|
//to create triples from {-1,1}^3. These three numbers represent
|
|
//the offset directions k, l, m of the three circles.
|
|
for(dc=0; dc<=7; dc++) {
|
|
const double k = (dc & 4) ? 1 : -1;
|
|
const double l = (dc & 2) ? 1 : -1;
|
|
const double m = (dc & 1) ? 1 : -1;
|
|
|
|
//If radius is 0 a offsetting in the interior of circle is not possible
|
|
//This is only for performance boosting, since t has to be > -ZERO anyway
|
|
if (((k == -1) && le(r1, ZERO)) ||
|
|
((l == -1) && le(r2, ZERO)))
|
|
continue;
|
|
|
|
kx2 = ARC_KX2( c1.x, c1.y, c2.x, c2.y, s1a, s1b, s1c, r1, r2, k, l, m );
|
|
ky2 = ARC_KY2( c1.x, c1.y, c2.x, c2.y, s1a, s1b, s1c, r1, r2, k, l, m );
|
|
|
|
//Note: if kt1 wouldn't be modified by Roots2abc, then this could be extracted from loop
|
|
kt1 = ARC_KT1(nn, kx1, kx2, ky1, ky2, c1.x, c1.y, r1);
|
|
kt2 = ARC_KT2(nn, kx1, kx2, ky1, ky2, c1.x, c1.y, r1, k);
|
|
kt3 = ARC_KT3(nn, kx2, ky2);
|
|
|
|
|
|
//Solve the 2nd degree polynomal equation
|
|
numroots = ArcRoots2abc(kt3, kt2, kt1, roots);
|
|
|
|
//Save all roots which have been found
|
|
for (i=0; i<numroots; i++) {
|
|
//printf("\troot: %e\n", roots[i]);
|
|
//printf("k: %e, l: %e, m: %e\n", k, l, m);
|
|
//printf("\tkx1: %e, kx2: %e, ky1: %e, ky2: %e, kt1: %e, kt2: %e, kt3: %e nn: %e\n", kx1, kx2, ky1, ky2, kt1, kt2, kt3, nn);
|
|
|
|
|
|
//Only add, if
|
|
// radius of found delauney circle is positive
|
|
// radii of offset circles are positive
|
|
if (lt(roots[i], ZERO) || lt(r1+roots[i]*k, ZERO) ||
|
|
lt(r2+roots[i]*l, ZERO))
|
|
continue;
|
|
|
|
//Too big.
|
|
if (Abs(roots[i]) > too_large)
|
|
continue;
|
|
|
|
//If not almost zero, set it to zero
|
|
if (roots[i] < 0.0) roots[i] = 0.0;
|
|
if ((r1+roots[i]*k) < 0.0) roots[i] = -r1 / k;
|
|
if ((r2+roots[i]*l) < 0.0) roots[i] = -r2 / l;
|
|
|
|
if (!eq(nn, eps)) {
|
|
//Insert in parameterization of x and y
|
|
centers[num].x = (kx1 + kx2*roots[i])/nn;
|
|
centers[num].y = (ky1 + ky2*roots[i])/nn;
|
|
radii[num] = roots[i];
|
|
//printf("\t\t> sol: (%e, %e), %e\n", centers[num].x, centers[num].y, radii[num]);
|
|
num++;
|
|
}
|
|
|
|
if (eq(nn, ZERO_MAX_ARC)) {
|
|
//The segment is orthogonal to circle-center-joint-line
|
|
//We can not insert in parameterization of x and y
|
|
//We have to calculate the intersection of segment and first
|
|
//arc "by hand"
|
|
//printf("nn=%e\n", nn);
|
|
coord left, right;
|
|
int cntsol;
|
|
cntsol = IntersectCircleSeg(c1, r1+k*roots[i], s1a, s1b,
|
|
s1c+m*roots[i], &left, &right);
|
|
|
|
if (cntsol >= 1) {
|
|
//printf("\t\t> sol1: (%e, %e), %e\n", centers[num].x, centers[num].y, radii[num]);
|
|
centers[num] = left;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
if (cntsol >= 2) {
|
|
//printf("\t\t> sol2: (%e, %e), %e\n", centers[num].x, centers[num].y, radii[num]);
|
|
centers[num] = right;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
assert(num<=VRONI_MAXSOL);
|
|
return num;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//These expressions are used to determine equations for x and y of the new center:
|
|
//x = (kx1 + kx2 * t) / nn
|
|
//y = (ky1 + ky2 * t) / nn
|
|
#undef ARC_NN
|
|
#undef ARC_KX1
|
|
#undef ARC_KX2
|
|
#undef ARC_KY1
|
|
#undef ARC_KY2
|
|
|
|
|
|
int vroniObject::CircSegSegCenters( const coord & c1, double_arg r1,
|
|
double_arg s1a, double_arg s1b,
|
|
double_arg s1c,
|
|
double_arg s2a, double_arg s2b,
|
|
double_arg s2c,
|
|
coord* centers, double* radii,
|
|
double eps)
|
|
{
|
|
int dc, i;
|
|
//Number of found delauney circles
|
|
int num=0;
|
|
|
|
const double nn = s1a*s2b - s1b*s2a;
|
|
const double kx1 = s2b*s1c - s1b*s2c;
|
|
const double ky1 = s1a*s2c - s1c*s2a;
|
|
|
|
double roots[2];
|
|
int numroots;
|
|
double kx2, ky2;
|
|
double kt1, kt2, kt3;
|
|
|
|
//printf("CircSegSeg: (%e, %e) %e; %e, %e %e; %e, %e, %e \n", c1.x, c1.y, r1,
|
|
// s1a, s1b, s1c, s2a, s2b, s2c);
|
|
|
|
|
|
|
|
|
|
//dc is a number 0...7. Its binary representation is used
|
|
//to create triples from {-1,1}^3. These three numbers represent
|
|
//the offset directions k, l, m of the three circles.
|
|
for(dc=0; dc<=7; dc++) {
|
|
const double k = (dc & 4) ? 1 : -1;
|
|
const double l = (dc & 2) ? 1 : -1;
|
|
const double m = (dc & 1) ? 1 : -1;
|
|
|
|
//If radius is 0 a offsetting in the interior of circle is not possible
|
|
//This is only for performance boosting, since t has to be > -ZERO anyway
|
|
if ( k==-1 && le(r1, ZERO))
|
|
continue;
|
|
|
|
kx2 = s2b*l - s1b*m;
|
|
ky2 = s1a*m - s2a*l;
|
|
|
|
//Note: if kt1 wouldn't be modified by Roots2abc, then this could be extracted from loop
|
|
kt1 = ARC_KT1(nn, kx1, kx2, ky1, ky2, c1.x, c1.y, r1);
|
|
kt2 = ARC_KT2(nn, kx1, kx2, ky1, ky2, c1.x, c1.y, r1, k);
|
|
kt3 = ARC_KT3(nn, kx2, ky2);
|
|
|
|
//Solve the 2nd degree polynom
|
|
numroots = ArcRoots2abc(kt3, kt2, kt1, roots);
|
|
|
|
//Save all roots which have been found
|
|
for (i=0; i<numroots; i++) {
|
|
//Only add, if
|
|
// radius of found delauney circle is positive
|
|
// radii of offset circles are positive
|
|
if (lt(roots[i], ZERO) || lt(r1+roots[i]*k, ZERO))
|
|
continue;
|
|
|
|
//Too big.
|
|
if (Abs(roots[i]) > too_large)
|
|
continue;
|
|
|
|
//If not almost zero, set it to zero
|
|
if (roots[i] < 0.0) roots[i] = 0.0;
|
|
if ((r1+roots[i]*k) < 0.0) roots[i] = -r1 / k;
|
|
|
|
if (!eq(nn, eps)) {
|
|
//Insert in parameterization of x and y
|
|
centers[num].x = (kx1 + kx2*roots[i])/nn;
|
|
centers[num].y = (ky1 + ky2*roots[i])/nn;
|
|
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
|
|
if (eq(nn, ZERO_MAX_ARC)) {
|
|
//Two segments are parallel
|
|
//We can not insert in parameterization of x and y
|
|
//We have to calculate the intersection of first segment and
|
|
//arc "by hand"
|
|
coord left, right;
|
|
int cntsol;
|
|
cntsol = IntersectCircleSeg(c1, r1+k*roots[i], s1a, s1b,
|
|
s1c+l*roots[i], &left, &right);
|
|
|
|
if (cntsol >= 1) {
|
|
centers[num] = left;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
if (cntsol >= 2) {
|
|
centers[num] = right;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
assert(num<=VRONI_MAXSOL);
|
|
return num;
|
|
}
|
|
|
|
|
|
|
|
|
|
vr_bool vroniObject::IsPntInArcConeEps(int arc, const coord & pnt,
|
|
double_arg eps)
|
|
{
|
|
const coord c = GetArcCenter(arc);
|
|
const coord sn = GetArcStartNormal(arc);
|
|
const coord en = GetArcEndNormal(arc);
|
|
coord cp, cs, ce, n;
|
|
|
|
assert(InArcsList(arc));
|
|
|
|
cp = VecSub(pnt, c);
|
|
cs = VecSub(GetArcStartCoord(arc), c);
|
|
ce = VecSub(GetArcEndCoord(arc), c);
|
|
n = VecAdd(cs, ce);
|
|
|
|
//Avoid long accepting ranges!
|
|
if ( VecDotProd(n, cp) < (eps<0 ? 0 : -eps) )
|
|
return 0;
|
|
|
|
return IsInArcConeZero(sn, en, cp, eps)==0;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Get intersection point of circle with center c1, radius r1 and center c2, radius r2.
|
|
* Since there are in general two intersection points, we will take the one on the left
|
|
* of the line c1->c2 if isleft!=0, the right one otherwise.
|
|
*
|
|
* This function returns the number of intersection points: 0, 1, or 2 (-1 on error)
|
|
* If there are 1, left and right are set to the single one
|
|
* If there are 2, left is the one left of the line c1->c2
|
|
* */
|
|
int vroniObject::IntersectTwoCircles( const coord & c1, const coord & c2,
|
|
double_arg r1, double_arg r2,
|
|
coord* left, coord* right)
|
|
{
|
|
//Tactics:
|
|
//
|
|
//There is a triangle between the c1, p, c2. What we are interested, is at first
|
|
//the ortho-projection projpnt of p on the straight line c1->c2. Then c1, p, q are forming
|
|
//a right-angled triangle and the p is given by the ortho-projection and ortho-distance
|
|
//on c1-c2.
|
|
|
|
|
|
//printf("\tIntersection: (%e, %e) %e, (%e, %e) %e\n", c1.x, c1.y, r1, c2.x, c2.y, r2);
|
|
|
|
//Get squared distance between ca and cb
|
|
const double w = PntPntDistSq(c1, c2);
|
|
|
|
assert(left!=0);
|
|
assert(right!=0);
|
|
|
|
|
|
//Two circles are (almost) cocentric
|
|
if( w < ZERO )
|
|
{
|
|
//printf("cocentric\n");
|
|
return -1;
|
|
}
|
|
|
|
//Ortho-distance of intersection to line c1-c2
|
|
//a := |projpnt-c1|, b:=|projpnt-c2|
|
|
//r1^2 - a^2 = dist^2
|
|
//r2^2 - b^2 = dist^2
|
|
//a + b = w, iff
|
|
const double a = (w + r1*r1 - r2*r2)/(2*sqrt(w));
|
|
|
|
//Squared ortho-dist
|
|
const double h = r1*r1-a*a;
|
|
|
|
//printf("\tsqr_w=%e, a=%e, h=%e\n", sqrt(w), a, h);
|
|
|
|
//Discriminant gets negative
|
|
if( h < -ZERO )
|
|
{
|
|
//printf("imaginary intersection\n");
|
|
return 0;
|
|
}
|
|
|
|
|
|
//Calculate the projection of intersection on line pa-pb
|
|
// projpnt = c1 + a/sqrt(w) * (c2-c1)
|
|
const coord projpnt = VecAdd(c1, VecMult(a/sqrt(w), VecSub(c2,c1)));
|
|
|
|
//Discriminant is almost zero --> one solution
|
|
if( h < ZERO2 )
|
|
{
|
|
*left = projpnt;
|
|
*right = projpnt;
|
|
|
|
return 1;
|
|
}
|
|
//Non-zero discriminat --> two solutions
|
|
else
|
|
{
|
|
//Normalized and orthogonal (CCW) to c1->c2
|
|
const coord ortho = VecMult(1/sqrt(w), VecCCW( VecSub(c2,c1) ));
|
|
|
|
assert(left!=0);
|
|
assert(right!=0);
|
|
|
|
*left = VecAdd(projpnt, VecMult(sqrt(h), ortho));
|
|
*right = VecSub(projpnt, VecMult(sqrt(h), ortho));
|
|
|
|
return 2;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Get solutions when intersecting a segment a.x+b.y=d with circle centered at c and with radious r.
|
|
* This function returns the number of solution: 0, 1 or 2.
|
|
* If two solutions exists, left is the point on the left part of the segment and right on the right part.
|
|
* If one solution exists, left=right=single solution
|
|
*/
|
|
int vroniObject::IntersectCircleSeg( const coord & c, double_arg r,
|
|
double_arg a, double_arg b, double_arg d,
|
|
coord* left, coord* right)
|
|
{
|
|
//Distance of segment and circle center
|
|
const double h = PntLineDist(a, b, -d, c);
|
|
//Midpoint of intersection points of segment and first offset-circle
|
|
const coord p = VecSub(c, VecMult(h, MakeVec(a,b)));
|
|
|
|
assert(left!=0);
|
|
assert(right!=0);
|
|
|
|
|
|
//Negative discriminant
|
|
if( h*h - r*r > ZERO)
|
|
return 0;
|
|
|
|
//Discriminant almoast zero
|
|
if( h*h - r*r > - ZERO2)
|
|
{
|
|
*left = *right = p;
|
|
return 1;
|
|
}
|
|
//Two solutions
|
|
else
|
|
{
|
|
const coord q = VecMult( sqrt(r*r-h*h), VecCW(MakeVec(a,b)) );
|
|
*left = VecAdd(p, q);
|
|
*right = VecSub(p, q);
|
|
return 2;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
vr_bool vroniObject::DoArcPntIntersect(int arc, const coord & pnt,
|
|
double_arg eps)
|
|
{
|
|
coord c;
|
|
double r;
|
|
|
|
//Check for strict containment...
|
|
if ( ! IsPntInArcConeEps(arc, pnt, -eps) ) {
|
|
return false;
|
|
}
|
|
|
|
c = GetArcCenter(arc);
|
|
r = GetArcRadius(arc);
|
|
return eq( AbsPntCircleDist(c,r, pnt), eps);
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* The circle centered at c1 with radius r1 and the circle c2 with the radius r2 are placed
|
|
* side-by-side and a hyperbola is defined as bisector. This bisector has two asymptotes which
|
|
* are determined. The directions of those are saved in dir1 and dir2. The start points of
|
|
* these rays are at (a,b) resp. (a,-b) modulo rotation and offset from the crossing point.
|
|
* These is useful to get an approximation to min. distance to both circles.
|
|
*/
|
|
void vroniObject::GetAsymptotes(const coord & c1, double_arg r1,
|
|
const coord & c2, double_arg r2,
|
|
coord *start1, coord *dir1,
|
|
coord* start2, coord *dir2)
|
|
{
|
|
const double dist = PntPntDist(c1,c2);
|
|
const double e = dist/2.0;
|
|
const double a = (r1-r2)/2.0;
|
|
double b;
|
|
coord mid;
|
|
coord v1, v2;
|
|
double crot, srot;
|
|
|
|
mid = MidPoint(c1, c2);
|
|
|
|
assert(e>=a);
|
|
assert(e>ZERO);
|
|
|
|
b = sqrt(e*e-a*a);
|
|
|
|
//printf("GetAsymptotes of (%e, %e; %e), (%e, %e; %e)\n", c1.x, c1.y, r1, c2.x, c2.y, r2);
|
|
|
|
//printf("mid: %e, a: %e, b: %e\n", mid, a, b);
|
|
|
|
//The un-rotated directions
|
|
v1.x = a/e;
|
|
v1.y = b/e;
|
|
v2.x = a/e;
|
|
v2.y = -b/e;
|
|
|
|
assert( Abs(VecLenSq(v1)-1) < ZERO_MAX);
|
|
assert( Abs(VecLenSq(v2)-1) < ZERO_MAX);
|
|
|
|
//Rotation-Matrix entries
|
|
crot = (c2.x-c1.x)/dist;
|
|
srot = (c2.y-c1.y)/dist;
|
|
|
|
//printf("crot: %e, srot: %e\n", crot, srot);
|
|
|
|
//Rotate the directions
|
|
dir1->x = crot*v1.x - srot*v1.y;
|
|
dir1->y = srot*v1.x + crot*v1.y;
|
|
dir2->x = crot*v2.x - srot*v2.y;
|
|
dir2->y = srot*v2.x + crot*v2.y;
|
|
|
|
//Get the rotated starts
|
|
start1->x = mid.x + crot*a - srot*b;
|
|
start1->y = mid.y + srot*a + crot*b;
|
|
start2->x = mid.x + crot*a + srot*b;
|
|
start2->y = mid.y + srot*a - crot*b;
|
|
|
|
//printf("start1: (%e, %e), dir1: (%e, %e), start2: (%e, %e), dir2: (%e, %e)\n",
|
|
// start1->x, start1->y, dir1->x, dir1->y, start2->x, start2->y, dir2->x, dir2->y);
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
* We shoot a ray starting at p in direction v and intersect this ray with
|
|
* the offset circles of the point c. Hence, the intersection lies on the ray
|
|
* and is equidistant to p and to c. Effectively, this means intersecting
|
|
* the bisector between p and c with the ray.
|
|
*
|
|
* returns the number of solutions
|
|
*
|
|
*/
|
|
int vroniObject::IntersectRayPoint(const coord & p, coord v, const coord & c,
|
|
coord* centers, double* radii, double eps )
|
|
{
|
|
coord pc;
|
|
double t, denom;
|
|
|
|
//Normalize v
|
|
v = VecNorm(v);
|
|
//printf("v: (%20.16f %20.16f) inside\n", v.x, v.y);
|
|
|
|
//Vector c->p and distance
|
|
pc = VecSub(c,p);
|
|
|
|
denom = VecDotProd(v, pc);
|
|
|
|
// printf("denom = %20.16f\n", denom);
|
|
|
|
if (eq(denom, eps)) {
|
|
/* */
|
|
/* the ray and the bisector are parallel. no solution! */
|
|
/* */
|
|
return 0;
|
|
}
|
|
else {
|
|
t = 0.5 * VecDotProd(pc, pc) / denom;
|
|
centers[0] = VecAdd(p, VecMult(t, v));
|
|
if (t < 0.0) radii[0] = -t;
|
|
else radii[0] = t;
|
|
//printf("center:\n (%20.16f, %20.16f)\n", centers[0].x, centers[0].y);
|
|
//printf("radius:\n %20.16f\n", t);
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* We shoot a ray starting at p in direction v and intersect this ray with the
|
|
* offset-circles of the circle centered at c and with radius r. So the
|
|
* intersection lies on the ray and is equidistant to p and to the circle (c,r)
|
|
*
|
|
* returns the number of solutions
|
|
*
|
|
*/
|
|
int vroniObject::IntersectRayCircle(const coord & p, coord v, const
|
|
coord & c, double_arg r,
|
|
coord* centers, double* radii, double eps )
|
|
{
|
|
double t;
|
|
coord cp;
|
|
int num=0, dc;
|
|
double ba, bb, bc;
|
|
|
|
|
|
//Normalize v
|
|
v = VecNorm(v);
|
|
|
|
//Vector c->p and distance
|
|
cp = VecSub(p,c);
|
|
|
|
//printf("IntersectRayCircle(): p=(%e,%e), v=(%e, %e), c=(%e, %e), r=%e\n",
|
|
// p.x, p.y, v.x, v.y, c.x, c.y, r);
|
|
|
|
//p is on (c,r)
|
|
if( eq( PntCircleDist(c, r, p), eps) ) {
|
|
//printf("ray start-point on circle\n");
|
|
|
|
//So this is one solution
|
|
centers[0] = p;
|
|
radii[0] = 0.0;
|
|
|
|
//And this is the other
|
|
centers[1] = c;
|
|
radii[1] = r;
|
|
return 2;
|
|
}
|
|
|
|
|
|
//printf("center not on ray: %e\n", Abs(cp.x*v.y - cp.y*v.x)/VecLen(cp)/VecLen(v));
|
|
|
|
|
|
//Check all offset directions for solutions
|
|
//The following calculations (Note (I) and (II)) are based on offset-based
|
|
//determination of equidistant points. See the corresponding Mathematica
|
|
//files for that.
|
|
for (dc=0; dc<=3; dc++) {
|
|
const double k = (dc & 2) ? 1 : -1;
|
|
const double l = (dc & 1) ? 1 : -1;
|
|
|
|
if ((r > 0.0) || (l == 1)) {
|
|
//Note (I)
|
|
const double denom = l*r - k*VecDotProd(cp,v);
|
|
|
|
if( Abs(denom) < eps ) {
|
|
//printf("Skip solution\n");
|
|
continue;
|
|
}
|
|
|
|
//Note (II)
|
|
t = (PntPntDistSq(p,c) - r*r)/(2*denom);
|
|
|
|
//printf("k=%e, l=%e, t=%e\n", k, l, t);
|
|
|
|
//Solution is valid
|
|
if (-eps <= t) {
|
|
if (t < 0.0) t = 0.0;
|
|
radii[num] = t;
|
|
// p + k*t * v
|
|
centers[num] = VecAdd(p, VecMult(k*t, v));
|
|
num++;
|
|
}
|
|
}
|
|
}
|
|
|
|
//Found a solution
|
|
if( num > 0 )
|
|
return num;
|
|
//printf("No solution?\n");
|
|
|
|
|
|
//The base segments equation data
|
|
ba = v.x;
|
|
bb = v.y;
|
|
bc = ba*p.x + bb*p.y;
|
|
|
|
//Determine the intersections
|
|
return CircCircSegCenters( p, c, 0, r, ba,bb,bc, centers, radii, eps);
|
|
}
|
|
|
|
|
|
/**
|
|
* We shoot a ray starting at p in direction v and intersect this ray with the offset-segments
|
|
* of the segment with normal vector (a,b) and hesse form a.x+b.y=c. So the intersection lies on the
|
|
* ray and is equidistant to p and to the segment.
|
|
*
|
|
* returns the number of solutions
|
|
* */
|
|
int vroniObject::IntersectRaySegment(const coord & p, coord v,
|
|
double_arg a, double_arg b, double_arg c,
|
|
coord* centers, double* radii, double eps )
|
|
{
|
|
double t;
|
|
int num=0, dc;
|
|
coord segnorm;
|
|
double ba, bb, bc;
|
|
|
|
//The point ep lies already on the line
|
|
if (eq(AbsPntLineDist(a, b, -c, p), eps ))
|
|
{
|
|
//So this is the only solution
|
|
centers[0] = p;
|
|
radii[0] = 0.0;
|
|
return 1;
|
|
}
|
|
|
|
//Normalize v
|
|
v = VecNorm(v);
|
|
segnorm = MakeVec(a,b);
|
|
|
|
//printf("IntersectRaySegment(): p=(%e,%e), v=(%e,%e), seg=(%e,%e;%e)\n",
|
|
// p.x, p.y, v.x, v.y, a, b, c);
|
|
|
|
|
|
//Check all offset directions for solutions
|
|
//The following calculations (Note (I) and (II)) are based on offset-based
|
|
//determination of equidistant points. See the corresponding Mathematica
|
|
//files for that.
|
|
for (dc = 0; dc <= 3; dc++) {
|
|
const double k = (dc & 2) ? 1 : -1;
|
|
const double l = (dc & 1) ? 1 : -1;
|
|
|
|
//Note (I)
|
|
const double denom = k*VecDotProd(segnorm,v) - l;
|
|
|
|
if (!eq(denom, eps)) {
|
|
//Note (II)
|
|
t = (c - VecDotProd(segnorm,p))/(denom);
|
|
|
|
//printf("k=%e, l=%e, t=%e\n", k, l, t);
|
|
|
|
//Solution is valid
|
|
if ( -eps <= t ) {
|
|
if (t < 0.0) t = 0.0;
|
|
radii[num] = t;
|
|
centers[num] = VecAdd(p, VecMult(k*t, v));
|
|
num++;
|
|
}
|
|
}
|
|
}
|
|
|
|
//Found a solution
|
|
if (num > 0) return num;
|
|
|
|
//printf("No solution?\n");
|
|
|
|
|
|
//The base segments equation data
|
|
ba = v.x;
|
|
bb = v.y;
|
|
bc = ba*p.x + bb*p.y;
|
|
|
|
//printf("p.x=%e, p.y=%e, ba=%e, bb=%e, bc=%e, a=%e, b=%e, c=%e\n", p.x, p.y, ba, bb, bc, a, b, c);
|
|
|
|
//Determine the intersections
|
|
return CircSegSegCenters( p, 0, ba,bb,bc, a,b,c, centers, radii, eps);
|
|
}
|
|
|
|
|
|
|
|
int vroniObject::CircPntPntCenters( const coord & c1, const coord & c2,
|
|
const coord & c3, double_arg r1,
|
|
double_arg l,
|
|
coord* centers, double* radii )
|
|
{
|
|
int num = 0, i;
|
|
int dc;
|
|
coord m, v, mc;
|
|
double d, a, b, delta;
|
|
double roots[2];
|
|
int numroots;
|
|
|
|
/* */
|
|
/* mid point and unit vector that is normal onto c2 -> c3 */
|
|
/* */
|
|
v = VecSub(c2, c3);
|
|
d = VecLen(v);
|
|
assert(d > ZERO);
|
|
v = VecCCW(v);
|
|
v = VecDiv(d, v);
|
|
m = MidPoint(c2, c3);
|
|
|
|
d /= 2.0;
|
|
mc = VecSub(c1, m);
|
|
a = 2.0 * VecDotProd(mc, v);
|
|
b = r1 * r1 + d * d - VecLenSq(mc);
|
|
|
|
for (dc = 0; dc <= 1; dc++) {
|
|
const double k = (dc & 1) ? 1 : -1;
|
|
|
|
numroots = ArcRoots2abc(a * a - 4.0 * r1 * r1,
|
|
- 4.0 * b * r1 * l,
|
|
- d * d * a * a - b * b, roots);
|
|
/* */
|
|
/* save all roots which have been found. */
|
|
/* note: radii of Delaunay circle and offset circle are positive */
|
|
/* */
|
|
for (i = 0; i < numroots; i++) {
|
|
//printf("%1f:%1f: root: %e\n", k,l, roots[i]);
|
|
|
|
if (Abs(roots[i]) > too_large)
|
|
continue;
|
|
if (lt(roots[i], ZERO))
|
|
continue;
|
|
if (roots[i] < 0.0)
|
|
roots[i] = 0.0;
|
|
if (lt(r1+roots[i]*l, ZERO))
|
|
continue;
|
|
if ((r1+roots[i]*l) < 0.0)
|
|
roots[i] = -r1 / l;
|
|
if (lt((Abs(roots[i]) - Abs(d)), ZERO))
|
|
continue;
|
|
if ((Abs(roots[i]) - Abs(d)) < 0.0)
|
|
roots[i] = Sign(roots[i]) * Abs(d);
|
|
|
|
/* */
|
|
/* get center and radius */
|
|
/* */
|
|
radii[num] = roots[i];
|
|
delta = roots[i] * roots[i] - d * d;
|
|
if (delta < 0.0) delta = 0.0;
|
|
delta = sqrt(delta);
|
|
centers[num].x = m.x + k * delta * v.x;
|
|
centers[num].y = m.y + k * delta * v.y;
|
|
radii[num] = roots[i];
|
|
num++;
|
|
}
|
|
}
|
|
|
|
/* */
|
|
/* re-compute the clearance as the mean of the distances */
|
|
/* */
|
|
for (i = 0; i < num; i++) {
|
|
const coord c = centers[i];
|
|
const double r = AbsPntCircleDist(c1, r1, c) +
|
|
AbsPntCircleDist(c2, 0.0, c) + AbsPntCircleDist(c3, 0.0, c);
|
|
radii[i] = r / 3.0;
|
|
}
|
|
|
|
assert(num <= VRONI_MAXSOL);
|
|
|
|
return num;
|
|
}
|