[occt.git] / src / GeomFill / GeomFill_GuideTrihedronPlan.cxx

// Created on: 1998-07-02
// Created by: Stephanie HUMEAU
// Copyright (c) 1998-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.


#include <Adaptor3d_Curve.hxx>
#include <Adaptor3d_HCurve.hxx>
#include <ElCLib.hxx>
#include <Geom_Plane.hxx>
#include <GeomAdaptor_HCurve.hxx>
#include <GeomAdaptor_HSurface.hxx>
#include <GeomFill_Frenet.hxx>
#include <GeomFill_GuideTrihedronPlan.hxx>
#include <GeomFill_PlanFunc.hxx>
#include <GeomFill_TrihedronLaw.hxx>
#include <gp_Pnt.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec.hxx>
#include <IntCurveSurface_HInter.hxx>
#include <IntCurveSurface_IntersectionPoint.hxx>
#include <math_FunctionRoot.hxx>
#include <math_Matrix.hxx>
#include <math_Vector.hxx>
#include <Precision.hxx>
#include <Standard_ConstructionError.hxx>
#include <Standard_OutOfRange.hxx>
#include <Standard_Type.hxx>

IMPLEMENT_STANDARD_RTTIEXT(GeomFill_GuideTrihedronPlan,GeomFill_TrihedronWithGuide)

//#include <gp_Trsf2d.hxx>
//#include <Bnd_Box2d.hxx>
#if DRAW
#include <DrawTrSurf.hxx>
#endif

#ifdef OCCT_DEBUG
static void TracePlan(const Handle(Geom_Surface)& /*Plan*/)
{
  std::cout << "Pas d'intersection Guide/Plan" << std::endl;	
#if DRAW
  char* Temp = "ThePlan" ;
  DrawTrSurf::Set(Temp, Plan);
//  DrawTrSurf::Set("ThePlan", Plan);
#endif 
}
#endif

//==================================================================
//Function: InGoodPeriod
//Purpose : Recadre un paramtere
//==================================================================
static void InGoodPeriod(const Standard_Real Prec,
			 const Standard_Real  Period,
                         Standard_Real& Current)
{
  Standard_Real Diff=Current-Prec;
  Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
  Current -= nb*Period;
  Diff = Current-Prec;
  if (Diff > Period/2) Current -= Period;
  else if (Diff < -Period/2) Current += Period;
}

//=======================================================================
//function : GuideTrihedronPlan
//purpose  : Constructor
//=======================================================================
GeomFill_GuideTrihedronPlan::GeomFill_GuideTrihedronPlan (const Handle(Adaptor3d_HCurve)& theGuide) :
							  X(1,1),  
							  XTol(1,1),
							  Inf(1,1), Sup(1,1),
							  myStatus(GeomFill_PipeOk)
{
  myCurve.Nullify();
  myGuide = theGuide; // guide
  myTrimG = theGuide;
  myNbPts = 20;     // nb points pour calculs
  Pole = new (TColgp_HArray2OfPnt2d)(1,1,1,myNbPts);//tab pr stocker Pprime (pt sur guide)
  frenet = new (GeomFill_Frenet)();
  XTol.Init(1.e-6);
  XTol(1) = myGuide->Resolution(1.e-6);
}

//=======================================================================
//function : Init
//purpose  : calcule myNbPts points sur la courbe guide (<=> normale)
//=======================================================================
 void GeomFill_GuideTrihedronPlan::Init()
{
  myStatus = GeomFill_PipeOk;
  gp_Pnt P;
//  Bnd_Box2d Box;
//  Box.Update(-0.1, -0.1, 0.1, 0.1); // Taille minimal
  gp_Vec Tangent,Normal,BiNormal;
  Standard_Integer ii;
  Standard_Real t, DeltaG, w = 0.;
  Standard_Real f = myCurve->FirstParameter();
  Standard_Real l = myCurve->LastParameter();
  

  Handle(Geom_Plane) Plan;
  Handle(GeomAdaptor_HSurface) Pl;
  IntCurveSurface_IntersectionPoint PInt;
  IntCurveSurface_HInter Int;
  frenet->SetCurve(myCurve);
  DeltaG = (myGuide->LastParameter() -  myGuide->FirstParameter())/2;
  
  Inf(1) = myGuide->FirstParameter() - DeltaG;
  Sup(1) = myGuide->LastParameter() + DeltaG;

  if (!myGuide->IsPeriodic()) {
    myTrimG = myGuide->Trim(myGuide->FirstParameter()- DeltaG/100, 
			    myGuide->LastParameter() + DeltaG/100, 
			    DeltaG*1.e-7);
  }
  else {
    myTrimG = myGuide; 
  }
//  Standard_Real Step = DeltaG/100;
  DeltaG /= 3;
  for (ii=1; ii<=myNbPts; ii++) 
    {
      t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
      t /= (myNbPts-1);
      myCurve->D0(t, P); 
      frenet->D0(t, Tangent, Normal, BiNormal);
      Plan = new (Geom_Plane) (P, Tangent);
      Pl = new(GeomAdaptor_HSurface) (Plan);

      Int.Perform(myTrimG, Pl); // intersection plan / guide 
      if (Int.NbPoints() == 0) {
#ifdef OCCT_DEBUG
	TracePlan(Plan);
#endif
        w = (fabs(myGuide->LastParameter() -w) > fabs(myGuide->FirstParameter()-w) ? myGuide->FirstParameter() : myGuide->LastParameter());
                                                                                   
        myStatus = GeomFill_PlaneNotIntersectGuide;
	//return;
      }
      else 
	{
	  gp_Pnt Pmin;
	  PInt = Int.Point(1);
	  Pmin = PInt.Pnt();
	  Standard_Real Dmin = P.Distance(Pmin);
	  for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++)
	    {
	      Pmin = Int.Point(jj).Pnt();
	      if (P.Distance(Pmin) < Dmin) 
		{
		  PInt = Int.Point(jj);
		  Dmin = P.Distance(Pmin);
		}
	    }//for_jj
          
	  w = PInt.W();
        }
      if (ii>1) {
        Standard_Real Diff = w -  Pole->Value(1, ii-1).Y();
        if (Abs(Diff) > DeltaG) {
          if (myGuide->IsPeriodic()) {
            InGoodPeriod (Pole->Value(1, ii-1).Y(),
                          myGuide->Period(), w);
            
            Diff =  w -  Pole->Value(1, ii-1).Y();
          }
        }
        
#ifdef OCCT_DEBUG
        if (Abs(Diff) > DeltaG) {
          std::cout << "Trihedron Plan Diff on Guide : " << 
            Diff << std::endl;
        }
#endif
      }
      
      gp_Pnt2d p1(t, w); // on stocke les parametres
      Pole->SetValue(1, ii, p1);
      
    }// for_ii
}

