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