[occt.git] / src / Blend / Blend_Walking_2.gxx

Blend_Status Blend_Walking::TestArret(Blend_Function& Function,
				      const Blend_Status State,
				      const Standard_Boolean TestDefl,
				      const Standard_Boolean TestSolu,
				      const Standard_Boolean TestLengthStep)

// On regarde si le point donne est solution.
// Si c est le cas,
//  On verifie le critere de fleche sur surf1 et surf2
//   Si OK, on classifie les points sur surf1 et sur surf2.
//    Si les deux sont dedans : on retourne Blend_OK
//    sinon si un seul est dedans
//     on resout le pb inverse sur la restriction concernee
//    sinon on resout le pb inverse sur la surface pour laquelle
//     le point est le plus loin.
//   sinon (fleche non OK)
//    on renvoie Blend_StepTooLarge.
// sinon on renvoie Blend_StepTooLarge.
//

{
  gp_Pnt pt1,pt2;
  gp_Vec V1,V2;
  gp_Vec Tgp1,Tgp2,Nor1,Nor2;
  gp_Vec2d V12d,V22d;
  Blend_Status State1,State2;
  IntSurf_TypeTrans tras1,tras2;
  Blend_Point curpoint;
  Standard_Boolean loctwist1 = Standard_False, loctwist2 = Standard_False;
  Standard_Real tolsolu = tolesp;

  if ( !TestSolu) tolsolu *= 1000; //Ca doit toujours etre bon
  if (Function.IsSolution(sol,tolsolu)) {

#ifdef DEB
    sectioncalculee = 1;
#endif
    Standard_Boolean curpointistangent = Function.IsTangencyPoint();
    pt1  = Function.PointOnS1();
    pt2  = Function.PointOnS2();
    if(curpointistangent){
      curpoint.SetValue(pt1,pt2,param,
			sol(1),sol(2),sol(3),sol(4));
    }
    else{
      V1   = Function.TangentOnS1();
      V2   = Function.TangentOnS2();
      V12d = Function.Tangent2dOnS1();
      V22d = Function.Tangent2dOnS2();
      curpoint.SetValue(pt1,pt2,param,
			sol(1),sol(2),sol(3),sol(4),
			V1,V2,V12d,V22d);
      if(Function.TwistOnS1()) loctwist1 = Standard_True;
      if(Function.TwistOnS2()) loctwist2 = Standard_True;
    }

    if (TestDefl && check) {

      // Verification du critere de fleche sur chaque surface
      //et sur la ligne guide
    
      State1 = CheckDeflection(Standard_True,curpoint);
      State2 = CheckDeflection(Standard_False,curpoint);
    }
    else {
       State1 = Blend_OK;
       State2 = Blend_OK;
       if (TestLengthStep) {
	 // On verifie juste que le pas n'est pas trop grand
	 // (Cas des prolongements foireux)
	 Standard_Real curparamu,curparamv, prevparamu,prevparamv;
	 math_Vector inf(1,4), sup(1,4);
	 Function.GetBounds(inf, sup);
	 sup -= inf;
	 sup *= 0.05; // Pas max : 5% du domaine
     
	 curpoint.ParametersOnS1(curparamu,curparamv);
	 previousP.ParametersOnS1(prevparamu,prevparamv);
	 if (Abs(curparamu-prevparamu) > sup(1)) State1 = Blend_StepTooLarge;
	 if (Abs(curparamv-prevparamv) > sup(2)) State1 = Blend_StepTooLarge;     
	 curpoint.ParametersOnS2(curparamu,curparamv);
	 previousP.ParametersOnS2(prevparamu,prevparamv);
	 if (Abs(curparamu-prevparamu) > sup(3)) State2 = Blend_StepTooLarge;
	 if (Abs(curparamv-prevparamv) > sup(4)) State2 = Blend_StepTooLarge;
       }
    }
      
    if (State1 == Blend_Backward) {
      State1 = Blend_StepTooLarge;
      rebrou= Standard_True;
    }

    if (State2 == Blend_Backward) {
      State2 = Blend_StepTooLarge;
      rebrou = Standard_True;
    }

    if (State1 == Blend_StepTooLarge ||
	State2 == Blend_StepTooLarge) {
      
      return Blend_StepTooLarge;
    }
    

    // Ici seulement on peut statuer sur le twist
    // Car les rejet ont ete effectue (BUC60322)
    if (loctwist1) twistflag1 = Standard_True;
    if (loctwist2) twistflag2 = Standard_True;

    if (!comptra && !curpointistangent) {
      Function.Tangent(sol(1),sol(2),sol(3),sol(4),Tgp1,Tgp2,Nor1,Nor2);
      Nor1.Normalize();
      Nor2.Normalize();
      Standard_Real testra = Tgp1.Dot(Nor1.Crossed(V1));
      if (Abs(testra) > Precision::Confusion()) {
	tras1 = IntSurf_In;
	if ((testra > 0. && !loctwist1) || (testra < 0. && loctwist1)) {
	  tras1 = IntSurf_Out;
	}
      
	testra = Tgp2.Dot(Nor2.Crossed(V2));
	if (Abs(testra) > Precision::Confusion()) {
	  tras2 = IntSurf_Out;
	  if ((testra > 0. && !loctwist2) || (testra < 0. && loctwist2)) {
	    tras2 = IntSurf_In;
	  }
	  comptra = Standard_True;
	  line->Set(tras1,tras2);
	}
      }
    }

    if (State1 == Blend_OK ||
	State2 == Blend_OK ) {
      previousP = curpoint;
      return State;
    }

    if (State1 == Blend_StepTooSmall &&
	State2 == Blend_StepTooSmall) {
      previousP = curpoint;
      if (State == Blend_OK) {
	return Blend_StepTooSmall;
      }
      else {
	return State;
      }
    }

    if (State == Blend_OK) {
      return Blend_SamePoints;
    }
    else {
      return State;
    }

  }
  else {
    return Blend_StepTooLarge;
  }
}