//=======================================================================
//function : SetCurve
//purpose  : calculation of trihedron
//=======================================================================
void GeomFill_GuideTrihedronPlan::SetCurve(const Handle(Adaptor3d_HCurve)& C)
{
  myCurve = C;
  if (!myCurve.IsNull()) Init();
}

//=======================================================================
//function : Guide
//purpose  : calculation of trihedron
//=======================================================================

 Handle(Adaptor3d_HCurve) GeomFill_GuideTrihedronPlan::Guide()const
{
  return myGuide;
}

//=======================================================================
//function : D0
//purpose  : calculation of trihedron
//=======================================================================
 Standard_Boolean GeomFill_GuideTrihedronPlan::D0(const Standard_Real Param,
						  gp_Vec& Tangent,
						  gp_Vec& Normal,
						  gp_Vec& BiNormal) 
{ 
  gp_Pnt P, Pprime;
//  gp_Vec To;

  myCurve->D0(Param, P); 

  frenet->D0(Param,Tangent,Normal,BiNormal);
  
  //initialisation de la recherche
  InitX(Param);
      
  Standard_Integer Iter = 50;
  
  // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
  GeomFill_PlanFunc E(P, Tangent, myGuide);
  
  // resolution
  math_FunctionRoot Result(E, X(1), XTol(1), 
			   Inf(1), Sup(1), Iter); 
  
  if (Result.IsDone()) 
    {
      Standard_Real Res = Result.Root();
//      R = Result.Root();    // solution    
   
      Pprime = myTrimG->Value(Res); // pt sur courbe guide 
      gp_Vec n (P, Pprime); // vecteur definissant la normale du triedre 
            
      Normal = n.Normalized();
      BiNormal = Tangent.Crossed(Normal);
      BiNormal.Normalize();
    }
  else { // Erreur...
#ifdef OCCT_DEBUG
    std::cout << "D0 :";
    // plan ortho a la trajectoire pour determiner Pprime
    Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
    TracePlan(Plan);
#endif
    myStatus = GeomFill_PlaneNotIntersectGuide;
    return Standard_False;
  }
  
  return Standard_True;
}

//=======================================================================
//function : D1
//purpose  : calculation of trihedron and first derivative
//=======================================================================
 Standard_Boolean GeomFill_GuideTrihedronPlan::D1(const Standard_Real Param,
						  gp_Vec& Tangent,
						  gp_Vec& DTangent,
						  gp_Vec& Normal,
						  gp_Vec& DNormal,
						  gp_Vec& BiNormal,	   
						  gp_Vec& DBiNormal) 
{ 
//  return Standard_False;
  gp_Pnt P, PG;
  gp_Vec To,TG;


  // triedre de frenet sur la trajectoire
  myCurve->D1(Param, P, To);
  frenet->D1(Param,Tangent,DTangent,Normal,DNormal,BiNormal,DBiNormal);

 
  // tolerance sur E
  Standard_Integer Iter = 50;

  // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
  InitX(Param);
  GeomFill_PlanFunc E(P, Tangent, myGuide);

  // resolution
  math_FunctionRoot Result(E, X(1), XTol(1), 
			   Inf(1), Sup(1), Iter);

  if (Result.IsDone()) 
    {
      Standard_Real Res =  Result.Root();
//      R = Result.Root();    // solution  
      myTrimG->D1(Res, PG, TG);
      gp_Vec n (P, PG), dn; // vecteur definissant la normale du triedre
      Standard_Real Norm = n.Magnitude();
      if (Norm < 1.e-12) {
	Norm = 1.0;
      }
      n /=Norm;

      
      Normal = n; 
      BiNormal = Tangent.Crossed(Normal);

// derivee premiere du triedre
      Standard_Real dedx, dedt, dtg_dt;
      E.Derivative(Res, dedx);
      E.DEDT(Res, To, DTangent, dedt);
      dtg_dt = -dedt/dedx;


/*      Standard_Real h=1.e-7, e, etg, etc;
      E.Value(Res, e);
      E.Value(Res+h, etg);
      if ( Abs( (etg-e)/h - dedx) > 1.e-4) {
	std::cout << "err :" <<  (etg-e)/h - dedx << std::endl;
      }
      gp_Pnt pdbg;
      gp_Vec td, nb, bnb;
      myCurve->D0(Param+h, pdbg);      
      frenet->D0(Param+h,td, nb, bnb);

      GeomFill_PlanFunc Edeb(pdbg, td, myGuide); 
      Edeb.Value(Res, etc);
      if ( Abs( (etc-e)/h - dedt) > 1.e-4) {
	std::cout << "err :" <<  (etc-e)/h - dedt << std::endl;
      } */           

      dn.SetLinearForm(dtg_dt, TG, -1, To);
      
      DNormal.SetLinearForm(-(n*dn), n, dn);
      DNormal /= Norm;
      DBiNormal.SetLinearForm(Tangent.Crossed(DNormal),
			      DTangent.Crossed(Normal));
    }
  else {// Erreur...
#ifdef OCCT_DEBUG
    std::cout << "D1 :";
    // plan ortho a la trajectoire
    Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
    TracePlan(Plan);
#endif
    myStatus = GeomFill_PlaneNotIntersectGuide;
    return Standard_False;
  }
 
  return Standard_True; 
}


