Select Git revision
robustPredicates.cpp
Numeric.cpp 44.83 KiB
// Gmsh - Copyright (C) 1997-2018 C. Geuzaine, J.-F. Remacle
//
// See the LICENSE.txt file for license information. Please report all
// bugs and problems to the public mailing list <gmsh@onelab.info>.
#include <algorithm>
#include "GmshConfig.h"
#include "GmshMessage.h"
#include "Numeric.h"
double myatan2(double a, double b)
{
if(a == 0.0 && b == 0)
return 0.0;
return atan2(a, b);
}
double myasin(double a)
{
if(a <= -1.)
return -M_PI / 2.;
else if(a >= 1.)
return M_PI / 2.;
else
return asin(a);
}
double myacos(double a)
{
if(a <= -1.)
return M_PI;
else if(a >= 1.)
return 0.;
else
return acos(a);
}
double norm2(double a[3][3])
{
double norm2sq =
gmsh_SQU(a[0][0])+
gmsh_SQU(a[0][1])+
gmsh_SQU(a[0][2])+
gmsh_SQU(a[1][0])+
gmsh_SQU(a[1][1])+
gmsh_SQU(a[1][2])+
gmsh_SQU(a[2][0])+
gmsh_SQU(a[2][1])+
gmsh_SQU(a[2][2]);
return sqrt(norm2sq);
}
void matvec(double mat[3][3], double vec[3], double res[3])
{
res[0] = mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2];
res[1] = mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2];
res[2] = mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2];
}
void matmat(double mat1[3][3], double mat2[3][3], double res[3][3])
{
res[0][0] = mat1[0][0]*mat2[0][0] + mat1[0][1]*mat2[1][0] + mat1[0][2]*mat2[2][0];
res[0][1] = mat1[0][0]*mat2[0][1] + mat1[0][1]*mat2[1][1] + mat1[0][2]*mat2[2][1];
res[0][2] = mat1[0][0]*mat2[0][2] + mat1[0][1]*mat2[1][2] + mat1[0][2]*mat2[2][2];
res[1][0] = mat1[1][0]*mat2[0][0] + mat1[1][1]*mat2[1][0] + mat1[1][2]*mat2[2][0];
res[1][1] = mat1[1][0]*mat2[0][1] + mat1[1][1]*mat2[1][1] + mat1[1][2]*mat2[2][1];
res[1][2] = mat1[1][0]*mat2[0][2] + mat1[1][1]*mat2[1][2] + mat1[1][2]*mat2[2][2];
res[2][0] = mat1[2][0]*mat2[0][0] + mat1[2][1]*mat2[1][0] + mat1[2][2]*mat2[2][0];
res[2][1] = mat1[2][0]*mat2[0][1] + mat1[2][1]*mat2[1][1] + mat1[2][2]*mat2[2][1];
res[2][2] = mat1[2][0]*mat2[0][2] + mat1[2][1]*mat2[1][2] + mat1[2][2]*mat2[2][2];}
void normal3points(double x0, double y0, double z0,
double x1, double y1, double z1,
double x2, double y2, double z2,
double n[3])
{
double t1[3] = {x1 - x0, y1 - y0, z1 - z0};
double t2[3] = {x2 - x0, y2 - y0, z2 - z0};
prodve(t1, t2, n);
norme(n);
}
void normal2points(double x0, double y0, double z0,
double x1, double y1, double z1,
double n[3])
{
// this computes one of the normals to the edge
double t[3] = {x1 - x0, y1 - y0, z1 - z0};
double ex[3] = {0., 0., 0.};
if(t[0] == 0.)
ex[0] = 1.;
else if(t[1] == 0.)
ex[1] = 1.;
else
ex[2] = 1.;
prodve(t, ex, n);
norme(n);
}
int sys2x2(double mat[2][2], double b[2], double res[2])
{
double det, ud, norm;
int i;
norm = gmsh_SQU(mat[0][0]) + gmsh_SQU(mat[1][1]) + gmsh_SQU(mat[0][1]) + gmsh_SQU(mat[1][0]);
det = mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1];
// TOLERANCE ! WARNING WARNING
if(norm == 0.0 || fabs(det) / norm < 1.e-12) {
if(norm)
Msg::Debug("Assuming 2x2 matrix is singular (det/norm == %.16g)",
fabs(det) / norm);
res[0] = res[1] = 0.0;
return 0;
}
ud = 1. / det;
res[0] = b[0] * mat[1][1] - mat[0][1] * b[1];
res[1] = mat[0][0] * b[1] - mat[1][0] * b[0];
for(i = 0; i < 2; i++)
res[i] *= ud;
return (1);
}
double det3x3(double mat[3][3])
{
return (mat[0][0] * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) -
mat[0][1] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) +
mat[0][2] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));
}
double trace3x3(double mat[3][3])
{
return mat[0][0] + mat[1][1] + mat[2][2];
}
double trace2 (double mat[3][3]){
double a00 = mat[0][0] * mat[0][0] + mat[1][0] * mat[0][1] + mat[2][0] * mat[0][2];
double a11 = mat[1][0] * mat[0][1] + mat[1][1] * mat[1][1] + mat[1][2] * mat[2][1];
double a22 = mat[2][0] * mat[0][2] + mat[2][1] * mat[1][2] + mat[2][2] * mat[2][2];
return a00 + a11 + a22;
}
int sys3x3(double mat[3][3], double b[3], double res[3], double *det)
{
double ud;
int i;
*det = det3x3(mat);
if(*det == 0.0) {
res[0] = res[1] = res[2] = 0.0;
return (0);
}
ud = 1. / (*det);
res[0] = b[0] * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) -
mat[0][1] * (b[1] * mat[2][2] - mat[1][2] * b[2]) +
mat[0][2] * (b[1] * mat[2][1] - mat[1][1] * b[2]);
res[1] = mat[0][0] * (b[1] * mat[2][2] - mat[1][2] * b[2]) -
b[0] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) +
mat[0][2] * (mat[1][0] * b[2] - b[1] * mat[2][0]);
res[2] = mat[0][0] * (mat[1][1] * b[2] - b[1] * mat[2][1]) -
mat[0][1] * (mat[1][0] * b[2] - b[1] * mat[2][0]) +
b[0] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]);
for(i = 0; i < 3; i++)
res[i] *= ud;
return (1);
}
int sys3x3_with_tol(double mat[3][3], double b[3], double res[3], double *det)
{
int out;
double norm;
out = sys3x3(mat, b, res, det);
norm =
gmsh_SQU(mat[0][0]) + gmsh_SQU(mat[0][1]) + gmsh_SQU(mat[0][2]) +
gmsh_SQU(mat[1][0]) + gmsh_SQU(mat[1][1]) + gmsh_SQU(mat[1][2]) +
gmsh_SQU(mat[2][0]) + gmsh_SQU(mat[2][1]) + gmsh_SQU(mat[2][2]);
// TOLERANCE ! WARNING WARNING
if(norm == 0.0 || fabs(*det) / norm < 1.e-12) {
if(norm)
Msg::Debug("Assuming 3x3 matrix is singular (det/norm == %.16g)",
fabs(*det) / norm);
res[0] = res[1] = res[2] = 0.0;
return 0;
}
return out;
}
double det2x2(double mat[2][2])
{
return mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1];
}
double det2x3(double mat[2][3])
{
double v1[3] = {mat[0][0], mat[0][1], mat[0][2]}; double v2[3] = {mat[1][0], mat[1][1], mat[1][2]};
double n[3];
prodve(v1, v2, n);
return norm3(n);
}
double inv2x2(double mat[2][2], double inv[2][2])
{
const double det = det2x2(mat);
if(det){
double ud = 1. / det;
inv[0][0] = mat[1][1] * ud;
inv[1][0] = -mat[1][0] * ud;
inv[0][1] = -mat[0][1] * ud;
inv[1][1] = mat[0][0] * ud;
}
else{
Msg::Error("Singular matrix 2x2");
for(int i = 0; i < 2; i++)
for(int j = 0; j < 2; j++)
inv[i][j] = 0.;
}
return det;
}
double inv3x3(double mat[3][3], double inv[3][3])
{
double det = det3x3(mat);
if(det){
double ud = 1. / det;
inv[0][0] = (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]) * ud;
inv[1][0] = -(mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) * ud;
inv[2][0] = (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]) * ud;
inv[0][1] = -(mat[0][1] * mat[2][2] - mat[0][2] * mat[2][1]) * ud;
inv[1][1] = (mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]) * ud;
inv[2][1] = -(mat[0][0] * mat[2][1] - mat[0][1] * mat[2][0]) * ud;
inv[0][2] = (mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]) * ud;
inv[1][2] = -(mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0]) * ud;
inv[2][2] = (mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]) * ud;
}
else{
Msg::Error("Singular matrix 3x3");
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
inv[i][j] = 0.;
}
return det;
}
double angle_02pi(double A3)
{
double DP = 2 * M_PI;
while(A3 > DP || A3 < 0.) {
if(A3 > 0)
A3 -= DP;
else
A3 += DP;
}
return A3;
}
double angle_plan(double v[3], double p1[3], double p2[3], double n[3])
{
double PA[3], PB[3], angplan;
PA[0] = p1[0] - v[0];
PA[1] = p1[1] - v[1];
PA[2] = p1[2] - v[2];
PB[0] = p2[0] - v[0];
PB[1] = p2[1] - v[1];
PB[2] = p2[2] - v[2];
norme(PA);
norme(PB);
double c[3];
prodve(PA, PB, c);
double const cosc = prosca(PA, PB);
double const sinc = prosca(c, n);
angplan = myatan2(sinc, cosc);
return angplan;
}
double triangle_area(double p0[3], double p1[3], double p2[3])
{
double a[3], b[3], c[3];
a[0] = p2[0] - p1[0];
a[1] = p2[1] - p1[1];
a[2] = p2[2] - p1[2];
b[0] = p0[0] - p1[0];
b[1] = p0[1] - p1[1];
b[2] = p0[2] - p1[2];
prodve(a, b, c);
return 0.5 * std::sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]);
}
double triangle_area2d(double p0[2], double p1[2], double p2[2])
{
const double c =
(p2[0] - p1[0])*(p0[1] - p1[1]) -
(p2[1] - p1[1])*(p0[0] - p1[0]);
return 0.5 * std::sqrt(c*c);
}
void circumCenterXY(double *p1, double *p2, double *p3, double *res)
{
double d, a1, a2, a3;
const double x1 = p1[0];
const double x2 = p2[0];
const double x3 = p3[0];
const double y1 = p1[1];
const double y2 = p2[1];
const double y3 = p3[1];
d = 2. * (double)(y1 * (x2 - x3) + y2 * (x3 - x1) + y3 * (x1 - x2));
if(d == 0.0) {
// Msg::Warning("Colinear points in circum circle computation");
res[0] = res[1] = -99999.;
return ;
}
a1 = x1 * x1 + y1 * y1;
a2 = x2 * x2 + y2 * y2;
a3 = x3 * x3 + y3 * y3;
res[0] = (double)((a1 * (y3 - y2) + a2 * (y1 - y3) + a3 * (y2 - y1)) / d);
res[1] = (double)((a1 * (x2 - x3) + a2 * (x3 - x1) + a3 * (x1 - x2)) / d);
}
void circumCenterXYZ(double *p1, double *p2, double *p3, double *res, double *uv)
{ double v1[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
double v2[3] = {p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]};
double vx[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
double vy[3] = {p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]};
double vz[3]; prodve(vx, vy, vz); prodve(vz, vx, vy);
norme(vx); norme(vy); norme(vz);
double p1P[2] = {0.0, 0.0};
double p2P[2] = {prosca(v1, vx), prosca(v1, vy)};
double p3P[2] = { prosca(v2, vx), prosca(v2, vy)};
double resP[2];
circumCenterXY(p1P, p2P, p3P,resP);
if(uv){
double mat[2][2] = {{p2P[0] - p1P[0], p3P[0] - p1P[0]},
{p2P[1] - p1P[1], p3P[1] - p1P[1]}};
double rhs[2] = {resP[0] - p1P[0], resP[1] - p1P[1]};
sys2x2(mat, rhs, uv);
}
res[0] = p1[0] + resP[0] * vx[0] + resP[1] * vy[0];
res[1] = p1[1] + resP[0] * vx[1] + resP[1] * vy[1];
res[2] = p1[2] + resP[0] * vx[2] + resP[1] * vy[2];
}
void planarQuad_xyz2xy(double *x, double *y, double *z, double *xn, double *yn)
{
double v1[3] = {x[1] - x[0], y[1] - y[0], z[1] - z[0]};
double v2[3] = {x[2] - x[0], y[2] - y[0], z[2] - z[0]};
double v3[3] = {x[3] - x[0], y[3] - y[0], z[3] - z[0]};
double vx[3] = {x[1] - x[0], y[1] - y[0], z[1] - z[0]};
double vy[3] = {x[2] - x[0], y[2] - y[0], z[2] - z[0]};
double vz[3]; prodve(vx, vy, vz); prodve(vz, vx, vy);
norme(vx); norme(vy); norme(vz);
double p1P[2] = {0.0, 0.0};
double p2P[2] = {prosca(v1, vx), prosca(v1, vy)};
double p3P[2] = {prosca(v2, vx), prosca(v2, vy)};
double p4P[2] = {prosca(v3, vx), prosca(v3, vy)};
xn[0] = p1P[0];
xn[1] = p2P[0];
xn[2] = p3P[0];
xn[3] = p4P[0];
yn[0] = p1P[1];
yn[1] = p2P[1];
yn[2] = p3P[1];
yn[3] = p4P[1];
}
double computeInnerRadiusForQuad(double *x, double *y, int i)
{
// parameters of the equations of the 3 edges
double a1 = y[(4+i)%4]-y[(5+i)%4];
double a2 = y[(5+i)%4]-y[(6+i)%4];
double a3 = y[(6+i)%4]-y[(7+i)%4];
double b1 = x[(5+i)%4]-x[(4+i)%4];
double b2 = x[(6+i)%4]-x[(5+i)%4];
double b3 = x[(7+i)%4]-x[(6+i)%4];
double c1 = y[(5+i)%4]*x[(4+i)%4]-y[(4+i)%4]*x[(5+i)%4];
double c2 = y[(6+i)%4]*x[(5+i)%4]-y[(5+i)%4]*x[(6+i)%4];
double c3 = y[(7+i)%4]*x[(6+i)%4]-y[(6+i)%4]*x[(7+i)%4];
// length of the 3 edges double l1 = std::sqrt(a1*a1+b1*b1);
double l2 = std::sqrt(a2*a2+b2*b2);
double l3 = std::sqrt(a3*a3+b3*b3);
// parameters of the 2 bisectors
double a12 = a1/l1-a2/l2;
double a23 = a2/l2-a3/l3;
double b12 = b1/l1-b2/l2;
double b23 = b2/l2-b3/l3;
double c12 = c1/l1-c2/l2;
double c23 = c2/l2-c3/l3;
// compute the coordinates of the center of the incircle,
// that is the point where the 2 bisectors meet
double x_s = (c12*b23-c23*b12)/(a23*b12-a12*b23);
double y_s = 0.;
if (b12 != 0) {
y_s = -a12/b12*x_s-c12/b12;
}
else {
y_s = -a23/b23*x_s-c23/b23;
}
// finally get the radius of the circle
double r = (a1*x_s+b1*y_s+c1)/l1;
return r;
}
char float2char(float f)
{
// float normalized in [-1, 1], char in [-127, 127]
float c = f * 127.;
if(c > 127.) return 127;
else if(c < -127.) return -127;
else return (int)c;
}
float char2float(char c)
{
float f = c;
if(f > 127.) return 1.;
else if(f < -127) return -1.;
else return f / 127.;
}
void gradSimplex(double *x, double *y, double *z, double *v, double *grad)
{
// p = p1 * (1-u-v-w) + p2 u + p3 v + p4 w
double mat[3][3];
double det, b[3];
mat[0][0] = x[1] - x[0];
mat[1][0] = x[2] - x[0];
mat[2][0] = x[3] - x[0];
mat[0][1] = y[1] - y[0];
mat[1][1] = y[2] - y[0];
mat[2][1] = y[3] - y[0];
mat[0][2] = z[1] - z[0];
mat[1][2] = z[2] - z[0];
mat[2][2] = z[3] - z[0];
b[0] = v[1] - v[0];
b[1] = v[2] - v[0];
b[2] = v[3] - v[0];
sys3x3(mat, b, grad, &det);
}
double ComputeVonMises(double *V){
double tr = (V[0] + V[4] + V[8]) / 3.;
double v11 = V[0] - tr, v12 = V[1], v13 = V[2];
double v21 = V[3], v22 = V[4] - tr, v23 = V[5];
double v31 = V[6], v32 = V[7], v33 = V[8] - tr;
return sqrt(1.5 * (v11 * v11 + v12 * v12 + v13 * v13 +
v21 * v21 + v22 * v22 + v23 * v23 +
v31 * v31 + v32 * v32 + v33 * v33));
}
double ComputeScalarRep(int numComp, double *val)
{
if(numComp == 1)
return val[0];
else if(numComp == 3)
return sqrt(val[0] * val[0] + val[1] * val[1] + val[2] * val[2]);
else if(numComp == 9)
return ComputeVonMises(val);
return 0.;
}
void eigenvalue2x2(double mat[2][2], double v[2])
{
double a=1;
double b=-(mat[0][0]+mat[1][1]);
double c= (mat[0][0]*mat[1][1])-(mat[0][1]*mat[1][0]);
double det = b*b-4.*a*c;
v[0] = (-b+sqrt(det))/(2*a);
v[1] = (-b-sqrt(det))/(2*a);
}
void eigenvalue(double mat[3][3], double v[3])
{
// characteristic polynomial of T : find v root of
// v^3 - I1 v^2 + I2 T - I3 = 0
// I1 : first invariant , trace(T)
// I2 : second invariant , 1/2 (I1^2 -trace(T^2))
// I3 : third invariant , det T
double c[4];
c[3] = 1.0;
c[2] = - trace3x3(mat);
c[1] = 0.5 * (c[2] * c[2] - trace2(mat));
c[0] = - det3x3(mat);
// printf("%g %g %g\n", mat[0][0], mat[0][1], mat[0][2]);
// printf("%g %g %g\n", mat[1][0], mat[1][1], mat[1][2]);
// printf("%g %g %g\n", mat[2][0], mat[2][1], mat[2][2]);
// printf("%g x^3 + %g x^2 + %g x + %g = 0\n", c[3], c[2], c[1], c[0]);
double imag[3];
FindCubicRoots(c, v, imag);
eigsort(v);
}
void FindCubicRoots(const double coef[4], double real[3], double imag[3])
{
double a = coef[3];
double b = coef[2];
double c = coef[1];
double d = coef[0];
if(!a || !d){
// Msg::Error("Degenerate cubic: use a second degree solver!");
return;
}
b /= a; c /= a;
d /= a;
double q = (3.0*c - (b*b))/9.0;
double r = -(27.0*d) + b*(9.0*c - 2.0*(b*b));
r /= 54.0;
double discrim = q*q*q + r*r;
imag[0] = 0.0; // The first root is always real.
double term1 = (b/3.0);
if (discrim > 0) { // one root is real, two are complex
double s = r + sqrt(discrim);
s = ((s < 0) ? -pow(-s, (1.0/3.0)) : pow(s, (1.0/3.0)));
double t = r - sqrt(discrim);
t = ((t < 0) ? -pow(-t, (1.0/3.0)) : pow(t, (1.0/3.0)));
real[0] = -term1 + s + t;
term1 += (s + t)/2.0;
real[1] = real[2] = -term1;
term1 = sqrt(3.0)*(-t + s)/2;
imag[1] = term1;
imag[2] = -term1;
return;
}
// The remaining options are all real
imag[1] = imag[2] = 0.0;
double r13;
if (discrim == 0){ // All roots real, at least two are equal.
r13 = ((r < 0) ? -pow(-r,(1.0/3.0)) : pow(r,(1.0/3.0)));
real[0] = -term1 + 2.0*r13;
real[1] = real[2] = -(r13 + term1);
return;
}
// Only option left is that all roots are real and unequal (to get
// here, q < 0)
q = -q;
double dum1 = q*q*q;
dum1 = acos(r/sqrt(dum1));
r13 = 2.0*sqrt(q);
real[0] = -term1 + r13*cos(dum1/3.0);
real[1] = -term1 + r13*cos((dum1 + 2.0*M_PI)/3.0);
real[2] = -term1 + r13*cos((dum1 + 4.0*M_PI)/3.0);
}
void eigsort(double d[3])
{
int k, j, i;
double p;
for (i=0; i<3; i++) {
p=d[k=i];
for (j=i+1;j<3;j++)
if (d[j] >= p) p=d[k=j];
if (k != i) {
d[k]=d[i];
d[i]=p;
}
}
}
void invert_singular_matrix3x3(double MM[3][3], double II[3][3])
{
int i, j, k, n = 3;
double TT[3][3];
for(i = 1; i <= n; i++) {
for(j = 1; j <= n; j++) { II[i - 1][j - 1] = 0.0;
TT[i - 1][j - 1] = 0.0;
}
}
fullMatrix<double> M(3, 3), V(3, 3);
fullVector<double> W(3);
for(i = 1; i <= n; i++){
for(j = 1; j <= n; j++){
M(i - 1, j - 1) = MM[i - 1][j - 1];
}
}
M.svd(V, W);
for(i = 1; i <= n; i++) {
for(j = 1; j <= n; j++) {
double ww = W(i - 1);
if(fabs(ww) > 1.e-16) { // singular value precision
TT[i - 1][j - 1] += M(j - 1, i - 1) / ww;
}
}
}
for(i = 1; i <= n; i++) {
for(j = 1; j <= n; j++) {
for(k = 1; k <= n; k++) {
II[i - 1][j - 1] += V(i - 1, k - 1) * TT[k - 1][j - 1];
}
}
}
}
bool newton_fd(bool (*func)(fullVector<double> &, fullVector<double> &, void *),
fullVector<double> &x, void *data, double relax, double tolx)
{
const int MAXIT = 100;
const double EPS = 1.e-4;
const int N = x.size();
fullMatrix<double> J(N, N);
fullVector<double> f(N), feps(N), dx(N);
for (int iter = 0; iter < MAXIT; iter++){
if (x.norm() > 1.e6)return false;
if(!func(x, f, data)) {
return false;
}
bool isZero = false;
for (int k=0; k<N; k++) {
if (f(k) == 0. ) isZero = true;
else isZero = false;
if (isZero == false) break;
}
if (isZero) break;
for (int j = 0; j < N; j++){
double h = EPS * fabs(x(j));
if(h == 0.) h = EPS;
x(j) += h;
if(!func(x, feps, data)) {
return false;
}
for (int i = 0; i < N; i++){
J(i, j) = (feps(i) - f(i)) / h;
}
x(j) -= h;
}
if (N == 1)
dx(0) = f(0) / J(0, 0); else
if (!J.luSolve(f, dx)){
return false;
}
for (int i = 0; i < N; i++)
x(i) -= relax * dx(i);
if(dx.norm() < tolx) {
return true;
}
}
return false;
}
/*
distance to triangle
P(p) = p1 + t1 xi + t2 eta
t1 = (p2-p1) ; t2 = (p3-p1) ;
(P(p) - p) = d n
(p1 + t1 xi + t2 eta - p) = d n
t1 xi + t2 eta - d n = p - p1
| t1x t2x -nx | |xi | |px-p1x|
| t1y t2y -ny | |eta | = |py-p1y|
| t1z t2z -nz | |d | |pz-p1z|
distance to segment
P(p) = p1 + t (p2-p1)
(p - P(p)) * (p2-p1) = 0
(p - p1 - t (p2-p1) ) * (p2-p1) = 0
- t ||p2-p1||^2 + (p-p1)(p2-p1) = 0
t = (p-p1)*(p2-p1)/||p2-p1||^2
*/
void signedDistancePointTriangle(const SPoint3 &p1,const SPoint3 &p2, const SPoint3 &p3,
const SPoint3 &p, double &d, SPoint3 &closePt)
{
SVector3 t1 = p2 - p1;
SVector3 t2 = p3 - p1;
SVector3 t3 = p3 - p2;
SVector3 n = crossprod(t1, t2);
n.normalize();
const double n2t1 = dot(t1, t1);
const double n2t2 = dot(t2, t2);
const double n2t3 = dot(t3, t3);
double mat[3][3] = {{t1.x(), t2.x(), -n.x()},
{t1.y(), t2.y(), -n.y()},
{t1.z(), t2.z(), -n.z()}};
double inv[3][3];
double det = inv3x3(mat, inv);
if(det == 0.0) return;
double u, v;
SVector3 pp1 = p - p1;
u = (inv[0][0] * pp1.x() + inv[0][1] * pp1.y() + inv[0][2] * pp1.z());
v = (inv[1][0] * pp1.x() + inv[1][1] * pp1.y() + inv[1][2] * pp1.z());
d = (inv[2][0] * pp1.x() + inv[2][1] * pp1.y() + inv[2][2] * pp1.z());
double sign = (d > 0) ? 1. : -1.;
if (d == 0.) sign = 1.;
if (u >= 0. && v >= 0. && 1.-u-v >= 0.0){ //P(p) inside triangle
closePt = p1 + (p2-p1)*u + (p3-p1)*v; }
else {
const double t12 = dot(pp1, t1) / n2t1;
const double t13 = dot(pp1, t2) / n2t2;
SVector3 pp2 = p - p2;
const double t23 = dot(pp2, t3) / n2t3;
d = 1.e10;
if (t12 >= 0 && t12 <= 1.){
d = sign * std::min(fabs(d), p.distance(p1 + (p2 - p1) * t12));
closePt = p1 + (p2 - p1) * t12;
}
if (t13 >= 0 && t13 <= 1.){
if (p.distance(p1 + (p3 - p1) * t13) < fabs(d)) closePt = p1 + (p3 - p1) * t13;
d = sign * std::min(fabs(d), p.distance(p1 + (p3 - p1) * t13));
}
if (t23 >= 0 && t23 <= 1.){
if (p.distance(p2 + (p3 - p2) * t23) < fabs(d)) closePt = p2 + (p3 - p2) * t23;
d = sign * std::min(fabs(d), p.distance(p2 + (p3 - p2) * t23));
}
if (p.distance(p1) < fabs(d)){
closePt = p1;
d = sign * std::min(fabs(d), p.distance(p1));
}
if (p.distance(p2) < fabs(d)){
closePt = p2;
d = sign * std::min(fabs(d), p.distance(p2));
}
if (p.distance(p3) < fabs(d)){
closePt = p3;
d = sign * std::min(fabs(d), p.distance(p3));
}
}
}
void signedDistancesPointsTriangle(std::vector<double> &distances,
std::vector<SPoint3> &closePts,
const std::vector<SPoint3> &pts,
const SPoint3 &p1,
const SPoint3 &p2,
const SPoint3 &p3)
{
const unsigned pts_size = pts.size();
distances.clear();
distances.resize(pts_size);
closePts.clear();
closePts.resize(pts_size);
for (unsigned int i = 0; i < pts_size; ++i)
distances[i] = 1.e22;
SVector3 t1 = p2 - p1;
SVector3 t2 = p3 - p1;
SVector3 t3 = p3 - p2;
SVector3 n = crossprod(t1, t2);
n.normalize();
const double n2t1 = dot(t1, t1);
const double n2t2 = dot(t2, t2);
const double n2t3 = dot(t3, t3);
double mat[3][3] = {{t1.x(), t2.x(), -n.x()},
{t1.y(), t2.y(), -n.y()},
{t1.z(), t2.z(), -n.z()}};
double inv[3][3];
double det = inv3x3(mat, inv);
if(det == 0.0) return;
for (unsigned int i = 0; i < pts.size(); i++) {
double d;
SPoint3 closePt;
const SPoint3 &p = pts[i];
//signedDistancePointTrianglePrecomputed(p1, p2, p3, p, d, closePt);
double u, v;
SVector3 pp1 = p - p1;
u = (inv[0][0] * pp1.x() + inv[0][1] * pp1.y() + inv[0][2] * pp1.z());
v = (inv[1][0] * pp1.x() + inv[1][1] * pp1.y() + inv[1][2] * pp1.z());
d = (inv[2][0] * pp1.x() + inv[2][1] * pp1.y() + inv[2][2] * pp1.z());
double sign = (d > 0) ? 1. : -1.;
if (d == 0.) sign = 1.;
if (u >= 0 && v >= 0 && 1.-u-v >= 0.0){
closePt = SPoint3(0.,0.,0.);//TO DO
}
else {
const double t12 = dot(pp1, t1) / n2t1;
const double t13 = dot(pp1, t2) / n2t2;
SVector3 pp2 = p - p2;
const double t23 = dot(pp2, t3) / n2t3;
d = 1.e10;
SPoint3 closePt;
if (t12 >= 0 && t12 <= 1.){
d = sign * std::min(fabs(d), p.distance(p1 + (p2 - p1) * t12));
closePt = p1 + (p2 - p1) * t12;
}
if (t13 >= 0 && t13 <= 1.){
if (p.distance(p1 + (p3 - p1) * t13) < fabs(d)) closePt = p1 + (p3 - p1) * t13;
d = sign * std::min(fabs(d), p.distance(p1 + (p3 - p1) * t13));
}
if (t23 >= 0 && t23 <= 1.){
if (p.distance(p2 + (p3 - p2) * t23) < fabs(d)) closePt = p2 + (p3 - p2) * t23;
d = sign * std::min(fabs(d), p.distance(p2 + (p3 - p2) * t23));
}
if (p.distance(p1) < fabs(d)){
closePt = p1;
d = sign * std::min(fabs(d), p.distance(p1));
}
if (p.distance(p2) < fabs(d)){
closePt = p2;
d = sign * std::min(fabs(d), p.distance(p2));
}
if (p.distance(p3) < fabs(d)){
closePt = p3;
d = sign * std::min(fabs(d), p.distance(p3));
}
}
//end signedDistance
distances[i] = d;
closePts[i] = closePt;
}
}
void signedDistancePointLine(const SPoint3 &p1, const SPoint3 &p2, const SPoint3 &p,
double &d, SPoint3 &closePt)
{
SVector3 v12 = p2 - p1;
SVector3 v1p = p - p1;
const double alpha = dot(v1p, v12) / dot(v12, v12);
if (alpha <= 0.)
closePt = p1;
else if (alpha >= 1.)
closePt = p2;
else
closePt = p1 + (p2 - p1) * alpha;
d = p.distance(closePt);
}
void signedDistancesPointsLine(std::vector<double> &distances,
std::vector<SPoint3> &closePts,
const std::vector<SPoint3> &pts,
const SPoint3 &p1,
const SPoint3 &p2){
distances.clear();
distances.resize(pts.size());
closePts.clear();
closePts.resize(pts.size());
for (unsigned int i = 0; i < pts.size(); i++) {
double d;
SPoint3 closePt;
const SPoint3 &p = pts[i];
signedDistancePointLine(p1, p2, p, d, closePt);
distances[i] = d;
closePts[i] = closePt;
}
}
void changeReferential(const int direction,const SPoint3 &p,const SPoint3 &closePt,
const SPoint3 &p1, const SPoint3 &p2, double* xp, double* yp,
double* otherp, double* x, double* y, double* other)
{
if(direction == 1){
const SPoint3 &d1 = SPoint3(1.0, 0.0, 0.0);
const SPoint3 &d = SPoint3(p2.x() - p1.x(), p2.y() - p1.y(), p2.z() - p1.z());
double norm = sqrt(d.x() * d.x() + d.y() * d.y() + d.z() * d.z());
const SPoint3 &dn = SPoint3(d.x() / norm, d.y() / norm, d.z() / norm);
const SPoint3 &d3 = SPoint3(d1.y() * dn.z() - d1.z() * dn.y(),
d1.z() * dn.x() - d1.x() * dn.z(),
d1.x() * dn.y() - d1.y() * dn.x());
norm = sqrt(d3.x() * d3.x() + d3.y() * d3.y() + d3.z() * d3.z());
const SPoint3 &d3n = SPoint3(d3.x() / norm, d3.y() / norm, d3.z() / norm);
const SPoint3 &d2 = SPoint3(d3n.y() * d1.z() - d3n.z() * d1.y(),
d3n.z() * d1.x() - d3n.x() * d1.z(),
d3n.x() * d1.y() - d3n.y() * d1.x());
norm = sqrt(d2.x() * d2.x() + d2.y() * d2.y() + d2.z() * d2.z());
const SPoint3 &d2n = SPoint3(d2.x() / norm, d2.y() / norm, d2.z() / norm);
*xp = p.x() * d1.x() + p.y() * d1.y() + p.z() * d1.z();
*yp = p.x() * d3n.x() + p.y() * d3n.y() + p.z() * d3n.z();
*otherp = p.x() * d2n.x() + p.y() * d2n.y() + p.z() * d2n.z();
*x = closePt.x() * d1.x() + closePt.y() * d1.y() + closePt.z() * d1.z();
*y = closePt.x() * d3n.x() + closePt.y() * d3n.y() + closePt.z() * d3n.z();
*other = closePt.x() * d2n.x() + closePt.y() * d2n.y() + closePt.z() * d2n.z();
}
else{
const SPoint3 &d2 = SPoint3(0.0, 1.0, 0.0);
const SPoint3 &d = SPoint3(p2.x() - p1.x(), p2.y() - p1.y(), p2.z() - p1.z());
double norm = sqrt(d.x() * d.x() + d.y() * d.y() + d.z() * d.z());
const SPoint3 &dn = SPoint3(d.x() / norm, d.y() / norm, d.z() / norm);
const SPoint3 &d3 = SPoint3(dn.y() * d2.z() - dn.z() * d2.y(),
dn.z() * d2.x() - dn.x() * d2.z(),
dn.x() * d2.y() - dn.y() * d2.x());
norm = sqrt(d3.x() * d3.x() + d3.y() * d3.y() + d3.z() * d3.z());
const SPoint3 &d3n = SPoint3(d3.x() / norm, d3.y() / norm, d3.z() / norm);
const SPoint3 &d1 = SPoint3(d2.y() * d3n.z() - d2.z() * d3n.y(),
d2.z() * d3n.x() - d2.x() * d3n.z(),
d2.x() * d3n.y() - d2.y() * d3n.x());
norm = sqrt(d1.x() * d1.x() + d1.y() * d1.y() + d1.z() * d1.z());
const SPoint3 &d1n = SPoint3(d1.x() / norm, d1.y() / norm, d1.z() / norm);
*xp = p.x() * d2.x() + p.y() * d2.y() + p.z() * d2.z();
*yp = p.x() * d3n.x() + p.y() * d3n.y() + p.z() * d3n.z();
*otherp = p.x() * d1n.x() + p.y() * d1n.y() + p.z() * d1n.z();
*x = closePt.x() * d2.x() + closePt.y() * d2.y() + closePt.z() * d2.z();
*y = closePt.x() * d3n.x() + closePt.y() * d3n.y() + closePt.z() * d3n.z();
*other = closePt.x() * d1n.x() + closePt.y() * d1n.y() + closePt.z() * d1n.z();
}
}
int computeDistanceRatio(const double &y, const double &yp, const double &x,
const double &xp, double *distance, const double &r1,
const double &r2)
{
double b; double a;
if (y == yp){
b = -y;
a = 0.0;
}
else{
if (x == xp){
b = -x;
a = 0.0;
}
else{
b = (xp * y - x * yp) / (yp - y);
if (yp == 0.0){
a=-(b+x)/y;
}
else{
a = -(b + xp) / yp;
}
}
}
double ae;
double be;
double ce;
double da = r1 * r1;
double db = r2 * r2;
if (y == yp){
ae = 1.0 / da;
be = -(2 * x) / da;
ce = (x * x / da) - 1.0;
}
else{
if (x == xp){
ae = 1.0 / db;
be = -(2.0 * y) / db;
ce = (y * y / db) - 1.0;
}
else{
if (fabs(a) < 0.00001){
ae = 1.0 / db;
be = -(2.0 * y) / db;
ce = (y * y / db) - 1.0;
}
else{
double a2 = a * a;
ae = (1.0 / da) + (1.0 / (db * a2));
be = (2.0 * y)/(db * a) + (2.0 * b) / (a2 * db) - ((2.0 * x) / da);
ce = (x * x) / da + (b * b) / (db * a2) +
(2.0 * b * y) / (a * db) + (y * y / db) - 1.0;
}
}
}
double rho = be * be - 4 * ae * ce;
double x1, x2, y1, y2, propdist;
if (rho < 0) {
return 1;
}
else{
x1 = -(be + sqrt(rho)) / (2.0 * ae);
x2 = (-be + sqrt(rho)) / (2.0 * ae);
if (y == yp){
y1 = -b;
y2 = -b;
}
else{
if (x == xp){
y1 = x1;
y2 = x2;
x1 = -b;
x2 = -b;
} else{
if (fabs(a) < 0.00001){
y1 = x1;
y2 = x2;
x1 = -b;
x2 = -b;
}
else{
y1 = -(b + x1) / a;
y2 = -(b + x2) / a;
}
}
}
if (x1 == x2){
propdist = (y1 - y) / (yp - y);
if(propdist < 0.0){
propdist = (y2 - y) / (yp - y);
}
}
else{
if (xp != x){
propdist = (x1 - x) / (xp - x);
if (propdist < 0.0){
propdist = (x2 - x) / (xp - x);
}
}
else{
if (yp != y){
propdist = (y1 - y) / (yp - y);
if(propdist < 0.0){
propdist = (y2 - y) / (yp - y);
}
}
else{
propdist = 0.01;
}
}
}
*distance = propdist;
return 0;
}
}
void signedDistancesPointsEllipsePoint(std::vector<double> &distances,
std::vector<double> &distancesE,
std::vector<int> &isInYarn,
std::vector<SPoint3> &closePts,
const std::vector<SPoint3> &pts,
const SPoint3 &p1,
const SPoint3 &p2,
const double radius)
{
distances.clear();
distances.resize(pts.size());
distancesE.clear();
distancesE.resize(pts.size());
isInYarn.clear();
isInYarn.resize(pts.size());
closePts.clear();
closePts.resize(pts.size());
double d;
for (unsigned int i = 0; i < pts.size();i++){
SPoint3 closePt;
const SPoint3 &p = pts[i];
signedDistancePointLine(p1, p2, p, d, closePt);
closePts[i] = closePt;
distances[i] = d;
if (d <= radius){
isInYarn[i] = 1;
distancesE[i] = radius - d; }
else{
isInYarn[i] = 0;
distancesE[i] = d - radius;
}
}
}
void signedDistancesPointsEllipseLine(std::vector<double>&distances,
std::vector<double> &distancesE,
std::vector<int>&isInYarn,
std::vector<SPoint3>&closePts,
const std::vector<SPoint3> &pts,
const SPoint3 &p1,
const SPoint3 &p2,
const double maxA,
const double minA,
const double maxB,
const double minB,
const int typeLevelSet)
{
distances.clear();
distances.resize(pts.size());
distancesE.clear();
distancesE.resize(pts.size());
isInYarn.clear();
isInYarn.resize(pts.size());
closePts.clear();
closePts.resize(pts.size());
double d;
for (unsigned int i = 0; i < pts.size();i++){
SPoint3 closePt;
const SPoint3 &p = pts[i];
signedDistancePointLine(p1,p2,p,d,closePt);
distances[i] = d;
closePts[i] = closePt;
int direction=0;
if (!(p.x()==closePt.x() && p.y()==closePt.y() && p.z()==closePt.z())){
double xp,yp,x,y,otherp,other,propdist;
if (typeLevelSet==2){
if (p1.x()==p2.x()){
direction=1;
if (fabs(closePt.x()-0.0)<0.00000001) isInYarn[i]=1;
if (fabs(closePt.x()-2.2)<0.00000001) isInYarn[i]=1;
}
else{
if (p1.y()==p2.y()){
direction=2;
if (fabs(closePt.y()-0.0)<0.00000001) isInYarn[i]=6;
if (fabs(closePt.y()-2.2)<0.00000001) isInYarn[i]=6;
}
else{
printf("Error %lf %lf\n",closePt.x(),closePt.y());
}
}
}
if (typeLevelSet==1){
if (p1.x()==p2.x()){
direction=1;
//if (fabs(closePt.x() - 0.0) < 0.00000001) isInYarn[i] = 1;
if (fabs(closePt.x() - 2.2) < 0.00000001) isInYarn[i] = 4;
//if (fabs(closePt.x() - 4.4) < 0.00000001) isInYarn[i] = 2;
//if (fabs(closePt.x() - 6.6) < 0.00000001) isInYarn[i] = 5;
//if (fabs(closePt.x() - 8.8) < 0.00000001) isInYarn[i] = 3;
//if (fabs(closePt.x() - 11.0) < 0.00000001) isInYarn[i] = 1;
}
else{
if (p1.y() == p2.y()){
direction = 2; //if (fabs(closePt.y() - 0.0) < 0.00000001) isInYarn[i] = 6;
if (fabs(closePt.y() - 2.2) < 0.00000001) isInYarn[i] = 7;
//if (fabs(closePt.y() - 4.4) < 0.00000001) isInYarn[i] = 8;
//if (fabs(closePt.y() - 6.6) < 0.00000001) isInYarn[i] = 9;
//if (fabs(closePt.y() - 8.8) < 0.00000001) isInYarn[i] = 10;
//if (fabs(closePt.y() - 11.0) < 0.00000001) isInYarn[i] = 6;
}
else{
printf("Error %lf %lf\n", closePt.x(), closePt.y());
}
}
}
if (typeLevelSet==4){
if (p1.x()==p2.x()){
direction=1;
if (fabs(closePt.x()-0.0)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=1;
if (fabs(closePt.x()-2.2)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=4;
if (fabs(closePt.x()-4.4)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=2;
if (fabs(closePt.x()-6.6)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=5;
if (fabs(closePt.x()-8.8)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=3;
if (fabs(closePt.x()-11.0)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=1;
if (fabs(closePt.x()-0.0)<0.00000001 && closePt.z()>0.35) isInYarn[i]=11;
if (fabs(closePt.x()-2.2)<0.00000001 && closePt.z()>0.35) isInYarn[i]=14;
if (fabs(closePt.x()-4.4)<0.00000001 && closePt.z()>0.35) isInYarn[i]=12;
if (fabs(closePt.x()-6.6)<0.00000001 && closePt.z()>0.35) isInYarn[i]=15;
if (fabs(closePt.x()-8.8)<0.00000001 && closePt.z()>0.35) isInYarn[i]=13;
if (fabs(closePt.x()-11.0)<0.00000001 && closePt.z()>0.35) isInYarn[i]=11;
}
else{
if (p1.y()==p2.y()){
direction=2;
if (fabs(closePt.y()-0.0)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=6;
if (fabs(closePt.y()-2.2)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=7;
if (fabs(closePt.y()-4.4)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=8;
if (fabs(closePt.y()-6.6)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=9;
if (fabs(closePt.y()-8.8)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=10;
if (fabs(closePt.y()-11.0)<0.00000001 && closePt.z()<=0.35) isInYarn[i]=6;
if (fabs(closePt.y()-0.0)<0.00000001 && closePt.z()>0.35) isInYarn[i]=16;
if (fabs(closePt.y()-2.2)<0.00000001 && closePt.z()>0.35) isInYarn[i]=17;
if (fabs(closePt.y()-4.4)<0.00000001 && closePt.z()>0.35) isInYarn[i]=18;
if (fabs(closePt.y()-6.6)<0.00000001 && closePt.z()>0.35) isInYarn[i]=19;
if (fabs(closePt.y()-8.8)<0.00000001 && closePt.z()>0.35) isInYarn[i]=20;
if (fabs(closePt.y()-11.0)<0.00000001 && closePt.z()>0.35) isInYarn[i]=16;
}
else{
printf("Error %lf %lf\n",closePt.x(),closePt.y());
}
}
}
if (typeLevelSet==3){
direction=3;
isInYarn[i]=1;
}
if (typeLevelSet==5){
if(p1.x()==p2.x()){
direction=1;
if (fabs(closePt.x() - 0.0) < 0.00000001) isInYarn[i] = 1;
if (fabs(closePt.x() - 3.225) < 0.00000001) isInYarn[i] = 2;
if (fabs(closePt.x() - 6.45) < 0.00000001) isInYarn[i] = 3;
if (fabs(closePt.x() - 9.675) < 0.00000001) isInYarn[i] = 4;
if (fabs(closePt.x() - 12.9) < 0.00000001) isInYarn[i] = 1;
}
else{
if (p1.y()==p2.y()){
direction=2;
if (fabs(closePt.y() - 0.0) < 0.00000001) isInYarn[i] = 5;
if (fabs(closePt.y() - 1.665) < 0.00000001) isInYarn[i] = 6;
if (fabs(closePt.y() - 3.33) < 0.00000001) isInYarn[i] = 7;
if (fabs(closePt.y() - 4.995) < 0.00000001) isInYarn[i] = 8;
if (fabs(closePt.y() - 6.66) < 0.00000001) isInYarn[i] = 5; }
else{
printf("Error %lf %lf\n",closePt.x(),closePt.y());
}
}
}
changeReferential(direction, p, closePt, p1, p2, &xp, &yp,
&otherp, &x, &y, &other);
int result = 1;
if (fabs(other-otherp) > 0.01){
result = 1;
}
else{
if (direction==1){
result = computeDistanceRatio(y, yp, x, xp, &propdist, maxA, minA);
}
else{
if (direction==2){
result = computeDistanceRatio(y, yp, x, xp, &propdist, maxB, minB);
}
}
}
if (result == 1){
distancesE[i] = 1.e10;
isInYarn[i] = 0;
}
else{
if (propdist < 1.0){
isInYarn[i] = 0;
distancesE[i] = (1.0 / propdist) - 1.0;
}
else{
distancesE[i] = (1.0 - (1.0 / propdist));
}
}
}
else{
isInYarn[i] = 0;
distancesE[i] = 1000000.0;
}
}
}
int intersection_segments(const SPoint2 &p1, const SPoint2 &p2,
const SPoint2 &q1, const SPoint2 &q2,
double x[2])
{
double xp_max = std::max(p1.x(), p2.x());
double yp_max = std::max(p1.y(), p2.y());
double xq_max = std::max(q1.x(), q2.x());
double yq_max = std::max(q1.y(), q2.y());
double xp_min = std::min(p1.x(), p2.x());
double yp_min = std::min(p1.y(), p2.y());
double xq_min = std::min(q1.x(), q2.x());
double yq_min = std::min(q1.y(), q2.y());
if (yq_min > yp_max || xq_min > xp_max ||
yq_max < yp_min || xq_max < xp_min){
return 0;
}
else{
double A[2][2];
A[0][0] = p2.x() - p1.x();
A[0][1] = q1.x() - q2.x();
A[1][0] = p2.y() - p1.y();
A[1][1] = q1.y() - q2.y();
double b[2] = {q1.x() - p1.x(), q1.y() - p1.y()};
sys2x2(A, b, x);
return (x[0] >= 0.0 && x[0] <= 1. && x[1] >= 0.0 && x[1] <= 1.);
}
}
/// 3D VERSION
int intersection_segments(const SPoint3 &p1, const SPoint3 &p2,
const SPoint3 &q1, const SPoint3 &q2,
double x[2])
{
SVector3 v1(p2,p1),v2(q2,q1);
double n1 = v1.norm();
double n2 = v2.norm();
double EPS = 1.e-10*std::max(n1,n2);
double A[2][2];
A[0][0] = p2.x() - p1.x();
A[0][1] = q1.x() - q2.x();
A[1][0] = p2.y() - p1.y();
A[1][1] = q1.y() - q2.y();
double a[2] = {q1.x() - p1.x(), q1.y() - p1.y()};
double B[2][2];
B[0][0] = p2.z() - p1.z();
B[0][1] = q1.z() - q2.z();
B[1][0] = p2.y() - p1.y();
B[1][1] = q1.y() - q2.y();
double b[2] = {q1.z() - p1.z(), q1.y() - p1.y()};
double C[2][2];
C[0][0] = p2.z() - p1.z();
C[0][1] = q1.z() - q2.z();
C[1][0] = p2.x() - p1.x();
C[1][1] = q1.x() - q2.x();
double c[2] = {q1.z() - p1.z(), q1.x() - p1.x()};
double detA = fabs(det2x2(A));
double detB = fabs(det2x2(B));
double detC = fabs(det2x2(C));
// printf("%12.5E %12.5E %12.5E\n",detA,detB,detC);
if (detA > detB && detA > detC)
sys2x2(A, a, x);
else if (detB > detA && detB > detC)
sys2x2(B, b, x);
else
sys2x2(C, c, x);
if(x[0] >= 0.0 && x[0] <= 1. &&
x[1] >= 0.0 && x[1] <= 1.) {
SPoint3 x1 (p1.x()*(1.-x[0]) + p2.x()*x[0],
p1.y()*(1.-x[0]) + p2.y()*x[0],
p1.z()*(1.-x[0]) + p2.z()*x[0]);
SPoint3 x2 (q1.x()*(1.-x[0]) + q2.x()*x[0],
q1.y()*(1.-x[0]) + q2.y()*x[0],
q1.z()*(1.-x[0]) + q2.z()*x[0]);
SVector3 d (x2,x1);
double nd = norm(d);
if (nd > EPS){
x[0] = x[1] = 1.e22;
return false;
}
return true;
}
return false;
}
void fillMeanPlane(double res[4], double t1[3], double t2[3], mean_plane &meanPlane)
{
for(int i = 0; i < 3; i++)
meanPlane.plan[0][i] = t1[i];
for(int i = 0; i < 3; i++)
meanPlane.plan[1][i] = t2[i];
for(int i = 0; i < 3; i++)
meanPlane.plan[2][i] = res[i];
meanPlane.a = res[0];
meanPlane.b = res[1];
meanPlane.c = res[2];
meanPlane.d = res[3];
meanPlane.x = meanPlane.y = meanPlane.z = 0.;
if(fabs(meanPlane.a) >= fabs(meanPlane.b) &&
fabs(meanPlane.a) >= fabs(meanPlane.c) ){
meanPlane.x = meanPlane.d / meanPlane.a;
}
else if(fabs(meanPlane.b) >= fabs(meanPlane.a) &&
fabs(meanPlane.b) >= fabs(meanPlane.c)){
meanPlane.y = meanPlane.d / meanPlane.b;
}
else{
meanPlane.z = meanPlane.d / meanPlane.c;
}
}
void computeMeanPlaneSimple(const std::vector<SPoint3> &points, mean_plane &meanPlane)
{
double xm = 0., ym = 0., zm = 0.;
int ndata = points.size();
int na = 3;
for(int i = 0; i < ndata; i++) {
xm += points[i].x();
ym += points[i].y();
zm += points[i].z();
}
xm /= (double)ndata;
ym /= (double)ndata;
zm /= (double)ndata;
fullMatrix<double> U(ndata, na), V(na, na);
fullVector<double> sigma(na);
for(int i = 0; i < ndata; i++) {
U(i, 0) = points[i].x() - xm;
U(i, 1) = points[i].y() - ym;
U(i, 2) = points[i].z() - zm;
}
U.svd(V, sigma);
double res[4], svd[3];
svd[0] = sigma(0);
svd[1] = sigma(1);
svd[2] = sigma(2);
int min;
if(fabs(svd[0]) < fabs(svd[1]) && fabs(svd[0]) < fabs(svd[2]))
min = 0;
else if(fabs(svd[1]) < fabs(svd[0]) && fabs(svd[1]) < fabs(svd[2]))
min = 1;
else
min = 2;
res[0] = V(0, min);
res[1] = V(1, min);
res[2] = V(2, min);
norme(res);
double ex[3], t1[3], t2[3];
ex[0] = ex[1] = ex[2] = 0.0;
if(res[0] == 0.)
ex[0] = 1.0;
else if(res[1] == 0.)
ex[1] = 1.0;
else
ex[2] = 1.0;
prodve(res, ex, t1);
norme(t1);
prodve(t1, res, t2); norme(t2);
res[3] = (xm * res[0] + ym * res[1] + zm * res[2]);
fillMeanPlane(res, t1, t2, meanPlane);
}
void projectPointToPlane(const SPoint3 &pt, SPoint3 &ptProj, const mean_plane &meanPlane)
{
double u = pt.x();
double v = pt.y();
double w = pt.z();
double a = meanPlane.a;
double b = meanPlane.b;
double c = meanPlane.c;
double d = meanPlane.d;
double t0 = -(a*u+b*v+c*w+d)/(a*a+b*b+c*c);
ptProj[0] = u + a*t0;
ptProj[1] = v + b*t0;
ptProj[2] = w + c*t0;
}
void projectPointsToPlane(const std::vector<SPoint3> &pts, std::vector<SPoint3> &ptsProj,
const mean_plane &meanPlane)
{
ptsProj.resize(pts.size());
for (unsigned int i= 0; i< pts.size(); i++){
projectPointToPlane(pts[i],ptsProj[i], meanPlane);
}
}
void transformPointsIntoOrthoBasis(const std::vector<SPoint3> &ptsProj,
std::vector<SPoint3> &pointsUV,
const SPoint3 &ptCG, const mean_plane &meanPlane)
{
pointsUV.resize(ptsProj.size());
SVector3 normal(meanPlane.a, meanPlane.b, meanPlane.c);
SVector3 tangent, binormal;
buildOrthoBasis(normal, tangent, binormal);
for (unsigned int i= 0; i< ptsProj.size(); i++){
SVector3 pp(ptsProj[i][0]-ptCG[0],ptsProj[i][1]-ptCG[1],ptsProj[i][2]-ptCG[2]) ;
pointsUV[i][0] = dot(pp, tangent);
pointsUV[i][1] = dot(pp, binormal);
pointsUV[i][2] = dot(pp, normal);
}
}
static bool catenary_fct(fullVector<double> &x, fullVector<double> &res, void *data)
{
double *param = (double*)data;
double x0 = param[0], x1 = param[1], y0 = param[2], y1 = param[3], ys = param[4];
res(0) = (ys-1/x(0)) + 1/x(0)*cosh(x(0)*(x0-x(1))) - y0;
res(1) = (ys-1/x(0)) + 1/x(0)*cosh(x(0)*(x1-x(1))) - y1;
return true;
}
bool catenary(double x0, double x1, double y0, double y1, double ys, int N,
double *yp)
{
// In the z=0 plane, catenary equation is y(x) = a + 1/b cosh(b(x-c))
//
// Three parameters a, b, c determined by imposing
// - left point: y0 = y(x0) = a + 1/b cosh(b(x0-c))
// - right point: y1 = y(x1) = a + 1/b cosh(b(x1-c))
// - lowest point (sag): ys = y(c) = a + 1/b , i.e. a = ys - 1/b
//
// Thus solve syst of 2 nl equations with 2 unknowns b and c:
// // ys - 1/b + 1/b cosh(b(x0-c)) - y0 = 0
// ys - 1/b + 1/b cosh(b(x1-c)) - y1 = 0
double param[5] = {x0, x1, y0, y1, ys};
fullVector<double> x(2);
bool success = false;
if(x0 != x1){
x(0) = 1./(x1-x0);
x(1) = (x0+x1)/2.;
success = newton_fd(catenary_fct, x, param, 1., 1e-6*fabs(x1-x0));
}
if(success){
double a = ys-1/x(0);
for(int i = 0; i < N; i++){
double r = x0 + (i+1) * (x1-x0)/(N+1);
yp[i] = a+1/x(0)*cosh(x(0)*(r-x(1)));
}
return true;
}
else{
for(int i = 0; i < N; i++){
yp[i] = y0 + (i+1) * (y1-y0)/(N+1);
}
return false;
}
}