0031007: Coding - eliminate warnings issued while compiling with -pedantic flag
[occt.git] / src / BRepBlend / BRepBlend_SurfRstConstRad.cxx
1 // Created on: 1997-02-10
2 // Created by: Jacques GOUSSARD
3 // Copyright (c) 1997-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 <Adaptor2d_HCurve2d.hxx>
19 #include <Adaptor3d_HCurve.hxx>
20 #include <Adaptor3d_HSurface.hxx>
21 #include <Blend_Point.hxx>
22 #include <BlendFunc.hxx>
23 #include <BRepBlend_SurfRstConstRad.hxx>
24 #include <ElCLib.hxx>
25 #include <GeomFill.hxx>
26 #include <gp.hxx>
27 #include <gp_Circ.hxx>
28 #include <gp_Pnt.hxx>
29 #include <gp_Pnt2d.hxx>
30 #include <gp_Vec.hxx>
31 #include <gp_Vec2d.hxx>
32 #include <math_Gauss.hxx>
33 #include <math_Matrix.hxx>
34 #include <math_SVD.hxx>
35 #include <Precision.hxx>
36 #include <Standard_DomainError.hxx>
37 #include <Standard_NotImplemented.hxx>
38
39 #define Eps 1.e-15
40
41 static void t3dto2d(Standard_Real& a,
42                     Standard_Real& b,
43                     const gp_Vec& A,
44                     const gp_Vec& B,
45                     const gp_Vec& C)
46 {
47   Standard_Real AB = A.Dot(B);
48   Standard_Real AC = A.Dot(C);
49   Standard_Real BC = B.Dot(C);
50   Standard_Real BB = B.Dot(B);
51   Standard_Real CC = C.Dot(C);
52   Standard_Real deno = (BB*CC-BC*BC);
53   a = (AB*CC-AC*BC)/deno;
54   b = (AC*BB-AB*BC)/deno;
55 }
56
57 //=======================================================================
58 //function : BRepBlend_SurfRstConstRad
59 //purpose  : 
60 //=======================================================================
61
62 BRepBlend_SurfRstConstRad::BRepBlend_SurfRstConstRad
63 (const Handle(Adaptor3d_HSurface)& Surf,
64  const Handle(Adaptor3d_HSurface)& SurfRst,
65  const Handle(Adaptor2d_HCurve2d)& Rst,
66  const Handle(Adaptor3d_HCurve)&   CGuide):
67  surf(Surf), surfrst(SurfRst), rst(Rst), cons(Rst,SurfRst), 
68  guide(CGuide), tguide(CGuide),
69  istangent(Standard_True), theD(0.), maxang(RealFirst()), minang(RealLast()),
70  distmin(RealLast()),
71  mySShape(BlendFunc_Rational)
72 {}
73
74 //=======================================================================
75 //function : NbVariables
76 //purpose  : 
77 //=======================================================================
78
79 Standard_Integer BRepBlend_SurfRstConstRad::NbVariables() const
80 {
81   return 3;
82 }
83
84 //=======================================================================
85 //function : NbEquations
86 //purpose  : 
87 //=======================================================================
88
89 Standard_Integer BRepBlend_SurfRstConstRad::NbEquations() const
90 {
91   return 3;
92 }
93
94 //=======================================================================
95 //function : Value
96 //purpose  : 
97 //=======================================================================
98
99 Standard_Boolean BRepBlend_SurfRstConstRad::Value(const math_Vector& X,
100                                                   math_Vector&       F)
101 {
102   gp_Vec d1u1,d1v1,ns,vref;
103   Standard_Real norm;
104   
105   surf->D1(X(1),X(2),pts,d1u1,d1v1);
106   ptrst = cons.Value(X(3));
107   
108   F(1) = nplan.XYZ().Dot(pts.XYZ()) + theD;
109   F(2) = nplan.XYZ().Dot(ptrst.XYZ()) + theD;
110   
111   ns = d1u1.Crossed(d1v1);
112   norm = nplan.Crossed(ns).Magnitude();
113   ns.SetLinearForm(nplan.Dot(ns)/norm,nplan, -1./norm,ns);
114   vref.SetLinearForm(ray,ns,gp_Vec(ptrst,pts));
115   vref /= ray;
116   F(3) = (vref.SquareMagnitude() - 1)*ray*ray;
117   return Standard_True;
118 }
119
120 //=======================================================================
121 //function : Derivatives
122 //purpose  : 
123 //=======================================================================
124
125 Standard_Boolean BRepBlend_SurfRstConstRad::Derivatives(const math_Vector& X,
126                                                         math_Matrix&       D)
127 {
128   gp_Vec d1u1,d1v1,d2u1,d2v1,d2uv1,d1;
129   gp_Vec ns,ncrossns,resul,temp, vref;
130   
131   Standard_Real norm,ndotns,grosterme;
132   
133   surf->D2(X(1),X(2),pts,d1u1,d1v1,d2u1,d2v1,d2uv1);
134   cons.D1(X(3),ptrst,d1);
135   
136   D(1,1) = nplan.Dot(d1u1);
137   D(1,2) = nplan.Dot(d1v1);
138   D(1,3) = 0.;
139   
140   D(2,1) = 0.;
141   D(2,2) = 0.;
142   D(2,3) = nplan.Dot(d1);
143   
144   
145   ns = d1u1.Crossed(d1v1);
146   ncrossns = nplan.Crossed(ns);
147   norm = ncrossns.Magnitude();
148   ndotns = nplan.Dot(ns);
149   
150   vref.SetLinearForm(ndotns,nplan,-1.,ns);
151   vref.Divide(norm);
152   vref.SetLinearForm(ray,vref,gp_Vec(ptrst,pts));
153   
154   // Derivative by u1
155   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
156   grosterme = ncrossns.Dot(nplan.Crossed(temp))/norm/norm;
157   resul.SetLinearForm(-ray/norm*(grosterme*ndotns-nplan.Dot(temp)),nplan,
158                       ray*grosterme/norm,ns,
159                       -ray/norm,temp,
160                       d1u1);
161   
162   D(3,1) = resul.Dot(vref);
163   D(3,1) = D(3,1) * 2.;
164   
165   
166   // Derivative by v1
167   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
168   grosterme = ncrossns.Dot(nplan.Crossed(temp))/norm/norm;
169   resul.SetLinearForm(-ray/norm*(grosterme*ndotns-nplan.Dot(temp)),nplan,
170                       ray*grosterme/norm,ns,
171                       -ray/norm,temp,
172                       d1v1);
173   
174   D(3,2) = resul.Dot(vref);
175   D(3,2) =  D(3,2) * 2.;
176   
177   D(3,3) = d1.Dot(vref);
178   D(3,3) = D(3,3) * (-2.);
179   
180   return Standard_True;
181 }
182
183 //=======================================================================
184 //function : Values
185 //purpose  : 
186 //=======================================================================
187
188 Standard_Boolean BRepBlend_SurfRstConstRad::Values(const math_Vector& X,
189                                                    math_Vector&       F,
190                                                    math_Matrix&       D)
191 {
192   gp_Vec d1u1,d1v1,d1;
193   gp_Vec d2u1,d2v1,d2uv1;
194   gp_Vec ns,ncrossns,resul,temp,vref;
195   
196   Standard_Real norm,ndotns,grosterme;
197
198   surf->D2(X(1),X(2),pts,d1u1,d1v1,d2u1,d2v1,d2uv1);
199   cons.D1(X(3),ptrst,d1);
200   
201   F(1) = nplan.XYZ().Dot(pts.XYZ()) + theD;
202   F(2) = nplan.XYZ().Dot(ptrst.XYZ()) + theD;
203   
204   D(1,1) = nplan.Dot(d1u1);
205   D(1,2) = nplan.Dot(d1v1);
206   D(1,3) = 0.;
207   
208   D(2,1) = 0.;
209   D(2,2) = 0.;
210   D(2,3) = nplan.Dot(d1);
211   
212   ns = d1u1.Crossed(d1v1);
213   ncrossns = nplan.Crossed(ns);
214   norm = ncrossns.Magnitude();
215   ndotns = nplan.Dot(ns);
216   
217   vref.SetLinearForm(ndotns,nplan,-1.,ns);
218   vref.Divide(norm);
219   vref.SetLinearForm(ray,vref,gp_Vec(ptrst,pts));
220   
221   temp = vref/ray;
222 //  F(3) = vref.SquareMagnitude() - ray*ray;
223   F(3) = (temp.SquareMagnitude() - 1)*ray*ray; // more stable numerically
224   
225   // Derivative by u1
226   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
227   grosterme = ncrossns.Dot(nplan.Crossed(temp))/norm/norm;
228   resul.SetLinearForm(-ray/norm*(grosterme*ndotns-nplan.Dot(temp)),nplan,
229                       ray*grosterme/norm,ns,
230                       -ray/norm,temp,
231                       d1u1);
232   
233   D(3,1) = resul.Dot(vref);
234   D(3,1) = D(3,1)*2.;
235
236   
237   
238   // Derivative by v1
239   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
240   grosterme = ncrossns.Dot(nplan.Crossed(temp))/norm/norm;
241   resul.SetLinearForm(-ray/norm*(grosterme*ndotns-nplan.Dot(temp)),nplan,
242                       ray*grosterme/norm,ns,
243                       -ray/norm,temp,
244                       d1v1);
245   
246   D(3,2) = resul.Dot(vref);
247   D(3,2) = D(3,2)*2.;
248
249   D(3,3) = d1.Dot(vref);
250   D(3,3) = D(3,3)*(-2.);
251
252   return Standard_True;
253 }
254
255 //=======================================================================
256 //function : Set
257 //purpose  : 
258 //=======================================================================
259
260 void BRepBlend_SurfRstConstRad::Set(const Handle(Adaptor3d_HSurface)& SurfRef,
261                                     const Handle(Adaptor2d_HCurve2d)& RstRef)
262 {
263   surfref = SurfRef;
264   rstref = RstRef;
265 }
266
267 //=======================================================================
268 //function : Set
269 //purpose  : 
270 //=======================================================================
271
272 void BRepBlend_SurfRstConstRad::Set(const Standard_Real Param)
273 {
274   d1gui = gp_Vec(0.,0.,0.);
275   nplan = gp_Vec(0.,0.,0.);
276   tguide->D2(Param,ptgui,d1gui,d2gui);
277   normtg = d1gui.Magnitude();
278   nplan.SetXYZ(d1gui.Normalized().XYZ());
279   gp_XYZ nplanXYZ(nplan.XYZ());
280   gp_XYZ ptguiXYZ(ptgui.XYZ());
281   theD =  nplanXYZ.Dot(ptguiXYZ)  ;
282   theD = theD  * (-1.) ;
283 }
284
285 //=======================================================================
286 //function : Set
287 //purpose  : 
288 //=======================================================================
289
290 void BRepBlend_SurfRstConstRad::Set(const Standard_Real First,
291                                     const Standard_Real Last)
292
293  tguide = guide->Trim(First, Last, 1.e-12);
294 }
295
296 //=======================================================================
297 //function : GetTolerance
298 //purpose  : 
299 //=======================================================================
300
301 void BRepBlend_SurfRstConstRad::GetTolerance(math_Vector&        Tolerance,
302                                              const Standard_Real Tol) const
303 {
304   Tolerance(1) = surf->UResolution(Tol);
305   Tolerance(2) = surf->VResolution(Tol);
306   Tolerance(3) = cons.Resolution(Tol);
307 }
308
309 //=======================================================================
310 //function : GetBounds
311 //purpose  : 
312 //=======================================================================
313
314 void BRepBlend_SurfRstConstRad::GetBounds(math_Vector& InfBound,
315                                           math_Vector& SupBound) const
316 {
317   InfBound(1) = surf->FirstUParameter();
318   InfBound(2) = surf->FirstVParameter();
319   InfBound(3) = cons.FirstParameter();
320   SupBound(1) = surf->LastUParameter();
321   SupBound(2) = surf->LastVParameter();
322   SupBound(3) = cons.LastParameter();
323   
324   if(!Precision::IsInfinite(InfBound(1)) &&
325      !Precision::IsInfinite(SupBound(1))) {
326     Standard_Real range = (SupBound(1) - InfBound(1));
327     InfBound(1) -= range;
328     SupBound(1) += range;
329   }
330   if(!Precision::IsInfinite(InfBound(2)) &&
331      !Precision::IsInfinite(SupBound(2))) {
332     Standard_Real range = (SupBound(2) - InfBound(2));
333     InfBound(2) -= range;
334     SupBound(2) += range;
335   }
336 }
337
338 //=======================================================================
339 //function : IsSolution
340 //purpose  : 
341 //=======================================================================
342
343 Standard_Boolean BRepBlend_SurfRstConstRad::IsSolution(const math_Vector&  Sol,
344                                                        const Standard_Real Tol)
345      
346      
347 {
348   math_Vector valsol(1,3),secmember(1,3);
349   math_Matrix gradsol(1,3,1,3);
350   
351   gp_Vec dnplan,d1u1,d1v1,d1urst,d1vrst,d1,temp,ns,ns2,ncrossns,resul;
352   gp_Pnt bid;
353   Standard_Real norm,ndotns,grosterme;
354   Standard_Real Cosa,Sina,Angle;
355   
356   Values(Sol,valsol,gradsol);
357   if (Abs(valsol(1)) <= Tol &&
358       Abs(valsol(2)) <= Tol &&
359       Abs(valsol(3)) <= 2*Tol*Abs(ray) ) {
360     
361     // Calculation of tangents
362     
363     pt2ds  = gp_Pnt2d(Sol(1),Sol(2));
364     prmrst = Sol(3);
365     pt2drst = rst->Value(prmrst);
366     surf->D1(Sol(1),Sol(2),pts,d1u1,d1v1);
367     cons.D1(Sol(3),ptrst,d1);
368     dnplan.SetLinearForm(1./normtg,d2gui,
369                          -1./normtg*(nplan.Dot(d2gui)),nplan);
370     
371     temp.SetXYZ(pts.XYZ() - ptgui.XYZ());
372     secmember(1) = normtg - dnplan.Dot(temp);
373     
374     temp.SetXYZ(ptrst.XYZ() - ptgui.XYZ());
375     secmember(2) = normtg - dnplan.Dot(temp);
376     
377     ns = d1u1.Crossed(d1v1);
378     ncrossns = nplan.Crossed(ns);
379     ndotns = nplan.Dot(ns);
380     norm = ncrossns.Magnitude();
381     
382     grosterme = ncrossns.Dot(dnplan.Crossed(ns))/norm/norm;
383     temp.SetLinearForm(ray/norm*(dnplan.Dot(ns)-grosterme*ndotns),nplan,
384                        ray*ndotns/norm,dnplan,
385                        ray*grosterme/norm,ns);
386     
387     ns.SetLinearForm(ndotns/norm,nplan, -1./norm,ns);
388     resul.SetLinearForm(ray,ns,gp_Vec(ptrst,pts));
389     secmember(3) = -2.*(temp.Dot(resul));
390     
391     math_Gauss Resol(gradsol);
392     if (Resol.IsDone()) {    
393       Resol.Solve(secmember);
394       istangent = Standard_False;
395     }
396   else {
397     math_SVD SingRS (gradsol);
398     if (SingRS.IsDone()) {
399       math_Vector DEDT(1,3);
400       DEDT = secmember;
401       SingRS.Solve(DEDT, secmember, 1.e-6);
402        istangent = Standard_False;
403     }
404     else istangent = Standard_True;
405   }
406
407     if (!istangent) {      
408       tgs.SetLinearForm(secmember(1),d1u1,secmember(2),d1v1);
409       tgrst = secmember(3)*d1;
410       tg2ds.SetCoord(secmember(1),secmember(2));
411       surfrst->D1(pt2drst.X(),pt2drst.Y(),bid,d1urst,d1vrst);
412       Standard_Real a, b;
413       t3dto2d(a,b,tgrst,d1urst,d1vrst);
414       tg2drst.SetCoord(a,b);
415     }
416
417     // update of maxang
418     if(ray>0.) ns.Reverse();
419     ns2 = -resul.Normalized();
420     
421     Cosa = ns.Dot(ns2);
422     Sina = nplan.Dot(ns.Crossed(ns2));
423     if (choix%2 != 0) {
424       Sina = -Sina;  //nplan is changed to -nplan
425     }
426     
427     Angle = ACos(Cosa);
428     if (Sina <0.) {
429       Angle = 2.*M_PI - Angle;
430     }
431     
432     if (Angle>maxang) {maxang = Angle;}
433     if (Angle<minang) {minang = Angle;}
434     distmin = Min( distmin, pts.Distance(ptrst));
435
436     return Standard_True;
437   }
438   istangent = Standard_True;
439   return Standard_False;
440 }
441
442 //=======================================================================
443 //function : GetMinimalDistance
444 //purpose  : 
445 //=======================================================================
446
447 Standard_Real BRepBlend_SurfRstConstRad::GetMinimalDistance() const
448 {
449   return distmin;
450 }
451
452 //=======================================================================
453 //function : PointOnS
454 //purpose  : 
455 //=======================================================================
456
457 const gp_Pnt& BRepBlend_SurfRstConstRad::PointOnS() const
458 {
459   return pts;
460 }
461
462 //=======================================================================
463 //function : PointOnRst
464 //purpose  : 
465 //=======================================================================
466
467 const gp_Pnt& BRepBlend_SurfRstConstRad::PointOnRst() const
468 {
469   return ptrst ;
470 }
471
472 //=======================================================================
473 //function : Pnt2dOnS
474 //purpose  : 
475 //=======================================================================
476
477 const gp_Pnt2d& BRepBlend_SurfRstConstRad::Pnt2dOnS() const
478 {
479   return pt2ds;
480 }
481
482 //=======================================================================
483 //function : Pnt2dOnRst
484 //purpose  : 
485 //=======================================================================
486
487 const gp_Pnt2d& BRepBlend_SurfRstConstRad::Pnt2dOnRst() const
488 {
489   return pt2drst;
490 }
491
492 //=======================================================================
493 //function : ParameterOnRst
494 //purpose  : 
495 //=======================================================================
496
497 Standard_Real BRepBlend_SurfRstConstRad::ParameterOnRst() const
498 {
499   return prmrst;
500 }
501
502 //=======================================================================
503 //function : IsTangencyPoint
504 //purpose  : 
505 //=======================================================================
506
507 Standard_Boolean BRepBlend_SurfRstConstRad::IsTangencyPoint() const
508 {
509   return istangent;
510 }
511
512 //=======================================================================
513 //function : TangentOnS
514 //purpose  : 
515 //=======================================================================
516
517 const gp_Vec& BRepBlend_SurfRstConstRad::TangentOnS() const
518 {
519   if (istangent) {throw Standard_DomainError();}
520   return tgs;
521 }
522
523 //=======================================================================
524 //function : Tangent2dOnS
525 //purpose  : 
526 //=======================================================================
527
528 const gp_Vec2d& BRepBlend_SurfRstConstRad::Tangent2dOnS() const
529 {
530   if (istangent) {throw Standard_DomainError();}
531   return tg2ds;
532 }
533
534 //=======================================================================
535 //function : TangentOnRst
536 //purpose  : 
537 //=======================================================================
538
539 const gp_Vec& BRepBlend_SurfRstConstRad::TangentOnRst() const
540 {
541   if (istangent) {throw Standard_DomainError();}
542   return tgrst;
543 }
544
545 //=======================================================================
546 //function : Tangent2dOnRst
547 //purpose  : 
548 //=======================================================================
549
550 const gp_Vec2d& BRepBlend_SurfRstConstRad::Tangent2dOnRst() const
551 {
552   if (istangent) {throw Standard_DomainError();}
553   return tg2drst;
554 }
555
556 //=======================================================================
557 //function : Decroch
558 //purpose  : 
559 //=======================================================================
560
561 Standard_Boolean BRepBlend_SurfRstConstRad::Decroch(const math_Vector& Sol,
562                                                     gp_Vec&            NS,
563                                                     gp_Vec&            TgS)const
564 {
565   gp_Vec TgRst, NRst, NRstInPlane, NSInPlane;
566   gp_Pnt bid,Center;
567   gp_Vec d1u,d1v;
568   Standard_Real norm,unsurnorm;
569   
570   surf->D1(Sol(1),Sol(2),bid,d1u,d1v);
571   NS = NSInPlane = d1u.Crossed(d1v);
572   
573   norm = nplan.Crossed(NS).Magnitude();
574   unsurnorm = 1./norm;
575   NSInPlane.SetLinearForm(nplan.Dot(NS)*unsurnorm,nplan,-unsurnorm,NS);
576   
577   Center.SetXYZ(bid.XYZ()+ray*NSInPlane.XYZ());
578   if(choix>2) NSInPlane.Reverse();
579   TgS = nplan.Crossed(gp_Vec(Center,bid));
580   if (choix%2 == 1) {
581     TgS.Reverse();
582   }
583   Standard_Real u,v;
584   rstref->Value(Sol(3)).Coord(u,v);
585   surfref->D1(u,v,bid,d1u,d1v);
586   NRst = d1u.Crossed(d1v);
587   norm = nplan.Crossed(NRst).Magnitude();
588   unsurnorm = 1./norm;
589   NRstInPlane.SetLinearForm(nplan.Dot(NRst)*unsurnorm,nplan,-unsurnorm,NRst);
590   gp_Vec centptrst(Center,bid);
591   if(centptrst.Dot(NRstInPlane) < 0.) NRstInPlane.Reverse();
592   TgRst = nplan.Crossed(centptrst);
593   if (choix%2 == 1) {
594     TgRst.Reverse();
595   }
596   
597   Standard_Real dot, NT = NRstInPlane.Magnitude();
598   NT *= TgRst.Magnitude();
599   if (Abs(NT) < 1.e-7) {
600     return Standard_False; // Singularity or Incoherence.
601   }
602   dot = NRstInPlane.Dot(TgRst);
603   dot /= NT;
604          
605   return (dot < 1.e-10);
606 }
607
608 //=======================================================================
609 //function : Set
610 //purpose  : 
611 //=======================================================================
612
613 void BRepBlend_SurfRstConstRad::Set(const Standard_Real    Radius,
614                                     const Standard_Integer Choix)
615 {
616   choix = Choix;
617   switch (choix) {
618   case 1 :
619   case 2 :
620     ray = -Abs(Radius);
621     break;
622   case 3 :
623   case 4 :
624     ray = Abs(Radius);
625     break;
626   default :
627     ray = -Abs(Radius);
628     break;
629   }
630 }
631
632 //=======================================================================
633 //function : Set
634 //purpose  : 
635 //=======================================================================
636
637 void BRepBlend_SurfRstConstRad::Set(const BlendFunc_SectionShape TypeSection)
638 {
639   mySShape = TypeSection;
640 }
641
642 //=======================================================================
643 //function : Section
644 //purpose  : 
645 //=======================================================================
646
647 void BRepBlend_SurfRstConstRad::Section(const Standard_Real Param,
648                                         const Standard_Real U,
649                                         const Standard_Real V,
650                                         const Standard_Real W,
651                                         Standard_Real& Pdeb,
652                                         Standard_Real& Pfin,
653                                         gp_Circ& C)
654 {
655   gp_Vec d1u1,d1v1;
656   gp_Vec ns, np;
657   Standard_Real norm;
658   gp_Pnt Center;
659   
660   tguide->D1(Param,ptgui,d1gui);
661   np  = d1gui.Normalized();
662   
663   surf->D1(U,V,pts,d1u1,d1v1);
664   ptrst = cons.Value(W);
665   
666   ns = d1u1.Crossed(d1v1);
667   
668   norm = nplan.Crossed(ns).Magnitude();
669   ns.SetLinearForm(nplan.Dot(ns)/norm,nplan, -1./norm,ns);
670   Center.SetXYZ(pts.XYZ()+ray*ns.XYZ());
671   C.SetRadius(Abs(ray));
672
673   if (ray > 0) {
674     ns.Reverse();
675   }
676   
677   if (choix%2 != 0) {
678     np.Reverse();
679   }
680
681   C.SetPosition(gp_Ax2(Center,np,ns));
682   Pdeb = 0; //ElCLib::Parameter(C,pts);
683   Pfin = ElCLib::Parameter(C,ptrst);
684
685   // Test negative and almost null angles : Special case
686   if (Pfin>1.5*M_PI) {
687     np.Reverse();
688     C.SetPosition(gp_Ax2(Center,np,ns));
689     Pfin = ElCLib::Parameter(C,ptrst);
690   }
691   if (Pfin < Precision::PConfusion()) Pfin += Precision::PConfusion();
692 }
693
694 //=======================================================================
695 //function : IsRational
696 //purpose  : 
697 //=======================================================================
698
699 Standard_Boolean BRepBlend_SurfRstConstRad::IsRational () const
700 {
701   return  (mySShape==BlendFunc_Rational || mySShape==BlendFunc_QuasiAngular);
702 }
703
704 //=======================================================================
705 //function : GetSectionSize
706 //purpose  :
707 //=======================================================================
708
709 Standard_Real BRepBlend_SurfRstConstRad::GetSectionSize() const 
710 {
711   return maxang*Abs(ray);
712 }
713
714 //=======================================================================
715 //function : GetMinimalWeight
716 //purpose  : 
717 //=======================================================================
718
719 void BRepBlend_SurfRstConstRad::GetMinimalWeight(TColStd_Array1OfReal& Weights) const 
720 {
721   BlendFunc::GetMinimalWeights(mySShape, myTConv, minang, maxang, Weights );
722   // It is supposed that it does not depend on the Radius! 
723 }
724
725 //=======================================================================
726 //function : NbIntervals
727 //purpose  : 
728 //=======================================================================
729
730 Standard_Integer BRepBlend_SurfRstConstRad::NbIntervals (const GeomAbs_Shape S) const
731 {
732   return guide->NbIntervals(BlendFunc::NextShape(S));
733 }
734
735 //=======================================================================
736 //function : Intervals
737 //purpose  : 
738 //=======================================================================
739
740 void BRepBlend_SurfRstConstRad::Intervals (TColStd_Array1OfReal& T,
741                                            const GeomAbs_Shape S) const
742 {
743   guide->Intervals(T, BlendFunc::NextShape(S));
744 }
745
746 //=======================================================================
747 //function : GetShape
748 //purpose  : 
749 //=======================================================================
750
751 void BRepBlend_SurfRstConstRad::GetShape (Standard_Integer& NbPoles,
752                                           Standard_Integer& NbKnots,
753                                           Standard_Integer& Degree,
754                                           Standard_Integer& NbPoles2d)
755 {
756   NbPoles2d = 2;
757   BlendFunc::GetShape(mySShape,maxang,NbPoles,NbKnots,Degree,myTConv);
758 }
759
760 //=======================================================================
761 //function : GetTolerance
762 //purpose  : Find Tolerance to be used in approximations.
763 //=======================================================================
764
765 void BRepBlend_SurfRstConstRad::GetTolerance(const Standard_Real BoundTol, 
766                                              const Standard_Real SurfTol, 
767                                              const Standard_Real AngleTol, 
768                                              math_Vector& Tol3d, 
769                                              math_Vector& Tol1d) const
770 {
771   Standard_Integer low = Tol3d.Lower() , up=Tol3d.Upper();
772   Standard_Real Tol;
773   Tol= GeomFill::GetTolerance(myTConv, minang, Abs(ray), 
774                                AngleTol, SurfTol);
775   Tol1d.Init(SurfTol);
776   Tol3d.Init(SurfTol);
777   Tol3d(low+1) = Tol3d(up-1) = Min( Tol, SurfTol);
778   Tol3d(low) = Tol3d(up) = Min( Tol, BoundTol);
779 }
780
781 //=======================================================================
782 //function : Knots
783 //purpose  : 
784 //=======================================================================
785
786 void BRepBlend_SurfRstConstRad::Knots(TColStd_Array1OfReal& TKnots)
787 {
788   GeomFill::Knots(myTConv,TKnots);
789 }
790
791 //=======================================================================
792 //function : Mults
793 //purpose  : 
794 //=======================================================================
795
796 void BRepBlend_SurfRstConstRad::Mults(TColStd_Array1OfInteger& TMults)
797 {
798   GeomFill::Mults(myTConv, TMults);
799 }
800
801 //=======================================================================
802 //function : Section
803 //purpose  : 
804 //=======================================================================
805
806 void BRepBlend_SurfRstConstRad::Section(const Blend_Point& P,
807                                         TColgp_Array1OfPnt& Poles,
808                                         TColgp_Array1OfPnt2d& Poles2d,
809                                         TColStd_Array1OfReal& Weights)
810 {
811   gp_Vec d1u1,d1v1;//,,d1;
812   gp_Vec ns,ns2;//,temp,np2;
813   gp_Pnt Center;
814   
815   Standard_Real norm,u1,v1,w;
816   
817   Standard_Real prm = P.Parameter();
818   Standard_Integer low = Poles.Lower();
819   Standard_Integer upp = Poles.Upper();
820   
821   tguide->D1(prm,ptgui,d1gui);
822   nplan  = d1gui.Normalized();
823   
824   P.ParametersOnS(u1,v1);
825   w = P.ParameterOnC(); //jlr : point on curve not on surface
826   gp_Pnt2d  pt2d = rst->Value(w);
827
828   surf->D1(u1,v1,pts,d1u1,d1v1);
829   ptrst = cons.Value(w);
830   distmin = Min (distmin, pts.Distance(ptrst)); 
831
832   Poles2d(Poles2d.Lower()).SetCoord(u1,v1);
833   Poles2d(Poles2d.Upper()).SetCoord(pt2d.X(),pt2d.Y());
834   
835   // Linear Case
836   if (mySShape == BlendFunc_Linear) {
837     Poles(low) = pts;
838     Poles(upp) = ptrst;
839     Weights(low) = 1.0;
840     Weights(upp) = 1.0;
841     return;
842   }
843   
844   ns = d1u1.Crossed(d1v1);
845   norm = nplan.Crossed(ns).Magnitude();
846   
847   ns.SetLinearForm(nplan.Dot(ns)/norm,nplan, -1./norm,ns);
848   
849   Center.SetXYZ(pts.XYZ()+ray*ns.XYZ());
850   
851   ns2 = gp_Vec(Center,ptrst).Normalized();
852   if(ray>0) ns.Reverse();
853   if (choix%2 != 0) {
854     nplan.Reverse();
855   }
856   
857   GeomFill::GetCircle(myTConv,
858                      ns, ns2, 
859                      nplan, pts, ptrst,
860                      Abs(ray), Center, 
861                      Poles, Weights);
862 }
863
864 //=======================================================================
865 //function : Section
866 //purpose  : 
867 //=======================================================================
868
869 Standard_Boolean BRepBlend_SurfRstConstRad::Section
870 (const Blend_Point& P,
871  TColgp_Array1OfPnt& Poles,
872  TColgp_Array1OfVec& DPoles,
873  TColgp_Array1OfPnt2d& Poles2d,
874  TColgp_Array1OfVec2d& DPoles2d,
875  TColStd_Array1OfReal& Weights,
876  TColStd_Array1OfReal& DWeights)
877 {
878   
879   gp_Vec d1u1,d1v1,d2u1,d2v1,d2uv1,d1;
880   gp_Vec ns,ns2,dnplan,dnw,dn2w; //,np2,dnp2;
881   gp_Vec ncrossns;
882   gp_Vec resulu,resulv,temp,tgct,resul;
883   gp_Vec d1urst,d1vrst;
884   gp_Pnt Center,bid;
885   
886   Standard_Real norm,ndotns,grosterme;
887   
888   math_Vector sol(1,3),valsol(1,3),secmember(1,3);
889   math_Matrix gradsol(1,3,1,3);
890   
891   Standard_Real prm = P.Parameter();
892   Standard_Integer low = Poles.Lower();
893   Standard_Integer upp = Poles.Upper();
894   Standard_Boolean istgt;
895   
896   tguide->D2(prm,ptgui,d1gui,d2gui);
897   normtg = d1gui.Magnitude();
898   nplan  = d1gui.Normalized();
899   dnplan.SetLinearForm(1./normtg,d2gui,
900                        -1./normtg*(nplan.Dot(d2gui)),nplan);
901   
902   P.ParametersOnS(sol(1),sol(2));
903   sol(3) = prmrst = P.ParameterOnC();
904   pt2drst = rst->Value(prmrst);
905   
906   Values(sol,valsol,gradsol);
907   
908   surf->D2(sol(1),sol(2),pts,d1u1,d1v1,d2u1,d2v1,d2uv1);
909   cons.D1(sol(3),ptrst,d1);
910   
911   temp.SetXYZ(pts.XYZ()- ptgui.XYZ());
912   secmember(1) = normtg - dnplan.Dot(temp);
913   
914   temp.SetXYZ(ptrst.XYZ()- ptgui.XYZ());
915   secmember(2) = normtg - dnplan.Dot(temp);
916   
917   ns = d1u1.Crossed(d1v1);
918   ncrossns = nplan.Crossed(ns);
919   ndotns = nplan.Dot(ns);
920   norm = ncrossns.Magnitude();
921   if (norm < Eps)  {
922     norm = 1; // Not enough, but it is not necessary to stop
923 #ifdef OCCT_DEBUG
924     std::cout << " SurfRstConstRad : Singular Surface " << std::endl;
925 #endif
926   }  
927   
928   // Derivative of n1 corresponding to w
929   
930   grosterme = ncrossns.Dot(dnplan.Crossed(ns))/norm/norm;
931   dnw.SetLinearForm((dnplan.Dot(ns)-grosterme*ndotns)/norm,nplan,
932                     ndotns/norm,dnplan,
933                     grosterme/norm,ns);
934   
935   temp.SetLinearForm(ndotns/norm,nplan, -1./norm,ns);
936   resul.SetLinearForm(ray,temp,gp_Vec(ptrst,pts));
937   secmember(3) = dnw.Dot(resul); 
938   secmember(3) = -2.*ray*secmember(3);
939   
940   math_Gauss Resol(gradsol, 1.e-9);
941
942   
943   if (Resol.IsDone()) {
944     istgt = Standard_False;
945     Resol.Solve(secmember);
946   }
947   else {
948     math_SVD SingRS (gradsol);
949     if (SingRS.IsDone()) {
950       math_Vector DEDT(1,3);
951       DEDT = secmember;
952       SingRS.Solve(DEDT, secmember, 1.e-6);
953       istgt = Standard_False;
954     }
955     else istgt = Standard_True;
956   }
957
958   if (!istgt) {  
959     tgs.SetLinearForm(secmember(1),d1u1,secmember(2),d1v1);
960     tgrst = secmember(3)*d1;
961     
962     // Derivative of n1 corresponding to u1
963     temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
964     grosterme = ncrossns.Dot(nplan.Crossed(temp))/norm/norm;
965     resulu.SetLinearForm(-(grosterme*ndotns-nplan.Dot(temp))/norm,nplan,
966                          grosterme/norm,ns,
967                          -1./norm,temp);
968     
969     // Derivative of n1 corresponding to v1
970     temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
971     grosterme = ncrossns.Dot(nplan.Crossed(temp))/norm/norm;
972     resulv.SetLinearForm(-(grosterme*ndotns-nplan.Dot(temp))/norm,nplan,
973                          grosterme/norm,ns,
974                          -1./norm,temp);
975     
976     
977     dnw.SetLinearForm(secmember(1),resulu,secmember(2),resulv,dnw);
978     ns.SetLinearForm(ndotns/norm,nplan, -1./norm,ns);
979     
980     dn2w.SetLinearForm(ray, dnw, -1., tgrst, tgs);
981     norm = resul.Magnitude();
982     dn2w.Divide(norm);
983     ns2 = -resul.Normalized();
984     dn2w.SetLinearForm(ns2.Dot(dn2w),ns2,-1.,dn2w);
985   }
986   else {   
987     ns.SetLinearForm(ndotns/norm,nplan, -1./norm,ns);
988     ns2 = -resul.Normalized();
989   }
990   
991   // Tops 2D
992   
993   Poles2d(Poles2d.Lower()).SetCoord(sol(1),sol(2));
994   Poles2d(Poles2d.Upper()).SetCoord(pt2drst.X(),pt2drst.Y());
995   if (!istgt) {
996     DPoles2d(Poles2d.Lower()).SetCoord(secmember(1),secmember(2));
997     surfrst->D1(pt2drst.X(),pt2drst.Y(),bid,d1urst,d1vrst);
998     Standard_Real a, b;
999     t3dto2d(a,b,tgrst,d1urst,d1vrst);
1000     DPoles2d(Poles2d.Upper()).SetCoord(a,b);
1001   }
1002   
1003   // Linear Case
1004   if (mySShape == BlendFunc_Linear) {
1005     Poles(low) = pts;
1006     Poles(upp) = ptrst;
1007     Weights(low) = 1.0;
1008     Weights(upp) = 1.0;
1009     if (!istgt) {
1010       DPoles(low) = tgs;
1011       DPoles(upp) = tgrst;
1012       DWeights(low) = 0.0;
1013       DWeights(upp) = 0.0;
1014     }
1015     return (!istgt);
1016   }
1017   
1018   // Case of the circle
1019   Center.SetXYZ(pts.XYZ()+ray*ns.XYZ());
1020   if (!istgt) {
1021     tgct = tgs.Added(ray*dnw);
1022   }
1023   
1024   if (ray > 0.) {
1025     ns.Reverse();
1026     if (!istgt) {
1027       dnw.Reverse();
1028     }
1029   }
1030   if (choix%2 != 0) {
1031     nplan.Reverse();
1032     dnplan.Reverse();
1033   }
1034   if (!istgt) {
1035     return GeomFill::GetCircle(myTConv, 
1036                               ns, ns2, 
1037                               dnw, dn2w, 
1038                               nplan, dnplan, 
1039                               pts, ptrst, 
1040                               tgs, tgrst, 
1041                               Abs(ray), 0, 
1042                               Center, tgct, 
1043                               Poles, 
1044                               DPoles,
1045                               Weights, 
1046                               DWeights); 
1047   }
1048   else {
1049     GeomFill::GetCircle(myTConv,
1050                        ns, ns2, 
1051                        nplan, pts, ptrst,
1052                        Abs(ray), Center, 
1053                        Poles, Weights);
1054     return Standard_False;
1055   }
1056 }
1057
1058 //=======================================================================
1059 //function : Section
1060 //purpose  : 
1061 //=======================================================================
1062
1063 Standard_Boolean BRepBlend_SurfRstConstRad::Section
1064 (const Blend_Point&,
1065  TColgp_Array1OfPnt&,
1066  TColgp_Array1OfVec&,
1067  TColgp_Array1OfVec&,
1068  TColgp_Array1OfPnt2d&,
1069  TColgp_Array1OfVec2d&,
1070  TColgp_Array1OfVec2d&,
1071  TColStd_Array1OfReal&,
1072  TColStd_Array1OfReal&,
1073  TColStd_Array1OfReal&)
1074 {
1075   return Standard_False;
1076 }
1077
1078 void BRepBlend_SurfRstConstRad::Resolution(const Standard_Integer IC2d,
1079                                            const Standard_Real Tol,
1080                                            Standard_Real& TolU,
1081                                            Standard_Real& TolV) const
1082 {
1083   if(IC2d == 1){
1084     TolU = surf->UResolution(Tol);
1085     TolV = surf->VResolution(Tol);
1086   }
1087   else {
1088     TolU = surfrst->UResolution(Tol);
1089     TolV = surfrst->VResolution(Tol);
1090   }
1091 }
1092