//=======================================================================
//function : D2
//purpose  : calculation of trihedron and derivatives
//=======================================================================
 Standard_Boolean GeomFill_GuideTrihedronPlan::D2(const Standard_Real Param,
						  gp_Vec& Tangent,
						  gp_Vec& DTangent,
						  gp_Vec& D2Tangent,
						  gp_Vec& Normal,
						  gp_Vec& DNormal,
						  gp_Vec& D2Normal,
						  gp_Vec& BiNormal,			  
						  gp_Vec& DBiNormal,		  
						  gp_Vec& D2BiNormal) 
{ 
//  gp_Pnt P, PG;
  gp_Pnt P;
//  gp_Vec To,DTo,TG,DTG;
  gp_Vec To,DTo;

  myCurve->D2(Param, P, To, DTo); 

  // triedre de Frenet sur la trajectoire
  frenet->D2(Param,Tangent,DTangent,D2Tangent,
	     Normal,DNormal,D2Normal,
	     BiNormal,DBiNormal,D2BiNormal);

/*
  // plan ortho a Tangent pour trouver la pt Pprime sur le guide
  Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent); 
  Handle(GeomAdaptor_HSurface) Pl= new(GeomAdaptor_HSurface)(Plan);
  

  Standard_Integer Iter = 50;
  // fonction dont il faut trouver la racine : G(W) - Pl(U,V)=0
  GeomFill_FunctionPipe E(Pl , myGuide);
  InitX(Param);
  
  // resolution
  math_FunctionSetRoot Result(E, X, XTol, 
				    Inf, Sup, Iter); 
  if (Result.IsDone()) 
    {
      math_Vector R(1,3); 
      R = Result.Root();    // solution 
      myTrimG->D2(R(1), PG, TG, DTG); 

      gp_Vec n (P, PG); // vecteur definissant la normale du triedre
      Standard_Real Norm = n.Magnitude();
      n /= Norm;      
      Normal = n.Normalized(); 
      BiNormal = Tangent.Crossed(Normal);

 
   // derivee premiere du triedre 
      Standard_Real dtp_dt;
      dtp_dt = (To*Tangent - Norm*(n*DTangent))/(Tangent*TG);
      gp_Vec dn, d2n;
      dn.SetLinearForm(dtp_dt, TG, -1,  To);
      
      DNormal.SetLinearForm(-(n*dn), n, dn);
      DNormal /= Norm;  
      DBiNormal = Tangent.Crossed(DNormal) + DTangent.Crossed(Normal);
  
    // derivee seconde du triedre
      Standard_Real d2tp_dt2;
      d2tp_dt2 = (DTo*Tangent+To*DTangent - dn*DTangent-Norm*n*D2Tangent)/(TG*Tangent)
	- (To*Tangent-Norm*n*DTangent) * (DTG*dtp_dt*Tangent+TG*DTangent)
	  / ((TG*Tangent)*(TG*Tangent));


      d2n.SetLinearForm(dtp_dt*dtp_dt, DTG, d2tp_dt2, TG, -DTo);
      dn/=Norm;
      d2n/=Norm;

      D2Normal.SetLinearForm(3*Pow(n*dn,2)- (dn.SquareMagnitude() + n*d2n), n,
			     -2*(n*dn), dn,
			     d2n);
 
      D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal),
			       2, DTangent.Crossed(DNormal), 
			       Tangent.Crossed(D2Normal));
    }
  else {// Erreur...
#ifdef OCCT_DEBUG
    std::cout << "D2 :";
    TracePlan(Plan);
#endif
    myStatus = GeomFill_PlaneNotIntersectGuide;
    return Standard_False;
   }
*/
//  return Standard_True;
 return Standard_False;
}


//=======================================================================
//function : Copy
//purpose  : 
//=======================================================================
 Handle(GeomFill_TrihedronLaw) GeomFill_GuideTrihedronPlan::Copy() const
{
 Handle(GeomFill_GuideTrihedronPlan) copy = 
   new (GeomFill_GuideTrihedronPlan) (myGuide);
 copy->SetCurve(myCurve);
 return copy;
}

//=======================================================================
//function : ErrorStatus
//purpose  : 
//=======================================================================
 GeomFill_PipeError GeomFill_GuideTrihedronPlan::ErrorStatus() const
{
  return myStatus;
}
 

