0024360: Hang up trying to intersect two faces

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

// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-2012 OPEN CASCADE SAS
//
// The content of this file is subject to the Open CASCADE Technology Public
// License Version 6.5 (the "License"). You may not use the content of this file
// except in compliance with the License. Please obtain a copy of the License
// at http://www.opencascade.org and read it completely before using this file.
//
// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
//
// The Original Code and all software distributed under the License is
// distributed on an "AS IS" basis, without warranty of any kind, and the
// Initial Developer hereby disclaims all such warranties, including without
// limitation, any warranties of merchantability, fitness for a particular
// purpose or non-infringement. Please see the License for the specific terms
// and conditions governing the rights and limitations under the License.

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, Paramv, StepU,StepV;
  Standard_Real Cosi, Cosi2, Norme;

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

  Norme = Corde.SquareMagnitude();
//  if (Norme <= epsilon*epsilon) {
  if ((++NbPointsConfondusConsecutifs < 10) && (Norme <= epsilon)) { // the square is already taken in the constructor
    Status = IntWalk_PointConfondu;
    if (StatusPrecedent == IntWalk_PasTropGrand) {
      return IntWalk_ArretSurPointPrecedent;
    }

    if(++EpsilonSembleTropGrand > 5   &&  NbPointsConfondusConsecutifs == 8)  {     //--    Temporary 
      if(epsilon>0.00000000001) epsilon*=0.5;                                       //--    Temporary
      EpsilonSembleTropGrand = 0;                                                   //--    Temporary 
    }
  }
  else {
    NbPointsConfondusConsecutifs = 0;   //--    Temporary
    EpsilonSembleTropGrand = 0;         //--    Temporary
    if(Norme<1e-16) Norme = 1e-16;      //--    Temporary 
    
    Cosi = Corde * previousd3d;
    if (Cosi*StepSign < 0.) { // angle 3d > pi/2 !!!!
      Cosi2 = 0.;
    }
    else {
      Cosi2 = Cosi * Cosi / previousd3d.SquareMagnitude() / Norme;
    }
    if (Cosi2 < CosRef3D) { //angle 3d too great
      Step = Step /2.0;
      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 (!reversed) {
    previousPoint.ParametersOnS2(Paramu, Paramv);
  }
  else {
    previousPoint.ParametersOnS1(Paramu, Paramv);
  }
  Standard_Real Du = UV(1) - Paramu;
  Standard_Real Dv = UV(2) - Paramv;
  Standard_Real Duv = Du * Du + Dv * Dv;
  if (Abs(Du) < tolerance(1) && Abs(Dv) < tolerance(2))
    return IntWalk_ArretSurPointPrecedent; //confused point 2d
  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 ) {
    Cosi2 = Cosi * Cosi / Duv;
    if (Cosi2 < CosRef2D || Cosi < 0  ) {
      Step = Step / 2.0;
      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.;
      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.;
      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) {
      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()); 

	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.;
          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()); 

	  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;     
}