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