//=======================================================================
//function : NbIntervals
//purpose  : Version provisoire : Il faut tenir compte du guide
//=======================================================================
 Standard_Integer GeomFill_GuideTrihedronPlan::NbIntervals(const GeomAbs_Shape S)const
{
  Standard_Integer Nb;
  GeomAbs_Shape tmpS;
  switch (S) {
  case GeomAbs_C0: tmpS = GeomAbs_C1; break;
  case GeomAbs_C1: tmpS = GeomAbs_C2; break;
  case GeomAbs_C2: tmpS = GeomAbs_C3; break;
  default: tmpS = GeomAbs_CN;
  }  

  Nb = myCurve->NbIntervals(tmpS);
  return Nb;
}
//======================================================================
//function :Intervals
//purpose  : 
//=======================================================================
 void GeomFill_GuideTrihedronPlan::Intervals(TColStd_Array1OfReal& TT,
					    const GeomAbs_Shape S) const
{
  GeomAbs_Shape tmpS;
  switch (S) {
  case GeomAbs_C0: tmpS = GeomAbs_C1; break;
  case GeomAbs_C1: tmpS = GeomAbs_C2; break;
  case GeomAbs_C2: tmpS = GeomAbs_C3; break;
  default: tmpS = GeomAbs_CN;
  }
  myCurve->Intervals(TT, tmpS);
}

//======================================================================
//function :SetInterval
//purpose  : 
//=======================================================================
void GeomFill_GuideTrihedronPlan::SetInterval(const Standard_Real First,
					      const Standard_Real Last) 
{
  myTrimmed = myCurve->Trim(First, Last, Precision::Confusion());  
}


//=======================================================================
//function : GetAverageLaw
//purpose  : 
//=======================================================================
 void GeomFill_GuideTrihedronPlan::GetAverageLaw(gp_Vec& ATangent,
				    gp_Vec& ANormal,
				    gp_Vec& ABiNormal) 
{
  Standard_Integer ii;
  Standard_Real t, Delta = (myCurve->LastParameter() - 
			    myCurve->FirstParameter())/20.001;

  ATangent.SetCoord(0.,0.,0.);
  ANormal.SetCoord(0.,0.,0.);
  ABiNormal.SetCoord(0.,0.,0.);
  gp_Vec T, N, B;
  
  for (ii=1; ii<=20; ii++) {
    t = myCurve->FirstParameter() +(ii-1)*Delta;
    D0(t, T, N, B);
    ATangent +=T;
    ANormal  +=N;
    ABiNormal+=B;
  }
  ATangent  /= 20;
  ANormal   /= 20;
  ABiNormal /= 20; 
}

//=======================================================================
//function : IsConstant
//purpose  : 
//=======================================================================
 Standard_Boolean GeomFill_GuideTrihedronPlan::IsConstant() const
{
  if ((myCurve->GetType() == GeomAbs_Line) &&
      (myGuide->GetType() == GeomAbs_Line)) {
    Standard_Real Angle;
    Angle = myCurve->Line().Angle(myGuide->Line());
    if ((Angle<1.e-12) || ((2*M_PI-Angle)<1.e-12) )
      return Standard_True;
  }

  return Standard_False;
}

//=======================================================================
//function : IsOnlyBy3dCurve
//purpose  : 
//=======================================================================
 Standard_Boolean GeomFill_GuideTrihedronPlan::IsOnlyBy3dCurve() const
{
  return Standard_False;
}

//=======================================================================
//function : Origine
//purpose  : Nothing!!
//=======================================================================
void  GeomFill_GuideTrihedronPlan::Origine(const Standard_Real ,
					   const Standard_Real  )
{
}

//==================================================================
//Function : InitX
//Purpose : recherche par interpolation d'une valeur initiale
//==================================================================
void GeomFill_GuideTrihedronPlan::InitX(const Standard_Real Param)
{

  Standard_Integer Ideb = 1, Ifin =  Pole->RowLength(), Idemi;
  Standard_Real Valeur, t1, t2;

  
  Valeur = Pole->Value(1, Ideb).X();
  if (Param == Valeur) {
    Ifin = Ideb+1; 
  }
  
  Valeur =  Pole->Value(1, Ifin).X();
  if (Param == Valeur) {
    Ideb = Ifin-1; 
  } 
  
  while ( Ideb+1 != Ifin) {
    Idemi = (Ideb+Ifin)/2;
    Valeur = Pole->Value(1, Idemi).X();
    if (Valeur < Param) {
      Ideb = Idemi;
    }
    else { 
      if ( Valeur > Param) { Ifin = Idemi;}
      else { 
	Ideb = Idemi;		     
	Ifin = Ideb+1;
      }
    }
  }

  t1 = Pole->Value(1,Ideb).X();
  t2 = Pole->Value(1,Ifin).X();
  Standard_Real diff = t2-t1;
  if (diff > 1.e-7) {
    Standard_Real b = (Param-t1) / diff,
    a = (t2-Param) / diff;

    X(1) = Pole->Value(1,Ideb).Coord(2) * a 
      + Pole->Value(1,Ifin).Coord(2) * b; //param guide 
  }
  else {
    X(1) = (Pole->Value(1, Ideb).Coord(2) + 
	    Pole->Value(1, Ifin).Coord(2)) / 2;
  }
  if (myGuide->IsPeriodic()) {
    X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(), 
			          myGuide->LastParameter());
  }
}
Commit	Line	Data
b311480e	1	// Created on: 1998-07-02
	2	// Created by: Stephanie HUMEAU
	3	// Copyright (c) 1998-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
d5f74e42	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
973c2be1	15	// commercial license or contractual agreement.
7fd59977	16
7fd59977	17
7fd59977	18	#include <Adaptor3d_Curve.hxx>
42cf5bc1	19	#include <Adaptor3d_HCurve.hxx>
	20	#include <ElCLib.hxx>
	21	#include <Geom_Plane.hxx>
