src/geometrytools.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 "geometrytools.h"
  24 #include "containertricks.h"
  25 #include "engrid.h"
  26 #include "utilities.h"
  27
  28 #include <vtkCellType.h>
  29 #include <cmath>
  30
  31 namespace GeometryTools {
  32
  33 double rad2deg( double rad )
  34 {
  35   return rad/M_PI*180;
  36 }
  37
  38 double deg2rad( double deg )
  39 {
  40   return deg/180*M_PI;
  41 }
  42
  43 void rotate(vec3_t g1, vec3_t g2,vec3_t g3, vec3_t &b, double theta)
  44 {
  45   //cout << b << endl;
  46   mat3_t g_t;
  47   g_t[0] = g1;
  48   g_t[1] = g2;
  49   g_t[2] = g3;
  50   mat3_t g = g_t.transp();
  51   //cout << g << endl;
  52   vec3_t bb = g_t*b;
  53   //cout << bb << endl;
  54   mat3_t rot = mat3_t::identity();
  55   rot[0][0] = cos(theta);
  56   rot[0][1] = -sin(theta);
  57   rot[1][0] = sin(theta);
  58   rot[1][1] = cos(theta);
  59   //cout << rot << endl;
  60   vec3_t bbb = rot*bb;
  61   //cout << bbb << endl;
  62   b = g*bbb;
  63   //cout << b << endl;
  64 }
  65
  66 vec3_t rotate(vec3_t v, vec3_t axis, double theta)
  67 {
  68   axis.normalise();
  69
  70   // transposed base of rotate system
  71   mat3_t g_t;
  72
  73   // compute projection of v in axis direction
  74   vec3_t v_axis = (axis*v)*axis;
  75
  76   // compute first orthogonal vector (first base vector)
  77   g_t[0] = v-v_axis;
  78
  79   //In case of points on the rotation axis, do nothing
  80   if(g_t[0].abs()==0) return v;
  81
  82   g_t[0].normalise();
  83
  84   // second base vector is the normalised axis
  85   g_t[1] = axis;
  86
  87   // compute second orthogonal vector (third base vector)
  88   g_t[2] = g_t[0].cross(g_t[1]);
  89
  90   // base of rotate system
  91   mat3_t g = g_t.transp();
  92
  93   // matrix for rotation around g_t[1];
  94   mat3_t rot = mat3_t::identity();
  95   rot[0][0] =  cos(theta);
  96   rot[0][2] =  sin(theta);
  97   rot[2][0] = -sin(theta);
  98   rot[2][2] =  cos(theta);
  99
 100   // transfer v to rotate system
 101   vec3_t v_r = g_t*v;
 102
 103   // rotate the vector and transfer it back
 104   v_r = rot*v_r;
 105   v = g*v_r;
 106
 107   return v;
 108 }
 109
 110 vec3_t orthogonalVector(vec3_t v)
 111 {
 112   // get absolute values
 113   double xx = v[0] < 0.0 ? -v[0] : v[0];
 114   double yy = v[1] < 0.0 ? -v[1] : v[1];
 115   double zz = v[2] < 0.0 ? -v[2] : v[2];
 116   // switch both biggest values and set the other one to zero
 117   vec3_t u;
 118   if (xx < yy) {
 119     u = xx < zz ? vec3_t(0,v[2],-v[1]) : vec3_t(v[1],-v[0],0);
 120   } else {
 121     u = yy < zz ? vec3_t(-v[2],0,v[0]) : vec3_t(v[1],-v[0],0);
 122   }
 123   u.normalise();
 124   return u;
 125 }
 126
 127
 128 double intersection(vec3_t x_straight, vec3_t v_straight, vec3_t x_plane, vec3_t u_plane, vec3_t v_plane)
 129 {
 130   vec3_t n = u_plane.cross(v_plane);
 131   return intersection(x_straight,v_straight,x_plane,n);
 132 }
 133
 134 double intersection(vec3_t x_straight, vec3_t v_straight, vec3_t x_plane, vec3_t n_plane)
 135 {
 136   double k = (x_plane*n_plane - x_straight*n_plane)/(v_straight*n_plane);
 137   return k;
 138 }
 139
 140 bool intersection (double &k1, double &k2, vec2_t r1, vec2_t u1, vec2_t r2, vec2_t u2)
 141 {
 142   double ave_length = .5*(sqrt(u1[0]*u1[0]+u1[1]*u1[1]) + sqrt(u2[0]*u2[0]+u2[1]*u2[1]));
 143   double DET = (u1[0]*u2[1]-u1[1]*u2[0]);
 144   if (fabs(DET) > 1e-6*ave_length) {
 145     k1 = -(u2[0]*r2[1]-u2[0]*r1[1]-r2[0]*u2[1]+r1[0]*u2[1])/DET;
 146     k2 = -(-u1[1]*r2[0]+u1[0]*r2[1]-u1[0]*r1[1]+u1[1]*r1[0])/DET;
 147     return true;
 148   } else {
 149     return false;
 150   }
 151 }
 152
 153 void sliceTriangle(const vector<vec3_t> &Tin, vec3_t x, vec3_t n, vector<vector<vec3_t> > &Tout)
 154 {
 155   vec3_t a = Tin[0];
 156   vec3_t b = Tin[1];
 157   vec3_t c = Tin[2];
 158   double kab = intersection(a,b-a,x,n);
 159   double kbc = intersection(b,c-b,x,n);
 160   double kca = intersection(c,a-c,x,n);
 161   bool ab_cut = ((kab >= 0) && (kab <= 1));
 162   bool bc_cut = ((kbc >= 0) && (kbc <= 1));
 163   bool ca_cut = ((kca >= 0) && (kca <= 1));
 164   if (ab_cut && bc_cut && ca_cut) {
 165     //cerr << "invalid triangle (SliceTriangle) A" << endl;
 166     //exit(EXIT_FAILURE);
 167     if      ((kab <= kbc) && (kab <= kca)) ab_cut = false;
 168     else if ((kbc <= kab) && (kbc <= kca)) bc_cut = false;
 169     else                                   ca_cut = false;
 170   }
 171   if (ab_cut && bc_cut) {
 172     vec3_t ab = a + kab*(b-a);
 173     vec3_t bc = b + kbc*(c-b);
 174     Tout.resize(3,vector<vec3_t>(3));
 175     clinit(Tout[0]) = a,ab,bc;
 176     clinit(Tout[1]) = ab,b,bc;
 177     clinit(Tout[2]) = bc,c,a;
 178   } else if (bc_cut && ca_cut) {
 179     vec3_t bc = b + kbc*(c-b);
 180     vec3_t ca = c + kca*(a-c);
 181     Tout.resize(3,vector<vec3_t>(3));
 182     clinit(Tout[0]) = a,bc,ca;
 183     clinit(Tout[1]) = a,b,bc;
 184     clinit(Tout[2]) = bc,c,ca;
 185   } else if (ca_cut && ab_cut) {
 186     vec3_t ca = c + kca*(a-c);
 187     vec3_t ab = a + kab*(b-a);
 188     Tout.resize(3,vector<vec3_t>(3));
 189     clinit(Tout[0]) = a,ab,ca;
 190     clinit(Tout[1]) = ab,b,ca;
 191     clinit(Tout[2]) = b,c,ca;
 192   } else {
 193     Tout.resize(1,vector<vec3_t>(3));
 194     clinit(Tout[0]) = a,b,c;
 195   }
 196 }
 197
 198
 199 double tetraVol(const vec3_t& x0, const vec3_t& x1, const vec3_t& x2, const vec3_t& x3, bool neg)
 200 {
 201   static double f16 = 1.0/6.0;
 202   vec3_t v1(x1[0]-x0[0], x1[1]-x0[1], x1[2]-x0[2]);
 203   vec3_t v2(x2[0]-x0[0], x2[1]-x0[1], x2[2]-x0[2]);
 204   vec3_t v3(x3[0]-x0[0], x3[1]-x0[1], x3[2]-x0[2]);
 205   double V = v1[0]*(v2[1]*v3[2]-v2[2]*v3[1]) + v1[1]*(v2[2]*v3[0]-v2[0]*v3[2]) + v1[2]*(v2[0]*v3[1]-v2[1]*v3[0]);
 206   V *= f16;
 207   if (!neg && (V < 0)) {
 208     V = -1e99;
 209   }
 210   return V; //fabs(V);
 211 }
 212
 213 double pyraVol(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3, vec3_t x4, bool neg)
 214 {
 215   double V1 = tetraVol(x0, x1, x3, x4, neg) + tetraVol(x1, x2, x3, x4, neg);
 216   double V2 = tetraVol(x0, x1, x2, x4, neg) + tetraVol(x2, x3, x0, x4, neg);
 217   return min(V1,V2);
 218   /*
 219   double V = 0;
 220   vec3_t m0 = .25*(x0+x1+x2+x3);
 221   V += tetraVol(x0, x1, m0, x4, neg);
 222   V += tetraVol(x1, x2, m0, x4, neg);
 223   V += tetraVol(x2, x3, m0, x4, neg);
 224   V += tetraVol(x3, x0, m0, x4, neg);
 225   return V;
 226   */
 227 }
 228
 229 double prismVol(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3, vec3_t x4, vec3_t x5, bool neg)
 230 {
 231   double V = 0;
 232   vec3_t p  = 1.0/6.0*(x0+x1+x2+x3+x4+x5);
 233   V += tetraVol(x0, x2, x1, p, neg);
 234   V += tetraVol(x3, x4, x5, p, neg);
 235   V += pyraVol (x0, x1, x4, x3, p, neg);
 236   V += pyraVol (x1, x2, x5, x4, p, neg);
 237   V += pyraVol (x0, x3, x5, x2, p, neg);
 238   return V;
 239 }
 240
 241 double hexaVol(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3, vec3_t x4, vec3_t x5, vec3_t x6, vec3_t x7, bool neg)
 242 {
 243   double V = 0;
 244   vec3_t p = 1.0/8.0*(x0+x1+x2+x3+x4+x5+x6+x7);
 245   V += pyraVol(x0, x1, x3, x2, p, neg);
 246   V += pyraVol(x0, x4, x5, x1, p, neg);
 247   V += pyraVol(x4, x6, x7, x5, p, neg);
 248   V += pyraVol(x2, x3, x7, x6, p, neg);
 249   V += pyraVol(x1, x5, x7, x3, p, neg);
 250   V += pyraVol(x0, x2, x6, x4, p, neg);
 251   return V;
 252 }
 253
 254 double triArea(vec3_t x0, vec3_t x1, vec3_t x2)
 255 {
 256   vec3_t a = x1-x0;
 257   vec3_t b = x2-x0;
 258   double A = 0.5*((a.cross(b)).abs());
 259   return A;
 260 }
 261
 262 double quadArea(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3)
 263 {
 264   double A = 0;
 265   vec3_t p = .25*(x0+x1+x2+x3);
 266   A += triArea(x0,x1,p);
 267   A += triArea(x1,x2,p);
 268   A += triArea(x2,x3,p);
 269   A += triArea(x3,x0,p);
 270   return A;
 271 }
 272
 273 vec3_t triNormal(vec3_t x0, vec3_t x1, vec3_t x2)
 274 {
 275   vec3_t a = x1-x0;
 276   vec3_t b = x2-x0;
 277   vec3_t n = 0.5*(a.cross(b));
 278   return n;
 279 }
 280
 281 vec3_t quadNormal(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3)
 282 {
 283   vec3_t n;
 284   clinit(n) = 0,0,0;
 285   vec3_t p = .25*(x0+x1+x2+x3);
 286   n += triNormal(x0,x1,p);
 287   n += triNormal(x1,x2,p);
 288   n += triNormal(x2,x3,p);
 289   n += triNormal(x3,x0,p);
 290   return n;
 291 }
 292
 293 vec3_t triNormal(vtkUnstructuredGrid *grid, vtkIdType p1, vtkIdType p2, vtkIdType p3)
 294 {
 295   vec3_t x1, x2, x3;
 296   grid->GetPoint(p1,x1.data());
 297   grid->GetPoint(p2,x2.data());
 298   grid->GetPoint(p3,x3.data());
 299   return triNormal(x1,x2,x3);
 300 }
 301
 302 vec3_t quadNormal(vtkUnstructuredGrid *grid, vtkIdType p1, vtkIdType p2, vtkIdType p3, vtkIdType p4)
 303 {
 304   vec3_t x1, x2, x3, x4;
 305   grid->GetPoint(p1,x1.data());
 306   grid->GetPoint(p2,x2.data());
 307   grid->GetPoint(p3,x3.data());
 308   grid->GetPoint(p4,x4.data());
 309   return quadNormal(x1,x2,x3,x4);
 310 }
 311
 312 vec3_t cellNormal(vtkUnstructuredGrid *grid, vtkIdType i)
 313 {
 314   vtkIdType *pts;
 315   vtkIdType npts;
 316   vec3_t n(0,0,0);
 317   grid->GetCellPoints(i, npts, pts);
 318   if (npts == 3) {
 319     return triNormal(grid,pts[0],pts[1],pts[2]);
 320   } else if (npts == 4) {
 321     return quadNormal(grid,pts[0],pts[1],pts[2],pts[3]);
 322   } else {
 323     EG_BUG;
 324   }
 325   return n;
 326 }
 327
 328 double cellVA(vtkUnstructuredGrid *grid, vtkIdType cellId, bool neg)
 329 {
 330   vtkIdType *pts;
 331   vtkIdType  Npts;
 332   vec3_t     p[8];
 333   grid->GetCellPoints(cellId, Npts, pts);
 334   for (int i_pts = 0; i_pts < Npts; ++i_pts) {
 335     grid->GetPoints()->GetPoint(pts[i_pts], p[i_pts].data());
 336   }
 337   vtkIdType cellType = grid->GetCellType(cellId);
 338   if      (cellType == VTK_TRIANGLE)   return triArea (p[0], p[1], p[2]);
 339   else if (cellType == VTK_QUAD)       return quadArea(p[0], p[1], p[2], p[3]);
 340   else if (cellType == VTK_TETRA)      return tetraVol(p[0], p[1], p[2], p[3], neg);
 341   else if (cellType == VTK_PYRAMID)    return pyraVol (p[0], p[1], p[2], p[3], p[4], neg);
 342   else if (cellType == VTK_WEDGE)      return prismVol(p[0], p[1], p[2], p[3], p[4], p[5], neg);
 343   else if (cellType == VTK_HEXAHEDRON) return hexaVol (p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], neg);
 344   return 0.0;
 345 }
 346
 347 double angle(const vec3_t & u, const vec3_t & v)
 348 {
 349   // return the angle w.r.t. another 3-vector
 350   double ptot2 = u.abs2()*v.abs2();
 351   if(ptot2 <= 0) {
 352       return 0.0;
 353   } else {
 354     double arg = (u*v)/sqrt(ptot2);
 355     if(arg >  1.0) arg =  1.0;
 356     if(arg < -1.0) arg = -1.0;
 357     return acos(arg);
 358   }
 359 }
 360
 361 double deviation(vtkUnstructuredGrid *grid, vtkIdType p1, vtkIdType p2, vtkIdType p3)
 362 {
 363   vec3_t x1, x2, x3;
 364   grid->GetPoint(p1,x1.data());
 365   grid->GetPoint(p2,x2.data());
 366   grid->GetPoint(p3,x3.data());
 367   vec3_t u=x2-x1;
 368   vec3_t v=x3-x2;
 369   return angle(u,v);
 370 }
 371
 372 double angle(vtkUnstructuredGrid *grid, vtkIdType p1, vtkIdType p2, vtkIdType p3)
 373 {
 374   vec3_t x1, x2, x3;
 375   grid->GetPoint(p1,x1.data());
 376   grid->GetPoint(p2,x2.data());
 377   grid->GetPoint(p3,x3.data());
 378   vec3_t u=x1-x2;
 379   vec3_t v=x3-x2;
 380   return angle(u,v);
 381 }
 382
 383 double CosAngle(vtkUnstructuredGrid *grid, vtkIdType cell1, vtkIdType cell2)
 384 {
 385   vec3_t u1 = cellNormal(grid, cell1);
 386   vec3_t u2 = cellNormal(grid, cell2);
 387   u1.normalise();
 388   u2.normalise();
 389   return(u1*u2);
 390 }
 391
 392 vec3_t getCenter(vtkUnstructuredGrid *grid, vtkIdType cellId, double& Rmin, double& Rmax)
 393 {
 394   vtkIdType *pts, Npts;
 395   grid->GetCellPoints(cellId, Npts, pts);
 396   if(Npts<=0) {
 397     cout<<"FATAL ERROR: Npts<=0"<<endl;
 398     abort();
 399   }
 400
 401   //calculate center position
 402   vec3_t xc(0,0,0);
 403   for (vtkIdType i = 0; i < Npts; ++i) {
 404     vec3_t xp;
 405     grid->GetPoints()->GetPoint(pts[i], xp.data());
 406     xc += xp;
 407   }
 408   xc = 1.0/(double)Npts * xc;
 409
 410   //calculate Rmin+Rmax
 411   vec3_t xp;
 412   grid->GetPoints()->GetPoint(pts[0], xp.data());
 413   Rmin = 0.25*(xp-xc).abs();
 414   Rmax = 0.25*(xp-xc).abs();
 415   for (vtkIdType i = 1; i < Npts; ++i) {
 416     grid->GetPoints()->GetPoint(pts[i], xp.data());
 417     Rmin = min(Rmin, 0.25*(xp-xc).abs());
 418     Rmax = max(Rmax, 0.25*(xp-xc).abs());
 419   }
 420
 421   return(xc);
 422 }
 423
 424 bool intersectEdgeAndTriangle(const vec3_t& a, const vec3_t& b, const vec3_t& c,
 425                               const vec3_t& x1, const vec3_t& x2, vec3_t& xi, vec3_t& ri, double tol)
 426 {
 427   // triangle base
 428   vec3_t g1 = b - a;
 429   vec3_t g2 = c - a;
 430   vec3_t g3 = g1.cross(g2);
 431   g3.normalise();
 432
 433   // direction of the edge
 434   vec3_t v = x2 - x1;
 435
 436   // parallel?
 437   if (fabs(g3*v) < 1e-6) {
 438     return false;
 439   }
 440
 441   // compute intersection between straight and triangular plane
 442   double k = intersection(x1, v, a, g3);
 443
 444   // intersection outside of edge range?
 445   if (k < 0 - tol*(x1-x2).abs()) {
 446     return false;
 447   }
 448   if (k > 1 + tol*(x1-x2).abs()) {
 449     return false;
 450   }
 451
 452   xi = x1 + k*v;
 453
 454   // transform xi to triangular base
 455   mat3_t G;
 456   G.column(0, g1);
 457   G.column(1, g2);
 458   G.column(2, g3);
 459   mat3_t GI = G.inverse();
 460   ri = xi - a;
 461   ri = GI*ri;
 462
 463   // intersection outside of triangle?
 464   if (ri[0] + ri[1] > 1) {
 465     return false;
 466   }
 467   if ((ri[0] < 0 - tol) || (ri[0] > 1 + tol)) {
 468     return false;
 469   }
 470   if ((ri[1] < 0 - tol) || (ri[1] > 1 + tol)) {
 471     return false;
 472   }
 473
 474   return true;
 475 }
 476
 477 double distance(vtkUnstructuredGrid *grid, vtkIdType id_node1, vtkIdType id_node2) {
 478   vec3_t A;
 479   vec3_t B;
 480   grid->GetPoints()->GetPoint(id_node1, A.data());
 481   grid->GetPoints()->GetPoint(id_node2, B.data());
 482   return((B-A).abs());
 483 }
 484
 485 double distance2(vtkUnstructuredGrid *grid, vtkIdType id_node1, vtkIdType id_node2) {
 486   vec3_t A;
 487   vec3_t B;
 488   grid->GetPoints()->GetPoint(id_node1, A.data());
 489   grid->GetPoints()->GetPoint(id_node2, B.data());
 490   return((B-A).abs2());
 491 }
 492
 493 double areaOfCircumscribedCircle(vtkUnstructuredGrid *grid, vtkIdType id_cell) {
 494   vtkIdType N_pts, *pts;
 495   grid->GetCellPoints(id_cell, N_pts, pts);
 496   vec3_t A,B,C;
 497   grid->GetPoints()->GetPoint(pts[0], A.data());
 498   grid->GetPoints()->GetPoint(pts[1], B.data());
 499   grid->GetPoints()->GetPoint(pts[2], C.data());
 500   double a=(C-B).abs();
 501   double alpha=angle((B-A),(C-A));
 502   double R=a/(2*sin(alpha));
 503   return(M_PI*R*R);
 504 }
 505
 506 vec3_t getBarycentricCoordinates(double x, double y)
 507 {
 508   double x_1=0;
 509   double y_1=0;
 510   double x_2=1;
 511   double y_2=0;
 512   double x_3=0;
 513   double y_3=1;
 514
 515   mat2_t T;
 516   T[0][0]=x_1-x_3; T[0][1]=x_2-x_3;
 517   T[1][0]=y_1-y_3; T[1][1]=y_2-y_3;
 518
 519   if(T.det()==0) {
 520     qWarning()<<"T.det()="<<T.det();
 521     qWarning()<<T[0][0]<<T[0][1];
 522     qWarning()<<T[1][0]<<T[1][1];
 523     qWarning()<<"T[0][0]*T[1][1]-T[1][0]*T[0][1]="<<T[0][0]*T[1][1]-T[1][0]*T[0][1];
 524     EG_BUG;
 525   }
 526
 527   double lambda_1 = ((y_2-y_3)*(x-x_3)-(x_2-x_3)*(y-y_3))/(T.det());
 528   double lambda_2 = (-(y_1-y_3)*(x-x_3)+(x_1-x_3)*(y-y_3))/(T.det());
 529   double lambda_3 = 1-lambda_1-lambda_2;
 530
 531   vec3_t bary_coords(lambda_1,lambda_2,lambda_3);
 532   return bary_coords;
 533
 534   // initialize
 535 /*  double t1=0;
 536   double t2=0;
 537   double t3=0;*/
 538
 539 /*  if(x==0) {
 540     t3=y;
 541     t1=1-y;
 542     t2=0;
 543   }
 544   else if(y==0) {
 545     t2=x;
 546     t1=1-x;
 547     t3=0;
 548   }
 549   else if((x+y)==1) {
 550
 551   }
 552   else {
 553   }
 554
 555   double k1,k2;
 556   if(!intersection (k1, k2, t_A, t_M-t_A, t_B, t_C-t_B)) EG_BUG;
 557   vec2_t t_I1 = t_A+k1*(t_M-t_A);
 558   vec3_t g_nI1 = (1-k2)*g_nB + k2*g_nC;
 559   vec2_t pm1_M(1.0/k1,0);
 560
 561   // normalize
 562   double total = t1+t2+t3;
 563   t1=t1/total;
 564   t2=t2/total;
 565   t3=t3/total;*/
 566
 567 /*  t2 = x;
 568   t3 = y;
 569   t1 = 1-t2-t3;*/
 570
 571   // return value
 572 //   vec3_t bary_coords(t1,t2,t3);
 573 //   return bary_coords;
 574 }
 575
 576 vec3_t intersectionOnPlane(vec3_t v, vec3_t A, vec3_t nA, vec3_t B, vec3_t nB)
 577 {
 578   vec3_t u = B-A;
 579 //   u.normalise();
 580   v.normalise();
 581   v = u.abs()*v;
 582
 583   //cout<<"u="<<u<<" v="<<v<<endl;
 584
 585   vec2_t p_A(0,0);
 586   vec2_t p_B(1,0);
 587   vec2_t p_nA = projectVectorOnPlane(nA,u,v);
 588   vec2_t p_nB = projectVectorOnPlane(nB,u,v);
 589
 590   vec2_t p_tA = turnRight(p_nA);
 591   vec2_t p_tB = turnRight(p_nB);
 592
 593   double k1, k2;
 594   vec2_t p_K;
 595   if(!intersection(k1, k2, p_A, p_tA, p_B, p_tB)) {
 596     //qDebug()<<"WARNING: No intersection found!!!";
 597     p_K = 0.5*(p_A + p_B);
 598   }
 599   else {
 600     p_K = p_A + k1*p_tA;
 601   }
 602
 603   //cout<<"nA="<<nA<<endl;
 604   //cout<<"p_nA="<<p_nA<<endl;
 605   //cout<<"p_tA="<<p_tA<<endl;
 606   //cout<<"p_K="<<p_K<<endl;
 607   vec3_t K = A + p_K[0]*u + p_K[1]*v;
 608   //cout<<"K="<<K<<endl;
 609   return K;
 610 }
 611
 612 vec2_t projectVectorOnPlane(vec3_t V,vec3_t i,vec3_t j)
 613 {
 614   double x = V*i/i.abs2();
 615   double y = V*j/j.abs2();
 616   return vec2_t(x,y);
 617 }
 618
 619 vec3_t projectPointOnEdge(const vec3_t& M,const vec3_t& A, const vec3_t& u)
 620 {
 621   checkVector(u);
 622   if(u.abs2()==0) EG_BUG;
 623   double k = ((M-A)*u)/u.abs2();
 624   return A + k*u;
 625 }
 626
 627 } // namespace