0024510: Remove unused local variables
[occt.git] / src / AppParCurves / AppParCurves_LeastSquare.gxx
CommitLineData
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//
973c2be1 6// This library is free software; you can redistribute it and / or modify it
7// under the terms of the GNU Lesser General Public version 2.1 as published
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
50static 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
60AppParCurves_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
96AppParCurves_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
129AppParCurves_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
173AppParCurves_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
215void 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
427void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters) {
428
429 done = Standard_False;
430 if (!isready) {
431 return;
432 }
96a95605 433 Standard_Integer i, j, k, Ci, Nincx, 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
526 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
623void 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
647void 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
677void 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
897void 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
971Standard_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
981void 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
1034void 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
1100const 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
1114Standard_Real AppParCurves_LeastSquare::FirstLambda() const
1115{
1116 return lambda1;
1117}
1118
1119Standard_Real AppParCurves_LeastSquare::LastLambda() const
1120{
1121 return lambda2;
1122}
1123
1124
1125
1126Standard_Boolean AppParCurves_LeastSquare::IsDone() const
1127{
1128 return done;
1129}
1130
1131
1132AppParCurves_MultiCurve AppParCurves_LeastSquare::BezierValue()
1133{
1134 if (!myknots.IsNull()) Standard_NoSuchObject::Raise();
1135 return (AppParCurves_MultiCurve)(BSplineValue());
1136}
1137
1138
1139const 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
1171const math_Matrix& AppParCurves_LeastSquare::FunctionMatrix() const
1172{
1173 if (!done) {StdFail_NotDone::Raise();}
1174 return A;
1175}
1176
1177
1178const math_Matrix& AppParCurves_LeastSquare::DerivativeFunctionMatrix() const
1179{
1180 if (!done) {StdFail_NotDone::Raise();}
1181 return DA;
1182}
1183
1184
1185
1186
1187Standard_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
1199Standard_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
1210void 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
1223const math_Matrix& AppParCurves_LeastSquare::Points() const
1224{
1225 return mypoints;
1226}
1227
1228const math_Matrix& AppParCurves_LeastSquare::Poles() const
1229{
1230 return mypoles;
1231}
1232
1233
1234
1235const math_IntegerVector& AppParCurves_LeastSquare::KIndex() const
1236{
1237 return myindex;
1238}
1239
1240
1241
1242
1243void 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
1426void 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
1476void 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
1525void 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}