src/INTERP_KERNEL/PlanarIntersector.txx

   1 // Copyright (C) 2007-2024  CEA, EDF
   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         ConnType 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 (ConnType 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 bounding 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   /*!
 126    * @param icellT id in target mesh in format of MyMeshType.
 127    * @param coordsT output val that stores coordinates of the target cell automatically resized to the right length.
 128    */
 129   template<class MyMeshType, class MyMatrix>
 130   void PlanarIntersector<MyMeshType,MyMatrix>::getRealTargetCoordinates(ConnType icellT, std::vector<double>& coordsT)
 131   {
 132     ConnType nbNodesT=_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)+1]-_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)];
 133     coordsT.resize(SPACEDIM*nbNodesT);
 134     for (ConnType iT=0; iT<nbNodesT; iT++)
 135       for(int idim=0; idim<SPACEDIM; idim++)
 136         coordsT[SPACEDIM*iT+idim]=_coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
 137   }
 138
 139   /*!
 140    * @param icellS id in source mesh in format of MyMeshType.
 141    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
 142    */
 143   template<class MyMeshType, class MyMatrix>
 144   void PlanarIntersector<MyMeshType,MyMatrix>::getRealSourceCoordinates(ConnType icellS, std::vector<double>& coordsS)
 145   {
 146     ConnType nbNodesS=_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)+1]-_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)];
 147     coordsS.resize(SPACEDIM*nbNodesS);
 148     for (ConnType iS=0; iS<nbNodesS; iS++)
 149       for(int idim=0; idim<SPACEDIM; idim++)
 150         coordsS[SPACEDIM*iS+idim]=_coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
 151   }
 152
 153   /*!
 154    * @param icellT id in target mesh in format of MyMeshType.
 155    * @param offset is a value in C format that indicates the number of circular permutation.
 156    * @param coordsT output val that stores coordinates of the target cell automatically resized to the right length.
 157    */
 158   template<class MyMeshType, class MyMatrix>
 159   void PlanarIntersector<MyMeshType,MyMatrix>::getRealTargetCoordinatesPermute(ConnType icellT, ConnType offset, std::vector<double>& coordsT)
 160   {
 161     ConnType nbNodesT=_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)+1]-_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)];
 162     coordsT.resize(SPACEDIM*nbNodesT);
 163     for (ConnType iTTmp=0; iTTmp<nbNodesT; iTTmp++)
 164       {
 165         ConnType iT=(iTTmp+offset)%nbNodesT;
 166         for(int idim=0; idim<SPACEDIM; idim++)
 167           coordsT[SPACEDIM*iTTmp+idim]=_coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
 168       }
 169   }
 170
 171   /*!
 172    * @param icellS id in source mesh in format of MyMeshType.
 173    * @param offset is a value in C format that indicates the number of circular permutation.
 174    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
 175    */
 176   template<class MyMeshType, class MyMatrix>
 177   void PlanarIntersector<MyMeshType,MyMatrix>::getRealSourceCoordinatesPermute(ConnType icellS, ConnType offset, std::vector<double>& coordsS)
 178   {
 179     ConnType nbNodesS=_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)+1]-_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)];
 180     coordsS.resize(SPACEDIM*nbNodesS);
 181     for (ConnType iSTmp=0; iSTmp<nbNodesS; iSTmp++)
 182       {
 183         ConnType iS=(iSTmp+offset)%nbNodesS;
 184         for(int idim=0; idim<SPACEDIM; idim++)
 185           coordsS[SPACEDIM*iSTmp+idim]=_coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
 186       }
 187   }
 188
 189   /*!
 190    * @param icellT id in target mesh in format of MyMeshType.
 191    * @param icellS id in source mesh in format of MyMeshType.
 192    * @param nbNodesT nb of nodes of the target cell.
 193    * @param nbNodesS nb of nodes of the source cell.
 194    * @param coordsT output val that stores coordinates of the target cell automatically resized to the right length.
 195    * @param coordsS output val that stores coordinates of the source cell automatically resized to the right length.
 196    * @param orientation is an output value too, only set if SPACEDIM==3.
 197    */
 198   template<class MyMeshType, class MyMatrix>
 199   void PlanarIntersector<MyMeshType,MyMatrix>::getRealCoordinates(ConnType icellT, ConnType icellS, ConnType nbNodesT, ConnType nbNodesS, std::vector<double>& coordsT, std::vector<double>& coordsS, int& orientation)
 200   {
 201     coordsT.resize(SPACEDIM*nbNodesT);
 202     coordsS.resize(SPACEDIM*nbNodesS);
 203     for (int idim=0; idim<SPACEDIM; idim++)
 204       {
 205         for (ConnType iT=0; iT<nbNodesT; iT++)
 206           coordsT[SPACEDIM*iT+idim] = _coordsT[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectT[OTT<ConnType,numPol>::conn2C(_connIndexT[OTT<ConnType,numPol>::ind2C(icellT)]+iT)])+idim];
 207         for (ConnType iS=0; iS<nbNodesS; iS++)
 208           coordsS[SPACEDIM*iS+idim] = _coordsS[SPACEDIM*OTT<ConnType,numPol>::coo2C(_connectS[OTT<ConnType,numPol>::conn2C(_connIndexS[OTT<ConnType,numPol>::ind2C(icellS)]+iS)])+idim];
 209       }
 210
 211     //project cells T and S on the median plane and rotate the median plane
 212     if(SPACEDIM==3)
 213       orientation = projectionThis(&coordsT[0], &coordsS[0], nbNodesT, nbNodesS);
 214
 215     //DEBUG PRINTS
 216     if(_print_level >= 3)
 217       {
 218         std::cout << std::endl << "Cell coordinates (possibly after projection)" << std::endl;
 219         std::cout << std::endl << "icellT= " << icellT << ", nb nodes T= " <<  nbNodesT << std::endl;
 220         for(int iT =0; iT< nbNodesT; iT++)
 221           {for (int idim=0; idim<SPACEDIM; idim++) std::cout << coordsT[SPACEDIM*iT+idim] << " "; std::cout << std::endl;}
 222         std::cout << std::endl << "icellS= " << icellS << ", nb nodes S= " <<  nbNodesS << std::endl;
 223         for(int iS =0; iS< nbNodesS; iS++)
 224           {for (int idim=0; idim<SPACEDIM; idim++) std::cout << coordsS[SPACEDIM*iS+idim]<< " "; std::cout << std::endl; }
 225       }
 226   }
 227
 228   /*!
 229    * Filtering out zero surfaces and badly oriented surfaces
 230    * _orientation = -1,0,1,2
 231    * -1 : the intersection is taken into account if target and cells have different orientation
 232    * 0 : the intersection is always taken into account
 233    * 1 : the intersection is taken into account if target and cells have the same orientation
 234    * 2 : the absolute value of intersection is always taken into account
 235    */
 236   template<class MyMeshType, class MyMatrix>
 237   double PlanarIntersector<MyMeshType,MyMatrix>::getValueRegardingOption(double val) const
 238   {
 239     if(_orientation==0)
 240       return val;
 241     if(_orientation==2)
 242       return fabs(val);
 243     if (( val > 0.0 && _orientation==1) || ( val < 0.0 && _orientation==-1 ))
 244       return _orientation*val;
 245     return 0.;
 246   }
 247
 248   template<class MyMeshType, class MyMatrix>
 249   int PlanarIntersector<MyMeshType,MyMatrix>::projectionThis(double *Coords_A, double *Coords_B, ConnType nb_NodesA, ConnType nb_NodesB)
 250   {
 251     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);
 252   }
 253
 254   template<class MyMeshType, class MyMatrix>
 255   int PlanarIntersector<MyMeshType,MyMatrix>::Projection(double *Coords_A, double *Coords_B,
 256                                                          ConnType nb_NodesA, ConnType nb_NodesB, double epsilon, double md3DSurf, double minDot3DSurf, double median_plane, bool do_rotate)
 257   {
 258     double normal_A[3]={0,0,0};
 259     double normal_B[3]={0,0,0};
 260     double linear_comb[3];
 261     double proj;
 262     bool same_orientation;
 263
 264     //Find the normal to cells A and B
 265     ConnType i_A1(1);
 266     while(i_A1<nb_NodesA && distance2<SPACEDIM>(Coords_A,&Coords_A[SPACEDIM*i_A1])< epsilon)
 267       i_A1++;
 268     ConnType i_A2(i_A1+1);
 269     crossprod<SPACEDIM>(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2],normal_A);
 270     double normA(sqrt(dotprod<SPACEDIM>(normal_A,normal_A)));
 271     while(i_A2<nb_NodesA && normA < epsilon)
 272       {
 273         crossprod<SPACEDIM>(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2],normal_A);
 274         i_A2++;
 275         normA = sqrt(dotprod<SPACEDIM>(normal_A,normal_A));
 276
 277       }
 278     ConnType i_B1(1);
 279     while(i_B1<nb_NodesB && distance2<SPACEDIM>(Coords_B,Coords_B+SPACEDIM*i_B1)< epsilon)
 280       i_B1++;
 281     ConnType i_B2(i_B1+1);
 282     crossprod<SPACEDIM>(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2,normal_B);
 283     double normB(sqrt(dotprod<SPACEDIM>(normal_B,normal_B)));
 284     while(i_B2<nb_NodesB && normB < epsilon)
 285       {
 286         crossprod<SPACEDIM>(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2,normal_B);
 287         i_B2++;
 288         normB = sqrt(dotprod<SPACEDIM>(normal_B,normal_B));
 289       }
 290
 291     //fabien option
 292     if(md3DSurf>0.)
 293       {
 294         double coords_GA[3];
 295         for(int i=0;i<3;i++)
 296           {
 297             coords_GA[i]=0.;
 298             for (int j=0;j<nb_NodesA;j++)
 299               coords_GA[i]+=Coords_A[3*j+i];
 300             coords_GA[i]/=(double)nb_NodesA;
 301           }
 302         double G1[3],G2[3],G3[3];
 303         for(int i=0;i<3;i++)
 304           {
 305             G1[i]=Coords_B[i]-coords_GA[i];
 306             G2[i]=Coords_B[i+3]-coords_GA[i];
 307             G3[i]=Coords_B[i+6]-coords_GA[i];
 308           }
 309         double prodvect[3];
 310         prodvect[0]=G1[1]*G2[2]-G1[2]*G2[1];
 311         prodvect[1]=G1[2]*G2[0]-G1[0]*G2[2];
 312         prodvect[2]=G1[0]*G2[1]-G1[1]*G2[0];
 313         double prodscal=prodvect[0]*G3[0]+prodvect[1]*G3[1]+prodvect[2]*G3[2];
 314         if(fabs(prodscal)>md3DSurf)
 315           return 0;
 316       }
 317     if(i_A2<nb_NodesA && i_B2<nb_NodesB)
 318       {
 319         //Build the normal of the median plane
 320
 321         double dotProd(dotprod<SPACEDIM>(normal_A,normal_B)/(normA*normB));
 322
 323         if(fabs(dotProd)<minDot3DSurf)
 324           return 0;
 325
 326         same_orientation=(dotProd>=0);
 327
 328         if(!same_orientation)
 329           for(int idim =0; idim< SPACEDIM; idim++)
 330             normal_A[idim] *=-1;
 331
 332         double normBB(sqrt(dotprod<SPACEDIM>(normal_B,normal_B)));
 333
 334         for(int idim =0; idim< SPACEDIM; idim++)
 335           linear_comb[idim] = median_plane*normal_A[idim]/normA + (1-median_plane)*normal_B[idim]/normBB;
 336         double norm= sqrt(dotprod<SPACEDIM>(linear_comb,linear_comb));
 337
 338         //Necessarily: norm>epsilon, no need to check
 339         for(int idim =0; idim< SPACEDIM; idim++)
 340           linear_comb[idim]/=norm;
 341
 342         //Project the nodes of A and B on the median plane
 343         for(ConnType i_A=0; i_A<nb_NodesA; i_A++)
 344           {
 345             proj = dotprod<SPACEDIM>(&Coords_A[SPACEDIM*i_A],linear_comb);
 346             for(int idim =0; idim< SPACEDIM; idim++)
 347               Coords_A[SPACEDIM*i_A+idim] -=  proj*linear_comb[idim];
 348           }
 349         for(ConnType i_B=0; i_B<nb_NodesB; i_B++)
 350           {
 351             proj = dotprod<SPACEDIM>(Coords_B+SPACEDIM*i_B,linear_comb);
 352             for(int idim =0; idim< SPACEDIM; idim++)
 353               Coords_B[SPACEDIM*i_B+idim] -=  proj*linear_comb[idim];
 354           }
 355
 356         //Build the matrix sending  A into the Oxy plane and apply it to A and B
 357         if(do_rotate)
 358           {
 359             TranslationRotationMatrix rotation;
 360             //rotate3DTriangle(Coords_A, &Coords_A[SPACEDIM*i_A1], &Coords_A[SPACEDIM*i_A2], rotation);
 361             Rotate3DTriangle(Coords_B, Coords_B+SPACEDIM*i_B1, Coords_B+SPACEDIM*i_B2, rotation);
 362             for (int i=0; i<nb_NodesA; i++)
 363               rotation.transform_vector(Coords_A+SPACEDIM*i);
 364             for (int i=0; i<nb_NodesB; i++)
 365               rotation.transform_vector(Coords_B+SPACEDIM*i);
 366           }
 367         return same_orientation?1:-1;
 368       }
 369     else
 370       {
 371         std::cout << " Degenerated cell " << "epsilon = " << epsilon << std::endl;
 372         std::cout << " i_A1= " << i_A1 << " i_A2= " << i_A2 << std::endl;
 373         std::cout << " distance2<SPACEDIM>(Coords_A,&Coords_A[i_A1])= " <<  distance2<SPACEDIM>(Coords_A,&Coords_A[i_A1]) << std::endl;
 374         std::cout << "abs(normal_A) = " << fabs(normal_A[0]) << " ; " <<fabs( normal_A[1]) << " ; " << fabs(normal_A[2]) << std::endl;
 375         std::cout << " i_B1= " << i_B1 << " i_B2= " << i_B2 << std::endl;
 376         std::cout << " distance2<SPACEDIM>(&Coords_B[0],&Coords_B[i_B1])= " <<  distance2<SPACEDIM>(Coords_B,Coords_B+i_B1) << std::endl;
 377         std::cout << "normal_B = " << normal_B[0] << " ; " << normal_B[1] << " ; " << normal_B[2] << std::endl;
 378         return 1;
 379       }
 380   }
 381
 382   template<class MyMeshType, class MyMatrix>
 383   void PlanarIntersector<MyMeshType,MyMatrix>::Rotate3DTriangle(double* PP1, double*PP2, double*PP3,
 384                                                                 TranslationRotationMatrix& rotation_matrix)
 385   {
 386     //initializes
 387     rotation_matrix.translate(PP1);
 388
 389     double P2w[3];
 390     double P3w[3];
 391     P2w[0]=PP2[0]; P2w[1]=PP2[1];P2w[2]=PP2[2];
 392     P3w[0]=PP3[0]; P3w[1]=PP3[1];P3w[2]=PP3[2];
 393
 394     // translating to set P1 at the origin
 395     for (int i=0; i<3; i++)
 396       {
 397         P2w[i]-=PP1[i];
 398         P3w[i]-=PP1[i];
 399       }
 400
 401     // rotating to set P2 on the Oxy plane
 402     TranslationRotationMatrix A;
 403     A.rotate_x(P2w);
 404     A.rotate_vector(P3w);
 405     rotation_matrix.multiply(A);
 406
 407     //rotating to set P2 on the Ox axis
 408     TranslationRotationMatrix B;
 409     B.rotate_z(P2w);
 410     B.rotate_vector(P3w);
 411     rotation_matrix.multiply(B);
 412
 413     //rotating to set P3 on the Oxy plane
 414     TranslationRotationMatrix C;
 415     C.rotate_x(P3w);
 416     rotation_matrix.multiply(C);
 417   }
 418 }
 419
 420 #endif