0024002: Overall code and build procedure refactoring -- automatic
[occt.git] / src / BRepFill / BRepFill_LocationLaw.cxx
1 // Created on: 1998-01-14
2 // Created by: Philippe MANGIN
3 // Copyright (c) 1998-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
18 #include <Adaptor3d_HCurve.hxx>
19 #include <BRep_Builder.hxx>
20 #include <BRep_Tool.hxx>
21 #include <BRepAdaptor_Curve.hxx>
22 #include <BRepBuilderAPI_Transform.hxx>
23 #include <BRepFill_LocationLaw.hxx>
24 #include <BRepTools_WireExplorer.hxx>
25 #include <GCPnts_AbscissaPoint.hxx>
26 #include <GeomFill_LocationLaw.hxx>
27 #include <gp_Mat.hxx>
28 #include <gp_Trsf.hxx>
29 #include <gp_Vec.hxx>
30 #include <gp_XYZ.hxx>
31 #include <Precision.hxx>
32 #include <Standard_OutOfRange.hxx>
33 #include <Standard_Type.hxx>
34 #include <TColgp_Array1OfPnt2d.hxx>
35 #include <TColgp_Array1OfVec2d.hxx>
36 #include <TColStd_SequenceOfInteger.hxx>
37 #include <TopExp.hxx>
38 #include <TopLoc_Location.hxx>
39 #include <TopoDS.hxx>
40 #include <TopoDS_Edge.hxx>
41 #include <TopoDS_Shape.hxx>
42 #include <TopoDS_Vertex.hxx>
43 #include <TopoDS_Wire.hxx>
44
45 //=======================================================================
46 //function : Norm
47 //purpose  : Norm of a Matrix
48 //=======================================================================
49 static Standard_Real Norm(const gp_Mat& M) {
50   Standard_Real R, Norme;
51   gp_XYZ Coord;
52   Coord = M.Row(1);
53   Norme = Abs(Coord.X()) + Abs(Coord.Y())+ Abs(Coord.Z());
54   Coord = M.Row(2);
55   R = Abs(Coord.X()) + Abs(Coord.Y())+ Abs(Coord.Z());
56   if (R>Norme) Norme = R;
57   Coord = M.Row(3);
58   R = Abs(Coord.X()) + Abs(Coord.Y())+ Abs(Coord.Z());
59   if (R>Norme) Norme = R;
60
61   return Norme;
62 }
63
64 //=======================================================================
65 //function : ToG0
66 //purpose  : Calculate tranformation T such as T.M2 = M1
67 //=======================================================================
68
69 static void ToG0(const gp_Mat& M1, const gp_Mat& M2, gp_Mat& T) {
70   T =  M2.Inverted();
71   T *= M1;
72 }
73                           
74 //=======================================================================
75 //function : BRepFill_LocationLaw
76 //purpose  : 
77 //=======================================================================
78
79 void BRepFill_LocationLaw::Init(const TopoDS_Wire& Path)
80  
81 {
82   Standard_Integer NbEdge;
83   BRepTools_WireExplorer wexp;
84 // Class BRep_Tool without fields and without Constructor :
85 //  BRep_Tool B;
86   TopoDS_Edge E;
87
88   myPath = Path;
89   myTol = 1.e-4;
90
91   for (NbEdge=0, wexp.Init(myPath); 
92        wexp.More(); wexp.Next()) 
93 //    if (! B.Degenerated(wexp.Current())) NbEdge++;
94     if (! BRep_Tool::Degenerated(wexp.Current())) NbEdge++;
95   
96
97   myLaws = new (GeomFill_HArray1OfLocationLaw)(1, NbEdge);
98   myLength = new (TColStd_HArray1OfReal) (1, NbEdge+1);
99   myLength->Init(-1.);
100   myLength->SetValue(1, 0.);
101   myEdges = new (TopTools_HArray1OfShape) (1, NbEdge);
102   myDisc.Nullify();
103   TangentIsMain();
104 }
105
106 //=======================================================================
107 //function : GetStatus
108 //purpose  : 
109 //=======================================================================
110  GeomFill_PipeError BRepFill_LocationLaw::GetStatus() const
111 {
112   Standard_Integer ii, N = myLaws->Length();
113   GeomFill_PipeError Status =  GeomFill_PipeOk;
114   for (ii=1; ii<=N && (Status == GeomFill_PipeOk); ii++) {
115     Status = myLaws->Value(ii)->ErrorStatus();
116   }
117   return  Status;
118 }
119
120 //=======================================================================
121 //function : TangentIsMain
122 //purpose  : 
123 //=======================================================================
124 void BRepFill_LocationLaw::TangentIsMain() 
125 {
126   myType = 1;
127 }
128
129 //=======================================================================
130 //function : NormalIsMain
131 //purpose  : 
132 //=======================================================================
133 void BRepFill_LocationLaw::NormalIsMain() 
134 {
135   myType = 2;
136 }
137
138 //=======================================================================
139 //function : BiNormalIsMain
140 //purpose  : 
141 //=======================================================================
142 void BRepFill_LocationLaw::BiNormalIsMain() 
143 {
144   myType = 3;
145 }
146
147 //=======================================================================
148 //function : TransformInCompatibleLaw
149 //purpose  : Set in continuity of laws
150 //=======================================================================
151  void BRepFill_LocationLaw::TransformInCompatibleLaw(const Standard_Real TolAngular)
152 {
153
154   Standard_Real First, Last, Angle;
155   Standard_Integer ipath;
156   gp_Mat Trsf, M1, M2;
157   gp_Vec V, T1, T2, N1, N2;
158   gp_XYZ OZ(0, 0, 1);
159
160   myLaws->Value(1)->GetDomain(First, Last);
161
162   for (ipath=2; ipath<=myLaws->Length(); ipath++) {
163     myLaws->Value(ipath-1)->D0(Last, M1, V);
164     myLaws->Value(ipath)->GetDomain(First, Last);
165     myLaws->Value(ipath)->D0(First, M2, V);
166     T1.SetXYZ(M1.Column(3));
167     T2.SetXYZ(M2.Column(3));
168     N1.SetXYZ(M1.Column(1));
169     N2.SetXYZ(M2.Column(1));
170     if (T1.IsParallel(T2, TolAngular ) && 
171         !T1.IsOpposite(T2, TolAngular)) { // Correction G0
172       ToG0(M1, M2, Trsf);
173     }
174     else {
175       Standard_Real alpha;
176       gp_Vec cross(T1);
177       cross.Cross(T2);
178       alpha = T2.AngleWithRef(T1, cross);
179       gp_Ax1 axe(gp::Origin(), cross.XYZ());
180       N2.Rotate(axe, alpha); 
181
182 #ifdef OCCT_DEBUG
183       if (N2.Dot(T1) > 1.e-9) {
184         cout << "Inprecision in TransformInCompatibleLaw" << endl;
185         cout << "--- T1.R(N2) = " << N2.Dot(T1) << endl;
186         gp_Vec tt;
187         tt = T1;
188         tt.Rotate(axe, alpha);
189         cout << "--- T1.R(T2) = " << tt.Dot(T1) << endl;
190         cout << "--- R(N2).R(T2) = " << N2.Dot(tt) << endl;
191       }      
192 #endif
193       Angle = N2.AngleWithRef(N1, T1);
194       Trsf.SetRotation(OZ, Angle);
195     }
196     myLaws->Value(ipath)->SetTrsf(Trsf);
197   }
198
199
200 //=======================================================================
201 //function : TransformInG0Law
202 //purpose  : Set in continuity of laws
203 //=======================================================================
204  void BRepFill_LocationLaw::TransformInG0Law()
205 {
206
207   Standard_Real First, Last;
208   Standard_Integer ipath;
209   gp_Mat  M1, M2, aux;//,Trsf
210   gp_Vec V;
211   myLaws->Value(1)->GetDomain(First, Last);
212   for (ipath=2; ipath<=myLaws->Length(); ipath++) {
213     myLaws->Value(ipath-1)->D0(Last, M1, V);
214     myLaws->Value(ipath)->GetDomain(First, Last);
215     myLaws->Value(ipath)->D0(First, M2, V);
216     ToG0(M1, M2, aux);
217     myLaws->Value(ipath)->SetTrsf(aux);
218   }
219   
220   // Is the law periodical ?
221   if  (myPath.Closed()) {
222     myLaws->Value(myLaws->Length())->D0(Last, M1, V);
223     myLaws->Value(1)->GetDomain(First, Last);
224     myLaws->Value(1)->D0(First, M2, V);
225   }
226 }
227
228 //=======================================================================
229 //function : DeleteTransform
230 //purpose  : Remove the setting in continuity of law. 
231 //=======================================================================
232  void BRepFill_LocationLaw::DeleteTransform()
233 {
234   gp_Mat Id;
235   Id.SetIdentity();
236   for (Standard_Integer ii=1; ii<=myEdges->Length(); ii++) {
237     myLaws->ChangeValue(ii)->SetTrsf(Id);
238   }
239   myDisc.Nullify();
240 }
241
242 //=======================================================================
243 //function : NbHoles
244 //purpose  : Find "Holes"
245 //=======================================================================
246  Standard_Integer BRepFill_LocationLaw::NbHoles(const Standard_Real Tol) 
247 {
248   if (myDisc.IsNull()) {
249     TColStd_SequenceOfInteger Seq;
250     Standard_Integer ii, NbDisc;
251     for (ii=2, NbDisc=-1; ii<=myLaws->Length()+1; ii++) {
252       if (IsG1(ii-1, Tol, 1.e-12) == -1) {
253         Seq.Append(ii);
254       }
255     }
256     NbDisc = Seq.Length();
257     if ( NbDisc > 0) {
258       myDisc = new (TColStd_HArray1OfInteger)(1, NbDisc);
259       for (ii=1; ii<=NbDisc; ii++)
260          myDisc->SetValue(ii, Seq(ii));
261     }   
262   }
263   if (myDisc.IsNull()) return 0;
264   return myDisc->Length();
265
266 }
267
268 //=======================================================================
269 //function : Holes
270 //purpose  : 
271 //=======================================================================
272  void BRepFill_LocationLaw::Holes(TColStd_Array1OfInteger& Disc) const
273 {
274   if (!myDisc.IsNull()) {
275     for (Standard_Integer ii=1; ii<=myDisc->Length(); ii++)
276       Disc(ii) = myDisc->Value(ii);
277   }
278 }
279
280 //=======================================================================
281 //function : NbLaw
282 //purpose  : 
283 //=======================================================================
284  Standard_Integer BRepFill_LocationLaw::NbLaw() const
285 {
286   return myLaws->Length();
287 }
288
289 //=======================================================================
290 //function : Law
291 //purpose  : 
292 //=======================================================================
293 const Handle(GeomFill_LocationLaw)& 
294 BRepFill_LocationLaw::Law(const Standard_Integer Index) const
295 {
296   return myLaws->Value(Index);
297 }
298
299 //=======================================================================
300 //function : Wire
301 //purpose  : 
302 //=======================================================================
303 const TopoDS_Wire& BRepFill_LocationLaw::Wire() const
304 {
305   return myPath;
306 }
307
308 //=======================================================================
309 //function : Edge
310 //purpose  : 
311 //=======================================================================
312 const TopoDS_Edge& BRepFill_LocationLaw::Edge(const Standard_Integer Index) const
313 {
314   return TopoDS::Edge(myEdges->Value(Index));
315 }
316
317 //=======================================================================
318 //function : Vertex
319 //purpose  : 
320 //=======================================================================
321  TopoDS_Vertex BRepFill_LocationLaw::Vertex(const Standard_Integer Index) const
322 {
323   TopoDS_Edge E;
324   TopoDS_Vertex V;
325   if (Index <= myEdges->Length()) {
326     E = TopoDS::Edge(myEdges->Value(Index));
327     if (E.Orientation() == TopAbs_REVERSED) 
328        V = TopExp::LastVertex(E);
329     else V = TopExp::FirstVertex(E);
330   }
331   else if (Index == myEdges->Length()+1) {
332     E = TopoDS::Edge(myEdges->Value(Index-1));
333     if (E.Orientation() == TopAbs_REVERSED) 
334       V = TopExp::FirstVertex(E);
335     else V = TopExp::LastVertex(E);
336   }
337   return V;
338 }
339
340 //===================================================================
341 //function : PerformVertex
342 //purpose  : Calculate a vertex of sweeping from a vertex of section
343 //           and the index of the edge in the trajectory
344 //===================================================================
345 void BRepFill_LocationLaw::PerformVertex(const Standard_Integer Index,
346                                          const TopoDS_Vertex& Input,
347                                          const Standard_Real TolMin,
348                                          TopoDS_Vertex& Output,
349                                          const Standard_Integer ILoc) const
350 {
351   BRep_Builder B;
352   Standard_Boolean IsBary = (ILoc == 0);
353   Standard_Real First, Last;
354   gp_Pnt P;
355   gp_Vec V1, V2;//, V;
356   gp_Mat M1, M2;
357
358   if (Index>0 && Index<myLaws->Length()) {
359     if (ILoc <=0) {
360       myLaws->Value(Index)->GetDomain(First, Last);
361       myLaws->Value(Index)->D0(Last, M1, V1);
362     }
363
364     if (ILoc >= 0) {
365       myLaws->Value(Index+1)->GetDomain(First, Last);
366       if (ILoc == 0)
367         myLaws->Value(Index+1)->D0(First, M2, V2);
368       else
369         myLaws->Value(Index+1)->D0(First, M1, V1);
370     }
371   }
372
373   if (Index == 0 || Index == myLaws->Length()) {
374     if (!myPath.Closed() || (IsG1(Index, TolMin) != 1)) {
375       IsBary = Standard_False;
376       if (Index == 0) {
377         myLaws->Value(1)->GetDomain(First, Last);
378         myLaws->Value(1)->D0(First, M1, V1);
379       }
380       else {
381         myLaws->Value(myLaws->Length())->GetDomain(First, Last);
382         myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
383       } 
384     }
385     else {
386       if (ILoc <=0) {
387         myLaws->Value(myLaws->Length())->GetDomain(First, Last);
388         myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
389       }
390
391       if (ILoc >=0) {      
392         myLaws->Value(1)->GetDomain(First, Last);
393         if (ILoc==0)
394           myLaws->Value(1)->D0(First, M2, V2);
395         else
396           myLaws->Value(1)->D0(First, M1, V1);
397       }
398     }
399   }
400
401   P = BRep_Tool::Pnt(Input);
402
403   if (IsBary) { 
404     gp_XYZ P1(P.XYZ()), P2(P.XYZ());
405     P1 *= M1;
406     P1 += V1.XYZ();
407     P2 *= M2;
408     P2 += V2.XYZ();
409   
410     P.ChangeCoord().SetLinearForm(0.5, P1, 0.5, P2);
411     P1 -= P2;
412     Standard_Real Tol =  P1.Modulus()/2;
413     Tol += TolMin;
414     B.MakeVertex(Output, P, Tol);
415   }
416   else {
417     P.ChangeCoord() *= M1;
418     P.ChangeCoord() += V1.XYZ();
419     B.MakeVertex(Output, P, TolMin);    
420   }
421   
422 }
423
424 //=======================================================================
425 //function : CurvilinearBounds
426 //purpose  : 
427 //=======================================================================
428 void BRepFill_LocationLaw::CurvilinearBounds(const Standard_Integer Index,
429                                              Standard_Real& First, 
430                                              Standard_Real& Last) const
431 {
432   First = myLength->Value(Index);
433   Last  = myLength->Value(Index+1);
434   if (Last<0) { //It is required to carry out the calculation 
435     Standard_Integer ii, NbE = myEdges->Length();
436     Standard_Real Length, f, l;
437     GCPnts_AbscissaPoint AbsC;
438
439     for (ii=1, Length=0.; ii<=NbE; ii++) {
440       myLaws->Value(ii)->GetDomain(f, l);
441       Length += AbsC.Length(myLaws->Value(ii)->GetCurve()->GetCurve(), myTol);
442       myLength->SetValue(ii+1, Length);
443     }
444
445     First = myLength->Value(Index);
446     Last  = myLength->Value(Index+1); 
447   }
448 }
449
450  Standard_Boolean BRepFill_LocationLaw::IsClosed() const
451 {
452   return myPath.Closed();
453 }
454
455 //=======================================================================
456 //function : IsG1
457 //purpose  : Evaluate the continuity of the law by a vertex
458 //=======================================================================
459  Standard_Integer 
460  BRepFill_LocationLaw::IsG1(const Standard_Integer Index,
461                             const Standard_Real SpatialTolerance,
462                             const Standard_Real AngularTolerance) const
463 {
464   gp_Vec V1, DV1, V2, DV2;
465   gp_Mat M1, M2, DM1, DM2;
466   Standard_Real First, Last, EpsNul = 1.e-12;
467   Standard_Real TolEps = SpatialTolerance;
468   Standard_Boolean Ok_D1 = Standard_False;
469   TopoDS_Vertex V;
470   TopoDS_Edge E;
471   TColgp_Array1OfPnt2d Bid1 (1,1);
472   TColgp_Array1OfVec2d Bid2 (1,1);
473   
474   if (Index>0 && Index<myLaws->Length()) {
475     myLaws->Value(Index)->GetDomain(First, Last);
476     Ok_D1 = myLaws->Value(Index)->D1(Last, M1, V1, DM1, DV1, 
477                                      Bid1, Bid2);
478     if (!Ok_D1)  myLaws->Value(Index)->D0(Last, M1, V1);
479
480     myLaws->Value(Index+1)->GetDomain(First, Last);
481     if (Ok_D1)
482       Ok_D1 = myLaws->Value(Index+1)->D1(First, M2, V2, DM2, DV2, 
483                                          Bid1, Bid2);
484     if (!Ok_D1)  myLaws->Value(Index+1)->D0(First, M2, V2);
485
486     E = TopoDS::Edge(myEdges->Value(Index+1));
487   }
488   if (Index == 0 || Index == myLaws->Length()) {
489     if (!myPath.Closed()) return -1;
490     myLaws->Value(myLaws->Length())->GetDomain(First, Last);
491     Ok_D1 = myLaws->Value(myLaws->Length())->D1(Last, M1, V1, DM1, DV1, 
492                                                 Bid1, Bid2);
493     if (!Ok_D1)  myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
494
495     myLaws->Value(1)->GetDomain(First, Last);
496     if (Ok_D1)
497       myLaws->Value(1)->D1(First, M2, V2, DM2, DV2, 
498                            Bid1, Bid2);
499     if (!Ok_D1)  myLaws->Value(1)->D0(First, M2, V2);
500
501     E = TopoDS::Edge(myEdges->Value(1));
502   }
503
504   if (E.Orientation() == TopAbs_REVERSED)
505     V = TopExp::LastVertex(E);
506   else 
507     V = TopExp::FirstVertex(E);
508   
509   TolEps += 2*BRep_Tool::Tolerance(V);
510
511   Standard_Boolean isG0 = Standard_True;
512   Standard_Boolean isG1 = Standard_True;
513   
514   if ((V1-V2).Magnitude() > TolEps) isG0 = Standard_False;
515   if (Norm(M1-M2) > SpatialTolerance) isG0 = Standard_False;
516   
517   if (!isG0) return -1;
518   if (!Ok_D1) return 0; // No control of the derivative
519   
520   if ( (DV1.Magnitude()>EpsNul) && (DV2.Magnitude()>EpsNul)
521        && (DV1.Angle(DV2) > AngularTolerance) ) isG1 = Standard_False;
522
523   // For the next, the tests are mostly empirical
524   Standard_Real Norm1 = Norm(DM1);
525   Standard_Real Norm2 = Norm(DM2);
526   // It two 2 norms are null, it is good
527   if ((Norm1 > EpsNul) || (Norm2 > EpsNul)) {
528     // otherwise the normalized matrices are compared
529     if ((Norm1 > EpsNul) && (Norm2 > EpsNul)) {
530       DM1 /= Norm1;
531       DM2 /= Norm2;
532       if (Norm(DM1 - DM2) > AngularTolerance) isG1 = Standard_False;
533     }
534     else isG1 = Standard_False; // 1 Null the other is not   
535   }
536   
537   if (isG1) return 1;
538   else return 0;
539 }
540
541
542 //=======================================================================
543 //function : Parameter
544 //purpose  : 
545 //=======================================================================
546  void BRepFill_LocationLaw::Parameter(const Standard_Real Abcissa,
547                                       Standard_Integer& Index,
548                                       Standard_Real& U)
549 {
550   Standard_Integer iedge, NbE=myEdges->Length();
551   Standard_Boolean Trouve =  Standard_False;
552
553   //Control that the lengths are calculated
554   if (myLength->Value(NbE+1) < 0) {
555     Standard_Real f, l;
556     CurvilinearBounds(NbE, f, l);
557   }
558
559   // Find the interval
560   for (iedge=1; iedge<=NbE && !Trouve; ) {
561     if (myLength->Value(iedge+1) >= Abcissa) {
562       Trouve = Standard_True;
563     }
564     else iedge++;
565   }
566   
567   if (Trouve) {
568     Standard_Real f, l;
569     const Handle(GeomFill_LocationLaw)& Law =  myLaws->Value(iedge);
570     Law->GetDomain(f, l);
571
572     if  (Abcissa == myLength->Value(iedge+1)) {
573       U = l;
574     }
575     else if  (Abcissa == myLength->Value(iedge)) {
576       U = f;
577     } 
578     else {
579       GCPnts_AbscissaPoint 
580         AbsC(myTol, 
581              myLaws->Value(iedge)->GetCurve()->GetCurve(), 
582              Abcissa-myLength->Value(iedge), f);
583       U =  AbsC.Parameter();
584     }
585     Index = iedge;
586   }
587   else {
588     Index = 0;
589   } 
590 }
591
592
593 //===================================================================
594 //function : D0
595 //purpose  : Position of a section, with a given curviline abscissa
596 //===================================================================
597  void BRepFill_LocationLaw::D0(const Standard_Real Abcissa,
598                                TopoDS_Shape& W)
599 {
600   Standard_Real u;
601   Standard_Integer ind;
602   gp_Mat M;
603   gp_Vec V;
604
605   Parameter(Abcissa, ind, u);
606   if (ind != 0) {
607     // Positionement
608     myLaws->Value(ind)->D0(u, M, V);
609     gp_Trsf fila;
610     fila.SetValues(M(1,1), M(1,2), M(1,3), V.X(),
611                    M(2,1), M(2,2), M(2,3), V.Y(),
612                    M(3,1), M(3,2), M(3,3), V.Z());
613     //TopLoc_Location Loc(fila);
614     //W.Location(Loc.Multiplied(W.Location()));
615     W = BRepBuilderAPI_Transform(W, fila, Standard_True); //copy
616     ///////////////////////////////////////////
617   }
618   else {
619     W.Nullify();
620 #ifdef OCCT_DEBUG
621     cout << "BRepFill_LocationLaw::D0 : Attention position out of limits" 
622          << endl;
623 #endif
624   }
625 }
626
627 //=======================================================================
628 //function : Abscissa
629 //purpose  : Calculate the abscissa of a point
630 //=======================================================================
631  Standard_Real BRepFill_LocationLaw::Abscissa(const Standard_Integer Index,
632                                               const Standard_Real Param)
633 {
634   GCPnts_AbscissaPoint AbsC;
635   Standard_Real Length = myLength->Value(Index);
636   if (Length < 0) {
637     Standard_Real bid;
638     CurvilinearBounds(Index, bid, Length);
639   }
640  
641   Length += AbsC.Length(myLaws->Value(Index)->GetCurve()->GetCurve(),
642                         myLaws->Value(Index)->GetCurve()->FirstParameter(),
643                         Param, myTol);
644   return Length;
645 }