src/INTERP_KERNEL/VolSurfUser.cxx

   1 // Copyright (C) 2007-2016  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
  24 #include <cmath>
  25 #include <limits>
  26 #include <algorithm>
  27 #include <functional>
  28
  29 namespace INTERP_KERNEL
  30 {
  31   /* Orthogonal distance from a point to a plane defined by three points p1, p2, p3.
  32    * Returns a signed distance, the normal of the plane being defined by (p1-p2)x(p3-p2)
  33    */
  34   double OrthoDistanceFromPtToPlaneInSpaceDim3(const double *p, const double *p1, const double *p2, const double *p3)
  35   {
  36     double prec = 1.0e-14;
  37     double T[2][3] = {{p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]},
  38                       {p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]}};
  39     double N[3] = {T[0][1]*T[1][2]-T[0][2]*T[1][1],
  40                    T[0][2]*T[1][0]-T[0][0]*T[1][2],
  41                    T[0][0]*T[1][1]-T[0][1]*T[1][0]};
  42
  43     double norm2 = N[0]*N[0] + N[1]*N[1] + N[2]*N[2];
  44     if (norm2 < prec)
  45       throw INTERP_KERNEL::Exception("OrthoDistanceFromPtToPlaneInSpaceDim3: degenerated normal vector!");
  46     double num = N[0]*(p[0]-p1[0]) + N[1]*(p[1]-p1[1]) + N[2]*(p[2]-p1[2]);
  47     return num/sqrt(norm2);
  48   }
  49
  50   double SquareDistanceFromPtToSegInSpaceDim2(const double *pt, const double *pt0Seg2, const double *pt1Seg2, std::size_t &nbOfHint)
  51   {
  52     double dx=pt1Seg2[0]-pt0Seg2[0],dy=pt1Seg2[1]-pt0Seg2[1];
  53     double norm=sqrt(dx*dx+dy*dy);
  54     if(norm==0.)
  55       return (pt[0]-pt0Seg2[0])*(pt[0]-pt0Seg2[0])+(pt[1]-pt0Seg2[1])*(pt[1]-pt0Seg2[1]);//return std::numeric_limits<double>::max();
  56     dx/=norm; dy/=norm;
  57     double dx2=pt[0]-pt0Seg2[0],dy2=pt[1]-pt0Seg2[1];
  58     double dotP=(dx2*dx+dy2*dy);
  59     if(dotP<0. || dotP>norm)
  60       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]);
  61     nbOfHint++;
  62     double x=pt0Seg2[0]+dotP*dx,y=pt0Seg2[1]+dotP*dy;
  63     return (x-pt[0])*(x-pt[0])+(y-pt[1])*(y-pt[1]);
  64   }
  65
  66   double DistanceFromPtToTriInSpaceDim3(const double *pt, const double *pt0Tri3, const double *pt1Tri3, const double *pt2Tri3)
  67   {
  68     double matrix[12];
  69     if(!ComputeRotTranslationMatrixToPut3PointsOnOXY(pt0Tri3,pt1Tri3,pt2Tri3,matrix))
  70       return std::numeric_limits<double>::max();
  71     double xy0[2],xy1[2],xy2[2],xy[2]; xy0[0]=0.; xy0[1]=0.;
  72     xy1[0]=matrix[0]*pt1Tri3[0]+matrix[1]*pt1Tri3[1]+matrix[2]*pt1Tri3[2]+matrix[3]; xy1[1]=0.;
  73     xy2[0]=matrix[0]*pt2Tri3[0]+matrix[1]*pt2Tri3[1]+matrix[2]*pt2Tri3[2]+matrix[3];
  74     xy2[1]=matrix[4]*pt2Tri3[0]+matrix[5]*pt2Tri3[1]+matrix[6]*pt2Tri3[2]+matrix[7];
  75     xy[0]=matrix[0]*pt[0]+matrix[1]*pt[1]+matrix[2]*pt[2]+matrix[3];
  76     xy[1]=matrix[4]*pt[0]+matrix[5]*pt[1]+matrix[6]*pt[2]+matrix[7];
  77     double z=matrix[8]*pt[0]+matrix[9]*pt[1]+matrix[10]*pt[2]+matrix[11];
  78     double ret=std::numeric_limits<double>::max();
  79     std::size_t nbOfHint=0;
  80     if(xy[0]>0. && xy[0]<xy1[0])
  81       { ret=std::min(ret,z*z+xy[1]*xy[1]); nbOfHint++; } //distance pt to edge [pt0Tri3,pt1Tri3]
  82     double tmp=SquareDistanceFromPtToSegInSpaceDim2(xy,xy1,xy2,nbOfHint); //distance pt to edge [pt1Tri3,pt2Tri3]
  83     ret=std::min(ret,z*z+tmp);
  84     tmp=SquareDistanceFromPtToSegInSpaceDim2(xy,xy2,xy0,nbOfHint);//distance pt to edge [pt2Tri3,pt0Tri3]
  85     ret=std::min(ret,z*z+tmp);
  86     if(nbOfHint==3)
  87       ret=std::min(ret,z*z);
  88   return sqrt(ret);
  89   }
  90
  91   double DistanceFromPtToPolygonInSpaceDim3(const double *pt, const int *connOfPolygonBg, const int *connOfPolygonEnd, const double *coords)
  92   {
  93     std::size_t nbOfEdges=std::distance(connOfPolygonBg,connOfPolygonEnd);
  94     if(nbOfEdges<3)
  95       throw INTERP_KERNEL::Exception("DistanceFromPtToPolygonInSpaceDim3 : trying to compute a distance to a polygon containing less than 3 edges !");
  96     double baryOfNodes[3]={0.,0.,0.};
  97     for(std::size_t i=0;i<nbOfEdges;i++)
  98       { baryOfNodes[0]+=coords[3*connOfPolygonBg[i]]; baryOfNodes[1]+=coords[3*connOfPolygonBg[i]+1]; baryOfNodes[2]+=coords[3*connOfPolygonBg[i]+2]; }
  99     std::transform(baryOfNodes,baryOfNodes+3,baryOfNodes,std::bind2nd(std::multiplies<double>(),1./((double)nbOfEdges)));
 100     double matrix[12];
 101     if(!ComputeRotTranslationMatrixToPut3PointsOnOXY(coords+3*connOfPolygonBg[0],coords+3*connOfPolygonBg[1],baryOfNodes,matrix))
 102       return std::numeric_limits<double>::max();
 103     INTERP_KERNEL::AutoPtr<double> ptXY=new double[2*nbOfEdges]; ptXY[0]=0.; ptXY[1]=0.;
 104     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.;
 105     for(std::size_t i=2;i<nbOfEdges;i++)
 106       {
 107         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];
 108         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];
 109       }
 110     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]};
 111     double z=matrix[8]*pt[0]+matrix[9]*pt[1]+matrix[10]*pt[2]+matrix[11];
 112     double ret=std::numeric_limits<double>::max();
 113     std::size_t nbOfHint=0;
 114     for(std::size_t i=0;i<nbOfEdges;i++)
 115       {
 116         double tmp=SquareDistanceFromPtToSegInSpaceDim2(xy,((double *)ptXY)+2*i,((double *)ptXY)+2*((i+1)%nbOfEdges),nbOfHint);
 117         ret=std::min(ret,z*z+tmp);
 118       }
 119     if(nbOfHint==nbOfEdges)
 120       ret=std::min(ret,z*z);
 121     return sqrt(ret);
 122   }
 123
 124   /*!
 125    * \param [out] matrix contain a dense matrix of size 12 with 3 rows containing each 4 colums. This matrix is the reduction of 4x4 matrix but the last
 126    *              line containing [0,0,0,1] is omitted.
 127    */
 128   bool ComputeRotTranslationMatrixToPut3PointsOnOXY(const double *p0, const double *p1, const double *p2, double *matrix)
 129   {
 130     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]));
 131     double c=(p1[0]-p0[0])/norm;
 132     double s=sqrt(1-c*c);
 133     double y=p1[2]-p0[2],z=p0[1]-p1[1];
 134     norm=sqrt(y*y+z*z);
 135     if(norm!=0.)
 136       { y/=norm; z/=norm; }
 137     double r0[9]={c,-z*s,y*s,
 138                   z*s,y*y*(1-c)+c,y*z*(1-c),
 139                   -y*s,z*y*(1-c),z*z*(1-c)+c};
 140     // 2nd rotation matrix
 141     double x=p2[0]-p0[0];
 142     y=p2[1]-p0[1]; z=p2[2]-p0[2];
 143     double y1=x*r0[3]+y*r0[4]+z*r0[5],z1=x*r0[6]+y*r0[7]+z*r0[8];
 144     c=y1/sqrt(y1*y1+z1*z1);
 145     s=sqrt(1.-c*c);
 146     //
 147     std::copy(r0,r0+3,matrix);
 148     matrix[4]=c*r0[3]-s*r0[6]; matrix[5]=c*r0[4]-s*r0[7]; matrix[6]=c*r0[5]-s*r0[8];
 149     matrix[8]=s*r0[3]+c*r0[6]; matrix[9]=s*r0[4]+c*r0[7]; matrix[10]=s*r0[5]+c*r0[8];
 150     matrix[3]=-p0[0]*matrix[0]-p0[1]*matrix[1]-p0[2]*matrix[2];
 151     matrix[7]=-p0[0]*matrix[4]-p0[1]*matrix[5]-p0[2]*matrix[6];
 152     matrix[11]=-p0[0]*matrix[8]-p0[1]*matrix[9]-p0[2]*matrix[10];
 153     return true;
 154   }
 155 }
 156