src/beziertriangle.cpp

   1 //
   2 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   3 // +                                                                      +
   4 // + This file is part of enGrid.                                         +
   5 // +                                                                      +
   6 // + Copyright 2008,2009 Oliver Gloth                                     +
   7 // +                                                                      +
   8 // + enGrid is free software: you can redistribute it and/or modify       +
   9 // + it under the terms of the GNU General Public License as published by +
  10 // + the Free Software Foundation, either version 3 of the License, or    +
  11 // + (at your option) any later version.                                  +
  12 // +                                                                      +
  13 // + enGrid is distributed in the hope that it will be useful,            +
  14 // + but WITHOUT ANY WARRANTY; without even the implied warranty of       +
  15 // + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        +
  16 // + GNU General Public License for more details.                         +
  17 // +                                                                      +
  18 // + You should have received a copy of the GNU General Public License    +
  19 // + along with enGrid. If not, see <http://www.gnu.org/licenses/>.       +
  20 // +                                                                      +
  21 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  22 //
  23 #include "beziertriangle.h"
  24 #include "engrid.h"
  25
  26 #include "geometrytools.h"
  27 using namespace GeometryTools;
  28
  29 #include "vtkUnstructuredGridWriter.h"
  30
  31 #include <vtkCellLocator.h>
  32
  33 BezierTriangle::BezierTriangle() : Triangle() {
  34 }
  35
  36 BezierTriangle::BezierTriangle(vec3_t X_200, vec3_t X_020, vec3_t X_002, vec3_t X_011, vec3_t X_101, vec3_t X_110)  : Triangle(X_200, X_020, X_002) {
  37   setControlPoints(X_200,  X_020,  X_002,  X_011,  X_101,  X_110);
  38 }
  39
  40 void BezierTriangle::setControlPoints(vec3_t X_200, vec3_t X_020, vec3_t X_002, vec3_t X_011, vec3_t X_101, vec3_t X_110) {
  41   m_X_200 = X_200;
  42   m_X_020 = X_020;
  43   m_X_002 = X_002;
  44   m_X_011 = X_011;
  45   m_X_101 = X_101;
  46   m_X_110 = X_110;
  47   setupFunctionVariables();
  48 }
  49
  50 void BezierTriangle::getControlPoints(vec3_t& X_200, vec3_t& X_020, vec3_t& X_002, vec3_t& X_011, vec3_t& X_101, vec3_t& X_110) {
  51   X_200 = m_X_200;
  52   X_020 = m_X_020;
  53   X_002 = m_X_002;
  54   X_011 = m_X_011;
  55   X_101 = m_X_101;
  56   X_110 = m_X_110;
  57 }
  58
  59 vec3_t BezierTriangle::quadraticBezierTriangle(double u, double v, double w) {
  60   double total = u + v + w;
  61   u = u / total;
  62   v = v / total;
  63   w = w / total;
  64   return pow(u, 2)*m_X_200 + pow(v, 2)*m_X_020 + pow(w, 2)*m_X_002 + 2*u*v*m_X_110 + 2*v*w*m_X_011 + 2*w*u*m_X_101;
  65 }
  66
  67 vec3_t BezierTriangle::quadraticBezierTriangle(vec2_t M) {
  68   vec3_t bary_coords = getBarycentricCoordinates(M[0], M[1]);
  69   double u, v, w;
  70   u = bary_coords[0];
  71   v = bary_coords[1];
  72   w = bary_coords[2];
  73   return quadraticBezierTriangle(u, v, w);
  74 }
  75
  76 vec3_t BezierTriangle::quadraticBezierTriangle_g(vec3_t g_M) {
  77   vec2_t t_M = global3DToLocal2D(g_M);
  78   return quadraticBezierTriangle(t_M);
  79 }
  80
  81 void BezierTriangle::setupFunctionVariables() {
  82   m_l_X_200 = global3DToLocal3D(m_X_200);
  83   m_l_X_020 = global3DToLocal3D(m_X_020);
  84   m_l_X_002 = global3DToLocal3D(m_X_002);
  85   m_l_X_011 = global3DToLocal3D(m_X_011);
  86   m_l_X_101 = global3DToLocal3D(m_X_101);
  87   m_l_X_110 = global3DToLocal3D(m_X_110);
  88
  89   m_coeff_x2 = m_l_X_020 - 2 * m_l_X_110;
  90   m_coeff_y2 = m_l_X_002 - 2 * m_l_X_101;
  91   m_coeff_xy = -2 * m_l_X_110 + 2 * m_l_X_011 - 2 * m_l_X_101;
  92   m_coeff_x = 2 * m_l_X_110;
  93   m_coeff_y = 2 * m_l_X_101;
  94
  95   /*  qDebug()<<"@@@@@@@@@@@@@@@@@@@@@@@@@@@@";
  96     qDebug()<<"m_X_200"<<m_X_200;
  97     qDebug()<<"m_X_020"<<m_X_020;
  98     qDebug()<<"m_X_002"<<m_X_002;
  99     qDebug()<<"m_X_011"<<m_X_011;
 100     qDebug()<<"m_X_101"<<m_X_101;
 101     qDebug()<<"m_X_110"<<m_X_110;
 102     qDebug()<<"@@@@@@@@@@@@@@@@@@@@@@@@@@@@";
 103     qDebug()<<"m_l_X_200"<<m_l_X_200;
 104     qDebug()<<"m_l_X_020"<<m_l_X_020;
 105     qDebug()<<"m_l_X_002"<<m_l_X_002;
 106     qDebug()<<"m_l_X_011"<<m_l_X_011;
 107     qDebug()<<"m_l_X_101"<<m_l_X_101;
 108     qDebug()<<"m_l_X_110"<<m_l_X_110;
 109     qDebug()<<"@@@@@@@@@@@@@@@@@@@@@@@@@@@@";
 110     qDebug()<<"m_coeff_x2"<<m_coeff_x2;
 111     qDebug()<<"m_coeff_y2"<<m_coeff_y2;
 112     qDebug()<<"m_coeff_xy"<<m_coeff_xy;
 113     qDebug()<<"m_coeff_x"<<m_coeff_x;
 114     qDebug()<<"m_coeff_y"<<m_coeff_y;
 115     qDebug()<<"@@@@@@@@@@@@@@@@@@@@@@@@@@@@";*/
 116 }
 117
 118 vec2_t BezierTriangle::fixedPointFunction(vec2_t t_inputPoint, double x, double y) {
 119   /*  qDebug()<<"@@@@@@@@@@@@@@@@@@@@@@@@@@@@";
 120     qDebug()<<"m_coeff_x2"<<m_coeff_x2;
 121     qDebug()<<"m_coeff_y2"<<m_coeff_y2;
 122     qDebug()<<"m_coeff_xy"<<m_coeff_xy;
 123     qDebug()<<"m_coeff_x"<<m_coeff_x;
 124     qDebug()<<"m_coeff_y"<<m_coeff_y;
 125     qDebug()<<"@@@@@@@@@@@@@@@@@@@@@@@@@@@@";*/
 126   vec2_t F;
 127   F[0] = pow(x, 2) * m_coeff_x2[0] + pow(y, 2) * m_coeff_y2[0] + x * y * m_coeff_xy[0] + x * m_coeff_x[0] + y * m_coeff_y[0] - t_inputPoint[0];
 128   F[1] = pow(x, 2) * m_coeff_x2[1] + pow(y, 2) * m_coeff_y2[1] + x * y * m_coeff_xy[1] + x * m_coeff_x[1] + y * m_coeff_y[1] - t_inputPoint[1];
 129   return F;
 130 }
 131
 132 vec2_t BezierTriangle::fixedPointFunction(vec2_t t_inputPoint, vec2_t A) {
 133   return fixedPointFunction(t_inputPoint, A[0], A[1]);
 134 }
 135
 136 mat2_t BezierTriangle::jacobiMatrix(double x, double y) {
 137   mat2_t J;
 138   J[0][0] = 2 * x * m_coeff_x2[0] + y * m_coeff_xy[0] + m_coeff_x[0];
 139   J[1][0] = 2 * x * m_coeff_x2[1] + y * m_coeff_xy[1] + m_coeff_x[1];
 140   J[0][1] = 2 * y * m_coeff_y2[0] + x * m_coeff_xy[0] + m_coeff_y[0];
 141   J[1][1] = 2 * y * m_coeff_y2[1] + x * m_coeff_xy[1] + m_coeff_y[1];
 142   return J;
 143 }
 144
 145 // mat3_t BezierTriangle::jacobiMatrix_no_projection(double x, double y)
 146 // {
 147 //   mat3_t J;
 148 //   for(int i=0;i<3;i++) {
 149 //     J[i][0] = 2*x*m_coeff_x2[i] + y*m_coeff_xy[i] + m_coeff_x[i];
 150 //     J[i][1] = 2*y*m_coeff_y2[i] + x*m_coeff_xy[i] + m_coeff_y[i];
 151 //   }
 152 //   return J;
 153 // }
 154
 155 mat2_t BezierTriangle::jacobiMatrix_numeric(vec2_t t_inputPoint, double x, double y, double dx, double dy) {
 156   mat2_t J;
 157   vec2_t df = fixedPointFunction(t_inputPoint, x + dx, y + dy) - fixedPointFunction(t_inputPoint, x, y);
 158   if (dx < 10e-9) {
 159     J[0][0] = 0;
 160     J[1][0] = 0;
 161   } else {
 162     J[0][0] = df[0] / dx;
 163     J[1][0] = df[1] / dx;
 164   }
 165   if (dy < 10e-9) {
 166     J[0][1] = 0;
 167     J[1][1] = 0;
 168   } else {
 169     J[0][1] = df[0] / dy;
 170     J[1][1] = df[1] / dy;
 171   }
 172   return J;
 173 }
 174
 175 vec3_t BezierTriangle::projectLocal2DOnQuadraticBezierTriangle(vec2_t t_M) {
 176   bool DEBUG = false;
 177
 178   vec2_t t_X = t_M;//vec2_t(1./3.,1./3.,1./3.);
 179   if (DEBUG) qDebug() << "t_X=" << t_X;
 180   vec2_t F = fixedPointFunction(t_M, t_X[0], t_X[1]);
 181   if (DEBUG) qDebug() << "F.abs()=" << F.abs();
 182   int maxloops = 100;
 183   int Nloops = 0;
 184   while (F.abs() > 0.001 && Nloops < maxloops) {
 185     if (DEBUG) qDebug() << "test passed with F.abs()=" << F.abs() << " and " << Nloops << "<" << maxloops;
 186     mat2_t J = jacobiMatrix(t_X[0], t_X[1]);
 187     if (J.det() == 0) {
 188       qDebug() << "WARNING: Matrix not invertible!";
 189     }
 190     if (fabs(J[0][0]) + fabs(J[0][1]) >= 1) {
 191       if (DEBUG) qDebug() << "WARNING: will not converge (case 1)";
 192     }
 193     if (fabs(J[1][0]) + fabs(J[1][1]) >= 1) {
 194       if (DEBUG) qDebug() << "WARNING: will not converge (case 2)";
 195     }
 196
 197     mat2_t JI = J.inverse();
 198     vec2_t deltaX = -1 * (JI * F);
 199     t_X = t_X + deltaX;
 200     if (DEBUG) qDebug() << "t_X=" << t_X;
 201     F = fixedPointFunction(t_M, t_X[0], t_X[1]);
 202     if (DEBUG) qDebug() << "F=" << F;
 203     if (DEBUG) qDebug() << "F.abs()=" << F.abs();
 204     Nloops++;
 205   }
 206 //   if (Nloops >= maxloops) qDebug() << "WARNING: Exited before converging! Nloops=" << Nloops;
 207
 208   return quadraticBezierTriangle(t_X);
 209 }
 210
 211 vec3_t BezierTriangle::projectOnQuadraticBezierTriangle(vec3_t g_M, int output) {
 212   vec2_t t_M = global3DToLocal2D(g_M);
 213 //   qDebug()<<"t_M="<<t_M;
 214
 215   if (!insideBezierSurface(g_M)) {
 216 //     return vec3_t(0,0,0);
 217 //     qDebug() << "WARNING: Not on bezier triangle! t_M=" << t_M;
 218     //get closest point M' on triangle
 219     int side = -1;
 220     double Lmin = 0;
 221     vec3_t g_Mp = closestPointOnBezierCurves(g_M, side, Lmin);
 222
 223     if (!insideBezierSurface(g_Mp)) {
 224       setDebugLevel(1);
 225       insideBezierSurface(g_Mp);
 226       saveBezierTriangle("crash");
 227       EG_BUG;
 228     }
 229     int zone = -1;
 230     vec3_t xi(0, 0, 0);
 231     vec3_t ri(0, 0, 0);
 232     double d = 0;
 233     projectOnTriangle(g_M, xi, ri, d, zone, true);
 234     if(zone==3 || zone==4 || zone==5) {
 235       side = zone;
 236     }
 237
 238     /*    vec2_t t_Mp(ri[0], ri[1]);
 239     qDebug() << "t_Mp=" << t_Mp;
 240     vec3_t g_Mp = local2DToGlobal3D(t_Mp);*/
 241 //     qDebug() << "g_Mp=" << g_Mp;
 242     vec2_t t_Mp = global3DToLocal2D(g_Mp);
 243     if(!checkVector(g_Mp)) EG_BUG;
 244     if(!checkVector(t_Mp)) EG_BUG;
 245
 246 //     if (output == 0) return g_Mp;
 247 //     else return surfaceNormal(t_Mp, 0);
 248
 249 //     vec3_t g_Mp_proj = projectLocal2DOnQuadraticBezierTriangle(t_Mp);
 250 //     qDebug() << "g_Mp_proj=" << g_Mp_proj;
 251
 252 //     qDebug()<<"side="<<side;
 253     if (m_has_neighbour[side]) {
 254       // no extrapolation, restrict
 255       if (output == 0) return g_Mp;
 256       else return surfaceNormal(t_Mp, 0);
 257     } else {
 258       // extrapolate
 259
 260       //get normal vector N at that point
 261       vec3_t g_N = surfaceNormal(t_Mp, 0);
 262 //       qDebug() << "g_N=" << g_N;
 263
 264       //project original point M onto plane (M',N)
 265       if(!checkVector(g_Mp)) EG_BUG;
 266       if(!checkVector(g_N)) EG_BUG;
 267       double k = intersection(g_M, m_g3, g_Mp, g_N);
 268 //     vec3_t g_P = projectPointOnPlane(g_M, g_Mp_proj, g_N);
 269       vec3_t g_P = g_M + k * m_g3;
 270
 271       if(isnan(k) || isinf(k)) {
 272         qWarning()<<"g_M="<<g_M;
 273         qWarning()<<"m_g3="<<m_g3;
 274         qWarning()<<"g_Mp="<<g_Mp;
 275         qWarning()<<"g_N="<<g_N;
 276         EG_BUG;
 277       }
 278       if(!checkVector(g_M)) EG_BUG;
 279       if(!checkVector(m_g3)) EG_BUG;
 280       if(!checkVector(g_P)) EG_BUG;
 281
 282       if (output == 0) return g_P;
 283       else return g_N;
 284     }
 285
 286   } else {
 287     if (output == 0) return projectLocal2DOnQuadraticBezierTriangle(t_M);
 288     else return surfaceNormal(t_M, 0);
 289   }
 290
 291 }
 292
 293 double BezierTriangle::z_func(vec2_t t_M) {
 294   return z_func(t_M[0], t_M[1]);
 295 }
 296
 297 double BezierTriangle::z_func(double x, double y) {
 298   bool DEBUG = false;
 299   vec2_t t_M = vec2_t(x, y);
 300   if (DEBUG) qDebug() << "t_M=" << t_M;
 301   vec3_t g_B = projectLocal2DOnQuadraticBezierTriangle(t_M);
 302   vec3_t l_B = global3DToLocal3D(g_B);
 303   return l_B[2];
 304 }
 305
 306 vec3_t BezierTriangle::surfaceNormal(vec2_t t_M, int output) {
 307   vec3_t bary_coords = getBarycentricCoordinates(t_M[0], t_M[1]);
 308   double u = bary_coords[0];
 309   double v = bary_coords[1];
 310   double w = bary_coords[2];
 311
 312   vec2_t dx, dy;
 313   vec2_t ex(1, 0);
 314   vec2_t ey(0, 1);
 315   if (u >= v && u > w) {
 316     dx = ex;
 317     dy = ey;
 318   } else if (v > u && v >= w) {
 319     dx = ey - ex;
 320     dy = -1 * ex;
 321   } else if (w >= u && w > v) {
 322     dx = -1 * ey;
 323     dy = ex - ey;
 324   } else {
 325     qWarning() << "bary_coords=" << bary_coords;
 326     EG_BUG;
 327   }
 328
 329   /*  qDebug()<<"##############";
 330     qDebug()<<"dx="<<dx;
 331     qDebug()<<"dy="<<dy;
 332     qDebug()<<"##############";*/
 333
 334   if (!isInsideTriangle(t_M)) {
 335     //TODO: special dx,dy for points outside basic triangle
 336     // get closest point on beziercurves
 337     int side = -1;
 338     double Lmin = 0;
 339     vec3_t g_M = local2DToGlobal3D(t_M);
 340     vec3_t g_Mp = closestPointOnBezierCurves(g_M, side, Lmin);
 341     vec2_t t_Mp = global3DToLocal2D(g_Mp);
 342     // get tangent at that point
 343     vec2_t t_tangent;
 344     insideBezierCurve(t_M, side, t_tangent);
 345     vec2_t t_normal = turnRight(t_tangent);// t_M-t_Mp;
 346 //     vec2_t t_tangent = turnLeft(t_normal);
 347     checkVector(t_normal);
 348     checkVector(t_tangent);
 349     t_normal.normalise();
 350     t_tangent.normalise();
 351     checkVector(t_normal);
 352     checkVector(t_tangent);
 353     // build dx,dy according to that tangent + orig point + closest point
 354     dx = t_normal - t_tangent;
 355     dy = t_normal + t_tangent;
 356   }
 357
 358   dx.normalise();
 359   dy.normalise();
 360   double k = 0.01;// * m_smallest_length;
 361   dx = k*dx;
 362   dy = k*dy;
 363
 364   vec2_t t_P0 = t_M;
 365   double z0 = z_func(t_P0);
 366   vec3_t l_P0(t_P0[0], t_P0[1], z0);
 367
 368   if (!insideBezierSurface(t_P0)) {
 369     qWarning() << "t_P0=" << t_P0;
 370   //     return vec3_t(0, 0, 0);
 371     EG_BUG;
 372   }
 373
 374   vec3_t l_u1;
 375   vec3_t l_u2;
 376
 377   int Nloops = 0;
 378   int maxloops = 100;
 379   bool dxdy_ok = false;
 380   while(!dxdy_ok && Nloops<maxloops) {
 381
 382     dxdy_ok = true;
 383
 384     vec2_t t_Px1 = t_P0 - dx;
 385     vec2_t t_Px2 = t_P0 + dx;
 386     vec2_t t_Py1 = t_P0 - dy;
 387     vec2_t t_Py2 = t_P0 + dy;
 388     double zx1 = z_func(t_Px1);
 389     double zx2 = z_func(t_Px2);
 390     double zy1 = z_func(t_Py1);
 391     double zy2 = z_func(t_Py2);
 392     vec3_t l_Px1(t_Px1[0], t_Px1[1], zx1);
 393     vec3_t l_Px2(t_Px2[0], t_Px2[1], zx2);
 394     vec3_t l_Py1(t_Py1[0], t_Py1[1], zy1);
 395     vec3_t l_Py2(t_Py2[0], t_Py2[1], zy2);
 396
 397     if (insideBezierSurface(t_Px1) && insideBezierSurface(t_Px2)) {
 398       l_u1 = l_Px2 - l_Px1;
 399     } else if (!insideBezierSurface(t_Px1) && insideBezierSurface(t_Px2)) {
 400       l_u1 = l_Px2 - l_P0;
 401     } else if (insideBezierSurface(t_Px1) && !insideBezierSurface(t_Px2)) {
 402       l_u1 = l_P0 - l_Px1;
 403     } else {
 404 /*      qWarning() << "t_P0=" << t_P0;
 405       qWarning() << "t_Px1=" << t_Px1;
 406       qWarning() << "t_Px2=" << t_Px2;
 407       qWarning() << "t_Py1=" << t_Py1;
 408       qWarning() << "t_Py2=" << t_Py2;*/
 409       dxdy_ok = false;
 410 //       return vec3_t(0, 0, 0);
 411     }
 412
 413
 414     if (insideBezierSurface(t_Py1) && insideBezierSurface(t_Py2)) {
 415       l_u2 = l_Py2 - l_Py1;
 416     } else if (!insideBezierSurface(t_Py1) && insideBezierSurface(t_Py2)) {
 417       l_u2 = l_Py2 - l_P0;
 418     } else if (insideBezierSurface(t_Py1) && !insideBezierSurface(t_Py2)) {
 419       l_u2 = l_P0 - l_Py1;
 420     } else {
 421 /*      qWarning() << "##############";
 422       qWarning() << "dx="<<dx;
 423       qWarning() << "dy="<<dy;
 424       qWarning() << "angle(dx,dy)=" << rad2deg(acos((dx*dy)/(dx.abs()*dy.abs())));
 425       qWarning() << "##############";
 426       qWarning() << "t_P0=" << t_P0 << " : " <<insideBezierSurface(t_P0);
 427       qWarning() << "t_Px1=" << t_Px1 << " : " <<insideBezierSurface(t_Px1);
 428       qWarning() << "t_Px2=" << t_Px2 << " : " <<insideBezierSurface(t_Px2);
 429       qWarning() << "t_Py1=" << t_Py1 << " : " <<insideBezierSurface(t_Py1);
 430       qWarning() << "t_Py2=" << t_Py2 << " : " <<insideBezierSurface(t_Py2);*/
 431       dxdy_ok = false;
 432 //       return vec3_t(0, 0, 0);
 433     }
 434
 435     if(!dxdy_ok) {
 436       dx = 0.5*dx;
 437       dy = 0.5*dy;
 438     }
 439
 440     Nloops++;
 441   }
 442   if(Nloops>=maxloops) EG_BUG;
 443
 444   vec3_t g_u1 = m_G * l_u1;
 445   g_u1.normalise();
 446   vec3_t g_u2 = m_G * l_u2;
 447   g_u2.normalise();
 448   vec3_t g_N = g_u1.cross(g_u2);
 449   g_N.normalise();
 450   if (output == 0) {
 451     return g_N;
 452   } else if (output == 1) {
 453     return g_u1;
 454   } else {
 455     return g_u2;
 456   }
 457 }
 458
 459 bool BezierTriangle::insideBezierSurface(vec3_t g_M)
 460 {
 461   vec2_t t_M = global3DToLocal2D(g_M);
 462 //   qDebug()<<"g_M="<<g_M;
 463 //   qDebug()<<"t_M="<<t_M;
 464   vec3_t xi(0, 0, 0);
 465   vec3_t ri(0, 0, 0);
 466   double d = 0;
 467   int side;
 468   projectOnTriangle(g_M, xi, ri, d, side, true);
 469   if(side==3 || side==4 || side==5) {
 470     if(DebugLevel>0) qWarning()<<"side==3 || side==4 || side==5";
 471     return false;
 472   }
 473   else {
 474     vec2_t t_tangent;
 475     if(insideBezierCurve(t_M,0,t_tangent) && insideBezierCurve(t_M,1,t_tangent) && insideBezierCurve(t_M,2,t_tangent)) {
 476       if(DebugLevel>0) qWarning()<<"insideBezierCurves";
 477       return true;
 478     }
 479     else {
 480       if(DebugLevel>0) qWarning()<<"else";
 481       return false;
 482     }
 483   }
 484 }
 485
 486 bool BezierTriangle::insideBezierSurface(vec2_t t_M) {
 487   return insideBezierSurface(local2DToGlobal3D(t_M));
 488 }
 489
 490 //TODO: merge projectOnBezierSide + insideBezierCurve ?
 491 vec3_t BezierTriangle::projectOnBezierSide(vec3_t g_M, int side, double& Lmin, double& u)
 492 {
 493   vec3_t a,b,c;
 494   if(side==0) { // w=0
 495     // B-M = a*u^2 + b*u + c
 496     a = (m_X_200 + m_X_020 - 2*m_X_110);
 497     b = (-2*m_X_020 + 2*m_X_110);
 498     c = m_X_020 - g_M;
 499   }
 500   else if(side==1) { // u=0
 501     a = (m_X_020 + m_X_002 - 2*m_X_011);
 502     b = (-2*m_X_002 + 2*m_X_011);
 503     c = m_X_002 - g_M;
 504   }
 505   else { // v=0
 506     a = (m_X_002 + m_X_200 - 2*m_X_101);
 507     b = (-2*m_X_200 + 2*m_X_101);
 508     c = m_X_200 - g_M;
 509   }
 510
 511   // d((B-M).abs())/du = coeff3*u^3 + coeff2*u^2 + coeff1*u + coeff0
 512   double coeff3, coeff2, coeff1, coeff0;
 513   coeff3 = 4*a*a;
 514   coeff2 = 6*a*b;
 515   coeff1 = 4*a*c+2*b*b;
 516   coeff0 = 2*b*c;
 517
 518   double x[3];
 519   int N;
 520   if(coeff3!=0) N = poly_solve_cubic(coeff2/coeff3, coeff1/coeff3, coeff0/coeff3, &(x[0]), &(x[1]), &(x[2]));
 521   else N = poly_solve_quadratic (coeff2, coeff1, coeff0, &(x[0]), &(x[1]));
 522
 523   if(N==0) EG_BUG;
 524
 525   double L[3];
 526   Lmin = 0;
 527   u = 0;
 528   bool first = true;
 529
 530   for(int i=0;i<N;i++) {
 531     if(x[i]<0) x[i]=0;
 532     if(x[i]>1) x[i]=1;
 533     L[i] = (pow(x[i],2)*a + x[i]*b + c).abs();
 534     if(first) {
 535       Lmin = L[i];
 536       u = x[i];
 537       first = false;
 538     }
 539     else if(L[i]<Lmin) {
 540       Lmin = L[i];
 541       u = x[i];
 542     }
 543   }
 544
 545   vec3_t g_B = g_M + pow(u,2)*a + u*b + c;
 546   return g_B;
 547 }
 548
 549 bool BezierTriangle::insideBezierCurve(vec2_t t_M, int side, vec2_t& t_tangent, double tol)
 550 {
 551 //   qDebug()<<"t_M="<<t_M;
 552 //   qDebug()<<"side="<<side;
 553
 554   vec2_t t_X_200 = global3DToLocal2D(m_X_200);
 555   vec2_t t_X_020 = global3DToLocal2D(m_X_020);
 556   vec2_t t_X_002 = global3DToLocal2D(m_X_002);
 557   vec2_t t_X_011 = global3DToLocal2D(m_X_011);
 558   vec2_t t_X_101 = global3DToLocal2D(m_X_101);
 559   vec2_t t_X_110 = global3DToLocal2D(m_X_110);
 560
 561   vec2_t a,b,c;
 562   if(side==0) { // w=0
 563     // B -M = a*u^2 + b*u + c
 564     a = (t_X_200 + t_X_020 - 2*t_X_110);
 565     b = (-2*t_X_020 + 2*t_X_110);
 566     c = t_X_020 - t_M;
 567   }
 568   else if(side==1) { // u=0
 569     a = (t_X_020 + t_X_002 - 2*t_X_011);
 570     b = (-2*t_X_002 + 2*t_X_011);
 571     c = t_X_002 - t_M;
 572   }
 573   else { // v=0
 574     a = (t_X_002 + t_X_200 - 2*t_X_101);
 575     b = (-2*t_X_200 + 2*t_X_101);
 576     c = t_X_200 - t_M;
 577   }
 578
 579   // d((B-M).abs())/du = coeff3*u^3 + coeff2*u^2 + coeff1*u + coeff0
 580   double coeff3, coeff2, coeff1, coeff0;
 581   coeff3 = 4*a*a;
 582   coeff2 = 6*a*b;
 583   coeff1 = 4*a*c+2*b*b;
 584   coeff0 = 2*b*c;
 585
 586   double x[3];
 587   int N;
 588   if(coeff3!=0) {
 589 //     qWarning()<<"using poly_solve_cubic";
 590     N = poly_solve_cubic(coeff2/coeff3, coeff1/coeff3, coeff0/coeff3, &(x[0]), &(x[1]), &(x[2]));
 591   }
 592   else {
 593 //     qWarning()<<"using poly_solve_quadratic";
 594     N = poly_solve_quadratic (coeff2, coeff1, coeff0, &(x[0]), &(x[1]));
 595   }
 596
 597   if(N==0) {
 598     qWarning()<<"coeff3="<<coeff3;
 599     qWarning()<<"coeff2="<<coeff2;
 600     qWarning()<<"coeff1="<<coeff1;
 601     qWarning()<<"coeff0="<<coeff0;
 602     EG_BUG;
 603   }
 604
 605   double L[3];
 606   double Lmin = 0;
 607   double u = 0;
 608   bool first = true;
 609
 610   for(int i=0;i<N;i++) {
 611     if(isnan(x[i]) || isinf(x[i])) {
 612       qWarning()<<"NAN OR INF";
 613       qWarning()<<"x["<<i<<"]="<<x[i];
 614       qWarning()<<"coeff3="<<coeff3;
 615       qWarning()<<"coeff2="<<coeff2;
 616       qWarning()<<"coeff1="<<coeff1;
 617       qWarning()<<"coeff0="<<coeff0;
 618
 619       qWarning()<<"checkVector(t_X_200)="<<checkVector(t_X_200);
 620       qWarning()<<"checkVector(t_X_020)="<<checkVector(t_X_020);
 621       qWarning()<<"checkVector(t_X_002)="<<checkVector(t_X_002);
 622       qWarning()<<"checkVector(t_X_011)="<<checkVector(t_X_011);
 623       qWarning()<<"checkVector(t_X_101)="<<checkVector(t_X_101);
 624       qWarning()<<"checkVector(t_X_110)="<<checkVector(t_X_110);
 625       qWarning()<<"checkVector(m_X_200)="<<checkVector(m_X_200);
 626       qWarning()<<"checkVector(m_X_020)="<<checkVector(m_X_020);
 627       qWarning()<<"checkVector(m_X_002)="<<checkVector(m_X_002);
 628       qWarning()<<"checkVector(m_X_011)="<<checkVector(m_X_011);
 629       qWarning()<<"checkVector(m_X_101)="<<checkVector(m_X_101);
 630       qWarning()<<"checkVector(m_X_110)="<<checkVector(m_X_110);
 631       qWarning()<<"checkVector(a)="<<checkVector(a);
 632       qWarning()<<"checkVector(b)="<<checkVector(b);
 633       qWarning()<<"checkVector(c)="<<checkVector(c);
 634
 635       EG_BUG;
 636     }
 637     if(x[i]<0) x[i]=0;
 638     if(x[i]>1) x[i]=1;
 639     L[i] = (pow(x[i],2)*a + x[i]*b + c).abs();
 640     if(first) {
 641       Lmin = L[i];
 642       u = x[i];
 643       first = false;
 644     }
 645     else if(L[i]<Lmin) {
 646       Lmin = L[i];
 647       u = x[i];
 648     }
 649   }
 650
 651   vec2_t t_B = t_M + pow(u,2)*a + u*b + c;
 652   t_tangent = 2*u*a + b;
 653
 654   vec3_t l_M = vec3_t(t_M[0],t_M[1],0);
 655   vec3_t l_B = vec3_t(t_B[0],t_B[1],0);
 656   vec3_t l_tangent = vec3_t(t_tangent[0],t_tangent[1],0);
 657
 658   checkVector(l_B);
 659   checkVector(l_tangent);
 660   if(DebugLevel>0) {
 661     qWarning()<<"l_M="<<l_M;
 662     qWarning()<<"l_B="<<l_B;
 663     qWarning()<<"l_tangent="<<l_tangent;
 664     qWarning()<<"l_tangent.cross(l_M-l_B)="<<l_tangent.cross(l_M-l_B);
 665     qWarning()<<"(l_tangent.cross(l_M-l_B))[2]="<<(l_tangent.cross(l_M-l_B))[2];
 666   }
 667   return ( (l_tangent.cross(l_M-l_B))[2]<=0+tol );
 668 }
 669
 670 vec3_t BezierTriangle::closestPointOnBezierCurves(vec3_t g_M, int& side, double& Lmin)
 671 {
 672   Lmin = 0;
 673   side = -1;
 674   vec3_t g_Mp;
 675   bool first = true;
 676   for(int i_side=0; i_side<3; i_side++) {
 677     double L,u;
 678     vec3_t foo = projectOnBezierSide(g_M, i_side,L,u);
 679     if(first || L<Lmin) {
 680       Lmin = L;
 681       g_Mp = foo;
 682       side = i_side;
 683       first = false;
 684     }
 685   }
 686   return g_Mp;
 687 }
 688
 689 bool BezierTriangle::checkControlPoints()
 690 {
 691   vec2_t t_X_200 = global3DToLocal2D(m_X_200);
 692   vec2_t t_X_020 = global3DToLocal2D(m_X_020);
 693   vec2_t t_X_002 = global3DToLocal2D(m_X_002);
 694   vec2_t t_X_011 = global3DToLocal2D(m_X_011);
 695   vec2_t t_X_101 = global3DToLocal2D(m_X_101);
 696   vec2_t t_X_110 = global3DToLocal2D(m_X_110);
 697
 698   vec3_t bary_coords_011 = getBarycentricCoordinates(t_X_011[0], t_X_011[1]);
 699   double u_011, v_011, w_011;
 700   u_011 = bary_coords_011[0];
 701   v_011 = bary_coords_011[1];
 702   w_011 = bary_coords_011[2];
 703
 704   vec3_t bary_coords_101 = getBarycentricCoordinates(t_X_101[0], t_X_101[1]);
 705   double u_101, v_101, w_101;
 706   u_101 = bary_coords_101[0];
 707   v_101 = bary_coords_101[1];
 708   w_101 = bary_coords_101[2];
 709
 710   vec3_t bary_coords_110 = getBarycentricCoordinates(t_X_110[0], t_X_110[1]);
 711   double u_110, v_110, w_110;
 712   u_110 = bary_coords_110[0];
 713   v_110 = bary_coords_110[1];
 714   w_110 = bary_coords_110[2];
 715
 716   double k1;
 717   double k2;
 718   vec2_t r[6];
 719   vec2_t u[6];
 720
 721   r[0] = t_X_200;
 722   r[1] = t_X_110;
 723   r[2] = t_X_020;
 724   r[3] = t_X_011;
 725   r[4] = t_X_002;
 726   r[5] = t_X_101;
 727
 728   for(int i=0;i<6;i++) {
 729     u[i] = r[(i+1)%6]-r[i];
 730   }
 731
 732   for(int i=0;i<9;i++) {
 733     int jmax = 6;
 734     if(i==0) jmax = 6-1;
 735     for(int j=i+2;j<jmax;j++) {
 736 //       qDebug()<<"i="<<i<<"->j="<<j;
 737       if(intersection (k1, k2, r[i], u[i], r[j], u[j])) {
 738         if( 0<k1 && k1<1 && 0<k2 && k2<1 ) {
 739           qDebug()<<"k1="<<k1;
 740           qDebug()<<"k2="<<k2;
 741           return(false);
 742         }
 743       }
 744       else {
 745         qDebug()<<"NO INTERSECTION";
 746       }
 747     }
 748   }
 749 /*  bool intersection (double &k1, double &k2, vec2_t r1, vec2_t u1, vec2_t r2, vec2_t u2);
 750
 751   // segments:
 752   X_200 - X_101;
 753   X_101 - X_002;
 754
 755   X_002 - X_011;
 756   X_011 - X_020;
 757
 758   X_020 - X_110;
 759   X_110 - X_200;
 760
 761   X_200 - X_101;
 762     X_002 - X_011;
 763     X_011 - X_020;
 764
 765     X_020 - X_110;
 766     X_110 - X_200;
 767
 768   X_101 - X_002;
 769     X_002 - X_011;
 770     X_011 - X_020;
 771
 772     X_020 - X_110;
 773     X_110 - X_200;
 774
 775   X_002 - X_011;
 776   X_011 - X_020;
 777
 778   X_020 - X_110;
 779   X_110 - X_200;*/
 780
 781   return(true);
 782 }
 783
 784 void BezierTriangle::saveBezierTriangle(QString filename)
 785 {
 786   int N_cells = 2;
 787   int N_points = 6;
 788   int N = 10;
 789
 790   //qDebug()<<"N_cells="<<N_cells;
 791   //qDebug()<<"N_points="<<N_points;
 792   //qDebug()<<"N_cells_per_triangle="<<N_cells_per_triangle;
 793   //qDebug()<<"N_points_per_triangle="<<N_points_per_triangle;
 794
 795   EG_VTKSP(vtkUnstructuredGrid, interpolationGrid);
 796   allocateGrid(interpolationGrid , N_cells, N_points);
 797
 798   vtkIdType node_count = 0;
 799   int cell_count = 0;
 800
 801
 802   vtkIdType pts[3];
 803   interpolationGrid->GetPoints()->SetPoint(node_count, m_a.data()); pts[0]=node_count; node_count++;
 804   interpolationGrid->GetPoints()->SetPoint(node_count, m_b.data()); pts[1]=node_count; node_count++;
 805   interpolationGrid->GetPoints()->SetPoint(node_count, m_c.data()); pts[2]=node_count; node_count++;
 806
 807   vtkIdType idx_J1, idx_J2, idx_J3;
 808   interpolationGrid->GetPoints()->SetPoint(node_count, m_X_011.data()); idx_J1=node_count; node_count++;
 809   interpolationGrid->GetPoints()->SetPoint(node_count, m_X_101.data()); idx_J2=node_count; node_count++;
 810   interpolationGrid->GetPoints()->SetPoint(node_count, m_X_110.data()); idx_J3=node_count; node_count++;
 811
 812   vtkIdType polyline[7];
 813   polyline[0]=pts[0];
 814   polyline[1]=idx_J3;
 815   polyline[2]=pts[1];
 816   polyline[3]=idx_J1;
 817   polyline[4]=pts[2];
 818   polyline[5]=idx_J2;
 819   polyline[6]=pts[0];
 820
 821   interpolationGrid->InsertNextCell(VTK_TRIANGLE,3,pts);cell_count++;
 822   interpolationGrid->InsertNextCell(4,7,polyline);cell_count++;
 823
 824   //qDebug()<<"node_count="<<node_count;
 825   //qDebug()<<"cell_count="<<cell_count;
 826   //qDebug()<<"offset="<<offset;
 827
 828   saveGrid(interpolationGrid, filename+"_InterpolationGrid");
 829 }
 830
 831 void BezierTriangle::writeBezierSurface(QString filename, int N) {
 832   EG_VTKSP(vtkUnstructuredGrid, bezier);
 833   getBezierSurface(bezier, N);
 834   saveGrid(bezier, filename);
 835 }
 836
 837 void BezierTriangle::getBezierSurface(vtkUnstructuredGrid* bezier, int N)
 838 {
 839   int N_cells = (N - 1) * (N - 1);
 840   int N_points = (N * N + N) / 2;
 841   allocateGrid(bezier, N_cells, N_points);
 842
 843   int offset = 0;
 844
 845   vtkIdType node_count = 0;
 846   for(int i=0;i<N;i++) {
 847     for(int j=0;j<N-i;j++) {
 848       double x = i/(double)(N-1);
 849       double y = j/(double)(N-1);
 850       vec3_t bary_coords = getBarycentricCoordinates(x,y);
 851       double u,v,w;
 852       u=bary_coords[0];
 853       v=bary_coords[1];
 854       w=bary_coords[2];
 855       vec3_t M = this->quadraticBezierTriangle(u, v, w);
 856       bezier->GetPoints()->SetPoint(offset + node_count, M.data());node_count++;
 857     }
 858   }
 859
 860   int cell_count = 0;
 861   for(int i=0;i<N-1;i++) {
 862     for(int j=0;j<N-1-i;j++) {
 863
 864       vtkIdType pts_triangle1[3];
 865       pts_triangle1[0]=offset + trigrid_idx(N, i  ,j  );
 866       pts_triangle1[1]=offset + trigrid_idx(N, i+1,j  );
 867       pts_triangle1[2]=offset + trigrid_idx(N, i  ,j+1);
 868       bezier->InsertNextCell(VTK_TRIANGLE,3,pts_triangle1);cell_count++;
 869
 870       if(i+j<N-2) {
 871         vtkIdType pts_triangle2[3];
 872         pts_triangle2[0]=offset + trigrid_idx(N, i+1,j  );
 873         pts_triangle2[1]=offset + trigrid_idx(N, i+1,j+1);
 874         pts_triangle2[2]=offset + trigrid_idx(N, i  ,j+1);
 875         bezier->InsertNextCell(VTK_TRIANGLE,3,pts_triangle2);cell_count++;
 876       }
 877
 878     }
 879   }
 880 }