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