Back to index

salome-med  6.5.0
VolSurfFormulae.hxx
Go to the documentation of this file.
00001 // Copyright (C) 2007-2012  CEA/DEN, EDF R&D
00002 //
00003 // This library is free software; you can redistribute it and/or
00004 // modify it under the terms of the GNU Lesser General Public
00005 // License as published by the Free Software Foundation; either
00006 // version 2.1 of the License.
00007 //
00008 // This library is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011 // Lesser General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU Lesser General Public
00014 // License along with this library; if not, write to the Free Software
00015 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
00016 //
00017 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
00018 //
00019 
00020 #ifndef __VOLSURFFORMULAE_HXX__
00021 #define __VOLSURFFORMULAE_HXX__
00022 
00023 #include "InterpolationUtils.hxx"
00024 
00025 #include <cmath>
00026 
00027 namespace INTERP_KERNEL
00028 {
00029   inline void calculateBarycenterDyn(const double **pts, int nbPts,
00030                                      int dim, double *bary);
00031 
00032   inline double calculateAreaForPolyg(const double **coords, int nbOfPtsInPolygs,
00033                                       int spaceDim);
00034 
00035 
00036   inline double calculateLgthForSeg2(const double *p1, const double *p2, int spaceDim)
00037   {
00038     if(spaceDim==1)
00039       return *p2-*p1;
00040     else
00041       {
00042         double ret=0;
00043         for(int i=0;i<spaceDim;i++)
00044           ret+=(p2[i]-p1[i])*(p2[i]-p1[i]);
00045         return sqrt(ret);
00046       }
00047   }
00048 
00049   // ===========================
00050   // Calculate Area for triangle
00051   // ===========================
00052   inline double calculateAreaForTria(const double *p1, const double *p2,
00053                                      const double *p3, int spaceDim)
00054   {
00055     double area ;
00056 
00057     if ( spaceDim == 2 )
00058       {
00059         area = -((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1]))/2.0;
00060       }
00061     else
00062       {
00063         area = sqrt(((p2[1]-p1[1])*(p3[2]-p1[2]) - (p3[1]-p1[1])*(p2[2]-p1[2]))*
00064                     ((p2[1]-p1[1])*(p3[2]-p1[2]) - (p3[1]-p1[1])*(p2[2]-p1[2]))
00065                     +
00066                     ((p3[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p3[2]-p1[2]))*
00067                     ((p3[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p3[2]-p1[2]))
00068                     +
00069                     ((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1]))*
00070                     ((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1])))/2.0;
00071       }
00072 
00073     return area ;
00074   }
00075 
00076   // =============================
00077   // Calculate Area for quadrangle
00078   // =============================
00079   inline double calculateAreaForQuad(const double *p1, const double *p2,
00080                                      const double *p3, const double *p4,
00081                                      int spaceDim)
00082   {
00083     double area ;
00084 
00085     if (spaceDim==2)
00086       {
00087         double a1 = (p2[0]-p1[0])/4.0, a2 = (p2[1]-p1[1])/4.0;
00088         double b1 = (p3[0]-p4[0])/4.0, b2 = (p3[1]-p4[1])/4.0;
00089         double c1 = (p3[0]-p2[0])/4.0, c2 = (p3[1]-p2[1])/4.0;
00090         double d1 = (p4[0]-p1[0])/4.0, d2 = (p4[1]-p1[1])/4.0;
00091 
00092         area = - 4.0*(  b1*c2 - c1*b2 + a1*c2 - c1*a2
00093                         + b1*d2 - d1*b2 + a1*d2 - d1*a2);
00094       }
00095     else
00096       {
00097         area = (sqrt(((p2[1]-p1[1])*(p4[2]-p1[2]) - (p4[1]-p1[1])*(p2[2]-p1[2]))*
00098                      ((p2[1]-p1[1])*(p4[2]-p1[2]) - (p4[1]-p1[1])*(p2[2]-p1[2]))
00099                      + ((p4[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p4[2]-p1[2]))*
00100                      ((p4[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p4[2]-p1[2]))
00101                      + ((p2[0]-p1[0])*(p4[1]-p1[1]) - (p4[0]-p1[0])*(p2[1]-p1[1]))*
00102                      ((p2[0]-p1[0])*(p4[1]-p1[1]) - (p4[0]-p1[0])*(p2[1]-p1[1])))
00103                 +
00104                 sqrt(((p4[1]-p3[1])*(p2[2]-p3[2]) - (p2[1]-p3[1])*(p4[2]-p3[2]))*
00105                      ((p4[1]-p3[1])*(p2[2]-p3[2]) - (p2[1]-p3[1])*(p4[2]-p3[2]))
00106                      + ((p2[0]-p3[0])*(p4[2]-p3[2]) - (p4[0]-p3[0])*(p2[2]-p3[2]))*
00107                      ((p2[0]-p3[0])*(p4[2]-p3[2]) - (p4[0]-p3[0])*(p2[2]-p3[2]))
00108                      + ((p4[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p4[1]-p3[1]))*
00109                      ((p4[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p4[1]-p3[1])))
00110                 )/2.0;
00111       }
00112 
00113     return area ;
00114   }
00115 
00116   // ====================================
00117   // Calculate Normal Vector for Triangle
00118   // ====================================
00119   inline void calculateNormalForTria(const double *p1, const double *p2,
00120                                      const double *p3, double *normal)
00121   {
00122     normal[0] = ((p2[1]-p1[1])*(p3[2]-p1[2]) - (p3[1]-p1[1])*(p2[2]-p1[2]))/2.0;
00123     normal[1] = ((p3[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p3[2]-p1[2]))/2.0;
00124     normal[2] = ((p2[0]-p1[0])*(p3[1]-p1[1]) - (p3[0]-p1[0])*(p2[1]-p1[1]))/2.0;
00125   }
00126 
00127   // ======================================
00128   // Calculate Normal Vector for Quadrangle
00129   // ======================================
00130   inline void calculateNormalForQuad(const double *p1, const double *p2,
00131                                      const double *p3, const double *p4,
00132                                      double *normal)
00133   {
00134     double xnormal1 = (p2[1]-p1[1])*(p4[2]-p1[2]) - (p4[1]-p1[1])*(p2[2]-p1[2]);
00135     double xnormal2 = (p4[0]-p1[0])*(p2[2]-p1[2]) - (p2[0]-p1[0])*(p4[2]-p1[2]);
00136     double xnormal3 = (p2[0]-p1[0])*(p4[1]-p1[1]) - (p4[0]-p1[0])*(p2[1]-p1[1]);
00137     double xarea = sqrt(xnormal1*xnormal1 + xnormal2*xnormal2 + xnormal3*xnormal3);
00138     xnormal1 = xnormal1/xarea;
00139     xnormal2 = xnormal2/xarea;
00140     xnormal3 = xnormal3/xarea;
00141     xarea = calculateAreaForQuad(p1,p2,p3,p4,3);
00142     normal[0] = xnormal1*xarea ;
00143     normal[1] = xnormal2*xarea ;
00144     normal[2] = xnormal3*xarea ;
00145   }
00146 
00147   // ===================================
00148   // Calculate Normal Vector for Polygon
00149   // ===================================
00150   inline void calculateNormalForPolyg(const double **coords, int nbOfPtsInPolygs,
00151                                       double *normal)
00152   {
00153     double coordOfBary[3];
00154 
00155     calculateBarycenterDyn(coords,nbOfPtsInPolygs,3,coordOfBary);
00156     double xnormal1 = (coords[0][1]-coords[1][1]) * (coordOfBary[2]-coords[1][2])
00157       - (coords[0][2]-coords[1][2]) * (coordOfBary[1]-coords[1][1]);
00158 
00159     double xnormal2 = (coords[0][2]-coords[1][2]) * (coordOfBary[0]-coords[1][0])
00160       - (coords[0][0]-coords[1][0]) * (coordOfBary[2]-coords[1][2]);
00161 
00162     double xnormal3 = (coords[0][0]-coords[1][0]) * (coordOfBary[1]-coords[1][1])
00163       - (coords[0][1]-coords[1][1]) * (coordOfBary[0]-coords[1][0]);
00164 
00165     double xarea=sqrt(xnormal1*xnormal1 + xnormal2*xnormal2 + xnormal3*xnormal3);
00166 
00167     if ( xarea < 1.e-6 )
00168       {
00169         //std::string diagnosis"Can not calculate normal - polygon is singular";
00170         throw std::exception();
00171       }
00172 
00173     xnormal1 = -xnormal1/xarea;
00174     xnormal2 = -xnormal2/xarea;
00175     xnormal3 = -xnormal3/xarea;
00176     xarea = calculateAreaForPolyg(coords,nbOfPtsInPolygs,3);
00177     normal[0] = xnormal1*xarea ;
00178     normal[1] = xnormal2*xarea ;
00179     normal[2] = xnormal3*xarea ;
00180   }
00181 
00182   // ==========================
00183   // Calculate Area for Polygon
00184   // ==========================
00185   inline double calculateAreaForPolyg(const double **coords, int nbOfPtsInPolygs,
00186                                       int spaceDim)
00187   {
00188     double ret=0.;
00189     double coordOfBary[3];
00190 
00191     calculateBarycenterDyn(coords,nbOfPtsInPolygs,spaceDim,coordOfBary);
00192     for ( int i=0; i<nbOfPtsInPolygs; i++ )
00193       {
00194         double tmp = calculateAreaForTria(coords[i],coords[(i+1)%nbOfPtsInPolygs],
00195                                           coordOfBary,spaceDim);
00196         ret+=tmp;
00197       }
00198     return ret;
00199   }
00200 
00201   // ==========================
00202   // Calculate Volume for Tetra
00203   // ==========================
00204   inline double calculateVolumeForTetra(const double *p1, const double *p2,
00205                                         const double *p3, const double *p4)
00206   {
00207     return (  (p3[0]-p1[0])*(  (p2[1]-p1[1])*(p4[2]-p1[2])
00208                                - (p2[2]-p1[2])*(p4[1]-p1[1]) )
00209               - (p2[0]-p1[0])*(  (p3[1]-p1[1])*(p4[2]-p1[2])
00210                                  - (p3[2]-p1[2])*(p4[1]-p1[1]) )
00211               + (p4[0]-p1[0])*(  (p3[1]-p1[1])*(p2[2]-p1[2])
00212                                  - (p3[2]-p1[2])*(p2[1]-p1[1]) )
00213               ) / 6.0 ;
00214   }
00215 
00216   // =========================
00217   // Calculate Volume for Pyra
00218   // =========================
00219   inline double calculateVolumeForPyra(const double *p1, const double *p2,
00220                                        const double *p3, const double *p4,
00221                                        const double *p5)
00222   {
00223     return ( ((p3[0]-p1[0])*(  (p2[1]-p1[1])*(p5[2]-p1[2])
00224                                - (p2[2]-p1[2])*(p5[1]-p1[1]) )
00225               -(p2[0]-p1[0])*(  (p3[1]-p1[1])*(p5[2]-p1[2])
00226                                 - (p3[2]-p1[2])*(p5[1]-p1[1]) )
00227               +(p5[0]-p1[0])*(  (p3[1]-p1[1])*(p2[2]-p1[2])
00228                                 - (p3[2]-p1[2])*(p2[1]-p1[1]) ))
00229              +
00230              ((p4[0]-p1[0])*(  (p3[1]-p1[1])*(p5[2]-p1[2])
00231                                - (p3[2]-p1[2])*(p5[1]-p1[1]) )
00232               -(p3[0]-p1[0])*(  (p4[1]-p1[1])*(p5[2]-p1[2])
00233                                 - (p4[2]-p1[2])*(p5[1]-p1[1]))
00234               +(p5[0]-p1[0])*(  (p4[1]-p1[1])*(p3[2]-p1[2])
00235                                 - (p4[2]-p1[2])*(p3[1]-p1[1]) ))
00236              ) / 6.0 ;
00237   }
00238 
00239   // ==========================
00240   // Calculate Volume for Penta
00241   // ==========================
00242   inline double calculateVolumeForPenta(const double *p1, const double *p2,
00243                                         const double *p3, const double *p4,
00244                                         const double *p5, const double *p6)
00245   {
00246     double a1 = (p2[0]-p3[0])/2.0, a2 = (p2[1]-p3[1])/2.0, a3 = (p2[2]-p3[2])/2.0;
00247     double b1 = (p5[0]-p6[0])/2.0, b2 = (p5[1]-p6[1])/2.0, b3 = (p5[2]-p6[2])/2.0;
00248     double c1 = (p4[0]-p1[0])/2.0, c2 = (p4[1]-p1[1])/2.0, c3 = (p4[2]-p1[2])/2.0;
00249     double d1 = (p5[0]-p2[0])/2.0, d2 = (p5[1]-p2[1])/2.0, d3 = (p5[2]-p2[2])/2.0;
00250     double e1 = (p6[0]-p3[0])/2.0, e2 = (p6[1]-p3[1])/2.0, e3 = (p6[2]-p3[2])/2.0;
00251     double f1 = (p1[0]-p3[0])/2.0, f2 = (p1[1]-p3[1])/2.0, f3 = (p1[2]-p3[2])/2.0;
00252     double h1 = (p4[0]-p6[0])/2.0, h2 = (p4[1]-p6[1])/2.0, h3 = (p4[2]-p6[2])/2.0;
00253 
00254     double A = a1*c2*f3 - a1*c3*f2 - a2*c1*f3 + a2*c3*f1 + a3*c1*f2 - a3*c2*f1;
00255     double B = b1*c2*h3 - b1*c3*h2 - b2*c1*h3 + b2*c3*h1 + b3*c1*h2 - b3*c2*h1;
00256     double C = (a1*c2*h3 + b1*c2*f3) - (a1*c3*h2 + b1*c3*f2)
00257       - (a2*c1*h3 + b2*c1*f3) + (a2*c3*h1 + b2*c3*f1)
00258       + (a3*c1*h2 + b3*c1*f2) - (a3*c2*h1 + b3*c2*f1);
00259     double D = a1*d2*f3 - a1*d3*f2 - a2*d1*f3 + a2*d3*f1 + a3*d1*f2 - a3*d2*f1;
00260     double E = b1*d2*h3 - b1*d3*h2 - b2*d1*h3 + b2*d3*h1 + b3*d1*h2 - b3*d2*h1;
00261     double F = (a1*d2*h3 + b1*d2*f3) - (a1*d3*h2 + b1*d3*f2)
00262       - (a2*d1*h3 + b2*d1*f3) + (a2*d3*h1 + b2*d3*f1)
00263       + (a3*d1*h2 + b3*d1*f2) - (a3*d2*h1 + b3*d2*f1);
00264     double G = a1*e2*f3 - a1*e3*f2 - a2*e1*f3 + a2*e3*f1 + a3*e1*f2 - a3*e2*f1;
00265     double H = b1*e2*h3 - b1*e3*h2 - b2*e1*h3 + b2*e3*h1 + b3*e1*h2 - b3*e2*h1;
00266     double P = (a1*e2*h3 + b1*e2*f3) - (a1*e3*h2 + b1*e3*f2)
00267       - (a2*e1*h3 + b2*e1*f3) + (a2*e3*h1 + b2*e3*f1)
00268       + (a3*e1*h2 + b3*e1*f2) - (a3*e2*h1 + b3*e2*f1);
00269 
00270     return (-2.0*(2.0*(A + B + D + E + G + H) + C + F + P)/9.0);
00271   }
00272 
00273   // =========================
00274   // Calculate Volume for Hexa
00275   // =========================
00276   inline double calculateVolumeForHexa(const double *pt1, const double *pt2,
00277                                        const double *pt3, const double *pt4,
00278                                        const double *pt5, const double *pt6,
00279                                        const double *pt7, const double *pt8)
00280   {
00281     double a1=(pt3[0]-pt4[0])/8.0, a2=(pt3[1]-pt4[1])/8.0, a3=(pt3[2]-pt4[2])/8.0;
00282     double b1=(pt2[0]-pt1[0])/8.0, b2=(pt2[1]-pt1[1])/8.0, b3=(pt2[2]-pt1[2])/8.0;
00283     double c1=(pt7[0]-pt8[0])/8.0, c2=(pt7[1]-pt8[1])/8.0, c3=(pt7[2]-pt8[2])/8.0;
00284     double d1=(pt6[0]-pt5[0])/8.0, d2=(pt6[1]-pt5[1])/8.0, d3=(pt6[2]-pt5[2])/8.0;
00285     double e1=(pt3[0]-pt2[0])/8.0, e2=(pt3[1]-pt2[1])/8.0, e3=(pt3[2]-pt2[2])/8.0;
00286     double f1=(pt4[0]-pt1[0])/8.0, f2=(pt4[1]-pt1[1])/8.0, f3=(pt4[2]-pt1[2])/8.0;
00287     double h1=(pt7[0]-pt6[0])/8.0, h2=(pt7[1]-pt6[1])/8.0, h3=(pt7[2]-pt6[2])/8.0;
00288     double p1=(pt8[0]-pt5[0])/8.0, p2=(pt8[1]-pt5[1])/8.0, p3=(pt8[2]-pt5[2])/8.0;
00289     double q1=(pt3[0]-pt7[0])/8.0, q2=(pt3[1]-pt7[1])/8.0, q3=(pt3[2]-pt7[2])/8.0;
00290     double r1=(pt4[0]-pt8[0])/8.0, r2=(pt4[1]-pt8[1])/8.0, r3=(pt4[2]-pt8[2])/8.0;
00291     double s1=(pt2[0]-pt6[0])/8.0, s2=(pt2[1]-pt6[1])/8.0, s3=(pt2[2]-pt6[2])/8.0;
00292     double t1=(pt1[0]-pt5[0])/8.0, t2=(pt1[1]-pt5[1])/8.0, t3=(pt1[2]-pt5[2])/8.0;
00293 
00294     double A = a1*e2*q3 - a1*e3*q2 - a2*e1*q3 + a2*e3*q1 + a3*e1*q2 - a3*e2*q1;
00295     double B = c1*h2*q3 - c1*h3*q2 - c2*h1*q3 + c2*h3*q1 + c3*h1*q2 - c3*h2*q1;
00296     double C = (a1*h2 + c1*e2)*q3 - (a1*h3 + c1*e3)*q2
00297       - (a2*h1 + c2*e1)*q3 + (a2*h3 + c2*e3)*q1
00298       + (a3*h1 + c3*e1)*q2 - (a3*h2 + c3*e2)*q1;
00299     double D = b1*e2*s3 - b1*e3*s2 - b2*e1*s3 + b2*e3*s1 + b3*e1*s2 - b3*e2*s1;
00300     double E = d1*h2*s3 - d1*h3*s2 - d2*h1*s3 + d2*h3*s1 + d3*h1*s2 - d3*h2*s1;
00301     double F = (b1*h2 + d1*e2)*s3 - (b1*h3 + d1*e3)*s2
00302       - (b2*h1 + d2*e1)*s3 + (b2*h3 + d2*e3)*s1
00303       + (b3*h1 + d3*e1)*s2 - (b3*h2 + d3*e2)*s1;
00304     double G = (a1*e2*s3 + b1*e2*q3) - (a1*e3*s2 + b1*e3*q2)
00305       - (a2*e1*s3 + b2*e1*q3) + (a2*e3*s1 + b2*e3*q1)
00306       + (a3*e1*s2 + b3*e1*q2) - (a3*e2*s1 + b3*e2*q1);
00307     double H = (c1*h2*s3 + d1*h2*q3) - (c1*h3*s2 + d1*h3*q2)
00308       - (c2*h1*s3 + d2*h1*q3) + (c2*h3*s1 + d2*h3*q1)
00309       + (c3*h1*s2 + d3*h1*q2) - (c3*h2*s1 + d3*h2*q1);
00310     double I = ((a1*h2 + c1*e2)*s3 + (b1*h2 + d1*e2)*q3)
00311       - ((a1*h3 + c1*e3)*s2 + (b1*h3 + d1*e3)*q2)
00312       - ((a2*h1 + c2*e1)*s3 + (b2*h1 + d2*e1)*q3)
00313       + ((a2*h3 + c2*e3)*s1 + (b2*h3 + d2*e3)*q1)
00314       + ((a3*h1 + c3*e1)*s2 + (b3*h1 + d3*e1)*q2)
00315       - ((a3*h2 + c3*e2)*s1 + (b3*h2 + d3*e2)*q1);
00316     double J = a1*f2*r3 - a1*f3*r2 - a2*f1*r3 + a2*f3*r1 + a3*f1*r2 - a3*f2*r1;
00317     double K = c1*p2*r3 - c1*p3*r2 - c2*p1*r3 + c2*p3*r1 + c3*p1*r2 - c3*p2*r1;
00318     double L = (a1*p2 + c1*f2)*r3 - (a1*p3 + c1*f3)*r2
00319       - (a2*p1 + c2*f1)*r3 + (a2*p3 + c2*f3)*r1
00320       + (a3*p1 + c3*f1)*r2 - (a3*p2 + c3*f2)*r1;
00321     double M = b1*f2*t3 - b1*f3*t2 - b2*f1*t3 + b2*f3*t1 + b3*f1*t2 - b3*f2*t1;
00322     double N = d1*p2*t3 - d1*p3*t2 - d2*p1*t3 + d2*p3*t1 + d3*p1*t2 - d3*p2*t1;
00323     double O = (b1*p2 + d1*f2)*t3 - (b1*p3 + d1*f3)*t2
00324       - (b2*p1 + d2*f1)*t3 + (b2*p3 + d2*f3)*t1
00325       + (b3*p1 + d3*f1)*t2 - (b3*p2 + d3*f2)*t1;
00326     double P = (a1*f2*t3 + b1*f2*r3) - (a1*f3*t2 + b1*f3*r2)
00327       - (a2*f1*t3 + b2*f1*r3) + (a2*f3*t1 + b2*f3*r1)
00328       + (a3*f1*t2 + b3*f1*r2) - (a3*f2*t1 + b3*f2*r1);
00329     double Q = (c1*p2*t3 + d1*p2*r3) - (c1*p3*t2 + d1*p3*r2)
00330       - (c2*p1*t3 + d2*p1*r3) + (c2*p3*t1 + d2*p3*r1)
00331       + (c3*p1*t2 + d3*p1*r2) - (c3*p2*t1 + d3*p2*r1);
00332     double R = ((a1*p2 + c1*f2)*t3 + (b1*p2 + d1*f2)*r3)
00333       - ((a1*p3 + c1*f3)*t2 + (b1*p3 + d1*f3)*r2)
00334       - ((a2*p1 + c2*f1)*t3 + (b2*p1 + d2*f1)*r3)
00335       + ((a2*p3 + c2*f3)*t1 + (b2*p3 + d2*f3)*r1)
00336       + ((a3*p1 + c3*f1)*t2 + (b3*p1 + d3*f1)*r2)
00337       - ((a3*p2 + c3*f2)*t1 + (b3*p2 + d3*f2)*r1);
00338     double S = (a1*e2*r3 + a1*f2*q3) - (a1*e3*r2 + a1*f3*q2)
00339       - (a2*e1*r3 + a2*f1*q3) + (a2*e3*r1 + a2*f3*q1)
00340       + (a3*e1*r2 + a3*f1*q2) - (a3*e2*r1 + a3*f2*q1);
00341     double T = (c1*h2*r3 + c1*p2*q3) - (c1*h3*r2 + c1*p3*q2)
00342       - (c2*h1*r3 + c2*p1*q3) + (c2*h3*r1 + c2*p3*q1)
00343       + (c3*h1*r2 + c3*p1*q2) - (c3*h2*r1 + c3*p2*q1);
00344     double U = ((a1*h2 + c1*e2)*r3 + (a1*p2 + c1*f2)*q3)
00345       - ((a1*h3 + c1*e3)*r2 + (a1*p3 + c1*f3)*q2)
00346       - ((a2*h1 + c2*e1)*r3 + (a2*p1 + c2*f1)*q3)
00347       + ((a2*h3 + c2*e3)*r1 + (a2*p3 + c2*f3)*q1)
00348       + ((a3*h1 + c3*e1)*r2 + (a3*p1 + c3*f1)*q2)
00349       - ((a3*h2 + c3*e2)*r1 + (a3*p2 + c3*f2)*q1);
00350     double V = (b1*e2*t3 + b1*f2*s3) - (b1*e3*t2 + b1*f3*s2)
00351       - (b2*e1*t3 + b2*f1*s3) + (b2*e3*t1 + b2*f3*s1)
00352       + (b3*e1*t2 + b3*f1*s2) - (b3*e2*t1 + b3*f2*s1);
00353     double W = (d1*h2*t3 + d1*p2*s3) - (d1*h3*t2 + d1*p3*s2)
00354       - (d2*h1*t3 + d2*p1*s3) + (d2*h3*t1 + d2*p3*s1)
00355       + (d3*h1*t2 + d3*p1*s2) - (d3*h2*t1 + d3*p2*s1);
00356     double X = ((b1*h2 + d1*e2)*t3 + (b1*p2 + d1*f2)*s3)
00357       - ((b1*h3 + d1*e3)*t2 + (b1*p3 + d1*f3)*s2)
00358       - ((b2*h1 + d2*e1)*t3 + (b2*p1 + d2*f1)*s3)
00359       + ((b2*h3 + d2*e3)*t1 + (b2*p3 + d2*f3)*s1)
00360       + ((b3*h1 + d3*e1)*t2 + (b3*p1 + d3*f1)*s2)
00361       - ((b3*h2 + d3*e2)*t1 + (b3*p2 + d3*f2)*s1);
00362     double Y = (a1*e2*t3 + a1*f2*s3 + b1*e2*r3 + b1*f2*q3)
00363       - (a1*e3*t2 + a1*f3*s2 + b1*e3*r2 + b1*f3*q2)
00364       - (a2*e1*t3 + a2*f1*s3 + b2*e1*r3 + b2*f1*q3)
00365       + (a2*e3*t1 + a2*f3*s1 + b2*e3*r1 + b2*f3*q1)
00366       + (a3*e1*t2 + a3*f1*s2 + b3*e1*r2 + b3*f1*q2)
00367       - (a3*e2*t1 + a3*f2*s1 + b3*e2*r1 + b3*f2*q1);
00368     double Z = (c1*h2*t3 + c1*p2*s3 + d1*h2*r3 + d1*p2*q3)
00369       - (c1*h3*t2 + c1*p3*s2 + d1*h3*r2 + d1*p3*q2)
00370       - (c2*h1*t3 + c2*p1*s3 + d2*h1*r3 + d2*p1*q3)
00371       + (c2*h3*t1 + c2*p3*s1 + d2*h3*r1 + d2*p3*q1)
00372       + (c3*h1*t2 + c3*p1*s2 + d3*h1*r2 + d3*p1*q2)
00373       - (c3*h2*t1 + c3*p2*s1 + d3*h2*r1 + d3*p2*q1);
00374     double AA = ((a1*h2 + c1*e2)*t3 + (a1*p2 + c1*f2)*s3
00375                  +(b1*h2 + d1*e2)*r3 + (b1*p2 + d1*f2)*q3)
00376       - ((a1*h3 + c1*e3)*t2 + (a1*p3 + c1*f3)*s2
00377          +(b1*h3 + d1*e3)*r2 + (b1*p3 + d1*f3)*q2)
00378       - ((a2*h1 + c2*e1)*t3 + (a2*p1 + c2*f1)*s3
00379          +(b2*h1 + d2*e1)*r3 + (b2*p1 + d2*f1)*q3)
00380       + ((a2*h3 + c2*e3)*t1 + (a2*p3 + c2*f3)*s1
00381          +(b2*h3 + d2*e3)*r1 + (b2*p3 + d2*f3)*q1)
00382       + ((a3*h1 + c3*e1)*t2 + (a3*p1 + c3*f1)*s2
00383          +(b3*h1 + d3*e1)*r2 + (b3*p1 + d3*f1)*q2)
00384       - ((a3*h2 + c3*e2)*t1 + (a3*p2 + c3*f2)*s1
00385          + (b3*h2 + d3*e2)*r1 + (b3*p2 + d3*f2)*q1);
00386 
00387     return  64.0*(  8.0*(A + B + D + E + J + K + M + N)
00388                     + 4.0*(C + F + G + H + L + O + P + Q + S + T + V + W)
00389                     + 2.0*(I + R + U + X + Y + Z) + AA ) / 27.0 ;
00390   }
00391 
00392   // =========================================================================================================================
00393   // Calculate Volume for Generic Polyedron, even not convex one, WARNING !!! The polyedron's faces must be correctly ordered
00394   // =========================================================================================================================
00395   inline double calculateVolumeForPolyh(const double ***pts,
00396                                         const int *nbOfNodesPerFaces,
00397                                         int nbOfFaces,
00398                                         const double *bary)
00399   {
00400     double volume=0.;
00401 
00402     for ( int i=0; i<nbOfFaces; i++ )
00403       {
00404         double normal[3];
00405         double vecForAlt[3];
00406 
00407         calculateNormalForPolyg(pts[i],nbOfNodesPerFaces[i],normal);
00408         vecForAlt[0]=bary[0]-pts[i][0][0];
00409         vecForAlt[1]=bary[1]-pts[i][0][1];
00410         vecForAlt[2]=bary[2]-pts[i][0][2];
00411         volume+=vecForAlt[0]*normal[0]+vecForAlt[1]*normal[1]+vecForAlt[2]*normal[2];
00412       }
00413     return -volume/3.;
00414   }
00415 
00421   template<class ConnType, NumberingPolicy numPol>
00422   inline double calculateVolumeForPolyh2(const ConnType *connec, int lgth, const double *coords)
00423   {
00424     std::size_t nbOfFaces=std::count(connec,connec+lgth,-1)+1;
00425     double volume=0.;
00426     const int *work=connec;
00427     for(std::size_t iFace=0;iFace<nbOfFaces;iFace++)
00428       {
00429         const int *work2=std::find(work+1,connec+lgth,-1);
00430         std::size_t nbOfNodesOfCurFace=std::distance(work,work2);
00431         double areaVector[3]={0.,0.,0.};
00432         for(std::size_t ptId=0;ptId<nbOfNodesOfCurFace;ptId++)
00433           {
00434             const double *pti=coords+3*OTT<ConnType,numPol>::coo2C(work[ptId]);
00435             const double *pti1=coords+3*OTT<ConnType,numPol>::coo2C(work[(ptId+1)%nbOfNodesOfCurFace]);
00436             areaVector[0]+=pti[1]*pti1[2]-pti[2]*pti1[1];
00437             areaVector[1]+=pti[2]*pti1[0]-pti[0]*pti1[2];
00438             areaVector[2]+=pti[0]*pti1[1]-pti[1]*pti1[0];
00439           }
00440         const double *pt=coords+3*work[0];
00441         volume+=pt[0]*areaVector[0]+pt[1]*areaVector[1]+pt[2]*areaVector[2];
00442         work=work2+1;
00443       }
00444     return volume/6.;
00445   }
00446 
00452   template<class ConnType, NumberingPolicy numPol>
00453   inline void areaVectorOfPolygon(const ConnType *connec, int lgth, const double *coords, double *res)
00454   {
00455     res[0]=0.; res[1]=0.; res[2]=0.;
00456     for(int ptId=0;ptId<lgth;ptId++)
00457       {
00458         const double *pti=coords+3*OTT<ConnType,numPol>::coo2C(connec[ptId]);
00459         const double *pti1=coords+3*OTT<ConnType,numPol>::coo2C(connec[(ptId+1)%lgth]);
00460         res[0]+=pti[1]*pti1[2]-pti[2]*pti1[1];
00461         res[1]+=pti[2]*pti1[0]-pti[0]*pti1[2];
00462         res[2]+=pti[0]*pti1[1]-pti[1]*pti1[0];
00463       }
00464   }
00465 
00466   inline double integrationOverA3DLine(double u1, double v1, double u2, double v2, double A, double B, double C)
00467   {
00468     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))+ 
00469                     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.;
00470   }
00471 
00472   template<class ConnType, NumberingPolicy numPol>
00473   inline void barycenterOfPolyhedron(const ConnType *connec, int lgth, const double *coords, double *res)
00474   {
00475     std::size_t nbOfFaces=std::count(connec,connec+lgth,-1)+1;
00476     res[0]=0.; res[1]=0.; res[2]=0.;
00477     const int *work=connec;
00478     for(std::size_t i=0;i<nbOfFaces;i++)
00479       {
00480         const int *work2=std::find(work+1,connec+lgth,-1);
00481         int nbOfNodesOfCurFace=(int)std::distance(work,work2);
00482         // projection to (u,v) of each faces of polyh to compute integral(x^2/2) on each faces.
00483         double normal[3];
00484         areaVectorOfPolygon<ConnType,numPol>(work,nbOfNodesOfCurFace,coords,normal);
00485         double normOfNormal=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
00486         normal[0]/=normOfNormal; normal[1]/=normOfNormal; normal[2]/=normOfNormal;
00487         double u[2]={normal[1],-normal[0]};
00488         double s=sqrt(u[0]*u[0]+u[1]*u[1]);
00489         double c=normal[2];
00490         if(fabs(s)>1e-12)
00491           {
00492             u[0]/=std::abs(s); u[1]/=std::abs(s);
00493           }
00494         else
00495           { u[0]=1.; u[1]=0.; }
00496         //C : high in plane of polyhedron face : always constant
00497         double w=normal[0]*coords[3*OTT<ConnType,numPol>::coo2C(work[0])]+
00498           normal[1]*coords[3*OTT<ConnType,numPol>::coo2C(work[0])+1]+
00499           normal[2]*coords[3*OTT<ConnType,numPol>::coo2C(work[0])+2];
00500         // A,B,D,F,G,H,L,M,N coeffs of rotation matrix defined by (u,c,s)
00501         double A=u[0]*u[0]*(1-c)+c;
00502         double B=u[0]*u[1]*(1-c);
00503         double D=u[1]*s;
00504         double F=B;
00505         double G=u[1]*u[1]*(1-c)+c;
00506         double H=-u[0]*s;
00507         double L=-u[1]*s;
00508         double M=u[0]*s;
00509         double N=c;
00510         double CX=-w*D;
00511         double CY=-w*H;
00512         double CZ=-w*N;
00513         for(int j=0;j<nbOfNodesOfCurFace;j++)
00514           {
00515             const double *p1=coords+3*OTT<ConnType,numPol>::coo2C(work[j]);
00516             const double *p2=coords+3*OTT<ConnType,numPol>::coo2C(work[(j+1)%nbOfNodesOfCurFace]);
00517             double u1=A*p1[0]+B*p1[1]+D*p1[2];
00518             double u2=A*p2[0]+B*p2[1]+D*p2[2];
00519             double v1=F*p1[0]+G*p1[1]+H*p1[2];
00520             double v2=F*p2[0]+G*p2[1]+H*p2[2];
00521             //
00522             double gx=integrationOverA3DLine(u1,v1,u2,v2,A,B,CX);
00523             double gy=integrationOverA3DLine(u1,v1,u2,v2,F,G,CY);
00524             double gz=integrationOverA3DLine(u1,v1,u2,v2,L,M,CZ);
00525             res[0]+=gx*normal[0];
00526             res[1]+=gy*normal[1];
00527             res[2]+=gz*normal[2];
00528           }
00529         work=work2+1;
00530       }
00531     double vol=calculateVolumeForPolyh2<ConnType,numPol>(connec,lgth,coords);
00532     res[0]/=vol; res[1]/=vol; res[2]/=vol;
00533   }
00534 
00535   // ============================================================================================================================================
00536   // Calculate Volume for NON Generic Polyedron. Only polydrons with bary included in pts is supported by this method. Result is always positive.
00537   // ============================================================================================================================================
00538   inline double calculateVolumeForPolyhAbs(const double ***pts,
00539                                            const int *nbOfNodesPerFaces,
00540                                            int nbOfFaces,
00541                                            const double *bary)
00542   {
00543     double volume=0.;
00544     
00545     for ( int i=0; i<nbOfFaces; i++ )
00546       {
00547         double normal[3];
00548         double vecForAlt[3];
00549 
00550         calculateNormalForPolyg(pts[i],nbOfNodesPerFaces[i],normal);
00551         vecForAlt[0]=bary[0]-pts[i][0][0];
00552         vecForAlt[1]=bary[1]-pts[i][0][1];
00553         vecForAlt[2]=bary[2]-pts[i][0][2];
00554         volume+=fabs(vecForAlt[0]*normal[0]+vecForAlt[1]*normal[1]+vecForAlt[2]*normal[2]);
00555       }
00556     return volume/3.;
00557   }
00558 
00559   template<int N>
00560   inline double addComponentsOfVec(const double **pts, int rk)
00561   {
00562     return pts[N-1][rk]+addComponentsOfVec<N-1>(pts,rk);
00563   }
00564 
00565   template<>
00566   inline double addComponentsOfVec<1>(const double **pts, int rk)
00567   {
00568     return pts[0][rk];
00569   }
00570 
00571   template<int N, int DIM>
00572   inline void calculateBarycenter(const double **pts, double *bary)
00573   {
00574     bary[DIM-1]=addComponentsOfVec<N>(pts,DIM-1)/N;
00575     calculateBarycenter<N,DIM-1>(pts,bary);
00576   }
00577 
00578   template<>
00579   inline void calculateBarycenter<2,0>(const double **/*pts*/, double */*bary*/)
00580   {
00581   }
00582   
00583   template<>
00584   inline void calculateBarycenter<3,0>(const double **/*pts*/, double */*bary*/)
00585   {
00586   }
00587   
00588   template<>
00589   inline void calculateBarycenter<4,0>(const double **/*pts*/, double */*bary*/)
00590   {
00591   }
00592   
00593   template<>
00594   inline void calculateBarycenter<5,0>(const double **/*pts*/, double */*bary*/)
00595   {
00596   }
00597   
00598   template<>
00599   inline void calculateBarycenter<6,0>(const double **/*pts*/, double */*bary*/)
00600   {
00601   }
00602   
00603   template<>
00604   inline void calculateBarycenter<7,0>(const double **/*pts*/, double */*bary*/)
00605   {
00606   }
00607   
00608   template<>
00609   inline void calculateBarycenter<8,0>(const double **/*pts*/, double */*bary*/)
00610   {
00611   }
00612 
00613   inline void calculateBarycenterDyn(const double **pts, int nbPts,
00614                                      int dim, double *bary)
00615   {
00616     for(int i=0;i<dim;i++)
00617       {
00618         double temp=0.;
00619         for(int j=0;j<nbPts;j++)
00620           {
00621             temp+=pts[j][i];
00622           }
00623         bary[i]=temp/nbPts;
00624       }
00625   }
00626 
00627   template<int SPACEDIM>
00628   inline void calculateBarycenterDyn2(const double *pts, int nbPts, double *bary)
00629   {
00630     for(int i=0;i<SPACEDIM;i++)
00631       {
00632         double temp=0.;
00633         for(int j=0;j<nbPts;j++)
00634           {
00635             temp+=pts[j*SPACEDIM+i];
00636           }
00637         bary[i]=temp/nbPts;
00638       }
00639   }
00640   
00641   template<class ConnType, NumberingPolicy numPol>
00642   inline void computePolygonBarycenter2D(const ConnType *connec, int lgth, const double *coords, double *res)
00643   {
00644     double area=0.;
00645     res[0]=0.; res[1]=0.;
00646     for(int i=0;i<lgth;i++)
00647       {
00648         double cp=coords[2*OTT<ConnType,numPol>::coo2C(connec[i])]*coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])+1]-
00649           coords[2*OTT<ConnType,numPol>::coo2C(connec[i])+1]*coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])];
00650         area+=cp;
00651         res[0]+=cp*(coords[2*OTT<ConnType,numPol>::coo2C(connec[i])]+coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])]);
00652         res[1]+=cp*(coords[2*OTT<ConnType,numPol>::coo2C(connec[i])+1]+coords[2*OTT<ConnType,numPol>::coo2C(connec[(i+1)%lgth])+1]);
00653       }
00654     res[0]/=3.*area;
00655     res[1]/=3.*area;
00656   }
00657 
00658   template<class ConnType, NumberingPolicy numPol>
00659   inline void computePolygonBarycenter3D(const ConnType *connec, int lgth, const double *coords, double *res)
00660   {
00661     double area[3];
00662     areaVectorOfPolygon<ConnType,numPol>(connec,lgth,coords,area);
00663     double norm=sqrt(area[0]*area[0]+area[1]*area[1]+area[2]*area[2]);
00664     area[0]/=norm; area[1]/=norm; area[2]/=norm;
00665     res[0]=0.; res[1]=0.; res[2]=0.;
00666     for(int i=1;i<lgth-1;i++)
00667       {
00668         double v[3];
00669         double tmpArea[3];
00670         v[0]=(coords[3*OTT<ConnType,numPol>::coo2C(connec[0])]+
00671               coords[3*OTT<ConnType,numPol>::coo2C(connec[i])]+
00672               coords[3*OTT<ConnType,numPol>::coo2C(connec[i+1])])/3.;
00673         v[1]=(coords[3*OTT<ConnType,numPol>::coo2C(connec[0])+1]+
00674               coords[3*OTT<ConnType,numPol>::coo2C(connec[i])+1]+
00675               coords[3*OTT<ConnType,numPol>::coo2C(connec[i+1])+1])/3.;
00676         v[2]=(coords[3*OTT<ConnType,numPol>::coo2C(connec[0])+2]+
00677               coords[3*OTT<ConnType,numPol>::coo2C(connec[i])+2]+
00678               coords[3*OTT<ConnType,numPol>::coo2C(connec[i+1])+2])/3.;
00679         ConnType tmpConn[3]={connec[0],connec[i],connec[i+1]};
00680         areaVectorOfPolygon<ConnType,numPol>(tmpConn,3,coords,tmpArea);
00681         double norm2=sqrt(tmpArea[0]*tmpArea[0]+tmpArea[1]*tmpArea[1]+tmpArea[2]*tmpArea[2]);
00682         if(norm2>1e-12)
00683           {
00684             tmpArea[0]/=norm2; tmpArea[1]/=norm2; tmpArea[2]/=norm2;
00685             double signOfArea=area[0]*tmpArea[0]+area[1]*tmpArea[1]+area[2]*tmpArea[2];
00686             res[0]+=signOfArea*norm2*v[0]/norm; res[1]+=signOfArea*norm2*v[1]/norm; res[2]+=signOfArea*norm2*v[2]/norm;
00687           }
00688       }   
00689   }
00690 }
00691 
00692 #endif