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