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
  26 #include <vtkCellType.h>
  27 #include <cmath>
  28
  29 namespace GeometryTools {
  30
  31 double rad2deg( double rad )
  32 {
  33   return rad/M_PI*180;
  34 }
  35
  36 double deg2rad( double deg )
  37 {
  38   return deg/180*M_PI;
  39 }
  40
  41 void rotate(vec3_t g1, vec3_t g2,vec3_t g3, vec3_t &b, double theta)
  42 {
  43   //cout << b << endl;
  44   mat3_t g_t;
  45   g_t[0] = g1;
  46   g_t[1] = g2;
  47   g_t[2] = g3;
  48   mat3_t g = g_t.transp();
  49   //cout << g << endl;
  50   vec3_t bb = g_t*b;
  51   //cout << bb << endl;
  52   mat3_t rot = mat3_t::identity();
  53   rot[0][0] = cos(theta);
  54   rot[0][1] = -sin(theta);
  55   rot[1][0] = sin(theta);
  56   rot[1][1] = cos(theta);
  57   //cout << rot << endl;
  58   vec3_t bbb = rot*bb;
  59   //cout << bbb << endl;
  60   b = g*bbb;
  61   //cout << b << endl;
  62 };
  63
  64 vec3_t rotate(vec3_t v, vec3_t axis, double theta)
  65 {
  66   axis.normalise();
  67
  68   // transposed base of rotate system
  69   mat3_t g_t;
  70
  71   // compute projection of v in axis direction
  72   vec3_t v_axis = (axis*v)*axis;
  73
  74   // compute first orthogonal vector (first base vector)
  75   g_t[0] = v-v_axis;
  76
  77   //In case of points on the rotation axis, do nothing
  78   if(g_t[0].abs()==0) return v;
  79
  80   g_t[0].normalise();
  81
  82   // second base vector is the normalised axis
  83   g_t[1] = axis;
  84
  85   // compute second orthogonal vector (third base vector)
  86   g_t[2] = g_t[0].cross(g_t[1]);
  87
  88   // base of rotate system
  89   mat3_t g = g_t.transp();
  90
  91   // matrix for rotation around g_t[1];
  92   mat3_t rot = mat3_t::identity();
  93   rot[0][0] = cos(theta);
  94   rot[0][2] = sin(theta);
  95   rot[2][0] = -sin(theta);
  96   rot[2][2] = cos(theta);
  97
  98   // transfer v to rotate system
  99   vec3_t v_r = g_t*v;
 100
 101   // rotate the vector and transfer it back
 102   v_r = rot*v_r;
 103   v = g*v_r;
 104
 105   return v;
 106 };
 107
 108 vec3_t orthogonalVector(vec3_t v)
 109 {
 110   v.normalise();
 111   vec3_t u = v;
 112   int i_min = 0;
 113   int i_max = 0;
 114   for (int i = 1; i < 3; ++i) {
 115     if (v[i] > v[i_max]) i_max = i;
 116     if (v[i] < v[i_min]) i_min = i;
 117   };
 118   double h = u[i_min];
 119   u[i_min] = u[i_max];
 120   u[i_max] = h;
 121   u -= (u*v)*v;
 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
 198 double tetraVol(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3, bool neg)
 199 {
 200   vec3_t v1 = x1-x0;
 201   vec3_t v2 = x2-x0;
 202   vec3_t v3 = x3-x0;
 203   double V = v1*(v2.cross(v3));
 204   V /= 6.0;
 205   if (!neg && (V < 0)) {
 206     vec3_t v4 = x2-x1;
 207     vec3_t v5 = x3-x1;
 208     vec3_t v6 = x3-x2;
 209     double Lmin = 1e99;
 210     double Lmax = 0;
 211     Lmin = min(Lmin,v1.abs());
 212     Lmin = min(Lmin,v2.abs());
 213     Lmin = min(Lmin,v3.abs());
 214     Lmin = min(Lmin,v4.abs());
 215     Lmin = min(Lmin,v5.abs());
 216     Lmin = min(Lmin,v6.abs());
 217     Lmax = max(Lmax,v1.abs());
 218     Lmax = max(Lmax,v2.abs());
 219     Lmax = max(Lmax,v3.abs());
 220     Lmax = max(Lmax,v4.abs());
 221     Lmax = max(Lmax,v5.abs());
 222     Lmax = max(Lmax,v6.abs());
 223     if (Lmin/Lmax < 1e-6) {
 224       V = 0;
 225       V = -1e99;
 226     } else {
 227       V = -1e99;
 228     };
 229     //V *= 1e6;
 230   };
 231   return V; //fabs(V);
 232 };
 233
 234 double pyraVol(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3, vec3_t x4, bool neg)
 235 {
 236   double V1 = tetraVol(x0, x1, x3, x4, neg) + tetraVol(x1, x2, x3, x4, neg);
 237   double V2 = tetraVol(x0, x1, x2, x4, neg) + tetraVol(x2, x3, x0, x4, neg);
 238   return min(V1,V2);
 239   /*
 240   double V = 0;
 241   vec3_t m0 = .25*(x0+x1+x2+x3);
 242   V += tetraVol(x0, x1, m0, x4, neg);
 243   V += tetraVol(x1, x2, m0, x4, neg);
 244   V += tetraVol(x2, x3, m0, x4, neg);
 245   V += tetraVol(x3, x0, m0, x4, neg);
 246   return V;
 247   */
 248 };
 249
 250 double prismVol(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3, vec3_t x4, vec3_t x5, bool neg)
 251 {
 252   double V = 0;
 253   vec3_t p  = 1.0/6.0*(x0+x1+x2+x3+x4+x5);
 254   V += tetraVol(x0, x2, x1, p, neg);
 255   V += tetraVol(x3, x4, x5, p, neg);
 256   V += pyraVol (x0, x1, x4, x3, p, neg);
 257   V += pyraVol (x1, x2, x5, x4, p, neg);
 258   V += pyraVol (x0, x3, x5, x2, p, neg);
 259   return V;
 260 };
 261
 262 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)
 263 {
 264   double V = 0;
 265   vec3_t p = 1.0/8.0*(x0+x1+x2+x3+x4+x5+x6+x7);
 266   V += pyraVol(x0, x1, x3, x2, p, neg);
 267   V += pyraVol(x0, x4, x5, x1, p, neg);
 268   V += pyraVol(x4, x6, x7, x5, p, neg);
 269   V += pyraVol(x2, x3, x7, x6, p, neg);
 270   V += pyraVol(x1, x5, x7, x3, p, neg);
 271   V += pyraVol(x0, x2, x6, x4, p, neg);
 272   return V;
 273 };
 274
 275 double triArea(vec3_t x0, vec3_t x1, vec3_t x2)
 276 {
 277   vec3_t a = x1-x0;
 278   vec3_t b = x2-x0;
 279   double A = 0.5*((a.cross(b)).abs());
 280   return A;
 281 };
 282
 283 double quadArea(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3)
 284 {
 285   double A = 0;
 286   vec3_t p = .25*(x0+x1+x2+x3);
 287   A += triArea(x0,x1,p);
 288   A += triArea(x1,x2,p);
 289   A += triArea(x2,x3,p);
 290   A += triArea(x3,x0,p);
 291   return A;
 292 };
 293
 294 vec3_t triNormal(vec3_t x0, vec3_t x1, vec3_t x2)
 295 {
 296   vec3_t a = x1-x0;
 297   vec3_t b = x2-x0;
 298   vec3_t n = 0.5*(a.cross(b));
 299   return n;
 300 };
 301
 302 vec3_t quadNormal(vec3_t x0, vec3_t x1, vec3_t x2, vec3_t x3)
 303 {
 304   vec3_t n;
 305   clinit(n) = 0,0,0;
 306   vec3_t p = .25*(x0+x1+x2+x3);
 307   n += triNormal(x0,x1,p);
 308   n += triNormal(x1,x2,p);
 309   n += triNormal(x2,x3,p);
 310   n += triNormal(x3,x0,p);
 311   return n;
 312 };
 313
 314 vec3_t triNormal(vtkUnstructuredGrid *grid, vtkIdType p1, vtkIdType p2, vtkIdType p3)
 315 {
 316   vec3_t x1, x2, x3;
 317   grid->GetPoint(p1,x1.data());
 318   grid->GetPoint(p2,x2.data());
 319   grid->GetPoint(p3,x3.data());
 320   return triNormal(x1,x2,x3);
 321 };
 322
 323 vec3_t quadNormal(vtkUnstructuredGrid *grid, vtkIdType p1, vtkIdType p2, vtkIdType p3, vtkIdType p4)
 324 {
 325   vec3_t x1, x2, x3, x4;
 326   grid->GetPoint(p1,x1.data());
 327   grid->GetPoint(p2,x2.data());
 328   grid->GetPoint(p3,x3.data());
 329   grid->GetPoint(p4,x4.data());
 330   return quadNormal(x1,x2,x3,x4);
 331 };
 332
 333 vec3_t cellNormal(vtkUnstructuredGrid *grid, vtkIdType i)
 334 {
 335   vtkIdType *pts;
 336   vtkIdType npts;
 337   vec3_t n;
 338   clinit(n) = 0,0,0;
 339   grid->GetCellPoints(i,npts,pts);
 340   if      (npts == 3) return triNormal(grid,pts[0],pts[1],pts[2]);
 341   else if (npts == 4) return quadNormal(grid,pts[0],pts[1],pts[2],pts[3]);
 342   return n;
 343 };
 344
 345 double cellVA(vtkUnstructuredGrid *grid, vtkIdType cellId, bool neg)
 346 {
 347   vtkIdType *pts;
 348   vtkIdType  Npts;
 349   vec3_t     p[8];
 350   grid->GetCellPoints(cellId, Npts, pts);
 351   for (int i_pts = 0; i_pts < Npts; ++i_pts) {
 352     grid->GetPoints()->GetPoint(pts[i_pts], p[i_pts].data());
 353   };
 354   vtkIdType cellType = grid->GetCellType(cellId);
 355   if      (cellType == VTK_TRIANGLE)   return triArea (p[0], p[1], p[2]);
 356   else if (cellType == VTK_QUAD)       return quadArea(p[0], p[1], p[2], p[3]);
 357   else if (cellType == VTK_TETRA)      return tetraVol(p[0], p[1], p[2], p[3], neg);
 358   else if (cellType == VTK_PYRAMID)    return pyraVol (p[0], p[1], p[2], p[3], p[4], neg);
 359   else if (cellType == VTK_WEDGE)      return prismVol(p[0], p[1], p[2], p[3], p[4], p[5], neg);
 360   else if (cellType == VTK_HEXAHEDRON) return hexaVol (p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], neg);
 361   return 0.0;
 362 };
 363
 364 double angle(const vec3_t & u, const vec3_t & v)
 365 {
 366   // return the angle w.r.t. another 3-vector
 367   double ptot2 = u.abs2()*v.abs2();
 368   if(ptot2 <= 0) {
 369       return 0.0;
 370   } else {
 371     double arg = (u*v)/sqrt(ptot2);
 372     if(arg >  1.0) arg =  1.0;
 373     if(arg < -1.0) arg = -1.0;
 374     return acos(arg);
 375   }
 376 }
 377
 378 } // namespace
 379
 380
 381
 382