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