0025028: BRepAlgo::ConcatenateWire raises an exception
[occt.git] / src / BRepAlgo / BRepAlgo.cxx
1 // Created on: 1997-03-10
2 // Created by: Stagiaire Francois DUMONT
3 // Copyright (c) 1997-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 <BRepAlgo.ixx>
18 #include <BRepTools_WireExplorer.hxx>
19 #include <BRep_Tool.hxx>
20 #include <BRepLib_MakeEdge.hxx>
21 #include <BRepLib_MakeWire.hxx>
22 #include <gp_Pnt.hxx>
23 #include <GeomConvert.hxx>
24 #include <GeomLProp.hxx>
25 #include <Geom_TrimmedCurve.hxx>
26 #include <Precision.hxx>
27 #include <Standard_ConstructionError.hxx>
28 #include <TColGeom_Array1OfBSplineCurve.hxx>
29 #include <TColGeom_HArray1OfBSplineCurve.hxx>
30 #include <TColStd_Array1OfReal.hxx>
31 #include <TColStd_Array1OfBoolean.hxx>
32 #include <TopoDS_Edge.hxx>
33 #include <TopoDS_Vertex.hxx>
34 #include <TopLoc_Location.hxx>
35 #include <TopExp.hxx>
36
37 #include <TColGeom_SequenceOfCurve.hxx>
38 #include <TopTools_SequenceOfShape.hxx>
39 #include <TColStd_SequenceOfReal.hxx>
40 #include <TColStd_SequenceOfBoolean.hxx>
41 #include <GeomAbs_CurveType.hxx>
42 #include <BRepAdaptor_Curve.hxx>
43 #include <ElCLib.hxx>
44 #include <BRepLib.hxx>
45 #include <TopoDS.hxx>
46 #include <ShapeFix_Shape.hxx>
47 #include <GeomConvert_CompCurveToBSplineCurve.hxx>
48
49 //Temporary
50 #//include <DrawTrSurf.hxx>
51
52
53 //=======================================================================
54 //function : ConcatenateWire
55 //purpose  : 
56 //=======================================================================
57
58 TopoDS_Wire  BRepAlgo::ConcatenateWire(const TopoDS_Wire& W,
59   const GeomAbs_Shape Option,
60   const Standard_Real TolAngular) 
61 {
62
63
64   Standard_Integer        nb_curve,                         //number of curves in the Wire
65     index;
66   BRepTools_WireExplorer  WExp(W) ;
67   TopoDS_Edge             edge;
68   TopLoc_Location         L ;
69   Standard_Real           First=0.,Last=0.,                       //extremal values for the curve
70     First0 = 0.,
71     toler = 0.,
72     tolleft,tolright;                 //Vertex tolerances
73   TopoDS_Vertex           Vfirst,Vlast;                     //Vertex of the Wire
74   gp_Pnt                  Pfirst,Plast;            //, Pint;  corresponding points
75
76   BRepLib_MakeWire        MakeResult;                       
77   Standard_Real           closed_tolerance =0.0;
78   Standard_Boolean        closed_flag = Standard_False ;
79
80   nb_curve = 0;
81
82   while ( WExp.More()){                                     //computation of the curve number
83     nb_curve++ ;
84     WExp.Next();
85   }
86
87   if (nb_curve > 1) {
88     TColGeom_Array1OfBSplineCurve tab(0,nb_curve-1);          //array of the wire's curve
89     TColStd_Array1OfReal tabtolvertex(0,nb_curve-2);          //array of the tolerance's vertex
90
91     WExp.Init(W);
92
93     for (index=0 ;index<nb_curve; index++){                   //main loop
94       edge = WExp.Current() ;
95       const Handle(Geom_Curve)& aCurve = BRep_Tool::Curve(edge, L, First, Last);
96       Handle(Geom_TrimmedCurve) aTrCurve = new Geom_TrimmedCurve(aCurve, First, Last);
97       tab(index) = GeomConvert::CurveToBSplineCurve(aTrCurve); //storage in a array 
98       tab(index)->Transform(L.Transformation());
99       GeomConvert::C0BSplineToC1BSplineCurve(tab(index),Precision::Confusion());
100
101       if (index >= 1){                                         //continuity test loop
102         if (edge.Orientation()==TopAbs_REVERSED)
103           tab(index)->Reverse();
104         tolleft=BRep_Tool::Tolerance(TopExp::LastVertex(edge));
105         tolright=BRep_Tool::Tolerance(TopExp::FirstVertex(edge));
106         tabtolvertex(index-1)=Max(tolleft,tolright);
107       }
108
109       if(index==0){                                           //storage of the first edge features
110         First0=First;
111         if(edge.Orientation()==TopAbs_REVERSED){             //(usefull for the closed wire) 
112           Vfirst=TopExp::LastVertex(edge);
113           tab(index)->Reverse();
114         }
115         else
116           Vfirst=TopExp::FirstVertex(edge);
117       }
118
119       if(index==nb_curve-1){                                  //storage of the last edge features
120         if(edge.Orientation()==TopAbs_REVERSED)
121           Vlast=TopExp::FirstVertex(edge);
122         else
123           Vlast=TopExp::LastVertex(edge);
124       }
125       WExp.Next() ; 
126     }
127
128     if (BRep_Tool::Tolerance(Vfirst)>BRep_Tool::Tolerance(Vlast)) //computation of the closing tolerance
129       toler=BRep_Tool::Tolerance(Vfirst);
130     else
131       toler=BRep_Tool::Tolerance(Vlast);
132
133     Pfirst=BRep_Tool::Pnt(Vfirst);
134     Plast=BRep_Tool::Pnt(Vlast); 
135
136     if ((Pfirst.Distance(Plast)<=toler)&&                   //C0 continuity test at the closing point
137       (GeomLProp::Continuity(tab(nb_curve-1),tab(0),Last,First0,
138       Standard_True,Standard_True, 
139       toler, TolAngular)>=GeomAbs_G1)) 
140     {
141       closed_tolerance =toler;                                        //if ClosedG1!=0 it will be True and
142       closed_flag = Standard_True ;
143     }                                                        //with the toler value
144     Handle(TColGeom_HArray1OfBSplineCurve)  concatcurve;     //array of the concatenated curves
145     Handle(TColStd_HArray1OfInteger)        ArrayOfIndices;  //array of the remining Vertex
146     if (Option==GeomAbs_G1)
147       GeomConvert::ConcatG1(tab,
148       tabtolvertex,
149       concatcurve,
150       closed_flag,
151       closed_tolerance) ;    //G1 concatenation
152     else
153       GeomConvert::ConcatC1(tab,
154       tabtolvertex,
155       ArrayOfIndices,
156       concatcurve,
157       closed_flag,
158       closed_tolerance);   //C1 concatenation
159
160     for (index=0;index<=(concatcurve->Length()-1);index++){    //building of the resulting Wire
161       BRepLib_MakeEdge EdgeBuilder(concatcurve->Value(index));
162       edge = EdgeBuilder.Edge();
163       MakeResult.Add(edge);
164     } 
165
166   }
167   else {
168
169     WExp.Init(W);
170
171     edge = WExp.Current() ;
172     const Handle(Geom_Curve)& aC = BRep_Tool::Curve(edge,L,First,Last);
173     Handle(Geom_BSplineCurve) aBS  = GeomConvert::CurveToBSplineCurve(new Geom_TrimmedCurve(aC,First,Last));
174     aBS->Transform(L.Transformation());
175     GeomConvert::C0BSplineToC1BSplineCurve(aBS, Precision::Confusion());
176     if (edge.Orientation()==TopAbs_REVERSED)
177     {
178       aBS->Reverse();
179     }
180
181     BRepLib_MakeEdge EdgeBuilder(aBS);
182     edge = EdgeBuilder.Edge();
183     MakeResult.Add(edge);
184   }
185   return MakeResult.Wire() ;  
186
187 }
188
189 //=======================================================================
190 //function : ConcatenateWireC0
191 //purpose  : 
192 //=======================================================================
193
194 TopoDS_Edge  BRepAlgo::ConcatenateWireC0(const TopoDS_Wire& aWire)
195 {
196   Standard_Real LinTol = Precision::Confusion();
197   Standard_Real AngTol = Precision::Angular();
198
199   TopoDS_Edge ResEdge;
200
201   TopoDS_Wire theWire = aWire;
202   BRepLib::BuildCurves3d(theWire);
203   Handle(ShapeFix_Shape) Fixer = new ShapeFix_Shape(theWire);
204   Fixer->SetPrecision(LinTol);
205   Fixer->SetMaxTolerance(LinTol);
206   Fixer->Perform();
207   theWire = TopoDS::Wire(Fixer->Shape());
208
209   TColGeom_SequenceOfCurve CurveSeq;
210   TColStd_SequenceOfReal FparSeq;
211   TColStd_SequenceOfReal LparSeq;
212   TColStd_SequenceOfReal TolSeq;
213   TColStd_SequenceOfBoolean IsFwdSeq;
214   GeomAbs_CurveType CurType = GeomAbs_OtherCurve;
215   TopoDS_Vertex FirstVertex, LastVertex;
216
217   BRepTools_WireExplorer wexp(theWire);
218
219   for (; wexp.More(); wexp.Next()) {
220     TopoDS_Edge anEdge = wexp.Current();
221     Standard_Real fpar, lpar;
222     Handle(Geom_Curve) aCurve = BRep_Tool::Curve(anEdge, fpar, lpar);
223
224     if (aCurve.IsNull())
225       continue;
226
227     GeomAdaptor_Curve  aGACurve(aCurve);
228     GeomAbs_CurveType  aType       = aGACurve.GetType();
229     Handle(Geom_Curve) aBasisCurve = aGACurve.Curve();
230     Standard_Boolean   isFwd       = (wexp.Orientation() != TopAbs_REVERSED);
231
232     if (aBasisCurve->IsPeriodic()) {
233       ElCLib::AdjustPeriodic
234         (aBasisCurve->FirstParameter(), aBasisCurve->LastParameter(),
235         Precision::PConfusion(), fpar, lpar);
236     }
237
238     if (CurveSeq.IsEmpty()) {
239       CurveSeq.Append(aCurve);
240       FparSeq.Append(fpar);
241       LparSeq.Append(lpar);
242       IsFwdSeq.Append(isFwd);
243       CurType     = aType;
244       FirstVertex = wexp.CurrentVertex();
245     } else {
246       Standard_Boolean isSameCurve = Standard_False;
247       Standard_Real NewFpar = RealFirst(), NewLpar = RealLast();
248       GeomAdaptor_Curve GAprevcurve(CurveSeq.Last());
249
250       if (aCurve == CurveSeq.Last()) {
251         NewFpar = fpar;
252         NewLpar = lpar;
253         isSameCurve = Standard_True;
254       } else if (aType == CurType &&
255         aType != GeomAbs_BezierCurve &&
256         aType != GeomAbs_BSplineCurve &&
257         aType != GeomAbs_OtherCurve) {
258           switch (aType) {
259           case GeomAbs_Line:
260             {
261               gp_Lin aLine    = aGACurve.Line();
262               gp_Lin PrevLine = GAprevcurve.Line(); 
263
264               if (aLine.Contains(PrevLine.Location(), LinTol) &&
265                 aLine.Direction().IsParallel(PrevLine.Direction(), AngTol)) {
266                   gp_Pnt P1 = ElCLib::Value(fpar, aLine);
267                   gp_Pnt P2 = ElCLib::Value(lpar, aLine);
268
269                   NewFpar = ElCLib::Parameter(PrevLine, P1);
270                   NewLpar = ElCLib::Parameter(PrevLine, P2);
271                   isSameCurve = Standard_True;
272               }
273               break;
274             }
275           case GeomAbs_Circle:
276             {
277               gp_Circ aCircle    = aGACurve.Circle();
278               gp_Circ PrevCircle = GAprevcurve.Circle();
279
280               if (aCircle.Location().Distance(PrevCircle.Location()) <= LinTol &&
281                 Abs(aCircle.Radius() - PrevCircle.Radius()) <= LinTol &&
282                 aCircle.Axis().IsParallel(PrevCircle.Axis(), AngTol)) {
283                   gp_Pnt P1 = ElCLib::Value(fpar, aCircle);
284                   gp_Pnt P2 = ElCLib::Value(lpar, aCircle);
285
286                   NewFpar = ElCLib::Parameter(PrevCircle, P1);
287                   NewLpar = ElCLib::Parameter(PrevCircle, P2);
288                   isSameCurve = Standard_True;
289               }
290               break;
291             }
292           case GeomAbs_Ellipse:
293             {
294               gp_Elips anEllipse   = aGACurve.Ellipse();
295               gp_Elips PrevEllipse = GAprevcurve.Ellipse();
296
297               if (anEllipse.Focus1().Distance(PrevEllipse.Focus1()) <= LinTol &&
298                 anEllipse.Focus2().Distance(PrevEllipse.Focus2()) <= LinTol &&
299                 Abs(anEllipse.MajorRadius() - PrevEllipse.MajorRadius()) <= LinTol &&
300                 Abs(anEllipse.MinorRadius() - PrevEllipse.MinorRadius()) <= LinTol &&
301                 anEllipse.Axis().IsParallel(PrevEllipse.Axis(), AngTol)) {
302                   gp_Pnt P1 = ElCLib::Value(fpar, anEllipse);
303                   gp_Pnt P2 = ElCLib::Value(lpar, anEllipse);
304
305                   NewFpar = ElCLib::Parameter(PrevEllipse, P1);
306                   NewLpar = ElCLib::Parameter(PrevEllipse, P2);
307                   isSameCurve = Standard_True;
308               }
309               break;
310             }
311           case GeomAbs_Hyperbola:
312             {
313               gp_Hypr aHypr    = aGACurve.Hyperbola();
314               gp_Hypr PrevHypr = GAprevcurve.Hyperbola();
315
316               if (aHypr.Focus1().Distance(PrevHypr.Focus1()) <= LinTol &&
317                 aHypr.Focus2().Distance(PrevHypr.Focus2()) <= LinTol &&
318                 Abs(aHypr.MajorRadius() - PrevHypr.MajorRadius()) <= LinTol &&
319                 Abs(aHypr.MinorRadius() - PrevHypr.MinorRadius()) <= LinTol &&
320                 aHypr.Axis().IsParallel(PrevHypr.Axis(), AngTol)) {
321                   gp_Pnt P1 = ElCLib::Value(fpar, aHypr);
322                   gp_Pnt P2 = ElCLib::Value(lpar, aHypr);
323
324                   NewFpar = ElCLib::Parameter(PrevHypr, P1);
325                   NewLpar = ElCLib::Parameter(PrevHypr, P2);
326                   isSameCurve = Standard_True;
327               }
328               break;
329             }
330           case GeomAbs_Parabola:
331             {
332               gp_Parab aParab    = aGACurve.Parabola();
333               gp_Parab PrevParab = GAprevcurve.Parabola();
334
335               if (aParab.Location().Distance(PrevParab.Location()) <= LinTol &&
336                 aParab.Focus().Distance(PrevParab.Focus()) <= LinTol &&
337                 Abs(aParab.Focal() - PrevParab.Focal()) <= LinTol &&
338                 aParab.Axis().IsParallel(PrevParab.Axis(), AngTol)) {
339                   gp_Pnt P1 = ElCLib::Value(fpar, aParab);
340                   gp_Pnt P2 = ElCLib::Value(lpar, aParab);
341
342                   NewFpar = ElCLib::Parameter(PrevParab, P1);
343                   NewLpar = ElCLib::Parameter(PrevParab, P2);
344                   isSameCurve = Standard_True;
345               }
346               break;
347             }
348           default:
349             break;
350           } //end of switch
351       } //end of else
352
353       if (isSameCurve) {
354         const Standard_Boolean isSameDir = (isFwd == IsFwdSeq.Last());
355
356         if (aBasisCurve->IsPeriodic()) {
357           // Treat periodic curves.
358           const Standard_Real aPeriod = aBasisCurve->Period();
359
360           if (isSameDir) {
361             // Check if first parameter is greater then the last one.
362             while (NewFpar > NewLpar) {
363               NewFpar -= aPeriod;
364             }
365           } else { // !isSameDir
366             // Check if last parameter is greater then the first one.
367             while (NewLpar > NewFpar) {
368               NewLpar -= aPeriod;
369             }
370
371             // Change parameters
372             const Standard_Real aTmpPar = NewLpar;
373
374             NewLpar = NewFpar;
375             NewFpar = aTmpPar;
376           }
377
378           // Udjust parameters on periodic curves.
379           if (IsFwdSeq.Last()) {
380             // The current curve should be after the previous one.
381             ElCLib::AdjustPeriodic(LparSeq.Last(), LparSeq.Last() + aPeriod,
382               Precision::PConfusion(), NewFpar, NewLpar);
383           } else {
384             // The current curve should be before the previous one.
385             ElCLib::AdjustPeriodic(FparSeq.Last() - aPeriod, FparSeq.Last(),
386               Precision::PConfusion(), NewFpar, NewLpar);
387           }
388         } else if (!isSameDir) {
389           // Not periodic curves. Opposite dirs.
390           const Standard_Real aTmpPar = NewLpar;
391
392           NewLpar = NewFpar;
393           NewFpar = aTmpPar;
394         }
395
396         if (IsFwdSeq.Last()) {
397           // Update last parameter 
398           LparSeq(LparSeq.Length()) = NewLpar;
399         } else {
400           // Update first parameter 
401           FparSeq(FparSeq.Length()) = NewFpar;
402         }
403       } else {
404         // Add new curve.
405         CurveSeq.Append(aCurve);
406         FparSeq.Append(fpar);
407         LparSeq.Append(lpar);
408         IsFwdSeq.Append(isFwd);
409         TolSeq.Append(BRep_Tool::Tolerance(wexp.CurrentVertex()));
410         CurType = aType;
411       }
412     }
413   }
414
415   LastVertex = wexp.CurrentVertex();
416   TolSeq.Append(BRep_Tool::Tolerance(LastVertex));
417
418   Standard_Boolean isReverse = Standard_False;
419
420   if (!IsFwdSeq.IsEmpty()) {
421     isReverse = !IsFwdSeq(1);
422   }
423
424   TopoDS_Vertex FirstVtx_final = (isReverse)? LastVertex : FirstVertex;
425   FirstVtx_final.Orientation(TopAbs_FORWARD);
426   TopoDS_Vertex LastVtx_final = (isReverse)? FirstVertex : LastVertex;
427   LastVtx_final.Orientation(TopAbs_REVERSED);
428
429   if (CurveSeq.IsEmpty())
430     return ResEdge;
431
432   Standard_Integer nb_curve = CurveSeq.Length();   //number of curves
433   TColGeom_Array1OfBSplineCurve tab(0,nb_curve-1);                    //array of the curves
434   TColStd_Array1OfReal tabtolvertex(0,nb_curve-1); //(0,nb_curve-2);  //array of the tolerances
435
436   Standard_Integer i;
437
438   if (nb_curve > 1)
439   {
440     for (i = 1; i <= nb_curve; i++)
441     {
442       if (CurveSeq(i)->IsInstance(STANDARD_TYPE(Geom_TrimmedCurve)))
443         CurveSeq(i) = (*((Handle(Geom_TrimmedCurve)*)&(CurveSeq(i))))->BasisCurve();
444
445       Handle(Geom_TrimmedCurve) aTrCurve = new Geom_TrimmedCurve(CurveSeq(i), FparSeq(i), LparSeq(i));
446       tab(i-1) = GeomConvert::CurveToBSplineCurve(aTrCurve);
447       GeomConvert::C0BSplineToC1BSplineCurve(tab(i-1), Precision::Confusion());
448
449       if (!IsFwdSeq(i)) {
450         tab(i-1)->Reverse();
451       }
452
453       //Temporary
454       //char* name = new char[100];
455       //sprintf(name, "c%d", i);
456       //DrawTrSurf::Set(name, tab(i-1));
457
458       if (i > 1)
459         tabtolvertex(i-2) = TolSeq(i-1) * 5.;
460     }
461     tabtolvertex(nb_curve-1) = TolSeq(TolSeq.Length()) * 5.;
462
463     Standard_Boolean closed_flag = Standard_False;
464     Standard_Real closed_tolerance = 0.;
465     if (FirstVertex.IsSame(LastVertex) &&
466       GeomLProp::Continuity(tab(0), tab(nb_curve-1),
467       tab(0)->FirstParameter(),
468       tab(nb_curve-1)->LastParameter(),
469       Standard_False, Standard_False, LinTol, AngTol) >= GeomAbs_G1)
470     {
471       closed_flag = Standard_True ;
472       closed_tolerance = BRep_Tool::Tolerance(FirstVertex);
473     }
474
475     Handle(TColGeom_HArray1OfBSplineCurve)  concatcurve;     //array of the concatenated curves
476     Handle(TColStd_HArray1OfInteger)        ArrayOfIndices;  //array of the remining Vertex
477     GeomConvert::ConcatC1(tab,
478       tabtolvertex,
479       ArrayOfIndices,
480       concatcurve,
481       closed_flag,
482       closed_tolerance);   //C1 concatenation
483
484     if (concatcurve->Length() > 1)
485     {
486       Standard_Real MaxTolVer = LinTol;
487       for (i = 1; i <= TolSeq.Length(); i++)
488         if (TolSeq(i) > MaxTolVer)
489           MaxTolVer = TolSeq(i);
490       MaxTolVer *= 5.;
491
492       GeomConvert_CompCurveToBSplineCurve Concat(concatcurve->Value(concatcurve->Lower()));
493
494       for (i = concatcurve->Lower()+1; i <= concatcurve->Upper(); i++)
495         Concat.Add( concatcurve->Value(i), MaxTolVer, Standard_True );
496
497       concatcurve->SetValue(concatcurve->Lower(), Concat.BSplineCurve());
498     }
499
500     ResEdge = BRepLib_MakeEdge(concatcurve->Value(concatcurve->Lower()),
501       FirstVtx_final, LastVtx_final,
502       concatcurve->Value(concatcurve->Lower())->FirstParameter(),
503       concatcurve->Value(concatcurve->Lower())->LastParameter());
504   }
505   else
506   {
507     if (CurveSeq(1)->IsInstance(STANDARD_TYPE(Geom_TrimmedCurve)))
508       CurveSeq(1) = (*((Handle(Geom_TrimmedCurve)*)&(CurveSeq(1))))->BasisCurve();
509
510     Handle(Geom_Curve) aCopyCurve =
511       Handle(Geom_Curve)::DownCast(CurveSeq(1)->Copy());
512
513     ResEdge = BRepLib_MakeEdge(aCopyCurve,
514       FirstVtx_final, LastVtx_final,
515       FparSeq(1), LparSeq(1));
516   }
517
518   if (isReverse)
519     ResEdge.Reverse();
520
521   return ResEdge;
522 }