0032402: Coding Rules - eliminate msvc warning C4668 (symbol is not defined as a...
[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 <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 transformation 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(), 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   if (myPath.Closed())
455     return Standard_True;
456
457   TopoDS_Vertex V1, V2;
458   TopExp::Vertices(myPath, V1, V2);
459   return (V1.IsSame(V2));
460 }
461
462 //=======================================================================
463 //function : IsG1
464 //purpose  : Evaluate the continuity of the law by a vertex
465 //=======================================================================
466  Standard_Integer 
467  BRepFill_LocationLaw::IsG1(const Standard_Integer Index,
468                             const Standard_Real SpatialTolerance,
469                             const Standard_Real AngularTolerance) const
470 {
471   gp_Vec V1, DV1, V2, DV2;
472   gp_Mat M1, M2, DM1, DM2;
473   Standard_Real First, Last, EpsNul = 1.e-12;
474   Standard_Real TolEps = SpatialTolerance;
475   Standard_Boolean Ok_D1 = Standard_False;
476   TopoDS_Vertex V;
477   TopoDS_Edge E;
478   TColgp_Array1OfPnt2d Bid1 (1,1);
479   TColgp_Array1OfVec2d Bid2 (1,1);
480   
481   if (Index>0 && Index<myLaws->Length()) {
482     myLaws->Value(Index)->GetDomain(First, Last);
483     Ok_D1 = myLaws->Value(Index)->D1(Last, M1, V1, DM1, DV1, 
484                                      Bid1, Bid2);
485     if (!Ok_D1)  myLaws->Value(Index)->D0(Last, M1, V1);
486
487     myLaws->Value(Index+1)->GetDomain(First, Last);
488     if (Ok_D1)
489       Ok_D1 = myLaws->Value(Index+1)->D1(First, M2, V2, DM2, DV2, 
490                                          Bid1, Bid2);
491     if (!Ok_D1)  myLaws->Value(Index+1)->D0(First, M2, V2);
492
493     E = TopoDS::Edge(myEdges->Value(Index+1));
494   }
495   if (Index == 0 || Index == myLaws->Length()) {
496     if (!myPath.Closed()) return -1;
497     myLaws->Value(myLaws->Length())->GetDomain(First, Last);
498     Ok_D1 = myLaws->Value(myLaws->Length())->D1(Last, M1, V1, DM1, DV1, 
499                                                 Bid1, Bid2);
500     if (!Ok_D1)  myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
501
502     myLaws->Value(1)->GetDomain(First, Last);
503     if (Ok_D1)
504       myLaws->Value(1)->D1(First, M2, V2, DM2, DV2, 
505                            Bid1, Bid2);
506     if (!Ok_D1)  myLaws->Value(1)->D0(First, M2, V2);
507
508     E = TopoDS::Edge(myEdges->Value(1));
509   }
510
511   if (E.Orientation() == TopAbs_REVERSED)
512     V = TopExp::LastVertex(E);
513   else 
514     V = TopExp::FirstVertex(E);
515   
516   TolEps += 2*BRep_Tool::Tolerance(V);
517
518   Standard_Boolean isG0 = Standard_True;
519   Standard_Boolean isG1 = Standard_True;
520   
521   if ((V1-V2).Magnitude() > TolEps) isG0 = Standard_False;
522   if (Norm(M1-M2) > SpatialTolerance) isG0 = Standard_False;
523   
524   if (!isG0) return -1;
525   if (!Ok_D1) return 0; // No control of the derivative
526   
527   if ( (DV1.Magnitude()>EpsNul) && (DV2.Magnitude()>EpsNul)
528        && (DV1.Angle(DV2) > AngularTolerance) ) isG1 = Standard_False;
529
530   // For the next, the tests are mostly empirical
531   Standard_Real Norm1 = Norm(DM1);
532   Standard_Real Norm2 = Norm(DM2);
533   // It two 2 norms are null, it is good
534   if ((Norm1 > EpsNul) || (Norm2 > EpsNul)) {
535     // otherwise the normalized matrices are compared
536     if ((Norm1 > EpsNul) && (Norm2 > EpsNul)) {
537       DM1 /= Norm1;
538       DM2 /= Norm2;
539       if (Norm(DM1 - DM2) > AngularTolerance) isG1 = Standard_False;
540     }
541     else isG1 = Standard_False; // 1 Null the other is not   
542   }
543   
544   if (isG1) return 1;
545   else return 0;
546 }
547
548
549 //=======================================================================
550 //function : Parameter
551 //purpose  : 
552 //=======================================================================
553  void BRepFill_LocationLaw::Parameter(const Standard_Real Abcissa,
554                                       Standard_Integer& Index,
555                                       Standard_Real& U)
556 {
557   Standard_Integer iedge, NbE=myEdges->Length();
558   Standard_Boolean Trouve =  Standard_False;
559
560   //Control that the lengths are calculated
561   if (myLength->Value(NbE+1) < 0) {
562     Standard_Real f, l;
563     CurvilinearBounds(NbE, f, l);
564   }
565
566   // Find the interval
567   for (iedge=1; iedge<=NbE && !Trouve; ) {
568     if (myLength->Value(iedge+1) >= Abcissa) {
569       Trouve = Standard_True;
570     }
571     else iedge++;
572   }
573   
574   if (Trouve) {
575     Standard_Real f, l;
576     const Handle(GeomFill_LocationLaw)& Law =  myLaws->Value(iedge);
577     Law->GetDomain(f, l);
578
579     if  (Abcissa == myLength->Value(iedge+1)) {
580       U = l;
581     }
582     else if  (Abcissa == myLength->Value(iedge)) {
583       U = f;
584     } 
585     else {
586       GCPnts_AbscissaPoint 
587         AbsC(myTol, 
588              *myLaws->Value(iedge)->GetCurve(),
589              Abcissa-myLength->Value(iedge), f);
590       U =  AbsC.Parameter();
591     }
592     Index = iedge;
593   }
594   else {
595     Index = 0;
596   } 
597 }
598
599
600 //===================================================================
601 //function : D0
602 //purpose  : Position of a section, with a given curviline abscissa
603 //===================================================================
604  void BRepFill_LocationLaw::D0(const Standard_Real Abcissa,
605                                TopoDS_Shape& W)
606 {
607   Standard_Real u;
608   Standard_Integer ind;
609   gp_Mat M;
610   gp_Vec V;
611
612   Parameter(Abcissa, ind, u);
613   if (ind != 0) {
614     // Positionement
615     myLaws->Value(ind)->D0(u, M, V);
616     gp_Trsf fila;
617     fila.SetValues(M(1,1), M(1,2), M(1,3), V.X(),
618                    M(2,1), M(2,2), M(2,3), V.Y(),
619                    M(3,1), M(3,2), M(3,3), V.Z());
620     //TopLoc_Location Loc(fila);
621     //W.Location(Loc.Multiplied(W.Location()));
622     W = BRepBuilderAPI_Transform(W, fila, Standard_True); //copy
623     ///////////////////////////////////////////
624   }
625   else {
626     W.Nullify();
627 #ifdef OCCT_DEBUG
628     std::cout << "BRepFill_LocationLaw::D0 : Attention position out of limits" 
629          << std::endl;
630 #endif
631   }
632 }
633
634 //=======================================================================
635 //function : Abscissa
636 //purpose  : Calculate the abscissa of a point
637 //=======================================================================
638  Standard_Real BRepFill_LocationLaw::Abscissa(const Standard_Integer Index,
639                                               const Standard_Real Param)
640 {
641   GCPnts_AbscissaPoint AbsC;
642   Standard_Real Length = myLength->Value(Index);
643   if (Length < 0) {
644     Standard_Real bid;
645     CurvilinearBounds(Index, bid, Length);
646   }
647  
648   Length += AbsC.Length(*myLaws->Value(Index)->GetCurve(),
649                         myLaws->Value(Index)->GetCurve()->FirstParameter(),
650                         Param, myTol);
651   return Length;
652 }