X-Git-Url: http://git.salome-platform.org/gitweb/?p=modules%2Fsmesh.git;a=blobdiff_plain;f=src%2FStdMeshers%2FStdMeshers_ProjectionUtils.cxx;h=58b69838b8d43161d18c8e1e138025c41b893b6d;hp=ae6f38419e465580eafdaad0461a7a1eaa107224;hb=eb22cf46cb0f386218d79291dfebcec8fc831055;hpb=b1f58b701eb4ec6fb4a14b070041f2c89e44723c diff --git a/src/StdMeshers/StdMeshers_ProjectionUtils.cxx b/src/StdMeshers/StdMeshers_ProjectionUtils.cxx index ae6f38419..58b69838b 100644 --- a/src/StdMeshers/StdMeshers_ProjectionUtils.cxx +++ b/src/StdMeshers/StdMeshers_ProjectionUtils.cxx @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -94,11 +95,11 @@ using namespace std; // cout << endl;\ // } -#define HERE StdMeshers_ProjectionUtils +namespace HERE = StdMeshers_ProjectionUtils; namespace { - static SMESHDS_Mesh* theMeshDS[2] = { 0, 0 }; // used to debug only + static SMESHDS_Mesh* theMeshDS[2] = { 0, 0 }; // used for debug only long shapeIndex(const TopoDS_Shape& S) { if ( theMeshDS[0] && theMeshDS[1] ) @@ -112,7 +113,7 @@ namespace { */ //================================================================================ - bool _StoreBadShape(const TopoDS_Shape& shape) + bool storeShapeForDebug(const TopoDS_Shape& shape) { #ifdef _DEBUG_ const char* type[] ={"COMPOUND","COMPSOLID","SOLID","SHELL","FACE","WIRE","EDGE","VERTEX"}; @@ -130,7 +131,7 @@ namespace { */ //================================================================================ - void Reverse( list< TopoDS_Edge > & edges, const int nbEdges, const int firstEdge=0) + void reverseEdges( list< TopoDS_Edge > & edges, const int nbEdges, const int firstEdge=0) { SHOW_LIST("BEFORE REVERSE", edges); @@ -160,7 +161,7 @@ namespace { */ //================================================================================ - bool IsPropagationPossible( SMESH_Mesh* theMesh1, SMESH_Mesh* theMesh2 ) + bool isPropagationPossible( SMESH_Mesh* theMesh1, SMESH_Mesh* theMesh2 ) { if ( theMesh1 != theMesh2 ) { TopoDS_Shape mainShape1 = theMesh1->GetMeshDS()->ShapeToMesh(); @@ -182,24 +183,60 @@ namespace { */ //================================================================================ - bool FixAssocByPropagation( const int nbEdges, + bool fixAssocByPropagation( const int nbEdges, list< TopoDS_Edge > & edges1, list< TopoDS_Edge > & edges2, SMESH_Mesh* theMesh1, SMESH_Mesh* theMesh2) { - if ( nbEdges == 2 && IsPropagationPossible( theMesh1, theMesh2 ) ) + if ( nbEdges == 2 && isPropagationPossible( theMesh1, theMesh2 ) ) { list< TopoDS_Edge >::iterator eIt2 = ++edges2.begin(); // 2nd edge of the 2nd face TopoDS_Edge edge2 = HERE::GetPropagationEdge( theMesh1, *eIt2, edges1.front() ).second; if ( !edge2.IsNull() ) { // propagation found for the second edge - Reverse( edges2, nbEdges ); + reverseEdges( edges2, nbEdges ); return true; } } return false; } + //================================================================================ + /*! + * \brief Associate faces having one edge in the outer wire. + * No check is done if there is really only one outer edge + */ + //================================================================================ + + bool assocFewEdgesFaces( const TopoDS_Face& face1, + SMESH_Mesh* mesh1, + const TopoDS_Face& face2, + SMESH_Mesh* mesh2, + HERE::TShapeShapeMap & theMap) + { + TopoDS_Vertex v1 = TopoDS::Vertex( HERE::OuterShape( face1, TopAbs_VERTEX )); + TopoDS_Vertex v2 = TopoDS::Vertex( HERE::OuterShape( face2, TopAbs_VERTEX )); + TopoDS_Vertex VV1[2] = { v1, v1 }; + TopoDS_Vertex VV2[2] = { v2, v2 }; + list< TopoDS_Edge > edges1, edges2; + if ( int nbE = HERE::FindFaceAssociation( face1, VV1, face2, VV2, edges1, edges2 )) + { + HERE::InsertAssociation( face1, face2, theMap ); + fixAssocByPropagation( nbE, edges1, edges2, mesh1, mesh2 ); + list< TopoDS_Edge >::iterator eIt1 = edges1.begin(); + list< TopoDS_Edge >::iterator eIt2 = edges2.begin(); + for ( ; eIt1 != edges1.end(); ++eIt1, ++eIt2 ) + { + HERE::InsertAssociation( *eIt1, *eIt2, theMap ); + v1 = SMESH_MesherHelper::IthVertex( 0, *eIt1 ); + v2 = SMESH_MesherHelper::IthVertex( 0, *eIt2 ); + HERE::InsertAssociation( v1, v2, theMap ); + } + return true; + } + return false; + } + //================================================================================ /*! * \brief Look for a group containing a target shape and similar to a source group @@ -210,7 +247,7 @@ namespace { */ //================================================================================ - TopoDS_Shape FindGroupContaining(const TopoDS_Shape& tgtShape, + TopoDS_Shape findGroupContaining(const TopoDS_Shape& tgtShape, const SMESH_Mesh* tgtMesh1, const TopoDS_Shape& srcGroup) { @@ -236,7 +273,7 @@ namespace { */ //================================================================================ - bool AssocGroupsByPropagation(const TopoDS_Shape& theGroup1, + bool assocGroupsByPropagation(const TopoDS_Shape& theGroup1, const TopoDS_Shape& theGroup2, SMESH_Mesh& theMesh, HERE::TShapeShapeMap& theMap) @@ -288,7 +325,7 @@ namespace { list< TopoDS_Edge > edges; list< int > nbEdgesInWire; SMESH_Block::GetOrderedEdges( face, edges, nbEdgesInWire, v1); if ( nbEdgesInWire.front() != 4 ) - return _StoreBadShape( face ); + return storeShapeForDebug( face ); list< TopoDS_Edge >::iterator edge = edges.begin(); if ( verticEdge.IsSame( *edge )) { edgeGr2 = *(++edge); @@ -465,11 +502,11 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the TopoDS_Shape group1, group2; if ( theShape1.ShapeType() == TopAbs_COMPOUND ) { group1 = theShape1; - group2 = FindGroupContaining( theShape2, theMesh2, group1 ); + group2 = findGroupContaining( theShape2, theMesh2, group1 ); } else if ( theShape2.ShapeType() == TopAbs_COMPOUND ) { group2 = theShape2; - group1 = FindGroupContaining( theShape1, theMesh1, group2 ); + group1 = findGroupContaining( theShape1, theMesh1, group2 ); } if ( group1.IsNull() || group2.IsNull() ) RETURN_BAD_RESULT("Different shape types"); @@ -493,6 +530,8 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the TShapePairsList::iterator s1_s2 = shapesQueue.begin(); for ( ; s1_s2 != shapesQueue.end(); ++s1_s2 ) { + if ( theMap.IsBound( s1_s2->first )) // avoid re-binding for a seam edge + continue; // to avoid this: Forward seam -> Reversed seam InsertAssociation( s1_s2->first, s1_s2->second, theMap ); TopoDS_Iterator s1It( s1_s2->first), s2It( s1_s2->second ); for ( ; s1It.More(); s1It.Next(), s2It.Next() ) @@ -556,7 +595,7 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the list< TopoDS_Edge > edges1, edges2; int nbE = FindFaceAssociation( face1, VV1, face2, VV2, edges1, edges2 ); if ( !nbE ) RETURN_BAD_RESULT("FindFaceAssociation() failed"); - FixAssocByPropagation( nbE, edges1, edges2, theMesh1, theMesh2 ); + fixAssocByPropagation( nbE, edges1, edges2, theMesh1, theMesh2 ); list< TopoDS_Edge >::iterator eIt1 = edges1.begin(); list< TopoDS_Edge >::iterator eIt2 = edges2.begin(); @@ -682,7 +721,7 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the " to " << theMesh2->GetMeshDS()->ShapeToIndex( face2 )); if ( nbE == 2 && (edge1.IsSame( edges1.front())) != (edge2.IsSame( edges2.front()))) { - Reverse( edges2, nbE ); + reverseEdges( edges2, nbE ); } list< TopoDS_Edge >::iterator eIt1 = edges1.begin(); list< TopoDS_Edge >::iterator eIt2 = edges2.begin(); @@ -690,13 +729,9 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the { if ( !boundEdges.Add( *eIt1 )) continue; // already associated InsertAssociation( *eIt1, *eIt2, theMap ); // assoc edges - MESSAGE("Assoc edge " << theMesh1->GetMeshDS()->ShapeToIndex( *eIt1 )<< - " to " << theMesh2->GetMeshDS()->ShapeToIndex( *eIt2 )); VV1[0] = TopExp::FirstVertex( *eIt1, true ); VV2[0] = TopExp::FirstVertex( *eIt2, true ); InsertAssociation( VV1[0], VV2[0], theMap ); // assoc vertices - MESSAGE("Assoc vertex " << theMesh1->GetMeshDS()->ShapeToIndex( VV1[0] )<< - " to " << theMesh2->GetMeshDS()->ShapeToIndex( VV2[0] )); // add adjacent faces to process TopoDS_Face nextFace1 = GetNextFace( edgeToFace1, *eIt1, face1 ); @@ -959,7 +994,7 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the // ---------------------------------------------------------------------- TopoDS_Edge edge1 = TopoDS::Edge( theShape1 ); TopoDS_Edge edge2 = TopoDS::Edge( theShape2 ); - if ( IsPropagationPossible( theMesh1, theMesh2 )) + if ( isPropagationPossible( theMesh1, theMesh2 )) { TopoDS_Edge prpEdge = GetPropagationEdge( theMesh1, edge2, edge1 ).second; if ( !prpEdge.IsNull() ) @@ -993,7 +1028,7 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the case TopAbs_FACE: { // ---------------------------------------------------------------------- - if ( IsPropagationPossible( theMesh1, theMesh2 )) // try by propagation in one mesh + if ( isPropagationPossible( theMesh1, theMesh2 )) // try by propagation in one mesh { TopoDS_Face face1 = TopoDS::Face(theShape1); TopoDS_Face face2 = TopoDS::Face(theShape2); @@ -1035,7 +1070,7 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the bool same2 = edge2.IsSame( edges2.front() ); if ( same1 != same2 ) { - Reverse(edges2, nbE); + reverseEdges(edges2, nbE); if ( nbE != 2 ) // 2 degen edges of 4 (issue 0021144) edges2.splice( edges2.end(), edges2, edges2.begin()); } @@ -1057,10 +1092,10 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the } case TopAbs_COMPOUND: { // ---------------------------------------------------------------------- - if ( IsPropagationPossible( theMesh1, theMesh2 )) { + if ( isPropagationPossible( theMesh1, theMesh2 )) { // try to accosiate all using propagation - if ( AssocGroupsByPropagation( theShape1, theShape2, *theMesh1, theMap )) + if ( assocGroupsByPropagation( theShape1, theShape2, *theMesh1, theMap )) return true; // find a boundary edge of theShape1 @@ -1137,9 +1172,13 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the RETURN_BAD_RESULT("Different nb of vertices"); } - if ( vMap1.Extent() == 1 ) { + if ( vMap1.Extent() == 1 || vMap2.Extent() == 1 ) { InsertAssociation( vMap1(1), vMap2(1), theMap ); if ( theShape1.ShapeType() == TopAbs_EDGE ) { + if ( vMap1.Extent() == 2 ) + InsertAssociation( vMap1(2), vMap2(1), theMap ); + else if ( vMap2.Extent() == 2 ) + InsertAssociation( vMap2(2), vMap1(1), theMap ); InsertAssociation( theShape1, theShape2, theMap ); return true; } @@ -1196,14 +1235,19 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the // get 2 linked vertices of shape 1 not belonging to an inner wire of a face std::list< TopoDS_Edge > allBndEdges1; if ( !getOuterEdges( theShape1, *theMesh1, allBndEdges1 )) - RETURN_BAD_RESULT("Edge not found"); - + { + if ( theShape1.ShapeType() != TopAbs_FACE ) + RETURN_BAD_RESULT("Edge not found"); + return assocFewEdgesFaces( TopoDS::Face( theShape1 ), theMesh1, + TopoDS::Face( theShape2 ), theMesh2, theMap ); + } std::list< TopoDS_Edge >::iterator edge1 = allBndEdges1.begin(); double minDist = std::numeric_limits::max(); for ( int nbChecked=0; edge1 != allBndEdges1.end() && nbChecked++ < 10; ++edge1 ) { - TopExp::Vertices( TopoDS::Edge( edge1->Oriented(TopAbs_FORWARD)), VV1[0], VV1[1]); - if ( VV1[0].IsSame( VV1[1] )) + TopoDS_Vertex edge1VV[2]; + TopExp::Vertices( TopoDS::Edge( edge1->Oriented(TopAbs_FORWARD)), edge1VV[0], edge1VV[1]); + if ( edge1VV[0].IsSame( edge1VV[1] )) continue;//RETURN_BAD_RESULT("Only closed edges"); // find vertices closest to 2 linked vertices of shape 1 @@ -1211,7 +1255,7 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the TopoDS_Vertex edge2VV[2]; for ( int i1 = 0; i1 < 2; ++i1 ) { - gp_Pnt p1 = BRep_Tool::Pnt( VV1[ i1 ]); + gp_Pnt p1 = BRep_Tool::Pnt( edge1VV[ i1 ]); p1.Scale( gc[0], scale ); p1.Translate( vec01 ); if ( !i1 ) { @@ -1247,6 +1291,8 @@ bool StdMeshers_ProjectionUtils::FindSubShapeAssociation(const TopoDS_Shape& the } } if ( dist2[0] + dist2[1] < minDist ) { + VV1[0] = edge1VV[0]; + VV1[1] = edge1VV[1]; VV2[0] = edge2VV[0]; VV2[1] = edge2VV[1]; minDist = dist2[0] + dist2[1]; @@ -1355,7 +1401,7 @@ int StdMeshers_ProjectionUtils::FindFaceAssociation(const TopoDS_Face& face1, } if ( reverse ) { - Reverse( edges2 , nbEInW2.front()); + reverseEdges( edges2 , nbEInW2.front()); if (( VV1[1].IsSame( TopExp::LastVertex( edges1.front(), true ))) != ( VV2[1].IsSame( TopExp::LastVertex( edges2.front(), true )))) CONT_BAD_RESULT("GetOrderedEdges() failed"); @@ -1434,7 +1480,7 @@ int StdMeshers_ProjectionUtils::FindFaceAssociation(const TopoDS_Face& face1, if ( iW1 == 0 ) OK = true; // OK is for the first wire // reverse edges2 if needed if ( !sameVertexUV( *edge2Beg, face2, 1, v1f1UV, vTolUV )) - Reverse( edges2 , *nbE2, std::distance( edges2.begin(),edge2Beg )); + reverseEdges( edges2 , *nbE2, std::distance( edges2.begin(),edge2Beg )); // put wire2 at a right place within edges2 if ( iW1 != iW2 ) { list< TopoDS_Edge >::iterator place2 = edges2.begin(); @@ -1472,7 +1518,7 @@ int StdMeshers_ProjectionUtils::FindFaceAssociation(const TopoDS_Face& face1, l2[1] = SMESH_Algo::EdgeLength( *edgeIt++ ); if (( l1[0] < l1[1] ) != ( l2[0] < l2[1] )) { - Reverse( edges2, nbEdges ); + reverseEdges( edges2, nbEdges ); } } } @@ -1760,7 +1806,8 @@ FindMatchingNodesOnFaces( const TopoDS_Face& face1, eE.Next(); // edge 1 if ( !assocMap.IsBound( e2, /*is2nd=*/true )) - RETURN_BAD_RESULT("Association not found for edge " << meshDS2->ShapeToIndex( e2 )); + continue; + //RETURN_BAD_RESULT("Association not found for edge " << meshDS2->ShapeToIndex( e2 )); TopoDS_Edge e1 = TopoDS::Edge( assocMap( e2, /*is2nd=*/true )); if ( !helper1.IsSubShape( e1, face1 )) RETURN_BAD_RESULT("Wrong association, edge " << meshDS1->ShapeToIndex( e1 ) << @@ -1804,7 +1851,11 @@ FindMatchingNodesOnFaces( const TopoDS_Face& face1, // get 2 matching vertices TopoDS_Vertex V2 = TopExp::FirstVertex( TopoDS::Edge( edge2 )); if ( !assocMap.IsBound( V2, /*is2nd=*/true )) - RETURN_BAD_RESULT("Association not found for vertex " << meshDS2->ShapeToIndex( V2 )); + { + V2 = TopExp::LastVertex( TopoDS::Edge( edge2 )); + if ( !assocMap.IsBound( V2, /*is2nd=*/true )) + RETURN_BAD_RESULT("Association not found for vertex " << meshDS2->ShapeToIndex( V2 )); + } TopoDS_Vertex V1 = TopoDS::Vertex( assocMap( V2, /*is2nd=*/true )); // nodes on vertices @@ -2085,7 +2136,7 @@ bool StdMeshers_ProjectionUtils::MakeComputed(SMESH_subMesh * sm, const int iter mesh->GetHypotheses( shape, hypoFilter, hyps, true, &assignedTo ); if ( nbAlgos > 1 ) // concurrent algos { - list smList; // where an algo is assigned + vector smList; // where an algo is assigned list< TopoDS_Shape >::iterator shapeIt = assignedTo.begin(); for ( ; shapeIt != assignedTo.end(); ++shapeIt ) smList.push_back( mesh->GetSubMesh( *shapeIt )); @@ -2206,9 +2257,10 @@ std::string StdMeshers_ProjectionUtils::SourceNotComputedError( SMESH_subMesh * */ //================================================================================ -TopoDS_Edge StdMeshers_ProjectionUtils::GetBoundaryEdge(const TopoDS_Shape& edgeContainer, - const SMESH_Mesh& mesh, - std::list< TopoDS_Edge >* allBndEdges) +TopoDS_Edge +StdMeshers_ProjectionUtils::GetBoundaryEdge(const TopoDS_Shape& edgeContainer, + const SMESH_Mesh& mesh, + std::list< TopoDS_Edge >* allBndEdges) { TopTools_IndexedMapOfShape facesOfEdgeContainer, facesNearEdge; TopExp::MapShapes( edgeContainer, TopAbs_FACE, facesOfEdgeContainer ); @@ -2237,7 +2289,7 @@ TopoDS_Edge StdMeshers_ProjectionUtils::GetBoundaryEdge(const TopoDS_Shape& namespace { // Definition of event listeners - SMESH_subMeshEventListener* GetSrcSubMeshListener(); + SMESH_subMeshEventListener* getSrcSubMeshListener(); //================================================================================ /*! @@ -2257,7 +2309,7 @@ namespace { // Definition of event listeners eventType == SMESH_subMesh::ALGO_EVENT) { // delete current source listener - subMesh->DeleteEventListener( GetSrcSubMeshListener() ); + subMesh->DeleteEventListener( getSrcSubMeshListener() ); // let algo set a new one if ( SMESH_Algo* algo = subMesh->GetAlgo() ) algo->SetEventListener( subMesh ); @@ -2270,7 +2322,7 @@ namespace { // Definition of event listeners */ //================================================================================ - SMESH_subMeshEventListener* GetHypModifWaiter() { + SMESH_subMeshEventListener* getHypModifWaiter() { static HypModifWaiter aHypModifWaiter; return &aHypModifWaiter; } @@ -2280,7 +2332,7 @@ namespace { // Definition of event listeners */ //================================================================================ - SMESH_subMeshEventListener* GetSrcSubMeshListener() { + SMESH_subMeshEventListener* getSrcSubMeshListener() { static SMESH_subMeshEventListener srcListener(false, // won't be deleted by submesh "StdMeshers_ProjectionUtils::SrcSubMeshListener"); return &srcListener; @@ -2302,7 +2354,7 @@ void StdMeshers_ProjectionUtils::SetEventListener(SMESH_subMesh* subMesh, { // Set the listener that resets an event listener on source submesh when // "ProjectionSource*D" hypothesis is modified since source shape can be changed - subMesh->SetEventListener( GetHypModifWaiter(),0,subMesh); + subMesh->SetEventListener( getHypModifWaiter(),0,subMesh); // Set an event listener to submesh of the source shape if ( !srcShape.IsNull() ) @@ -2324,21 +2376,259 @@ void StdMeshers_ProjectionUtils::SetEventListener(SMESH_subMesh* subMesh, if ( srcSM != subMesh ) { SMESH_subMeshEventListenerData* data = - srcSM->GetEventListenerData(GetSrcSubMeshListener()); + srcSM->GetEventListenerData(getSrcSubMeshListener()); if ( data ) data->mySubMeshes.push_back( subMesh ); else data = SMESH_subMeshEventListenerData::MakeData( subMesh ); - subMesh->SetEventListener ( GetSrcSubMeshListener(), data, srcSM ); + subMesh->SetEventListener ( getSrcSubMeshListener(), data, srcSM ); } } } else { - subMesh->SetEventListener( GetSrcSubMeshListener(), - SMESH_subMeshEventListenerData::MakeData( subMesh ), - srcShapeSM ); + if ( SMESH_subMeshEventListenerData* data = + srcShapeSM->GetEventListenerData( getSrcSubMeshListener() )) + { + bool alreadyIn = + (std::find( data->mySubMeshes.begin(), + data->mySubMeshes.end(), subMesh ) != data->mySubMeshes.end() ); + if ( !alreadyIn ) + data->mySubMeshes.push_back( subMesh ); + } + else + { + subMesh->SetEventListener( getSrcSubMeshListener(), + SMESH_subMeshEventListenerData::MakeData( subMesh ), + srcShapeSM ); + } } } } } + +namespace StdMeshers_ProjectionUtils +{ + + //================================================================================ + /*! + * \brief Computes transformation beween 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.SetTranslation( tgtGC ); + _srcOrig = srcGC; + + gp_Mat2d& M = const_cast< gp_Mat2d& >( _trsf.HVectorialPart()); + M( 1,1 ) = vec( 1 ); + M( 2,1 ) = vec( 2 ); + M( 1,2 ) = vec( 3 ); + 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 beween 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.SetTranslation( tgtOrig ); + + gp_Mat& M = const_cast< gp_Mat& >( _trsf.HVectorialPart() ); + 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.HVectorialPart() ); + } + //================================================================================ + /*! + * \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.HVectorialPart() ); + 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.SetTranslation( _srcOrig ); + _srcOrig = newSrcOrig; + M = Minv; + } + else + { + _trsf.Invert(); + } + return true; + } +}