7fd59977	22	#include <GeomAdaptor_HCurve.hxx>
7fd59977	23	#include <GeomAdaptor_HSurface.hxx>
7fd59977	24	#include <GeomFill_Frenet.hxx>
42cf5bc1	25	#include <GeomFill_GuideTrihedronPlan.hxx>
7fd59977	26	#include <GeomFill_PlanFunc.hxx>
42cf5bc1	27	#include <GeomFill_TrihedronLaw.hxx>
	28	#include <gp_Pnt.hxx>
	29	#include <gp_Pnt2d.hxx>
	30	#include <gp_Vec.hxx>
	31	#include <IntCurveSurface_HInter.hxx>
	32	#include <IntCurveSurface_IntersectionPoint.hxx>
7fd59977	33	#include <math_FunctionRoot.hxx>
7fd59977	34	#include <math_Matrix.hxx>
42cf5bc1	35	#include <math_Vector.hxx>
7fd59977	36	#include <Precision.hxx>
42cf5bc1	37	#include <Standard_ConstructionError.hxx>
	38	#include <Standard_OutOfRange.hxx>
	39	#include <Standard_Type.hxx>
7fd59977	40
92efcf78	41	IMPLEMENT_STANDARD_RTTIEXT(GeomFill_GuideTrihedronPlan,GeomFill_TrihedronWithGuide)
92efcf78	42
42cf5bc1	43	//#include <gp_Trsf2d.hxx>
42cf5bc1	44	//#include <Bnd_Box2d.hxx>
7fd59977	45	#if DRAW
	46	#include <DrawTrSurf.hxx>
	47	#endif
	48
0797d9d3	49	#ifdef OCCT_DEBUG
35e08fe8	50	static void TracePlan(const Handle(Geom_Surface)& /Plan/)
7fd59977	51	{
04232180	52	std::cout << "Pas d'intersection Guide/Plan" << std::endl;
7fd59977	53	#if DRAW
	54	char* Temp = "ThePlan" ;
	55	DrawTrSurf::Set(Temp, Plan);
	56	// DrawTrSurf::Set("ThePlan", Plan);
	57	#endif
	58	}
	59	#endif
	60
	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)
	68	{
	69	Standard_Real Diff=Current-Prec;
	70	Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
	71	Current -= nb*Period;
	72	Diff = Current-Prec;
	73	if (Diff > Period/2) Current -= Period;
	74	else if (Diff < -Period/2) Current += Period;
	75	}
	76
	77	//=======================================================================
	78	//function : GuideTrihedronPlan
	79	//purpose : Constructor
	80	//=======================================================================
	81	GeomFill_GuideTrihedronPlan::GeomFill_GuideTrihedronPlan (const Handle(Adaptor3d_HCurve)& theGuide) :
	82	X(1,1),
	83	XTol(1,1),
	84	Inf(1,1), Sup(1,1),
	85	myStatus(GeomFill_PipeOk)
	86	{
	87	myCurve.Nullify();
	88	myGuide = theGuide; // guide
	89	myTrimG = theGuide;
	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)();
	93	XTol.Init(1.e-6);
	94	XTol(1) = myGuide->Resolution(1.e-6);
	95	}
	96
	97	//=======================================================================
	98	//function : Init
	99	//purpose : calcule myNbPts points sur la courbe guide (<=> normale)
	100	//=======================================================================
	101	void GeomFill_GuideTrihedronPlan::Init()
	102	{
	103	myStatus = GeomFill_PipeOk;
	104	gp_Pnt P;
	105	// Bnd_Box2d Box;
	106	// Box.Update(-0.1, -0.1, 0.1, 0.1); // Taille minimal
	107	gp_Vec Tangent,Normal,BiNormal;
	108	Standard_Integer ii;
1d47d8d0	109	Standard_Real t, DeltaG, w = 0.;
7fd59977	110	Standard_Real f = myCurve->FirstParameter();
	111	Standard_Real l = myCurve->LastParameter();
	112
	113
	114
	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;
	121
	122	Inf(1) = myGuide->FirstParameter() - DeltaG;
	123	Sup(1) = myGuide->LastParameter() + DeltaG;
	124
	125	if (!myGuide->IsPeriodic()) {
	126	myTrimG = myGuide->Trim(myGuide->FirstParameter()- DeltaG/100,
	127	myGuide->LastParameter() + DeltaG/100,
	128	DeltaG*1.e-7);
	129	}
	130	else {
	131	myTrimG = myGuide;
	132	}
	133	// Standard_Real Step = DeltaG/100;
	134	DeltaG /= 3;
	135	for (ii=1; ii<=myNbPts; ii++)
	136	{
	137	t = Standard_Real(myNbPts - ii)f + Standard_Real(ii - 1)l;
	138	t /= (myNbPts-1);
	139	myCurve->D0(t, P);
	140	frenet->D0(t, Tangent, Normal, BiNormal);
	141	Plan = new (Geom_Plane) (P, Tangent);
	142	Pl = new(GeomAdaptor_HSurface) (Plan);
	143
	144	Int.Perform(myTrimG, Pl); // intersection plan / guide
	145	if (Int.NbPoints() == 0) {
0797d9d3	146	#ifdef OCCT_DEBUG
7fd59977	147	TracePlan(Plan);
	148	#endif
	149	w = (fabs(myGuide->LastParameter() -w) > fabs(myGuide->FirstParameter()-w) ? myGuide->FirstParameter() : myGuide->LastParameter());
	150
	151	myStatus = GeomFill_PlaneNotIntersectGuide;
	152	//return;
	153	}
	154	else
	155	{
	156	gp_Pnt Pmin;
	157	PInt = Int.Point(1);
	158	Pmin = PInt.Pnt();
	159	Standard_Real Dmin = P.Distance(Pmin);
	160	for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++)
	161	{
	162	Pmin = Int.Point(jj).Pnt();
	163	if (P.Distance(Pmin) < Dmin)
	164	{
	165	PInt = Int.Point(jj);
	166	Dmin = P.Distance(Pmin);
	167	}
	168	}//for_jj
	169
	170	w = PInt.W();
	171	}
	172	if (ii>1) {
	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);
	178
	179	Diff = w - Pole->Value(1, ii-1).Y();
	180	}
	181	}
	182
