+
+namespace StdMeshers_ProjectionUtils
+{
+
+ //================================================================================
+ /*!
+ * \brief Computes transformation between two sets of 2D points using
+ * a least square approximation
+ *
+ * See "Surface Mesh Projection For Hexahedral Mesh Generation By Sweeping"
+ * by X.Roca, J.Sarrate, A.Huerta. (2.2)
+ */
+ //================================================================================
+
+ bool TrsfFinder2D::Solve( const vector< gp_XY >& srcPnts,
+ const vector< gp_XY >& tgtPnts )
+ {
+ // find gravity centers
+ gp_XY srcGC( 0,0 ), tgtGC( 0,0 );
+ for ( size_t i = 0; i < srcPnts.size(); ++i )
+ {
+ srcGC += srcPnts[i];
+ tgtGC += tgtPnts[i];
+ }
+ srcGC /= srcPnts.size();
+ tgtGC /= tgtPnts.size();
+
+ // find trsf
+
+ math_Matrix mat (1,4,1,4, 0.);
+ math_Vector vec (1,4, 0.);
+
+ // cout << "m1 = smesh.Mesh('src')" << endl
+ // << "m2 = smesh.Mesh('tgt')" << endl;
+ double xx = 0, xy = 0, yy = 0;
+ for ( size_t i = 0; i < srcPnts.size(); ++i )
+ {
+ gp_XY srcUV = srcPnts[i] - srcGC;
+ gp_XY tgtUV = tgtPnts[i] - tgtGC;
+ xx += srcUV.X() * srcUV.X();
+ yy += srcUV.Y() * srcUV.Y();
+ xy += srcUV.X() * srcUV.Y();
+ vec( 1 ) += srcUV.X() * tgtUV.X();
+ vec( 2 ) += srcUV.Y() * tgtUV.X();
+ vec( 3 ) += srcUV.X() * tgtUV.Y();
+ vec( 4 ) += srcUV.Y() * tgtUV.Y();
+ // cout << "m1.AddNode( " << srcUV.X() << ", " << srcUV.Y() << ", 0 )" << endl
+ // << "m2.AddNode( " << tgtUV.X() << ", " << tgtUV.Y() << ", 0 )" << endl;
+ }
+ mat( 1,1 ) = mat( 3,3 ) = xx;
+ mat( 2,2 ) = mat( 4,4 ) = yy;
+ mat( 1,2 ) = mat( 2,1 ) = mat( 3,4 ) = mat( 4,3 ) = xy;
+
+ math_Gauss solver( mat );
+ if ( !solver.IsDone() )
+ return false;
+ solver.Solve( vec );
+ if ( vec.Norm2() < gp::Resolution() )
+ return false;
+ // cout << vec( 1 ) << "\t " << vec( 2 ) << endl
+ // << vec( 3 ) << "\t " << vec( 4 ) << endl;
+
+ _trsf.SetTranslationPart( tgtGC );
+ _srcOrig = srcGC;
+
+ gp_Mat2d& M = const_cast< gp_Mat2d& >( _trsf.VectorialPart());
+ M( 1,1 ) = vec( 1 );
+ M( 2,1 ) = vec( 2 ); // | 1 3 | -- is it correct ????????
+ M( 1,2 ) = vec( 3 ); // | 2 4 |
+ M( 2,2 ) = vec( 4 );
+
+ return true;
+ }
+
+ //================================================================================
+ /*!
+ * \brief Transforms a 2D points using a found transformation
+ */
+ //================================================================================
+
+ gp_XY TrsfFinder2D::Transform( const gp_Pnt2d& srcUV ) const
+ {
+ gp_XY uv = srcUV.XY() - _srcOrig ;
+ _trsf.Transforms( uv );
+ return uv;
+ }
+
+ //================================================================================
+ /*!
+ * \brief Computes transformation between two sets of 3D points using
+ * a least square approximation
+ *
+ * See "Surface Mesh Projection For Hexahedral Mesh Generation By Sweeping"
+ * by X.Roca, J.Sarrate, A.Huerta. (2.4)
+ */
+ //================================================================================
+
+ bool TrsfFinder3D::Solve( const vector< gp_XYZ > & srcPnts,
+ const vector< gp_XYZ > & tgtPnts )
+ {
+ // find gravity center
+ gp_XYZ srcGC( 0,0,0 ), tgtGC( 0,0,0 );
+ for ( size_t i = 0; i < srcPnts.size(); ++i )
+ {
+ srcGC += srcPnts[i];
+ tgtGC += tgtPnts[i];
+ }
+ srcGC /= srcPnts.size();
+ tgtGC /= tgtPnts.size();
+
+ gp_XYZ srcOrig = 2 * srcGC - tgtGC;
+ gp_XYZ tgtOrig = srcGC;
+
+ // find trsf
+
+ math_Matrix mat (1,9,1,9, 0.);
+ math_Vector vec (1,9, 0.);
+
+ double xx = 0, yy = 0, zz = 0;
+ double xy = 0, xz = 0, yz = 0;
+ for ( size_t i = 0; i < srcPnts.size(); ++i )
+ {
+ gp_XYZ src = srcPnts[i] - srcOrig;
+ gp_XYZ tgt = tgtPnts[i] - tgtOrig;
+ xx += src.X() * src.X();
+ yy += src.Y() * src.Y();
+ zz += src.Z() * src.Z();
+ xy += src.X() * src.Y();
+ xz += src.X() * src.Z();
+ yz += src.Y() * src.Z();
+ vec( 1 ) += src.X() * tgt.X();
+ vec( 2 ) += src.Y() * tgt.X();
+ vec( 3 ) += src.Z() * tgt.X();
+ vec( 4 ) += src.X() * tgt.Y();
+ vec( 5 ) += src.Y() * tgt.Y();
+ vec( 6 ) += src.Z() * tgt.Y();
+ vec( 7 ) += src.X() * tgt.Z();
+ vec( 8 ) += src.Y() * tgt.Z();
+ vec( 9 ) += src.Z() * tgt.Z();
+ }
+ mat( 1,1 ) = mat( 4,4 ) = mat( 7,7 ) = xx;
+ mat( 2,2 ) = mat( 5,5 ) = mat( 8,8 ) = yy;
+ mat( 3,3 ) = mat( 6,6 ) = mat( 9,9 ) = zz;
+ mat( 1,2 ) = mat( 2,1 ) = mat( 4,5 ) = mat( 5,4 ) = mat( 7,8 ) = mat( 8,7 ) = xy;
+ mat( 1,3 ) = mat( 3,1 ) = mat( 4,6 ) = mat( 6,4 ) = mat( 7,9 ) = mat( 9,7 ) = xz;
+ mat( 2,3 ) = mat( 3,2 ) = mat( 5,6 ) = mat( 6,5 ) = mat( 8,9 ) = mat( 9,8 ) = yz;
+
+ math_Gauss solver( mat );
+ if ( !solver.IsDone() )
+ return false;
+ solver.Solve( vec );
+ if ( vec.Norm2() < gp::Resolution() )
+ return false;
+ // cout << endl
+ // << vec( 1 ) << "\t " << vec( 2 ) << "\t " << vec( 3 ) << endl
+ // << vec( 4 ) << "\t " << vec( 5 ) << "\t " << vec( 6 ) << endl
+ // << vec( 7 ) << "\t " << vec( 8 ) << "\t " << vec( 9 ) << endl;
+
+ _srcOrig = srcOrig;
+ _trsf.SetTranslationPart( tgtOrig );
+
+ gp_Mat& M = const_cast< gp_Mat& >( _trsf.VectorialPart() );
+ M.SetRows( gp_XYZ( vec( 1 ), vec( 2 ), vec( 3 )),
+ gp_XYZ( vec( 4 ), vec( 5 ), vec( 6 )),
+ gp_XYZ( vec( 7 ), vec( 8 ), vec( 9 )));
+ return true;
+ }
+
+ //================================================================================
+ /*!
+ * \brief Transforms a 3D point using a found transformation
+ */
+ //================================================================================
+
+ gp_XYZ TrsfFinder3D::Transform( const gp_Pnt& srcP ) const
+ {
+ gp_XYZ p = srcP.XYZ() - _srcOrig;
+ _trsf.Transforms( p );
+ return p;
+ }
+
+ //================================================================================
+ /*!
+ * \brief Transforms a 3D vector using a found transformation
+ */
+ //================================================================================
+
+ gp_XYZ TrsfFinder3D::TransformVec( const gp_Vec& v ) const
+ {
+ return v.XYZ().Multiplied( _trsf.VectorialPart() );
+ }
+ //================================================================================
+ /*!
+ * \brief Inversion
+ */
+ //================================================================================
+
+ bool TrsfFinder3D::Invert()
+ {
+ if (( _trsf.Form() == gp_Translation ) &&
+ ( _srcOrig.X() != 0 || _srcOrig.Y() != 0 || _srcOrig.Z() != 0 ))
+ {
+ // seems to be defined via Solve()
+ gp_XYZ newSrcOrig = _trsf.TranslationPart();
+ gp_Mat& M = const_cast< gp_Mat& >( _trsf.VectorialPart() );
+ const double D = M.Determinant();
+ if ( D < 1e-3 * ( newSrcOrig - _srcOrig ).Modulus() )
+ {
+#ifdef _DEBUG_
+ cerr << "TrsfFinder3D::Invert()"
+ << "D " << M.Determinant() << " IsSingular " << M.IsSingular() << endl;
+#endif
+ return false;
+ }
+ gp_Mat Minv = M.Inverted();
+ _trsf.SetTranslationPart( _srcOrig );
+ _srcOrig = newSrcOrig;
+ M = Minv;
+ }
+ else
+ {
+ _trsf.Invert();
+ }
+ return true;
+ }
+
+ //================================================================================
+ /*!
+ * \brief Add in-FACE nodes surrounding a given node to a queue
+ */
+ //================================================================================
+
+ void Morph::AddCloseNodes( const SMDS_MeshNode* srcNode,
+ const BRepMesh_Triangle* bmTria,
+ const int srcFaceID,
+ TNodeTriaList & noTriQueue )
+ {
+ // find in-FACE nodes
+ SMDS_ElemIteratorPtr elems = srcNode->GetInverseElementIterator(SMDSAbs_Face);
+ while ( elems->more() )
+ {
+ const SMDS_MeshElement* elem = elems->next();
+ if ( elem->getshapeId() == srcFaceID )
+ {
+ for ( int i = 0, nb = elem->NbNodes(); i < nb; ++i )
+ {
+ const SMDS_MeshNode* n = elem->GetNode( i );
+ if ( !n->isMarked() /*&& n->getshapeId() == srcFaceID*/ )
+ noTriQueue.push_back( make_pair( n, bmTria ));
+ }
+ }
+ }
+ }
+
+ //================================================================================
+ /*!
+ * \brief Find a delauney triangle containing a given 2D point and return
+ * barycentric coordinates within the found triangle
+ */
+ //================================================================================
+
+ const BRepMesh_Triangle* Morph::FindTriangle( const gp_XY& uv,
+ const BRepMesh_Triangle* bmTria,
+ double bc[3],
+ int triaNodes[3] )
+ {
+ int nodeIDs[3];
+ gp_XY nodeUVs[3];
+ int linkIDs[3];
+ Standard_Boolean ori[3];
+
+ while ( bmTria )
+ {
+ // check bmTria
+
+ _triaDS->ElementNodes( *bmTria, nodeIDs );
+ nodeUVs[0] = _triaDS->GetNode( nodeIDs[0] ).Coord();
+ nodeUVs[1] = _triaDS->GetNode( nodeIDs[1] ).Coord();
+ nodeUVs[2] = _triaDS->GetNode( nodeIDs[2] ).Coord();
+
+ SMESH_MeshAlgos::GetBarycentricCoords( uv,
+ nodeUVs[0], nodeUVs[1], nodeUVs[2],
+ bc[0], bc[1] );
+ if ( bc[0] >= 0 && bc[1] >= 0 && bc[0] + bc[1] <= 1 )
+ {
+ bc[2] = 1 - bc[0] - bc[1];
+ triaNodes[0] = nodeIDs[0];
+ triaNodes[1] = nodeIDs[1];
+ triaNodes[2] = nodeIDs[2];
+ return bmTria;
+ }
+
+ // look for a neighbor triangle, which is adjacent to a link intersected
+ // by a segment( triangle center -> uv )
+
+ gp_XY gc = ( nodeUVs[0] + nodeUVs[1] + nodeUVs[2] ) / 3.;
+ gp_XY seg = uv - gc;
+
+ bmTria->Edges( linkIDs, ori );
+ int triaID = _triaDS->IndexOf( *bmTria );
+ bmTria = 0;
+
+ for ( int i = 0; i < 3; ++i )
+ {
+ const BRepMesh_PairOfIndex & triIDs = _triaDS->ElementsConnectedTo( linkIDs[i] );
+ if ( triIDs.Extent() < 2 )
+ continue; // no neighbor triangle
+
+ // check if a link intersects gc2uv
+ const BRepMesh_Edge & link = _triaDS->GetLink( linkIDs[i] );
+ const BRepMesh_Vertex & n1 = _triaDS->GetNode( link.FirstNode() );
+ const BRepMesh_Vertex & n2 = _triaDS->GetNode( link.LastNode() );
+ gp_XY uv1 = n1.Coord();
+ gp_XY lin = n2.Coord() - uv1; // link direction
+
+ double crossSegLin = seg ^ lin;
+ if ( Abs( crossSegLin ) < std::numeric_limits<double>::min() )
+ continue; // parallel
+
+ double uSeg = ( uv1 - gc ) ^ lin / crossSegLin;
+ if ( 0. <= uSeg && uSeg <= 1. )
+ {
+ bmTria = & _triaDS->GetElement( triIDs.Index( 1 + ( triIDs.Index(1) == triaID )));
+ break;
+ }
+ }
+ }
+ return bmTria;
+ }
+
+ //================================================================================
+ /*!
+ * \brief Return a triangle sharing a given boundary node
+ * \param [in] iBndNode - index of the boundary node
+ * \return const BRepMesh_Triangle* - a found triangle
+ */
+ //================================================================================
+
+ const BRepMesh_Triangle* Morph::GetTriangleNear( int iBndNode )
+ {
+ const BRepMesh::ListOfInteger & linkIds = _triaDS->LinksConnectedTo( iBndNode );
+ const BRepMesh_PairOfIndex & triaIds = _triaDS->ElementsConnectedTo( linkIds.First() );
+ const BRepMesh_Triangle& tria = _triaDS->GetElement( triaIds.Index(1) );
+ return &tria;
+ }
+
+ //================================================================================
+ /*!
+ * \brief triangulate the srcFace in 2D
+ * \param [in] srcWires - boundary of the src FACE
+ */
+ //================================================================================
+
+ Morph::Morph(const TSideVector& srcWires)
+ {
+ _srcSubMesh = srcWires[0]->GetMesh()->GetSubMesh( srcWires[0]->Face() );
+
+ // compute _scale
+ {
+ BRepAdaptor_Surface surf( srcWires[0]->Face() );
+ const int nbDiv = 100;
+ const double uRange = surf.LastUParameter() - surf.FirstUParameter();
+ const double vRange = surf.LastVParameter() - surf.FirstVParameter();
+ const double dU = uRange / nbDiv;
+ const double dV = vRange / nbDiv;
+ double u = surf.FirstUParameter(), v = surf.FirstVParameter();
+ gp_Pnt p0U = surf.Value( u, v ), p0V = p0U;
+ double lenU = 0, lenV = 0;
+ for ( ; u < surf.LastUParameter(); u += dU, v += dV )
+ {
+ gp_Pnt p1U = surf.Value( u, surf.FirstVParameter() );
+ lenU += p1U.Distance( p0U );
+ p0U = p1U;
+ gp_Pnt p1V = surf.Value( surf.FirstUParameter(), v );
+ lenV += p1V.Distance( p0V );
+ p0V = p1V;
+ }
+ _scale.SetCoord( lenU / uRange, lenV / vRange );
+ }
+
+ // count boundary points
+ int iP = 1, nbP = 0;
+ for ( size_t iW = 0; iW < srcWires.size(); ++iW )
+ nbP += srcWires[iW]->NbPoints() - 1; // 1st and last points coincide
+
+ _bndSrcNodes.resize( nbP + 1 ); _bndSrcNodes[0] = 0;
+
+ // fill boundary points
+ BRepMesh::Array1OfVertexOfDelaun srcVert( 1, 1 + nbP );
+ BRepMesh_Vertex v( 0, 0, BRepMesh_Frontier );
+ for ( size_t iW = 0; iW < srcWires.size(); ++iW )
+ {
+ const UVPtStructVec& srcPnt = srcWires[iW]->GetUVPtStruct();
+ for ( int i = 0, nb = srcPnt.size() - 1; i < nb; ++i, ++iP )
+ {
+ _bndSrcNodes[ iP ] = srcPnt[i].node;
+ srcPnt[i].node->setIsMarked( true );
+
+ v.ChangeCoord() = srcPnt[i].UV().Multiplied( _scale );
+ srcVert( iP ) = v;
+ }
+ }
+ // triangulate the srcFace in 2D
+ BRepMesh_Delaun delauney( srcVert );
+ _triaDS = delauney.Result();
+ }
+
+ //================================================================================
+ /*!
+ * \brief Move non-marked target nodes
+ * \param [in,out] tgtHelper - helper
+ * \param [in] tgtWires - boundary nodes of the target FACE; must be in the
+ * same order as the nodes in srcWires given in the constructor
+ * \param [in] src2tgtNodes - map of src -> tgt nodes
+ * \param [in] moveAll - to move all nodes; if \c false, move only non-marked nodes
+ * \return bool - Ok or not
+ */
+ //================================================================================
+
+ bool Morph::Perform(SMESH_MesherHelper& tgtHelper,
+ const TSideVector& tgtWires,
+ Handle(ShapeAnalysis_Surface) tgtSurface,
+ const TNodeNodeMap& src2tgtNodes,
+ const bool moveAll)
+ {
+ // get tgt boundary points corresponding to _bndSrcNodes
+ size_t nbP = 0;
+ for ( size_t iW = 0; iW < tgtWires.size(); ++iW )
+ nbP += tgtWires[iW]->NbPoints() - 1; // 1st and last points coincide
+ if ( nbP != _bndSrcNodes.size() - 1 )
+ return false;
+
+ BRepMesh::Array1OfVertexOfDelaun tgtVert( 1, 1 + nbP );
+ BRepMesh_Vertex v( 0, 0, BRepMesh_Frontier );
+ for ( size_t iW = 0, iP = 1; iW < tgtWires.size(); ++iW )
+ {
+ const UVPtStructVec& tgtPnt = tgtWires[iW]->GetUVPtStruct();
+ for ( int i = 0, nb = tgtPnt.size() - 1; i < nb; ++i, ++iP )
+ {
+ v.ChangeCoord() = tgtPnt[i].UV().Multiplied( _scale );
+ tgtVert( iP ) = v;
+ }
+ }
+
+ const TopoDS_Face& srcFace = TopoDS::Face( _srcSubMesh->GetSubShape() );
+ const int srcFaceID = _srcSubMesh->GetId();
+ SMESHDS_Mesh* tgtMesh = tgtHelper.GetMeshDS();
+ const SMDS_MeshNode *srcNode, *tgtNode;
+ const BRepMesh_Triangle *bmTria;
+
+ // un-mark internal src nodes; later we will mark moved nodes
+ int nbSrcNodes = 0;
+ SMDS_NodeIteratorPtr nIt = _srcSubMesh->GetSubMeshDS()->GetNodes();
+ if ( !nIt || !nIt->more() ) return true;
+ if ( moveAll )
+ {
+ nbSrcNodes = _srcSubMesh->GetSubMeshDS()->NbNodes();
+ while ( nIt->more() )
+ nIt->next()->setIsMarked( false );
+ }
+ else
+ {
+ while ( nIt->more() )
+ nbSrcNodes += int( !nIt->next()->isMarked() );
+ }
+
+ // Move tgt nodes
+
+ double bc[3]; // barycentric coordinates
+ int nodeIDs[3];
+ bool checkUV = true;
+ const SMDS_FacePosition* pos;
+
+ // a queue of nodes with starting triangles
+ TNodeTriaList noTriQueue;
+ size_t iBndSrcN = 1;
+
+ while ( nbSrcNodes > 0 )
+ {
+ while ( !noTriQueue.empty() )
+ {
+ srcNode = noTriQueue.front().first;
+ bmTria = noTriQueue.front().second;
+ noTriQueue.pop_front();
+ if ( srcNode->isMarked() )
+ continue;
+ --nbSrcNodes;
+ srcNode->setIsMarked( true );
+
+ // find a delauney triangle containing the src node
+ gp_XY uv = tgtHelper.GetNodeUV( srcFace, srcNode, NULL, &checkUV );
+ uv *= _scale;
+ bmTria = FindTriangle( uv, bmTria, bc, nodeIDs );
+ if ( !bmTria )
+ continue;
+
+ // compute new coordinates for a corresponding tgt node
+ gp_XY uvNew( 0., 0. ), nodeUV;
+ for ( int i = 0; i < 3; ++i )
+ uvNew += bc[i] * tgtVert( nodeIDs[i]).Coord();
+ uvNew.SetCoord( uvNew.X() / _scale.X(),
+ uvNew.Y() / _scale.Y() );
+ gp_Pnt xyz = tgtSurface->Value( uvNew );
+
+ // find and move tgt node
+ TNodeNodeMap::const_iterator n2n = src2tgtNodes.find( srcNode );
+ if ( n2n == src2tgtNodes.end() ) continue;
+ tgtNode = n2n->second;
+ tgtMesh->MoveNode( tgtNode, xyz.X(), xyz.Y(), xyz.Z() );
+
+ if (( pos = dynamic_cast< const SMDS_FacePosition* >( tgtNode->GetPosition() )))
+ const_cast<SMDS_FacePosition*>( pos )->SetParameters( uvNew.X(), uvNew.Y() );
+
+ AddCloseNodes( srcNode, bmTria, srcFaceID, noTriQueue );
+ }
+
+ if ( nbSrcNodes > 0 )
+ {
+ // assure that all src nodes are visited
+ for ( ; iBndSrcN < _bndSrcNodes.size() && noTriQueue.empty(); ++iBndSrcN )
+ {
+ const BRepMesh_Triangle* tria = GetTriangleNear( iBndSrcN );
+ AddCloseNodes( _bndSrcNodes[ iBndSrcN ], tria, srcFaceID, noTriQueue );
+ }
+ if ( noTriQueue.empty() )
+ {
+ SMDS_NodeIteratorPtr nIt = _srcSubMesh->GetSubMeshDS()->GetNodes();
+ while ( nIt->more() )
+ {
+ srcNode = nIt->next();
+ if ( !srcNode->isMarked() )
+ noTriQueue.push_back( make_pair( srcNode, bmTria ));
+ }
+ }
+ }
+ }
+
+ return true;
+
+ } // Morph::Perform
+
+ gp_XY Morph::GetBndUV(const int iNode) const
+ {
+ return _triaDS->GetNode( iNode ).Coord();
+ }
+
+
+} // namespace StdMeshers_ProjectionUtils