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 27/07/91 |
16 | // pmn, 30/10/98 : Protection dans les cas surcontraint -> "isready" |
17 | |
18 | // Approximation d une MultiLine de points decrite par le tool MLineTool. |
19 | // Ce programme utilise les moindres carres pour le cas suivant: |
20 | // passage et tangences aux extremites. |
21 | |
22 | |
23 | #define No_Standard_RangeError |
24 | #define No_Standard_OutOfRange |
25 | #define No_Standard_DimensionError |
26 | |
27 | #include <math_Householder.hxx> |
28 | #include <math_Crout.hxx> |
29 | #include <AppParCurves.hxx> |
30 | #include <gp_Pnt.hxx> |
31 | #include <gp_Pnt2d.hxx> |
32 | #include <gp_Vec.hxx> |
33 | #include <gp_Vec2d.hxx> |
34 | #include <TColgp_Array1OfPnt.hxx> |
35 | #include <TColgp_Array1OfPnt2d.hxx> |
36 | #include <TColgp_Array1OfVec.hxx> |
37 | #include <TColgp_Array1OfVec2d.hxx> |
38 | #include <TColStd_Array1OfInteger.hxx> |
39 | #include <TColStd_Array1OfReal.hxx> |
40 | #include <AppParCurves_Constraint.hxx> |
41 | #include <AppParCurves_MultiPoint.hxx> |
42 | #include <StdFail_NotDone.hxx> |
43 | #include <Standard_NoSuchObject.hxx> |
44 | #include <TColStd_Array1OfReal.hxx> |
45 | #include <math_Recipes.hxx> |
46 | #include <math_Crout.hxx> |
47 | |
48 | |
49 | |
50 | static int FlatLength(const TColStd_Array1OfInteger& Mults) { |
51 | |
52 | Standard_Integer sum = 0; |
53 | for (Standard_Integer i = Mults.Lower(); i <= Mults.Upper(); i++) { |
54 | sum += Mults.Value(i); |
55 | } |
56 | return sum; |
57 | } |
58 | |
59 | |
60 | AppParCurves_LeastSquare:: |
61 | AppParCurves_LeastSquare(const MultiLine& SSP, |
62 | const Standard_Integer FirstPoint, |
63 | const Standard_Integer LastPoint, |
64 | const AppParCurves_Constraint FirstCons, |
65 | const AppParCurves_Constraint LastCons, |
66 | const math_Vector& Parameters, |
67 | const Standard_Integer NbPol): |
68 | SCU(NbPol), |
69 | mypoles(1, NbPol, |
70 | 1, NbBColumns(SSP)), |
71 | A(FirstPoint, LastPoint, 1, NbPol), |
72 | DA(FirstPoint, LastPoint, 1, NbPol), |
73 | B2(TheFirstPoint(FirstCons, FirstPoint), |
74 | Max(TheFirstPoint(FirstCons, FirstPoint), |
75 | TheLastPoint(LastCons, LastPoint)), |
76 | 1, NbBColumns(SSP)), |
77 | mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)), |
78 | Vflatknots(1, 1), |
79 | Vec1t(1, NbBColumns(SSP)), |
80 | Vec1c(1, NbBColumns(SSP)), |
81 | Vec2t(1, NbBColumns(SSP)), |
82 | Vec2c(1, NbBColumns(SSP)), |
83 | theError(FirstPoint, LastPoint, |
84 | 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0), |
85 | myindex(FirstPoint, LastPoint, 0), |
86 | nbpoles(NbPol) |
87 | { |
88 | FirstConstraint = FirstCons; |
89 | LastConstraint = LastCons; |
90 | Init(SSP, FirstPoint, LastPoint); |
91 | Perform(Parameters); |
92 | } |
93 | |
94 | |
95 | |
96 | AppParCurves_LeastSquare:: |
97 | AppParCurves_LeastSquare(const MultiLine& SSP, |
98 | const Standard_Integer FirstPoint, |
99 | const Standard_Integer LastPoint, |
100 | const AppParCurves_Constraint FirstCons, |
101 | const AppParCurves_Constraint LastCons, |
102 | const Standard_Integer NbPol): |
103 | SCU(NbPol), |
104 | mypoles(1, NbPol, |
105 | 1, NbBColumns(SSP)), |
106 | A(FirstPoint, LastPoint, 1, NbPol), |
107 | DA(FirstPoint, LastPoint, 1, NbPol), |
108 | B2(TheFirstPoint(FirstCons, FirstPoint), |
109 | Max(TheFirstPoint(FirstCons, FirstPoint), |
110 | TheLastPoint(LastCons, LastPoint)), |
111 | 1, NbBColumns(SSP)), |
112 | mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)), |
113 | Vflatknots(1, 1), |
114 | Vec1t(1, NbBColumns(SSP)), |
115 | Vec1c(1, NbBColumns(SSP)), |
116 | Vec2t(1, NbBColumns(SSP)), |
117 | Vec2c(1, NbBColumns(SSP)), |
118 | theError(FirstPoint, LastPoint, |
119 | 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0), |
120 | myindex(FirstPoint, LastPoint, 0), |
121 | nbpoles(NbPol) |
122 | { |
123 | FirstConstraint = FirstCons; |
124 | LastConstraint = LastCons; |
125 | Init(SSP, FirstPoint, LastPoint); |
126 | } |
127 | |
128 | |
129 | AppParCurves_LeastSquare:: |
130 | AppParCurves_LeastSquare(const MultiLine& SSP, |
131 | const TColStd_Array1OfReal& Knots, |
132 | const TColStd_Array1OfInteger& Mults, |
133 | const Standard_Integer FirstPoint, |
134 | const Standard_Integer LastPoint, |
135 | const AppParCurves_Constraint FirstCons, |
136 | const AppParCurves_Constraint LastCons, |
137 | const math_Vector& Parameters, |
138 | const Standard_Integer NbPol): |
139 | SCU(NbPol), |
140 | mypoles(1, NbPol, |
141 | 1, NbBColumns(SSP)), |
142 | A(FirstPoint, LastPoint, 1, NbPol), |
143 | DA(FirstPoint, LastPoint, 1, NbPol), |
144 | B2(TheFirstPoint(FirstCons, FirstPoint), |
145 | Max(TheFirstPoint(FirstCons, FirstPoint), |
146 | TheLastPoint(LastCons, LastPoint)), |
147 | 1, NbBColumns(SSP)), |
148 | mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)), |
149 | Vflatknots(1, FlatLength(Mults)), |
150 | Vec1t(1, NbBColumns(SSP)), |
151 | Vec1c(1, NbBColumns(SSP)), |
152 | Vec2t(1, NbBColumns(SSP)), |
153 | Vec2c(1, NbBColumns(SSP)), |
154 | theError(FirstPoint, LastPoint, |
155 | 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0), |
156 | myindex(FirstPoint, LastPoint, 0), |
157 | nbpoles(NbPol) |
158 | { |
159 | FirstConstraint = FirstCons; |
160 | LastConstraint = LastCons; |
161 | myknots = new TColStd_HArray1OfReal(Knots.Lower(), Knots.Upper()); |
162 | myknots->ChangeArray1() = Knots; |
163 | mymults = new TColStd_HArray1OfInteger(Mults.Lower(), Mults.Upper()); |
164 | mymults->ChangeArray1() = Mults; |
165 | SCU.SetKnots(Knots); |
166 | SCU.SetMultiplicities(Mults); |
167 | Init(SSP, FirstPoint, LastPoint); |
168 | Perform(Parameters); |
169 | } |
170 | |
171 | |
172 | |
173 | AppParCurves_LeastSquare:: |
174 | AppParCurves_LeastSquare(const MultiLine& SSP, |
175 | const TColStd_Array1OfReal& Knots, |
176 | const TColStd_Array1OfInteger& Mults, |
177 | const Standard_Integer FirstPoint, |
178 | const Standard_Integer LastPoint, |
179 | const AppParCurves_Constraint FirstCons, |
180 | const AppParCurves_Constraint LastCons, |
181 | const Standard_Integer NbPol): |
182 | SCU(NbPol), |
183 | mypoles(1, NbPol, |
184 | 1, NbBColumns(SSP)), |
185 | A(FirstPoint, LastPoint, 1, NbPol), |
186 | DA(FirstPoint, LastPoint, 1, NbPol), |
187 | B2(TheFirstPoint(FirstCons, FirstPoint), |
188 | Max(TheFirstPoint(FirstCons, FirstPoint), |
189 | TheLastPoint(LastCons, LastPoint)), |
190 | 1, NbBColumns(SSP)), |
191 | mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)), |
192 | Vflatknots(1, FlatLength(Mults)), |
193 | Vec1t(1, NbBColumns(SSP)), |
194 | Vec1c(1, NbBColumns(SSP)), |
195 | Vec2t(1, NbBColumns(SSP)), |
196 | Vec2c(1, NbBColumns(SSP)), |
197 | theError(FirstPoint, LastPoint, |
198 | 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0), |
199 | myindex(FirstPoint, LastPoint, 0), |
200 | nbpoles(NbPol) |
201 | { |
202 | myknots = new TColStd_HArray1OfReal(Knots.Lower(), Knots.Upper()); |
203 | myknots->ChangeArray1() = Knots; |
204 | mymults = new TColStd_HArray1OfInteger(Mults.Lower(), Mults.Upper()); |
205 | mymults->ChangeArray1() = Mults; |
206 | SCU.SetKnots(Knots); |
207 | SCU.SetMultiplicities(Mults); |
208 | FirstConstraint = FirstCons; |
209 | LastConstraint = LastCons; |
210 | Init(SSP, FirstPoint, LastPoint); |
211 | } |
212 | |
213 | |
214 | |
215 | void AppParCurves_LeastSquare::Init(const MultiLine& SSP, |
216 | const Standard_Integer FirstPoint, |
217 | const Standard_Integer LastPoint) |
218 | { |
219 | // Variable de controle |
220 | iscalculated = Standard_False; |
221 | isready = Standard_True; |
222 | |
223 | myfirstp = FirstPoint; |
224 | mylastp = LastPoint; |
225 | FirstP = TheFirstPoint(FirstConstraint, myfirstp); |
226 | LastP = TheLastPoint(LastConstraint, mylastp); |
227 | |
228 | // Reperage des contraintes aux extremites: |
229 | // ======================================== |
230 | Standard_Integer i, j, k, i2; |
231 | |
232 | nbP2d = ToolLine::NbP2d(SSP); |
233 | nbP = ToolLine::NbP3d(SSP); |
234 | gp_Pnt Poi; |
235 | gp_Pnt2d Poi2d; |
236 | // gp_Vec V3d; |
237 | // gp_Vec2d V2d; |
238 | Standard_Integer mynbP2d = nbP2d, mynbP = nbP; |
239 | if (nbP2d == 0) mynbP2d = 1; |
240 | if (nbP == 0) mynbP = 1; |
241 | TColgp_Array1OfPnt TabP(1, mynbP); |
242 | TColgp_Array1OfPnt2d TabP2d(1, mynbP2d); |
243 | TColgp_Array1OfVec TabV(1, mynbP); |
244 | TColgp_Array1OfVec2d TabV2d(1, mynbP2d); |
245 | |
246 | |
247 | deg = nbpoles-1; |
248 | |
249 | if (!mymults.IsNull()) { |
250 | Standard_Integer sum = 0; |
251 | for (i = mymults->Lower(); i <= mymults->Upper(); i++) { |
252 | sum += mymults->Value(i); |
253 | } |
254 | deg = sum -nbpoles-1; |
255 | k = 1; |
256 | Standard_Real val; |
257 | for (i = myknots->Lower(); i <= myknots->Upper(); i++) { |
258 | for (j = 1; j <= mymults->Value(i); j++) { |
259 | val = myknots->Value(i); |
260 | Vflatknots(k) = val; |
261 | k++; |
262 | } |
263 | } |
264 | } |
265 | |
266 | |
267 | Affect(SSP, FirstPoint, FirstConstraint, Vec1t, Vec1c); |
268 | |
269 | Affect(SSP, LastPoint, LastConstraint, Vec2t, Vec2c); |
270 | |
271 | for (j = myfirstp; j <= mylastp; j++) { |
272 | i2 = 1; |
273 | if (nbP != 0 && nbP2d != 0) ToolLine::Value(SSP, j, TabP,TabP2d); |
274 | else if (nbP2d != 0) ToolLine::Value(SSP, j, TabP2d); |
275 | else ToolLine::Value(SSP, j, TabP); |
276 | for (i = 1; i <= nbP; i++) { |
277 | (TabP(i)).Coord(mypoints(j,i2),mypoints(j,i2+1),mypoints(j,i2+2)); |
278 | i2 += 3; |
279 | } |
280 | for (i = 1;i <= nbP2d; i++) { |
281 | (TabP2d(i)).Coord(mypoints(j, i2), mypoints(j, i2+1)); |
282 | i2 += 2; |
283 | } |
284 | } |
285 | |
286 | AppParCurves_MultiPoint Pole1(nbP, nbP2d), PoleN(nbP, nbP2d); |
287 | |
288 | if (FirstConstraint == AppParCurves_PassPoint || |
289 | FirstConstraint == AppParCurves_TangencyPoint || |
290 | FirstConstraint == AppParCurves_CurvaturePoint) { |
291 | i2 = 1; |
292 | for (i = 1; i <= nbP; i++) { |
293 | Poi.SetCoord(mypoints(myfirstp, i2), |
294 | mypoints(myfirstp, i2+1), |
295 | mypoints(myfirstp, i2+2)); |
296 | Pole1.SetPoint(i, Poi); |
297 | i2 += 3; |
298 | } |
299 | for (i = 1; i <= nbP2d; i++) { |
300 | Poi2d.SetCoord(mypoints(myfirstp, i2), mypoints(myfirstp, i2+1)); |
301 | Pole1.SetPoint2d(i+nbP, Poi2d); |
302 | i2 += 2; |
303 | } |
304 | for (i = 1; i <= mypoles.ColNumber(); i++) |
305 | mypoles(1, i) = mypoints(myfirstp, i); |
306 | } |
307 | |
308 | |
309 | |
310 | if (LastConstraint == AppParCurves_PassPoint || |
311 | LastConstraint == AppParCurves_TangencyPoint || |
312 | FirstConstraint == AppParCurves_CurvaturePoint) { |
313 | i2 = 1; |
314 | for (i = 1; i <= nbP; i++) { |
315 | Poi.SetCoord(mypoints(mylastp, i2), |
316 | mypoints(mylastp, i2+1), |
317 | mypoints(mylastp, i2+2)); |
318 | PoleN.SetPoint(i, Poi); |
319 | i2 += 3; |
320 | } |
321 | for (i = 1; i <= nbP2d; i++) { |
322 | Poi2d.SetCoord(mypoints(mylastp, i2), |
323 | mypoints(mylastp, i2+1)); |
324 | PoleN.SetPoint2d(i+nbP, Poi2d); |
325 | i2 += 2; |
326 | } |
327 | |
328 | for (i = 1; i <= mypoles.ColNumber(); i++) |
329 | mypoles(nbpoles, i) = mypoints(mylastp, i); |
330 | } |
331 | |
332 | |
333 | if (FirstConstraint == AppParCurves_NoConstraint) { |
334 | resinit = 1; |
335 | SCU.SetValue(1, Pole1); |
336 | if (LastConstraint == AppParCurves_NoConstraint) { |
337 | resfin = nbpoles; |
338 | } |
339 | else if (LastConstraint == AppParCurves_PassPoint) { |
340 | resfin = nbpoles-1; |
341 | SCU.SetValue(nbpoles, PoleN); |
342 | } |
343 | else if (LastConstraint == AppParCurves_TangencyPoint) { |
344 | resfin = nbpoles-2; |
345 | SCU.SetValue(nbpoles, PoleN); |
346 | } |
347 | else if (LastConstraint == AppParCurves_CurvaturePoint) { |
348 | resfin = nbpoles-3; |
349 | SCU.SetValue(nbpoles, PoleN); |
350 | } |
351 | } |
352 | else if (FirstConstraint == AppParCurves_PassPoint) { |
353 | resinit = 2; |
354 | SCU.SetValue(1, Pole1); |
355 | if (LastConstraint == AppParCurves_NoConstraint) { |
356 | resfin = nbpoles; |
357 | } |
358 | else if (LastConstraint == AppParCurves_PassPoint) { |
359 | resfin = nbpoles-1; |
360 | SCU.SetValue(nbpoles, PoleN); |
361 | } |
362 | else if (LastConstraint == AppParCurves_TangencyPoint) { |
363 | resfin = nbpoles-2; |
364 | SCU.SetValue(nbpoles, PoleN); |
365 | } |
366 | else if (LastConstraint == AppParCurves_CurvaturePoint) { |
367 | resfin = nbpoles-3; |
368 | SCU.SetValue(nbpoles, PoleN); |
369 | } |
370 | } |
371 | else if (FirstConstraint == AppParCurves_TangencyPoint) { |
372 | resinit = 3; |
373 | SCU.SetValue(1, Pole1); |
374 | if (LastConstraint == AppParCurves_NoConstraint) { |
375 | resfin = nbpoles; |
376 | } |
377 | if (LastConstraint == AppParCurves_PassPoint) { |
378 | resfin = nbpoles-1; |
379 | SCU.SetValue(nbpoles, PoleN); |
380 | } |
381 | if (LastConstraint == AppParCurves_TangencyPoint) { |
382 | resfin = nbpoles-2; |
383 | SCU.SetValue(nbpoles, PoleN); |
384 | } |
385 | else if (LastConstraint == AppParCurves_CurvaturePoint) { |
386 | resfin = nbpoles-3; |
387 | SCU.SetValue(nbpoles, PoleN); |
388 | } |
389 | } |
390 | else if (FirstConstraint == AppParCurves_CurvaturePoint) { |
391 | resinit = 4; |
392 | SCU.SetValue(1, Pole1); |
393 | if (LastConstraint == AppParCurves_NoConstraint) { |
394 | resfin = nbpoles; |
395 | } |
396 | if (LastConstraint == AppParCurves_PassPoint) { |
397 | resfin = nbpoles-1; |
398 | SCU.SetValue(nbpoles, PoleN); |
399 | } |
400 | if (LastConstraint == AppParCurves_TangencyPoint) { |
401 | resfin = nbpoles-2; |
402 | SCU.SetValue(nbpoles, PoleN); |
403 | } |
404 | else if (LastConstraint == AppParCurves_CurvaturePoint) { |
405 | resfin = nbpoles-3; |
406 | SCU.SetValue(nbpoles, PoleN); |
407 | } |
408 | } |
409 | |
410 | Standard_Integer Nincx = resfin-resinit+1; |
411 | if (Nincx<1) { //Impossible d'aller plus loin |
412 | isready = Standard_False; |
413 | return; |
414 | } |
415 | Standard_Integer Neq = LastP-FirstP+1; |
416 | |
417 | NA = 3*nbP+2*nbP2d; |
418 | Nlignes = NA*Neq; |
419 | Ninc = NA*Nincx; |
420 | if (FirstConstraint >= AppParCurves_TangencyPoint) Ninc++; |
421 | if (LastConstraint >= AppParCurves_TangencyPoint) Ninc++; |
422 | } |
423 | |
424 | |
425 | |
426 | |
427 | void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters) { |
428 | |
429 | done = Standard_False; |
430 | if (!isready) { |
431 | return; |
432 | } |
51740958 |
433 | Standard_Integer i, j, k, Ci, i2, k1, k2; |
7fd59977 |
434 | Standard_Integer nbpol1 = nbpoles-1, Ninc1 = Ninc-1; |
435 | Standard_Real AD1, A0; |
436 | // gp_Pnt Pt; |
437 | // gp_Pnt2d Pt2d; |
438 | iscalculated = Standard_False; |
439 | |
440 | // calcul de la matrice A et DA des fonctions d approximation: |
441 | ComputeFunction(Parameters); |
442 | |
443 | if (FirstConstraint != AppParCurves_TangencyPoint && |
444 | LastConstraint != AppParCurves_TangencyPoint) { |
445 | if (FirstConstraint == AppParCurves_NoConstraint) { |
446 | if (LastConstraint == AppParCurves_NoConstraint ) { |
447 | |
448 | math_Householder HouResol(A, mypoints); |
449 | if (!(HouResol.IsDone())) { |
450 | done = Standard_False; |
451 | return; |
452 | } |
453 | done = Standard_True; |
454 | mypoles = HouResol.AllValues(); |
455 | return; |
456 | |
457 | } |
458 | else { |
459 | for (j = FirstP; j <= LastP; j++) { |
460 | AD1 = A(j, nbpoles); |
461 | for (i = 1; i <= B2.ColNumber(); i++) { |
462 | B2(j, i) = mypoints(j,i) - AD1*mypoles(nbpoles, i); |
463 | } |
464 | } |
465 | } |
466 | } |
467 | else if (FirstConstraint == AppParCurves_PassPoint) { |
468 | if (LastConstraint == AppParCurves_NoConstraint) { |
469 | for (j = FirstP; j <= LastP; j++) { |
470 | A0 = A(j, 1); |
471 | for (i = 1; i <= B2.ColNumber(); i++) { |
472 | B2(j, i) = mypoints(j, i)- A0*mypoles(1, i); |
473 | } |
474 | } |
475 | } |
476 | else if (LastConstraint == AppParCurves_PassPoint) { |
477 | for (j = FirstP; j <= LastP; j++) { |
478 | A0 = A(j, 1); |
479 | AD1 = A(j, nbpoles); |
480 | for (i = 1; i <= B2.ColNumber(); i++) { |
481 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
482 | - AD1* mypoles(nbpoles, i); |
483 | } |
484 | } |
485 | } |
486 | } |
487 | |
488 | // resolution: |
489 | |
490 | Standard_Integer Nincx = resfin-resinit+1; |
491 | if (Nincx < 1) { |
492 | done = Standard_True; |
493 | return; |
494 | } |
495 | math_IntegerVector Index(1, Nincx); |
496 | SearchIndex(Index); |
497 | math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0); |
498 | math_Vector TheAA(1, Index(Nincx), 0.0); |
499 | math_Vector myTABB(1, Nincx, 0.0); |
500 | |
501 | MakeTAA(TheAA, mytab); |
96a95605 |
502 | DACTCL_Decompose(TheAA, Index); |
7fd59977 |
503 | |
504 | Standard_Integer kk2; |
505 | for (j = 1; j <= B2.ColNumber(); j++) { |
506 | kk2 = 1; |
507 | for (i = resinit; i <= resfin; i++) { |
508 | myTABB(kk2) = mytab(i, j); |
509 | kk2++; |
510 | } |
96a95605 |
511 | DACTCL_Solve(TheAA, myTABB, Index); |
7fd59977 |
512 | |
513 | i2 = 1; |
514 | for (k = resinit; k <= resfin; k++) { |
515 | mypoles(k, j) = myTABB.Value(i2); |
516 | i2++; |
517 | } |
518 | } |
519 | done = Standard_True; |
520 | } |
521 | |
522 | // =========================================================== |
523 | // cas de tangence: |
524 | // =========================================================== |
525 | |
51740958 |
526 | Standard_Integer Nincx = resfin-resinit+1; |
7fd59977 |
527 | Standard_Integer deport = 0, Nincx2 = 2*Nincx; |
528 | |
529 | math_IntegerVector InternalIndex(1, Nincx); |
530 | SearchIndex(InternalIndex); |
531 | math_IntegerVector Index(1, Ninc); |
532 | |
533 | Standard_Integer l = 1; |
534 | if (resinit <= resfin) { |
535 | for (j = 0; j <= NA-1; j++) { |
536 | deport = j*InternalIndex(Nincx); |
537 | for (i = 1; i <= Nincx; i++) { |
538 | Index(l) = InternalIndex(i) + deport; |
539 | l++; |
540 | } |
541 | } |
542 | } |
543 | |
544 | if (resinit > resfin) Index(1) = 1; |
545 | if (Ninc1 > 1) { |
546 | if (FirstConstraint >= AppParCurves_TangencyPoint && |
547 | LastConstraint >= AppParCurves_TangencyPoint) |
548 | Index(Ninc1) = Index(Ninc1 - 1) + Ninc1; |
549 | } |
550 | if (FirstConstraint >= AppParCurves_TangencyPoint || |
551 | LastConstraint >= AppParCurves_TangencyPoint) |
552 | Index(Ninc) = Index(Ninc-1) + Ninc; |
553 | |
554 | |
555 | math_Vector TheA(1, Index(Ninc), 0.0); |
556 | math_Vector myTAB(1, Ninc, 0.0); |
557 | |
558 | MakeTAA(TheA, myTAB); |
559 | |
560 | Standard_Integer Error = DACTCL_Decompose(TheA, Index); |
561 | Error = DACTCL_Solve(TheA, myTAB, Index); |
562 | |
563 | if (!Error) done = Standard_True; |
564 | |
565 | if (FirstConstraint >= AppParCurves_TangencyPoint && |
566 | LastConstraint >= AppParCurves_TangencyPoint) { |
567 | lambda1 = myTAB.Value(Ninc1); |
568 | lambda2 = myTAB.Value(Ninc); |
569 | } |
570 | else if (FirstConstraint >= AppParCurves_TangencyPoint) |
571 | lambda1 = myTAB.Value(Ninc); |
572 | else if (LastConstraint >= AppParCurves_TangencyPoint) |
573 | lambda2 = myTAB.Value(Ninc); |
574 | |
575 | |
576 | |
577 | // Les resultats sont stockes dans mypoles. |
578 | //========================================= |
579 | k = 1; |
580 | i2 = 1; |
581 | for (Ci = 1; Ci <= nbP; Ci++) { |
582 | k1 = k+1; k2 = k+2; |
583 | for (j = resinit; j <= resfin; j++) { |
584 | mypoles(j, k) = myTAB.Value(i2); |
585 | mypoles(j, k1) = myTAB.Value(i2+Nincx); |
586 | mypoles(j, k2) = myTAB.Value(i2+Nincx2); |
587 | i2++; |
588 | } |
589 | |
590 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
591 | mypoles(2, k) = mypoints(myfirstp, k) + lambda1*Vec1t(k); |
592 | mypoles(2, k1) = mypoints(myfirstp, k1) + lambda1*Vec1t(k1); |
593 | mypoles(2, k2) = mypoints(myfirstp, k2) + lambda1*Vec1t(k2); |
594 | } |
595 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
596 | mypoles(nbpol1, k) = mypoints(mylastp, k) - lambda2*Vec2t(k); |
597 | mypoles(nbpol1, k1) = mypoints(mylastp, k1) - lambda2*Vec2t(k1); |
598 | mypoles(nbpol1, k2) = mypoints(mylastp, k2) - lambda2*Vec2t(k2); |
599 | } |
600 | k += 3; i2 += Nincx2; |
601 | } |
602 | |
603 | for (Ci = 1; Ci <= nbP2d; Ci++) { |
604 | k1 = k+1; k2 = k+2; |
605 | for (j = resinit; j <= resfin; j++) { |
606 | mypoles(j, k) = myTAB.Value(i2); |
607 | mypoles(j, k1) = myTAB.Value(i2+Nincx); |
608 | i2++; |
609 | } |
610 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
611 | mypoles(2, k) = mypoints(myfirstp, k) + lambda1*Vec1t(k); |
612 | mypoles(2, k1) = mypoints(myfirstp, k1) + lambda1*Vec1t(k1); |
613 | } |
614 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
615 | mypoles(nbpol1, k) = mypoints(mylastp, k) - lambda2*Vec2t(k); |
616 | mypoles(nbpol1, k1) = mypoints(mylastp, k1) - lambda2*Vec2t(k1); |
617 | } |
618 | k += 2; i2 += Nincx; |
619 | } |
620 | |
621 | } |
622 | |
623 | void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters, |
624 | const math_Vector& V1t, |
625 | const math_Vector& V2t, |
626 | const Standard_Real l1, |
627 | const Standard_Real l2) |
628 | { |
629 | done = Standard_False; |
630 | if (!isready) { |
631 | return; |
632 | } |
633 | Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower(); |
634 | resinit = 3; resfin = nbpoles-2; |
635 | Standard_Integer Nincx = resfin-resinit+1; |
636 | Ninc = NA*Nincx + 2; |
637 | FirstConstraint = AppParCurves_TangencyPoint; |
638 | LastConstraint = AppParCurves_TangencyPoint; |
639 | for (i = 1; i <= Vec1t.Upper(); i++) { |
640 | Vec1t(i) = V1t(i+lower1-1); |
641 | Vec2t(i) = V2t(i+lower2-1); |
642 | } |
643 | Perform (Parameters, l1, l2); |
644 | } |
645 | |
646 | |
647 | void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters, |
648 | const math_Vector& V1t, |
649 | const math_Vector& V2t, |
650 | const math_Vector& V1c, |
651 | const math_Vector& V2c, |
652 | const Standard_Real l1, |
653 | const Standard_Real l2) { |
654 | done = Standard_False; |
655 | if (!isready) { |
656 | return; |
657 | } |
658 | Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower(); |
659 | Standard_Integer lowc1 = V1c.Lower(), lowc2 = V2c.Lower(); |
660 | resinit = 4; resfin = nbpoles-3; |
661 | Standard_Integer Nincx = resfin-resinit+1; |
662 | Ninc = NA*Nincx + 2; |
663 | FirstConstraint = AppParCurves_CurvaturePoint; |
664 | LastConstraint = AppParCurves_CurvaturePoint; |
665 | |
666 | for (i = 1; i <= Vec1t.Upper(); i++) { |
667 | Vec1t(i) = V1t(i+lower1-1); |
668 | Vec2t(i) = V2t(i+lower2-1); |
669 | Vec1c(i) = V1c(i+lowc1-1); |
670 | Vec2c(i) = V2c(i+lowc2-1); |
671 | } |
672 | Perform (Parameters, l1, l2); |
673 | } |
674 | |
675 | |
676 | |
677 | void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters, |
678 | const Standard_Real l1, |
679 | const Standard_Real l2) { |
680 | done = Standard_False; |
681 | if (!isready) { |
682 | return; |
683 | } |
684 | if (FirstConstraint < AppParCurves_TangencyPoint && |
685 | LastConstraint < AppParCurves_TangencyPoint) { |
686 | Perform(Parameters); |
687 | return; |
688 | } |
689 | iscalculated = Standard_False; |
690 | |
691 | lambda1 = l1; |
692 | lambda2 = l2; |
693 | Standard_Integer i, j, k, i2; |
694 | Standard_Real AD0, AD1, AD2, A0, A1, A2; |
695 | // gp_Pnt Pt, P1, P2; |
696 | // gp_Pnt2d Pt2d, P12d, P22d; |
697 | Standard_Real l11 = deg*l1, l22 = deg*l2; |
698 | |
699 | ComputeFunction(Parameters); |
700 | |
701 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
702 | for (i = 1; i <= mypoles.ColNumber(); i++) |
703 | mypoles(2, i) = mypoints(myfirstp, i) + l1*Vec1t(i); |
704 | } |
705 | |
706 | |
707 | if (FirstConstraint == AppParCurves_CurvaturePoint) { |
708 | for (i = 1; i <= mypoles.ColNumber(); i++) |
709 | mypoles(3, i) = 2*mypoles(2, i)-mypoles(1, i) |
710 | + l11*l11*Vec1c(i)/(deg*(deg-1)); |
711 | } |
712 | |
713 | |
714 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
715 | for (i = 1; i <= mypoles.ColNumber(); i++) |
716 | mypoles(nbpoles-1, i) = mypoints(mylastp, i) - l2*Vec2t(i); |
717 | } |
718 | |
719 | |
720 | if (LastConstraint == AppParCurves_CurvaturePoint) { |
721 | for (i = 1; i <= mypoles.ColNumber(); i++) |
722 | mypoles(nbpoles-2, i) = 2*mypoles(nbpoles-1, i) - mypoles(nbpoles, i) |
723 | + l22*l22*Vec2c(i)/(deg*(deg-1)); |
724 | } |
725 | |
726 | if (resinit > resfin) { |
727 | done = Standard_True; |
728 | return; |
729 | } |
730 | |
731 | if (FirstConstraint == AppParCurves_NoConstraint) { |
732 | if (LastConstraint == AppParCurves_TangencyPoint) { |
733 | for (j = FirstP; j <= LastP; j++) { |
734 | AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); |
735 | for (i = 1; i <= B2.ColNumber(); i++) { |
736 | B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i) |
737 | - AD1 * mypoles(nbpoles-1, i); |
738 | } |
739 | } |
740 | } |
741 | if (LastConstraint == AppParCurves_CurvaturePoint) { |
742 | for (j = FirstP; j <= LastP; j++) { |
743 | AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2); |
744 | for (i = 1; i <= B2.ColNumber(); i++) { |
745 | B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i) |
746 | - AD1 * mypoles(nbpoles-1, i) |
747 | - AD2 * mypoles(nbpoles-2, i); |
748 | } |
749 | } |
750 | } |
751 | } |
752 | else if (FirstConstraint == AppParCurves_PassPoint) { |
753 | if (LastConstraint == AppParCurves_TangencyPoint) { |
754 | for (j = FirstP; j <= LastP; j++) { |
755 | A0 = A(j, 1); |
756 | AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); |
757 | for (i = 1; i <= B2.ColNumber(); i++) { |
758 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
759 | - AD0 * mypoles(nbpoles, i) |
760 | - AD1 * mypoles(nbpoles-1, i); |
761 | } |
762 | } |
763 | } |
764 | if (LastConstraint == AppParCurves_CurvaturePoint) { |
765 | for (j = FirstP; j <= LastP; j++) { |
766 | A0 = A(j, 1); |
767 | AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2); |
768 | for (i = 1; i <= B2.ColNumber(); i++) { |
769 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
770 | - AD0 * mypoles(nbpoles, i) |
771 | - AD1 * mypoles(nbpoles-1, i) |
772 | - AD2 * mypoles(nbpoles-2, i); |
773 | } |
774 | } |
775 | } |
776 | } |
777 | else if (FirstConstraint == AppParCurves_TangencyPoint) { |
778 | if (LastConstraint == AppParCurves_NoConstraint) { |
779 | for (j = FirstP; j <= LastP; j++) { |
780 | A0 = A(j, 1); A1 = A(j, 2); |
781 | for (i = 1; i <= B2.ColNumber(); i++) { |
782 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
783 | - A1 * mypoles(2, i); |
784 | } |
785 | } |
786 | } |
787 | else if (LastConstraint == AppParCurves_PassPoint) { |
788 | for (j = FirstP; j <= LastP; j++) { |
789 | A0 = A(j, 1); AD0 = A(j, nbpoles); A1 = A(j, 2); |
790 | for (i = 1; i <= B2.ColNumber(); i++) { |
791 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
792 | - AD0 * mypoles(nbpoles, i) |
793 | - A1 * mypoles(2, i); |
794 | } |
795 | } |
796 | } |
797 | else if (LastConstraint == AppParCurves_TangencyPoint) { |
798 | for (j = FirstP; j <= LastP; j++) { |
799 | A0 = A(j, 1); AD0 = A(j, nbpoles); A1 = A(j, 2); AD1 = A(j, nbpoles-1); |
800 | for (i = 1; i <= B2.ColNumber(); i++) { |
801 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
802 | - AD0 * mypoles(nbpoles, i) |
803 | - A1 * mypoles(2, i) |
804 | - AD1 * mypoles(nbpoles-1, i); |
805 | } |
806 | } |
807 | } |
808 | } |
809 | else if (FirstConstraint == AppParCurves_CurvaturePoint) { |
810 | if (LastConstraint == AppParCurves_NoConstraint) { |
811 | for (j = FirstP; j <= LastP; j++) { |
812 | A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3); |
813 | for (i = 1; i <= B2.ColNumber(); i++) { |
814 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
815 | - A1 * mypoles(2, i) |
816 | - A2 * mypoles(3, i); |
817 | } |
818 | } |
819 | } |
820 | else if (LastConstraint == AppParCurves_PassPoint) { |
821 | for (j = FirstP; j <= LastP; j++) { |
822 | A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3); AD0 = A(j, nbpoles); |
823 | for (i = 1; i <= B2.ColNumber(); i++) { |
824 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
825 | - A1 * mypoles(2, i) |
826 | - A2 * mypoles(3, i) |
827 | - AD0 * mypoles(nbpoles, i); |
828 | } |
829 | } |
830 | } |
831 | else if (LastConstraint == AppParCurves_TangencyPoint) { |
832 | for (j = FirstP; j <= LastP; j++) { |
833 | A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3); |
834 | AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); |
835 | for (i = 1; i <= B2.ColNumber(); i++) { |
836 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
837 | - A1 * mypoles(2, i) |
838 | - A2 * mypoles(3, i) |
839 | - AD0 * mypoles(nbpoles, i) |
840 | - AD1 * mypoles(nbpoles-1, i); |
841 | } |
842 | } |
843 | } |
844 | else if (LastConstraint == AppParCurves_CurvaturePoint) { |
845 | for (j = FirstP; j <= LastP; j++) { |
846 | A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3); |
847 | AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2); |
848 | for (i = 1; i <= B2.ColNumber(); i++) { |
849 | B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) |
850 | - A1 * mypoles(2, i) |
851 | - A2 * mypoles(3, i) |
852 | - AD0 * mypoles(nbpoles, i) |
853 | - AD1 * mypoles(nbpoles-1, i) |
854 | - AD2 * mypoles(nbpoles-2, i); |
855 | } |
856 | } |
857 | } |
858 | } |
859 | |
860 | Standard_Integer Nincx = resfin-resinit+1; |
861 | |
862 | math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0); |
863 | math_IntegerVector Index(1, Nincx); |
864 | SearchIndex(Index); |
865 | math_Vector AA(1, Index(Nincx), 0.0); |
866 | MakeTAA(AA, mytab); |
867 | |
868 | math_Vector myTABB(1, Nincx, 0.0); |
869 | |
96a95605 |
870 | DACTCL_Decompose(AA, Index); |
7fd59977 |
871 | |
872 | Standard_Integer kk2; |
873 | for (j = 1; j <= B2.ColNumber(); j++) { |
874 | kk2 = 1; |
875 | for (i = resinit; i <= resfin; i++) { |
876 | myTABB(kk2) = mytab(i, j); |
877 | kk2++; |
878 | } |
879 | |
96a95605 |
880 | DACTCL_Solve(AA, myTABB, Index); |
7fd59977 |
881 | |
882 | i2 = 1; |
883 | for (k = resinit; k <= resfin; k++) { |
884 | mypoles(k, j) = myTABB.Value(i2); |
885 | i2++; |
886 | } |
887 | |
888 | } |
889 | |
890 | done = Standard_True; |
891 | |
892 | } |
893 | |
894 | |
895 | |
896 | |
897 | void AppParCurves_LeastSquare::Affect(const MultiLine& SSP, |
898 | const Standard_Integer Index, |
899 | AppParCurves_Constraint& Cons, |
900 | math_Vector& Vt, |
901 | math_Vector& Vc) |
902 | { |
903 | // Vt: vecteur tangent, Vc: vecteur courbure. |
904 | |
905 | if (Cons >= AppParCurves_TangencyPoint) { |
906 | Standard_Integer i, i2 = 1; |
907 | Standard_Boolean Ok; |
908 | Standard_Integer mynbP2d = nbP2d, mynbP = nbP; |
909 | if (nbP2d == 0) mynbP2d = 1; |
910 | if (nbP == 0) mynbP = 1; |
911 | TColgp_Array1OfPnt TabP(1, mynbP); |
912 | TColgp_Array1OfPnt2d TabP2d(1, mynbP2d); |
913 | TColgp_Array1OfVec TabV(1, mynbP); |
914 | TColgp_Array1OfVec2d TabV2d(1, mynbP2d); |
915 | |
916 | if (Cons == AppParCurves_CurvaturePoint) { |
917 | if (nbP != 0 && nbP2d != 0) { |
918 | Ok = ToolLine::Curvature(SSP, Index,TabV,TabV2d); |
919 | if (!Ok) { Cons = AppParCurves_TangencyPoint;} |
920 | } |
921 | else if (nbP2d != 0) { |
922 | Ok = ToolLine::Curvature(SSP, Index, TabV2d); |
923 | if (!Ok) { Cons = AppParCurves_TangencyPoint;} |
924 | } |
925 | else { |
926 | Ok = ToolLine::Curvature(SSP, Index, TabV); |
927 | if (!Ok) { Cons = AppParCurves_TangencyPoint;} |
928 | } |
929 | if (Ok) { |
930 | for (i = 1; i <= nbP; i++) { |
931 | (TabV(i)).Coord(Vc(i2), Vc(i2+1), Vc(i2+2)); |
932 | i2 += 3; |
933 | } |
934 | for (i = 1; i <= nbP2d; i++) { |
935 | (TabV2d(i)).Coord(Vc(i2), Vc(i2+1)); |
936 | i2 += 2; |
937 | } |
938 | } |
939 | } |
940 | |
941 | i2 = 1; |
942 | if (Cons >= AppParCurves_TangencyPoint) { |
943 | if (nbP != 0 && nbP2d != 0) { |
944 | Ok = ToolLine::Tangency(SSP, Index, TabV, TabV2d); |
945 | if (!Ok) { Cons = AppParCurves_PassPoint;} |
946 | } |
947 | else if (nbP2d != 0) { |
948 | Ok = ToolLine::Tangency(SSP, Index, TabV2d); |
949 | if (!Ok) { Cons = AppParCurves_PassPoint;} |
950 | } |
951 | else { |
952 | Ok = ToolLine::Tangency(SSP, Index, TabV); |
953 | if (!Ok) { Cons = AppParCurves_PassPoint;} |
954 | } |
955 | if (Ok) { |
956 | for (i = 1; i <= nbP; i++) { |
957 | (TabV(i)).Coord(Vt(i2), Vt(i2+1), Vt(i2+2)); |
958 | i2 += 3; |
959 | } |
960 | for (i = 1; i <= nbP2d; i++) { |
961 | (TabV2d(i)).Coord(Vt(i2), Vt(i2+1)); |
962 | i2 += 2; |
963 | } |
964 | } |
965 | } |
966 | } |
967 | } |
968 | |
969 | |
970 | |
971 | Standard_Integer AppParCurves_LeastSquare::NbBColumns( |
972 | const MultiLine& SSP) const |
973 | { |
974 | Standard_Integer BCol; |
975 | BCol = (ToolLine::NbP3d(SSP))*3 + |
976 | (ToolLine::NbP2d(SSP))*2; |
977 | return BCol; |
978 | } |
979 | |
980 | |
981 | void AppParCurves_LeastSquare::Error(Standard_Real& F, |
982 | Standard_Real& MaxE3d, |
983 | Standard_Real& MaxE2d) |
984 | { |
985 | |
986 | if (!done) {StdFail_NotDone::Raise();} |
987 | Standard_Integer i, j, k, i2, indexdeb, indexfin; |
988 | Standard_Integer i21, i22; |
989 | Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ; |
990 | MaxE3d = MaxE2d = 0.0; |
991 | F = 0.0; |
992 | i2 = 1; |
993 | math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles); |
7fd59977 |
994 | |
995 | for (k = 1 ; k <= nbP+nbP2d; k++) { |
996 | i21 = i2+1; i22 = i2+2; |
997 | for (i = 1; i <= nbpoles; i++) { |
998 | Px(i) = mypoles(i, i2); |
999 | Py(i) = mypoles(i, i21); |
1000 | if (k <= nbP) Pz(i) = mypoles(i, i22); |
1001 | } |
1002 | for (i = FirstP; i <= LastP; i++) { |
1003 | AA = 0.0; BB = 0.0; CC = 0.0; |
1004 | indexdeb = myindex(i) + 1; |
1005 | indexfin = indexdeb + deg; |
7fd59977 |
1006 | for (j = indexdeb; j <= indexfin; j++) { |
1007 | AIJ = A(i, j); |
1008 | AA += AIJ*Px(j); |
1009 | BB += AIJ*Py(j); |
1010 | if (k <= nbP) CC += AIJ*Pz(j); |
1011 | } |
1012 | FX = AA-mypoints(i, i2); |
1013 | FY = BB-mypoints(i, i21); |
1014 | Fi= FX*FX + FY*FY; |
1015 | if (k <= nbP) { |
1016 | FZ = CC-mypoints(i, i22); |
1017 | Fi += FZ*FZ; |
1018 | if (Fi > MaxE3d) MaxE3d = Fi; |
1019 | } |
1020 | else { |
1021 | if (Fi > MaxE2d) MaxE2d = Fi; |
1022 | } |
1023 | theError(i, k) = Fi; |
1024 | F += Fi; |
1025 | } |
1026 | if (k <= nbP) i2 += 3; |
1027 | else i2 += 2; |
1028 | } |
1029 | MaxE3d = Sqrt(MaxE3d); |
1030 | MaxE2d = Sqrt(MaxE2d); |
1031 | } |
1032 | |
1033 | |
1034 | void AppParCurves_LeastSquare::ErrorGradient(math_Vector& Grad, |
1035 | Standard_Real& F, |
1036 | Standard_Real& MaxE3d, |
1037 | Standard_Real& MaxE2d) |
1038 | { |
1039 | if (!done) {StdFail_NotDone::Raise();} |
1040 | Standard_Integer i, j, k, i2, indexdeb, indexfin; |
1041 | Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ; |
1042 | // Standard_Real DAIJ, DAA, DBB, DCC, Gr, gr1= 0.0, gr2= 0.0; |
1043 | Standard_Real DAIJ, DAA, DBB, DCC, Gr; |
1044 | MaxE3d = MaxE2d = 0.0; |
1045 | // Standard_Integer i21, i22, diff = (deg-1); |
1046 | Standard_Integer i21, i22; |
1047 | F = 0.0; |
1048 | i2 = 1; |
1049 | math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles); |
1050 | |
1051 | for (k = Grad.Lower(); k <= Grad.Upper(); k++) Grad(k) = 0.0; |
1052 | |
1053 | for (k = 1 ; k <= nbP+nbP2d; k++) { |
1054 | i21 = i2+1; i22 = i2+2; |
1055 | for (i = 1; i <= nbpoles; i++) { |
1056 | Px(i) = mypoles(i, i2); |
1057 | Py(i) = mypoles(i, i21); |
1058 | if (k <= nbP) Pz(i) = mypoles(i, i22); |
1059 | } |
1060 | for (i = FirstP; i <= LastP; i++) { |
1061 | AA = 0.0; BB = 0.0; CC = 0.0; DAA = 0.0; DBB = 0.0; DCC = 0.0; |
1062 | indexdeb = myindex(i) + 1; |
1063 | indexfin = indexdeb + deg; |
1064 | for (j = indexdeb; j <= indexfin; j++) { |
1065 | AIJ = A(i, j); DAIJ = DA(i, j); |
1066 | AA += AIJ*Px(j); DAA += DAIJ*Px(j); |
1067 | BB += AIJ*Py(j); DBB += DAIJ*Py(j); |
1068 | if (k <= nbP) { |
1069 | CC += AIJ*Pz(j); |
1070 | DCC += DAIJ*Pz(j); |
1071 | } |
1072 | } |
1073 | FX = AA-mypoints(i, i2); |
1074 | FY = BB-mypoints(i, i2+1); |
1075 | Fi = FX*FX + FY*FY; |
1076 | Gr = 2.0*(DAA*FX + DBB*FY); |
1077 | |
1078 | if (k <= nbP) { |
1079 | FZ = CC-mypoints(i, i2+2); |
1080 | Fi += FZ*FZ; |
1081 | Gr += 2.0*DCC*FZ; |
1082 | if (Fi > MaxE3d) MaxE3d = Fi; |
1083 | } |
1084 | else { |
1085 | if (Fi > MaxE2d) MaxE2d = Fi; |
1086 | } |
1087 | theError(i, k) = Fi; |
1088 | Grad(i) += Gr; |
1089 | F += Fi; |
1090 | } |
1091 | if (k <= nbP) i2 += 3; |
1092 | else i2 += 2; |
1093 | } |
1094 | MaxE3d = Sqrt(MaxE3d); |
1095 | MaxE2d = Sqrt(MaxE2d); |
1096 | |
1097 | } |
1098 | |
1099 | |
1100 | const math_Matrix& AppParCurves_LeastSquare::Distance() |
1101 | { |
1102 | if (!iscalculated) { |
1103 | for (Standard_Integer i = myfirstp; i <= mylastp; i++) { |
1104 | for (Standard_Integer j = 1; j <= nbP+nbP2d; j++) { |
1105 | theError(i, j) = Sqrt(theError(i, j)); |
1106 | } |
1107 | } |
1108 | iscalculated = Standard_True; |
1109 | } |
1110 | return theError; |
1111 | } |
1112 | |
1113 | |
1114 | Standard_Real AppParCurves_LeastSquare::FirstLambda() const |
1115 | { |
1116 | return lambda1; |
1117 | } |
1118 | |
1119 | Standard_Real AppParCurves_LeastSquare::LastLambda() const |
1120 | { |
1121 | return lambda2; |
1122 | } |
1123 | |
1124 | |
1125 | |
1126 | Standard_Boolean AppParCurves_LeastSquare::IsDone() const |
1127 | { |
1128 | return done; |
1129 | } |
1130 | |
1131 | |
1132 | AppParCurves_MultiCurve AppParCurves_LeastSquare::BezierValue() |
1133 | { |
1134 | if (!myknots.IsNull()) Standard_NoSuchObject::Raise(); |
1135 | return (AppParCurves_MultiCurve)(BSplineValue()); |
1136 | } |
1137 | |
1138 | |
1139 | const AppParCurves_MultiBSpCurve& AppParCurves_LeastSquare::BSplineValue() |
1140 | { |
1141 | if (!done) {StdFail_NotDone::Raise();} |
1142 | |
1143 | Standard_Integer i, j, j2, npoints = nbP+nbP2d;; |
1144 | gp_Pnt Pt; |
1145 | gp_Pnt2d Pt2d; |
1146 | Standard_Integer ideb = resinit, ifin = resfin; |
1147 | if (ideb >= 2) ideb = 2; |
1148 | if (ifin <= nbpoles-1) ifin = nbpoles-1; |
1149 | |
1150 | // On met le resultat dans les curves correspondantes |
1151 | for (i = ideb; i <= ifin; i++) { |
1152 | j2 = 1; |
1153 | AppParCurves_MultiPoint MPole(nbP, nbP2d); |
1154 | for (j = 1; j <= nbP; j++) { |
1155 | Pt.SetCoord(mypoles(i, j2), mypoles(i, j2+1), mypoles(i,j2+2)); |
1156 | MPole.SetPoint(j, Pt); |
1157 | j2 += 3; |
1158 | } |
1159 | for (j = nbP+1;j <= npoints; j++) { |
1160 | Pt2d.SetCoord(mypoles(i, j2), mypoles(i, j2+1)); |
1161 | MPole.SetPoint2d(j, Pt2d); |
1162 | j2 += 2; |
1163 | } |
1164 | SCU.SetValue(i, MPole); |
1165 | } |
1166 | return SCU; |
1167 | } |
1168 | |
1169 | |
1170 | |
1171 | const math_Matrix& AppParCurves_LeastSquare::FunctionMatrix() const |
1172 | { |
1173 | if (!done) {StdFail_NotDone::Raise();} |
1174 | return A; |
1175 | } |
1176 | |
1177 | |
1178 | const math_Matrix& AppParCurves_LeastSquare::DerivativeFunctionMatrix() const |
1179 | { |
1180 | if (!done) {StdFail_NotDone::Raise();} |
1181 | return DA; |
1182 | } |
1183 | |
1184 | |
1185 | |
1186 | |
1187 | Standard_Integer AppParCurves_LeastSquare:: |
1188 | TheFirstPoint(const AppParCurves_Constraint FirstCons, |
1189 | const Standard_Integer FirstPoint) const |
1190 | { |
1191 | if (FirstCons == AppParCurves_NoConstraint) |
1192 | return FirstPoint; |
1193 | else |
1194 | return FirstPoint + 1; |
1195 | } |
1196 | |
1197 | |
1198 | |
1199 | Standard_Integer AppParCurves_LeastSquare:: |
1200 | TheLastPoint(const AppParCurves_Constraint LastCons, |
1201 | const Standard_Integer LastPoint) const |
1202 | { |
1203 | if (LastCons == AppParCurves_NoConstraint) |
1204 | return LastPoint; |
1205 | else |
1206 | return LastPoint - 1; |
1207 | } |
1208 | |
1209 | |
1210 | void AppParCurves_LeastSquare::ComputeFunction(const math_Vector& Parameters) |
1211 | { |
1212 | if (myknots.IsNull()) { |
1213 | AppParCurves::Bernstein(nbpoles, Parameters, A, DA); |
1214 | } |
1215 | else { |
1216 | AppParCurves::SplineFunction(nbpoles, deg, Parameters, |
1217 | Vflatknots, A, DA, myindex); |
1218 | } |
1219 | } |
1220 | |
1221 | |
1222 | |
1223 | const math_Matrix& AppParCurves_LeastSquare::Points() const |
1224 | { |
1225 | return mypoints; |
1226 | } |
1227 | |
1228 | const math_Matrix& AppParCurves_LeastSquare::Poles() const |
1229 | { |
1230 | return mypoles; |
1231 | } |
1232 | |
1233 | |
1234 | |
1235 | const math_IntegerVector& AppParCurves_LeastSquare::KIndex() const |
1236 | { |
1237 | return myindex; |
1238 | } |
1239 | |
1240 | |
1241 | |
1242 | |
1243 | void AppParCurves_LeastSquare::MakeTAA(math_Vector& TheA, |
1244 | math_Vector& myTAB) |
1245 | { |
1246 | Standard_Integer i, j, k, Ci, i2, i21, i22; |
1247 | Standard_Real xx = 0.0, yy = 0.0; |
1248 | |
1249 | Standard_Integer Nincx = resfin-resinit+1; |
1250 | Standard_Integer Nrow, Neq = LastP-FirstP+1; |
1251 | |
1252 | Standard_Integer Ninc1; |
1253 | |
1254 | if (FirstConstraint >= AppParCurves_TangencyPoint && |
1255 | LastConstraint >= AppParCurves_TangencyPoint) { |
1256 | Ninc1 = Ninc-1; |
1257 | } |
1258 | else Ninc1 = Ninc; |
1259 | |
1260 | Standard_Integer myfirst = A.LowerRow(); |
1261 | Standard_Integer ix, iy, iz; |
1262 | Standard_Integer mylast = myfirst+Nlignes-1; |
1263 | Standard_Integer k1; |
1264 | Standard_Real taf1 = 0.0, taf2 = 0.0, taf3 = 0.0, |
1265 | tab1 = 0.0, tab2 = 0.0; |
1266 | Standard_Integer nb, inc, jinit, jfin, u; |
1267 | Standard_Integer indexdeb, indexfin; |
1268 | Standard_Integer NA1 = NA-1; |
1269 | Standard_Real v1=0, v2=0, b; |
1270 | Standard_Real Ai2, Aid, Akj; |
1271 | math_Vector myB(myfirst, mylast, 0.0), |
1272 | myV1(myfirst, mylast, 0.0), |
1273 | myV2(myfirst, mylast, 0.0); |
1274 | math_Vector TheV1(1, Ninc, 0.0), TheV2(1, Ninc, 0.0); |
1275 | |
1276 | |
1277 | for (i = FirstP; i <= LastP; i++) { |
1278 | Ai2 = A(i, 2); |
1279 | Aid = A(i, nbpoles-1); |
1280 | if (FirstConstraint >= AppParCurves_PassPoint) xx = A(i, 1); |
1281 | if (FirstConstraint >= AppParCurves_TangencyPoint) xx += Ai2; |
1282 | if (LastConstraint >= AppParCurves_PassPoint) yy = A(i, nbpoles); |
1283 | if (LastConstraint >= AppParCurves_TangencyPoint) yy += Aid; |
1284 | i2 = 1; |
1285 | Nrow = myfirst-FirstP; |
1286 | for (Ci = 1; Ci <= nbP; Ci++) { |
1287 | i21 = i2+1; i22 = i2+2; |
1288 | ix = i+Nrow; iy = ix+Neq; iz = iy+Neq; |
1289 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
1290 | myV1(ix) = Ai2*Vec1t(i2); |
1291 | myV1(iy) = Ai2*Vec1t(i21); |
1292 | myV1(iz) = Ai2*Vec1t(i22); |
1293 | } |
1294 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
1295 | myV2(ix) = -Aid*Vec2t(i2); |
1296 | myV2(iy) = -Aid*Vec2t(i21); |
1297 | myV2(iz) = -Aid*Vec2t(i22); |
1298 | } |
1299 | myB(ix) = mypoints(i, i2) - xx*mypoints(myfirstp, i2) |
1300 | - yy*mypoints(mylastp, i2); |
1301 | myB(iy) = mypoints(i, i21) - xx*mypoints(myfirstp, i21) |
1302 | - yy*mypoints(mylastp, i21); |
1303 | myB(iz) = mypoints(i, i22) - xx*mypoints(myfirstp, i22) |
1304 | - yy*mypoints(mylastp, i22); |
1305 | i2 += 3; |
1306 | Nrow = Nrow + 3*Neq; |
1307 | } |
1308 | |
1309 | for (Ci = 1; Ci <= nbP2d; Ci++) { |
1310 | i21 = i2+1; i22 = i2+2; |
1311 | ix = i+Nrow; iy = ix+Neq; |
1312 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
1313 | myV1(ix) = Ai2*Vec1t(i2); |
1314 | myV1(iy) = Ai2*Vec1t(i21); |
1315 | } |
1316 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
1317 | myV2(ix) = -Aid*Vec2t(i2); |
1318 | myV2(iy) = -Aid*Vec2t(i21); |
1319 | } |
1320 | myB(ix) = mypoints(i, i2) - xx*mypoints(myfirstp, i2) |
1321 | - yy*mypoints(mylastp, i2); |
1322 | myB(iy) = mypoints(i, i21) - xx*mypoints(myfirstp, i21) |
1323 | - yy*mypoints(mylastp, i21); |
1324 | Nrow = Nrow + 2*Neq; |
1325 | i2 += 2; |
1326 | } |
1327 | } |
1328 | |
1329 | |
1330 | |
1331 | // Construction de TA*A et TA*B: |
1332 | |
1333 | for (k = FirstP; k <= LastP; k++) { |
1334 | indexdeb = myindex(k)+1; |
1335 | indexfin = indexdeb + deg; |
1336 | jinit = Max(resinit, indexdeb); |
1337 | jfin = Min(resfin, indexfin); |
1338 | k1 = k + myfirst - FirstP; |
1339 | for (i = 0; i <= NA1; i++) { |
1340 | nb = i*Neq + k1; |
1341 | if (FirstConstraint >= AppParCurves_TangencyPoint) |
1342 | v1 = myV1(nb); |
1343 | if (LastConstraint >= AppParCurves_TangencyPoint) |
1344 | v2 = myV2(nb); |
1345 | b = myB(nb); |
1346 | inc = i*Nincx-resinit+1; |
1347 | for (j = jinit; j <= jfin; j++) { |
1348 | Akj = A(k, j); |
1349 | u = j+inc; |
1350 | if (FirstConstraint >= AppParCurves_TangencyPoint) |
1351 | TheV1(u) += Akj*v1; |
1352 | if (LastConstraint >= AppParCurves_TangencyPoint) |
1353 | TheV2(u) += Akj*v2; |
1354 | myTAB(u) += Akj*b; |
1355 | } |
1356 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
1357 | taf1 += v1*v1; |
1358 | tab1 += v1*b; |
1359 | } |
1360 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
1361 | taf2 += v2*v2; |
1362 | tab2 += v2*b; |
1363 | } |
1364 | if (FirstConstraint >= AppParCurves_TangencyPoint && |
1365 | LastConstraint >= AppParCurves_TangencyPoint) { |
1366 | taf3 += v1*v2; |
1367 | } |
1368 | } |
1369 | } |
1370 | |
1371 | |
1372 | if (FirstConstraint >= AppParCurves_TangencyPoint) { |
1373 | TheV1(Ninc1) = taf1; |
1374 | myTAB(Ninc1) = tab1; |
1375 | } |
1376 | if (LastConstraint >= AppParCurves_TangencyPoint) { |
1377 | TheV2(Ninc) = taf2; |
1378 | myTAB(Ninc) = tab2; |
1379 | } |
1380 | if (FirstConstraint >= AppParCurves_TangencyPoint && |
1381 | LastConstraint >= AppParCurves_TangencyPoint) { |
1382 | TheV2(Ninc1) = taf3; |
1383 | } |
1384 | |
1385 | |
1386 | if (resinit <= resfin) { |
1387 | math_IntegerVector Index(1, Nincx); |
1388 | SearchIndex(Index); |
1389 | math_Vector AA(1, Index(Nincx)); |
1390 | MakeTAA(AA); |
1391 | |
1392 | Standard_Integer kk = 1; |
1393 | for (k = 1; k <= NA; k++) { |
1394 | for(i = 1; i <= AA.Length(); i++) { |
1395 | TheA(kk) = AA(i); |
1396 | kk++; |
1397 | } |
1398 | } |
1399 | } |
1400 | |
1401 | Standard_Integer length = TheA.Length(); |
1402 | |
1403 | if (FirstConstraint >= AppParCurves_TangencyPoint && |
1404 | LastConstraint >= AppParCurves_TangencyPoint) { |
1405 | for (j = 1; j <= Ninc1; j++) |
1406 | TheA(length- 2*Ninc+j+1) = TheV1(j); |
1407 | for (j = 1; j <= Ninc; j++) |
1408 | TheA(length- Ninc+j) = TheV2(j); |
1409 | } |
1410 | |
1411 | |
1412 | else if (FirstConstraint >= AppParCurves_TangencyPoint) { |
1413 | for (j = 1; j <= Ninc; j++) |
1414 | TheA(length- Ninc+j) = TheV1(j); |
1415 | } |
1416 | |
1417 | else if (LastConstraint >= AppParCurves_TangencyPoint) { |
1418 | for (j = 1; j <= Ninc; j++) |
1419 | TheA(length- Ninc+j) = TheV2(j); |
1420 | } |
1421 | } |
1422 | |
1423 | |
1424 | |
1425 | |
1426 | void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA, |
1427 | math_Matrix& myTAB) |
1428 | { |
1429 | Standard_Integer i, j, k; |
1430 | Standard_Integer indexdeb, indexfin, jinit, jfin; |
1431 | Standard_Integer iinit, ifin; |
1432 | Standard_Real Akj; |
1433 | math_Matrix TheA(resinit, resfin, resinit, resfin); |
1434 | TheA.Init(0.0); |
1435 | |
1436 | for (k = FirstP ; k <= LastP; k++) { |
1437 | indexdeb = myindex(k)+1; |
1438 | indexfin = indexdeb + deg; |
1439 | jinit = Max(resinit, indexdeb); |
1440 | jfin = Min(resfin, indexfin); |
1441 | for (i = jinit; i <= jfin; i++) { |
1442 | Akj = A(k, i); |
1443 | for (j = jinit; j <= i; j++) { |
1444 | TheA(i, j) += A(k, j) * Akj; |
1445 | } |
1446 | for (j = 1; j <= B2.ColNumber(); j++) { |
1447 | myTAB(i, j) += Akj*B2(k, j); |
1448 | } |
1449 | } |
1450 | } |
1451 | |
1452 | Standard_Integer len; |
1453 | if (!myknots.IsNull()) len = myknots->Length(); |
1454 | else len = 2; |
1455 | Standard_Integer d, i2 = 1; |
1456 | iinit = resinit; |
1457 | jinit = resinit; |
1458 | ifin = Min(resfin, deg+1); |
1459 | for (k = 2; k <= len; k++) { |
1460 | for (i = iinit; i <= ifin; i++) { |
1461 | for (j = jinit; j <= i; j++) { |
1462 | AA(i2) = TheA(i, j); |
1463 | i2++; |
1464 | } |
1465 | } |
1466 | if (!mymults.IsNull()) { |
1467 | iinit = ifin+1; |
1468 | d = ifin + mymults->Value(k); |
1469 | ifin = Min(d, resfin); |
1470 | jinit = Max(resinit, d - deg); |
1471 | } |
1472 | } |
1473 | } |
1474 | |
1475 | |
1476 | void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA) |
1477 | { |
1478 | Standard_Integer i, j, k; |
1479 | Standard_Integer indexdeb, indexfin, jinit, jfin; |
1480 | Standard_Integer iinit, ifin; |
1481 | Standard_Real Akj; |
1482 | math_Matrix TheA(resinit, resfin, resinit, resfin, 0.0); |
1483 | |
1484 | |
1485 | for (k = FirstP ; k <= LastP; k++) { |
1486 | indexdeb = myindex(k)+1; |
1487 | indexfin = indexdeb + deg; |
1488 | jinit = Max(resinit, indexdeb); |
1489 | jfin = Min(resfin, indexfin); |
1490 | for (i = jinit; i <= jfin; i++) { |
1491 | Akj = A(k, i); |
1492 | for (j = jinit; j <= i; j++) { |
1493 | TheA(i, j) += A(k, j) * Akj; |
1494 | } |
1495 | } |
1496 | } |
1497 | |
1498 | |
1499 | Standard_Integer i2 = 1; |
1500 | iinit = resinit; |
1501 | jinit = resinit; |
1502 | ifin = Min(resfin, deg+1); |
1503 | Standard_Integer len, d; |
1504 | if (!myknots.IsNull()) len = myknots->Length(); |
1505 | else len = 2; |
1506 | for (k = 2; k <= len; k++) { |
1507 | for (i = iinit; i <= ifin; i++) { |
1508 | for (j = jinit; j <= i; j++) { |
1509 | AA(i2) = TheA(i, j); |
1510 | i2++; |
1511 | } |
1512 | } |
1513 | if (!mymults.IsNull()) { |
1514 | iinit = ifin+1; |
1515 | d = ifin + mymults->Value(k); |
1516 | ifin = Min(d, resfin); |
1517 | jinit = Max(resinit, d - deg); |
1518 | } |
1519 | } |
1520 | |
1521 | } |
1522 | |
1523 | |
1524 | |
1525 | void AppParCurves_LeastSquare::SearchIndex(math_IntegerVector& Index) |
1526 | { |
1527 | Standard_Integer iinit, jinit, ifin; |
1528 | Standard_Integer i, j, k, d, i2 = 1; |
1529 | Standard_Integer len; |
1530 | Standard_Integer Nincx = resfin-resinit+1; |
1531 | Index(1) = 1; |
1532 | |
1533 | if (myknots.IsNull()) { |
1534 | if (resinit <= resfin) { |
1535 | Standard_Integer l = 1; |
1536 | for (i = 2; i <= Nincx; i++) { |
1537 | l++; |
1538 | Index(l) = Index(l-1)+i; |
1539 | } |
1540 | } |
1541 | |
1542 | } |
1543 | else { |
1544 | iinit = resinit; |
1545 | jinit = resinit; |
1546 | ifin = Min(resfin, deg+1); |
1547 | len = myknots->Length(); |
1548 | |
1549 | for (k = 2; k <= len; k++) { |
1550 | for (i = iinit; i <= ifin; i++) { |
1551 | for (j = jinit; j <= i; j++) { |
1552 | if (i2 != 1) |
1553 | Index(i2) = Index(i2-1) + i-jinit+1; |
1554 | } |
1555 | i2++; |
1556 | } |
1557 | iinit = ifin+1; |
1558 | d = ifin + mymults->Value(k); |
1559 | ifin = Min(d, resfin); |
1560 | jinit = Max(resinit, d - deg); |
1561 | } |
1562 | } |
1563 | } |