0030874: Modeling Algorithms - GCPnts_TangentialDeflection inserts the points between...

[occt.git] / src / GCPnts / GCPnts_TangentialDeflection.pxx

// Created on: 1996-11-08
// Created by: Jean Claude VAUTHIER
// Copyright (c) 1996-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 <GCPnts_DeflectionType.hxx>
#include <Standard_ConstructionError.hxx>
#include <Precision.hxx>
#include <gp_XYZ.hxx>
#include <gp_Pnt.hxx>
#include <gp_Vec.hxx>
#include <gp.hxx>
#include <NCollection_List.hxx>
#include <math_PSO.hxx>
#include <math_BrentMinimum.hxx>

#define Us3 0.3333333333333333333333333333

void GCPnts_TangentialDeflection::EvaluateDu (
  const TheCurve& C,
  const Standard_Real      U,
  gp_Pnt&   P,
  Standard_Real&     Du,
  Standard_Boolean&  NotDone) const {

  gp_Vec T, N;
  D2 (C, U, P, T, N);
  Standard_Real Lt   = T.Magnitude ();
  Standard_Real LTol = Precision::Confusion ();
  if (Lt > LTol && N.Magnitude () > LTol) {
    Standard_Real Lc = N.CrossMagnitude (T);
    Standard_Real Ln = Lc/Lt;
    if (Ln > LTol) {
      Du = sqrt (8.0 * Max(curvatureDeflection, myMinLen) / Ln);
      NotDone = Standard_False;
    }
  }
}


//=======================================================================
//function : GCPnts_TangentialDeflection
//purpose  : 
//=======================================================================

GCPnts_TangentialDeflection::GCPnts_TangentialDeflection (
 const TheCurve&        C,
 const Standard_Real    AngularDeflection,
 const Standard_Real    CurvatureDeflection,
 const Standard_Integer MinimumOfPoints,
 const Standard_Real    UTol,
 const Standard_Real    theMinLen)

{ 
  Initialize (C,AngularDeflection,CurvatureDeflection,MinimumOfPoints,UTol,theMinLen); 
}


//=======================================================================
//function : GCPnts_TangentialDeflection
//purpose  : 
//=======================================================================

GCPnts_TangentialDeflection::GCPnts_TangentialDeflection (
 const TheCurve&        C,
 const Standard_Real    FirstParameter,
 const Standard_Real    LastParameter,
 const Standard_Real    AngularDeflection,
 const Standard_Real    CurvatureDeflection,
 const Standard_Integer MinimumOfPoints,
 const Standard_Real    UTol,
 const Standard_Real    theMinLen)

{ 
  Initialize (C, 
        FirstParameter, 
        LastParameter,
        AngularDeflection, 
        CurvatureDeflection, 
        MinimumOfPoints,
        UTol, theMinLen);
}



//=======================================================================
//function : Initialize
//purpose  : 
//=======================================================================

void GCPnts_TangentialDeflection::Initialize (
 const TheCurve&        C, 
 const Standard_Real    AngularDeflection, 
 const Standard_Real    CurvatureDeflection,
 const Standard_Integer MinimumOfPoints,
 const Standard_Real    UTol,
 const Standard_Real    theMinLen)

{
  Initialize (C, 
              C.FirstParameter (), 
              C.LastParameter (),
              AngularDeflection, 
              CurvatureDeflection,
              MinimumOfPoints,
              UTol, theMinLen);
}


//=======================================================================
//function : Initialize
//purpose  : 
//=======================================================================

void GCPnts_TangentialDeflection::Initialize (
 const TheCurve&        C, 
 const Standard_Real    FirstParameter, 
 const Standard_Real    LastParameter,
 const Standard_Real    AngularDeflection, 
 const Standard_Real    CurvatureDeflection,
 const Standard_Integer MinimumOfPoints,
 const Standard_Real    UTol,
 const Standard_Real    theMinLen)

