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 | // lpa, le 11/09/91 |
16 | |
17 | |
18 | // Application de la methode du gradient corrige pour minimiser |
19 | // F = somme(||C(ui, Poles(ui)) - ptli||2. |
20 | // La methode de gradient conjugue est programmee dans la bibliotheque |
21 | // mathematique: math_BFGS. |
22 | |
23 | |
24 | #define No_Standard_RangeError |
25 | #define No_Standard_OutOfRange |
26 | |
27 | #include <AppParCurves_Constraint.hxx> |
28 | #include <AppParCurves_ConstraintCouple.hxx> |
29 | #include <math_BFGS.hxx> |
30 | #include <StdFail_NotDone.hxx> |
31 | #include <AppParCurves_MultiPoint.hxx> |
32 | #include <gp_Pnt.hxx> |
33 | #include <gp_Pnt2d.hxx> |
34 | #include <gp_Vec.hxx> |
35 | #include <gp_Vec2d.hxx> |
36 | #include <TColgp_Array1OfPnt.hxx> |
37 | #include <TColgp_Array1OfPnt2d.hxx> |
38 | #include <TColgp_Array1OfVec.hxx> |
39 | #include <TColgp_Array1OfVec2d.hxx> |
40 | |
41 | #include <OSD_Chronometer.hxx> |
42 | |
43 | static OSD_Chronometer chr1; |
44 | |
45 | |
7fd59977 |
46 | |
47 | static AppParCurves_Constraint FirstConstraint |
48 | (const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints, |
49 | const Standard_Integer FirstPoint) |
50 | { |
51 | Standard_Integer i, myindex; |
52 | Standard_Integer low = TheConstraints->Lower(), upp= TheConstraints->Upper(); |
53 | AppParCurves_ConstraintCouple mycouple; |
54 | AppParCurves_Constraint Cons = AppParCurves_NoConstraint; |
55 | |
56 | for (i = low; i <= upp; i++) { |
57 | mycouple = TheConstraints->Value(i); |
58 | Cons = mycouple.Constraint(); |
59 | myindex = mycouple.Index(); |
60 | if (myindex == FirstPoint) { |
61 | break; |
62 | } |
63 | } |
64 | return Cons; |
65 | } |
66 | |
67 | |
68 | static AppParCurves_Constraint LastConstraint |
69 | (const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints, |
70 | const Standard_Integer LastPoint) |
71 | { |
72 | Standard_Integer i, myindex; |
73 | Standard_Integer low = TheConstraints->Lower(), upp= TheConstraints->Upper(); |
74 | AppParCurves_ConstraintCouple mycouple; |
75 | AppParCurves_Constraint Cons = AppParCurves_NoConstraint; |
76 | |
77 | for (i = low; i <= upp; i++) { |
78 | mycouple = TheConstraints->Value(i); |
79 | Cons = mycouple.Constraint(); |
80 | myindex = mycouple.Index(); |
81 | if (myindex == LastPoint) { |
82 | break; |
83 | } |
84 | } |
85 | return Cons; |
86 | } |
87 | |
88 | |
89 | |
90 | |
91 | |
92 | AppParCurves_BSpGradient:: |
93 | AppParCurves_BSpGradient(const MultiLine& SSP, |
94 | const Standard_Integer FirstPoint, |
95 | const Standard_Integer LastPoint, |
96 | const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints, |
97 | math_Vector& Parameters, |
98 | const TColStd_Array1OfReal& Knots, |
99 | const TColStd_Array1OfInteger& Mults, |
100 | const Standard_Integer Deg, |
101 | const Standard_Real Tol3d, |
102 | const Standard_Real Tol2d, |
103 | const Standard_Integer NbIterations): |
89bc1237 |
104 | ParError(FirstPoint, LastPoint,0.0), |
105 | mylambda1(0.0), |
106 | mylambda2(0.0), |
107 | myIsLambdaDefined(Standard_False) |
7fd59977 |
108 | { |
109 | Perform(SSP, FirstPoint, LastPoint, TheConstraints, Parameters, |
110 | Knots, Mults, Deg, Tol3d, Tol2d, NbIterations); |
111 | } |
112 | |
113 | |
114 | AppParCurves_BSpGradient:: |
115 | AppParCurves_BSpGradient(const MultiLine& SSP, |
116 | const Standard_Integer FirstPoint, |
117 | const Standard_Integer LastPoint, |
118 | const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints, |
119 | math_Vector& Parameters, |
120 | const TColStd_Array1OfReal& Knots, |
121 | const TColStd_Array1OfInteger& Mults, |
122 | const Standard_Integer Deg, |
123 | const Standard_Real Tol3d, |
124 | const Standard_Real Tol2d, |
125 | const Standard_Integer NbIterations, |
126 | const Standard_Real lambda1, |
127 | const Standard_Real lambda2): |
89bc1237 |
128 | ParError(FirstPoint, LastPoint,0.0), |
129 | mylambda1(lambda1), |
130 | mylambda2(lambda2), |
131 | myIsLambdaDefined(Standard_True) |
7fd59977 |
132 | { |
7fd59977 |
133 | Perform(SSP, FirstPoint, LastPoint, TheConstraints, Parameters, |
134 | Knots, Mults, Deg, Tol3d, Tol2d, NbIterations); |
135 | } |
136 | |
137 | |
138 | |
139 | |
140 | |
141 | |
142 | |
143 | void AppParCurves_BSpGradient:: |
144 | Perform(const MultiLine& SSP, |
145 | const Standard_Integer FirstPoint, |
146 | const Standard_Integer LastPoint, |
147 | const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints, |
148 | math_Vector& Parameters, |
149 | const TColStd_Array1OfReal& Knots, |
150 | const TColStd_Array1OfInteger& Mults, |
151 | const Standard_Integer Deg, |
152 | const Standard_Real Tol3d, |
153 | const Standard_Real Tol2d, |
154 | const Standard_Integer NbIterations) |
155 | { |
156 | |
157 | // Standard_Boolean grad = Standard_True; |
158 | Standard_Integer i, j, k, i2, l; |
159 | Standard_Real UF, DU, Fval = 0.0, FU, DFU; |
160 | Standard_Integer nbP3d = ToolLine::NbP3d(SSP); |
161 | Standard_Integer nbP2d = ToolLine::NbP2d(SSP); |
162 | Standard_Integer mynbP3d=nbP3d, mynbP2d=nbP2d; |
163 | Standard_Integer nbP = nbP3d + nbP2d; |
164 | // gp_Pnt Pt, P1, P2; |
165 | gp_Pnt Pt; |
166 | // gp_Pnt2d Pt2d, P12d, P22d; |
167 | gp_Pnt2d Pt2d; |
168 | // gp_Vec V1, V2, MyV; |
169 | gp_Vec V1, MyV; |
170 | // gp_Vec2d V12d, V22d, MyV2d; |
171 | gp_Vec2d V12d, MyV2d; |
172 | Done = Standard_False; |
173 | |
174 | if (nbP3d == 0) mynbP3d = 1; |
175 | if (nbP2d == 0) mynbP2d = 1; |
176 | TColgp_Array1OfPnt TabP(1, mynbP3d); |
177 | TColgp_Array1OfPnt2d TabP2d(1, mynbP2d); |
178 | TColgp_Array1OfVec TabV(1, mynbP3d); |
179 | TColgp_Array1OfVec2d TabV2d(1, mynbP2d); |
180 | |
181 | // Calcul de la fonction F= somme(||C(ui)-Ptli||2): |
182 | // Appel a une fonction heritant de MultipleVarFunctionWithGradient |
183 | // pour calculer F et grad_F. |
184 | // ================================================================ |
185 | |
186 | Standard_Integer nbpoles = -Deg -1; |
187 | for (i = Mults.Lower() ;i <= Mults.Upper(); i++) { |
188 | nbpoles += Mults(i); |
189 | } |
190 | |
191 | TColgp_Array1OfPnt TabPole(1, nbpoles); |
192 | TColgp_Array1OfPnt2d TabPole2d(1, nbpoles); |
193 | TColgp_Array1OfPnt ThePoles(1, (nbpoles)*mynbP3d); |
194 | TColgp_Array1OfPnt2d ThePoles2d(1, (nbpoles)*mynbP2d); |
195 | |
196 | |
197 | AppParCurves_Constraint |
198 | FirstCons = FirstConstraint(TheConstraints, FirstPoint), |
199 | LastCons = LastConstraint(TheConstraints, LastPoint); |
200 | |
201 | |
202 | AppParCurves_BSpParFunction MyF(SSP, FirstPoint,LastPoint, TheConstraints, |
203 | Parameters, Knots, Mults, nbpoles); |
204 | |
205 | |
206 | |
207 | if (FirstCons >= AppParCurves_TangencyPoint || |
208 | LastCons >= AppParCurves_TangencyPoint) { |
209 | |
89bc1237 |
210 | if (!myIsLambdaDefined) { |
7fd59977 |
211 | AppParCurves_BSpParLeastSquare thefitt(SSP, Knots, Mults, FirstPoint, |
212 | LastPoint, FirstCons, LastCons, |
213 | Parameters, nbpoles); |
214 | if (FirstCons >= AppParCurves_TangencyPoint) { |
215 | mylambda1 = thefitt.FirstLambda(); |
216 | MyF.SetFirstLambda(mylambda1); |
217 | } |
218 | if (LastCons >= AppParCurves_TangencyPoint) { |
219 | mylambda2 = thefitt.LastLambda(); |
220 | MyF.SetLastLambda(mylambda2); |
221 | } |
222 | } |
223 | else { |
224 | MyF.SetFirstLambda(mylambda1); |
225 | MyF.SetLastLambda(mylambda2); |
226 | } |
227 | } |
228 | |
229 | |
230 | MyF.Value(Parameters, Fval); |
231 | MError3d = MyF.MaxError3d(); |
232 | MError2d = MyF.MaxError2d(); |
233 | SCU = MyF.CurveValue(); |
234 | |
235 | if (MError3d > Tol3d || MError2d > Tol2d) { |
236 | |
237 | // Stockage des Poles des courbes pour projeter: |
238 | // ============================================ |
239 | i2 = 0; |
240 | for (k = 1; k <= nbP3d; k++) { |
241 | SCU.Curve(k, TabPole); |
242 | for (j=1; j<=nbpoles; j++) ThePoles(j+i2) = TabPole(j); |
243 | i2 += nbpoles; |
244 | } |
245 | i2 = 0; |
246 | for (k = 1; k <= nbP2d; k++) { |
247 | SCU.Curve(nbP3d+k, TabPole2d); |
248 | for (j=1; j<=nbpoles; j++) ThePoles2d(j+i2) = TabPole2d(j); |
249 | i2 += nbpoles; |
250 | } |
251 | |
252 | // Une iteration rapide de projection est faite par la methode de |
253 | // Rogers & Fog 89, methode equivalente a Hoschek 88 qui ne necessite pas |
254 | // le calcul de D2. |
255 | |
256 | const math_Matrix& A = MyF.FunctionMatrix(); |
257 | const math_Matrix& DA = MyF.DerivativeFunctionMatrix(); |
258 | const math_IntegerVector& Index = MyF.Index(); |
259 | Standard_Real aa, da, a, b, c, d , e , f, px, py, pz; |
260 | Standard_Integer indexdeb, indexfin; |
261 | |
262 | for (j = FirstPoint+1; j <= LastPoint-1; j++) { |
263 | |
264 | UF = Parameters(j); |
265 | if (nbP3d != 0 && nbP2d != 0) ToolLine::Value(SSP, j, TabP, TabP2d); |
266 | else if (nbP2d != 0) ToolLine::Value(SSP, j, TabP2d); |
267 | else ToolLine::Value(SSP, j, TabP); |
268 | |
269 | FU = 0.0; |
270 | DFU = 0.0; |
271 | i2 = 0; |
272 | |
273 | indexdeb = Index(j) + 1; |
274 | indexfin = indexdeb + Deg; |
275 | |
276 | for (k = 1; k <= nbP3d; k++) { |
277 | a = b = c = d = e = f = 0.0; |
278 | for (l = indexdeb; l <= indexfin; l++) { |
279 | Pt = ThePoles(l+i2); |
280 | px = Pt.X(); py = Pt.Y(); pz = Pt.Z(); |
281 | aa = A(j, l); da = DA(j, l); |
282 | a += aa* px; d += da* px; |
283 | b += aa* py; e += da* py; |
284 | c += aa* pz; f += da* pz; |
285 | } |
286 | Pt.SetCoord(a, b, c); |
287 | V1.SetCoord(d, e, f); |
288 | i2 += nbpoles; |
289 | |
290 | MyV = gp_Vec(Pt, TabP(k)); |
291 | FU += MyV*V1; |
292 | DFU += V1.SquareMagnitude(); |
293 | } |
294 | i2 = 0; |
295 | for (k = 1; k <= nbP2d; k++) { |
296 | a = b = d = e = 0.0; |
297 | for (l = indexdeb; l <= indexfin; l++) { |
298 | Pt2d = ThePoles2d(l+i2); |
299 | px = Pt2d.X(); py = Pt2d.Y(); |
300 | aa = A(j, l); da = DA(j, l); |
301 | a += aa* px; d += da* px; |
302 | b += aa* py; e += da* py; |
303 | } |
304 | Pt2d.SetCoord(a, b); |
305 | V12d.SetCoord(d, e); |
306 | i2 += nbpoles; |
307 | |
308 | MyV2d = gp_Vec2d(Pt2d, TabP2d(k)); |
309 | FU += MyV2d*V12d; |
310 | DFU += V12d.SquareMagnitude(); |
311 | } |
312 | |
313 | if (DFU >= RealEpsilon()) { |
314 | DU = FU/DFU; |
315 | DU = Sign(Min(5.e-02, Abs(DU)), DU); |
316 | UF += DU; |
317 | Parameters(j) = UF; |
318 | } |
319 | } |
320 | |
321 | MyF.Value(Parameters, Fval); |
322 | MError3d = MyF.MaxError3d(); |
323 | MError2d = MyF.MaxError2d(); |
324 | } |
325 | |
326 | |
327 | if (MError3d<= Tol3d && MError2d <= Tol2d) { |
328 | Done = Standard_True; |
329 | } |
330 | else if (NbIterations != 0) { |
331 | // NbIterations de gradient conjugue: |
332 | // ================================= |
333 | Standard_Real Eps = 1.e-07; |
334 | AppParCurves_BSpGradient_BFGS FResol(MyF, Parameters, Tol3d, |
335 | Tol2d, Eps, NbIterations); |
336 | } |
337 | |
338 | SCU = MyF.CurveValue(); |
339 | |
340 | |
341 | AvError = 0.; |
342 | for (j = FirstPoint; j <= LastPoint; j++) { |
343 | Parameters(j) = MyF.NewParameters()(j); |
344 | // Recherche des erreurs maxi et moyenne a un index donne: |
345 | for (k = 1; k <= nbP; k++) { |
346 | ParError(j) = Max(ParError(j), MyF.Error(j, k)); |
347 | } |
348 | AvError += ParError(j); |
349 | } |
350 | AvError = AvError/(LastPoint-FirstPoint+1); |
351 | |
352 | |
353 | MError3d = MyF.MaxError3d(); |
354 | MError2d = MyF.MaxError2d(); |
355 | if (MError3d <= Tol3d && MError2d <= Tol2d) { |
356 | Done = Standard_True; |
357 | } |
358 | |
359 | } |
360 | |
361 | |
362 | |
363 | AppParCurves_MultiBSpCurve AppParCurves_BSpGradient::Value() const { |
364 | return SCU; |
365 | } |
366 | |
367 | |
368 | Standard_Boolean AppParCurves_BSpGradient::IsDone() const { |
369 | return Done; |
370 | } |
371 | |
372 | |
373 | Standard_Real AppParCurves_BSpGradient::Error(const Standard_Integer Index) const { |
374 | return ParError(Index); |
375 | } |
376 | |
377 | Standard_Real AppParCurves_BSpGradient::AverageError() const { |
378 | return AvError; |
379 | } |
380 | |
381 | Standard_Real AppParCurves_BSpGradient::MaxError3d() const { |
382 | return MError3d; |
383 | } |
384 | |
385 | Standard_Real AppParCurves_BSpGradient::MaxError2d() const { |
386 | return MError2d; |
387 | } |
388 | |
389 | |