b311480e |
1 | // Copyright (c) 1995-1999 Matra Datavision |
973c2be1 |
2 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
3 | // |
973c2be1 |
4 | // This file is part of Open CASCADE Technology software library. |
b311480e |
5 | // |
d5f74e42 |
6 | // This library is free software; you can redistribute it and/or modify it under |
7 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
8 | // by the Free Software Foundation, with special exception defined in the file |
9 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
10 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
11 | // |
973c2be1 |
12 | // Alternatively, this file may be used under the terms of Open CASCADE |
13 | // commercial license or contractual agreement. |
b311480e |
14 | |
7fd59977 |
15 | #include <Standard_TypeMismatch.hxx> |
408ecc39 |
16 | #include <Precision.hxx> |
32ca7a51 |
17 | static const Standard_Real TolFactor = 1.e-12; |
18 | static const Standard_Real MinTol = 1.e-20; |
19 | static const Standard_Real MinStep = 1e-7; |
20 | |
21 | static const Standard_Integer MaxOrder = 3; |
22 | |
23 | |
7fd59977 |
24 | |
25 | /*----------------------------------------------------------------------------- |
28cec2ba |
26 | Fonction permettant de rechercher une distance extremale entre un point P et |
7fd59977 |
27 | une courbe C (en partant d'un point approche C(u0)). |
28cec2ba |
28 | Cette classe herite de math_FunctionWithDerivative et est utilisee par |
7fd59977 |
29 | les algorithmes math_FunctionRoot et math_FunctionRoots. |
28cec2ba |
30 | Si on note D1c et D2c les derivees premiere et seconde: |
31 | { F(u) = (C(u)-P).D1c(u)/ ||Du||} |
32 | { DF(u) = ||Du|| + (C(u)-P).D2c(u)/||Du|| - F(u)*Duu*Du/||Du||**2 |
7fd59977 |
33 | |
34 | |
28cec2ba |
35 | { F(u) = (C(u)-P).D1c(u) } |
36 | { DF(u) = D1c(u).D1c(u) + (C(u)-P).D2c(u) |
37 | = ||D1c(u)|| ** 2 + (C(u)-P).D2c(u) } |
7fd59977 |
38 | ----------------------------------------------------------------------------*/ |
39 | |
32ca7a51 |
40 | Standard_Real Extrema_FuncExtPC::SearchOfTolerance() |
28cec2ba |
41 | { |
32ca7a51 |
42 | const Standard_Integer NPoint = 10; |
43 | const Standard_Real aStep = (myUsupremum - myUinfium)/(Standard_Real)NPoint; |
28cec2ba |
44 | |
32ca7a51 |
45 | Standard_Integer aNum = 0; |
46 | Standard_Real aMax = -Precision::Infinite(); //Maximum value of 1st derivative |
28cec2ba |
47 | //(it is computed with using NPoint point) |
48 | |
32ca7a51 |
49 | do |
28cec2ba |
50 | { |
32ca7a51 |
51 | Standard_Real u = myUinfium + aNum*aStep; //parameter for every point |
52 | if(u > myUsupremum) |
53 | u = myUsupremum; |
28cec2ba |
54 | |
32ca7a51 |
55 | Pnt Ptemp; //empty point (is not used below) |
56 | Vec VDer; // 1st derivative vector |
57 | Tool::D1(*((Curve*)myC), u, Ptemp, VDer); |
f4dee9bb |
58 | |
59 | if(Precision::IsInfinite(VDer.X()) || Precision::IsInfinite(VDer.Y())) |
60 | { |
61 | continue; |
62 | } |
63 | |
32ca7a51 |
64 | Standard_Real vm = VDer.Magnitude(); |
65 | if(vm > aMax) |
66 | aMax = vm; |
28cec2ba |
67 | } |
32ca7a51 |
68 | while(++aNum < NPoint+1); |
28cec2ba |
69 | |
32ca7a51 |
70 | return Max(aMax*TolFactor,MinTol); |
28cec2ba |
71 | |
72 | } |
32ca7a51 |
73 | |
74 | //============================================================================= |
7fd59977 |
75 | |
76 | Extrema_FuncExtPC::Extrema_FuncExtPC(): |
77 | myU(0.), |
78 | myD1f(0.) |
79 | { |
80 | myPinit = Standard_False; |
81 | myCinit = Standard_False; |
82 | myD1Init = Standard_False; |
28cec2ba |
83 | |
32ca7a51 |
84 | SubIntervalInitialize(0.0,0.0); |
85 | myMaxDerivOrder = 0; |
86 | myTol=MinTol; |
7fd59977 |
87 | } |
88 | |
89 | //============================================================================= |
90 | |
91 | Extrema_FuncExtPC::Extrema_FuncExtPC (const Pnt& P, |
28cec2ba |
92 | const Curve& C): myU(0.), myD1f(0.) |
93 | { |
7fd59977 |
94 | myP = P; |
95 | myC = (Standard_Address)&C; |
96 | myPinit = Standard_True; |
97 | myCinit = Standard_True; |
98 | myD1Init = Standard_False; |
28cec2ba |
99 | |
32ca7a51 |
100 | SubIntervalInitialize(Tool::FirstParameter(*((Curve*)myC)), |
28cec2ba |
101 | Tool::LastParameter(*((Curve*)myC))); |
102 | |
32ca7a51 |
103 | switch(Tool::GetType(*((Curve*)myC))) |
28cec2ba |
104 | { |
105 | case GeomAbs_BezierCurve: |
106 | case GeomAbs_BSplineCurve: |
1aec3320 |
107 | case GeomAbs_OffsetCurve: |
28cec2ba |
108 | case GeomAbs_OtherCurve: |
109 | myMaxDerivOrder = MaxOrder; |
110 | myTol = SearchOfTolerance(); |
111 | break; |
112 | default: |
113 | myMaxDerivOrder = 0; |
114 | myTol=MinTol; |
115 | break; |
32ca7a51 |
116 | } |
28cec2ba |
117 | } |
7fd59977 |
118 | //============================================================================= |
119 | |
120 | void Extrema_FuncExtPC::Initialize(const Curve& C) |
28cec2ba |
121 | { |
7fd59977 |
122 | myC = (Standard_Address)&C; |
123 | myCinit = Standard_True; |
124 | myPoint.Clear(); |
125 | mySqDist.Clear(); |
126 | myIsMin.Clear(); |
28cec2ba |
127 | |
32ca7a51 |
128 | SubIntervalInitialize(Tool::FirstParameter(*((Curve*)myC)), |
28cec2ba |
129 | Tool::LastParameter(*((Curve*)myC))); |
130 | |
32ca7a51 |
131 | switch(Tool::GetType(*((Curve*)myC))) |
28cec2ba |
132 | { |
133 | case GeomAbs_BezierCurve: |
134 | case GeomAbs_BSplineCurve: |
1aec3320 |
135 | case GeomAbs_OffsetCurve: |
28cec2ba |
136 | case GeomAbs_OtherCurve: |
137 | myMaxDerivOrder = MaxOrder; |
138 | myTol = SearchOfTolerance(); |
139 | break; |
140 | default: |
141 | myMaxDerivOrder = 0; |
142 | myTol=MinTol; |
143 | break; |
32ca7a51 |
144 | } |
28cec2ba |
145 | } |
7fd59977 |
146 | |
147 | //============================================================================= |
148 | |
149 | void Extrema_FuncExtPC::SetPoint(const Pnt& P) |
150 | { |
151 | myP = P; |
152 | myPinit = Standard_True; |
153 | myPoint.Clear(); |
154 | mySqDist.Clear(); |
155 | myIsMin.Clear(); |
156 | } |
157 | |
158 | //============================================================================= |
159 | |
32ca7a51 |
160 | |
7fd59977 |
161 | Standard_Boolean Extrema_FuncExtPC::Value (const Standard_Real U, Standard_Real& F) |
162 | { |
32ca7a51 |
163 | if (!myPinit || !myCinit) |
9775fa61 |
164 | throw Standard_TypeMismatch("No init"); |
28cec2ba |
165 | |
7fd59977 |
166 | myU = U; |
167 | Vec D1c; |
168 | Tool::D1(*((Curve*)myC),myU,myPc,D1c); |
f4dee9bb |
169 | |
170 | if(Precision::IsInfinite(D1c.X()) || Precision::IsInfinite(D1c.Y())) |
171 | { |
172 | F = Precision::Infinite(); |
173 | return Standard_False; |
174 | } |
175 | |
7fd59977 |
176 | Standard_Real Ndu = D1c.Magnitude(); |
32ca7a51 |
177 | |
178 | if(myMaxDerivOrder != 0) |
28cec2ba |
179 | { |
32ca7a51 |
180 | if (Ndu <= myTol) // Cas Singulier (PMN 22/04/1998) |
28cec2ba |
181 | { |
32ca7a51 |
182 | const Standard_Real DivisionFactor = 1.e-3; |
183 | Standard_Real du; |
184 | if((myUsupremum >= RealLast()) || (myUinfium <= RealFirst())) |
185 | du = 0.0; |
186 | else |
187 | du = myUsupremum-myUinfium; |
28cec2ba |
188 | |
32ca7a51 |
189 | const Standard_Real aDelta = Max(du*DivisionFactor,MinStep); |
28cec2ba |
190 | //Derivative is approximated by Taylor-series |
191 | |
32ca7a51 |
192 | Standard_Integer n = 1; //Derivative order |
193 | Vec V; |
194 | Standard_Boolean IsDeriveFound; |
28cec2ba |
195 | |
32ca7a51 |
196 | do |
28cec2ba |
197 | { |
32ca7a51 |
198 | V = Tool::DN(*((Curve*)myC),myU,++n); |
199 | Ndu = V.Magnitude(); |
200 | IsDeriveFound = (Ndu > myTol); |
28cec2ba |
201 | } |
32ca7a51 |
202 | while(!IsDeriveFound && n < myMaxDerivOrder); |
28cec2ba |
203 | |
32ca7a51 |
204 | if(IsDeriveFound) |
28cec2ba |
205 | { |
32ca7a51 |
206 | Standard_Real u; |
28cec2ba |
207 | |
32ca7a51 |
208 | if(myU-myUinfium < aDelta) |
209 | u = myU+aDelta; |
210 | else |
211 | u = myU-aDelta; |
28cec2ba |
212 | |
32ca7a51 |
213 | Pnt P1, P2; |
214 | Tool::D0(*((Curve*)myC),Min(myU, u),P1); |
215 | Tool::D0(*((Curve*)myC),Max(myU, u),P2); |
28cec2ba |
216 | |
32ca7a51 |
217 | Vec V1(P1,P2); |
218 | Standard_Real aDirFactor = V.Dot(V1); |
28cec2ba |
219 | |
32ca7a51 |
220 | if(aDirFactor < 0.0) |
221 | D1c = -V; |
222 | else |
223 | D1c = V; |
28cec2ba |
224 | }//if(IsDeriveFound) |
32ca7a51 |
225 | else |
28cec2ba |
226 | { |
227 | //Derivative is approximated by three points |
32ca7a51 |
228 | |
229 | Pnt Ptemp; //(0,0,0)-coordinate |
230 | Pnt P1, P2, P3; |
231 | Standard_Boolean IsParameterGrown; |
28cec2ba |
232 | |
32ca7a51 |
233 | if(myU-myUinfium < 2*aDelta) |
28cec2ba |
234 | { |
32ca7a51 |
235 | Tool::D0(*((Curve*)myC),myU,P1); |
236 | Tool::D0(*((Curve*)myC),myU+aDelta,P2); |
237 | Tool::D0(*((Curve*)myC),myU+2*aDelta,P3); |
238 | IsParameterGrown = Standard_True; |
28cec2ba |
239 | } |
32ca7a51 |
240 | else |
28cec2ba |
241 | { |
32ca7a51 |
242 | Tool::D0(*((Curve*)myC),myU-2*aDelta,P1); |
243 | Tool::D0(*((Curve*)myC),myU-aDelta,P2); |
244 | Tool::D0(*((Curve*)myC),myU,P3); |
245 | IsParameterGrown = Standard_False; |
28cec2ba |
246 | } |
247 | |
32ca7a51 |
248 | Vec V1(Ptemp,P1), V2(Ptemp,P2), V3(Ptemp,P3); |
28cec2ba |
249 | |
32ca7a51 |
250 | if(IsParameterGrown) |
251 | D1c=-3*V1+4*V2-V3; |
252 | else |
253 | D1c=V1-4*V2+3*V3; |
28cec2ba |
254 | } |
255 | Ndu = D1c.Magnitude(); |
256 | }//(if (Ndu <= myTol)) condition |
257 | }//if(myMaxDerivOrder != 0) |
32ca7a51 |
258 | |
259 | if (Ndu <= MinTol) |
28cec2ba |
260 | { |
261 | //Warning: 1st derivative is equal to zero! |
32ca7a51 |
262 | return Standard_False; |
28cec2ba |
263 | } |
264 | |
7fd59977 |
265 | Vec PPc (myP,myPc); |
266 | F = PPc.Dot(D1c)/Ndu; |
267 | return Standard_True; |
268 | } |
269 | |
270 | //============================================================================= |
271 | |
272 | Standard_Boolean Extrema_FuncExtPC::Derivative (const Standard_Real U, Standard_Real& D1f) |
273 | { |
9775fa61 |
274 | if (!myPinit || !myCinit) throw Standard_TypeMismatch(); |
7fd59977 |
275 | Standard_Real F; |
276 | return Values(U,F,D1f); /* on fait appel a Values pour simplifier la |
28cec2ba |
277 | sauvegarde de l'etat. */ |
7fd59977 |
278 | } |
279 | //============================================================================= |
280 | |
28cec2ba |
281 | Standard_Boolean Extrema_FuncExtPC::Values (const Standard_Real U, |
282 | Standard_Real& F, |
283 | Standard_Real& D1f) |
284 | { |
32ca7a51 |
285 | if (!myPinit || !myCinit) |
9775fa61 |
286 | throw Standard_TypeMismatch("No init"); |
28cec2ba |
287 | |
32ca7a51 |
288 | Pnt myPc_old = myPc, myP_old = myP; |
289 | |
290 | if(Value(U,F) == Standard_False) |
28cec2ba |
291 | { |
292 | //Warning: No function value found!; |
32ca7a51 |
293 | |
294 | myD1Init = Standard_False; |
295 | return Standard_False; |
28cec2ba |
296 | } |
297 | |
7fd59977 |
298 | myU = U; |
32ca7a51 |
299 | myPc = myPc_old; |
300 | myP = myP_old; |
28cec2ba |
301 | |
7fd59977 |
302 | Vec D1c,D2c; |
303 | Tool::D2(*((Curve*)myC),myU,myPc,D1c,D2c); |
304 | |
305 | Standard_Real Ndu = D1c.Magnitude(); |
32ca7a51 |
306 | if (Ndu <= myTol) // Cas Singulier (PMN 22/04/1998) |
28cec2ba |
307 | { |
308 | //Derivative is approximated by three points |
32ca7a51 |
309 | |
28cec2ba |
310 | //Attention: aDelta value must be greater than same value for "Value(...)" |
311 | // function to avoid of points' collisions. |
32ca7a51 |
312 | const Standard_Real DivisionFactor = 0.01; |
313 | Standard_Real du; |
314 | if((myUsupremum >= RealLast()) || (myUinfium <= RealFirst())) |
315 | du = 0.0; |
316 | else |
317 | du = myUsupremum-myUinfium; |
28cec2ba |
318 | |
32ca7a51 |
319 | const Standard_Real aDelta = Max(du*DivisionFactor,MinStep); |
28cec2ba |
320 | |
32ca7a51 |
321 | Standard_Real F1, F2, F3; |
28cec2ba |
322 | |
32ca7a51 |
323 | if(myU-myUinfium < 2*aDelta) |
28cec2ba |
324 | { |
32ca7a51 |
325 | F1=F; |
cc9d78db |
326 | //const Standard_Real U1 = myU; |
327 | const Standard_Real U2 = myU + aDelta; |
328 | const Standard_Real U3 = myU + aDelta * 2.0; |
28cec2ba |
329 | |
32ca7a51 |
330 | if(!((Value(U2,F2)) && (Value(U3,F3)))) |
28cec2ba |
331 | { |
332 | //There are many points close to singularity points and |
333 | //which have zero-derivative. Try to decrease aDelta variable's value. |
334 | |
32ca7a51 |
335 | myD1Init = Standard_False; |
336 | return Standard_False; |
32ca7a51 |
337 | } |
28cec2ba |
338 | |
339 | //After calling of Value(...) function variable myU will be redeterminated. |
340 | //So we must return it previous value. |
341 | D1f=(-3*F1+4*F2-F3)/(2.0*aDelta); |
342 | } |
32ca7a51 |
343 | else |
28cec2ba |
344 | { |
32ca7a51 |
345 | F3 = F; |
cc9d78db |
346 | const Standard_Real U1 = myU - aDelta * 2.0; |
347 | const Standard_Real U2 = myU - aDelta; |
348 | //const Standard_Real U3 = myU; |
349 | |
32ca7a51 |
350 | if(!((Value(U2,F2)) && (Value(U1,F1)))) |
28cec2ba |
351 | { |
352 | //There are many points close to singularity points and |
353 | //which have zero-derivative. Try to decrease aDelta variable's value. |
32ca7a51 |
354 | myD1Init = Standard_False; |
355 | return Standard_False; |
32ca7a51 |
356 | } |
28cec2ba |
357 | //After calling of Value(...) function variable myU will be redeterminated. |
358 | //So we must return it previous value. |
359 | D1f=(F1-4*F2+3*F3)/(2.0*aDelta); |
32ca7a51 |
360 | } |
28cec2ba |
361 | myU = U; |
362 | myPc = myPc_old; |
363 | myP = myP_old; |
364 | } |
32ca7a51 |
365 | else |
28cec2ba |
366 | { |
32ca7a51 |
367 | Vec PPc (myP,myPc); |
368 | D1f = Ndu + (PPc.Dot(D2c)/Ndu) - F*(D1c.Dot(D2c))/(Ndu*Ndu); |
28cec2ba |
369 | } |
7fd59977 |
370 | |
371 | myD1f = D1f; |
32ca7a51 |
372 | |
7fd59977 |
373 | myD1Init = Standard_True; |
374 | return Standard_True; |
28cec2ba |
375 | } |
7fd59977 |
376 | //============================================================================= |
377 | |
378 | Standard_Integer Extrema_FuncExtPC::GetStateNumber () |
379 | { |
9775fa61 |
380 | if (!myPinit || !myCinit) throw Standard_TypeMismatch(); |
7fd59977 |
381 | mySqDist.Append(myPc.SquareDistance(myP)); |
c8bf1eb7 |
382 | |
383 | // It is necessary to always compute myD1f. |
384 | myD1Init = Standard_True; |
385 | Standard_Real FF, DD; |
386 | Values(myU, FF, DD); |
387 | |
388 | Standard_Integer IntVal = 0; |
389 | if (myD1f > 0.0) |
390 | { |
391 | IntVal = 1; |
7fd59977 |
392 | } |
c8bf1eb7 |
393 | |
7fd59977 |
394 | myIsMin.Append(IntVal); |
395 | myPoint.Append(POnC(myU,myPc)); |
396 | return 0; |
397 | } |
398 | //============================================================================= |
399 | |
400 | Standard_Integer Extrema_FuncExtPC::NbExt () const { return mySqDist.Length(); } |
401 | //============================================================================= |
402 | |
403 | Standard_Real Extrema_FuncExtPC::SquareDistance (const Standard_Integer N) const |
404 | { |
9775fa61 |
405 | if (!myPinit || !myCinit) throw Standard_TypeMismatch(); |
7fd59977 |
406 | return mySqDist.Value(N); |
407 | } |
408 | //============================================================================= |
409 | Standard_Boolean Extrema_FuncExtPC::IsMin (const Standard_Integer N) const |
410 | { |
9775fa61 |
411 | if (!myPinit || !myCinit) throw Standard_TypeMismatch(); |
7fd59977 |
412 | return (myIsMin.Value(N) == 1); |
413 | } |
414 | //============================================================================= |
5d99f2c8 |
415 | const POnC & Extrema_FuncExtPC::Point (const Standard_Integer N) const |
7fd59977 |
416 | { |
9775fa61 |
417 | if (!myPinit || !myCinit) throw Standard_TypeMismatch(); |
7fd59977 |
418 | return myPoint.Value(N); |
419 | } |
420 | //============================================================================= |
421 | |
32ca7a51 |
422 | void Extrema_FuncExtPC::SubIntervalInitialize(const Standard_Real theUfirst, const Standard_Real theUlast) |
28cec2ba |
423 | { |
32ca7a51 |
424 | myUinfium = theUfirst; |
425 | myUsupremum = theUlast; |
28cec2ba |
426 | } |