{
  
  Standard_ConstructionError_Raise_if (CurvatureDeflection < Precision::Confusion () ||
                                       AngularDeflection < Precision::Angular (),
                                       "GCPnts_TangentialDeflection::Initialize - Zero Deflection")

 parameters.Clear ();
 points    .Clear ();
 if (FirstParameter < LastParameter) {
   firstu = FirstParameter;
   lastu  = LastParameter;
 }
 else {
   lastu  = FirstParameter;
   firstu = LastParameter;
 }
 uTol                = UTol;
 angularDeflection   = AngularDeflection;
 curvatureDeflection = CurvatureDeflection;
 minNbPnts           = Max (MinimumOfPoints, 2);
 myMinLen            = Max(theMinLen, Precision::Confusion());

 switch (C.GetType()) {

 case GeomAbs_Line:   
   PerformLinear (C);
   break;

 case GeomAbs_Circle: 
   PerformCircular (C);
   break;

 case GeomAbs_BSplineCurve:
   {
     Handle_TheBSplineCurve BS = C.BSpline() ;
     if (BS->NbPoles() == 2 ) PerformLinear (C);
     else                     PerformCurve  (C);
     break;
   }
 case GeomAbs_BezierCurve:
   {
     Handle_TheBezierCurve  BZ = C.Bezier(); 
     if (BZ->NbPoles() == 2) PerformLinear (C);
     else                    PerformCurve  (C);
     break;
   }
 default: PerformCurve (C);
   
 }
}


//=======================================================================
//function : PerformLinear
//purpose  : 
//=======================================================================

void GCPnts_TangentialDeflection::PerformLinear (const TheCurve& C) {

  gp_Pnt P;
  D0 (C, firstu, P);
  parameters.Append (firstu);
  points    .Append (P);
  if (minNbPnts > 2) {
    Standard_Real Du = (lastu - firstu) / minNbPnts;
    Standard_Real U = firstu + Du;
    for (Standard_Integer i = 2; i <= minNbPnts; i++) {
      D0 (C, U, P);
      parameters.Append (U);
      points    .Append (P);
      U += Du;
    }
  }
  D0 (C, lastu, P);
  parameters.Append (lastu);
  points    .Append (P);
}

//=======================================================================
//function : PerformCircular
//purpose  : 
//=======================================================================

void GCPnts_TangentialDeflection::PerformCircular (const TheCurve& C) 
{
  // akm 8/01/02 : check the radius before divide by it
  Standard_Real dfR = C.Circle().Radius();
  Standard_Real Du = GCPnts_TangentialDeflection::ArcAngularStep(
    dfR, curvatureDeflection, angularDeflection, myMinLen);
    
  const Standard_Real aDiff = lastu - firstu;
  // Round up number of points to satisfy curvatureDeflection more precisely
  Standard_Integer NbPoints = (Standard_Integer)Min(Ceiling(aDiff / Du), 1.0e+6);
  NbPoints = Max(NbPoints, minNbPnts - 1);
  Du       = aDiff / NbPoints;

  gp_Pnt P;
  Standard_Real U = firstu;
  for (Standard_Integer i = 1; i <= NbPoints; i++)
  {
    D0 (C, U, P);
    parameters.Append (U);
    points    .Append (P);
    U += Du;
  }
  D0 (C, lastu, P);
  parameters.Append (lastu);
  points    .Append (P);
}


//=======================================================================
//function : PerformCurve
//purpose  : On respecte ll'angle et la fleche, on peut imposer un nombre
//           minimum de points sur un element lineaire
//=======================================================================
void GCPnts_TangentialDeflection::PerformCurve (const TheCurve& C)