0797d9d3	183	#ifdef OCCT_DEBUG
7fd59977	184	if (Abs(Diff) > DeltaG) {
04232180	185	std::cout << "Trihedron Plan Diff on Guide : " <<
04232180	186	Diff << std::endl;
7fd59977	187	}
	188	#endif
	189	}
	190
	191	gp_Pnt2d p1(t, w); // on stocke les parametres
	192	Pole->SetValue(1, ii, p1);
	193
	194	}// for_ii
	195	}
	196
	197	//=======================================================================
	198	//function : SetCurve
	199	//purpose : calculation of trihedron
	200	//=======================================================================
	201	void GeomFill_GuideTrihedronPlan::SetCurve(const Handle(Adaptor3d_HCurve)& C)
	202	{
	203	myCurve = C;
	204	if (!myCurve.IsNull()) Init();
	205	}
	206
	207	//=======================================================================
	208	//function : Guide
	209	//purpose : calculation of trihedron
	210	//=======================================================================
	211
	212	Handle(Adaptor3d_HCurve) GeomFill_GuideTrihedronPlan::Guide()const
	213	{
	214	return myGuide;
	215	}
	216
	217	//=======================================================================
	218	//function : D0
	219	//purpose : calculation of trihedron
	220	//=======================================================================
	221	Standard_Boolean GeomFill_GuideTrihedronPlan::D0(const Standard_Real Param,
	222	gp_Vec& Tangent,
	223	gp_Vec& Normal,
	224	gp_Vec& BiNormal)
	225	{
	226	gp_Pnt P, Pprime;
	227	// gp_Vec To;
	228
	229	myCurve->D0(Param, P);
	230
	231	frenet->D0(Param,Tangent,Normal,BiNormal);
	232
	233	//initialisation de la recherche
	234	InitX(Param);
	235
	236	Standard_Integer Iter = 50;
	237
	238	// fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
	239	GeomFill_PlanFunc E(P, Tangent, myGuide);
	240
	241	// resolution
	242	math_FunctionRoot Result(E, X(1), XTol(1),
	243	Inf(1), Sup(1), Iter);
	244
	245	if (Result.IsDone())
	246	{
	247	Standard_Real Res = Result.Root();
	248	// R = Result.Root(); // solution
	249
	250	Pprime = myTrimG->Value(Res); // pt sur courbe guide
251	gp_Vec n (P, Pprime); // vecteur definissant la normale du triedre
252
253	Normal = n.Normalized();
254	BiNormal = Tangent.Crossed(Normal);
0be7dbe1	255	BiNormal.Normalize();
7fd59977	256	}
7fd59977	257	else { // Erreur...
0797d9d3	258	#ifdef OCCT_DEBUG
04232180	259	std::cout << "D0 :";
7fd59977	260	// plan ortho a la trajectoire pour determiner Pprime
	261	Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
	262	TracePlan(Plan);
	263	#endif
	264	myStatus = GeomFill_PlaneNotIntersectGuide;
	265	return Standard_False;
	266	}
	267
	268	return Standard_True;
	269	}
	270
	271	//=======================================================================
	272	//function : D1
	273	//purpose : calculation of trihedron and first derivative
	274	//=======================================================================
	275	Standard_Boolean GeomFill_GuideTrihedronPlan::D1(const Standard_Real Param,
	276	gp_Vec& Tangent,
	277	gp_Vec& DTangent,
	278	gp_Vec& Normal,
	279	gp_Vec& DNormal,
	280	gp_Vec& BiNormal,
	281	gp_Vec& DBiNormal)
	282	{
	283	// return Standard_False;
	284	gp_Pnt P, PG;
	285	gp_Vec To,TG;
	286
	287
	288
	289	// triedre de frenet sur la trajectoire
	290	myCurve->D1(Param, P, To);
	291	frenet->D1(Param,Tangent,DTangent,Normal,DNormal,BiNormal,DBiNormal);
	292
	293
	294	// tolerance sur E
	295	Standard_Integer Iter = 50;
	296
	297	// fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
	298	InitX(Param);
	299	GeomFill_PlanFunc E(P, Tangent, myGuide);
	300
	301	// resolution
	302	math_FunctionRoot Result(E, X(1), XTol(1),
	303	Inf(1), Sup(1), Iter);
	304
	305	if (Result.IsDone())
	306	{
	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();
	312	if (Norm < 1.e-12) {
	313	Norm = 1.0;
	314	}
	315	n /=Norm;
	316
	317
	318	Normal = n;
	319	BiNormal = Tangent.Crossed(Normal);
	320
	321	// derivee premiere du triedre
	322	Standard_Real dedx, dedt, dtg_dt;
	323	E.Derivative(Res, dedx);
324	E.DEDT(Res, To, DTangent, dedt);
325	dtg_dt = -dedt/dedx;
326
327
328	/* Standard_Real h=1.e-7, e, etg, etc;
329	E.Value(Res, e);
330	E.Value(Res+h, etg);
331	if ( Abs( (etg-e)/h - dedx) > 1.e-4) {
04232180	332	std::cout << "err :" << (etg-e)/h - dedx << std::endl;
7fd59977	333	}
	334	gp_Pnt pdbg;
	335	gp_Vec td, nb, bnb;
	336	myCurve->D0(Param+h, pdbg);
	337	frenet->D0(Param+h,td, nb, bnb);
	338
	339	GeomFill_PlanFunc Edeb(pdbg, td, myGuide);
	340	Edeb.Value(Res, etc);
	341	if ( Abs( (etc-e)/h - dedt) > 1.e-4) {
04232180	342	std::cout << "err :" << (etc-e)/h - dedt << std::endl;
7fd59977	343	} */
	344
	345	dn.SetLinearForm(dtg_dt, TG, -1, To);
	346
	347	DNormal.SetLinearForm(-(n*dn), n, dn);
	348	DNormal /= Norm;
	349	DBiNormal.SetLinearForm(Tangent.Crossed(DNormal),
	350	DTangent.Crossed(Normal));
	351	}
	352	else {// Erreur...
0797d9d3	353	#ifdef OCCT_DEBUG
04232180	354	std::cout << "D1 :";
7fd59977	355	// plan ortho a la trajectoire
	356	Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
	357	TracePlan(Plan);
	358	#endif
	359	myStatus = GeomFill_PlaneNotIntersectGuide;
	360	return Standard_False;
	361	}
	362
	363	return Standard_True;
	364	}
	365
	366
	367	//=======================================================================
	368	//function : D2
	369	//purpose : calculation of trihedron and derivatives
	370	//=======================================================================
	371	Standard_Boolean GeomFill_GuideTrihedronPlan::D2(const Standard_Real Param,
	372	gp_Vec& Tangent,
	373	gp_Vec& DTangent,
	374	gp_Vec& D2Tangent,
	375	gp_Vec& Normal,
	376	gp_Vec& DNormal,
	377	gp_Vec& D2Normal,
	378	gp_Vec& BiNormal,
	379	gp_Vec& DBiNormal,
	380	gp_Vec& D2BiNormal)
	381	{
	382	// gp_Pnt P, PG;
	383	gp_Pnt P;
	384	// gp_Vec To,DTo,TG,DTG;
	385	gp_Vec To,DTo;
	386
	387	myCurve->D2(Param, P, To, DTo);
	388
	389	// triedre de Frenet sur la trajectoire
	390	frenet->D2(Param,Tangent,DTangent,D2Tangent,
	391	Normal,DNormal,D2Normal,
	392	BiNormal,DBiNormal,D2BiNormal);
	393
	394	/*
	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);
	398
	399
	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);
	403	InitX(Param);
	404
	405	// resolution
	406	math_FunctionSetRoot Result(E, X, XTol,
	407	Inf, Sup, Iter);
	408	if (Result.IsDone())
	409	{
	410	math_Vector R(1,3);
	411	R = Result.Root(); // solution
	412	myTrimG->D2(R(1), PG, TG, DTG);
	413
	414	gp_Vec n (P, PG); // vecteur definissant la normale du triedre
	415	Standard_Real Norm = n.Magnitude();
	416	n /= Norm;
	417	Normal = n.Normalized();
	418	BiNormal = Tangent.Crossed(Normal);
419
420
421
422	// derivee premiere du triedre
423	Standard_Real dtp_dt;
424	dtp_dt = (ToTangent - Norm(nDTangent))/(TangentTG);
425	gp_Vec dn, d2n;
426	dn.SetLinearForm(dtp_dt, TG, -1, To);
427
428	DNormal.SetLinearForm(-(n*dn), n, dn);
429	DNormal /= Norm;
430	DBiNormal = Tangent.Crossed(DNormal) + DTangent.Crossed(Normal);
431
432	// derivee seconde du triedre
433	Standard_Real d2tp_dt2;
434	d2tp_dt2 = (DToTangent+ToDTangent - dnDTangent-NormnD2Tangent)/(TGTangent)
435	- (ToTangent-NormnDTangent) (DTGdtp_dtTangent+TG*DTangent)
436	/ ((TGTangent)(TG*Tangent));
437
438
439	d2n.SetLinearForm(dtp_dt*dtp_dt, DTG, d2tp_dt2, TG, -DTo);
440	dn/=Norm;
441	d2n/=Norm;
442
443	D2Normal.SetLinearForm(3Pow(ndn,2)- (dn.SquareMagnitude() + n*d2n), n,
444	-2(ndn), dn,
445	d2n);
446
447	D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal),
448	2, DTangent.Crossed(DNormal),
449	Tangent.Crossed(D2Normal));
450	}
451	else {// Erreur...
0797d9d3	452	#ifdef OCCT_DEBUG
04232180	453	std::cout << "D2 :";
7fd59977	454	TracePlan(Plan);
	455	#endif
	456	myStatus = GeomFill_PlaneNotIntersectGuide;
	457	return Standard_False;
	458	}
	459	*/
	460	// return Standard_True;
	461	return Standard_False;
	462	}
	463
	464
	465	//=======================================================================
	466	//function : Copy
	467	//purpose :
	468	//=======================================================================
	469	Handle(GeomFill_TrihedronLaw) GeomFill_GuideTrihedronPlan::Copy() const
	470	{
	471	Handle(GeomFill_GuideTrihedronPlan) copy =
	472	new (GeomFill_GuideTrihedronPlan) (myGuide);
	473	copy->SetCurve(myCurve);
	474	return copy;
	475	}
	476
	477	//=======================================================================
	478	//function : ErrorStatus
	479	//purpose :
	480	//=======================================================================
	481	GeomFill_PipeError GeomFill_GuideTrihedronPlan::ErrorStatus() const
	482	{
	483	return myStatus;
	484	}
	485
	486
	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
	492	{
	493	Standard_Integer Nb;
	494	GeomAbs_Shape tmpS;
	495	switch (S) {
	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;
	500	}
	501
	502	Nb = myCurve->NbIntervals(tmpS);
	503	return Nb;
	504	}
	505	//======================================================================
	506	//function :Intervals
	507	//purpose :
	508	//=======================================================================
	509	void GeomFill_GuideTrihedronPlan::Intervals(TColStd_Array1OfReal& TT,
	510	const GeomAbs_Shape S) const
	511	{
	512	GeomAbs_Shape tmpS;
	513	switch (S) {
	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;
518	}
519	myCurve->Intervals(TT, tmpS);
520	}
521
522	//======================================================================
523	//function :SetInterval
524	//purpose :
525	//=======================================================================
526	void GeomFill_GuideTrihedronPlan::SetInterval(const Standard_Real First,
527	const Standard_Real Last)
528	{
529	myTrimmed = myCurve->Trim(First, Last, Precision::Confusion());
530	}
531
532
533	//=======================================================================
534	//function : GetAverageLaw
535	//purpose :
536	//=======================================================================
537	void GeomFill_GuideTrihedronPlan::GetAverageLaw(gp_Vec& ATangent,
538	gp_Vec& ANormal,
539	gp_Vec& ABiNormal)
540	{
541	Standard_Integer ii;
542	Standard_Real t, Delta = (myCurve->LastParameter() -
543	myCurve->FirstParameter())/20.001;
544
545	ATangent.SetCoord(0.,0.,0.);
546	ANormal.SetCoord(0.,0.,0.);
547	ABiNormal.SetCoord(0.,0.,0.);
548	gp_Vec T, N, B;
549
b6abaec0	550	for (ii=1; ii<=20; ii++) {
7fd59977	551	t = myCurve->FirstParameter() +(ii-1)*Delta;
	552	D0(t, T, N, B);
	553	ATangent +=T;
	554	ANormal +=N;
	555	ABiNormal+=B;
	556	}
	557	ATangent /= 20;
	558	ANormal /= 20;
	559	ABiNormal /= 20;
	560	}
	561
	562	//=======================================================================
	563	//function : IsConstant
	564	//purpose :
	565	//=======================================================================
	566	Standard_Boolean GeomFill_GuideTrihedronPlan::IsConstant() const
	567	{
	568	if ((myCurve->GetType() == GeomAbs_Line) &&
	569	(myGuide->GetType() == GeomAbs_Line)) {
	570	Standard_Real Angle;
	571	Angle = myCurve->Line().Angle(myGuide->Line());
c6541a0c	572	if ((Angle<1.e-12) \|\| ((2*M_PI-Angle)<1.e-12) )
7fd59977	573	return Standard_True;
	574	}
	575
	576	return Standard_False;
	577	}
	578
	579	//=======================================================================
	580	//function : IsOnlyBy3dCurve
	581	//purpose :
	582	//=======================================================================
	583	Standard_Boolean GeomFill_GuideTrihedronPlan::IsOnlyBy3dCurve() const
	584	{
	585	return Standard_False;
	586	}
	587
	588	//=======================================================================
	589	//function : Origine
	590	//purpose : Nothing!!
	591	//=======================================================================
	592	void GeomFill_GuideTrihedronPlan::Origine(const Standard_Real ,
	593	const Standard_Real )
	594	{
	595	}
	596
	597	//==================================================================
	598	//Function : InitX
	599	//Purpose : recherche par interpolation d'une valeur initiale
	600	//==================================================================
	601	void GeomFill_GuideTrihedronPlan::InitX(const Standard_Real Param)
	602	{
	603
	604	Standard_Integer Ideb = 1, Ifin = Pole->RowLength(), Idemi;
	605	Standard_Real Valeur, t1, t2;
	606
	607
	608	Valeur = Pole->Value(1, Ideb).X();
	609	if (Param == Valeur) {
	610	Ifin = Ideb+1;
	611	}
	612
	613	Valeur = Pole->Value(1, Ifin).X();
	614	if (Param == Valeur) {
	615	Ideb = Ifin-1;
	616	}
	617
	618	while ( Ideb+1 != Ifin) {
	619	Idemi = (Ideb+Ifin)/2;
	620	Valeur = Pole->Value(1, Idemi).X();
	621	if (Valeur < Param) {
	622	Ideb = Idemi;
	623	}
	624	else {
	625	if ( Valeur > Param) { Ifin = Idemi;}
	626	else {
	627	Ideb = Idemi;
	628	Ifin = Ideb+1;
	629	}
	630	}
	631	}
	632
	633	t1 = Pole->Value(1,Ideb).X();
	634	t2 = Pole->Value(1,Ifin).X();
	635	Standard_Real diff = t2-t1;
	636	if (diff > 1.e-7) {
637	Standard_Real b = (Param-t1) / diff,
638	a = (t2-Param) / diff;
639
640	X(1) = Pole->Value(1,Ideb).Coord(2) * a
641	+ Pole->Value(1,Ifin).Coord(2) * b; //param guide
642	}
643	else {
644	X(1) = (Pole->Value(1, Ideb).Coord(2) +
645	Pole->Value(1, Ifin).Coord(2)) / 2;
646	}
647	if (myGuide->IsPeriodic()) {
648	X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(),
649	myGuide->LastParameter());
650	}
651	}