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