Skip to content

GitLab

Commit da39bbd0 authored by Phillip Lino Rall's avatar Phillip Lino Rall
Browse files

trilinear interpolator now workinggit reset program2D/program.pro.user!

parent eadd7402
Hide whitespace changes
Inline Side-by-side
program/source/extemp/variable.h
View file @ da39bbd0
......@@ -374,7 +374,7 @@ class Variable : public Expr<Variable< DTyp > >, public Object_based_on_ug {
Blockgrid* blockgrid;
Unstructured_grid* ug;
// own data
// own data
DTyp** data_hexahedra; // num_hexahedra
DTyp** data_quadrangles; // num_quadrangles
DTyp** data_edges; // num_edges
......
program/source/interpol/interpol.cc
View file @ da39bbd0
......@@ -71,13 +71,17 @@ bool contained_in_tet_direct(D3vector lam) {
}
bool new_lam_better(D3vector lam_old, D3vector lam_new) {
if (MIN(lam_new ) < -0.2 || MAX(lam_new) > 1.2) return false;
if (MIN(lam_old ) < -0.2 || MAX(lam_old) > 1.2) return true;
if( (MIN(lam_new ) < 0 || MAX(lam_new) >1 )&& (MIN(lam_old ) >= 0 || MAX(lam_old) <=1 )) return false;
if (MIN(lam_new) > MIN(lam_old)) return true;
if (MAX(lam_new) < MAX(lam_old)) return true;
double min_new = MIN(lam_new);
double max_new = MAX(lam_new);
if (min_new < -0.2 || max_new > 1.2) return false;
double min_old = MIN(lam_old);
double max_old = MAX(lam_old);
if (min_old < -0.2 || max_old > 1.2) return true;
if((min_new < -1e-10 || max_new >1.0+1e-10 ) && (min_old >= -1e-10 && max_old <=1.0+1e-10 )) return false;
if (min_new > min_old && min_old<-1e-10) return true;
if (max_new < max_old && max_old>1.0+1e-10) return true;
if (SUM(lam_old) > SUM(lam_new) && min_new > -1e-10) return true;
return false;
}
......@@ -196,149 +200,8 @@ void Interpolate_on_structured_grid::update_Interpolate_on_structured_grid(Block
//if (k == 0 || k == (Nz-1) || j == 0 || j == (Ny-1) || i == 0 || i == (Nx-1))
{
// corner points of general hex-cell
cWSD = blockgrid->Give_coord_hexahedron(id_hex,i, j, k );
cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j ,k );
cWND = blockgrid->Give_coord_hexahedron(id_hex,i, j+1,k );
cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k );
cWST = blockgrid->Give_coord_hexahedron(id_hex,i, j, k+1);
cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j ,k+1);
cWNT = blockgrid->Give_coord_hexahedron(id_hex,i, j+1,k+1);
cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);
// bounding box calculation
boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x))) - factor *hx;
boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y))) - factor *hy;
boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z))) - factor *hz;
boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x))) + factor *hx;
boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y))) + factor *hy;
boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z))) + factor *hz;
// calculation of indices of a collection of cells of structured grid which contains bounding box
ilmin = Ganzzahliger_Anteil((boxWSD.x - pWSD.x) / hx);
jlmin = Ganzzahliger_Anteil((boxWSD.y - pWSD.y) / hy);
klmin = Ganzzahliger_Anteil((boxWSD.z - pWSD.z) / hz);
ilmax = Ganzzahliger_Anteil((boxENT.x - pWSD.x) / hx);
jlmax = Ganzzahliger_Anteil((boxENT.y - pWSD.y) / hy);
klmax = Ganzzahliger_Anteil((boxENT.z - pWSD.z) / hz);
/*
cout << " indices: "
<< " ilmin: " << ilmin
<< " jlmin: " << jlmin
<< " klmin: " << klmin
<< " ilmax: " << ilmax
<< " jlmax: " << jlmax
<< " klmax: " << klmax
<< " boxWSD.x: " << boxWSD.x
<< " cWSD.x: " << cWSD.x
<< " Nx: " << Nx
<< endl;
*/
/*
bool now;
if(boxWSD.z < 0 && boxENT.z > 0.0 && boxWSD.y < 0.5 && boxENT.y > 0.5 && boxWSD.x < 1.0 && boxENT.x > 1.0 ) {
cout << "\n \n WSD : "; boxWSD.Print();
cout << "\n ENT : "; boxENT.Print();
now = true;
}
else now = false;
cout << " tt: " << boxWSD.x << " " << pWSD.x << " " << hx << endl;
cout << (boxWSD.x - pWSD.x) << endl;
cout << " z: " << 0.1 << " g: " << Ganzzahliger_Anteil(0.1) << endl;
cout << " z: " << -0.1 << " g: " << Ganzzahliger_Anteil(-0.1) << endl;
cout << " z: " << 5.1 << " g: " << Ganzzahliger_Anteil(5.1) << endl;
cout << " z: " << -5.1 << " g: " << Ganzzahliger_Anteil(-5.1) << endl;
*/
if(ilmin<0) ilmin=0;
if(jlmin<0) jlmin=0;
if(klmin<0) klmin=0;
for(int il = ilmin; (il <= ilmax) && (il < nx);++il)
for(int jl = jlmin; (jl <= jlmax) && (jl < ny);++jl)
for(int kl = klmin; (kl <= klmax) && (kl < nz);++kl) {
ploc = D3vector(il * hx, jl * hy, kl * hz) + pWSD;
// cout << "HI" << endl;
typ = -1;
lam = lambda_of_p_in_tet(ploc,cWND,cWNT,cWST,cEST);
if(contained_in_tet(lam)) typ=0;
else {
lam = lambda_of_p_in_tet(ploc,cEST,cWND,cWST,cESD);
if(contained_in_tet(lam)) typ=1;
else {
lam = lambda_of_p_in_tet(ploc,cWND,cWSD,cWST,cESD);
if(contained_in_tet(lam)) typ=2;
else {
lam = lambda_of_p_in_tet(ploc,cEST,cWND,cESD,cEND);
if(contained_in_tet(lam)) typ=3;
else {
lam = lambda_of_p_in_tet(ploc,cENT,cWNT,cEST,cEND);
if(contained_in_tet(lam)) typ=4;
else {
lam = lambda_of_p_in_tet(ploc,cWNT,cWND,cEST,cEND);
if(contained_in_tet(lam)) typ=5;
}
}
}
}
}
if (trilinearInterpolationFlag && MIN(lam)>= 0.0 && MAX(lam) <= 1.0 )
{
lam = trilinarInterpolation(ploc, id_hex,i, j, k);
typ = 6;
}
/*
cout << " typ " << typ << id_hex
<< " il: " << il
<< " jl: " << jl
<< " kl: " << kl
<< endl;
*/
if(typ!=-1) {
int ind_global;
ind_global = il+nx*(jl+ny*kl);
bool stop;
stop=false;
if(ids_hex[ind_global]!=-1) {
stop=new_lam_worse(lambda[ind_global],lam);
}
if(stop==false) {
ids_hex[ind_global] = id_hex;
ids_i[ind_global] = i;
ids_j[ind_global] = j;
ids_k[ind_global] = k;
typ_tet[ind_global] = typ;
lambda[ind_global] = lam;
}
//go_on = false;
}
findLambdaForInterpolation(id_hex,i,j,k);
}
}
}
......@@ -359,6 +222,154 @@ void Interpolate_on_structured_grid::update_Interpolate_on_structured_grid(Block
}
}
void Interpolate_on_structured_grid::findLambdaForInterpolation(int id_hex, int i, int j, int k)
{
double factor = 0.1;
// corner points of general hex-cell
D3vector cWSD = blockgrid->Give_coord_hexahedron(id_hex,i, j, k );
D3vector cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j ,k );
D3vector cWND = blockgrid->Give_coord_hexahedron(id_hex,i, j+1,k );
D3vector cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k );
D3vector cWST = blockgrid->Give_coord_hexahedron(id_hex,i, j, k+1);
D3vector cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j ,k+1);
D3vector cWNT = blockgrid->Give_coord_hexahedron(id_hex,i, j+1,k+1);
D3vector cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);
// bounding box calculation
D3vector boxWSD, boxENT;
boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x))) - factor *hx;
boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y))) - factor *hy;
boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z))) - factor *hz;
boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x))) + factor *hx;
boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y))) + factor *hy;
boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z))) + factor *hz;
// boxWSD = D3vector(-5.926666666666667,-2.1166666666666685,12.5);
// boxENT = boxWSD + D3vector(3);
// calculation of indices of a collection of cells of structured grid which contains bounding box
int ilmin = Ganzzahliger_Anteil((boxWSD.x - pWSD.x) / hx);
int jlmin = Ganzzahliger_Anteil((boxWSD.y - pWSD.y) / hy);
int klmin = Ganzzahliger_Anteil((boxWSD.z - pWSD.z) / hz);
int ilmax = Ganzzahliger_Anteil((boxENT.x - pWSD.x) / hx);
int jlmax = Ganzzahliger_Anteil((boxENT.y - pWSD.y) / hy);
int klmax = Ganzzahliger_Anteil((boxENT.z - pWSD.z) / hz);
// std::cout << "ilmin " <<ilmin << "\n";
// std::cout << "jlmin " <<jlmin << "\n";
// std::cout << "klmin " <<klmin << "\n";
// std::cout << "ilmax " <<ilmax << "\n";
// std::cout << "jlmax " <<jlmax << "\n";
// std::cout << "klmax " <<klmax << "\n";
int typ;
D3vector lam;
if(ilmin<0) ilmin=0;
if(jlmin<0) jlmin=0;
if(klmin<0) klmin=0;
for(int il = ilmin; (il <= ilmax) && (il < nx);++il)
for(int jl = jlmin; (jl <= jlmax) && (jl < ny);++jl)
for(int kl = klmin; (kl <= klmax) && (kl < nz);++kl) {
D3vector ploc = D3vector(il * hx, jl * hy, kl * hz) + pWSD;
typ = -1;
int ind_global;
ind_global = il+nx*(jl+ny*kl);
D3vector lamTemp = D3vector(-1);
//lamTemp = lambda[ind_global];
lam = lambda_of_p_in_tet(ploc,cWND,cWNT,cWST,cEST);
if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=0;lamTemp = lam;}
lam = lambda_of_p_in_tet(ploc,cEST,cWND,cWST,cESD);
if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=1;lamTemp = lam;}
lam = lambda_of_p_in_tet(ploc,cWND,cWSD,cWST,cESD);
if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=2;lamTemp = lam;}
lam = lambda_of_p_in_tet(ploc,cEST,cWND,cESD,cEND);
if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=3;lamTemp = lam;}
lam = lambda_of_p_in_tet(ploc,cENT,cWNT,cEST,cEND);
if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=4;lamTemp = lam;}
lam = lambda_of_p_in_tet(ploc,cWNT,cWND,cEST,cEND);
if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=5;lamTemp = lam;}
lam = lamTemp;
if (trilinearInterpolationFlag)
{
typ = -1;
}
//if (trilinearInterpolationFlag && MIN(lam)>= -0.1 && MAX(lam) <= 1.1 && SUM(lam) <= 1.2 && SUM(lam)>=-0.2)
if (trilinearInterpolationFlag && MIN(lam)>= -0.1 && MAX(lam) <= 1.1 && SUM(lam) <= 1.1 && SUM(lam)>=-0.1)
{
lam = trilinarInterpolation(ploc, id_hex,i, j, k);
typ = 6;
}
if(typ!=-1) {
int ind_global;
ind_global = il+nx*(jl+ny*kl);
bool stop;
stop=false;
if(ids_hex[ind_global]!=-1) {
stop=!new_lam_better(lambda[ind_global],lam);
if (trilinearInterpolationFlag)
{
if (MIN(lam) >= -1e-10 && MAX(lam)<1.0+1e-10)// || ( MAX(lambda[ind_global])>1.0 || MIN(lambda[ind_global])<0.0 ))
{
stop = false;
}
if ( MAX(lambda[ind_global])>1.0+1e-10 || MIN(lambda[ind_global])<-1e-10 )
{
stop = false;
}
}
}
if(stop==false)// && ((typ_tet[ind_global] != 6) || (typ_tet[ind_global] == 6 & (MIN(lambda[ind_global])<-1e-10)|| MAX(lambda[ind_global])>(1+1e-10))))
{
ids_hex[ind_global] = id_hex;
ids_i[ind_global] = i;
ids_j[ind_global] = j;
ids_k[ind_global] = k;
typ_tet[ind_global] = typ;
lambda[ind_global] = lam;
}
}
}
}
D3vector Interpolate_on_structured_grid::trilinarInterpolation(D3vector X, int id_Hex, int i, int j, int k)
{
D3vector cWSD = blockgrid->Give_coord_hexahedron(id_Hex,i, j, k );
......@@ -371,207 +382,319 @@ D3vector Interpolate_on_structured_grid::trilinarInterpolation(D3vector X, int i
D3vector cWNT = blockgrid->Give_coord_hexahedron(id_Hex,i, j+1,k+1);
D3vector cENT = blockgrid->Give_coord_hexahedron(id_Hex,i+1,j+1,k+1);
D3vector B = cESD - cEND; // 100 - 000
D3vector C = cWND - cEND; // 010 - 000
D3vector D = cENT - cEND; // 001 - 000
//std::cout << "to be implemented" << std::endl;
// D3vector B = cESD - cEND; // 100 - 000
// D3vector C = cWND - cEND; // 010 - 000
// D3vector D = cENT - cEND; // 001 - 000
// //std::cout << "to be implemented" << std::endl;
//// X = (cWSD+ cESD+ cWND + cEND + cWST + cEST + cWNT + cENT ) / 8.0 ;
//// X.x = X.x +15;
//// X.y = X.y +15;
// D3vector normalTD = cross_product(-1*B,C);
// D3vector normalTDOPP = -1*cross_product(cWST-cEST , cWNT - cWST);
// //D3vector normalTDOPP2 = cross_product(cENT-cEST ,cWNT - cENT);
// D3vector normalNS = cross_product(-1*C,D);
// D3vector normalNSOPP = -1*cross_product(cWST-cEST,cWST - cWSD);
// //D3vector normalNSOPP2 = cross_product(cESD-cEST,cESD - cWSD);
// D3vector normalEW = cross_product(-1*D,B);
// D3vector normalEWOPP = -1*cross_product(cWST-cWSD,cWST - cWNT);
// //D3vector normalEWOPP2 = cross_product(cWND-cWSD,cWND - cWNT);
// normalTD = normalTD / D3VectorNorm(normalTD);
// normalTDOPP = normalTDOPP / D3VectorNorm(normalTDOPP);
// normalNS = normalNS / D3VectorNorm(normalNS);
// normalNSOPP = normalNSOPP / D3VectorNorm(normalNSOPP);
// normalEW = normalEW / D3VectorNorm(normalEW);
// normalEWOPP = normalEWOPP / D3VectorNorm(normalEWOPP);
// double eta = 0.5;
// double xi = 0.5;
// double phi = 0.5;
// bool fixEta = false;
// bool fixXi = false;
// bool fixPhi = false;
// D3vector coord(eta,xi,phi);
// double distTD0 = product(normalTD,cEND);
// double TEST1 = product(normalTD,cWND);
// double TEST2 = product(normalTD,cESD);
// double TEST3 = product(normalTD,cWSD);
// double distTD1 = product(normalTDOPP,cWST);
// double distTDMid = distTD0-product(normalTD,X);
// double distTDMid2 = distTD1-product(normalTDOPP,X);
// double distNS0 = product(normalNS,cEND);
// double distNS1 = product(normalNSOPP,cWST);
// double distNSMid = distNS0-product(normalNS,X);
// double distNSMid2 = distNS1 -product(normalNSOPP,X);
// double distEW0 = product(normalEW,cEND);
// double distEW1 = product(normalEWOPP,cWST);
// double distEWMid = distEW0-product(normalEW,X);
// double distEWMid2 = distEW1 -product(normalEWOPP,X);
// D3vector A = cEND; // 000
// D3vector E = cWSD - cESD - cWND + cEND; // 110 - 100 - 010 + 000
// D3vector F = cWNT - cWND - cENT + cEND; // 011 - 010 - 001 + 000
// D3vector G = cEST - cESD - cENT + cEND; // 101 - 100 - 001 + 000
// D3vector H = cWST + cESD + cWND + cENT - cEND - cWSD - cWNT - cEST; // 111 + 100 + 010 + 001 - 000 - 110 - 011 - 101
// if (fabs(angle_between_vectors(normalTD,normalTDOPP)-180*0) < 0.5)
// {
// double delta = (distTD1 - distTD0);
// //phi = (distTDMid + delta - distTDMid2) /delta/ 2.0;
// //std::cout << "phi 1 "<< phi << std::endl;
// phi = -distTDMid / delta;
// //std::cout << "phi 2 "<< phi << std::endl;
// //phi = distTDMid2 / delta;
// //std::cout << "phi 3 "<< phi << std::endl;
// coord.z = phi;
// fixPhi = true;
// }
// if (fabs(angle_between_vectors(normalNS,normalNSOPP)-180*0) < 0.5)
// {
// double delta = (distNS1 - distNS0);
// //eta = (distNSMid + delta - distNSMid2) /delta / 2.0;
// //std::cout << "eta 1 "<< eta << std::endl;
// eta = -distNSMid / delta;
// //std::cout << "eta 2 "<< eta << std::endl;
// //eta = distNSMid2 / delta;
// //std::cout << "eta 3 "<< eta << std::endl;
// coord.x = eta;
// fixEta = true;
// }
// if (fabs(angle_between_vectors(normalEW,normalEWOPP)-180*0) < 0.5)
// {
// double delta = (distEW1 - distEW0);
// xi = -distEWMid / delta;
// coord.y = xi;
// fixXi = true;
// }
//// std::cout << "initial guess\n";
//// coord.Print();std::cout<<std::endl;
// D3vector R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
// //
// X = (cWSD+ cESD+ cWND + cEND + cWST + cEST + cWNT + cENT ) / 8.0 ;
// X.x = X.x +15;
// X.y = X.y +15;
D3vector normalTD = cross_product(-1*B,C);
D3vector normalTDOPP = -1*cross_product(cWST-cEST , cWNT - cWST);
//D3vector normalTDOPP2 = cross_product(cENT-cEST ,cWNT - cENT);
D3vector normalNS = cross_product(-1*C,D);
D3vector normalNSOPP = -1*cross_product(cWST-cEST,cWST - cWSD);
//D3vector normalNSOPP2 = cross_product(cESD-cEST,cESD - cWSD);
D3vector normalEW = cross_product(-1*D,B);
D3vector normalEWOPP = -1*cross_product(cWST-cWSD,cWST - cWNT);
//D3vector normalEWOPP2 = cross_product(cWND-cWSD,cWND - cWNT);
normalTD = normalTD / D3VectorNorm(normalTD);
normalTDOPP = normalTDOPP / D3VectorNorm(normalTDOPP);
normalNS = normalNS / D3VectorNorm(normalNS);
normalNSOPP = normalNSOPP / D3VectorNorm(normalNSOPP);
normalEW = normalEW / D3VectorNorm(normalEW);
normalEWOPP = normalEWOPP / D3VectorNorm(normalEWOPP);
double eta = 0.5;
double xi = 0.5;
double phi = 0.5;
bool fixEta = false;
bool fixXi = false;
bool fixPhi = false;
D3vector coord(eta,xi,phi);
//D3vector x;
//D3vector coordPrev;
// for (int iter = 0 ; iter < 50 ; iter++)
// {
// coordPrev = coord;
// D3vector partialEta = B + E * coord.y + G * coord.z + H * coord.y*coord.z;
// D3vector partialXi = C + E * coord.x + F * coord.z + H * coord.x*coord.z;
// D3vector partialPhi = D + F * coord.y + G * coord.x + H * coord.x*coord.y;
double distTD0 = product(normalTD,cEND);
double TEST1 = product(normalTD,cWND);
double TEST2 = product(normalTD,cESD);
double TEST3 = product(normalTD,cWSD);
double distTD1 = product(normalTDOPP,cWST);
double distTDMid = distTD0-product(normalTD,X);
double distTDMid2 = distTD1-product(normalTDOPP,X);
double distNS0 = product(normalNS,cEND);
double distNS1 = product(normalNSOPP,cWST);
double distNSMid = distNS0-product(normalNS,X);
double distNSMid2 = distNS1 -product(normalNSOPP,X);
double distEW0 = product(normalEW,cEND);
double distEW1 = product(normalEWOPP,cWST);
double distEWMid = distEW0-product(normalEW,X);
double distEWMid2 = distEW1 -product(normalEWOPP,X);
// D3matrix jac2(partialEta,partialXi,partialPhi);
// //jac2.transpose();
// //jac2.invert_gauss_elimination();
// R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z -X;
// std::cout << "in range " << isInRange(distTDMid,distTDMid2) << std::endl;
// std::cout << "in range " << isInRange(distNSMid,distNSMid2) << std::endl;
// std::cout << "in range " << isInRange(distEWMid,distEWMid2) << std::endl;
// if (!isInRange(distTDMid,distTDMid2) || !isInRange(distNSMid,distNSMid2) || !isInRange(distEWMid,distEWMid2) )
// {
// return D3vector{-1,-1,-1};
// }
// std::cout << "in range " << isInRange(distTDMid,distTDMid2) << std::endl;
// std::cout << "in range " << isInRange(distNSMid,distNSMid2) << std::endl;
// std::cout << "in range " << isInRange(distEWMid,distEWMid2) << std::endl;
// //std::cout << "residuum f_xn" <<D3VectorNorm(R)<<std::endl;
//// if (D3VectorNormSquared(R) < 1e-10)
//// {
//// iter = 1000;
//// }
// // coord.Print(); std::cout << std::flush;
// coord = coord - jac2.invert_apply(R) * 1.0;
D3vector A = cEND; // 000
D3vector E = cWSD - cESD - cWND + cEND; // 110 - 100 - 010 + 000
D3vector F = cWNT - cWND - cENT + cEND; // 011 - 010 - 001 + 000
D3vector G = cEST - cESD - cENT + cEND; // 101 - 100 - 001 + 000
D3vector H = cWST + cESD + cWND + cENT - cEND - cWSD - cWNT - cEST; // 111 + 100 + 010 + 001 - 000 - 110 - 011 - 101
if (fabs(angle_between_vectors(normalTD,normalTDOPP)-180*0) < 0.5)
{
double delta = (distTD1 - distTD0);
//phi = (distTDMid + delta - distTDMid2) /delta/ 2.0;
//std::cout << "phi 1 "<< phi << std::endl;
phi = -distTDMid / delta;
//std::cout << "phi 2 "<< phi << std::endl;
//phi = distTDMid2 / delta;
//std::cout << "phi 3 "<< phi << std::endl;
coord.z = phi;
fixPhi = true;
}
if (fabs(angle_between_vectors(normalNS,normalNSOPP)-180*0) < 0.5)
{
double delta = (distNS1 - distNS0);
//eta = (distNSMid + delta - distNSMid2) /delta / 2.0;
//std::cout << "eta 1 "<< eta << std::endl;
eta = -distNSMid / delta;
//std::cout << "eta 2 "<< eta << std::endl;
//eta = distNSMid2 / delta;
//std::cout << "eta 3 "<< eta << std::endl;
coord.x = eta;
fixEta = true;
}
if (fabs(angle_between_vectors(normalEW,normalEWOPP)-180*0) < 0.5)
{
double delta = (distEW1 - distEW0);
//xi = (distEWMid + delta - distEWMid2) /delta/ 2.0;
//std::cout << "xi 1 "<< xi << std::endl;
xi = -distEWMid / delta;
//std::cout << "xi 2 "<< xi << std::endl;
//xi = distEWMid2 / delta;
//std::cout << "xi 3 "<< xi << std::endl;
//xi = (distEWMid + distEWMid2)/ 2.0 / (distEW1 - distEW0);
coord.y = xi;
fixXi = true;
}
// if (D3VectorNorm(coordPrev-coord) < 1e-15)
// {
// iter = 1000;
// }
// if (std::isnan(coord.x) || std::isnan(coord.y) || std::isnan(coord.z))
// {
// std::cout << "trilinear interpolation failed!!!\n"<< std::endl;
// }
// std::cout << "initial guess\n";
// coord.Print();std::cout<<std::endl;
D3vector R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
//
// coord.x = 1; coord.y = 0; coord.z = 0;
// R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
// R.Print();
// coord.x = 0; coord.y = 1; coord.z = 0;
// R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
// R.Print();
// coord.x = 0; coord.y = 0; coord.z = 1;
// R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
// R.Print();
// coord.x = 1; coord.y = 1; coord.z = 0;
// R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
// R.Print();
// coord.x = 0; coord.y = 1; coord.z = 1;
// R = A + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
// R.Print();