src/INTERP_KERNEL/VolSurfUser.cxx

   1 // Copyright (C) 2007-2022  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, or (at your option) any later version.
   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 #include "VolSurfUser.hxx"
  22 #include "InterpKernelAutoPtr.hxx"
  23 #include "InterpolationUtils.hxx"
  24 #include "VectorUtils.hxx"
  25
  26 #include <cmath>
  27 #include <limits>
  28 #include <algorithm>
  29 #include <functional>
  30
  31 namespace INTERP_KERNEL
  32 {
  33   /* Orthogonal distance from a point to a plane defined by three points p1, p2, p3.
  34    * Returns a signed distance, the normal of the plane being defined by (p1-p2)x(p3-p2)
  35    */
  36   double OrthoDistanceFromPtToPlaneInSpaceDim3(const double *p, const double *p1, const double *p2, const double *p3)
  37   {
  38     double prec = 1.0e-14;
  39     double T[2][3] = {{p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]},
  40                       {p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]}};
  41     double N[3] = {T[0][1]*T[1][2]-T[0][2]*T[1][1],
  42                    T[0][2]*T[1][0]-T[0][0]*T[1][2],
  43                    T[0][0]*T[1][1]-T[0][1]*T[1][0]};
  44
  45     double norm2 = N[0]*N[0] + N[1]*N[1] + N[2]*N[2];
  46     if (norm2 < prec)
  47       throw INTERP_KERNEL::Exception("OrthoDistanceFromPtToPlaneInSpaceDim3: degenerated normal vector!");
  48     double num = N[0]*(p[0]-p1[0]) + N[1]*(p[1]-p1[1]) + N[2]*(p[2]-p1[2]);
  49     return num/sqrt(norm2);
  50   }
  51
  52   double SquareDistanceFromPtToSegInSpaceDim2(const double *pt, const double *pt0Seg2, const double *pt1Seg2, std::size_t &nbOfHint)
  53   {
  54     double dx=pt1Seg2[0]-pt0Seg2[0],dy=pt1Seg2[1]-pt0Seg2[1];
  55     double norm=sqrt(dx*dx+dy*dy);
  56     if(norm==0.)
  57       return (pt[0]-pt0Seg2[0])*(pt[0]-pt0Seg2[0])+(pt[1]-pt0Seg2[1])*(pt[1]-pt0Seg2[1]);//return std::numeric_limits<double>::max();
  58     dx/=norm; dy/=norm;
  59     double dx2=pt[0]-pt0Seg2[0],dy2=pt[1]-pt0Seg2[1];
  60     double dotP=(dx2*dx+dy2*dy);
  61     if(dotP<0. || dotP>norm)
  62       return dotP<0.?(pt[0]-pt0Seg2[0])*(pt[0]-pt0Seg2[0])+(pt[1]-pt0Seg2[1])*(pt[1]-pt0Seg2[1]):(pt[0]-pt1Seg2[0])*(pt[0]-pt1Seg2[0])+(pt[1]-pt1Seg2[1])*(pt[1]-pt1Seg2[1]);
  63     nbOfHint++;
  64     double x=pt0Seg2[0]+dotP*dx,y=pt0Seg2[1]+dotP*dy;
  65     return (x-pt[0])*(x-pt[0])+(y-pt[1])*(y-pt[1]);
  66   }
  67
  68   /**
  69    * See http://geomalgorithms.com/a02-_lines.html#Distance-to-Ray-or-Segment
  70    */
  71   double DistanceFromPtToSegInSpaceDim3(const double *pt, const double *pt0Seg2, const double *pt1Seg2)
  72   {
  73     double v[3], w[3];
  74     for(int i=0; i < 3; i++) {
  75         v[i]=pt1Seg2[i]-pt0Seg2[i];
  76         w[i] = pt[i] - pt0Seg2[i];
  77     }
  78
  79     double c1 = dotprod<3>(w,v);
  80     if ( c1 <= 0 )
  81       return norm<3>(w);
  82     double c2 = dotprod<3>(v,v);
  83     if ( c2 <= c1 )
  84       {
  85         for(int i=0; i < 3; i++)
  86           w[i] = pt[i] - pt1Seg2[i];
  87         return norm<3>(w);
  88       }
  89     double b = c1 / c2;
  90     for(int i=0; i < 3; i++)
  91       w[i] = pt0Seg2[i] + b * v[i] - pt[i];
  92     return norm<3>(w);
  93   }
  94
  95   /**
  96      Helper for DistanceFromPtToTriInSpaceDim3
  97    */
  98   inline double _HelperDistancePtToTri3D_1(const double aXX, const double bX, const double c)
  99   {
 100     if (bX >= 0)
 101       return c;
 102     if (-bX >= aXX)
 103       return aXX + 2*bX + c;
 104     return bX*(-bX / aXX) + c;
 105   }
 106
 107   /**
 108     Helper for DistanceFromPtToTriInSpaceDim3
 109    */
 110   inline double _HelperDistancePtToTri3D_2(const double a01, const double aXX, const double aYY,
 111                                            const double bX, const double bY, const double c)
 112   {
 113     double tmp0 = a01 + bX, tmp1 = aXX + bY;
 114     if (tmp1 > tmp0) {
 115         double numer = tmp1 - tmp0, denom = aXX - 2*a01 + aYY;
 116         if (numer >= denom)
 117           return aXX + 2*bX + c;
 118         else {
 119             double s, t;
 120             s = numer / denom; t = 1 - s;
 121             return s*(aXX*s + a01*t + 2*bX) + t*(a01*s + aYY*t + 2*bY) + c;
 122         }
 123     }
 124     else
 125       {
 126         if (tmp1 <= 0)   return aYY + 2*bY + c;
 127         else {
 128             if (bY >= 0) return c;
 129             else         return bY*(-bY / aYY) + c;
 130         }
 131       }
 132   }
 133
 134   /**
 135    * From https://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
 136    */
 137   double DistanceFromPtToTriInSpaceDim3(const double *pt, const double *pt0Tri3, const double *pt1Tri3, const double *pt2Tri3)
 138   {
 139     double diff[3], edge0[3], edge1[3];
 140     for(int i=0; i < 3; i++) diff[i]=pt0Tri3[i]-pt[i];
 141     for(int i=0; i < 3; i++) edge0[i]=pt1Tri3[i]-pt0Tri3[i];
 142     for(int i=0; i < 3; i++) edge1[i]=pt2Tri3[i]-pt0Tri3[i];
 143
 144     double a00=dotprod<3>(edge0, edge0), a01=dotprod<3>(edge0,edge1), a11=dotprod<3>(edge1,edge1);
 145     double b0=dotprod<3>(diff, edge0), b1=dotprod<3>(diff, edge1), c=dotprod<3>(diff, diff);
 146     double det = fabs(a00*a11 - a01*a01);
 147     double s = a01*b1 - a11*b0, t = a01*b0 - a00*b1;
 148     double sDist;
 149
 150     if (s + t <= det)
 151       {
 152         if (s < 0)  {
 153             if (t < 0) { // region 4
 154                 if (b0 < 0) {
 155                     if (-b0 >= a00)  sDist = a00 + 2*b0 + c;
 156                     else             sDist = b0*(-b0 / a00) + c;
 157                   }
 158                 else
 159                   sDist = _HelperDistancePtToTri3D_1(a11, b1, c);
 160               }
 161             else  // region 3
 162               sDist = _HelperDistancePtToTri3D_1(a11, b1, c);
 163           }
 164         else       {
 165             if (t < 0)  // region 5
 166               sDist = _HelperDistancePtToTri3D_1(a00, b0, c);
 167             else  // region 0
 168               {
 169                 // minimum at interior point
 170                 if (fabs(det) < 1.0e-12)
 171                   {
 172                     // points are colinear (degenerated triangle)
 173                     // => Compute distance between segments
 174                      double distance = std::min(DistanceFromPtToSegInSpaceDim3(pt, pt0Tri3, pt1Tri3),
 175                                                 DistanceFromPtToSegInSpaceDim3(pt, pt1Tri3, pt2Tri3));
 176                      return distance;
 177                   }
 178
 179                 // else we can divide by non-zero
 180                 double invDet = 1 / det;
 181                 s *= invDet;    t *= invDet;
 182                 sDist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
 183               }
 184           }
 185       }
 186     else  // s+t > det
 187       {
 188         if (s < 0.0)  // region 2
 189           sDist = _HelperDistancePtToTri3D_2(a01, a00, a11, b0, b1, c);
 190         else {
 191             if (t < 0.0)  // region 6
 192               sDist = _HelperDistancePtToTri3D_2(a01, a11, a00, b1, b0, c);
 193             else {  // region 1
 194                 double numer = a11 + b1 - a01 - b0;
 195                 if (numer <= 0.0)
 196                   sDist = a11 + 2*b1 + c;
 197                 else {
 198                     double denom = a00 - 2*a01 + a11;
 199                     if (numer >= denom)
 200                       sDist = a00 + 2*b0 + c;
 201                     else {
 202                         s = numer / denom; t = 1 - s;
 203                         sDist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
 204                     }
 205                 }
 206             }
 207         }
 208       }
 209     // Account for numerical round-off error.
 210     if (sDist < 0)
 211       sDist = 0.0;
 212
 213     return sqrt(sDist);
 214   }
 215
 216   double DistanceFromPtToPolygonInSpaceDim3(const double *pt, const mcIdType *connOfPolygonBg, const mcIdType *connOfPolygonEnd, const double *coords)
 217   {
 218     std::size_t nbOfEdges=std::distance(connOfPolygonBg,connOfPolygonEnd);
 219     if(nbOfEdges<3)
 220       throw INTERP_KERNEL::Exception("DistanceFromPtToPolygonInSpaceDim3 : trying to compute a distance to a polygon containing less than 3 edges !");
 221     double baryOfNodes[3]={0.,0.,0.};
 222     for(std::size_t i=0;i<nbOfEdges;i++)
 223       { baryOfNodes[0]+=coords[3*connOfPolygonBg[i]]; baryOfNodes[1]+=coords[3*connOfPolygonBg[i]+1]; baryOfNodes[2]+=coords[3*connOfPolygonBg[i]+2]; }
 224     std::transform(baryOfNodes,baryOfNodes+3,baryOfNodes,std::bind(std::multiplies<double>(),std::placeholders::_1,1./((double)nbOfEdges)));
 225     double matrix[12];
 226     if(!ComputeRotTranslationMatrixToPut3PointsOnOXY(coords+3*connOfPolygonBg[0],coords+3*connOfPolygonBg[1],baryOfNodes,matrix))
 227       return std::numeric_limits<double>::max();
 228     INTERP_KERNEL::AutoPtr<double> ptXY=new double[2*nbOfEdges]; ptXY[0]=0.; ptXY[1]=0.;
 229     ptXY[2]=matrix[0]*coords[3*connOfPolygonBg[1]]+matrix[1]*coords[3*connOfPolygonBg[1]+1]+matrix[2]*coords[3*connOfPolygonBg[1]+2]+matrix[3]; ptXY[3]=0.;
 230     for(std::size_t i=2;i<nbOfEdges;i++)
 231       {
 232         ptXY[2*i]=matrix[0]*coords[3*connOfPolygonBg[i]]+matrix[1]*coords[3*connOfPolygonBg[i]+1]+matrix[2]*coords[3*connOfPolygonBg[i]+2]+matrix[3];
 233         ptXY[2*i+1]=matrix[4]*coords[3*connOfPolygonBg[i]]+matrix[5]*coords[3*connOfPolygonBg[i]+1]+matrix[6]*coords[3*connOfPolygonBg[i]+2]+matrix[7];
 234       }
 235     double xy[2]={matrix[0]*pt[0]+matrix[1]*pt[1]+matrix[2]*pt[2]+matrix[3],matrix[4]*pt[0]+matrix[5]*pt[1]+matrix[6]*pt[2]+matrix[7]};
 236     double z=matrix[8]*pt[0]+matrix[9]*pt[1]+matrix[10]*pt[2]+matrix[11];
 237     double ret=std::numeric_limits<double>::max();
 238     std::size_t nbOfHint=0;
 239     for(std::size_t i=0;i<nbOfEdges;i++)
 240       {
 241         double tmp=SquareDistanceFromPtToSegInSpaceDim2(xy,((double *)ptXY)+2*i,((double *)ptXY)+2*((i+1)%nbOfEdges),nbOfHint);
 242         ret=std::min(ret,z*z+tmp);
 243       }
 244     if(nbOfHint==nbOfEdges)
 245       ret=std::min(ret,z*z);
 246     return sqrt(ret);
 247   }
 248
 249   /*!
 250    * \param [out] matrix contain a dense matrix of size 12 with 3 rows containing each 4 columns. This matrix is the reduction of 4x4 matrix but the last
 251    *              line containing [0,0,0,1] is omitted.
 252    */
 253   bool ComputeRotTranslationMatrixToPut3PointsOnOXY(const double *p0, const double *p1, const double *p2, double *matrix)
 254   {
 255     double norm=sqrt((p1[0]-p0[0])*(p1[0]-p0[0])+(p1[1]-p0[1])*(p1[1]-p0[1])+(p1[2]-p0[2])*(p1[2]-p0[2]));
 256     double c=(p1[0]-p0[0])/norm;
 257     double s=sqrt(1-c*c);
 258     double y=p1[2]-p0[2],z=p0[1]-p1[1];
 259     norm=sqrt(y*y+z*z);
 260     if(norm!=0.)
 261       { y/=norm; z/=norm; }
 262     double r0[9]={c,-z*s,y*s,
 263                   z*s,y*y*(1-c)+c,y*z*(1-c),
 264                   -y*s,z*y*(1-c),z*z*(1-c)+c};
 265     // 2nd rotation matrix
 266     double x=p2[0]-p0[0];
 267     y=p2[1]-p0[1]; z=p2[2]-p0[2];
 268     double y1=x*r0[3]+y*r0[4]+z*r0[5],z1=x*r0[6]+y*r0[7]+z*r0[8];
 269     c=y1/sqrt(y1*y1+z1*z1);
 270     s=sqrt(1.-c*c);
 271     //
 272     std::copy(r0,r0+3,matrix);
 273     matrix[4]=c*r0[3]-s*r0[6]; matrix[5]=c*r0[4]-s*r0[7]; matrix[6]=c*r0[5]-s*r0[8];
 274     matrix[8]=s*r0[3]+c*r0[6]; matrix[9]=s*r0[4]+c*r0[7]; matrix[10]=s*r0[5]+c*r0[8];
 275     matrix[3]=-p0[0]*matrix[0]-p0[1]*matrix[1]-p0[2]*matrix[2];
 276     matrix[7]=-p0[0]*matrix[4]-p0[1]*matrix[5]-p0[2]*matrix[6];
 277     matrix[11]=-p0[0]*matrix[8]-p0[1]*matrix[9]-p0[2]*matrix[10];
 278     return true;
 279   }
 280
 281   template<int SPACEDIM>
 282   void ComputeTriangleHeight(const double *PA, const double *PB, const double *PC, double *res)
 283   {
 284     double AB = getDistanceBtw2Pts<SPACEDIM>(PA,PB);
 285     double BC = getDistanceBtw2Pts<SPACEDIM>(PB,PC);
 286     double CA = getDistanceBtw2Pts<SPACEDIM>(PC,PA);
 287     double perim( (AB+BC+CA)*0.5 );
 288     double num( 2*sqrt(perim*(perim-AB)*(perim-BC)*(perim-CA)) );
 289     res[0] = num/AB; res[1] = num/BC; res[2] = num/CA;
 290   }
 291 }