77b38825bfad88d41efe4d6aee72c440ff84bf82
[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 <BRepFill_LocationLaw.ixx>
19
20 #include <BRepTools_WireExplorer.hxx>
21 #include <BRep_Tool.hxx>
22 #include <BRep_Builder.hxx>
23 #include <BRepAdaptor_Curve.hxx>
24 #include <Adaptor3d_HCurve.hxx>
25 #include <TopoDS.hxx>
26 #include <TopoDS_Edge.hxx>
27 #include <TopExp.hxx>
28 #include <TopLoc_Location.hxx>
29
30 #include <GeomFill_LocationLaw.hxx>
31 #include <gp_Vec.hxx>
32 #include <gp_Mat.hxx>
33 #include <gp_XYZ.hxx>
34 #include <gp_Trsf.hxx>
35 #include <GCPnts_AbscissaPoint.hxx>
36 #include <TColgp_Array1OfPnt2d.hxx>
37 #include <TColgp_Array1OfVec2d.hxx>
38 #include <TColStd_SequenceOfInteger.hxx>
39 #include <Precision.hxx>
40 #include <BRepBuilderAPI_Transform.hxx>
41
42
43 //=======================================================================
44 //function : Norm
45 //purpose  : Norm of a Matrix
46 //=======================================================================
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 tranformation 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 #if DEB
182       if (N2.Dot(T1) > 1.e-9) {
183         cout << "Inprecision in TransformInCompatibleLaw" << endl;
184         cout << "--- T1.R(N2) = " << N2.Dot(T1) << endl;
185         gp_Vec tt;
186         tt = T1;
187         tt.Rotate(axe, alpha);
188         cout << "--- T1.R(T2) = " << tt.Dot(T1) << endl;
189         cout << "--- R(N2).R(T2) = " << N2.Dot(tt) << 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()->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   return myPath.Closed();
452 }
453
454 //=======================================================================
455 //function : IsG1
456 //purpose  : Evaluate the continuity of the law by a vertex
457 //=======================================================================
458  Standard_Integer 
459  BRepFill_LocationLaw::IsG1(const Standard_Integer Index,
460                             const Standard_Real SpatialTolerance,
461                             const Standard_Real AngularTolerance) const
462 {
463   gp_Vec V1, DV1, V2, DV2;
464   gp_Mat M1, M2, DM1, DM2;
465   Standard_Real First, Last, EpsNul = 1.e-12;
466   Standard_Real TolEps = SpatialTolerance;
467   Standard_Boolean Ok_D1 = Standard_False;
468   TopoDS_Vertex V;
469   TopoDS_Edge E;
470   TColgp_Array1OfPnt2d Bid1 (1,1);
471   TColgp_Array1OfVec2d Bid2 (1,1);
472   
473   if (Index>0 && Index<myLaws->Length()) {
474     myLaws->Value(Index)->GetDomain(First, Last);
475     Ok_D1 = myLaws->Value(Index)->D1(Last, M1, V1, DM1, DV1, 
476                                      Bid1, Bid2);
477     if (!Ok_D1)  myLaws->Value(Index)->D0(Last, M1, V1);
478
479     myLaws->Value(Index+1)->GetDomain(First, Last);
480     if (Ok_D1)
481       Ok_D1 = myLaws->Value(Index+1)->D1(First, M2, V2, DM2, DV2, 
482                                          Bid1, Bid2);
483     if (!Ok_D1)  myLaws->Value(Index+1)->D0(First, M2, V2);
484
485     E = TopoDS::Edge(myEdges->Value(Index+1));
486   }
487   if (Index == 0 || Index == myLaws->Length()) {
488     if (!myPath.Closed()) return -1;
489     myLaws->Value(myLaws->Length())->GetDomain(First, Last);
490     Ok_D1 = myLaws->Value(myLaws->Length())->D1(Last, M1, V1, DM1, DV1, 
491                                                 Bid1, Bid2);
492     if (!Ok_D1)  myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
493
494     myLaws->Value(1)->GetDomain(First, Last);
495     if (Ok_D1)
496       myLaws->Value(1)->D1(First, M2, V2, DM2, DV2, 
497                            Bid1, Bid2);
498     if (!Ok_D1)  myLaws->Value(1)->D0(First, M2, V2);
499
500     E = TopoDS::Edge(myEdges->Value(1));
501   }
502
503   if (E.Orientation() == TopAbs_REVERSED)
504     V = TopExp::LastVertex(E);
505   else 
506     V = TopExp::FirstVertex(E);
507   
508   TolEps += 2*BRep_Tool::Tolerance(V);
509
510   Standard_Boolean isG0 = Standard_True;
511   Standard_Boolean isG1 = Standard_True;
512   
513   if ((V1-V2).Magnitude() > TolEps) isG0 = Standard_False;
514   if (Norm(M1-M2) > SpatialTolerance) isG0 = Standard_False;
515   
516   if (!isG0) return -1;
517   if (!Ok_D1) return 0; // No control of the derivative
518   
519   if ( (DV1.Magnitude()>EpsNul) && (DV2.Magnitude()>EpsNul)
520        && (DV1.Angle(DV2) > AngularTolerance) ) isG1 = Standard_False;
521
522   // For the next, the tests are mostly empirical
523   Standard_Real Norm1 = Norm(DM1);
524   Standard_Real Norm2 = Norm(DM2);
525   // It two 2 norms are null, it is good
526   if ((Norm1 > EpsNul) || (Norm2 > EpsNul)) {
527     // otherwise the normalized matrices are compared
528     if ((Norm1 > EpsNul) && (Norm2 > EpsNul)) {
529       DM1 /= Norm1;
530       DM2 /= Norm2;
531       if (Norm(DM1 - DM2) > AngularTolerance) isG1 = Standard_False;
532     }
533     else isG1 = Standard_False; // 1 Null the other is not   
534   }
535   
536   if (isG1) return 1;
537   else return 0;
538 }
539
540
541 //=======================================================================
542 //function : Parameter
543 //purpose  : 
544 //=======================================================================
545  void BRepFill_LocationLaw::Parameter(const Standard_Real Abcissa,
546                                       Standard_Integer& Index,
547                                       Standard_Real& U)
548 {
549   Standard_Integer iedge, NbE=myEdges->Length();
550   Standard_Boolean Trouve =  Standard_False;
551
552   //Control that the lengths are calculated
553   if (myLength->Value(NbE+1) < 0) {
554     Standard_Real f, l;
555     CurvilinearBounds(NbE, f, l);
556   }
557
558   // Find the interval
559   for (iedge=1; iedge<=NbE && !Trouve; ) {
560     if (myLength->Value(iedge+1) >= Abcissa) {
561       Trouve = Standard_True;
562     }
563     else iedge++;
564   }
565   
566   if (Trouve) {
567     Standard_Real f, l;
568     const Handle(GeomFill_LocationLaw)& Law =  myLaws->Value(iedge);
569     Law->GetDomain(f, l);
570
571     if  (Abcissa == myLength->Value(iedge+1)) {
572       U = l;
573     }
574     else if  (Abcissa == myLength->Value(iedge)) {
575       U = f;
576     } 
577     else {
578       GCPnts_AbscissaPoint 
579         AbsC(myTol, 
580              myLaws->Value(iedge)->GetCurve()->GetCurve(), 
581              Abcissa-myLength->Value(iedge), f);
582       U =  AbsC.Parameter();
583     }
584     Index = iedge;
585   }
586   else {
587     Index = 0;
588   } 
589 }
590
591
592 //===================================================================
593 //function : D0
594 //purpose  : Position of a section, with a given curviline abscissa
595 //===================================================================
596  void BRepFill_LocationLaw::D0(const Standard_Real Abcissa,
597                                TopoDS_Shape& W)
598 {
599   Standard_Real u;
600   Standard_Integer ind;
601   gp_Mat M;
602   gp_Vec V;
603
604   Parameter(Abcissa, ind, u);
605   if (ind != 0) {
606     // Positionement
607     myLaws->Value(ind)->D0(u, M, V);
608     gp_Trsf fila;
609     fila.SetValues(M(1,1), M(1,2), M(1,3), V.X(),
610                    M(2,1), M(2,2), M(2,3), V.Y(),
611                    M(3,1), M(3,2), M(3,3), V.Z(),
612                    1.e-12, 1.e-14);
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 #if DEB    
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 }