53638655887b3718ee2284068275deed03732628
[occt.git] / src / BRepLib / BRepLib_FindSurface.cxx
1 // Created on: 1994-07-22
2 // Created by: Remi LEQUETTE
3 // Copyright (c) 1994-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17 #include <BRepLib_FindSurface.ixx>
18
19 #include <Precision.hxx>
20 #include <math_Matrix.hxx>
21 #include <math_Vector.hxx>
22 #include <math_Gauss.hxx>
23
24 #include <gp_Lin.hxx>
25 #include <gp_Circ.hxx>
26 #include <gp_Elips.hxx>
27 #include <gp_Hypr.hxx>
28 #include <gp_Parab.hxx>
29 #include <gp_Ax2.hxx>
30 #include <gp_Ax3.hxx>
31 #include <gp_Vec.hxx>
32 #include <TColgp_SequenceOfPnt.hxx>
33 #include <TColStd_SequenceOfReal.hxx>
34 #include <TColgp_Array1OfPnt.hxx>
35 #include <TColgp_HArray1OfPnt.hxx>
36 #include <Geom_Plane.hxx>
37
38 #include <BRepAdaptor_Curve.hxx>
39 #include <BRepAdaptor_Surface.hxx>
40 #include <BRepLib_MakeFace.hxx>
41 #include <BRepTools_WireExplorer.hxx>
42 #include <BRep_Tool.hxx>
43 #include <TopExp.hxx>
44 #include <TopExp_Explorer.hxx>
45 #include <TopoDS_Vertex.hxx>
46 #include <TopoDS_Wire.hxx>
47 #include <TopoDS.hxx>
48
49 #include <GeomLib.hxx>
50 #include <Geom2d_Curve.hxx>
51 #include <Geom_BezierCurve.hxx>
52 #include <Geom_BSplineCurve.hxx>
53 #include <Geom_RectangularTrimmedSurface.hxx>
54 #include <Standard_ErrorHandler.hxx>
55
56 //=======================================================================
57 //function : Controle
58 //purpose  : 
59 //=======================================================================
60 static Standard_Real Controle(const TColgp_SequenceOfPnt& thePoints,
61   const Handle(Geom_Plane)& thePlane)
62 {
63   Standard_Real dfMaxDist=0.;
64   Standard_Real a,b,c,d, dist;
65   Standard_Integer ii;
66   thePlane->Coefficients(a,b,c,d);
67   for (ii=1; ii<=thePoints.Length(); ii++) {
68     const gp_XYZ& xyz = thePoints(ii).XYZ();
69     dist = Abs(a*xyz.X() + b*xyz.Y() + c*xyz.Z() + d);
70     if (dist > dfMaxDist)
71       dfMaxDist = dist;
72   }
73
74   return dfMaxDist;
75 }
76 //=======================================================================
77 //function : Is2DConnected
78 //purpose  : Return true if the last vertex of theEdge1 coincides with
79 //           the first vertex of theEdge2 in parametric space of theFace
80 //=======================================================================
81 inline static Standard_Boolean Is2DConnected(const TopoDS_Edge& theEdge1,
82   const TopoDS_Edge& theEdge2,
83   const Handle(Geom_Surface)& theSurface,
84   const TopLoc_Location& theLocation)
85 {
86   Standard_Real f,l;
87   //TopLoc_Location aLoc;
88   Handle(Geom2d_Curve) aCurve;
89   gp_Pnt2d p1, p2;
90
91   // get 2D points
92   aCurve=BRep_Tool::CurveOnSurface(theEdge1, theSurface, theLocation, f, l);
93   p1       = aCurve->Value( theEdge1.Orientation() == TopAbs_FORWARD ? l : f );
94   aCurve=BRep_Tool::CurveOnSurface(theEdge2, theSurface, theLocation, f, l);
95   p2       = aCurve->Value(  theEdge2.Orientation() == TopAbs_FORWARD ? f : l );
96
97   // compare 2D points
98   GeomAdaptor_Surface aSurface( theSurface );
99   TopoDS_Vertex    aV = TopExp::FirstVertex( theEdge2, Standard_True );
100   Standard_Real tol3D = BRep_Tool::Tolerance( aV );
101   Standard_Real tol2D = aSurface.UResolution( tol3D ) + aSurface.VResolution( tol3D );
102   Standard_Real dist2 = p1.SquareDistance( p2 );
103   return dist2 < tol2D * tol2D;
104 }
105
106 //=======================================================================
107 //function : Is2DClosed
108 //purpose  : Return true if edges of theShape form a closed wire in
109 //           parametric space of theSurface
110 //=======================================================================
111
112 static Standard_Boolean Is2DClosed(const TopoDS_Shape&         theShape,
113   const Handle(Geom_Surface)& theSurface,
114   const TopLoc_Location& theLocation)
115 {
116   try
117   {
118     // get a wire theShape 
119     TopExp_Explorer aWireExp( theShape, TopAbs_WIRE );
120     if ( !aWireExp.More() ) {
121       return Standard_False;
122     }
123     TopoDS_Wire aWire = TopoDS::Wire( aWireExp.Current() );
124     // a tmp face
125     TopoDS_Face aTmpFace = BRepLib_MakeFace( theSurface, Precision::PConfusion() );
126
127     // check topological closeness using wire explorer, if the wire is not closed
128     // the 1st and the last vertices of wire are different
129     BRepTools_WireExplorer aWireExplorer( aWire, aTmpFace );
130     if ( !aWireExplorer.More()) {
131       return Standard_False;
132     }
133     // remember the 1st and the last edges of aWire
134     TopoDS_Edge aFisrtEdge = aWireExplorer.Current(), aLastEdge = aFisrtEdge;
135     // check if edges connected topologically (that is assured by BRepTools_WireExplorer)
136     // are connected in 2D
137     TopoDS_Edge aPrevEdge = aFisrtEdge;
138     for ( aWireExplorer.Next(); aWireExplorer.More(); aWireExplorer.Next() )    {
139       aLastEdge = aWireExplorer.Current();
140       if ( !Is2DConnected( aPrevEdge, aLastEdge, theSurface, theLocation)) { 
141         return false;
142       }
143       aPrevEdge = aLastEdge;
144     }
145     // wire is closed if ( 1st vertex of aFisrtEdge ) ==
146     // ( last vertex of aLastEdge ) in 2D
147     TopoDS_Vertex aV1 = TopExp::FirstVertex( aFisrtEdge, Standard_True );
148     TopoDS_Vertex aV2 = TopExp::LastVertex( aLastEdge, Standard_True );
149     return ( aV1.IsSame( aV2 ) && Is2DConnected( aLastEdge, aFisrtEdge, theSurface, theLocation));
150   }
151   catch ( Standard_Failure )  {
152     return Standard_False;
153   }
154 }
155 //=======================================================================
156 //function : BRepLib_FindSurface
157 //purpose  : 
158 //=======================================================================
159 BRepLib_FindSurface::BRepLib_FindSurface() 
160 {
161 }
162 //=======================================================================
163 //function : BRepLib_FindSurface
164 //purpose  : 
165 //=======================================================================
166 BRepLib_FindSurface::BRepLib_FindSurface(const TopoDS_Shape&    S, 
167   const Standard_Real    Tol,
168   const Standard_Boolean OnlyPlane,
169   const Standard_Boolean OnlyClosed)
170 {
171   Init(S,Tol,OnlyPlane,OnlyClosed);
172 }
173 //=======================================================================
174 //function : Init
175 //purpose  : 
176 //=======================================================================
177 void BRepLib_FindSurface::Init(const TopoDS_Shape&    S, 
178   const Standard_Real    Tol,
179   const Standard_Boolean OnlyPlane,
180   const Standard_Boolean OnlyClosed)
181 {
182   myTolerance = Tol;
183   myTolReached = 0.;
184   isExisted = Standard_False;
185   myLocation.Identity();
186   mySurface.Nullify();
187
188   // compute the tolerance
189   TopExp_Explorer ex;
190
191   for (ex.Init(S,TopAbs_EDGE); ex.More(); ex.Next()) {
192     Standard_Real t = BRep_Tool::Tolerance(TopoDS::Edge(ex.Current()));
193     if (t > myTolerance) myTolerance = t;
194   }
195
196   // search an existing surface
197   ex.Init(S,TopAbs_EDGE);
198   if (!ex.More()) return;    // no edges ....
199
200   TopoDS_Edge E = TopoDS::Edge(ex.Current());
201   Standard_Real f,l,ff,ll;
202   Handle(Geom2d_Curve) PC,PPC;
203   Handle(Geom_Surface) SS;
204   TopLoc_Location L;
205   Standard_Integer i = 0,j;
206
207   // iterate on the surfaces of the first edge
208   for(;;) {
209     i++;
210     BRep_Tool::CurveOnSurface(E,PC,mySurface,myLocation,f,l,i);
211     if (mySurface.IsNull()) {
212       break;
213     }
214     // check the other edges
215     for (ex.Init(S,TopAbs_EDGE); ex.More(); ex.Next()) {
216       if (!E.IsSame(ex.Current())) {
217         j = 0;
218         for(;;) {
219           j++;
220           BRep_Tool::CurveOnSurface(TopoDS::Edge(ex.Current()),
221             PPC,SS,L,ff,ll,j);
222           if (SS.IsNull()) {
223             break;
224           }
225           if (SS == mySurface) {
226             break;
227           }
228           SS.Nullify();
229         }
230
231         if (SS.IsNull()) {
232           mySurface.Nullify();
233           break;
234         }
235       }
236     }
237
238     // if OnlyPlane, eval if mySurface is a plane.
239     if ( OnlyPlane && !mySurface.IsNull() ) 
240     {
241       if ( mySurface->IsKind( STANDARD_TYPE(Geom_RectangularTrimmedSurface)))
242         mySurface = Handle(Geom_RectangularTrimmedSurface)::DownCast(mySurface)->BasisSurface();
243       mySurface = Handle(Geom_Plane)::DownCast(mySurface);
244     }
245
246     if (!mySurface.IsNull())
247       // if S is e.g. the bottom face of a cylinder, mySurface can be the
248       // lateral (cylindrical) face of the cylinder; reject an improper mySurface
249       if ( !OnlyClosed || Is2DClosed( S, mySurface, myLocation ))
250         break;
251   }
252
253   if (!mySurface.IsNull()) {
254     isExisted = Standard_True;
255     return;
256   }
257   //
258   // no existing surface, search a plane
259   // 07/02/02 akm vvv : (OCC157) changed algorithm
260   //                    1. Collect the points along all edges of the shape
261   //                       For each point calculate the WEIGHT = sum of
262   //                       distances from neighboring points (_only_ same edge)
263   //                    2. Minimizing the weighed sum of squared deviations
264   //                       compute coefficients of the sought plane.
265
266   TColgp_SequenceOfPnt aPoints;
267   TColStd_SequenceOfReal aWeight;
268
269   // ======================= Step #1
270   for (ex.Init(S,TopAbs_EDGE); ex.More(); ex.Next()) 
271   {
272     BRepAdaptor_Curve c(TopoDS::Edge(ex.Current()));
273
274     Standard_Real dfUf = c.FirstParameter();
275     Standard_Real dfUl = c.LastParameter();
276     if (IsEqual(dfUf,dfUl)) {
277       // Degenerate
278       continue;
279     }
280     Standard_Integer iNbPoints=0;
281
282     // Add the points with weights to the sequences
283     switch (c.GetType()) 
284     {
285     case GeomAbs_BezierCurve:
286       {
287         // Put all poles for bezier
288         Handle(Geom_BezierCurve) GC = c.Bezier();
289         Standard_Integer iNbPol = GC->NbPoles();
290         Standard_Real tf = GC->FirstParameter();
291         Standard_Real tl = GC->LastParameter();
292         Standard_Real r =  (dfUl - dfUf) / (tl - tf);
293         r *= iNbPol;
294         if ( iNbPol < 2 || r < 1.)
295           // Degenerate
296           continue;
297         else
298         {
299           Handle(TColgp_HArray1OfPnt) aPoles = new (TColgp_HArray1OfPnt) (1, iNbPol);
300           GC->Poles(aPoles->ChangeArray1());
301           gp_Pnt aPolePrev = aPoles->Value(1), aPoleNext;
302           Standard_Real dfDistPrev = 0., dfDistNext;
303           for (Standard_Integer iPol=1; iPol<=iNbPol; iPol++)
304           {
305             if (iPol<iNbPol)
306             {
307               aPoleNext = aPoles->Value(iPol+1);
308               dfDistNext = aPolePrev.Distance(aPoleNext);
309             }
310             else
311               dfDistNext = 0.;
312             aPoints.Append (aPolePrev);
313             aWeight.Append (dfDistPrev+dfDistNext);
314             dfDistPrev = dfDistNext;
315             aPolePrev = aPoleNext;
316           }
317         }
318       }
319       break;
320     case GeomAbs_BSplineCurve:
321       {
322         // Put all poles for bspline
323         Handle(Geom_BSplineCurve) GC = c.BSpline();
324         Standard_Integer iNbPol = GC->NbPoles();
325         Standard_Real tf = GC->FirstParameter();
326         Standard_Real tl = GC->LastParameter();
327         Standard_Real r =  (dfUl - dfUf) / (tl - tf);
328         r *= iNbPol;
329         if ( iNbPol < 2 || r < 1.)
330           // Degenerate
331           continue;
332         else
333         {
334           const Standard_Integer aNbPolMax = 200;
335           Standard_Integer incr = 1;
336           if(iNbPol > aNbPolMax)
337           {
338             Standard_Integer nb = iNbPol;
339             while(nb > aNbPolMax)
340             {
341               incr++;
342               nb = (iNbPol-1) / incr;
343             }
344           }
345           Handle(TColgp_HArray1OfPnt) aPoles = new (TColgp_HArray1OfPnt) (1, iNbPol);
346           GC->Poles(aPoles->ChangeArray1());
347           gp_Pnt aPolePrev = aPoles->Value(1), aPoleNext;
348           Standard_Real dfDistPrev = 0., dfDistNext;
349           Standard_Integer iPol;
350           for (iPol = 1; iPol <= iNbPol; iPol += incr)
351           {
352             if (iPol <= iNbPol - incr)
353             {
354               aPoleNext = aPoles->Value(iPol+incr);
355               dfDistNext = aPolePrev.Distance(aPoleNext);
356             }
357             else
358               dfDistNext = 0.;
359             aPoints.Append (aPolePrev);
360             aWeight.Append (dfDistPrev+dfDistNext);
361             dfDistPrev = dfDistNext;
362             aPolePrev = aPoleNext;
363           }
364         }
365       }
366       break;
367
368     case GeomAbs_Line:
369     case GeomAbs_Circle:
370     case GeomAbs_Ellipse:
371     case GeomAbs_Hyperbola:
372     case GeomAbs_Parabola:
373       if (c.GetType() == GeomAbs_Line)
374         // Two points on straight segment
375         iNbPoints=2;
376       else
377         // Four points on otheranalitical curves
378         iNbPoints=4;
379     default:
380       { 
381         // Put some points on other curves
382         if (iNbPoints==0)
383           iNbPoints = 15 + c.NbIntervals(GeomAbs_C3);
384         Standard_Real dfDelta = (dfUl-dfUf)/(iNbPoints-1);
385         Standard_Integer iPoint;
386         Standard_Real dfU;
387         gp_Pnt aPointPrev = c.Value(dfUf), aPointNext;
388         Standard_Real dfDistPrev = 0., dfDistNext;
389         for (iPoint=1, dfU=dfUf+dfDelta; 
390           iPoint<=iNbPoints; 
391           iPoint++, dfU+=dfDelta) 
392         {
393           if (iPoint<iNbPoints)
394           {
395             aPointNext = c.Value(dfU);
396             dfDistNext = aPointPrev.Distance(aPointNext);
397           }
398           else
399             dfDistNext = 0.;
400           aPoints.Append (aPointPrev);
401           aWeight.Append (dfDistPrev+dfDistNext);
402           dfDistPrev = dfDistNext;
403           aPointPrev = aPointNext;
404         }
405       } // default:
406     } // switch (c.GetType()) ...
407   } // for (ex.Init(S,TopAbs_EDGE); ex.More() && control; ex.Next()) ...
408
409   if (aPoints.Length() < 3) {
410     return;
411   }
412
413   // ======================= Step #2
414   myLocation.Identity();
415   Standard_Integer iPoint;
416   math_Matrix aMat (1,3,1,3, 0.);
417   math_Vector aVec (1,3, 0.);
418   // Find the barycenter and normalize weights 
419   Standard_Real dfMaxWeight=0.;
420   gp_XYZ aBaryCenter(0.,0.,0.);
421   Standard_Real dfSumWeight=0.;
422   for (iPoint=1; iPoint<=aPoints.Length(); iPoint++)  {
423     Standard_Real dfW = aWeight(iPoint);
424     aBaryCenter += dfW*aPoints(iPoint).XYZ();
425     dfSumWeight += dfW;
426     if (dfW > dfMaxWeight) {
427       dfMaxWeight = dfW;
428     }
429   }
430   aBaryCenter /= dfSumWeight;
431
432   // Fill the matrix and the right vector
433   for (iPoint=1; iPoint<=aPoints.Length(); iPoint++)  {
434     gp_XYZ p=aPoints(iPoint).XYZ()-aBaryCenter;
435     Standard_Real w=aWeight(iPoint)/dfMaxWeight;
436     aMat(1,1)+=w*p.X()*p.X(); 
437     aMat(1,2)+=w*p.X()*p.Y(); 
438     aMat(1,3)+=w*p.X()*p.Z();
439     aMat(2,1)+=w*p.Y()*p.X();  
440     aMat(2,2)+=w*p.Y()*p.Y();  
441     aMat(2,3)+=w*p.Y()*p.Z();
442     aMat(3,1)+=w*p.Z()*p.X();  
443     aMat(3,2)+=w*p.Z()*p.Y(); 
444     aMat(3,3)+=w*p.Z()*p.Z();
445     aVec(1) -= w*p.X();
446     aVec(2) -= w*p.Y();
447     aVec(3) -= w*p.Z();
448   }
449
450   // Solve the system of equations to get plane coefficients
451   math_Gauss aSolver(aMat);
452   Standard_Boolean isSolved = aSolver.IsDone();
453   //
454   //  let us be more tolerant (occ415)
455   Standard_Real dfDist = RealLast();
456   Handle(Geom_Plane) aPlane;
457   //
458   if (isSolved)  {
459     aSolver.Solve(aVec);
460     if (aVec.Norm2()<gp::Resolution()) {
461       isSolved = Standard_False;
462     }
463   }
464   //
465   if (isSolved)  {
466     aPlane = new Geom_Plane(aBaryCenter,gp_Dir(aVec(1),aVec(2),aVec(3)));
467     dfDist = Controle (aPoints, aPlane);
468   }
469   //
470   if (!isSolved || myTolerance < dfDist)  {
471     gp_Pnt aFirstPnt=aPoints(1);
472     for (iPoint=2; iPoint<=aPoints.Length(); iPoint++)  {
473       const gp_Pnt& aNextPnt = aPoints(iPoint); 
474       gp_Vec aDir(aFirstPnt, aNextPnt);
475       Standard_Real dfSide=aDir.Magnitude();
476       if (dfSide<myTolerance) {
477         continue; // degeneration
478       }
479       for (Standard_Integer iP1=iPoint+1; iP1<=aPoints.Length(); iP1++) {
480         gp_Vec aCross = gp_Vec(aFirstPnt,aPoints(iP1)) ^ aDir ;
481         if (aCross.Magnitude() > dfSide*myTolerance) {
482           Handle(Geom_Plane) aPlane2 = new Geom_Plane(aFirstPnt, aCross);
483           Standard_Real dfDist2 = Controle (aPoints, aPlane2);
484           if (dfDist2 < myTolerance)  {
485             myTolReached = dfDist2;
486             mySurface = aPlane2;
487             return;
488           }
489           if (dfDist2 < dfDist)  {
490             dfDist = dfDist2;
491             aPlane = aPlane2;
492           }
493         }
494       }
495     }
496   }
497   //
498   //XXf
499   //static Standard_Real weakness = 5.0;
500   Standard_Real weakness = 5.0;
501   //XXf
502   if(dfDist <= myTolerance || (dfDist < myTolerance*weakness && Tol<0)) { 
503     //XXf 
504     //myTolReached = dfDist;
505     //XXt
506     mySurface = aPlane;
507   }
508   //XXf
509   myTolReached = dfDist;
510   //XXt
511 }
512 //=======================================================================
513 //function : Found
514 //purpose  : 
515 //=======================================================================
516 Standard_Boolean BRepLib_FindSurface::Found() const 
517 {
518   return !mySurface.IsNull();
519 }
520 //=======================================================================
521 //function : Surface
522 //purpose  : 
523 //=======================================================================
524 Handle(Geom_Surface) BRepLib_FindSurface::Surface() const 
525 {
526   return mySurface;
527 }
528 //=======================================================================
529 //function : Tolerance
530 //purpose  : 
531 //=======================================================================
532 Standard_Real BRepLib_FindSurface::Tolerance() const 
533 {
534   return myTolerance;
535 }
536 //=======================================================================
537 //function : ToleranceReached
538 //purpose  : 
539 //=======================================================================
540 Standard_Real BRepLib_FindSurface::ToleranceReached() const 
541 {
542   return myTolReached;
543 }
544 //=======================================================================
545 //function : Existed
546 //purpose  : 
547 //=======================================================================
548 Standard_Boolean BRepLib_FindSurface::Existed() const 
549 {
550   return isExisted;
551 }
552 //=======================================================================
553 //function : Location
554 //purpose  : 
555 //=======================================================================
556 TopLoc_Location BRepLib_FindSurface::Location() const
557 {
558   return myLocation;
559 }
560