src/libengrid/geometrytools.cpp

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