b311480e |
1 | // Created on: 1997-12-19 |
2 | // Created by: Roman BORISOV /Philippe MANGIN |
3 | // Copyright (c) 1997-1999 Matra Datavision |
4 | // Copyright (c) 1999-2012 OPEN CASCADE SAS |
5 | // |
6 | // The content of this file is subject to the Open CASCADE Technology Public |
7 | // License Version 6.5 (the "License"). You may not use the content of this file |
8 | // except in compliance with the License. Please obtain a copy of the License |
9 | // at http://www.opencascade.org and read it completely before using this file. |
10 | // |
11 | // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its |
12 | // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France. |
13 | // |
14 | // The Original Code and all software distributed under the License is |
15 | // distributed on an "AS IS" basis, without warranty of any kind, and the |
16 | // Initial Developer hereby disclaims all such warranties, including without |
17 | // limitation, any warranties of merchantability, fitness for a particular |
18 | // purpose or non-infringement. Please see the License for the specific terms |
19 | // and conditions governing the rights and limitations under the License. |
20 | |
7fd59977 |
21 | |
22 | #include <stdio.h> |
23 | |
24 | #include <GeomFill_CorrectedFrenet.ixx> |
25 | #include <GeomAbs_CurveType.hxx> |
26 | #include <Adaptor3d_HCurve.hxx> |
27 | #include <gp_Trsf.hxx> |
28 | #include <Precision.hxx> |
29 | #include <TColStd_HArray1OfReal.hxx> |
30 | #include <Law_Interpolate.hxx> |
31 | #include <TColStd_SequenceOfReal.hxx> |
32 | #include <gp_Vec2d.hxx> |
33 | #include <BndLib_Add3dCurve.hxx> |
34 | #include <Bnd_Box.hxx> |
35 | #include <GeomLib.hxx> |
36 | #include <Law_Composite.hxx> |
37 | #include <Law_Constant.hxx> |
38 | #include <Law_BSpFunc.hxx> |
39 | #include <Law_BSpline.hxx> |
40 | #include <GeomFill_SnglrFunc.hxx> |
41 | //Patch |
42 | #include <Geom_Plane.hxx> |
43 | #include <Geom_BezierCurve.hxx> |
44 | #include <Geom_BSplineCurve.hxx> |
45 | #include <TColgp_HArray1OfPnt.hxx> |
46 | |
a31abc03 |
47 | |
7fd59977 |
48 | #ifdef DEB |
49 | static Standard_Boolean Affich=0; |
50 | #endif |
51 | |
52 | #ifdef DRAW |
53 | static Standard_Integer CorrNumber = 0; |
54 | #include <Draw_Appli.hxx> |
55 | #include <DrawTrSurf.hxx> |
56 | #include <Draw_Segment2D.hxx> |
57 | //#include <Draw.hxx> |
58 | #include <TColgp_Array1OfPnt.hxx> |
59 | #include <TColStd_Array1OfReal.hxx> |
60 | #include <TColStd_HArray1OfInteger.hxx> |
61 | #endif |
62 | |
63 | #ifdef DRAW |
64 | static void draw(const Handle(Law_Function)& law) |
65 | { |
66 | Standard_Real Step, u, v, tmin; |
67 | Standard_Integer NbInt, i, j, jmax; |
68 | NbInt = law->NbIntervals(GeomAbs_C3); |
69 | TColStd_Array1OfReal Int(1, NbInt+1); |
70 | law->Intervals(Int, GeomAbs_C3); |
71 | gp_Pnt2d old; |
72 | Handle(Draw_Segment2D) tg2d; |
73 | |
74 | for(i = 1; i <= NbInt; i++){ |
75 | tmin = Int(i); |
76 | Step = (Int(i+1)-Int(i))/4; |
77 | if (i == NbInt) jmax = 4; |
78 | else jmax = 3; |
79 | for (j=1; j<=jmax; j++) { |
80 | u = tmin + (j-1)*Step; |
81 | v = law->Value(u); |
82 | gp_Pnt2d point2d(u,v); |
83 | if ((i>1)||(j>1)) { |
84 | tg2d = new Draw_Segment2D(old, point2d,Draw_kaki); |
85 | dout << tg2d; |
86 | } |
87 | old = point2d; |
88 | } |
89 | } |
90 | dout.Flush(); |
91 | } |
92 | #endif |
93 | |
a31abc03 |
94 | |
95 | static Standard_Real ComputeTorsion(const Standard_Real Param, |
96 | const Handle(Adaptor3d_HCurve)& aCurve) |
97 | { |
98 | Standard_Real Torsion; |
99 | |
100 | gp_Pnt aPoint; |
101 | gp_Vec DC1, DC2, DC3; |
102 | aCurve->D3(Param, aPoint, DC1, DC2, DC3); |
103 | gp_Vec DC1crossDC2 = DC1 ^ DC2; |
104 | Standard_Real Norm_DC1crossDC2 = DC1crossDC2.Magnitude(); |
105 | |
106 | Standard_Real DC1DC2DC3 = DC1crossDC2 * DC3 ; //mixed product |
107 | |
108 | Standard_Real Tol = gp::Resolution(); |
109 | Standard_Real SquareNorm_DC1crossDC2 = Norm_DC1crossDC2 * Norm_DC1crossDC2; |
110 | if (SquareNorm_DC1crossDC2 <= Tol) |
111 | Torsion = 0.; |
112 | else |
113 | Torsion = DC1DC2DC3 / SquareNorm_DC1crossDC2 ; |
114 | |
115 | return Torsion; |
116 | } |
117 | |
7fd59977 |
118 | //=============================================================== |
119 | // Function : smoothlaw |
120 | // Purpose : to smooth a law : Reduce the number of knots |
121 | //=============================================================== |
122 | static void smoothlaw(Handle(Law_BSpline)& Law, |
123 | const Handle(TColStd_HArray1OfReal)& Points, |
124 | const Handle(TColStd_HArray1OfReal)& Param, |
125 | const Standard_Real Tol) |
126 | { |
127 | Standard_Real tol, d; |
128 | Standard_Integer ii, Nbk; |
129 | Standard_Boolean B, Ok; |
130 | Handle(Law_BSpline) BS = Law->Copy(); |
131 | |
132 | Nbk = BS->NbKnots(); |
133 | tol = Tol/10; |
134 | Ok = Standard_False; |
135 | |
136 | for (ii=Nbk-1; ii>1; ii--) { // Une premiere passe tolerance serres |
137 | B = BS->RemoveKnot(ii, 0, tol); |
138 | if (B) Ok = Standard_True; |
139 | } |
140 | |
141 | if (Ok) { // controle |
142 | tol = 0.; |
143 | for (ii=1; ii<=Param->Length() && Ok; ii++) { |
144 | d = Abs(BS->Value(Param->Value(ii))-Points->Value(ii)); |
145 | if (d > tol) tol = d; |
146 | Ok = (tol <= Tol); |
147 | } |
148 | if (Ok) |
149 | tol = (Tol-tol)/2; |
150 | else { |
151 | #if DEB |
152 | cout << "smooth law echec" << endl; |
153 | #endif |
154 | return; // Echec |
155 | } |
156 | } |
157 | else { |
158 | tol = Tol/2; |
159 | } |
160 | |
161 | |
162 | if (Ok) Law = BS; |
163 | |
164 | Ok = Standard_False; // Une deuxieme passe tolerance desserre |
165 | Nbk = BS->NbKnots(); |
166 | for (ii=Nbk-1; ii>1; ii--) { |
167 | B = BS->RemoveKnot(ii, 0, tol); |
168 | if (B) Ok = Standard_True; |
169 | } |
170 | |
171 | if (Ok) { // controle |
172 | tol = 0.; |
173 | for (ii=1; ii<=Param->Length() && Ok; ii++) { |
174 | d = Abs(BS->Value(Param->Value(ii))-Points->Value(ii)); |
175 | if (d > tol) tol = d; |
176 | Ok = (tol <= Tol); |
177 | } |
178 | if (!Ok) { |
179 | #if DEB |
180 | cout << "smooth law echec" << endl; |
181 | #endif |
182 | } |
183 | } |
184 | if (Ok) Law = BS; |
185 | |
186 | #if DEB |
187 | if (Affich) { |
188 | cout << "Knots Law : " << endl; |
189 | for (ii=1; ii<=BS->NbKnots(); ii++) { |
190 | cout << ii << " : " << BS->Knot(ii) << endl; |
191 | } |
192 | } |
193 | #endif |
194 | } |
195 | |
196 | //=============================================================== |
197 | // Function : FindPlane |
198 | // Purpose : |
199 | //=============================================================== |
200 | static Standard_Boolean FindPlane ( const Handle(Adaptor3d_HCurve)& c, |
201 | Handle( Geom_Plane )& P ) |
202 | { |
203 | Standard_Boolean found = Standard_True; |
204 | Handle(TColgp_HArray1OfPnt) TabP; |
205 | |
206 | switch (c->GetType()) { |
207 | |
208 | case GeomAbs_Line: |
209 | { |
210 | found = Standard_False; |
211 | } |
212 | break; |
213 | |
214 | case GeomAbs_Circle: |
215 | P = new Geom_Plane(gp_Ax3(c->Circle().Position())); |
216 | break; |
217 | |
218 | case GeomAbs_Ellipse: |
219 | P = new Geom_Plane(gp_Ax3(c->Ellipse().Position())); |
220 | break; |
221 | |
222 | case GeomAbs_Hyperbola: |
223 | P = new Geom_Plane(gp_Ax3(c->Hyperbola().Position())); |
224 | break; |
225 | |
226 | case GeomAbs_Parabola: |
227 | P = new Geom_Plane(gp_Ax3(c->Parabola().Position())); |
228 | break; |
229 | |
230 | case GeomAbs_BezierCurve: |
231 | { |
232 | Handle(Geom_BezierCurve) GC = c->Bezier(); |
233 | Standard_Integer nbp = GC->NbPoles(); |
234 | if ( nbp < 2) |
235 | found = Standard_False; |
236 | else if ( nbp == 2) { |
237 | found = Standard_False; |
238 | } |
239 | else { |
240 | TabP = new (TColgp_HArray1OfPnt) (1, nbp); |
241 | GC->Poles(TabP->ChangeArray1()); |
242 | } |
243 | } |
244 | break; |
245 | |
246 | case GeomAbs_BSplineCurve: |
247 | { |
248 | Handle(Geom_BSplineCurve) GC = c->BSpline(); |
249 | Standard_Integer nbp = GC->NbPoles(); |
250 | if ( nbp < 2) |
251 | found = Standard_False; |
252 | else if ( nbp == 2) { |
253 | found = Standard_False; |
254 | } |
255 | else { |
256 | TabP = new (TColgp_HArray1OfPnt) (1, nbp); |
257 | GC->Poles(TabP->ChangeArray1()); |
258 | } |
259 | } |
260 | break; |
261 | |
262 | default: |
263 | { // On utilise un echantillonage |
264 | Standard_Integer nbp = 15 + c->NbIntervals(GeomAbs_C3); |
265 | Standard_Real f, l, t, inv; |
266 | Standard_Integer ii; |
267 | f = c->FirstParameter(); |
268 | l = c->LastParameter(); |
269 | inv = 1./(nbp-1); |
270 | for (ii=1; ii<=nbp; ii++) { |
271 | t = ( f*(nbp-ii) + l*(ii-1)); |
272 | t *= inv; |
273 | TabP->SetValue(ii, c->Value(t)); |
274 | } |
275 | } |
276 | } |
277 | |
278 | if (! TabP.IsNull()) { // Recherche d'un plan moyen et controle |
279 | Standard_Boolean issingular; |
280 | gp_Ax2 inertia; |
281 | GeomLib::AxeOfInertia(TabP->Array1(), inertia, issingular); |
282 | if (issingular) { |
283 | found = Standard_False; |
284 | } |
285 | else { |
286 | P = new Geom_Plane(inertia); |
287 | } |
288 | if (found) |
289 | { |
290 | //control = Controle(TabP->Array1(), P, myTolerance); |
291 | // Standard_Boolean isOnPlane; |
292 | Standard_Real a,b,c,d, dist; |
293 | Standard_Integer ii; |
294 | P->Coefficients(a,b,c,d); |
295 | for (ii=1; ii<=TabP->Length() && found; ii++) { |
296 | const gp_XYZ& xyz = TabP->Value(ii).XYZ(); |
297 | dist = a*xyz.X() + b*xyz.Y() + c*xyz.Z() + d; |
298 | found = (Abs(dist) <= Precision::Confusion()); |
299 | } |
300 | return found; |
301 | } |
302 | } |
303 | |
304 | return found; |
305 | } |
306 | |
307 | //=============================================================== |
a31abc03 |
308 | // Function : Constructor |
7fd59977 |
309 | // Purpose : |
310 | //=============================================================== |
311 | GeomFill_CorrectedFrenet::GeomFill_CorrectedFrenet() |
312 | : isFrenet(Standard_False) |
313 | { |
314 | frenet = new GeomFill_Frenet(); |
a31abc03 |
315 | myForEvaluation = Standard_False; |
316 | } |
317 | |
318 | //=============================================================== |
319 | // Function : Constructor |
320 | // Purpose : |
321 | //=============================================================== |
322 | GeomFill_CorrectedFrenet::GeomFill_CorrectedFrenet(const Standard_Boolean ForEvaluation) |
323 | : isFrenet(Standard_False) |
324 | { |
325 | frenet = new GeomFill_Frenet(); |
326 | myForEvaluation = ForEvaluation; |
7fd59977 |
327 | } |
328 | |
329 | Handle(GeomFill_TrihedronLaw) GeomFill_CorrectedFrenet::Copy() const |
330 | { |
331 | Handle(GeomFill_CorrectedFrenet) copy = new (GeomFill_CorrectedFrenet)(); |
332 | if (!myCurve.IsNull()) copy->SetCurve(myCurve); |
333 | return copy; |
334 | } |
335 | |
336 | void GeomFill_CorrectedFrenet::SetCurve(const Handle(Adaptor3d_HCurve)& C) |
337 | { |
338 | |
339 | GeomFill_TrihedronLaw::SetCurve(C); |
340 | if (! C.IsNull()) { |
341 | frenet->SetCurve(C); |
342 | |
343 | GeomAbs_CurveType type; |
344 | type = C->GetType(); |
345 | switch (type) { |
346 | case GeomAbs_Circle: |
347 | case GeomAbs_Ellipse: |
348 | case GeomAbs_Hyperbola: |
349 | case GeomAbs_Parabola: |
350 | case GeomAbs_Line: |
351 | { |
352 | // No probleme isFrenet |
353 | isFrenet = Standard_True; |
a31abc03 |
354 | break; |
7fd59977 |
355 | } |
356 | default : |
357 | { |
358 | // We have to search singulaties |
359 | isFrenet = Standard_True; |
360 | Init(); |
361 | } |
362 | } |
363 | } |
364 | } |
365 | |
366 | |
367 | //=============================================================== |
368 | // Function : Init |
369 | // Purpose : Compute angle's law |
370 | //=============================================================== |
371 | void GeomFill_CorrectedFrenet::Init() |
372 | { |
373 | EvolAroundT = new Law_Composite(); |
374 | Standard_Integer NbI = frenet->NbIntervals(GeomAbs_C0), i; |
375 | TColStd_Array1OfReal T(1, NbI + 1); |
376 | frenet->Intervals(T, GeomAbs_C0); |
377 | Handle(Law_Function) Func; |
378 | //OCC78 |
379 | TColStd_SequenceOfReal SeqPoles, SeqAngle; |
380 | TColgp_SequenceOfVec SeqTangent, SeqNormal; |
381 | |
382 | gp_Vec Tangent, Normal, BN; |
383 | frenet->D0(myTrimmed->FirstParameter(), Tangent, Normal, BN); |
384 | Standard_Integer NbStep; |
385 | // Standard_Real StartAng = 0, AvStep, Step, t; |
386 | Standard_Real StartAng = 0, AvStep, Step; |
387 | |
388 | #if DRAW |
389 | Standard_Real t; |
390 | |
391 | if (Affich) { // Display the curve C'^C''(t) |
392 | GeomFill_SnglrFunc CS(myCurve); |
393 | NbStep = 99; |
394 | AvStep = (myTrimmed->LastParameter() - |
395 | myTrimmed->FirstParameter())/NbStep; |
396 | TColgp_Array1OfPnt TabP(1, NbStep+1); |
397 | |
398 | TColStd_Array1OfReal TI(1, NbStep+1); |
399 | TColStd_Array1OfInteger M(1,NbStep+1); |
400 | M.Init(1); |
401 | M(1) = M(NbStep+1) = 2; |
402 | for (i=1; i<=NbStep+1; i++) { |
403 | t = (myTrimmed->FirstParameter()+ (i-1)*AvStep); |
404 | CS.D0(t, TabP(i)); |
405 | TI(i) = t; |
406 | } |
407 | char tname[100]; |
408 | Standard_CString name = tname ; |
409 | sprintf(name,"Binorm_%d", ++CorrNumber); |
410 | Handle(Geom_BSplineCurve) BS = new |
411 | (Geom_BSplineCurve) (TabP, TI, M, 1); |
412 | // DrawTrSurf::Set(&name[0], BS); |
413 | DrawTrSurf::Set(name, BS); |
414 | } |
415 | #endif |
416 | |
417 | |
418 | NbStep = 10; |
419 | AvStep = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/NbStep; |
420 | for(i = 1; i <= NbI; i++) { |
421 | NbStep = Max(Standard_Integer((T(i+1) - T(i))/AvStep), 3); |
422 | Step = (T(i+1) - T(i))/NbStep; |
a31abc03 |
423 | if(!InitInterval(T(i), T(i+1), Step, StartAng, Tangent, Normal, AT, AN, Func, |
424 | SeqPoles, SeqAngle, SeqTangent, SeqNormal)) |
425 | { |
426 | if(isFrenet) |
427 | isFrenet = Standard_False; |
428 | } |
7fd59977 |
429 | Handle(Law_Composite)::DownCast(EvolAroundT)->ChangeLaws().Append(Func); |
430 | } |
431 | if(myTrimmed->IsPeriodic()) |
432 | Handle(Law_Composite)::DownCast(EvolAroundT)->SetPeriodic(); |
433 | |
434 | TLaw = EvolAroundT; |
435 | //OCC78 |
436 | Standard_Integer iEnd = SeqPoles.Length(); |
437 | HArrPoles = new TColStd_HArray1OfReal(1, iEnd); |
438 | HArrAngle = new TColStd_HArray1OfReal(1, iEnd); |
439 | HArrTangent = new TColgp_HArray1OfVec(1, iEnd); |
440 | HArrNormal = new TColgp_HArray1OfVec(1, iEnd); |
441 | for(i = 1; i <= iEnd; i++){ |
442 | HArrPoles->ChangeValue(i) = SeqPoles(i); |
443 | HArrAngle->ChangeValue(i) = SeqAngle(i); |
444 | HArrTangent->ChangeValue(i) = SeqTangent(i); |
445 | HArrNormal->ChangeValue(i) = SeqNormal(i); |
446 | }; |
a31abc03 |
447 | |
7fd59977 |
448 | #if DRAW |
449 | if (Affich) { |
450 | draw(EvolAroundT); |
451 | } |
452 | #endif |
453 | } |
454 | |
455 | //=============================================================== |
456 | // Function : InitInterval |
457 | // Purpose : Compute the angle law on a span |
458 | //=============================================================== |
459 | Standard_Boolean GeomFill_CorrectedFrenet:: |
460 | InitInterval(const Standard_Real First, const Standard_Real Last, |
461 | const Standard_Real Step, |
462 | Standard_Real& startAng, gp_Vec& prevTangent, |
463 | gp_Vec& prevNormal, gp_Vec& aT, gp_Vec& aN, |
464 | Handle(Law_Function)& FuncInt, |
465 | TColStd_SequenceOfReal& SeqPoles, |
466 | TColStd_SequenceOfReal& SeqAngle, |
467 | TColgp_SequenceOfVec& SeqTangent, |
468 | TColgp_SequenceOfVec& SeqNormal) const |
469 | { |
470 | Bnd_Box Boite; |
471 | gp_Vec Tangent, Normal, BN, cross; |
472 | TColStd_SequenceOfReal parameters; |
473 | TColStd_SequenceOfReal EvolAT; |
474 | Standard_Real Param = First, LengthMin, L, norm; |
475 | Standard_Boolean isZero = Standard_True, isConst = Standard_True; |
a31abc03 |
476 | const Standard_Real minnorm = 1.e-16; |
7fd59977 |
477 | Standard_Integer i; |
478 | gp_Pnt PonC; |
479 | gp_Vec D1; |
480 | |
481 | frenet->SetInterval(First, Last); //To have the rigth evaluation at bounds |
482 | GeomFill_SnglrFunc CS(myCurve); |
483 | BndLib_Add3dCurve::Add(CS, First, Last, 1.e-2, Boite); |
484 | LengthMin = Boite.GetGap()*1.e-4; |
485 | |
486 | aT = gp_Vec(0, 0, 0); |
487 | aN = gp_Vec(0, 0, 0); |
488 | |
489 | Standard_Real angleAT, currParam, currStep = Step; |
490 | |
491 | Handle( Geom_Plane ) aPlane; |
a31abc03 |
492 | Standard_Boolean isPlanar = Standard_False; |
493 | if (!myForEvaluation) |
494 | isPlanar = FindPlane( myCurve, aPlane ); |
7fd59977 |
495 | |
496 | i = 1; |
497 | currParam = Param; |
498 | Standard_Real DLast = Last - Precision::PConfusion(); |
499 | |
500 | while (Param < Last) { |
501 | if (currParam > DLast) { |
502 | currStep = DLast - Param; |
503 | currParam = Last; |
504 | } |
505 | if (isPlanar) |
506 | currParam = Last; |
507 | |
508 | frenet->D0(currParam, Tangent, Normal, BN); |
c6541a0c |
509 | if (prevTangent.Angle(Tangent) < M_PI/3 || i == 1) { |
7fd59977 |
510 | parameters.Append(currParam); |
511 | //OCC78 |
512 | SeqPoles.Append(Param); |
513 | SeqAngle.Append(i > 1? EvolAT(i-1) : startAng); |
514 | SeqTangent.Append(prevTangent); |
515 | SeqNormal.Append(prevNormal); |
516 | angleAT = CalcAngleAT(Tangent,Normal,prevTangent,prevNormal); |
517 | |
518 | if(isConst && i > 1) |
519 | if(Abs(angleAT) > Precision::PConfusion()) |
520 | isConst = Standard_False; |
521 | |
522 | angleAT += (i > 1) ? EvolAT(i-1) : startAng; |
523 | EvolAT.Append(angleAT); |
524 | prevNormal = Normal; |
525 | |
526 | if(isZero) |
527 | if(Abs(angleAT) > Precision::PConfusion()) |
528 | isZero = Standard_False; |
529 | |
530 | aT += Tangent; |
531 | cross = Tangent.Crossed(Normal); |
532 | aN.SetLinearForm(Sin(angleAT), cross, |
533 | 1 - Cos(angleAT), Tangent.Crossed(cross), |
534 | Normal+aN); |
535 | prevTangent = Tangent; |
536 | Param = currParam; |
537 | i++; |
538 | |
539 | //Evaluate the Next step |
540 | CS.D1(Param, PonC, D1); |
a31abc03 |
541 | |
542 | L = PonC.XYZ().Modulus()/2; |
7fd59977 |
543 | norm = D1.Magnitude(); |
a31abc03 |
544 | if (norm <= gp::Resolution()) |
545 | { |
546 | //norm = 2.*gp::Resolution(); |
547 | norm = minnorm; |
7fd59977 |
548 | } |
549 | currStep = L / norm; |
a31abc03 |
550 | if (currStep <= gp::Resolution()) //L = 0 => curvature = 0, linear segment |
551 | currStep = Step; |
552 | if (currStep < Precision::Confusion()) //too small step |
553 | currStep = Precision::Confusion(); |
554 | if (currStep > Step) //too big step |
555 | currStep = Step;//default value |
7fd59977 |
556 | } |
557 | else |
558 | currStep /= 2; // Step too long ! |
559 | |
560 | currParam = Param + currStep; |
561 | } |
562 | |
563 | if (! isPlanar) |
564 | { |
565 | aT /= parameters.Length() - 1; |
566 | aN /= parameters.Length() - 1; |
567 | } |
568 | startAng = angleAT; |
569 | |
570 | // Interpolation |
571 | if (isConst || isPlanar) { |
572 | FuncInt = new Law_Constant(); |
573 | Handle(Law_Constant)::DownCast(FuncInt)->Set( angleAT, First, Last ); |
574 | } |
575 | |
576 | else { |
577 | Standard_Integer Length = parameters.Length(); |
578 | Handle(TColStd_HArray1OfReal) pararr = |
579 | new TColStd_HArray1OfReal(1, Length); |
580 | Handle(TColStd_HArray1OfReal) angleATarr = |
581 | new TColStd_HArray1OfReal(1, Length); |
582 | |
583 | |
584 | for (i = 1; i <= Length; i++) { |
585 | pararr->ChangeValue(i) = parameters(i); |
586 | angleATarr->ChangeValue(i) = EvolAT(i); |
587 | } |
588 | |
589 | #if DEB |
590 | if (Affich) { |
591 | cout<<"NormalEvolution"<<endl; |
592 | for (i = 1; i <= Length; i++) { |
593 | cout<<"("<<pararr->Value(i)<<", "<<angleATarr->Value(i)<<")" << endl; |
594 | } |
595 | cout<<endl; |
596 | } |
597 | #endif |
598 | |
599 | Law_Interpolate lawAT(angleATarr, pararr, |
600 | Standard_False, Precision::PConfusion()); |
601 | lawAT.Perform(); |
602 | Handle(Law_BSpline) BS = lawAT.Curve(); |
603 | smoothlaw(BS, angleATarr, pararr, 0.1); |
604 | |
605 | FuncInt = new Law_BSpFunc(BS, First, Last); |
606 | } |
607 | return isZero; |
608 | } |
609 | //=============================================================== |
610 | // Function : CalcAngleAT (OCC78) |
611 | // Purpose : Calculate angle of rotation of trihedron normal and its derivatives relative |
612 | // at any position on his curve |
613 | //=============================================================== |
614 | Standard_Real GeomFill_CorrectedFrenet::CalcAngleAT(const gp_Vec& Tangent, const gp_Vec& Normal, |
615 | const gp_Vec& prevTangent, const gp_Vec& prevNormal) const |
616 | { |
617 | Standard_Real angle; |
618 | gp_Vec Normal_rot, cross; |
619 | angle = Tangent.Angle(prevTangent); |
620 | if (Abs(angle) > Precision::Angular()) { |
621 | cross = Tangent.Crossed(prevTangent).Normalized(); |
622 | Normal_rot = Normal + sin(angle)*cross.Crossed(Normal) + |
623 | (1 - cos(angle))*cross.Crossed(cross.Crossed(Normal)); |
624 | } |
625 | else |
626 | Normal_rot = Normal; |
627 | Standard_Real angleAT = Normal_rot.Angle(prevNormal); |
c6541a0c |
628 | if(angleAT > Precision::Angular() && M_PI - angleAT > Precision::Angular()) |
7fd59977 |
629 | if (Normal_rot.Crossed(prevNormal).IsOpposite(prevTangent, Precision::Angular())) |
630 | angleAT = -angleAT; |
631 | return angleAT; |
632 | }; |
633 | //=============================================================== |
634 | // Function : ... (OCC78) |
635 | // Purpose : This family of functions produce conversion of angle utility |
636 | //=============================================================== |
7fd59977 |
637 | static Standard_Real corr2PI_PI(Standard_Real Ang){ |
c6541a0c |
638 | return Ang = (Ang < M_PI? Ang: Ang-2*M_PI); |
7fd59977 |
639 | }; |
640 | static Standard_Real diffAng(Standard_Real A, Standard_Real Ao){ |
c6541a0c |
641 | Standard_Real dA = (A-Ao) - Floor((A-Ao)/2.0/M_PI)*2.0*M_PI; |
7fd59977 |
642 | return dA = dA >= 0? corr2PI_PI(dA): -corr2PI_PI(-dA); |
643 | }; |
644 | //=============================================================== |
645 | // Function : CalcAngleAT (OCC78) |
646 | // Purpose : Calculate angle of rotation of trihedron normal and its derivatives relative |
647 | // at any position on his curve |
648 | //=============================================================== |
649 | Standard_Real GeomFill_CorrectedFrenet::GetAngleAT(const Standard_Real Param) const{ |
650 | // Search index of low margin from poles of TLaw by bisection method |
651 | Standard_Integer iB = 1, iE = HArrPoles->Length(), iC = (iE+iB)/2; |
652 | if(Param == HArrPoles->Value(iB)) return TLaw->Value(Param); |
653 | if(Param > HArrPoles->Value(iE)) iC = iE; |
654 | if(iC < iE){ |
655 | while(!(HArrPoles->Value(iC) <= Param && Param <= HArrPoles->Value(iC+1))){ |
656 | if(HArrPoles->Value(iC) < Param) iB = iC; else iE = iC; |
657 | iC = (iE+iB)/2; |
658 | }; |
659 | if(HArrPoles->Value(iC) == Param || Param == HArrPoles->Value(iC+1)) return TLaw->Value(Param); |
660 | }; |
7fd59977 |
661 | // Calculate differenciation between apporoximated and local values of AngleAT |
662 | Standard_Real AngP = TLaw->Value(Param), AngPo = HArrAngle->Value(iC), dAng = AngP - AngPo; |
663 | gp_Vec Tangent, Normal, BN; |
664 | frenet->D0(Param, Tangent, Normal, BN); |
665 | Standard_Real DAng = CalcAngleAT(Tangent, Normal, HArrTangent->Value(iC), HArrNormal->Value(iC)); |
666 | Standard_Real DA = diffAng(DAng,dAng); |
667 | // The correction (there is core of OCC78 bug) |
c6541a0c |
668 | if(Abs(DA) > M_PI/2.0){ |
7fd59977 |
669 | AngP = AngPo + DAng; |
670 | }; |
671 | return AngP; |
672 | }; |
673 | //=============================================================== |
674 | // Function : D0 |
675 | // Purpose : |
676 | //=============================================================== |
677 | Standard_Boolean GeomFill_CorrectedFrenet::D0(const Standard_Real Param, |
678 | gp_Vec& Tangent, |
679 | gp_Vec& Normal, |
680 | gp_Vec& BiNormal) |
681 | { |
682 | frenet->D0(Param, Tangent, Normal, BiNormal); |
683 | if (isFrenet) return Standard_True; |
684 | |
685 | Standard_Real angleAT; |
686 | //angleAT = TLaw->Value(Param); |
687 | angleAT = GetAngleAT(Param); //OCC78 |
688 | |
689 | // rotation around Tangent |
690 | gp_Vec cross; |
691 | cross = Tangent.Crossed(Normal); |
692 | Normal.SetLinearForm(Sin(angleAT), cross, |
693 | (1 - Cos(angleAT)), Tangent.Crossed(cross), |
694 | Normal); |
695 | BiNormal = Tangent.Crossed(Normal); |
696 | |
697 | return Standard_True; |
698 | } |
699 | |
700 | //=============================================================== |
701 | // Function : D1 |
702 | // Purpose : |
703 | //=============================================================== |
704 | |
705 | Standard_Boolean GeomFill_CorrectedFrenet::D1(const Standard_Real Param, |
706 | gp_Vec& Tangent, |
707 | gp_Vec& DTangent, |
708 | gp_Vec& Normal, |
709 | gp_Vec& DNormal, |
710 | gp_Vec& BiNormal, |
711 | gp_Vec& DBiNormal) |
712 | { |
713 | frenet->D1(Param, Tangent, DTangent, Normal, DNormal, BiNormal, DBiNormal); |
714 | if (isFrenet) return Standard_True; |
715 | |
716 | Standard_Real angleAT, d_angleAT; |
717 | Standard_Real sina, cosa; |
718 | |
719 | TLaw->D1(Param, angleAT, d_angleAT); |
720 | angleAT = GetAngleAT(Param); //OCC78 |
721 | |
722 | gp_Vec cross, dcross, tcross, dtcross, aux; |
723 | sina = Sin(angleAT); |
724 | cosa = Cos(angleAT); |
725 | |
726 | cross = Tangent.Crossed(Normal); |
727 | dcross.SetLinearForm(1, DTangent.Crossed(Normal), |
728 | Tangent.Crossed(DNormal)); |
729 | |
730 | tcross = Tangent.Crossed(cross); |
731 | dtcross.SetLinearForm(1, DTangent.Crossed(cross), |
732 | Tangent.Crossed(dcross)); |
733 | |
734 | aux.SetLinearForm(sina, dcross, |
735 | cosa*d_angleAT, cross); |
736 | aux.SetLinearForm(1 - cosa, dtcross, |
737 | sina*d_angleAT, tcross, |
738 | aux); |
739 | DNormal+=aux; |
740 | |
741 | Normal.SetLinearForm( sina, cross, |
742 | (1 - cosa), tcross, |
743 | Normal); |
744 | |
745 | BiNormal = Tangent.Crossed(Normal); |
746 | |
747 | DBiNormal.SetLinearForm(1, DTangent.Crossed(Normal), |
748 | Tangent.Crossed(DNormal)); |
749 | |
750 | // for test |
751 | /* gp_Vec FDN, Tf, Nf, BNf; |
752 | Standard_Real h; |
753 | h = 1.0e-8; |
754 | if (Param + h > myTrimmed->LastParameter()) h = -h; |
755 | D0(Param + h, Tf, Nf, BNf); |
756 | FDN = (Nf - Normal)/h; |
757 | cout<<"Param = "<<Param<<endl; |
758 | cout<<"DN = ("<<DNormal.X()<<", "<<DNormal.Y()<<", "<<DNormal.Z()<<")"<<endl; |
759 | cout<<"FDN = ("<<FDN.X()<<", "<<FDN.Y()<<", "<<FDN.Z()<<")"<<endl; |
760 | */ |
761 | |
762 | return Standard_True; |
763 | } |
764 | |
765 | //=============================================================== |
766 | // Function : D2 |
767 | // Purpose : |
768 | //=============================================================== |
769 | Standard_Boolean GeomFill_CorrectedFrenet::D2(const Standard_Real Param, |
770 | gp_Vec& Tangent, |
771 | gp_Vec& DTangent, |
772 | gp_Vec& D2Tangent, |
773 | gp_Vec& Normal, |
774 | gp_Vec& DNormal, |
775 | gp_Vec& D2Normal, |
776 | gp_Vec& BiNormal, |
777 | gp_Vec& DBiNormal, |
778 | gp_Vec& D2BiNormal) |
779 | { |
780 | frenet->D2(Param, Tangent, DTangent, D2Tangent, |
781 | Normal, DNormal, D2Normal, |
782 | BiNormal, DBiNormal, D2BiNormal); |
783 | if (isFrenet) return Standard_True; |
784 | |
785 | Standard_Real angleAT, d_angleAT, d2_angleAT; |
786 | Standard_Real sina, cosa; |
787 | TLaw->D2(Param, angleAT, d_angleAT, d2_angleAT); |
788 | angleAT = GetAngleAT(Param); //OCC78 |
789 | |
790 | gp_Vec cross, dcross, d2cross, tcross, dtcross, d2tcross, aux; |
791 | sina = Sin(angleAT); |
792 | cosa = Cos(angleAT); |
793 | cross = Tangent.Crossed(Normal); |
794 | dcross.SetLinearForm(1, DTangent.Crossed(Normal), |
795 | Tangent.Crossed(DNormal)); |
796 | d2cross.SetLinearForm(1, D2Tangent.Crossed(Normal), |
797 | 2, DTangent.Crossed(DNormal), |
798 | Tangent.Crossed(D2Normal)); |
799 | |
800 | |
801 | tcross = Tangent.Crossed(cross); |
802 | dtcross.SetLinearForm(1, DTangent.Crossed(cross), |
803 | Tangent.Crossed(dcross)); |
804 | d2tcross.SetLinearForm(1, D2Tangent.Crossed(cross), |
805 | 2, DTangent.Crossed(dcross), |
806 | Tangent.Crossed(d2cross)); |
807 | |
808 | |
809 | aux.SetLinearForm(sina, d2cross, |
810 | 2*cosa*d_angleAT, dcross, |
811 | cosa*d2_angleAT - sina*d_angleAT*d_angleAT, cross); |
812 | |
813 | aux.SetLinearForm(1 - cosa, d2tcross, |
814 | 2*sina*d_angleAT, dtcross, |
815 | cosa*d_angleAT*d_angleAT + sina*d2_angleAT, tcross, |
816 | aux); |
817 | D2Normal += aux; |
818 | |
819 | /* D2Normal += sina*(D2Tangent.Crossed(Normal) + 2*DTangent.Crossed(DNormal) + Tangent.Crossed(D2Normal)) + |
820 | 2*cosa*d_angleAT*(DTangent.Crossed(Normal) + Tangent.Crossed(DNormal)) + |
821 | (cosa*d2_angleAT - sina*d_angleAT*d_angleAT)*Tangent.Crossed(Normal) + |
822 | 2*sina*d_angleAT*(DTangent.Crossed(Tangent.Crossed(Normal)) + Tangent.Crossed(DTangent.Crossed(Normal)) + Tangent.Crossed(Tangent.Crossed(DNormal))) + |
823 | (1 - cosa)*(D2Tangent.Crossed(Tangent.Crossed(Normal)) + Tangent.Crossed(D2Tangent.Crossed(Normal)) + Tangent.Crossed(Tangent.Crossed(D2Normal)) + 2*DTangent.Crossed(DTangent.Crossed(Normal)) + 2*DTangent.Crossed(Tangent.Crossed(DNormal)) + 2*Tangent.Crossed(DTangent.Crossed(DNormal))) |
824 | + |
825 | (cosa*d_angleAT*d_angleAT + sina*d2_angleAT)*Tangent.Crossed(Tangent.Crossed(Normal));*/ |
826 | |
827 | |
828 | aux.SetLinearForm(sina, dcross, |
829 | cosa*d_angleAT, cross); |
830 | aux.SetLinearForm(1 - cosa, dtcross, |
831 | sina*d_angleAT, tcross, |
832 | aux); |
833 | DNormal+=aux; |
834 | |
835 | |
836 | Normal.SetLinearForm( sina, cross, |
837 | (1 - cosa), tcross, |
838 | Normal); |
839 | |
840 | BiNormal = Tangent.Crossed(Normal); |
841 | |
842 | DBiNormal.SetLinearForm(1, DTangent.Crossed(Normal), |
843 | Tangent.Crossed(DNormal)); |
844 | |
845 | D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal), |
846 | 2, DTangent.Crossed(DNormal), |
847 | Tangent.Crossed(D2Normal)); |
848 | |
849 | // for test |
850 | /* gp_Vec FD2N, FD2T, FD2BN, Tf, DTf, Nf, DNf, BNf, DBNf; |
851 | Standard_Real h; |
852 | h = 1.0e-8; |
853 | if (Param + h > myTrimmed->LastParameter()) h = -h; |
854 | D1(Param + h, Tf, DTf, Nf, DNf, BNf, DBNf); |
855 | FD2N = (DNf - DNormal)/h; |
856 | FD2T = (DTf - DTangent)/h; |
857 | FD2BN = (DBNf - DBiNormal)/h; |
858 | cout<<"Param = "<<Param<<endl; |
859 | cout<<"D2N = ("<<D2Normal.X()<<", "<<D2Normal.Y()<<", "<<D2Normal.Z()<<")"<<endl; |
860 | cout<<"FD2N = ("<<FD2N.X()<<", "<<FD2N.Y()<<", "<<FD2N.Z()<<")"<<endl<<endl; |
861 | cout<<"D2T = ("<<D2Tangent.X()<<", "<<D2Tangent.Y()<<", "<<D2Tangent.Z()<<")"<<endl; |
862 | cout<<"FD2T = ("<<FD2T.X()<<", "<<FD2T.Y()<<", "<<FD2T.Z()<<")"<<endl<<endl; |
863 | cout<<"D2BN = ("<<D2BiNormal.X()<<", "<<D2BiNormal.Y()<<", "<<D2BiNormal.Z()<<")"<<endl; |
864 | cout<<"FD2BN = ("<<FD2BN.X()<<", "<<FD2BN.Y()<<", "<<FD2BN.Z()<<")"<<endl<<endl; |
865 | */ |
866 | // |
867 | return Standard_True; |
868 | } |
869 | |
870 | //=============================================================== |
871 | // Function : NbIntervals |
872 | // Purpose : |
873 | //=============================================================== |
874 | Standard_Integer GeomFill_CorrectedFrenet::NbIntervals(const GeomAbs_Shape S) const |
875 | { |
876 | Standard_Integer NbFrenet, NbLaw; |
877 | NbFrenet = frenet->NbIntervals(S); |
878 | if (isFrenet) return NbFrenet; |
879 | |
880 | NbLaw = EvolAroundT->NbIntervals(S); |
881 | if (NbFrenet == 1) |
882 | return NbLaw; |
883 | |
884 | TColStd_Array1OfReal FrenetInt(1, NbFrenet + 1); |
885 | TColStd_Array1OfReal LawInt(1, NbLaw + 1); |
886 | TColStd_SequenceOfReal Fusion; |
887 | |
888 | frenet->Intervals(FrenetInt, S); |
889 | EvolAroundT->Intervals(LawInt, S); |
890 | GeomLib::FuseIntervals(FrenetInt, LawInt, Fusion); |
891 | |
892 | return Fusion.Length()-1; |
893 | } |
894 | |
895 | //=============================================================== |
896 | // Function : Intervals |
897 | // Purpose : |
898 | //=============================================================== |
899 | void GeomFill_CorrectedFrenet::Intervals(TColStd_Array1OfReal& T, |
900 | const GeomAbs_Shape S) const |
901 | { |
902 | Standard_Integer NbFrenet, NbLaw; |
903 | if (isFrenet) { |
904 | frenet->Intervals(T, S); |
905 | return; |
906 | } |
907 | |
908 | NbFrenet = frenet->NbIntervals(S); |
909 | if(NbFrenet==1) { |
910 | EvolAroundT->Intervals(T, S); |
911 | } |
912 | |
913 | NbLaw = EvolAroundT->NbIntervals(S); |
914 | |
915 | TColStd_Array1OfReal FrenetInt(1, NbFrenet + 1); |
916 | TColStd_Array1OfReal LawInt(1, NbLaw + 1); |
917 | TColStd_SequenceOfReal Fusion; |
918 | |
919 | frenet->Intervals(FrenetInt, S); |
920 | EvolAroundT->Intervals(LawInt, S); |
921 | GeomLib::FuseIntervals(FrenetInt, LawInt, Fusion); |
922 | |
923 | for(Standard_Integer i = 1; i <= Fusion.Length(); i++) |
924 | T.ChangeValue(i) = Fusion.Value(i); |
925 | } |
926 | |
927 | //=============================================================== |
928 | // Function : SetInterval |
929 | // Purpose : |
930 | //=============================================================== |
931 | void GeomFill_CorrectedFrenet::SetInterval(const Standard_Real First, |
932 | const Standard_Real Last) |
933 | { |
934 | GeomFill_TrihedronLaw::SetInterval(First, Last); |
935 | frenet->SetInterval(First, Last); |
936 | if (!isFrenet) TLaw = EvolAroundT->Trim(First, Last, |
937 | Precision::PConfusion()/2); |
938 | } |
939 | |
940 | //=============================================================== |
a31abc03 |
941 | // Function : EvaluateBestMode |
942 | // Purpose : |
943 | //=============================================================== |
944 | GeomFill_Trihedron GeomFill_CorrectedFrenet::EvaluateBestMode() |
945 | { |
946 | if (EvolAroundT.IsNull()) |
947 | return GeomFill_IsFrenet; //Frenet |
948 | |
949 | const Standard_Real MaxAngle = 3.*M_PI/4.; |
950 | const Standard_Real MaxTorsion = 100.; |
951 | |
952 | Standard_Real Step, u, v, tmin, tmax; |
953 | Standard_Integer NbInt, i, j, k = 1; |
954 | NbInt = EvolAroundT->NbIntervals(GeomAbs_CN); |
955 | TColStd_Array1OfReal Int(1, NbInt+1); |
956 | EvolAroundT->Intervals(Int, GeomAbs_CN); |
957 | gp_Pnt2d old; |
958 | gp_Vec2d aVec, PrevVec; |
959 | |
960 | Standard_Integer NbSamples = 10; |
961 | for(i = 1; i <= NbInt; i++){ |
962 | tmin = Int(i); |
963 | tmax = Int(i+1); |
964 | Standard_Real Torsion = ComputeTorsion(tmin, myTrimmed); |
965 | if (Abs(Torsion) > MaxTorsion) |
966 | return GeomFill_IsDiscreteTrihedron; //DiscreteTrihedron |
967 | |
968 | Handle(Law_Function) trimmedlaw = EvolAroundT->Trim(tmin, tmax, Precision::PConfusion()/2); |
969 | Step = (Int(i+1)-Int(i))/NbSamples; |
970 | for (j = 0; j <= NbSamples; j++) { |
971 | u = tmin + j*Step; |
972 | v = trimmedlaw->Value(u); |
973 | gp_Pnt2d point2d(u,v); |
974 | if (j != 0) |
975 | { |
976 | aVec.SetXY(point2d.XY() - old.XY()); |
977 | if (k > 2) |
978 | { |
979 | Standard_Real theAngle = PrevVec.Angle(aVec); |
980 | if (Abs(theAngle) > MaxAngle) |
981 | return GeomFill_IsDiscreteTrihedron; //DiscreteTrihedron |
982 | } |
983 | PrevVec = aVec; |
984 | } |
985 | old = point2d; |
986 | k++; |
987 | } |
988 | } |
989 | |
990 | return GeomFill_IsCorrectedFrenet; //CorrectedFrenet |
991 | } |
992 | |
993 | //=============================================================== |
7fd59977 |
994 | // Function : GetAverageLaw |
995 | // Purpose : |
996 | //=============================================================== |
997 | void GeomFill_CorrectedFrenet::GetAverageLaw(gp_Vec& ATangent, |
998 | gp_Vec& ANormal, |
999 | gp_Vec& ABiNormal) |
1000 | { |
1001 | if (isFrenet) frenet->GetAverageLaw(ATangent, ANormal, ABiNormal); |
1002 | else { |
1003 | ATangent = AT; |
1004 | ANormal = AN; |
1005 | ABiNormal = ATangent; |
1006 | ABiNormal.Cross(ANormal); |
1007 | } |
1008 | } |
1009 | |
1010 | //=============================================================== |
1011 | // Function : IsConstant |
1012 | // Purpose : |
1013 | //=============================================================== |
1014 | Standard_Boolean GeomFill_CorrectedFrenet::IsConstant() const |
1015 | { |
1016 | return (myCurve->GetType() == GeomAbs_Line); |
1017 | } |
1018 | |
1019 | //=============================================================== |
1020 | // Function : IsOnlyBy3dCurve |
1021 | // Purpose : |
1022 | //=============================================================== |
1023 | Standard_Boolean GeomFill_CorrectedFrenet::IsOnlyBy3dCurve() const |
1024 | { |
1025 | return Standard_True; |
1026 | } |