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