0026632: HLR sample doesn't work with Algo mode

[occt.git] / src / IntWalk / IntWalk_IWalking_5.gxx

// Copyright (c) 1995-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.

namespace {
  static const Standard_Real CosRef3D = 0.98;// rule by tests in U4 
                                             // correspond to  11.478 d
  static const Standard_Real CosRef2D = 0.88; // correspond to 25 d
  static const Standard_Integer MaxDivision = 60;  // max number of step division 
                                                   // because the angle is too great in 2d (U4)
}

IntWalk_StatusDeflection IntWalk_IWalking::TestDeflection
  (TheIWFunction& sp,
   const Standard_Boolean Finished,
   const math_Vector& UV,
   const IntWalk_StatusDeflection StatusPrecedent,
   Standard_Integer& NbDivision,
   Standard_Real& Step,
   const Standard_Integer StepSign)
{
  // Check the step of advancement, AND recalculate this step :
  //
  // 1) test point confused
  //     if yes other tests are not done
  // 2) test angle 3d too great
  //     if yes divide the step and leave
  //     angle3d = angle ((previous point, calculated point),
  //                       previous tangent)
  // 3) check step of advancement in 2d
  // 4) test point confused
  // 5) test angle 2d too great
  // 6) test point of tangency 
  //     if yes leave
  // 7) calculate the tangent by u,v of the section 
  // 8) test angle 3d too great  
  //     angle3d = angle ((previous point, calculated point),  
  //                       new tangent)
  // 9) test angle 2d too great
  //10) test change of side (pass the tangent point not knowing it)
  //11) calculate the step of advancement depending on the vector
  //12) adjust the step depending on the previous steps 
  
  IntWalk_StatusDeflection Status = IntWalk_OK;

  //---------------------------------------------------------------------------------
  //-- lbr le 4 Avril 95 : it is possible that the status returns points confused
  //-- if epsilon is great enough (1e-11). In this case one loops 
  //-- without ever changing the values sent to Rsnld. 
  //---------------------------------------------------------------------------------
  Standard_Real Paramu = 0.0, Paramv = 0.0;

  if (!reversed) {
    previousPoint.ParametersOnS2(Paramu, Paramv);
  }
  else
  {
    previousPoint.ParametersOnS1(Paramu, Paramv);
  }

  const Standard_Real Du = UV(1) - Paramu;
  const Standard_Real Dv = UV(2) - Paramv;
  const Standard_Real Duv = Du * Du + Dv * Dv;

  gp_Vec Corde(previousPoint.Value(), sp.Point());

  const Standard_Real Norme = Corde.SquareMagnitude(), 
                      aTol = epsilon*Precision::PConfusion();

  //if ((++NbPointsConfondusConsecutifs < 10) && (Norme <= epsilon)) { // the square is already taken in the constructor
  if ((Norme <= epsilon) && ((Duv <= aTol) || (StatusPrecedent != IntWalk_OK)))
  { // the square is already taken in the constructor
    Status = IntWalk_PointConfondu;
    if (StatusPrecedent == IntWalk_PasTropGrand) {
      return IntWalk_ArretSurPointPrecedent;
    }
  }
  else {
    Standard_Real Cosi = Corde * previousd3d;
    Standard_Real Cosi2 = 0.0;

    if (Cosi*StepSign >= 0.) {// angle 3d <= pi/2 !!!!
      const Standard_Real aDiv = previousd3d.SquareMagnitude()*Norme;
      if(aDiv == 0)
        return Status;
      Cosi2 = Cosi * Cosi / aDiv;
    }
    if (Cosi2 < CosRef3D) { //angle 3d too great
      Step = Step /2.0;
      Standard_Real StepU = Abs(Step*previousd2d.X()),
                    StepV = Abs(Step*previousd2d.Y());
      if (StepU < tolerance(1) && StepV < tolerance(2)) 
        Status = IntWalk_ArretSurPointPrecedent;
      else 
        Status = IntWalk_PasTropGrand;
      return Status;
    }
  }

  const Standard_Real aMinTolU = 0.1*Abs(Step*previousd2d.X()),
                      aMinTolV = 0.1*Abs(Step*previousd2d.Y());
  const Standard_Real aTolU = (aMinTolU > 0.0) ? Min(tolerance(1), aMinTolU) : tolerance(1),
                      aTolV = (aMinTolV > 0.0) ? Min(tolerance(2), aMinTolV) : tolerance(2);

  //If aMinTolU==0.0 then (Abs(Du) < aMinTolU) is equivalent of (Abs(Du) < 0.0).
  //It is impossible. Therefore, this case should be processed separately.
  //Analogicaly for aMinTolV.

  if ((Abs(Du) < aTolU) && (Abs(Dv) < aTolV))
  {
    //Thin shapes (for which Ulast-Ufirst or/and Vlast-Vfirst is quite small)
    //exists (see bug #25820). In this case, step is quite small too.
    //Nevertheless, it not always means that we mark time. Therefore, Du and Dv
    //must consider step (aMinTolU and aMinTolV parameters).

    return IntWalk_ArretSurPointPrecedent; //confused point 2d
  }

  Standard_Real Cosi = StepSign * (Du * previousd2d.X() + Dv * previousd2d.Y());

  if (Cosi < 0 && Status == IntWalk_PointConfondu) 
    return IntWalk_ArretSurPointPrecedent; // leave as step back  
                                           // with confused point

  if (sp.IsTangent()) 
    return IntWalk_ArretSurPoint;       

//if during routing one has subdivided more than  MaxDivision for each
//previous step, bug on the square; do nothing (experience U4)

  if ((NbDivision < MaxDivision) && (Status != IntWalk_PointConfondu) && 
    (StatusPrecedent!= IntWalk_PointConfondu))
  {
    Standard_Real Cosi2 = Cosi * Cosi / Duv;
    if (Cosi2 < CosRef2D || Cosi < 0  ) {
      Step = Step / 2.0;
      Standard_Real StepU = Abs(Step*previousd2d.X()),
                    StepV = Abs(Step*previousd2d.Y());

      if (StepU < tolerance(1) && StepV < tolerance(2))
        Status = IntWalk_ArretSurPointPrecedent;
      else 
        Status = IntWalk_PasTropGrand;
      NbDivision = NbDivision + 1;
      return Status;
    }

    Cosi = Corde * sp.Direction3d(); 
    Cosi2 = Cosi * Cosi / sp.Direction3d().SquareMagnitude() / Norme;
    if (Cosi2 < CosRef3D ){ //angle 3d too great
      Step = Step / 2.;
      Standard_Real StepU = Abs(Step*previousd2d.X()),
                    StepV = Abs(Step*previousd2d.Y());
      if (StepU < tolerance(1) && StepV < tolerance(2))
        Status = IntWalk_ArretSurPoint;
      else 
        Status = IntWalk_PasTropGrand;
      return Status;
    }
    Cosi = Du * sp.Direction2d().X() + 
      Dv * sp.Direction2d().Y();
    Cosi2 = Cosi * Cosi / Duv;
    if (Cosi2 < CosRef2D || 
      sp.Direction2d() * previousd2d < 0) {
        //angle 2d too great or change the side       
        Step  = Step / 2.;
        Standard_Real StepU = Abs(Step*previousd2d.X()),
                      StepV = Abs(Step*previousd2d.Y());
        if (StepU < tolerance(1) && StepV < tolerance(2))
          Status = IntWalk_ArretSurPointPrecedent;
        else 
          Status = IntWalk_PasTropGrand;
        return Status;
    }
  }

  if (!Finished) {
    if (Status == IntWalk_PointConfondu)
    {
      Standard_Real StepU = Min(Abs(1.5 * Du),pas*(UM-Um)),
                    StepV = Min(Abs(1.5 * Dv),pas*(VM-Vm));

      Standard_Real d2dx = Abs(previousd2d.X()); 
      Standard_Real d2dy = Abs(previousd2d.Y()); 

      if (d2dx < tolerance(1))
      {
        Step = StepV/d2dy;
      }
      else if (d2dy < tolerance(2))
      {
        Step = StepU/d2dx;
      }
      else
      {
        Step = Min(StepU/d2dx,StepV/d2dy);
      }
    }
    else
    {
      //   estimate the current vector.
      //   if vector/2<=current vector<= vector it is considered that the criterion
      //   is observed.
      //   otherwise adjust the step depending on the previous step 

      /*
        Standard_Real Dist = Sqrt(Norme)/3.;
        TColgp_Array1OfPnt Poles(1,4);
        gp_Pnt POnCurv,Milieu;
        Poles(1) = previousPoint.Value();
        Poles(4) = sp.Point();
        Poles(2) = Poles(1).XYZ() + 
      StepSign * Dist* previousd3d.Normalized().XYZ();
        Poles(3) = Poles(4).XYZ() - 
      StepSign * Dist*sp.Direction3d().Normalized().XYZ();
        BzCLib::PntPole(0.5,Poles,POnCurv);
        Milieu = (Poles(1).XYZ() + Poles(4).XYZ())*0.5;
      //      FlecheCourante = Milieu.Distance(POnCurv);
        Standard_Real FlecheCourante = Milieu.SquareDistance(POnCurv);
      */

        // Direct calculation : 
        // POnCurv=(((p1+p2)/2.+(p2+p3)/2.)/2. + ((p2+p3)/2.+(p3+P4)/2.)/2.)/2.
        // either POnCurv = p1/8. + 3.p2/8. + 3.p3/8. + p4/8.
        // Or p2 = p1 + lambda*d1 et p3 = p4 - lambda*d4
        // So POnCurv = (p1 + p4)/2. + 3.*(lambda d1 - lambda d4)/8.
        // Calculate the deviation with (p1+p4)/2. . So it is just necessary to calculate
        // the norm (square) of 3.*lambda (d1 - d4)/8.
        // either the norm of :
        //    3.*(Sqrt(Norme)/3.)*StepSign*(d1-d4)/8.
        // which produces, takin the square :
        //         Norme * (d1-d4).SquareMagnitude()/64.

      Standard_Real FlecheCourante = 
	(previousd3d.Normalized().XYZ()-sp.Direction3d().Normalized().XYZ()).SquareModulus()*Norme/64.;

  
//      if (FlecheCourante <= 0.5*fleche) {
      if (FlecheCourante <= 0.25*fleche*fleche)
      {
        Standard_Real d2dx = Abs(sp.Direction2d().X()); 
        Standard_Real d2dy = Abs(sp.Direction2d().Y()); 
        
        Standard_Real StepU = Min(Abs(1.5*Du),pas*(UM-Um)),
                      StepV = Min(Abs(1.5*Dv),pas*(VM-Vm));

        if (d2dx < tolerance(1))
        {
          Step = StepV/d2dy;
        }
        else if (d2dy < tolerance(2))
        {
          Step = StepU/d2dx;
        }
        else
        {
          Step = Min(StepU/d2dx,StepV/d2dy);
        }	
      }
      else
      {
        //if (FlecheCourante > fleche) {  // step too great
        if (FlecheCourante > fleche*fleche)
        {  // step too great
          Step = Step /2.;
          Standard_Real StepU = Abs(Step*previousd2d.X()),
                        StepV = Abs(Step*previousd2d.Y());
          
          if (StepU < tolerance(1) && StepV < tolerance(2)) 
            Status = IntWalk_ArretSurPointPrecedent;
          else 
            Status = IntWalk_PasTropGrand;
        }
        else
        {
          Standard_Real d2dx = Abs(sp.Direction2d().X()); 
          Standard_Real d2dy = Abs(sp.Direction2d().Y()); 
          
          Standard_Real StepU = Min(Abs(1.5*Du),pas*(UM-Um)),
                        StepV = Min(Abs(1.5*Dv),pas*(VM-Vm));

          if (d2dx < tolerance(1))
          {
            Step = Min(Step,StepV/d2dy);
          }
          else if (d2dy < tolerance(2))
          {
            Step = Min(Step,StepU/d2dx);
          }
          else
          {
            Step = Min(Step,Min(StepU/d2dx,StepV/d2dy));
          }
        }
      }
    }
  }
  return Status;     
}