{
  Standard_Integer i, j;       
  gp_XYZ V1, V2;
  gp_Pnt MiddlePoint, CurrentPoint, LastPoint;   
  Standard_Real Du, Dusave, MiddleU, L1, L2;

  Standard_Real U1       = firstu;   
  Standard_Real LTol     = Precision::Confusion ();  //protection longueur nulle
  Standard_Real ATol     = 1.e-2 * angularDeflection;
  if(ATol > 1.e-2)
    ATol = 1.e-2;
  else if(ATol < 1.e-7)
    ATol = 1.e-7;

  D0 (C, lastu, LastPoint);

  //Initialization du calcul

  Standard_Boolean NotDone = Standard_True;
  Dusave = (lastu - firstu)*Us3;
  Du     = Dusave;
  EvaluateDu (C, U1, CurrentPoint, Du, NotDone);
  parameters.Append (U1);
  points    .Append (CurrentPoint);

  // Used to detect "isLine" current bspline and in Du computation in general handling.
  Standard_Integer NbInterv = C.NbIntervals(GeomAbs_CN);
  TColStd_Array1OfReal Intervs(1, NbInterv+1);
  C.Intervals(Intervs, GeomAbs_CN);

  if (NotDone || Du > 5. * Dusave) {
    //C'est soit une droite, soit une singularite :
    V1 = (LastPoint.XYZ() - CurrentPoint.XYZ());
    L1 = V1.Modulus ();
    if (L1 > LTol)
    {
      //Si c'est une droite on verifie en calculant minNbPoints :
      Standard_Boolean IsLine   = Standard_True;
      Standard_Integer NbPoints = (minNbPnts > 3) ? minNbPnts : 3;
      switch (C.GetType()) {
      case GeomAbs_BSplineCurve:
        {
          Handle_TheBSplineCurve BS = C.BSpline() ;
          NbPoints = Max(BS->Degree() + 1, NbPoints);
          break;
        }
      case GeomAbs_BezierCurve:
        {
          Handle_TheBezierCurve  BZ = C.Bezier();
          NbPoints = Max(BZ->Degree() + 1, NbPoints);
          break;
        }
      default:
      ;}
      ////
      Standard_Real param = 0.;
      for (i = 1; i <= NbInterv && IsLine; ++i)
      {
        // Avoid usage intervals out of [firstu, lastu].
        if ((Intervs(i+1) < firstu) ||
            (Intervs(i)   > lastu))
        {
          continue;
        }
        // Fix border points in applicable intervals, to avoid be out of target interval.
        if ((Intervs(i)   < firstu) &&
            (Intervs(i+1) > firstu))
        {
          Intervs(i) = firstu;
        }
        if ((Intervs(i)   < lastu) &&
            (Intervs(i+1) > lastu))
        {
          Intervs(i + 1) = lastu;
        }

        Standard_Real delta = (Intervs(i+1) - Intervs(i))/NbPoints;
        for (j = 1; j <= NbPoints && IsLine; ++j)
        {
          param = Intervs(i) + j*delta;
          D0 (C, param, MiddlePoint);
          V2 = (MiddlePoint.XYZ() - CurrentPoint.XYZ());
          L2 = V2.Modulus ();
          if (L2 > LTol)
          {
            const Standard_Real aAngle = V2.CrossMagnitude(V1)/(L1*L2);
            IsLine = (aAngle < ATol);
          }
        }
      }

      if (IsLine)
      {
        parameters.Clear();
        points    .Clear();

        PerformLinear(C);
        return;
      }
      else
      {
        //c'etait une singularite on continue :
        //Du = Dusave;
        EvaluateDu (C, param, MiddlePoint, Du, NotDone);
      }
    }
    else
    {
      Du = (lastu-firstu)/2.1;
      MiddleU = firstu + Du;
      D0 (C, MiddleU, MiddlePoint);
      V1 = (MiddlePoint.XYZ() - CurrentPoint.XYZ());
      L1 = V1.Modulus ();
      if (L1 < LTol)
      {
        // L1 < LTol C'est une courbe de longueur nulle, calcul termine :
        // on renvoi un segment de 2 points   (protection)
        parameters.Append (lastu);
        points    .Append (LastPoint);
        return;
      }
    }
  }

  if (Du > Dusave) Du = Dusave;
  else             Dusave = Du;

  if (Du < uTol) {
    Du = lastu - firstu;
    if (Du < uTol) {
      parameters.Append (lastu);
      points    .Append (LastPoint);
      return;
    }
  }

  //Traitement normal pour une courbe
  Standard_Boolean MorePoints = Standard_True;
  Standard_Real U2            = firstu;   
  Standard_Real AngleMax      = angularDeflection * 0.5;  //car on prend le point milieu
  Standard_Integer aIdx[2] = {Intervs.Lower(), Intervs.Lower()}; // Indexes of intervals of U1 and U2, used to handle non-uniform case.
  Standard_Boolean isNeedToCheck = Standard_False;
  gp_Pnt aPrevPoint = points.Last();

  while (MorePoints) {
    aIdx[0] = getIntervalIdx(U1, Intervs, aIdx[0]);
    U2 += Du;

    if (U2 >= lastu) {                       //Bout de courbe
      U2 = lastu;
      CurrentPoint = LastPoint;
      Du = U2-U1;
      Dusave = Du;
    }
    else D0 (C, U2, CurrentPoint);           //Point suivant

    Standard_Real Coef = 0.0, ACoef = 0., FCoef = 0.;
    Standard_Boolean Correction, TooLarge, TooSmall;
    TooLarge   = Standard_False;
    Correction = Standard_True;
    TooSmall = Standard_False;

    while (Correction) {                     //Ajustement Du
      if (isNeedToCheck)
      {
        aIdx[1] = getIntervalIdx(U2, Intervs, aIdx[0]);
        if (aIdx[1] > aIdx[0]) // Jump to another polynom.
        {
          if (Du > (Intervs(aIdx[0] + 1) - Intervs(aIdx[0]) ) * Us3) // Set Du to the smallest value and check deflection on it.
          {
            Du = (Intervs(aIdx[0] + 1) - Intervs(aIdx[0]) ) * Us3;
            U2 = U1 + Du;
            if (U2 > lastu)
              U2 = lastu;
            D0 (C, U2, CurrentPoint);
          }
        }
      }
      MiddleU = (U1+U2)*0.5;                 //Verif / au point milieu
      D0 (C, MiddleU, MiddlePoint);

      V1 = (CurrentPoint.XYZ() - aPrevPoint.XYZ()); //Critere de fleche
      V2 = (MiddlePoint.XYZ()  - aPrevPoint.XYZ());
      L1 = V1.Modulus ();

      FCoef = (L1 > myMinLen) ? 
        V1.CrossMagnitude(V2)/(L1*curvatureDeflection) : 0.0;

      V1 = (CurrentPoint.XYZ() - MiddlePoint.XYZ()); //Critere d'angle
      L1 = V1.Modulus ();
      L2 = V2.Modulus ();
      if (L1 > myMinLen && L2 > myMinLen)
      {
        Standard_Real angg = V1.CrossMagnitude(V2) / (L1 * L2);
        ACoef = angg / AngleMax;
      }
      else
        ACoef = 0.0;

      //On retient le plus penalisant
      Coef = Max(ACoef, FCoef);

      if (isNeedToCheck && Coef < 0.55)
      {
        isNeedToCheck = Standard_False;
        Du = Dusave;
        U2 = U1 + Du;
        if (U2 > lastu)
          U2 = lastu;
        D0 (C, U2, CurrentPoint);
        continue;
      }

      if (Coef <= 1.0) {
        if (Abs (lastu-U2) < uTol) {
          parameters.Append (lastu);
          points    .Append (LastPoint);
          MorePoints = Standard_False;
          Correction = Standard_False;
        }
        else {
          if (Coef >= 0.55 || TooLarge) { 
            parameters.Append (U2);
            points    .Append (CurrentPoint);
            aPrevPoint = CurrentPoint;
            Correction = Standard_False;
            isNeedToCheck = Standard_True;
          }
          else if (TooSmall) {
            Correction = Standard_False;
            aPrevPoint = CurrentPoint;
          }
          else {
            TooSmall = Standard_True;
            //Standard_Real UUU2 = U2;
            Du += Min((U2-U1)*(1.-Coef), Du*Us3);

            U2 = U1 + Du;
            if (U2 > lastu)
              U2 = lastu;
            D0 (C, U2, CurrentPoint);
          }
        }
      }
      else {

        if (Coef >= 1.5) {
          if (!aPrevPoint.IsEqual(points.Last(), Precision::Confusion()))
          {
            parameters.Append (U1);
            points    .Append (aPrevPoint);
          }
          U2 = MiddleU;
          Du  = U2-U1;
          CurrentPoint = MiddlePoint;
        }
        else {
          Du*=0.9;
          U2 = U1 + Du;
          D0 (C, U2, CurrentPoint);
          TooLarge = Standard_True;
        }

      }
    }
    
    Du  = U2-U1;

    if (MorePoints) {
      if (U1 > firstu) {
        if (FCoef > ACoef) {
          //La fleche est critere de decoupage
          EvaluateDu (C, U2, CurrentPoint, Du, NotDone);
          if (NotDone) {
            Du += (Du-Dusave)*(Du/Dusave);
            if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
            if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
          }
        }
        else {
          //L'angle est le critere de decoupage
          Du += (Du-Dusave)*(Du/Dusave);
          if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
          if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
        }
      }
      
      if (Du < uTol) {
        Du = lastu - U2;
        if (Du < uTol) {
          parameters.Append (lastu);
          points    .Append (LastPoint);
          MorePoints = Standard_False;
        }
        else if (Du*Us3 > uTol) Du*=Us3;
      }
      U1 = U2;
      Dusave = Du;
    }
  }
  //Recalage avant dernier point :
  i = points.Length()-1;
//  Real d = points (i).Distance (points (i+1));
// if (Abs(parameters (i) - parameters (i+1))<= 0.000001 || d < Precision::Confusion()) {
//    cout<<"deux points confondus"<<endl;
//    parameters.Remove (i+1);
//    points.Remove (i+1);
//    i--;
//  }
  if (i >= 2) {
    MiddleU = parameters (i-1);
    MiddleU = (lastu + MiddleU)*0.5;
    D0 (C, MiddleU, MiddlePoint);
    parameters.SetValue (i, MiddleU);
    points    .SetValue (i, MiddlePoint);
  }

  //-- On rajoute des points aux milieux des segments si le nombre
  //-- mini de points n'est pas atteint
  //--
  Standard_Integer Nbp =  points.Length();
  Standard_Integer MinNb= (9*minNbPnts)/10;
  //if(MinNb<4) MinNb=4;

  //-- if(Nbp <  MinNb) { cout<<"\n*"; } else {  cout<<"\n."; } 
  while(Nbp < MinNb) { 
    //-- cout<<" \nGCPnts TangentialDeflection : Ajout de Points ("<<Nbp<<" "<<minNbPnts<<" )"<<endl;
    for (i = 2; i <= Nbp; i += 2) {
      MiddleU = (parameters.Value(i-1)+parameters.Value(i))*0.5;
      D0 (C, MiddleU, MiddlePoint); 
      parameters.InsertBefore(i,MiddleU);
      points.InsertBefore(i,MiddlePoint);
      Nbp++;
    }
  }
  //Additional check for intervals
  Standard_Real MinLen2 = myMinLen * myMinLen;
  Standard_Integer MaxNbp = 10 * Nbp;
  for(i = 1; i < Nbp; ++i)
  {
    U1 = parameters(i);
    U2 = parameters(i + 1);

    if (U2 - U1 <= uTol)
    {
      continue;
    }

    // Check maximal deflection on interval;
    Standard_Real dmax = 0.;
    Standard_Real umax = 0.;
    Standard_Real amax = 0.;
    EstimDefl(C, U1, U2, dmax, umax);
    const gp_Pnt& P1 = points(i);
    const gp_Pnt& P2 = points(i+1);
    D0(C, umax, MiddlePoint);
    amax = EstimAngl(P1, MiddlePoint, P2);
    if(dmax > curvatureDeflection || amax > AngleMax)
    {
      if(umax - U1 > uTol && U2 - umax > uTol)
      {
        if (P1.SquareDistance(MiddlePoint) > MinLen2 && P2.SquareDistance(MiddlePoint) > MinLen2)
        {
          parameters.InsertAfter(i, umax);
          points.InsertAfter(i, MiddlePoint);
          ++Nbp;
          --i; //To compensate ++i in loop header: i must point to first part of splitted interval
          if(Nbp > MaxNbp)
          {
            break;
          }
        }
      }
    }
  }
  // 
}

