src/INTERP_KERNEL/PlanarIntersector.txx

   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 #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 minDot3DSurf, double medianPlane, bool doRotate, int orientation, int printLevel):
  34     _meshT(meshT),_meshS(meshS),
  35     _dim_caracteristic(dimCaracteristic),_max_distance_3Dsurf_intersect(md3DSurf),_min_dot_btw_3Dsurf_intersect(minDot3DSurf),_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,_min_dot_btw_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 minDot3DSurf, 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)
 294       i_A1++;
 295     int i_A2(i_A1+1);
 296     crossprod<SPACEDIM>(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2],normal_A);
 297     double normA(sqrt(dotprod<SPACEDIM>(normal_A,normal_A)));
 298     while(i_A2<nb_NodesA && normA < epsilon)
 299       {
 300         crossprod<SPACEDIM>(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2],normal_A);
 301         i_A2++;
 302         normA = sqrt(dotprod<SPACEDIM>(normal_A,normal_A));
 303
 304       }
 305     int i_B1(1);
 306     while(i_B1<nb_NodesB && distance2<SPACEDIM>(Coords_B,Coords_B+SPACEDIM*i_B1)< epsilon)
 307       i_B1++;
 308     int i_B2(i_B1+1);
 309     crossprod<SPACEDIM>(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2,normal_B);
 310     double normB(sqrt(dotprod<SPACEDIM>(normal_B,normal_B)));
 311     while(i_B2<nb_NodesB && normB < epsilon)
 312       {
 313         crossprod<SPACEDIM>(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2,normal_B);
 314         i_B2++;
 315         normB = sqrt(dotprod<SPACEDIM>(normal_B,normal_B));
 316       }
 317
 318     //fabien option
 319     if(md3DSurf>0.)
 320       {
 321         double coords_GA[3];
 322         for(int i=0;i<3;i++)
 323           {
 324             coords_GA[i]=0.;
 325             for (int j=0;j<nb_NodesA;j++)
 326               coords_GA[i]+=Coords_A[3*j+i];
 327             coords_GA[i]/=nb_NodesA;
 328           }
 329         double G1[3],G2[3],G3[3];
 330         for(int i=0;i<3;i++)
 331           {
 332             G1[i]=Coords_B[i]-coords_GA[i];
 333             G2[i]=Coords_B[i+3]-coords_GA[i];
 334             G3[i]=Coords_B[i+6]-coords_GA[i];
 335           }
 336         double prodvect[3];
 337         prodvect[0]=G1[1]*G2[2]-G1[2]*G2[1];
 338         prodvect[1]=G1[2]*G2[0]-G1[0]*G2[2];
 339         prodvect[2]=G1[0]*G2[1]-G1[1]*G2[0];
 340         double prodscal=prodvect[0]*G3[0]+prodvect[1]*G3[1]+prodvect[2]*G3[2];
 341         if(fabs(prodscal)>md3DSurf)
 342           return 0;
 343       }
 344     if(i_A2<nb_NodesA && i_B2<nb_NodesB)
 345       {
 346         //Build the normal of the median plane
 347
 348         double dotProd(dotprod<SPACEDIM>(normal_A,normal_B)/(normA*normB));
 349
 350         if(fabs(dotProd)<minDot3DSurf)
 351           return 0;
 352
 353         same_orientation=(dotProd>=0);
 354
 355         if(!same_orientation)
 356           for(int idim =0; idim< SPACEDIM; idim++)
 357             normal_A[idim] *=-1;
 358
 359         double normBB(sqrt(dotprod<SPACEDIM>(normal_B,normal_B)));
 360
 361         for(int idim =0; idim< SPACEDIM; idim++)
 362           linear_comb[idim] = median_plane*normal_A[idim]/normA + (1-median_plane)*normal_B[idim]/normBB;
 363         double norm= sqrt(dotprod<SPACEDIM>(linear_comb,linear_comb));
 364
 365         //Necessarily: norm>epsilon, no need to check
 366         for(int idim =0; idim< SPACEDIM; idim++)
 367           linear_comb[idim]/=norm;
 368
 369         //Project the nodes of A and B on the median plane
 370         for(int i_A=0; i_A<nb_NodesA; i_A++)
 371           {
 372             proj = dotprod<SPACEDIM>(&Coords_A[SPACEDIM*i_A],linear_comb);
 373             for(int idim =0; idim< SPACEDIM; idim++)
 374               Coords_A[SPACEDIM*i_A+idim] -=  proj*linear_comb[idim];
 375           }
 376         for(int i_B=0; i_B<nb_NodesB; i_B++)
 377           {
 378             proj = dotprod<SPACEDIM>(Coords_B+SPACEDIM*i_B,linear_comb);
 379             for(int idim =0; idim< SPACEDIM; idim++)
 380               Coords_B[SPACEDIM*i_B+idim] -=  proj*linear_comb[idim];
 381           }
 382
 383         //Buid the matrix sending  A into the Oxy plane and apply it to A and B
 384         if(do_rotate)
 385           {
 386             TranslationRotationMatrix rotation;
 387             //rotate3DTriangle(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2], rotation);
 388             Rotate3DTriangle(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2, rotation);
 389             for (int i=0; i<nb_NodesA; i++)
 390               rotation.transform_vector(Coords_A+SPACEDIM*i);
 391             for (int i=0; i<nb_NodesB; i++)
 392               rotation.transform_vector(Coords_B+SPACEDIM*i);
 393           }
 394         return same_orientation?1:-1;
 395       }
 396     else
 397       {
 398         std::cout << " Degenerated cell " << "epsilon = " << epsilon << std::endl;
 399         std::cout << " i_A1= " << i_A1 << " i_A2= " << i_A2 << std::endl;
 400         std::cout << " distance2<SPACEDIM>(Coords_A,&Coords_A[i_A1])= " <<  distance2<SPACEDIM>(Coords_A,&Coords_A[i_A1]) << std::endl;
 401         std::cout << "abs(normal_A) = " << fabs(normal_A[0]) << " ; " <<fabs( normal_A[1]) << " ; " << fabs(normal_A[2]) << std::endl;
 402         std::cout << " i_B1= " << i_B1 << " i_B2= " << i_B2 << std::endl;
 403         std::cout << " distance2<SPACEDIM>(&Coords_B[0],&Coords_B[i_B1])= " <<  distance2<SPACEDIM>(Coords_B,Coords_B+i_B1) << std::endl;
 404         std::cout << "normal_B = " << normal_B[0] << " ; " << normal_B[1] << " ; " << normal_B[2] << std::endl;
 405         return 1;
 406       }
 407   }
 408
 409   template<class MyMeshType, class MyMatrix>
 410   void PlanarIntersector<MyMeshType,MyMatrix>::Rotate3DTriangle(double* PP1, double*PP2, double*PP3,
 411                                                                 TranslationRotationMatrix& rotation_matrix)
 412   {
 413     //initializes
 414     rotation_matrix.translate(PP1);
 415
 416     double P2w[3];
 417     double P3w[3];
 418     P2w[0]=PP2[0]; P2w[1]=PP2[1];P2w[2]=PP2[2];
 419     P3w[0]=PP3[0]; P3w[1]=PP3[1];P3w[2]=PP3[2];
 420
 421     // translating to set P1 at the origin
 422     for (int i=0; i<3; i++)
 423       {
 424         P2w[i]-=PP1[i];
 425         P3w[i]-=PP1[i];
 426       }
 427
 428     // rotating to set P2 on the Oxy plane
 429     TranslationRotationMatrix A;
 430     A.rotate_x(P2w);
 431     A.rotate_vector(P3w);
 432     rotation_matrix.multiply(A);
 433
 434     //rotating to set P2 on the Ox axis
 435     TranslationRotationMatrix B;
 436     B.rotate_z(P2w);
 437     B.rotate_vector(P3w);
 438     rotation_matrix.multiply(B);
 439
 440     //rotating to set P3 on the Oxy plane
 441     TranslationRotationMatrix C;
 442     C.rotate_x(P3w);
 443     rotation_matrix.multiply(C);
 444   }
 445 }
 446
 447 #endif