1 // Created on: 1998-07-02
2 // Created by: Stephanie HUMEAU
3 // Copyright (c) 1998-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
6 // This file is part of Open CASCADE Technology software library.
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
18 #include <Adaptor3d_Curve.hxx>
19 #include <Adaptor3d_HCurve.hxx>
21 #include <Geom_Plane.hxx>
22 #include <GeomAdaptor_HCurve.hxx>
23 #include <GeomAdaptor_HSurface.hxx>
24 #include <GeomFill_Frenet.hxx>
25 #include <GeomFill_GuideTrihedronPlan.hxx>
26 #include <GeomFill_PlanFunc.hxx>
27 #include <GeomFill_TrihedronLaw.hxx>
29 #include <gp_Pnt2d.hxx>
31 #include <IntCurveSurface_HInter.hxx>
32 #include <IntCurveSurface_IntersectionPoint.hxx>
33 #include <math_FunctionRoot.hxx>
34 #include <math_Matrix.hxx>
35 #include <math_Vector.hxx>
36 #include <Precision.hxx>
37 #include <Standard_ConstructionError.hxx>
38 #include <Standard_OutOfRange.hxx>
39 #include <Standard_Type.hxx>
41 IMPLEMENT_STANDARD_RTTIEXT(GeomFill_GuideTrihedronPlan,GeomFill_TrihedronWithGuide)
43 //#include <gp_Trsf2d.hxx>
44 //#include <Bnd_Box2d.hxx>
46 #include <DrawTrSurf.hxx>
50 static void TracePlan(const Handle(Geom_Surface)& /*Plan*/)
52 std::cout << "Pas d'intersection Guide/Plan" << std::endl;
54 char* Temp = "ThePlan" ;
55 DrawTrSurf::Set(Temp, Plan);
56 // DrawTrSurf::Set("ThePlan", Plan);
61 //==================================================================
62 //Function: InGoodPeriod
63 //Purpose : Recadre un paramtere
64 //==================================================================
65 static void InGoodPeriod(const Standard_Real Prec,
66 const Standard_Real Period,
67 Standard_Real& Current)
69 Standard_Real Diff=Current-Prec;
70 Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
73 if (Diff > Period/2) Current -= Period;
74 else if (Diff < -Period/2) Current += Period;
77 //=======================================================================
78 //function : GuideTrihedronPlan
79 //purpose : Constructor
80 //=======================================================================
81 GeomFill_GuideTrihedronPlan::GeomFill_GuideTrihedronPlan (const Handle(Adaptor3d_HCurve)& theGuide) :
85 myStatus(GeomFill_PipeOk)
88 myGuide = theGuide; // guide
90 myNbPts = 20; // nb points pour calculs
91 Pole = new (TColgp_HArray2OfPnt2d)(1,1,1,myNbPts);//tab pr stocker Pprime (pt sur guide)
92 frenet = new (GeomFill_Frenet)();
94 XTol(1) = myGuide->Resolution(1.e-6);
97 //=======================================================================
99 //purpose : calcule myNbPts points sur la courbe guide (<=> normale)
100 //=======================================================================
101 void GeomFill_GuideTrihedronPlan::Init()
103 myStatus = GeomFill_PipeOk;
106 // Box.Update(-0.1, -0.1, 0.1, 0.1); // Taille minimal
107 gp_Vec Tangent,Normal,BiNormal;
109 Standard_Real t, DeltaG, w = 0.;
110 Standard_Real f = myCurve->FirstParameter();
111 Standard_Real l = myCurve->LastParameter();
115 Handle(Geom_Plane) Plan;
116 Handle(GeomAdaptor_HSurface) Pl;
117 IntCurveSurface_IntersectionPoint PInt;
118 IntCurveSurface_HInter Int;
119 frenet->SetCurve(myCurve);
120 DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/2;
122 Inf(1) = myGuide->FirstParameter() - DeltaG;
123 Sup(1) = myGuide->LastParameter() + DeltaG;
125 if (!myGuide->IsPeriodic()) {
126 myTrimG = myGuide->Trim(myGuide->FirstParameter()- DeltaG/100,
127 myGuide->LastParameter() + DeltaG/100,
133 // Standard_Real Step = DeltaG/100;
135 for (ii=1; ii<=myNbPts; ii++)
137 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
140 frenet->D0(t, Tangent, Normal, BiNormal);
141 Plan = new (Geom_Plane) (P, Tangent);
142 Pl = new(GeomAdaptor_HSurface) (Plan);
144 Int.Perform(myTrimG, Pl); // intersection plan / guide
145 if (Int.NbPoints() == 0) {
149 w = (fabs(myGuide->LastParameter() -w) > fabs(myGuide->FirstParameter()-w) ? myGuide->FirstParameter() : myGuide->LastParameter());
151 myStatus = GeomFill_PlaneNotIntersectGuide;
159 Standard_Real Dmin = P.Distance(Pmin);
160 for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++)
162 Pmin = Int.Point(jj).Pnt();
163 if (P.Distance(Pmin) < Dmin)
165 PInt = Int.Point(jj);
166 Dmin = P.Distance(Pmin);
173 Standard_Real Diff = w - Pole->Value(1, ii-1).Y();
174 if (Abs(Diff) > DeltaG) {
175 if (myGuide->IsPeriodic()) {
176 InGoodPeriod (Pole->Value(1, ii-1).Y(),
177 myGuide->Period(), w);
179 Diff = w - Pole->Value(1, ii-1).Y();
184 if (Abs(Diff) > DeltaG) {
185 std::cout << "Trihedron Plan Diff on Guide : " <<
191 gp_Pnt2d p1(t, w); // on stocke les parametres
192 Pole->SetValue(1, ii, p1);
197 //=======================================================================
198 //function : SetCurve
199 //purpose : calculation of trihedron
200 //=======================================================================
201 void GeomFill_GuideTrihedronPlan::SetCurve(const Handle(Adaptor3d_HCurve)& C)
204 if (!myCurve.IsNull()) Init();
207 //=======================================================================
209 //purpose : calculation of trihedron
210 //=======================================================================
212 Handle(Adaptor3d_HCurve) GeomFill_GuideTrihedronPlan::Guide()const
217 //=======================================================================
219 //purpose : calculation of trihedron
220 //=======================================================================
221 Standard_Boolean GeomFill_GuideTrihedronPlan::D0(const Standard_Real Param,
229 myCurve->D0(Param, P);
231 frenet->D0(Param,Tangent,Normal,BiNormal);
233 //initialisation de la recherche
236 Standard_Integer Iter = 50;
238 // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
239 GeomFill_PlanFunc E(P, Tangent, myGuide);
242 math_FunctionRoot Result(E, X(1), XTol(1),
243 Inf(1), Sup(1), Iter);
247 Standard_Real Res = Result.Root();
248 // R = Result.Root(); // solution
250 Pprime = myTrimG->Value(Res); // pt sur courbe guide
251 gp_Vec n (P, Pprime); // vecteur definissant la normale du triedre
253 Normal = n.Normalized();
254 BiNormal = Tangent.Crossed(Normal);
255 BiNormal.Normalize();
260 // plan ortho a la trajectoire pour determiner Pprime
261 Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
264 myStatus = GeomFill_PlaneNotIntersectGuide;
265 return Standard_False;
268 return Standard_True;
271 //=======================================================================
273 //purpose : calculation of trihedron and first derivative
274 //=======================================================================
275 Standard_Boolean GeomFill_GuideTrihedronPlan::D1(const Standard_Real Param,
283 // return Standard_False;
289 // triedre de frenet sur la trajectoire
290 myCurve->D1(Param, P, To);
291 frenet->D1(Param,Tangent,DTangent,Normal,DNormal,BiNormal,DBiNormal);
295 Standard_Integer Iter = 50;
297 // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
299 GeomFill_PlanFunc E(P, Tangent, myGuide);
302 math_FunctionRoot Result(E, X(1), XTol(1),
303 Inf(1), Sup(1), Iter);
307 Standard_Real Res = Result.Root();
308 // R = Result.Root(); // solution
309 myTrimG->D1(Res, PG, TG);
310 gp_Vec n (P, PG), dn; // vecteur definissant la normale du triedre
311 Standard_Real Norm = n.Magnitude();
319 BiNormal = Tangent.Crossed(Normal);
321 // derivee premiere du triedre
322 Standard_Real dedx, dedt, dtg_dt;
323 E.Derivative(Res, dedx);
324 E.DEDT(Res, To, DTangent, dedt);
328 /* Standard_Real h=1.e-7, e, etg, etc;
331 if ( Abs( (etg-e)/h - dedx) > 1.e-4) {
332 std::cout << "err :" << (etg-e)/h - dedx << std::endl;
336 myCurve->D0(Param+h, pdbg);
337 frenet->D0(Param+h,td, nb, bnb);
339 GeomFill_PlanFunc Edeb(pdbg, td, myGuide);
340 Edeb.Value(Res, etc);
341 if ( Abs( (etc-e)/h - dedt) > 1.e-4) {
342 std::cout << "err :" << (etc-e)/h - dedt << std::endl;
345 dn.SetLinearForm(dtg_dt, TG, -1, To);
347 DNormal.SetLinearForm(-(n*dn), n, dn);
349 DBiNormal.SetLinearForm(Tangent.Crossed(DNormal),
350 DTangent.Crossed(Normal));
355 // plan ortho a la trajectoire
356 Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
359 myStatus = GeomFill_PlaneNotIntersectGuide;
360 return Standard_False;
363 return Standard_True;
367 //=======================================================================
369 //purpose : calculation of trihedron and derivatives
370 //=======================================================================
371 Standard_Boolean GeomFill_GuideTrihedronPlan::D2(const Standard_Real Param,
384 // gp_Vec To,DTo,TG,DTG;
387 myCurve->D2(Param, P, To, DTo);
389 // triedre de Frenet sur la trajectoire
390 frenet->D2(Param,Tangent,DTangent,D2Tangent,
391 Normal,DNormal,D2Normal,
392 BiNormal,DBiNormal,D2BiNormal);
395 // plan ortho a Tangent pour trouver la pt Pprime sur le guide
396 Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
397 Handle(GeomAdaptor_HSurface) Pl= new(GeomAdaptor_HSurface)(Plan);
400 Standard_Integer Iter = 50;
401 // fonction dont il faut trouver la racine : G(W) - Pl(U,V)=0
402 GeomFill_FunctionPipe E(Pl , myGuide);
406 math_FunctionSetRoot Result(E, X, XTol,
411 R = Result.Root(); // solution
412 myTrimG->D2(R(1), PG, TG, DTG);
414 gp_Vec n (P, PG); // vecteur definissant la normale du triedre
415 Standard_Real Norm = n.Magnitude();
417 Normal = n.Normalized();
418 BiNormal = Tangent.Crossed(Normal);
422 // derivee premiere du triedre
423 Standard_Real dtp_dt;
424 dtp_dt = (To*Tangent - Norm*(n*DTangent))/(Tangent*TG);
426 dn.SetLinearForm(dtp_dt, TG, -1, To);
428 DNormal.SetLinearForm(-(n*dn), n, dn);
430 DBiNormal = Tangent.Crossed(DNormal) + DTangent.Crossed(Normal);
432 // derivee seconde du triedre
433 Standard_Real d2tp_dt2;
434 d2tp_dt2 = (DTo*Tangent+To*DTangent - dn*DTangent-Norm*n*D2Tangent)/(TG*Tangent)
435 - (To*Tangent-Norm*n*DTangent) * (DTG*dtp_dt*Tangent+TG*DTangent)
436 / ((TG*Tangent)*(TG*Tangent));
439 d2n.SetLinearForm(dtp_dt*dtp_dt, DTG, d2tp_dt2, TG, -DTo);
443 D2Normal.SetLinearForm(3*Pow(n*dn,2)- (dn.SquareMagnitude() + n*d2n), n,
447 D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal),
448 2, DTangent.Crossed(DNormal),
449 Tangent.Crossed(D2Normal));
456 myStatus = GeomFill_PlaneNotIntersectGuide;
457 return Standard_False;
460 // return Standard_True;
461 return Standard_False;
465 //=======================================================================
468 //=======================================================================
469 Handle(GeomFill_TrihedronLaw) GeomFill_GuideTrihedronPlan::Copy() const
471 Handle(GeomFill_GuideTrihedronPlan) copy =
472 new (GeomFill_GuideTrihedronPlan) (myGuide);
473 copy->SetCurve(myCurve);
477 //=======================================================================
478 //function : ErrorStatus
480 //=======================================================================
481 GeomFill_PipeError GeomFill_GuideTrihedronPlan::ErrorStatus() const
487 //=======================================================================
488 //function : NbIntervals
489 //purpose : Version provisoire : Il faut tenir compte du guide
490 //=======================================================================
491 Standard_Integer GeomFill_GuideTrihedronPlan::NbIntervals(const GeomAbs_Shape S)const
496 case GeomAbs_C0: tmpS = GeomAbs_C1; break;
497 case GeomAbs_C1: tmpS = GeomAbs_C2; break;
498 case GeomAbs_C2: tmpS = GeomAbs_C3; break;
499 default: tmpS = GeomAbs_CN;
502 Nb = myCurve->NbIntervals(tmpS);
505 //======================================================================
506 //function :Intervals
508 //=======================================================================
509 void GeomFill_GuideTrihedronPlan::Intervals(TColStd_Array1OfReal& TT,
510 const GeomAbs_Shape S) const
514 case GeomAbs_C0: tmpS = GeomAbs_C1; break;
515 case GeomAbs_C1: tmpS = GeomAbs_C2; break;
516 case GeomAbs_C2: tmpS = GeomAbs_C3; break;
517 default: tmpS = GeomAbs_CN;
519 myCurve->Intervals(TT, tmpS);
522 //======================================================================
523 //function :SetInterval
525 //=======================================================================
526 void GeomFill_GuideTrihedronPlan::SetInterval(const Standard_Real First,
527 const Standard_Real Last)
529 myTrimmed = myCurve->Trim(First, Last, Precision::Confusion());
533 //=======================================================================
534 //function : GetAverageLaw
536 //=======================================================================
537 void GeomFill_GuideTrihedronPlan::GetAverageLaw(gp_Vec& ATangent,
542 Standard_Real t, Delta = (myCurve->LastParameter() -
543 myCurve->FirstParameter())/20.001;
545 ATangent.SetCoord(0.,0.,0.);
546 ANormal.SetCoord(0.,0.,0.);
547 ABiNormal.SetCoord(0.,0.,0.);
550 for (ii=1; ii<=20; ii++) {
551 t = myCurve->FirstParameter() +(ii-1)*Delta;
562 //=======================================================================
563 //function : IsConstant
565 //=======================================================================
566 Standard_Boolean GeomFill_GuideTrihedronPlan::IsConstant() const
568 if ((myCurve->GetType() == GeomAbs_Line) &&
569 (myGuide->GetType() == GeomAbs_Line)) {
571 Angle = myCurve->Line().Angle(myGuide->Line());
572 if ((Angle<1.e-12) || ((2*M_PI-Angle)<1.e-12) )
573 return Standard_True;
576 return Standard_False;
579 //=======================================================================
580 //function : IsOnlyBy3dCurve
582 //=======================================================================
583 Standard_Boolean GeomFill_GuideTrihedronPlan::IsOnlyBy3dCurve() const
585 return Standard_False;
588 //=======================================================================
590 //purpose : Nothing!!
591 //=======================================================================
592 void GeomFill_GuideTrihedronPlan::Origine(const Standard_Real ,
593 const Standard_Real )
597 //==================================================================
599 //Purpose : recherche par interpolation d'une valeur initiale
600 //==================================================================
601 void GeomFill_GuideTrihedronPlan::InitX(const Standard_Real Param)
604 Standard_Integer Ideb = 1, Ifin = Pole->RowLength(), Idemi;
605 Standard_Real Valeur, t1, t2;
608 Valeur = Pole->Value(1, Ideb).X();
609 if (Param == Valeur) {
613 Valeur = Pole->Value(1, Ifin).X();
614 if (Param == Valeur) {
618 while ( Ideb+1 != Ifin) {
619 Idemi = (Ideb+Ifin)/2;
620 Valeur = Pole->Value(1, Idemi).X();
621 if (Valeur < Param) {
625 if ( Valeur > Param) { Ifin = Idemi;}
633 t1 = Pole->Value(1,Ideb).X();
634 t2 = Pole->Value(1,Ifin).X();
635 Standard_Real diff = t2-t1;
637 Standard_Real b = (Param-t1) / diff,
638 a = (t2-Param) / diff;
640 X(1) = Pole->Value(1,Ideb).Coord(2) * a
641 + Pole->Value(1,Ifin).Coord(2) * b; //param guide
644 X(1) = (Pole->Value(1, Ideb).Coord(2) +
645 Pole->Value(1, Ifin).Coord(2)) / 2;
647 if (myGuide->IsPeriodic()) {
648 X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(),
649 myGuide->LastParameter());