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