b311480e |
1 | // Created on: 1993-12-02 |
2 | // Created by: Jacques GOUSSARD |
3 | // Copyright (c) 1993-1999 Matra Datavision |
973c2be1 |
4 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
5 | // |
973c2be1 |
6 | // This file is part of Open CASCADE Technology software library. |
b311480e |
7 | // |
d5f74e42 |
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 |
973c2be1 |
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. |
b311480e |
13 | // |
973c2be1 |
14 | // Alternatively, this file may be used under the terms of Open CASCADE |
15 | // commercial license or contractual agreement. |
7fd59977 |
16 | |
17 | // Modified 10/09/1996 PMN Ajout de (Nb)Intervalles, IsRationnal |
18 | // + Utilisation de GeomFill::GetCircle dans Section. |
19 | |
42cf5bc1 |
20 | #include <Adaptor3d_HCurve.hxx> |
21 | #include <Adaptor3d_HSurface.hxx> |
22 | #include <Blend_Point.hxx> |
7fd59977 |
23 | #include <BlendFunc.hxx> |
42cf5bc1 |
24 | #include <BlendFunc_CSConstRad.hxx> |
25 | #include <ElCLib.hxx> |
7fd59977 |
26 | #include <GeomFill.hxx> |
42cf5bc1 |
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> |
7fd59977 |
36 | #include <Standard_DomainError.hxx> |
37 | #include <Standard_NotImplemented.hxx> |
7fd59977 |
38 | |
39 | //======================================================================= |
40 | //function : BlendFunc_CSConstRad |
41 | //purpose : |
42 | //======================================================================= |
7fd59977 |
43 | BlendFunc_CSConstRad::BlendFunc_CSConstRad(const Handle(Adaptor3d_HSurface)& S, |
44 | const Handle(Adaptor3d_HCurve)& C, |
45 | const Handle(Adaptor3d_HCurve)& CG) : |
46 | |
d533dafb |
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 | { |
50ae6dad |
53 | myTConv = Convert_TgtThetaOver2; |
d533dafb |
54 | } |
7fd59977 |
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 | |
35e08fe8 |
110 | void BlendFunc_CSConstRad::Set(const Standard_Real, const Standard_Real) |
7fd59977 |
111 | { |
9775fa61 |
112 | throw Standard_NotImplemented("BlendFunc_CSConstRad::Set"); |
7fd59977 |
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.) { |
c6541a0c |
231 | Angle = 2.*M_PI - Angle; |
7fd59977 |
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) |
9775fa61 |
459 | throw Standard_DomainError("BlendFunc_CSConstRad::TangentOnS"); |
7fd59977 |
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) |
9775fa61 |
471 | throw Standard_DomainError("BlendFunc_CSConstRad::TangentOnC"); |
7fd59977 |
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) |
9775fa61 |
483 | throw Standard_DomainError("BlendFunc_CSConstRad::Tangent2d"); |
7fd59977 |
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 | { |
9775fa61 |
556 | throw Standard_DomainError("BlendFunc_CSConstRad::Section : Not implemented"); |
7fd59977 |
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(); |
9775fa61 |
572 | if (NbPoint != tabV.Length() || NbPoint < 2) {throw Standard_RangeError();} |
7fd59977 |
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.) { |
c6541a0c |
672 | Angle = 2.*M_PI - Angle; |
7fd59977 |
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; |
8c2d3314 |
883 | gp_Vec ncrossns; |
7fd59977 |
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(); |
7fd59977 |
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 | } |