Blend_Status Blend_Walking::CheckDeflection
  (const Standard_Boolean OnFirst,
   const Blend_Point&     CurPoint)
{
  // regle par tests dans U4 correspond a 11.478 d
  const Standard_Real CosRef3D = 0.98;

  const Standard_Real CosRef2D = 0.88; // correspond a 25 d

  Standard_Real Norme, Cosi, Cosi2;
#ifndef DEB
  Standard_Real prevNorme = 0.;
#else
  Standard_Real prevNorme;
#endif
  Standard_Real FlecheCourante;
  Standard_Real Du,Dv,Duv;
  Standard_Real tolu,tolv;

  gp_Pnt Psurf;
  gp_Vec Tgsurf;
  gp_Vec2d Tgonsurf;
  Standard_Real curparamu, curparamv;
  Standard_Boolean curpointistangent = CurPoint.IsTangencyPoint();

  gp_Pnt prevP;
  gp_Vec prevTg;
  gp_Vec2d previousd2d;
  Standard_Real prevparamu, prevparamv;
  Standard_Boolean prevpointistangent = previousP.IsTangencyPoint();

  if (OnFirst) {
    Psurf = CurPoint.PointOnS1();
    if(!curpointistangent){
      Tgsurf = CurPoint.TangentOnS1();
    }
    prevP = previousP.PointOnS1();
    if(!prevpointistangent){
      prevTg = previousP.TangentOnS1();
    }
    tolu = TheSurfaceTool::UResolution(surf1,tolesp);
    tolv = TheSurfaceTool::VResolution(surf1,tolesp);
  }
  else {
    Psurf = CurPoint.PointOnS2();
    if(!curpointistangent){
      Tgsurf = CurPoint.TangentOnS2();
    }
    prevP = previousP.PointOnS2();
    if(!prevpointistangent){
      prevTg = previousP.TangentOnS2();
    }
    tolu = TheSurfaceTool::UResolution(surf2,tolesp);
    tolv = TheSurfaceTool::VResolution(surf2,tolesp);
  }

  gp_Vec Corde(prevP,Psurf);
  Norme = Corde.SquareMagnitude();
//  if(!curpointistangent) curNorme = Tgsurf.SquareMagnitude();
  if(!prevpointistangent) prevNorme = prevTg.SquareMagnitude();


  if (Norme <= tolesp*tolesp){
    // il faudra peut etre  forcer meme point
    return Blend_SamePoints;
  }
  if(!prevpointistangent){
    if(prevNorme <= tolesp*tolesp) {
      return Blend_SamePoints;
    }
    Cosi = sens*Corde*prevTg;
    if (Cosi <0.) { // angle 3d>pi/2. --> retour arriere
      return Blend_Backward;
    }
    
    Cosi2 = Cosi * Cosi / prevNorme / Norme;
    if (Cosi2 < CosRef3D) { 
      return Blend_StepTooLarge;
    }
  }

  if(!curpointistangent){
    // Voir s il faut faire le controle sur le signe de prevtg*Tgsurf
    Cosi = sens*Corde*Tgsurf;
    Cosi2 = Cosi * Cosi / Tgsurf.SquareMagnitude() / Norme;
    if (Cosi2 < CosRef3D || Cosi < 0.) { 
      return Blend_StepTooLarge;
    }
  }  
    
  if(check2d){
    if (OnFirst) {
      CurPoint.ParametersOnS1(curparamu,curparamv);
      if(!curpointistangent) Tgonsurf = CurPoint.Tangent2dOnS1();
      previousP.ParametersOnS1(prevparamu,prevparamv);
      if(!prevpointistangent) previousd2d = previousP.Tangent2dOnS1();
    }
    else {
      CurPoint.ParametersOnS2(curparamu,curparamv);
      if(!curpointistangent) Tgonsurf = CurPoint.Tangent2dOnS2();
      previousP.ParametersOnS2(prevparamu,prevparamv);
      if(!prevpointistangent) previousd2d = previousP.Tangent2dOnS2();
    }
    
    Du = curparamu - prevparamu;
    Dv = curparamv - prevparamv;
    Duv = Du * Du + Dv * Dv;
//    SqrtDuv = Sqrt(Duv);
    if (Abs(Du) < tolu && Abs(Dv) < tolv){
      // il faudra peut etre  forcer meme point
      return Blend_SamePoints; //point confondu 2d
    }
    if(!prevpointistangent){
      if(Abs(previousd2d.X()) < tolu && Abs(previousd2d.Y()) < tolv){
	// il faudra peut etre  forcer meme point
	return Blend_SamePoints; //point confondu 2d
      }
      Cosi = sens*(Du * previousd2d.X() + Dv * previousd2d.Y());
      if (Cosi < 0) {
	return Blend_Backward; 
      }
    }    
    if(!curpointistangent){
      // Voir s il faut faire le controle sur le signe de Cosi
      Cosi = sens*(Du * Tgonsurf.X() +  Dv * Tgonsurf.Y())/Tgonsurf.Magnitude();
      Cosi2 = Cosi * Cosi / Duv;
      if (Cosi2 < CosRef2D || Cosi <0.) { 
	return Blend_StepTooLarge;
      }
    }
  }
  if(!curpointistangent && !prevpointistangent){
    // Estimation de la fleche courante
    FlecheCourante = (prevTg.Normalized().XYZ()-Tgsurf.Normalized().XYZ()).SquareModulus()*Norme/64.;
    
    if (FlecheCourante <= 0.25*fleche*fleche) {
      return Blend_StepTooSmall;
    }
    if (FlecheCourante > fleche*fleche) {
      // pas trop grand : commentaire interessant
      return Blend_StepTooLarge;
    }
  }
  return Blend_OK;
}
Commit	Line	Data
7fd59977	1	Blend_Status Blend_Walking::TestArret(Blend_Function& Function,
	2	const Blend_Status State,
	3	const Standard_Boolean TestDefl,
	4	const Standard_Boolean TestSolu,
	5	const Standard_Boolean TestLengthStep)
	6
	7	// On regarde si le point donne est solution.
	8	// Si c est le cas,
	9	// On verifie le critere de fleche sur surf1 et surf2
	10	// Si OK, on classifie les points sur surf1 et sur surf2.
	11	// Si les deux sont dedans : on retourne Blend_OK
	12	// sinon si un seul est dedans
	13	// on resout le pb inverse sur la restriction concernee
	14	// sinon on resout le pb inverse sur la surface pour laquelle
	15	// le point est le plus loin.
	16	// sinon (fleche non OK)
	17	// on renvoie Blend_StepTooLarge.
	18	// sinon on renvoie Blend_StepTooLarge.
	19	//
	20
	21	{
	22	gp_Pnt pt1,pt2;
	23	gp_Vec V1,V2;
	24	gp_Vec Tgp1,Tgp2,Nor1,Nor2;
	25	gp_Vec2d V12d,V22d;
	26	Blend_Status State1,State2;
	27	IntSurf_TypeTrans tras1,tras2;
	28	Blend_Point curpoint;
	29	Standard_Boolean loctwist1 = Standard_False, loctwist2 = Standard_False;
	30	Standard_Real tolsolu = tolesp;
	31
	32	if ( !TestSolu) tolsolu *= 1000; //Ca doit toujours etre bon
	33	if (Function.IsSolution(sol,tolsolu)) {
	34
	35	#ifdef DEB
	36	sectioncalculee = 1;
	37	#endif
	38	Standard_Boolean curpointistangent = Function.IsTangencyPoint();
	39	pt1 = Function.PointOnS1();
	40	pt2 = Function.PointOnS2();
	41	if(curpointistangent){
	42	curpoint.SetValue(pt1,pt2,param,
	43	sol(1),sol(2),sol(3),sol(4));
	44	}
	45	else{
	46	V1 = Function.TangentOnS1();
	47	V2 = Function.TangentOnS2();
	48	V12d = Function.Tangent2dOnS1();
	49	V22d = Function.Tangent2dOnS2();
	50	curpoint.SetValue(pt1,pt2,param,
	51	sol(1),sol(2),sol(3),sol(4),
	52	V1,V2,V12d,V22d);
	53	if(Function.TwistOnS1()) loctwist1 = Standard_True;
	54	if(Function.TwistOnS2()) loctwist2 = Standard_True;
	55	}
	56
	57	if (TestDefl && check) {
	58
	59	// Verification du critere de fleche sur chaque surface
	60	//et sur la ligne guide
	61
	62	State1 = CheckDeflection(Standard_True,curpoint);
	63	State2 = CheckDeflection(Standard_False,curpoint);
	64	}
65	else {
66	State1 = Blend_OK;
67	State2 = Blend_OK;
68	if (TestLengthStep) {
69	// On verifie juste que le pas n'est pas trop grand
70	// (Cas des prolongements foireux)
71	Standard_Real curparamu,curparamv, prevparamu,prevparamv;
72	math_Vector inf(1,4), sup(1,4);
73	Function.GetBounds(inf, sup);
74	sup -= inf;
75	sup *= 0.05; // Pas max : 5% du domaine
76
77	curpoint.ParametersOnS1(curparamu,curparamv);
78	previousP.ParametersOnS1(prevparamu,prevparamv);
79	if (Abs(curparamu-prevparamu) > sup(1)) State1 = Blend_StepTooLarge;
80	if (Abs(curparamv-prevparamv) > sup(2)) State1 = Blend_StepTooLarge;
81	curpoint.ParametersOnS2(curparamu,curparamv);
82	previousP.ParametersOnS2(prevparamu,prevparamv);
83	if (Abs(curparamu-prevparamu) > sup(3)) State2 = Blend_StepTooLarge;
84	if (Abs(curparamv-prevparamv) > sup(4)) State2 = Blend_StepTooLarge;
85	}
86	}
87
88	if (State1 == Blend_Backward) {
89	State1 = Blend_StepTooLarge;
90	rebrou= Standard_True;
91	}
92
93	if (State2 == Blend_Backward) {
94	State2 = Blend_StepTooLarge;
95	rebrou = Standard_True;
96	}
97
98	if (State1 == Blend_StepTooLarge \|\|
99	State2 == Blend_StepTooLarge) {
100
101	return Blend_StepTooLarge;
102	}
103
104
105	// Ici seulement on peut statuer sur le twist
106	// Car les rejet ont ete effectue (BUC60322)
107	if (loctwist1) twistflag1 = Standard_True;
108	if (loctwist2) twistflag2 = Standard_True;
109
110	if (!comptra && !curpointistangent) {
111	Function.Tangent(sol(1),sol(2),sol(3),sol(4),Tgp1,Tgp2,Nor1,Nor2);
112	Nor1.Normalize();
113	Nor2.Normalize();
114	Standard_Real testra = Tgp1.Dot(Nor1.Crossed(V1));
115	if (Abs(testra) > Precision::Confusion()) {
116	tras1 = IntSurf_In;
117	if ((testra > 0. && !loctwist1) \|\| (testra < 0. && loctwist1)) {
118	tras1 = IntSurf_Out;
119	}
120
121	testra = Tgp2.Dot(Nor2.Crossed(V2));
122	if (Abs(testra) > Precision::Confusion()) {
123	tras2 = IntSurf_Out;
124	if ((testra > 0. && !loctwist2) \|\| (testra < 0. && loctwist2)) {
125	tras2 = IntSurf_In;
126	}
127	comptra = Standard_True;
128	line->Set(tras1,tras2);
129	}
130	}
131	}
132
133	if (State1 == Blend_OK \|\|
134	State2 == Blend_OK ) {
135	previousP = curpoint;
136	return State;
137	}
138
139	if (State1 == Blend_StepTooSmall &&
140	State2 == Blend_StepTooSmall) {
141	previousP = curpoint;
142	if (State == Blend_OK) {
143	return Blend_StepTooSmall;
144	}
145	else {
146	return State;
147	}
148	}
149
150	if (State == Blend_OK) {
151	return Blend_SamePoints;
152	}
153	else {
154	return State;
155	}
156
157	}
158	else {
159	return Blend_StepTooLarge;
160	}
161	}
162
163
164	Blend_Status Blend_Walking::CheckDeflection
165	(const Standard_Boolean OnFirst,
166	const Blend_Point& CurPoint)
167	{
168	// regle par tests dans U4 correspond a 11.478 d
169	const Standard_Real CosRef3D = 0.98;
170
171	const Standard_Real CosRef2D = 0.88; // correspond a 25 d
172
173	Standard_Real Norme, Cosi, Cosi2;
174	#ifndef DEB
175	Standard_Real prevNorme = 0.;
176	#else
177	Standard_Real prevNorme;
178	#endif
179	Standard_Real FlecheCourante;
180	Standard_Real Du,Dv,Duv;
181	Standard_Real tolu,tolv;
182
183	gp_Pnt Psurf;
184	gp_Vec Tgsurf;
185	gp_Vec2d Tgonsurf;
186	Standard_Real curparamu, curparamv;
187	Standard_Boolean curpointistangent = CurPoint.IsTangencyPoint();
188
189	gp_Pnt prevP;
190	gp_Vec prevTg;
191	gp_Vec2d previousd2d;
192	Standard_Real prevparamu, prevparamv;
193	Standard_Boolean prevpointistangent = previousP.IsTangencyPoint();
194
195	if (OnFirst) {
196	Psurf = CurPoint.PointOnS1();
197	if(!curpointistangent){
198	Tgsurf = CurPoint.TangentOnS1();
199	}
200	prevP = previousP.PointOnS1();
201	if(!prevpointistangent){
202	prevTg = previousP.TangentOnS1();
203	}
204	tolu = TheSurfaceTool::UResolution(surf1,tolesp);
205	tolv = TheSurfaceTool::VResolution(surf1,tolesp);
206	}
207	else {
208	Psurf = CurPoint.PointOnS2();
209	if(!curpointistangent){
210	Tgsurf = CurPoint.TangentOnS2();
211	}
212	prevP = previousP.PointOnS2();
213	if(!prevpointistangent){
214	prevTg = previousP.TangentOnS2();
215	}
216	tolu = TheSurfaceTool::UResolution(surf2,tolesp);
217	tolv = TheSurfaceTool::VResolution(surf2,tolesp);
218	}
219
220	gp_Vec Corde(prevP,Psurf);
221	Norme = Corde.SquareMagnitude();
222	// if(!curpointistangent) curNorme = Tgsurf.SquareMagnitude();
223	if(!prevpointistangent) prevNorme = prevTg.SquareMagnitude();
224
225
226	if (Norme <= tolesp*tolesp){
227	// il faudra peut etre forcer meme point
228	return Blend_SamePoints;
229	}
230	if(!prevpointistangent){
231	if(prevNorme <= tolesp*tolesp) {
232	return Blend_SamePoints;
233	}
234	Cosi = sensCordeprevTg;
235	if (Cosi <0.) { // angle 3d>pi/2. --> retour arriere
236	return Blend_Backward;
237	}
238
239	Cosi2 = Cosi * Cosi / prevNorme / Norme;
240	if (Cosi2 < CosRef3D) {
241	return Blend_StepTooLarge;
242	}
243	}
244
245	if(!curpointistangent){
246	// Voir s il faut faire le controle sur le signe de prevtg*Tgsurf
247	Cosi = sensCordeTgsurf;
248	Cosi2 = Cosi * Cosi / Tgsurf.SquareMagnitude() / Norme;
249	if (Cosi2 < CosRef3D \|\| Cosi < 0.) {
250	return Blend_StepTooLarge;
251	}
252	}
253
254	if(check2d){
255	if (OnFirst) {
256	CurPoint.ParametersOnS1(curparamu,curparamv);
257	if(!curpointistangent) Tgonsurf = CurPoint.Tangent2dOnS1();
258	previousP.ParametersOnS1(prevparamu,prevparamv);
259	if(!prevpointistangent) previousd2d = previousP.Tangent2dOnS1();
260	}
261	else {
262	CurPoint.ParametersOnS2(curparamu,curparamv);
263	if(!curpointistangent) Tgonsurf = CurPoint.Tangent2dOnS2();
264	previousP.ParametersOnS2(prevparamu,prevparamv);
265	if(!prevpointistangent) previousd2d = previousP.Tangent2dOnS2();
266	}
267
268	Du = curparamu - prevparamu;
269	Dv = curparamv - prevparamv;
270	Duv = Du * Du + Dv * Dv;
271	// SqrtDuv = Sqrt(Duv);
272	if (Abs(Du) < tolu && Abs(Dv) < tolv){
273	// il faudra peut etre forcer meme point
274	return Blend_SamePoints; //point confondu 2d
275	}
276	if(!prevpointistangent){
277	if(Abs(previousd2d.X()) < tolu && Abs(previousd2d.Y()) < tolv){
278	// il faudra peut etre forcer meme point
279	return Blend_SamePoints; //point confondu 2d
280	}
281	Cosi = sens(Du previousd2d.X() + Dv * previousd2d.Y());
282	if (Cosi < 0) {
283	return Blend_Backward;
284	}
285	}
286	if(!curpointistangent){
287	// Voir s il faut faire le controle sur le signe de Cosi
288	Cosi = sens(Du Tgonsurf.X() + Dv * Tgonsurf.Y())/Tgonsurf.Magnitude();
289	Cosi2 = Cosi * Cosi / Duv;
290	if (Cosi2 < CosRef2D \|\| Cosi <0.) {
291	return Blend_StepTooLarge;
292	}
293	}
294	}
295	if(!curpointistangent && !prevpointistangent){
296	// Estimation de la fleche courante
297	FlecheCourante = (prevTg.Normalized().XYZ()-Tgsurf.Normalized().XYZ()).SquareModulus()*Norme/64.;
298
299	if (FlecheCourante <= 0.25flechefleche) {
300	return Blend_StepTooSmall;
301	}
302	if (FlecheCourante > fleche*fleche) {
303	// pas trop grand : commentaire interessant
304	return Blend_StepTooLarge;
305	}
306	}
307	return Blend_OK;
308	}