src/INTERP_KERNEL/PlanarIntersector.txx

   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 #ifndef __PLANARINTERSECTOR_TXX__
  21 #define __PLANARINTERSECTOR_TXX__
  22
  23 #include "PlanarIntersector.hxx"
  24 #include "InterpolationUtils.hxx"
  25 #include "TranslationRotationMatrix.hxx"
  26
  27 #include <iostream>
  28 #include <limits>
  29
  30 namespace INTERP_KERNEL
  31 {
  32   template<class MyMeshType, class MyMatrix>
  33   PlanarIntersector<MyMeshType,MyMatrix>::PlanarIntersector(const MyMeshType& meshT, const MyMeshType& meshS, double dimCaracteristic, double precision, double md3DSurf, double medianPlane, bool doRotate, int orientation, int printLevel):
  34     _meshT(meshT),_meshS(meshS),
  35     _dim_caracteristic(dimCaracteristic),_max_distance_3Dsurf_intersect(md3DSurf),_precision(precision),_median_plane(medianPlane),
  36     _do_rotate(doRotate),_orientation(orientation),_print_level(printLevel)
  37   {
  38     _connectT=meshT.getConnectivityPtr();
  39     _connectS=meshS.getConnectivityPtr();
  40     _connIndexT=meshT.getConnectivityIndexPtr();
  41     _connIndexS=meshS.getConnectivityIndexPtr();
  42     _coordsT=meshT.getCoordinatesPtr();
  43     _coordsS=meshS.getCoordinatesPtr();
  44   }
  45
  46   template<class MyMeshType, class MyMatrix>
  47   PlanarIntersector<MyMeshType,MyMatrix>::~PlanarIntersector()
  48   {
  49   }
  50
  51   /*!
  52     \brief creates the bounding boxes for all the cells of mesh \a mesh
  53
  54     The method accepts mixed meshes (containing triangles and quadrangles).
  55     The vector returned is of dimension 6*nb_elems with bounding boxes stored as xmin1, xmax1, ymin1, ymax1, zmin1, zmax1, xmin2, xmax2, ymin2,...
  56     The returned pointer must be deleted by the calling code.
  57
  58     \param mesh structure pointing to the mesh
  59     \param bbox vector containing the bounding boxes
  60   */
  61   template<class MyMeshType, class MyMatrix>
  62   void PlanarIntersector<MyMeshType,MyMatrix>::createBoundingBoxes(const MyMeshType& mesh, std::vector<double>& bbox)
  63   {
  64     /* We build the segment tree for locating possible matching intersections*/
  65     long nbelems = mesh.getNumberOfElements();
  66     bbox.resize(2*SPACEDIM* nbelems);
  67     const double* coords = mesh.getCoordinatesPtr();
  68     const ConnType* conn = mesh.getConnectivityPtr();
  69     const ConnType* conn_index = mesh.getConnectivityIndexPtr();
  70     int ibox=0;
  71     for(long icell=0; icell<nbelems; icell++)
  72       {
  73         int nb_nodes_per_elem =conn_index[icell+1]-conn_index[icell];
  74         //initializing bounding box limits
  75         for(int idim=0; idim<SPACEDIM; idim++)
  76           {
  77             bbox[2*SPACEDIM*ibox+2*idim]   =  std::numeric_limits<double>::max();
  78             bbox[2*SPACEDIM*ibox+2*idim+1] = -std::numeric_limits<double>::max();
  79           }
  80         //updating the bounding box with each node of the element
  81         for (int j=0; j<nb_nodes_per_elem; j++)
  82           {
  83             const double* coord_node=coords+SPACEDIM*OTT<ConnType,numPol>::coo2C(conn[OTT<ConnType,numPol>::conn2C(conn_index[icell]+j)]);
  84             for(int idim=0; idim<SPACEDIM; idim++)
  85               {
  86                 double x=*(coord_node+idim);
  87                 bbox[ibox*2*SPACEDIM + 2*idim]   = (bbox[ibox*2*SPACEDIM + 2*idim]  <x)?bbox[ibox*2*SPACEDIM + 2*idim  ]:x;
  88                 bbox[ibox*2*SPACEDIM + 2*idim+1] = (bbox[ibox*2*SPACEDIM + 2*idim+1]>x)?bbox[ibox*2*SPACEDIM + 2*idim+1]:x;
  89               }
  90           }
  91         ibox++;
  92       }
  93   }
  94
  95   /*!
  96     Computes the bouding box of a given element. iP in numPol mode.
  97   */
  98   template<class MyMeshType, class MyMatrix>
  99   void PlanarIntersector<MyMeshType,MyMatrix>::getElemBB(double* bb, const MyMeshType& mesh, ConnType iP, ConnType nb_nodes)
 100   {
 101     const double* coords = mesh.getCoordinatesPtr();
 102     const ConnType* conn_index = mesh.getConnectivityIndexPtr();
 103     const ConnType* conn = mesh.getConnectivityPtr();
 104     //initializing bounding box limits
 105     for(int idim=0; idim<SPACEDIM; idim++)
 106       {
 107         bb[2*idim  ] =  std::numeric_limits<double>::max();
 108         bb[2*idim+1] = -std::numeric_limits<double>::max();
 109       }
 110
 111     for (ConnType i=0; i<nb_nodes; i++)
 112       {
 113         //MN: iP= cell index, not node index, use of connectivity array ?
 114         const double* coord_node=coords+SPACEDIM*(OTT<ConnType,numPol>::coo2C(conn[OTT<ConnType,numPol>::conn2C(conn_index[OTT<ConnType,numPol>::ind2C(iP)]+i)]));
 115         for(int idim=0; idim<SPACEDIM; idim++)
 116           {
 117             double x = *(coord_node+idim);
 118             //double y = *(mesh.getCoordinates(MED_FULL_INTERLACE)+3*(iP+i)+1);
 119             bb[2*idim  ] = (x<bb[2*idim  ])?x:bb[2*idim  ];
 120             bb[2*idim+1] = (x>bb[2*idim+1])?x:bb[2*idim+1];
 121           }
 122       }
 123   }
 124
 125   /*! Readjusts a set of bounding boxes so that they are extended
 126     in all dimensions for avoiding missing interesting intersections
 127
 128     \param bbox vector containing the bounding boxes
 129   */
 130   template<class MyMeshType, class MyMatrix>
 131   void PlanarIntersector<MyMeshType,MyMatrix>::adjustBoundingBoxes(std::vector<double>& bbox, double surf3DAdjustmentEps, double surf3DAdjustmentEpsAbs)
 132   {
 133     /* We build the segment tree for locating possible matching intersections*/
 134
 135     long size = bbox.size()/(2*SPACEDIM);
 136     for (int i=0; i<size; i++)
 137       {
 138         double max=- std::numeric_limits<double>::max();
 139         for(int idim=0; idim<SPACEDIM; idim++)
 140           {
 141             double Dx=bbox[i*2*SPACEDIM+1+2*idim]-bbox[i*2*SPACEDIM+2*idim];
 142             max=(max<Dx)?Dx:max;
 143           }
 144         for(int idim=0; idim<SPACEDIM; idim++)
 145           {
 146             bbox[i*2*SPACEDIM+2*idim  ] -= surf3DAdjustmentEps*max+surf3DAdjustmentEpsAbs;
 147             bbox[i*2*SPACEDIM+2*idim+1] += surf3DAdjustmentEps*max+surf3DAdjustmentEpsAbs;
 148           }
 149       }
 150   }
 151
 152   /*!
 153    * @param icellT id in target mesh in format of MyMeshType.
 154    * @param coordsT output val that stores coordinates of the target cell automatically resized to the right length.
 155    */
 156   template<class MyMeshType, class MyMatrix>
 157   void PlanarIntersector<MyMeshType,MyMatrix>::getRealTargetCoordinates(ConnType icellT, std::vector<double>& coordsT)
 158   {
 159     int nbNodesT=_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)+1]-_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)];
 160     coordsT.resize(SPACEDIM*nbNodesT);
 161     for (ConnType iT=0; iT<nbNodesT; iT++)
 162       for(int idim=0; idim<SPACEDIM; idim++)
 163         coordsT[SPACEDIM*iT+idim]=_coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
 164   }
 165
 166   /*!
 167    * @param icellS id in source mesh in format of MyMeshType.
 168    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
 169    */
 170   template<class MyMeshType, class MyMatrix>
 171   void PlanarIntersector<MyMeshType,MyMatrix>::getRealSourceCoordinates(ConnType icellS, std::vector<double>& coordsS)
 172   {
 173     int nbNodesS=_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)+1]-_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)];
 174     coordsS.resize(SPACEDIM*nbNodesS);
 175     for (ConnType iS=0; iS<nbNodesS; iS++)
 176       for(int idim=0; idim<SPACEDIM; idim++)
 177         coordsS[SPACEDIM*iS+idim]=_coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
 178   }
 179
 180   /*!
 181    * @param icellT id in target mesh in format of MyMeshType.
 182    * @param offset is a value in C format that indicates the number of circular permutation.
 183    * @param coordsT output val that stores coordinates of the target cell automatically resized to the right length.
 184    */
 185   template<class MyMeshType, class MyMatrix>
 186   void PlanarIntersector<MyMeshType,MyMatrix>::getRealTargetCoordinatesPermute(ConnType icellT, int offset, std::vector<double>& coordsT)
 187   {
 188     int nbNodesT=_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)+1]-_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)];
 189     coordsT.resize(SPACEDIM*nbNodesT);
 190     for (ConnType iTTmp=0; iTTmp<nbNodesT; iTTmp++)
 191       {
 192         ConnType iT=(iTTmp+offset)%nbNodesT;
 193         for(int idim=0; idim<SPACEDIM; idim++)
 194           coordsT[SPACEDIM*iTTmp+idim]=_coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
 195       }
 196   }
 197
 198   /*!
 199    * @param icellS id in source mesh in format of MyMeshType.
 200    * @param offset is a value in C format that indicates the number of circular permutation.
 201    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
 202    */
 203   template<class MyMeshType, class MyMatrix>
 204   void PlanarIntersector<MyMeshType,MyMatrix>::getRealSourceCoordinatesPermute(ConnType icellS, int offset, std::vector<double>& coordsS)
 205   {
 206     int nbNodesS=_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)+1]-_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)];
 207     coordsS.resize(SPACEDIM*nbNodesS);
 208     for (ConnType iSTmp=0; iSTmp<nbNodesS; iSTmp++)
 209       {
 210         ConnType iS=(iSTmp+offset)%nbNodesS;
 211         for(int idim=0; idim<SPACEDIM; idim++)
 212           coordsS[SPACEDIM*iSTmp+idim]=_coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
 213       }
 214   }
 215
 216   /*!
 217    * @param icellT id in target mesh in format of MyMeshType.
 218    * @param icellS id in source mesh in format of MyMeshType.
 219    * @param nbNodesT nb of nodes of the target cell.
 220    * @param nbNodesS nb of nodes of the source cell.
 221    * @param coordsT output val that stores coordinates of the target cell automatically resized to the right length.
 222    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
 223    * @param orientation is an output value too, only set if SPACEDIM==3.
 224    */
 225   template<class MyMeshType, class MyMatrix>
 226   void PlanarIntersector<MyMeshType,MyMatrix>::getRealCoordinates(ConnType icellT, ConnType icellS, ConnType nbNodesT, ConnType nbNodesS, std::vector<double>& coordsT, std::vector<double>& coordsS, int& orientation)
 227   {
 228     coordsT.resize(SPACEDIM*nbNodesT);
 229     coordsS.resize(SPACEDIM*nbNodesS);
 230     for (int idim=0; idim<SPACEDIM; idim++)
 231       {
 232         for (ConnType iT=0; iT<nbNodesT; iT++)
 233           coordsT[SPACEDIM*iT+idim] = _coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
 234         for (ConnType iS=0; iS<nbNodesS; iS++)
 235           coordsS[SPACEDIM*iS+idim] = _coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
 236       }
 237
 238     //project cells T and S on the median plane and rotate the median plane
 239     if(SPACEDIM==3)
 240       orientation = projectionThis(&coordsT[0], &coordsS[0], nbNodesT, nbNodesS);
 241
 242     //DEBUG PRINTS
 243     if(_print_level >= 3)
 244       {
 245         std::cout << std::endl << "Cell coordinates (possibly after projection)" << std::endl;
 246         std::cout << std::endl << "icellT= " << icellT << ", nb nodes T= " <<  nbNodesT << std::endl;
 247         for(int iT =0; iT< nbNodesT; iT++)
 248           {for (int idim=0; idim<SPACEDIM; idim++) std::cout << coordsT[SPACEDIM*iT+idim] << " "; std::cout << std::endl;}
 249         std::cout << std::endl << "icellS= " << icellS << ", nb nodes S= " <<  nbNodesS << std::endl;
 250         for(int iS =0; iS< nbNodesS; iS++)
 251           {for (int idim=0; idim<SPACEDIM; idim++) std::cout << coordsS[SPACEDIM*iS+idim]<< " "; std::cout << std::endl; }
 252       }
 253   }
 254
 255   /*!
 256    * Filtering out zero surfaces and badly oriented surfaces
 257    * _orientation = -1,0,1,2
 258    * -1 : the intersection is taken into account if target and cells have different orientation
 259    * 0 : the intersection is always taken into account
 260    * 1 : the intersection is taken into account if target and cells have the same orientation
 261    * 2 : the absolute value of intersection is always taken into account
 262    */
 263   template<class MyMeshType, class MyMatrix>
 264   double PlanarIntersector<MyMeshType,MyMatrix>::getValueRegardingOption(double val) const
 265   {
 266     if(_orientation==0)
 267       return val;
 268     if(_orientation==2)
 269       return fabs(val);
 270     if (( val > 0.0 && _orientation==1) || ( val < 0.0 && _orientation==-1 ))
 271       return _orientation*val;
 272     return 0.;
 273   }
 274
 275   template<class MyMeshType, class MyMatrix>
 276   int PlanarIntersector<MyMeshType,MyMatrix>::projectionThis(double *Coords_A, double *Coords_B, int nb_NodesA, int nb_NodesB)
 277   {
 278     return projection(Coords_A,Coords_B,nb_NodesA,nb_NodesB,_dim_caracteristic*_precision,_max_distance_3Dsurf_intersect,_median_plane,_do_rotate);
 279   }
 280
 281   template<class MyMeshType, class MyMatrix>
 282   int PlanarIntersector<MyMeshType,MyMatrix>::projection(double *Coords_A, double *Coords_B,
 283                                                          int nb_NodesA, int nb_NodesB, double epsilon, double md3DSurf, double median_plane, bool do_rotate)
 284   {
 285     double normal_A[3]={0,0,0};
 286     double normal_B[3]={0,0,0};
 287     double linear_comb[3];
 288     double proj;
 289     bool same_orientation;
 290
 291     //Find the normal to cells A and B
 292     int i_A1=1;
 293     while(i_A1<nb_NodesA && distance2<SPACEDIM>(Coords_A,&Coords_A[SPACEDIM*i_A1])< epsilon) i_A1++;
 294     int i_A2=i_A1+1;
 295     crossprod<SPACEDIM>(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2],normal_A);
 296     double normA = sqrt(dotprod<SPACEDIM>(normal_A,normal_A));
 297     while(i_A2<nb_NodesA && normA < epsilon)
 298       {
 299         crossprod<SPACEDIM>(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2],normal_A);
 300         i_A2++;
 301         normA = sqrt(dotprod<SPACEDIM>(normal_A,normal_A));
 302
 303       }
 304     int i_B1=1;
 305     while(i_B1<nb_NodesB && distance2<SPACEDIM>(Coords_B,Coords_B+SPACEDIM*i_B1)< epsilon) i_B1++;
 306     int i_B2=i_B1+1;
 307     crossprod<SPACEDIM>(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2,normal_B);
 308     double normB = sqrt(dotprod<SPACEDIM>(normal_B,normal_B));
 309     while(i_B2<nb_NodesB && normB < epsilon)
 310       {
 311         crossprod<SPACEDIM>(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2,normal_B);
 312         i_B2++;
 313         normB = sqrt(dotprod<SPACEDIM>(normal_B,normal_B));
 314       }
 315
 316     //fabien option
 317     if(md3DSurf>0.)
 318       {
 319         double coords_GA[3];
 320         for (int i=0;i<3;i++)
 321           {
 322             coords_GA[i]=0.;
 323             for (int j=0;j<nb_NodesA;j++)
 324               coords_GA[i]+=Coords_A[3*j+i];
 325             coords_GA[i]/=nb_NodesA;
 326           }
 327         double G1[3],G2[3],G3[3];
 328       for (int i=0;i<3;i++)
 329         {
 330           G1[i]=Coords_B[i]-coords_GA[i];
 331           G2[i]=Coords_B[i+3]-coords_GA[i];
 332           G3[i]=Coords_B[i+6]-coords_GA[i];
 333         }
 334       double prodvect[3];
 335       prodvect[0]=G1[1]*G2[2]-G1[2]*G2[1];
 336       prodvect[1]=G1[2]*G2[0]-G1[0]*G2[2];
 337       prodvect[2]=G1[0]*G2[1]-G1[1]*G2[0];
 338       double prodscal=prodvect[0]*G3[0]+prodvect[1]*G3[1]+prodvect[2]*G3[2];
 339       if(fabs(prodscal)>md3DSurf)
 340         return 0;
 341       }
 342     if(i_A2<nb_NodesA && i_B2<nb_NodesB)
 343       {
 344         //Build the normal of the median plane
 345         same_orientation = dotprod<SPACEDIM>(normal_A,normal_B)>=0;
 346
 347         if(!same_orientation)
 348           for(int idim =0; idim< SPACEDIM; idim++) normal_A[idim] *=-1;
 349
 350         double normBB= sqrt(dotprod<SPACEDIM>(normal_B,normal_B));
 351
 352         for(int idim =0; idim< SPACEDIM; idim++)
 353           linear_comb[idim] = median_plane*normal_A[idim]/normA + (1-median_plane)*normal_B[idim]/normBB;
 354         double norm= sqrt(dotprod<SPACEDIM>(linear_comb,linear_comb));
 355
 356         //Necessarily: norm>epsilon, no need to check
 357         for(int idim =0; idim< SPACEDIM; idim++) linear_comb[idim]/=norm;
 358
 359         //Project the nodes of A and B on the median plane
 360         for(int i_A=0; i_A<nb_NodesA; i_A++)
 361           {
 362             proj = dotprod<SPACEDIM>(&Coords_A[SPACEDIM*i_A],linear_comb);
 363             for(int idim =0; idim< SPACEDIM; idim++)
 364               Coords_A[SPACEDIM*i_A+idim] -=  proj*linear_comb[idim];
 365           }
 366         for(int i_B=0; i_B<nb_NodesB; i_B++)
 367           {
 368             proj = dotprod<SPACEDIM>(Coords_B+SPACEDIM*i_B,linear_comb);
 369             for(int idim =0; idim< SPACEDIM; idim++)
 370               Coords_B[SPACEDIM*i_B+idim] -=  proj*linear_comb[idim];
 371           }
 372
 373         //Buid the matrix sending  A into the Oxy plane and apply it to A and B
 374         if(do_rotate)
 375           {
 376             TranslationRotationMatrix rotation;
 377             //rotate3DTriangle(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2], rotation);
 378             rotate3DTriangle(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2, rotation);
 379             for (int i=0; i<nb_NodesA; i++)    rotation.transform_vector(Coords_A+SPACEDIM*i);
 380             for (int i=0; i<nb_NodesB; i++)    rotation.transform_vector(Coords_B+SPACEDIM*i);
 381           }
 382         if(same_orientation)
 383           return 1;
 384         else return -1;
 385       }
 386     else
 387       {
 388         std::cout << " Degenerated cell " << "epsilon = " << epsilon << std::endl;
 389         std::cout << " i_A1= " << i_A1 << " i_A2= " << i_A2 << std::endl;
 390         std::cout << " distance2<SPACEDIM>(Coords_A,&Coords_A[i_A1])= " <<  distance2<SPACEDIM>(Coords_A,&Coords_A[i_A1]) << std::endl;
 391         std::cout << "abs(normal_A) = " << fabs(normal_A[0]) << " ; " <<fabs( normal_A[1]) << " ; " << fabs(normal_A[2]) << std::endl;
 392         std::cout << " i_B1= " << i_B1 << " i_B2= " << i_B2 << std::endl;
 393         std::cout << " distance2<SPACEDIM>(&Coords_B[0],&Coords_B[i_B1])= " <<  distance2<SPACEDIM>(Coords_B,Coords_B+i_B1) << std::endl;
 394         std::cout << "normal_B = " << normal_B[0] << " ; " << normal_B[1] << " ; " << normal_B[2] << std::endl;
 395
 396         return 1;
 397       }
 398   }
 399
 400   template<class MyMeshType, class MyMatrix>
 401   void PlanarIntersector<MyMeshType,MyMatrix>::rotate3DTriangle(double* PP1, double*PP2, double*PP3,
 402                                                                 TranslationRotationMatrix& rotation_matrix)
 403   {
 404     //initializes
 405     rotation_matrix.translate(PP1);
 406
 407     double P2w[3];
 408     double P3w[3];
 409     P2w[0]=PP2[0]; P2w[1]=PP2[1];P2w[2]=PP2[2];
 410     P3w[0]=PP3[0]; P3w[1]=PP3[1];P3w[2]=PP3[2];
 411
 412     // translating to set P1 at the origin
 413     for (int i=0; i<3; i++)
 414       {
 415         P2w[i]-=PP1[i];
 416         P3w[i]-=PP1[i];
 417       }
 418
 419     // rotating to set P2 on the Oxy plane
 420     TranslationRotationMatrix A;
 421     A.rotate_x(P2w);
 422     A.rotate_vector(P3w);
 423     rotation_matrix.multiply(A);
 424
 425     //rotating to set P2 on the Ox axis
 426     TranslationRotationMatrix B;
 427     B.rotate_z(P2w);
 428     B.rotate_vector(P3w);
 429     rotation_matrix.multiply(B);
 430
 431     //rotating to set P3 on the Oxy plane
 432     TranslationRotationMatrix C;
 433     C.rotate_x(P3w);
 434     rotation_matrix.multiply(C);
 435   }
 436 }
 437
 438 #endif