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

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)
{
  // Verification du pas d avancement, ET recalcul de ce pas :
  //
  // 1)test point confondu
  //     si oui les autres tests ne sont pas faits
  // 2)test angle 3d trop grand
  //     si oui on divise le pas, on sort
  //     angle3d = angle ((point precedent, point calcule),
  //                       tangente precedente)
  // 3)verification du pas d avancement en 2d
  // 4)test point confondu
  // 5)test angle 2d trop grand
  // 6)test point de tangence 
  //     si oui on sort
  // 7)calcul de la tangente en u,v de la section 
  // 8)test angle 3d trop grand  
  //     angle3d = angle ((point precedent, point calcule),  
  //                       nouvelle tangente)
  // 9)test angle 2d trop grand
  //10)test de changement de rive(depasser le point de tangence sans le savoir)
  //11)calcul du pas d  avancement en fonction de la fleche
  //12)ajustement du pas en fonction des pas precedents
  
  IntWalk_StatusDeflection Status = IntWalk_OK;

  static const Standard_Real CosRef3D = 0.98;// regle par tests dans U4 
                                             // correspond a 11.478 d
  static const Standard_Real CosRef2D = 0.88; // correspond a 25 d
  static const Standard_Integer MaxDivision = 60;  // nombre maxi de division 
                                                   // du pas a cause de
                                              // l  angle trop grand en 2d (U4)
  //---------------------------------------------------------------------------------
  //-- lbr le 4 Avril 95 : On peut se trouver ds le cas ou Status renvoie point
  //-- confondus   si epsilon est assez grand (1e-11) . Dans ce cas on boucle 
  //-- sans jamais changer les valeurs envoyees a Rsnld. 
  static Standard_Integer NbPointsConfondusConsecutifs = 0 ;
  static Standard_Integer EpsilonSembleTropGrand       = 0 ; 
  //---------------------------------------------------------------------------------
  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)) { // le carre est deja pris dans le constructeur
    Status = IntWalk_PointConfondu;
    if (StatusPrecedent == IntWalk_PasTropGrand) {
      return IntWalk_ArretSurPointPrecedent;
    }

    if(++EpsilonSembleTropGrand > 5   &&  NbPointsConfondusConsecutifs == 8)  {     //--    Provisoire 
      if(epsilon>0.00000000001) epsilon*=0.5;                                       //--    Provisoire 
      EpsilonSembleTropGrand = 0;                                                   //--    Provisoire 
    }
  }
  else {
    NbPointsConfondusConsecutifs = 0;   //--    Provisoire
    EpsilonSembleTropGrand = 0;         //--    Provisoire
    if(Norme<1e-16) Norme = 1e-16;      //--    Provisoire 
    
    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 trop grand
      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; //point confondu 2d
  Cosi = StepSign * (Du * previousd2d.X() + 
                     Dv * previousd2d.Y());
  if (Cosi < 0 && Status == IntWalk_PointConfondu) 
    return IntWalk_ArretSurPointPrecedent; // on sort car retour arriere 
                                           // avec point confondu


  if (sp.IsTangent()) 
    return IntWalk_ArretSurPoint;       

//si au cours du cheminement on a subdivise plus de MaxDivision pour chaque
//pas precedent,anomalie sur le carreau;on ne fait plus rien (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 trop grand
      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 trop grand ou changement de rive       
      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 {
//   on estime la fleche courante.
//   si fleche/2<=flechecourante<= fleche on considere que le critere est
//   respecte.
//   sinon ajuster le pas en fonction du pas precedent 

/*
      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);
*/

      // Calcul direct : 
      // POnCurv=(((p1+p2)/2.+(p2+p3)/2.)/2. + ((p2+p3)/2.+(p3+P4)/2.)/2.)/2.
      // soit POnCurv = p1/8. + 3.p2/8. + 3.p3/8. + p4/8.
      // Or p2 = p1 + lambda*d1 et p3 = p4 - lambda*d4
      // Donc POnCurv = (p1 + p4)/2. + 3.*(lambda d1 - lambda d4)/8.
      // On calcule l'ecart avec (p1+p4)/2. . Il faut donc juste calculer
      // la norme (au carre) de 3.*lambda (d1 - d4)/8.
      // soit la norme de :
      //    3.*(Sqrt(Norme)/3.)*StepSign*(d1-d4)/8.
      // ce qui fait, en prenant le carre :
      //         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) {  // pas trop grand
	if (FlecheCourante > fleche*fleche) {  // pas trop grand
	  Step = Step /2.;
	  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;     
}
Commit	Line	Data
	1	IntWalk_StatusDeflection IntWalk_IWalking::TestDeflection
	2	(TheIWFunction& sp,
	3	const Standard_Boolean Finished,
	4	const math_Vector& UV,
	5	const IntWalk_StatusDeflection StatusPrecedent,
	6	Standard_Integer& NbDivision,
	7	Standard_Real& Step,
	8	const Standard_Integer StepSign)
	9	{
	10	// Verification du pas d avancement, ET recalcul de ce pas :
	11	//
	12	// 1)test point confondu
	13	// si oui les autres tests ne sont pas faits
	14	// 2)test angle 3d trop grand
	15	// si oui on divise le pas, on sort
	16	// angle3d = angle ((point precedent, point calcule),
	17	// tangente precedente)
	18	// 3)verification du pas d avancement en 2d
	19	// 4)test point confondu
	20	// 5)test angle 2d trop grand
	21	// 6)test point de tangence
	22	// si oui on sort
	23	// 7)calcul de la tangente en u,v de la section
	24	// 8)test angle 3d trop grand
	25	// angle3d = angle ((point precedent, point calcule),
	26	// nouvelle tangente)
	27	// 9)test angle 2d trop grand
	28	//10)test de changement de rive(depasser le point de tangence sans le savoir)
	29	//11)calcul du pas d avancement en fonction de la fleche
	30	//12)ajustement du pas en fonction des pas precedents
	31
	32	IntWalk_StatusDeflection Status = IntWalk_OK;
	33
	34	static const Standard_Real CosRef3D = 0.98;// regle par tests dans U4
	35	// correspond a 11.478 d
	36	static const Standard_Real CosRef2D = 0.88; // correspond a 25 d
	37	static const Standard_Integer MaxDivision = 60; // nombre maxi de division
	38	// du pas a cause de
	39	// l angle trop grand en 2d (U4)
	40	//---------------------------------------------------------------------------------
	41	//-- lbr le 4 Avril 95 : On peut se trouver ds le cas ou Status renvoie point
	42	//-- confondus si epsilon est assez grand (1e-11) . Dans ce cas on boucle
	43	//-- sans jamais changer les valeurs envoyees a Rsnld.
	44	static Standard_Integer NbPointsConfondusConsecutifs = 0 ;
	45	static Standard_Integer EpsilonSembleTropGrand = 0 ;
	46	//---------------------------------------------------------------------------------
	47	Standard_Real Paramu, Paramv, StepU,StepV;
	48	Standard_Real Cosi, Cosi2, Norme;
	49
	50	gp_Vec Corde(previousPoint.Value(), sp.Point());
	51
	52	Norme = Corde.SquareMagnitude();
	53	// if (Norme <= epsilon*epsilon) {
	54	if ((++NbPointsConfondusConsecutifs < 10) && (Norme <= epsilon)) { // le carre est deja pris dans le constructeur
	55	Status = IntWalk_PointConfondu;
	56	if (StatusPrecedent == IntWalk_PasTropGrand) {
	57	return IntWalk_ArretSurPointPrecedent;
	58	}
	59
	60	if(++EpsilonSembleTropGrand > 5 && NbPointsConfondusConsecutifs == 8) { //-- Provisoire
	61	if(epsilon>0.00000000001) epsilon*=0.5; //-- Provisoire
	62	EpsilonSembleTropGrand = 0; //-- Provisoire
	63	}
	64	}
	65	else {
	66	NbPointsConfondusConsecutifs = 0; //-- Provisoire
	67	EpsilonSembleTropGrand = 0; //-- Provisoire
	68	if(Norme<1e-16) Norme = 1e-16; //-- Provisoire
	69
	70	Cosi = Corde * previousd3d;
	71	if (Cosi*StepSign < 0.) { // angle 3d > pi/2 !!!!
	72	Cosi2 = 0.;
	73	}
	74	else {
	75	Cosi2 = Cosi * Cosi / previousd3d.SquareMagnitude() / Norme;
	76	}
	77	if (Cosi2 < CosRef3D) { //angle 3d trop grand
	78	Step = Step /2.0;
	79	StepU = Abs(Step*previousd2d.X());
	80	StepV = Abs(Step*previousd2d.Y());
	81	if (StepU < tolerance(1) && StepV < tolerance(2))
	82	Status = IntWalk_ArretSurPointPrecedent;
	83	else
	84	Status = IntWalk_PasTropGrand;
	85	return Status;
	86	}
	87	}
	88
	89	if (!reversed) {
	90	previousPoint.ParametersOnS2(Paramu, Paramv);
	91	}
	92	else {
	93	previousPoint.ParametersOnS1(Paramu, Paramv);
	94	}
	95	Standard_Real Du = UV(1) - Paramu;
	96	Standard_Real Dv = UV(2) - Paramv;
	97	Standard_Real Duv = Du * Du + Dv * Dv;
	98	if (Abs(Du) < tolerance(1) && Abs(Dv) < tolerance(2))
	99	return IntWalk_ArretSurPointPrecedent; //point confondu 2d
	100	Cosi = StepSign * (Du * previousd2d.X() +
	101	Dv * previousd2d.Y());
	102	if (Cosi < 0 && Status == IntWalk_PointConfondu)
	103	return IntWalk_ArretSurPointPrecedent; // on sort car retour arriere
	104	// avec point confondu
	105
	106
	107	if (sp.IsTangent())
	108	return IntWalk_ArretSurPoint;
	109
	110	//si au cours du cheminement on a subdivise plus de MaxDivision pour chaque
	111	//pas precedent,anomalie sur le carreau;on ne fait plus rien (experience U4)
	112
	113	if (NbDivision < MaxDivision &&
	114	Status != IntWalk_PointConfondu &&
	115	StatusPrecedent!= IntWalk_PointConfondu ) {
	116	Cosi2 = Cosi * Cosi / Duv;
	117	if (Cosi2 < CosRef2D \|\| Cosi < 0 ) {
	118	Step = Step / 2.0;
	119	StepU = Abs(Step*previousd2d.X());
	120	StepV = Abs(Step*previousd2d.Y());
	121
	122	if (StepU < tolerance(1) && StepV < tolerance(2))
	123	Status = IntWalk_ArretSurPointPrecedent;
	124	else
	125	Status = IntWalk_PasTropGrand;
	126	NbDivision = NbDivision + 1;
	127	return Status;
	128	}
	129
	130	Cosi = Corde * sp.Direction3d();
	131	Cosi2 = Cosi * Cosi / sp.Direction3d().SquareMagnitude() / Norme;
	132	if (Cosi2 < CosRef3D ){ //angle 3d trop grand
	133	Step = Step / 2.;
	134	StepU = Abs(Step*previousd2d.X());
	135	StepV = Abs(Step*previousd2d.Y());
	136	if (StepU < tolerance(1) && StepV < tolerance(2))
	137	Status = IntWalk_ArretSurPoint;
	138	else
	139	Status = IntWalk_PasTropGrand;
	140	return Status;
	141	}
	142	Cosi = Du * sp.Direction2d().X() +
	143	Dv * sp.Direction2d().Y();
	144	Cosi2 = Cosi * Cosi / Duv;
	145	if (Cosi2 < CosRef2D \|\|
	146	sp.Direction2d() * previousd2d < 0) {
	147	//angle 2d trop grand ou changement de rive
	148	Step = Step / 2.;
	149	StepU = Abs(Step*previousd2d.X());
	150	StepV = Abs(Step*previousd2d.Y());
	151	if (StepU < tolerance(1) && StepV < tolerance(2))
	152	Status = IntWalk_ArretSurPointPrecedent;
	153	else
	154	Status = IntWalk_PasTropGrand;
	155	return Status;
	156	}
	157	}
	158
	159	if (!Finished) {
	160
	161	if (Status == IntWalk_PointConfondu) {
	162	StepU = Min(Abs(1.5 * Du),pas*(UM-Um));
	163	StepV = Min(Abs(1.5 * Dv),pas*(VM-Vm));
	164
	165	Standard_Real d2dx = Abs(previousd2d.X());
	166	Standard_Real d2dy = Abs(previousd2d.Y());
	167
	168	if (d2dx < tolerance(1)) {
	169	Step = StepV/d2dy;
	170	}
	171	else if (d2dy < tolerance(2)) {
	172	Step = StepU/d2dx;
	173	}
	174	else {
	175	Step = Min(StepU/d2dx,StepV/d2dy);
	176	}
	177
	178
	179
	180	}
	181	else {
	182	// on estime la fleche courante.
	183	// si fleche/2<=flechecourante<= fleche on considere que le critere est
	184	// respecte.
	185	// sinon ajuster le pas en fonction du pas precedent
	186
	187	/*
	188	Standard_Real Dist = Sqrt(Norme)/3.;
	189	TColgp_Array1OfPnt Poles(1,4);
	190	gp_Pnt POnCurv,Milieu;
	191	Poles(1) = previousPoint.Value();
	192	Poles(4) = sp.Point();
	193	Poles(2) = Poles(1).XYZ() +
	194	StepSign * Dist* previousd3d.Normalized().XYZ();
	195	Poles(3) = Poles(4).XYZ() -
	196	StepSign * Dist*sp.Direction3d().Normalized().XYZ();
	197	BzCLib::PntPole(0.5,Poles,POnCurv);
	198	Milieu = (Poles(1).XYZ() + Poles(4).XYZ())*0.5;
	199	// FlecheCourante = Milieu.Distance(POnCurv);
	200	Standard_Real FlecheCourante = Milieu.SquareDistance(POnCurv);
	201	*/
	202
	203	// Calcul direct :
	204	// POnCurv=(((p1+p2)/2.+(p2+p3)/2.)/2. + ((p2+p3)/2.+(p3+P4)/2.)/2.)/2.
	205	// soit POnCurv = p1/8. + 3.p2/8. + 3.p3/8. + p4/8.
	206	// Or p2 = p1 + lambdad1 et p3 = p4 - lambdad4
	207	// Donc POnCurv = (p1 + p4)/2. + 3.*(lambda d1 - lambda d4)/8.
	208	// On calcule l'ecart avec (p1+p4)/2. . Il faut donc juste calculer
	209	// la norme (au carre) de 3.*lambda (d1 - d4)/8.
	210	// soit la norme de :
	211	// 3.(Sqrt(Norme)/3.)StepSign*(d1-d4)/8.
	212	// ce qui fait, en prenant le carre :
	213	// Norme * (d1-d4).SquareMagnitude()/64.
	214
	215	Standard_Real FlecheCourante =
	216	(previousd3d.Normalized().XYZ()-sp.Direction3d().Normalized().XYZ()).SquareModulus()*Norme/64.;
	217
	218
	219	// if (FlecheCourante <= 0.5*fleche) {
	220	if (FlecheCourante <= 0.25flechefleche) {
	221
	222	Standard_Real d2dx = Abs(sp.Direction2d().X());
	223	Standard_Real d2dy = Abs(sp.Direction2d().Y());
	224
	225	StepU = Min(Abs(1.5Du),pas(UM-Um));
	226	StepV = Min(Abs(1.5Dv),pas(VM-Vm));
	227
	228	if (d2dx < tolerance(1)) {
	229	Step = StepV/d2dy;
	230	}
	231	else if (d2dy < tolerance(2)) {
	232	Step = StepU/d2dx;
	233	}
	234	else {
	235	Step = Min(StepU/d2dx,StepV/d2dy);
	236	}
	237
	238	}
	239	else {
	240	// if (FlecheCourante > fleche) { // pas trop grand
	241	if (FlecheCourante > fleche*fleche) { // pas trop grand
	242	Step = Step /2.;
	243	Status = IntWalk_PasTropGrand;
	244	}
	245	else {
	246	Standard_Real d2dx = Abs(sp.Direction2d().X());
	247	Standard_Real d2dy = Abs(sp.Direction2d().Y());
	248
	249	StepU = Min(Abs(1.5Du),pas(UM-Um));
	250	StepV = Min(Abs(1.5Dv),pas(VM-Vm));
	251
	252	if (d2dx < tolerance(1)) {
	253	Step = Min(Step,StepV/d2dy);
	254	}
	255	else if (d2dy < tolerance(2)) {
	256	Step = Min(Step,StepU/d2dx);
	257	}
	258	else {
	259	Step = Min(Step,Min(StepU/d2dx,StepV/d2dy));
	260	}
	261	}
	262	}
	263	}
	264	}
	265	return Status;
	266	}
	267
	268
	269
	270