src/INTERP_KERNEL/VolSurfFormulae.hxx

   1 // Copyright (C) 2007-2012  CEA/DEN, EDF R&D
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19 // Author : Anthony Geay (CEA/DEN)
  20
  21 #ifndef __VOLSURFFORMULAE_HXX__
  22 #define __VOLSURFFORMULAE_HXX__
  23
  24 #include "InterpolationUtils.hxx"
  25 #include "InterpKernelException.hxx"
  26 #include "InterpKernelGeo2DQuadraticPolygon.hxx"
  27
  28 #include <sstream>
  29 #include <cmath>
  30
  31 namespace INTERP_KERNEL
  32 {
  33   inline void calculateBarycenterDyn(const double **pts, int nbPts,
  34                                      int dim, double *bary);
  35
  36   inline double calculateAreaForPolyg(const double **coords, int nbOfPtsInPolygs,
  37                                       int spaceDim);
  38
  39
  40   inline double calculateAreaForQPolyg(const double **coords, int nbOfPtsInPolygs,
  41                                        int spaceDim);
  42
  43   inline double calculateLgthForSeg2(const double *p1, const double *p2, int spaceDim)
  44   {
  45     if(spaceDim==1)
  46       return *p2-*p1;
  47     else
  48       {
  49         double ret=0;
  50         for(int i=0;i<spaceDim;i++)
  51           ret+=(p2[i]-p1[i])*(p2[i]-p1[i]);
  52         return sqrt(ret);
  53       }
  54   }
  55
  56   // ===========================
  57   // Calculate Area for triangle
  58   // ===========================
  59   inline double calculateAreaForTria(const double *p1, const double *p2,
  60                                      const double *p3, int spaceDim)
  61   {
  62     double area ;
  63
  64     if ( spaceDim == 2 )
  65       {
  66         area = -((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1]))/2.0;
  67       }
  68     else
  69       {
  70         area = sqrt(((p2[1]-p1[1])*(p3[2]-p1[2]) - (p3[1]-p1[1])*(p2[2]-p1[2]))*
  71                     ((p2[1]-p1[1])*(p3[2]-p1[2]) - (p3[1]-p1[1])*(p2[2]-p1[2]))
  72                     +
  73                     ((p3[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p3[2]-p1[2]))*
  74                     ((p3[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p3[2]-p1[2]))
  75                     +
  76                     ((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1]))*
  77                     ((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1])))/2.0;
  78       }
  79
  80     return area ;
  81   }
  82
  83   // =============================
  84   // Calculate Area for quadrangle
  85   // =============================
  86   inline double calculateAreaForQuad(const double *p1, const double *p2,
  87                                      const double *p3, const double *p4,
  88                                      int spaceDim)
  89   {
  90     double area ;
  91
  92     if (spaceDim==2)
  93       {
  94         double a1 = (p2[0]-p1[0])/4.0, a2 = (p2[1]-p1[1])/4.0;
  95         double b1 = (p3[0]-p4[0])/4.0, b2 = (p3[1]-p4[1])/4.0;
  96         double c1 = (p3[0]-p2[0])/4.0, c2 = (p3[1]-p2[1])/4.0;
  97         double d1 = (p4[0]-p1[0])/4.0, d2 = (p4[1]-p1[1])/4.0;
  98
  99         area = - 4.0*(  b1*c2 - c1*b2 + a1*c2 - c1*a2
 100                         + b1*d2 - d1*b2 + a1*d2 - d1*a2);
 101       }
 102     else
 103       {
 104         area = (sqrt(((p2[1]-p1[1])*(p4[2]-p1[2]) - (p4[1]-p1[1])*(p2[2]-p1[2]))*
 105                      ((p2[1]-p1[1])*(p4[2]-p1[2]) - (p4[1]-p1[1])*(p2[2]-p1[2]))
 106                      + ((p4[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p4[2]-p1[2]))*
 107                      ((p4[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p4[2]-p1[2]))
 108                      + ((p2[0]-p1[0])*(p4[1]-p1[1]) - (p4[0]-p1[0])*(p2[1]-p1[1]))*
 109                      ((p2[0]-p1[0])*(p4[1]-p1[1]) - (p4[0]-p1[0])*(p2[1]-p1[1])))
 110                 +
 111                 sqrt(((p4[1]-p3[1])*(p2[2]-p3[2]) - (p2[1]-p3[1])*(p4[2]-p3[2]))*
 112                      ((p4[1]-p3[1])*(p2[2]-p3[2]) - (p2[1]-p3[1])*(p4[2]-p3[2]))
 113                      + ((p2[0]-p3[0])*(p4[2]-p3[2]) - (p4[0]-p3[0])*(p2[2]-p3[2]))*
 114                      ((p2[0]-p3[0])*(p4[2]-p3[2]) - (p4[0]-p3[0])*(p2[2]-p3[2]))
 115                      + ((p4[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p4[1]-p3[1]))*
 116                      ((p4[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p4[1]-p3[1])))
 117                 )/2.0;
 118       }
 119
 120     return area ;
 121   }
 122
 123   // ====================================
 124   // Calculate Normal Vector for Triangle
 125   // ====================================
 126   inline void calculateNormalForTria(const double *p1, const double *p2,
 127                                      const double *p3, double *normal)
 128   {
 129     normal[0] = ((p2[1]-p1[1])*(p3[2]-p1[2]) - (p3[1]-p1[1])*(p2[2]-p1[2]))/2.0;
 130     normal[1] = ((p3[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p3[2]-p1[2]))/2.0;
 131     normal[2] = ((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1]))/2.0;
 132   }
 133
 134   // ======================================
 135   // Calculate Normal Vector for Quadrangle
 136   // ======================================
 137   inline void calculateNormalForQuad(const double *p1, const double *p2,
 138                                      const double *p3, const double *p4,
 139                                      double *normal)
 140   {
 141     double xnormal1 = (p2[1]-p1[1])*(p4[2]-p1[2]) - (p4[1]-p1[1])*(p2[2]-p1[2]);
 142     double xnormal2 = (p4[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p4[2]-p1[2]);
 143     double xnormal3 = (p2[0]-p1[0])*(p4[1]-p1[1]) - (p4[0]-p1[0])*(p2[1]-p1[1]);
 144     double xarea = sqrt(xnormal1*xnormal1 + xnormal2*xnormal2 + xnormal3*xnormal3);
 145     xnormal1 = xnormal1/xarea;
 146     xnormal2 = xnormal2/xarea;
 147     xnormal3 = xnormal3/xarea;
 148     xarea = calculateAreaForQuad(p1,p2,p3,p4,3);
 149     normal[0] = xnormal1*xarea ;
 150     normal[1] = xnormal2*xarea ;
 151     normal[2] = xnormal3*xarea ;
 152   }
 153
 154   // ===================================
 155   // Calculate Normal Vector for Polygon
 156   // ===================================
 157   inline void calculateNormalForPolyg(const double **coords, int nbOfPtsInPolygs,
 158                                       double *normal)
 159   {
 160     double coordOfBary[3];
 161
 162     calculateBarycenterDyn(coords,nbOfPtsInPolygs,3,coordOfBary);
 163     double xnormal1 = (coords[0][1]-coords[1][1]) * (coordOfBary[2]-coords[1][2])
 164       - (coords[0][2]-coords[1][2]) * (coordOfBary[1]-coords[1][1]);
 165
 166     double xnormal2 = (coords[0][2]-coords[1][2]) * (coordOfBary[0]-coords[1][0])
 167       - (coords[0][0]-coords[1][0]) * (coordOfBary[2]-coords[1][2]);
 168
 169     double xnormal3 = (coords[0][0]-coords[1][0]) * (coordOfBary[1]-coords[1][1])
 170       - (coords[0][1]-coords[1][1]) * (coordOfBary[0]-coords[1][0]);
 171
 172     double xarea=sqrt(xnormal1*xnormal1 + xnormal2*xnormal2 + xnormal3*xnormal3);
 173
 174     if ( xarea < 1.e-6 )
 175       {
 176         //std::string diagnosis"Can not calculate normal - polygon is singular";
 177         throw std::exception();
 178       }
 179
 180     xnormal1 = -xnormal1/xarea;
 181     xnormal2 = -xnormal2/xarea;
 182     xnormal3 = -xnormal3/xarea;
 183     xarea = calculateAreaForPolyg(coords,nbOfPtsInPolygs,3);
 184     normal[0] = xnormal1*xarea ;
 185     normal[1] = xnormal2*xarea ;
 186     normal[2] = xnormal3*xarea ;
 187   }
 188
 189   // ==========================
 190   // Calculate Area for Polygon
 191   // ==========================
 192   inline double calculateAreaForPolyg(const double **coords, int nbOfPtsInPolygs,
 193                                       int spaceDim)
 194   {
 195     double ret=0.;
 196     double coordOfBary[3];
 197
 198     calculateBarycenterDyn(coords,nbOfPtsInPolygs,spaceDim,coordOfBary);
 199     for ( int i=0; i<nbOfPtsInPolygs; i++ )
 200       {
 201         double tmp = calculateAreaForTria(coords[i],coords[(i+1)%nbOfPtsInPolygs],
 202                                           coordOfBary,spaceDim);
 203         ret+=tmp;
 204       }
 205     return ret;
 206   }
 207
 208   double calculateAreaForQPolyg(const double **coords, int nbOfPtsInPolygs, int spaceDim)
 209   {
 210
 211     if(nbOfPtsInPolygs%2==0)
 212       {
 213         if(spaceDim==2)
 214           {
 215             std::vector<Node *> nodes(nbOfPtsInPolygs);
 216             for(int i=0;i<nbOfPtsInPolygs;i++)
 217               nodes[i]=new Node(coords[i][0],coords[i][1]);
 218             QuadraticPolygon *pol=QuadraticPolygon::BuildArcCirclePolygon(nodes);
 219             double ret=pol->getArea();
 220             delete pol;
 221             return -ret;
 222           }
 223         else
 224           return calculateAreaForPolyg(coords,nbOfPtsInPolygs/2,spaceDim);
 225       }
 226     else
 227       {
 228         std::ostringstream oss; oss << "INTERP_KERNEL::calculateAreaForQPolyg : nb of points in quadratic polygon is " << nbOfPtsInPolygs << " should be even !";
 229         throw INTERP_KERNEL::Exception(oss.str().c_str());
 230       }
 231   }
 232
 233   // ==========================
 234   // Calculate Volume for Tetra
 235   // ==========================
 236   inline double calculateVolumeForTetra(const double *p1, const double *p2,
 237                                         const double *p3, const double *p4)
 238   {
 239     return (  (p3[0]-p1[0])*(  (p2[1]-p1[1])*(p4[2]-p1[2])
 240                                - (p2[2]-p1[2])*(p4[1]-p1[1]) )
 241               - (p2[0]-p1[0])*(  (p3[1]-p1[1])*(p4[2]-p1[2])
 242                                  - (p3[2]-p1[2])*(p4[1]-p1[1]) )
 243               + (p4[0]-p1[0])*(  (p3[1]-p1[1])*(p2[2]-p1[2])
 244                                  - (p3[2]-p1[2])*(p2[1]-p1[1]) )
 245               ) / 6.0 ;
 246   }
 247
 248   // =========================
 249   // Calculate Volume for Pyra
 250   // =========================
 251   inline double calculateVolumeForPyra(const double *p1, const double *p2,
 252                                        const double *p3, const double *p4,
 253                                        const double *p5)
 254   {
 255     return ( ((p3[0]-p1[0])*(  (p2[1]-p1[1])*(p5[2]-p1[2])
 256                                - (p2[2]-p1[2])*(p5[1]-p1[1]) )
 257               -(p2[0]-p1[0])*(  (p3[1]-p1[1])*(p5[2]-p1[2])
 258                                 - (p3[2]-p1[2])*(p5[1]-p1[1]) )
 259               +(p5[0]-p1[0])*(  (p3[1]-p1[1])*(p2[2]-p1[2])
 260                                 - (p3[2]-p1[2])*(p2[1]-p1[1]) ))
 261              +
 262              ((p4[0]-p1[0])*(  (p3[1]-p1[1])*(p5[2]-p1[2])
 263                                - (p3[2]-p1[2])*(p5[1]-p1[1]) )
 264               -(p3[0]-p1[0])*(  (p4[1]-p1[1])*(p5[2]-p1[2])
 265                                 - (p4[2]-p1[2])*(p5[1]-p1[1]))
 266               +(p5[0]-p1[0])*(  (p4[1]-p1[1])*(p3[2]-p1[2])
 267                                 - (p4[2]-p1[2])*(p3[1]-p1[1]) ))
 268              ) / 6.0 ;
 269   }
 270
 271   // ==========================
 272   // Calculate Volume for Penta
 273   // ==========================
 274   inline double calculateVolumeForPenta(const double *p1, const double *p2,
 275                                         const double *p3, const double *p4,
 276                                         const double *p5, const double *p6)
 277   {
 278     double a1 = (p2[0]-p3[0])/2.0, a2 = (p2[1]-p3[1])/2.0, a3 = (p2[2]-p3[2])/2.0;
 279     double b1 = (p5[0]-p6[0])/2.0, b2 = (p5[1]-p6[1])/2.0, b3 = (p5[2]-p6[2])/2.0;
 280     double c1 = (p4[0]-p1[0])/2.0, c2 = (p4[1]-p1[1])/2.0, c3 = (p4[2]-p1[2])/2.0;
 281     double d1 = (p5[0]-p2[0])/2.0, d2 = (p5[1]-p2[1])/2.0, d3 = (p5[2]-p2[2])/2.0;
 282     double e1 = (p6[0]-p3[0])/2.0, e2 = (p6[1]-p3[1])/2.0, e3 = (p6[2]-p3[2])/2.0;
 283     double f1 = (p1[0]-p3[0])/2.0, f2 = (p1[1]-p3[1])/2.0, f3 = (p1[2]-p3[2])/2.0;
 284     double h1 = (p4[0]-p6[0])/2.0, h2 = (p4[1]-p6[1])/2.0, h3 = (p4[2]-p6[2])/2.0;
 285
 286     double A = a1*c2*f3 - a1*c3*f2 - a2*c1*f3 + a2*c3*f1 + a3*c1*f2 - a3*c2*f1;
 287     double B = b1*c2*h3 - b1*c3*h2 - b2*c1*h3 + b2*c3*h1 + b3*c1*h2 - b3*c2*h1;
 288     double C = (a1*c2*h3 + b1*c2*f3) - (a1*c3*h2 + b1*c3*f2)
 289       - (a2*c1*h3 + b2*c1*f3) + (a2*c3*h1 + b2*c3*f1)
 290       + (a3*c1*h2 + b3*c1*f2) - (a3*c2*h1 + b3*c2*f1);
 291     double D = a1*d2*f3 - a1*d3*f2 - a2*d1*f3 + a2*d3*f1 + a3*d1*f2 - a3*d2*f1;
 292     double E = b1*d2*h3 - b1*d3*h2 - b2*d1*h3 + b2*d3*h1 + b3*d1*h2 - b3*d2*h1;
 293     double F = (a1*d2*h3 + b1*d2*f3) - (a1*d3*h2 + b1*d3*f2)
 294       - (a2*d1*h3 + b2*d1*f3) + (a2*d3*h1 + b2*d3*f1)
 295       + (a3*d1*h2 + b3*d1*f2) - (a3*d2*h1 + b3*d2*f1);
 296     double G = a1*e2*f3 - a1*e3*f2 - a2*e1*f3 + a2*e3*f1 + a3*e1*f2 - a3*e2*f1;
 297     double H = b1*e2*h3 - b1*e3*h2 - b2*e1*h3 + b2*e3*h1 + b3*e1*h2 - b3*e2*h1;
 298     double P = (a1*e2*h3 + b1*e2*f3) - (a1*e3*h2 + b1*e3*f2)
 299       - (a2*e1*h3 + b2*e1*f3) + (a2*e3*h1 + b2*e3*f1)
 300       + (a3*e1*h2 + b3*e1*f2) - (a3*e2*h1 + b3*e2*f1);
 301
 302     return (-2.0*(2.0*(A + B + D + E + G + H) + C + F + P)/9.0);
 303   }
 304
 305   // =========================
 306   // Calculate Volume for Hexa
 307   // =========================
 308   inline double calculateVolumeForHexa(const double *pt1, const double *pt2,
 309                                        const double *pt3, const double *pt4,
 310                                        const double *pt5, const double *pt6,
 311                                        const double *pt7, const double *pt8)
 312   {
 313     double a1=(pt3[0]-pt4[0])/8.0, a2=(pt3[1]-pt4[1])/8.0, a3=(pt3[2]-pt4[2])/8.0;
 314     double b1=(pt2[0]-pt1[0])/8.0, b2=(pt2[1]-pt1[1])/8.0, b3=(pt2[2]-pt1[2])/8.0;
 315     double c1=(pt7[0]-pt8[0])/8.0, c2=(pt7[1]-pt8[1])/8.0, c3=(pt7[2]-pt8[2])/8.0;
 316     double d1=(pt6[0]-pt5[0])/8.0, d2=(pt6[1]-pt5[1])/8.0, d3=(pt6[2]-pt5[2])/8.0;
 317     double e1=(pt3[0]-pt2[0])/8.0, e2=(pt3[1]-pt2[1])/8.0, e3=(pt3[2]-pt2[2])/8.0;
 318     double f1=(pt4[0]-pt1[0])/8.0, f2=(pt4[1]-pt1[1])/8.0, f3=(pt4[2]-pt1[2])/8.0;
 319     double h1=(pt7[0]-pt6[0])/8.0, h2=(pt7[1]-pt6[1])/8.0, h3=(pt7[2]-pt6[2])/8.0;
 320     double p1=(pt8[0]-pt5[0])/8.0, p2=(pt8[1]-pt5[1])/8.0, p3=(pt8[2]-pt5[2])/8.0;
 321     double q1=(pt3[0]-pt7[0])/8.0, q2=(pt3[1]-pt7[1])/8.0, q3=(pt3[2]-pt7[2])/8.0;
 322     double r1=(pt4[0]-pt8[0])/8.0, r2=(pt4[1]-pt8[1])/8.0, r3=(pt4[2]-pt8[2])/8.0;
 323     double s1=(pt2[0]-pt6[0])/8.0, s2=(pt2[1]-pt6[1])/8.0, s3=(pt2[2]-pt6[2])/8.0;
 324     double t1=(pt1[0]-pt5[0])/8.0, t2=(pt1[1]-pt5[1])/8.0, t3=(pt1[2]-pt5[2])/8.0;
 325
 326     double A = a1*e2*q3 - a1*e3*q2 - a2*e1*q3 + a2*e3*q1 + a3*e1*q2 - a3*e2*q1;
 327     double B = c1*h2*q3 - c1*h3*q2 - c2*h1*q3 + c2*h3*q1 + c3*h1*q2 - c3*h2*q1;
 328     double C = (a1*h2 + c1*e2)*q3 - (a1*h3 + c1*e3)*q2
 329       - (a2*h1 + c2*e1)*q3 + (a2*h3 + c2*e3)*q1
 330       + (a3*h1 + c3*e1)*q2 - (a3*h2 + c3*e2)*q1;
 331     double D = b1*e2*s3 - b1*e3*s2 - b2*e1*s3 + b2*e3*s1 + b3*e1*s2 - b3*e2*s1;
 332     double E = d1*h2*s3 - d1*h3*s2 - d2*h1*s3 + d2*h3*s1 + d3*h1*s2 - d3*h2*s1;
 333     double F = (b1*h2 + d1*e2)*s3 - (b1*h3 + d1*e3)*s2
 334       - (b2*h1 + d2*e1)*s3 + (b2*h3 + d2*e3)*s1
 335       + (b3*h1 + d3*e1)*s2 - (b3*h2 + d3*e2)*s1;
 336     double G = (a1*e2*s3 + b1*e2*q3) - (a1*e3*s2 + b1*e3*q2)
 337       - (a2*e1*s3 + b2*e1*q3) + (a2*e3*s1 + b2*e3*q1)
 338       + (a3*e1*s2 + b3*e1*q2) - (a3*e2*s1 + b3*e2*q1);
 339     double H = (c1*h2*s3 + d1*h2*q3) - (c1*h3*s2 + d1*h3*q2)
 340       - (c2*h1*s3 + d2*h1*q3) + (c2*h3*s1 + d2*h3*q1)
 341       + (c3*h1*s2 + d3*h1*q2) - (c3*h2*s1 + d3*h2*q1);
 342     double I = ((a1*h2 + c1*e2)*s3 + (b1*h2 + d1*e2)*q3)
 343       - ((a1*h3 + c1*e3)*s2 + (b1*h3 + d1*e3)*q2)
 344       - ((a2*h1 + c2*e1)*s3 + (b2*h1 + d2*e1)*q3)
 345       + ((a2*h3 + c2*e3)*s1 + (b2*h3 + d2*e3)*q1)
 346       + ((a3*h1 + c3*e1)*s2 + (b3*h1 + d3*e1)*q2)
 347       - ((a3*h2 + c3*e2)*s1 + (b3*h2 + d3*e2)*q1);
 348     double J = a1*f2*r3 - a1*f3*r2 - a2*f1*r3 + a2*f3*r1 + a3*f1*r2 - a3*f2*r1;
 349     double K = c1*p2*r3 - c1*p3*r2 - c2*p1*r3 + c2*p3*r1 + c3*p1*r2 - c3*p2*r1;
 350     double L = (a1*p2 + c1*f2)*r3 - (a1*p3 + c1*f3)*r2
 351       - (a2*p1 + c2*f1)*r3 + (a2*p3 + c2*f3)*r1
 352       + (a3*p1 + c3*f1)*r2 - (a3*p2 + c3*f2)*r1;
 353     double M = b1*f2*t3 - b1*f3*t2 - b2*f1*t3 + b2*f3*t1 + b3*f1*t2 - b3*f2*t1;
 354     double N = d1*p2*t3 - d1*p3*t2 - d2*p1*t3 + d2*p3*t1 + d3*p1*t2 - d3*p2*t1;
 355     double O = (b1*p2 + d1*f2)*t3 - (b1*p3 + d1*f3)*t2
 356       - (b2*p1 + d2*f1)*t3 + (b2*p3 + d2*f3)*t1
 357       + (b3*p1 + d3*f1)*t2 - (b3*p2 + d3*f2)*t1;
 358     double P = (a1*f2*t3 + b1*f2*r3) - (a1*f3*t2 + b1*f3*r2)
 359       - (a2*f1*t3 + b2*f1*r3) + (a2*f3*t1 + b2*f3*r1)
 360       + (a3*f1*t2 + b3*f1*r2) - (a3*f2*t1 + b3*f2*r1);
 361     double Q = (c1*p2*t3 + d1*p2*r3) - (c1*p3*t2 + d1*p3*r2)
 362       - (c2*p1*t3 + d2*p1*r3) + (c2*p3*t1 + d2*p3*r1)
 363       + (c3*p1*t2 + d3*p1*r2) - (c3*p2*t1 + d3*p2*r1);
 364     double R = ((a1*p2 + c1*f2)*t3 + (b1*p2 + d1*f2)*r3)
 365       - ((a1*p3 + c1*f3)*t2 + (b1*p3 + d1*f3)*r2)
 366       - ((a2*p1 + c2*f1)*t3 + (b2*p1 + d2*f1)*r3)
 367       + ((a2*p3 + c2*f3)*t1 + (b2*p3 + d2*f3)*r1)
 368       + ((a3*p1 + c3*f1)*t2 + (b3*p1 + d3*f1)*r2)
 369       - ((a3*p2 + c3*f2)*t1 + (b3*p2 + d3*f2)*r1);
 370     double S = (a1*e2*r3 + a1*f2*q3) - (a1*e3*r2 + a1*f3*q2)
 371       - (a2*e1*r3 + a2*f1*q3) + (a2*e3*r1 + a2*f3*q1)
 372       + (a3*e1*r2 + a3*f1*q2) - (a3*e2*r1 + a3*f2*q1);
 373     double T = (c1*h2*r3 + c1*p2*q3) - (c1*h3*r2 + c1*p3*q2)
 374       - (c2*h1*r3 + c2*p1*q3) + (c2*h3*r1 + c2*p3*q1)
 375       + (c3*h1*r2 + c3*p1*q2) - (c3*h2*r1 + c3*p2*q1);
 376     double U = ((a1*h2 + c1*e2)*r3 + (a1*p2 + c1*f2)*q3)
 377       - ((a1*h3 + c1*e3)*r2 + (a1*p3 + c1*f3)*q2)
 378       - ((a2*h1 + c2*e1)*r3 + (a2*p1 + c2*f1)*q3)
 379       + ((a2*h3 + c2*e3)*r1 + (a2*p3 + c2*f3)*q1)
 380       + ((a3*h1 + c3*e1)*r2 + (a3*p1 + c3*f1)*q2)
 381       - ((a3*h2 + c3*e2)*r1 + (a3*p2 + c3*f2)*q1);
 382     double V = (b1*e2*t3 + b1*f2*s3) - (b1*e3*t2 + b1*f3*s2)
 383       - (b2*e1*t3 + b2*f1*s3) + (b2*e3*t1 + b2*f3*s1)
 384       + (b3*e1*t2 + b3*f1*s2) - (b3*e2*t1 + b3*f2*s1);
 385     double W = (d1*h2*t3 + d1*p2*s3) - (d1*h3*t2 + d1*p3*s2)
 386       - (d2*h1*t3 + d2*p1*s3) + (d2*h3*t1 + d2*p3*s1)
 387       + (d3*h1*t2 + d3*p1*s2) - (d3*h2*t1 + d3*p2*s1);
 388     double X = ((b1*h2 + d1*e2)*t3 + (b1*p2 + d1*f2)*s3)
 389       - ((b1*h3 + d1*e3)*t2 + (b1*p3 + d1*f3)*s2)
 390       - ((b2*h1 + d2*e1)*t3 + (b2*p1 + d2*f1)*s3)
 391       + ((b2*h3 + d2*e3)*t1 + (b2*p3 + d2*f3)*s1)
 392       + ((b3*h1 + d3*e1)*t2 + (b3*p1 + d3*f1)*s2)
 393       - ((b3*h2 + d3*e2)*t1 + (b3*p2 + d3*f2)*s1);
 394     double Y = (a1*e2*t3 + a1*f2*s3 + b1*e2*r3 + b1*f2*q3)
 395       - (a1*e3*t2 + a1*f3*s2 + b1*e3*r2 + b1*f3*q2)
 396       - (a2*e1*t3 + a2*f1*s3 + b2*e1*r3 + b2*f1*q3)
 397       + (a2*e3*t1 + a2*f3*s1 + b2*e3*r1 + b2*f3*q1)
 398       + (a3*e1*t2 + a3*f1*s2 + b3*e1*r2 + b3*f1*q2)
 399       - (a3*e2*t1 + a3*f2*s1 + b3*e2*r1 + b3*f2*q1);
 400     double Z = (c1*h2*t3 + c1*p2*s3 + d1*h2*r3 + d1*p2*q3)
 401       - (c1*h3*t2 + c1*p3*s2 + d1*h3*r2 + d1*p3*q2)
 402       - (c2*h1*t3 + c2*p1*s3 + d2*h1*r3 + d2*p1*q3)
 403       + (c2*h3*t1 + c2*p3*s1 + d2*h3*r1 + d2*p3*q1)
 404       + (c3*h1*t2 + c3*p1*s2 + d3*h1*r2 + d3*p1*q2)
 405       - (c3*h2*t1 + c3*p2*s1 + d3*h2*r1 + d3*p2*q1);
 406     double AA = ((a1*h2 + c1*e2)*t3 + (a1*p2 + c1*f2)*s3
 407                  +(b1*h2 + d1*e2)*r3 + (b1*p2 + d1*f2)*q3)
 408       - ((a1*h3 + c1*e3)*t2 + (a1*p3 + c1*f3)*s2
 409          +(b1*h3 + d1*e3)*r2 + (b1*p3 + d1*f3)*q2)
 410       - ((a2*h1 + c2*e1)*t3 + (a2*p1 + c2*f1)*s3
 411          +(b2*h1 + d2*e1)*r3 + (b2*p1 + d2*f1)*q3)
 412       + ((a2*h3 + c2*e3)*t1 + (a2*p3 + c2*f3)*s1
 413          +(b2*h3 + d2*e3)*r1 + (b2*p3 + d2*f3)*q1)
 414       + ((a3*h1 + c3*e1)*t2 + (a3*p1 + c3*f1)*s2
 415          +(b3*h1 + d3*e1)*r2 + (b3*p1 + d3*f1)*q2)
 416       - ((a3*h2 + c3*e2)*t1 + (a3*p2 + c3*f2)*s1
 417          + (b3*h2 + d3*e2)*r1 + (b3*p2 + d3*f2)*q1);
 418
 419     return  64.0*(  8.0*(A + B + D + E + J + K + M + N)
 420                     + 4.0*(C + F + G + H + L + O + P + Q + S + T + V + W)
 421                     + 2.0*(I + R + U + X + Y + Z) + AA ) / 27.0 ;
 422   }
 423
 424   // =========================================================================================================================
 425   // Calculate Volume for Generic Polyedron, even not convex one, WARNING !!! The polyedron's faces must be correctly ordered
 426   // =========================================================================================================================
 427   inline double calculateVolumeForPolyh(const double ***pts,
 428                                         const int *nbOfNodesPerFaces,
 429                                         int nbOfFaces,
 430                                         const double *bary)
 431   {
 432     double volume=0.;
 433
 434     for ( int i=0; i<nbOfFaces; i++ )
 435       {
 436         double normal[3];
 437         double vecForAlt[3];
 438
 439         calculateNormalForPolyg(pts[i],nbOfNodesPerFaces[i],normal);
 440         vecForAlt[0]=bary[0]-pts[i][0][0];
 441         vecForAlt[1]=bary[1]-pts[i][0][1];
 442         vecForAlt[2]=bary[2]-pts[i][0][2];
 443         volume+=vecForAlt[0]*normal[0]+vecForAlt[1]*normal[1]+vecForAlt[2]*normal[2];
 444       }
 445     return -volume/3.;
 446   }
 447
 448   /*!
 449    * Calculate Volume for Generic Polyedron, even not convex one, WARNING !!! The polyedron's faces must be correctly ordered.
 450    * 2nd API avoiding to create double** arrays. The returned value could be negative if polyhedrons faces are not oriented with normal outside of the
 451    * polyhedron
 452    */
 453   template<class ConnType, NumberingPolicy numPol>
 454   inline double calculateVolumeForPolyh2(const ConnType *connec, int lgth, const double *coords)
 455   {
 456     std::size_t nbOfFaces=std::count(connec,connec+lgth,-1)+1;
 457     double volume=0.;
 458     const int *work=connec;
 459     for(std::size_t iFace=0;iFace<nbOfFaces;iFace++)
 460       {
 461         const int *work2=std::find(work+1,connec+lgth,-1);
 462         std::size_t nbOfNodesOfCurFace=std::distance(work,work2);
 463         double areaVector[3]={0.,0.,0.};
 464         for(std::size_t ptId=0;ptId<nbOfNodesOfCurFace;ptId++)
 465           {
 466             const double *pti=coords+3*OTT<ConnType,numPol>::coo2C(work[ptId]);
 467             const double *pti1=coords+3*OTT<ConnType,numPol>::coo2C(work[(ptId+1)%nbOfNodesOfCurFace]);
 468             areaVector[0]+=pti[1]*pti1[2]-pti[2]*pti1[1];
 469             areaVector[1]+=pti[2]*pti1[0]-pti[0]*pti1[2];
 470             areaVector[2]+=pti[0]*pti1[1]-pti[1]*pti1[0];
 471           }
 472         const double *pt=coords+3*work[0];
 473         volume+=pt[0]*areaVector[0]+pt[1]*areaVector[1]+pt[2]*areaVector[2];
 474         work=work2+1;
 475       }
 476     return volume/6.;
 477   }
 478
 479   /*!
 480    * This method returns the area oriented vector of a polygon. This method is useful for normal computation without
 481    * any troubles if several edges are colinears.
 482    * @param res must be of size at least 3 to store the result.
 483    */
 484   template<class ConnType, NumberingPolicy numPol>
 485   inline void areaVectorOfPolygon(const ConnType *connec, int lgth, const double *coords, double *res)
 486   {
 487     res[0]=0.; res[1]=0.; res[2]=0.;
 488     for(int ptId=0;ptId<lgth;ptId++)
 489       {
 490         const double *pti=coords+3*OTT<ConnType,numPol>::coo2C(connec[ptId]);
 491         const double *pti1=coords+3*OTT<ConnType,numPol>::coo2C(connec[(ptId+1)%lgth]);
 492         res[0]+=pti[1]*pti1[2]-pti[2]*pti1[1];
 493         res[1]+=pti[2]*pti1[0]-pti[0]*pti1[2];
 494         res[2]+=pti[0]*pti1[1]-pti[1]*pti1[0];
 495       }
 496   }
 497
 498   inline double integrationOverA3DLine(double u1, double v1, double u2, double v2, double A, double B, double C)
 499   {
 500     return (u1-u2)*(6.*C*C*(v1+v2)+B*B*(v1*v1*v1+v1*v1*v2+v1*v2*v2+v2*v2*v2)+A*A*(2.*u1*u2*(v1+v2)+u1*u1*(3.*v1+v2)+u2*u2*(v1+3.*v2))+
 501                     4.*C*(A*(2*u1*v1+u2*v1+u1*v2+2.*u2*v2)+B*(v1*v1+v1*v2+v2*v2))+A*B*(u1*(3.*v1*v1+2.*v1*v2+v2*v2)+u2*(v1*v1+2.*v1*v2+3.*v2*v2)))/24.;
 502   }
 503
 504   template<class ConnType, NumberingPolicy numPol>
 505   inline void barycenterOfPolyhedron(const ConnType *connec, int lgth, const double *coords, double *res)
 506   {
 507     std::size_t nbOfFaces=std::count(connec,connec+lgth,-1)+1;
 508     res[0]=0.; res[1]=0.; res[2]=0.;
 509     const int *work=connec;
 510     for(std::size_t i=0;i<nbOfFaces;i++)
 511       {
 512         const int *work2=std::find(work+1,connec+lgth,-1);
 513         int nbOfNodesOfCurFace=(int)std::distance(work,work2);
 514         // projection to (u,v) of each faces of polyh to compute integral(x^2/2) on each faces.
 515         double normal[3];
 516         areaVectorOfPolygon<ConnType,numPol>(work,nbOfNodesOfCurFace,coords,normal);
 517         double normOfNormal=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
 518         normal[0]/=normOfNormal; normal[1]/=normOfNormal; normal[2]/=normOfNormal;
 519         double u[2]={normal[1],-normal[0]};
 520         double s=sqrt(u[0]*u[0]+u[1]*u[1]);
 521         double c=normal[2];
 522         if(fabs(s)>1e-12)
 523           {
 524             u[0]/=std::abs(s); u[1]/=std::abs(s);
 525           }
 526         else
 527           { u[0]=1.; u[1]=0.; }
 528         //C : high in plane of polyhedron face : always constant
 529         double w=normal[0]*coords[3*OTT<ConnType,numPol>::coo2C(work[0])]+
 530           normal[1]*coords[3*OTT<ConnType,numPol>::coo2C(work[0])+1]+
 531           normal[2]*coords[3*OTT<ConnType,numPol>::coo2C(work[0])+2];
 532         // A,B,D,F,G,H,L,M,N coeffs of rotation matrix defined by (u,c,s)
 533         double A=u[0]*u[0]*(1-c)+c;
 534         double B=u[0]*u[1]*(1-c);
 535         double D=u[1]*s;
 536         double F=B;
 537         double G=u[1]*u[1]*(1-c)+c;
 538         double H=-u[0]*s;
 539         double L=-u[1]*s;
 540         double M=u[0]*s;
 541         double N=c;
 542         double CX=-w*D;
 543         double CY=-w*H;
 544         double CZ=-w*N;
 545         for(int j=0;j<nbOfNodesOfCurFace;j++)
 546           {
 547             const double *p1=coords+3*OTT<ConnType,numPol>::coo2C(work[j]);
 548             const double *p2=coords+3*OTT<ConnType,numPol>::coo2C(work[(j+1)%nbOfNodesOfCurFace]);
 549             double u1=A*p1[0]+B*p1[1]+D*p1[2];
 550             double u2=A*p2[0]+B*p2[1]+D*p2[2];
 551             double v1=F*p1[0]+G*p1[1]+H*p1[2];
 552             double v2=F*p2[0]+G*p2[1]+H*p2[2];
 553             //
 554             double gx=integrationOverA3DLine(u1,v1,u2,v2,A,B,CX);
 555             double gy=integrationOverA3DLine(u1,v1,u2,v2,F,G,CY);
 556             double gz=integrationOverA3DLine(u1,v1,u2,v2,L,M,CZ);
 557             res[0]+=gx*normal[0];
 558             res[1]+=gy*normal[1];
 559             res[2]+=gz*normal[2];
 560           }
 561         work=work2+1;
 562       }
 563     double vol=calculateVolumeForPolyh2<ConnType,numPol>(connec,lgth,coords);
 564     res[0]/=vol; res[1]/=vol; res[2]/=vol;
 565   }
 566
 567   // ============================================================================================================================================
 568   // Calculate Volume for NON Generic Polyedron. Only polydrons with bary included in pts is supported by this method. Result is always positive.
 569   // ============================================================================================================================================
 570   inline double calculateVolumeForPolyhAbs(const double ***pts,
 571                                            const int *nbOfNodesPerFaces,
 572                                            int nbOfFaces,
 573                                            const double *bary)
 574   {
 575     double volume=0.;
 576
 577     for ( int i=0; i<nbOfFaces; i++ )
 578       {
 579         double normal[3];
 580         double vecForAlt[3];
 581
 582         calculateNormalForPolyg(pts[i],nbOfNodesPerFaces[i],normal);
 583         vecForAlt[0]=bary[0]-pts[i][0][0];
 584         vecForAlt[1]=bary[1]-pts[i][0][1];
 585         vecForAlt[2]=bary[2]-pts[i][0][2];
 586         volume+=fabs(vecForAlt[0]*normal[0]+vecForAlt[1]*normal[1]+vecForAlt[2]*normal[2]);
 587       }
 588     return volume/3.;
 589   }
 590
 591   template<int N>
 592   inline double addComponentsOfVec(const double **pts, int rk)
 593   {
 594     return pts[N-1][rk]+addComponentsOfVec<N-1>(pts,rk);
 595   }
 596
 597   template<>
 598   inline double addComponentsOfVec<1>(const double **pts, int rk)
 599   {
 600     return pts[0][rk];
 601   }
 602
 603   template<int N, int DIM>
 604   inline void calculateBarycenter(const double **pts, double *bary)
 605   {
 606     bary[DIM-1]=addComponentsOfVec<N>(pts,DIM-1)/N;
 607     calculateBarycenter<N,DIM-1>(pts,bary);
 608   }
 609
 610   template<>
 611   inline void calculateBarycenter<2,0>(const double **/*pts*/, double */*bary*/)
 612   {
 613   }
 614
 615   template<>
 616   inline void calculateBarycenter<3,0>(const double **/*pts*/, double */*bary*/)
 617   {
 618   }
 619
 620   template<>
 621   inline void calculateBarycenter<4,0>(const double **/*pts*/, double */*bary*/)
 622   {
 623   }
 624
 625   template<>
 626   inline void calculateBarycenter<5,0>(const double **/*pts*/, double */*bary*/)
 627   {
 628   }
 629
 630   template<>
 631   inline void calculateBarycenter<6,0>(const double **/*pts*/, double */*bary*/)
 632   {
 633   }
 634
 635   template<>
 636   inline void calculateBarycenter<7,0>(const double **/*pts*/, double */*bary*/)
 637   {
 638   }
 639
 640   template<>
 641   inline void calculateBarycenter<8,0>(const double **/*pts*/, double */*bary*/)
 642   {
 643   }
 644
 645   inline void calculateBarycenterDyn(const double **pts, int nbPts,
 646                                      int dim, double *bary)
 647   {
 648     for(int i=0;i<dim;i++)
 649       {
 650         double temp=0.;
 651         for(int j=0;j<nbPts;j++)
 652           {
 653             temp+=pts[j][i];
 654           }
 655         bary[i]=temp/nbPts;
 656       }
 657   }
 658
 659   template<int SPACEDIM>
 660   inline void calculateBarycenterDyn2(const double *pts, int nbPts, double *bary)
 661   {
 662     for(int i=0;i<SPACEDIM;i++)
 663       {
 664         double temp=0.;
 665         for(int j=0;j<nbPts;j++)
 666           {
 667             temp+=pts[j*SPACEDIM+i];
 668           }
 669         bary[i]=temp/nbPts;
 670       }
 671   }
 672
 673   template<class ConnType, NumberingPolicy numPol>
 674   inline void computePolygonBarycenter2D(const ConnType *connec, int lgth, const double *coords, double *res)
 675   {
 676     double area=0.;
 677     res[0]=0.; res[1]=0.;
 678     for(int i=0;i<lgth;i++)
 679       {
 680         double cp=coords[2*OTT<ConnType,numPol>::coo2C(connec[i])]*coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])+1]-
 681           coords[2*OTT<ConnType,numPol>::coo2C(connec[i])+1]*coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])];
 682         area+=cp;
 683         res[0]+=cp*(coords[2*OTT<ConnType,numPol>::coo2C(connec[i])]+coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])]);
 684         res[1]+=cp*(coords[2*OTT<ConnType,numPol>::coo2C(connec[i])+1]+coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])+1]);
 685       }
 686     res[0]/=3.*area;
 687     res[1]/=3.*area;
 688   }
 689
 690   template<class ConnType, NumberingPolicy numPol>
 691   inline void computePolygonBarycenter3D(const ConnType *connec, int lgth, const double *coords, double *res)
 692   {
 693     double area[3];
 694     areaVectorOfPolygon<ConnType,numPol>(connec,lgth,coords,area);
 695     double norm=sqrt(area[0]*area[0]+area[1]*area[1]+area[2]*area[2]);
 696     area[0]/=norm; area[1]/=norm; area[2]/=norm;
 697     res[0]=0.; res[1]=0.; res[2]=0.;
 698     for(int i=1;i<lgth-1;i++)
 699       {
 700         double v[3];
 701         double tmpArea[3];
 702         v[0]=(coords[3*OTT<ConnType,numPol>::coo2C(connec[0])]+
 703               coords[3*OTT<ConnType,numPol>::coo2C(connec[i])]+
 704               coords[3*OTT<ConnType,numPol>::coo2C(connec[i+1])])/3.;
 705         v[1]=(coords[3*OTT<ConnType,numPol>::coo2C(connec[0])+1]+
 706               coords[3*OTT<ConnType,numPol>::coo2C(connec[i])+1]+
 707               coords[3*OTT<ConnType,numPol>::coo2C(connec[i+1])+1])/3.;
 708         v[2]=(coords[3*OTT<ConnType,numPol>::coo2C(connec[0])+2]+
 709               coords[3*OTT<ConnType,numPol>::coo2C(connec[i])+2]+
 710               coords[3*OTT<ConnType,numPol>::coo2C(connec[i+1])+2])/3.;
 711         ConnType tmpConn[3]={connec[0],connec[i],connec[i+1]};
 712         areaVectorOfPolygon<ConnType,numPol>(tmpConn,3,coords,tmpArea);
 713         double norm2=sqrt(tmpArea[0]*tmpArea[0]+tmpArea[1]*tmpArea[1]+tmpArea[2]*tmpArea[2]);
 714         if(norm2>1e-12)
 715           {
 716             tmpArea[0]/=norm2; tmpArea[1]/=norm2; tmpArea[2]/=norm2;
 717             double signOfArea=area[0]*tmpArea[0]+area[1]*tmpArea[1]+area[2]*tmpArea[2];
 718             res[0]+=signOfArea*norm2*v[0]/norm; res[1]+=signOfArea*norm2*v[1]/norm; res[2]+=signOfArea*norm2*v[2]/norm;
 719           }
 720       }
 721   }
 722 }
 723
 724 #endif