//=======================================================================
//function : EstimDefl
//purpose  : Estimation of maximal deflection for interval [U1, U2]
//           
//=======================================================================
void GCPnts_TangentialDeflection::EstimDefl (const TheCurve& C,
                            const Standard_Real U1, const Standard_Real U2, 
                            Standard_Real& MaxDefl, Standard_Real& UMax)
{
  Standard_Real Du = (lastu - firstu);
  //
  TheMaxCurvLinDist aFunc(C, U1, U2);
  //
  const Standard_Integer aNbIter = 100;
  Standard_Real reltol = Max(1.e-3, 2.*uTol/((Abs(U1) + Abs(U2))));
  //
  math_BrentMinimum anOptLoc(reltol, aNbIter, uTol);
  anOptLoc.Perform(aFunc, U1, (U1+U2)/2., U2);
  if(anOptLoc.IsDone())
  {
    MaxDefl = Sqrt(-anOptLoc.Minimum());
    UMax = anOptLoc.Location();
    return;
  }
  //
  math_Vector aLowBorder(1,1);
  math_Vector aUppBorder(1,1);
  math_Vector aSteps(1,1);
  //
  aSteps(1) = Max(0.1 * Du, 100. * uTol);
  Standard_Integer aNbParticles = Max(8, RealToInt(32 * (U2 - U1) / Du));
  //
  aLowBorder(1) = U1;
  aUppBorder(1) = U2;
  //
  //
  Standard_Real aValue;
  math_Vector aT(1,1);
  TheMaxCurvLinDistMV aFuncMV(aFunc);

  math_PSO aFinder(&aFuncMV, aLowBorder, aUppBorder, aSteps, aNbParticles); 
  aFinder.Perform(aSteps, aValue, aT);
  //
  anOptLoc.Perform(aFunc, Max(aT(1) - aSteps(1), U1) , aT(1), Min(aT(1) + aSteps(1),U2));
  if(anOptLoc.IsDone())
  {
    MaxDefl = Sqrt(-anOptLoc.Minimum());
    UMax = anOptLoc.Location();
    return;
  }
  MaxDefl = Sqrt(-aValue);
  UMax = aT(1);
  //
}