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