Warning, /include/opencascade/AppParCurves_LeastSquare.gxx is written in an unsupported language. File is not indexed.
0001 // Copyright (c) 1995-1999 Matra Datavision
0002 // Copyright (c) 1999-2014 OPEN CASCADE SAS
0003 //
0004 // This file is part of Open CASCADE Technology software library.
0005 //
0006 // This library is free software; you can redistribute it and/or modify it under
0007 // the terms of the GNU Lesser General Public License version 2.1 as published
0008 // by the Free Software Foundation, with special exception defined in the file
0009 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
0010 // distribution for complete text of the license and disclaimer of any warranty.
0011 //
0012 // Alternatively, this file may be used under the terms of Open CASCADE
0013 // commercial license or contractual agreement.
0014
0015 // lpa, le 27/07/91
0016 // pmn, 30/10/98 : Protection dans les cas surcontraint -> "isready"
0017
0018 // Approximation d une MultiLine de points decrite par le tool MLineTool.
0019 // Ce programme utilise les moindres carres pour le cas suivant:
0020 // passage et tangences aux extremites.
0021
0022
0023 #define No_Standard_RangeError
0024 #define No_Standard_OutOfRange
0025 #define No_Standard_DimensionError
0026
0027 #include <math_Householder.hxx>
0028 #include <math_Crout.hxx>
0029 #include <AppParCurves.hxx>
0030 #include <gp_Pnt.hxx>
0031 #include <gp_Pnt2d.hxx>
0032 #include <gp_Vec.hxx>
0033 #include <gp_Vec2d.hxx>
0034 #include <TColgp_Array1OfPnt.hxx>
0035 #include <TColgp_Array1OfPnt2d.hxx>
0036 #include <TColgp_Array1OfVec.hxx>
0037 #include <TColgp_Array1OfVec2d.hxx>
0038 #include <TColStd_Array1OfInteger.hxx>
0039 #include <TColStd_Array1OfReal.hxx>
0040 #include <AppParCurves_Constraint.hxx>
0041 #include <AppParCurves_MultiPoint.hxx>
0042 #include <StdFail_NotDone.hxx>
0043 #include <Standard_NoSuchObject.hxx>
0044 #include <TColStd_Array1OfReal.hxx>
0045 #include <math_Recipes.hxx>
0046 #include <math_Crout.hxx>
0047
0048
0049
0050 static int FlatLength(const TColStd_Array1OfInteger& Mults) {
0051
0052 Standard_Integer sum = 0;
0053 for (Standard_Integer i = Mults.Lower(); i <= Mults.Upper(); i++) {
0054 sum += Mults.Value(i);
0055 }
0056 return sum;
0057 }
0058
0059 //=======================================================================
0060 //function : CheckTangents
0061 //purpose : Checks if theArrTg3d and theArrTg2d have direction
0062 // corresponded to the direction between theArrPt1 and theArrPt2.
0063 // If it is not then reverses tangent vectors.
0064 // theArrPt1 (as same as theArrPt2) is sub-set of all 3D-points in
0065 // one multy-point (multy-point is union of sets of 2D- and 3D-points).
0066 //
0067 //ATTENTION!!!
0068 // The property of correlation between Tg3d and Tg2d is used here.
0069 // Therefore, only 3D-coinciding is checked.
0070 //=======================================================================
0071 static void CheckTangents(const TColgp_Array1OfPnt& theArrPt1,
0072 const TColgp_Array1OfPnt& theArrPt2,
0073 TColgp_Array1OfVec& theArrTg3d,
0074 TColgp_Array1OfVec2d& theArrTg2d)
0075 {
0076 if(theArrPt1.Lower() != theArrPt2.Lower())
0077 return;
0078
0079 if(theArrPt1.Upper() != theArrPt2.Upper())
0080 return;
0081
0082 if(theArrTg3d.Length() != theArrPt1.Length())
0083 return;
0084
0085 Standard_Boolean isToChangeDir = Standard_False;
0086
0087 for(Standard_Integer i = theArrPt1.Lower(); i <= theArrPt1.Upper(); i++)
0088 {
0089 const gp_Vec aV1(theArrPt1(i), theArrPt2(i));
0090 const gp_Vec& aV2 = theArrTg3d(i);
0091
0092 if(aV1.Dot(aV2) < 0.0)
0093 {
0094 isToChangeDir = Standard_True;
0095 break;
0096 }
0097 }
0098
0099 if(!isToChangeDir)
0100 return;
0101
0102 //Change directions for every 2D- and 3D-tangents
0103
0104 for(Standard_Integer i = theArrTg3d.Lower(); i <= theArrTg3d.Upper(); i++)
0105 {
0106 theArrTg3d(i).Reverse();
0107 }
0108
0109 for(Standard_Integer i = theArrTg2d.Lower(); i <= theArrTg2d.Upper(); i++)
0110 {
0111 theArrTg2d(i).Reverse();
0112 }
0113 }
0114
0115 //=======================================================================
0116 //function : CheckTangents
0117 //purpose : Checks if theArrTg2d have direction
0118 // corresponded to the direction between theArrPt1 and theArrPt2.
0119 // If it is not then reverses tangent vector.
0120 // theArrPt1 (as same as theArrPt2) is sub-set of all 2D-points in
0121 // one multy-point (multy-point is union of sets of 2D- and 3D-points).
0122 //=======================================================================
0123 static void CheckTangents(const TColgp_Array1OfPnt2d& theArrPt1,
0124 const TColgp_Array1OfPnt2d& theArrPt2,
0125 TColgp_Array1OfVec2d& theArrTg2d)
0126 {
0127 if(theArrPt1.Lower() != theArrPt2.Lower())
0128 return;
0129
0130 if(theArrPt1.Upper() != theArrPt2.Upper())
0131 return;
0132
0133 for(Standard_Integer i = theArrPt1.Lower(); i <= theArrPt1.Upper(); i++)
0134 {
0135 const gp_Vec2d aV1(theArrPt1(i), theArrPt2(i));
0136 const gp_Vec2d& aV2 = theArrTg2d(i);
0137
0138 if(aV1.Dot(aV2) < 0.0)
0139 {
0140 theArrTg2d(i).Reverse();
0141 }
0142 }
0143 }
0144
0145
0146 AppParCurves_LeastSquare::
0147 AppParCurves_LeastSquare(const MultiLine& SSP,
0148 const Standard_Integer FirstPoint,
0149 const Standard_Integer LastPoint,
0150 const AppParCurves_Constraint FirstCons,
0151 const AppParCurves_Constraint LastCons,
0152 const math_Vector& Parameters,
0153 const Standard_Integer NbPol):
0154 SCU(NbPol),
0155 mypoles(1, NbPol,
0156 1, NbBColumns(SSP)),
0157 A(FirstPoint, LastPoint, 1, NbPol),
0158 DA(FirstPoint, LastPoint, 1, NbPol),
0159 B2(TheFirstPoint(FirstCons, FirstPoint),
0160 Max(TheFirstPoint(FirstCons, FirstPoint),
0161 TheLastPoint(LastCons, LastPoint)),
0162 1, NbBColumns(SSP)),
0163 mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
0164 Vflatknots(1, 1),
0165 Vec1t(1, NbBColumns(SSP)),
0166 Vec1c(1, NbBColumns(SSP)),
0167 Vec2t(1, NbBColumns(SSP)),
0168 Vec2c(1, NbBColumns(SSP)),
0169 theError(FirstPoint, LastPoint,
0170 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
0171 myindex(FirstPoint, LastPoint, 0),
0172 nbpoles(NbPol)
0173 {
0174 FirstConstraint = FirstCons;
0175 LastConstraint = LastCons;
0176 Init(SSP, FirstPoint, LastPoint);
0177 Perform(Parameters);
0178 }
0179
0180
0181
0182 AppParCurves_LeastSquare::
0183 AppParCurves_LeastSquare(const MultiLine& SSP,
0184 const Standard_Integer FirstPoint,
0185 const Standard_Integer LastPoint,
0186 const AppParCurves_Constraint FirstCons,
0187 const AppParCurves_Constraint LastCons,
0188 const Standard_Integer NbPol):
0189 SCU(NbPol),
0190 mypoles(1, NbPol,
0191 1, NbBColumns(SSP)),
0192 A(FirstPoint, LastPoint, 1, NbPol),
0193 DA(FirstPoint, LastPoint, 1, NbPol),
0194 B2(TheFirstPoint(FirstCons, FirstPoint),
0195 Max(TheFirstPoint(FirstCons, FirstPoint),
0196 TheLastPoint(LastCons, LastPoint)),
0197 1, NbBColumns(SSP)),
0198 mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
0199 Vflatknots(1, 1),
0200 Vec1t(1, NbBColumns(SSP)),
0201 Vec1c(1, NbBColumns(SSP)),
0202 Vec2t(1, NbBColumns(SSP)),
0203 Vec2c(1, NbBColumns(SSP)),
0204 theError(FirstPoint, LastPoint,
0205 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
0206 myindex(FirstPoint, LastPoint, 0),
0207 nbpoles(NbPol)
0208 {
0209 FirstConstraint = FirstCons;
0210 LastConstraint = LastCons;
0211 Init(SSP, FirstPoint, LastPoint);
0212 }
0213
0214
0215 AppParCurves_LeastSquare::
0216 AppParCurves_LeastSquare(const MultiLine& SSP,
0217 const TColStd_Array1OfReal& Knots,
0218 const TColStd_Array1OfInteger& Mults,
0219 const Standard_Integer FirstPoint,
0220 const Standard_Integer LastPoint,
0221 const AppParCurves_Constraint FirstCons,
0222 const AppParCurves_Constraint LastCons,
0223 const math_Vector& Parameters,
0224 const Standard_Integer NbPol):
0225 SCU(NbPol),
0226 mypoles(1, NbPol,
0227 1, NbBColumns(SSP)),
0228 A(FirstPoint, LastPoint, 1, NbPol),
0229 DA(FirstPoint, LastPoint, 1, NbPol),
0230 B2(TheFirstPoint(FirstCons, FirstPoint),
0231 Max(TheFirstPoint(FirstCons, FirstPoint),
0232 TheLastPoint(LastCons, LastPoint)),
0233 1, NbBColumns(SSP)),
0234 mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
0235 Vflatknots(1, FlatLength(Mults)),
0236 Vec1t(1, NbBColumns(SSP)),
0237 Vec1c(1, NbBColumns(SSP)),
0238 Vec2t(1, NbBColumns(SSP)),
0239 Vec2c(1, NbBColumns(SSP)),
0240 theError(FirstPoint, LastPoint,
0241 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
0242 myindex(FirstPoint, LastPoint, 0),
0243 nbpoles(NbPol)
0244 {
0245 FirstConstraint = FirstCons;
0246 LastConstraint = LastCons;
0247 myknots = new TColStd_HArray1OfReal(Knots.Lower(), Knots.Upper());
0248 myknots->ChangeArray1() = Knots;
0249 mymults = new TColStd_HArray1OfInteger(Mults.Lower(), Mults.Upper());
0250 mymults->ChangeArray1() = Mults;
0251 SCU.SetKnots(Knots);
0252 SCU.SetMultiplicities(Mults);
0253 Init(SSP, FirstPoint, LastPoint);
0254 Perform(Parameters);
0255 }
0256
0257
0258
0259 AppParCurves_LeastSquare::
0260 AppParCurves_LeastSquare(const MultiLine& SSP,
0261 const TColStd_Array1OfReal& Knots,
0262 const TColStd_Array1OfInteger& Mults,
0263 const Standard_Integer FirstPoint,
0264 const Standard_Integer LastPoint,
0265 const AppParCurves_Constraint FirstCons,
0266 const AppParCurves_Constraint LastCons,
0267 const Standard_Integer NbPol):
0268 SCU(NbPol),
0269 mypoles(1, NbPol,
0270 1, NbBColumns(SSP)),
0271 A(FirstPoint, LastPoint, 1, NbPol),
0272 DA(FirstPoint, LastPoint, 1, NbPol),
0273 B2(TheFirstPoint(FirstCons, FirstPoint),
0274 Max(TheFirstPoint(FirstCons, FirstPoint),
0275 TheLastPoint(LastCons, LastPoint)),
0276 1, NbBColumns(SSP)),
0277 mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
0278 Vflatknots(1, FlatLength(Mults)),
0279 Vec1t(1, NbBColumns(SSP)),
0280 Vec1c(1, NbBColumns(SSP)),
0281 Vec2t(1, NbBColumns(SSP)),
0282 Vec2c(1, NbBColumns(SSP)),
0283 theError(FirstPoint, LastPoint,
0284 1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
0285 myindex(FirstPoint, LastPoint, 0),
0286 nbpoles(NbPol)
0287 {
0288 myknots = new TColStd_HArray1OfReal(Knots.Lower(), Knots.Upper());
0289 myknots->ChangeArray1() = Knots;
0290 mymults = new TColStd_HArray1OfInteger(Mults.Lower(), Mults.Upper());
0291 mymults->ChangeArray1() = Mults;
0292 SCU.SetKnots(Knots);
0293 SCU.SetMultiplicities(Mults);
0294 FirstConstraint = FirstCons;
0295 LastConstraint = LastCons;
0296 Init(SSP, FirstPoint, LastPoint);
0297 }
0298
0299
0300
0301 void AppParCurves_LeastSquare::Init(const MultiLine& SSP,
0302 const Standard_Integer FirstPoint,
0303 const Standard_Integer LastPoint)
0304 {
0305 // Variable de controle
0306 iscalculated = Standard_False;
0307 isready = Standard_True;
0308
0309 myfirstp = FirstPoint;
0310 mylastp = LastPoint;
0311 FirstP = TheFirstPoint(FirstConstraint, myfirstp);
0312 LastP = TheLastPoint(LastConstraint, mylastp);
0313
0314 // Reperage des contraintes aux extremites:
0315 // ========================================
0316 Standard_Integer i, j, k, i2;
0317
0318 nbP2d = ToolLine::NbP2d(SSP);
0319 nbP = ToolLine::NbP3d(SSP);
0320 gp_Pnt Poi;
0321 gp_Pnt2d Poi2d;
0322 // gp_Vec V3d;
0323 // gp_Vec2d V2d;
0324 Standard_Integer mynbP2d = nbP2d, mynbP = nbP;
0325 if (nbP2d == 0) mynbP2d = 1;
0326 if (nbP == 0) mynbP = 1;
0327 TColgp_Array1OfPnt TabP(1, mynbP);
0328 TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
0329 TColgp_Array1OfVec TabV(1, mynbP);
0330 TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
0331
0332
0333 deg = nbpoles-1;
0334
0335 if (!mymults.IsNull()) {
0336 Standard_Integer sum = 0;
0337 for (i = mymults->Lower(); i <= mymults->Upper(); i++) {
0338 sum += mymults->Value(i);
0339 }
0340 deg = sum -nbpoles-1;
0341 k = 1;
0342 Standard_Real val;
0343 for (i = myknots->Lower(); i <= myknots->Upper(); i++) {
0344 for (j = 1; j <= mymults->Value(i); j++) {
0345 val = myknots->Value(i);
0346 Vflatknots(k) = val;
0347 k++;
0348 }
0349 }
0350 }
0351
0352
0353 Affect(SSP, FirstPoint, FirstConstraint, Vec1t, Vec1c);
0354
0355 Affect(SSP, LastPoint, LastConstraint, Vec2t, Vec2c);
0356
0357 for (j = myfirstp; j <= mylastp; j++) {
0358 i2 = 1;
0359 if (nbP != 0 && nbP2d != 0) ToolLine::Value(SSP, j, TabP,TabP2d);
0360 else if (nbP2d != 0) ToolLine::Value(SSP, j, TabP2d);
0361 else ToolLine::Value(SSP, j, TabP);
0362 for (i = 1; i <= nbP; i++) {
0363 (TabP(i)).Coord(mypoints(j,i2),mypoints(j,i2+1),mypoints(j,i2+2));
0364 i2 += 3;
0365 }
0366 for (i = 1;i <= nbP2d; i++) {
0367 (TabP2d(i)).Coord(mypoints(j, i2), mypoints(j, i2+1));
0368 i2 += 2;
0369 }
0370 }
0371
0372 AppParCurves_MultiPoint Pole1(nbP, nbP2d), PoleN(nbP, nbP2d);
0373
0374 if (FirstConstraint == AppParCurves_PassPoint ||
0375 FirstConstraint == AppParCurves_TangencyPoint ||
0376 FirstConstraint == AppParCurves_CurvaturePoint) {
0377 i2 = 1;
0378 for (i = 1; i <= nbP; i++) {
0379 Poi.SetCoord(mypoints(myfirstp, i2),
0380 mypoints(myfirstp, i2+1),
0381 mypoints(myfirstp, i2+2));
0382 Pole1.SetPoint(i, Poi);
0383 i2 += 3;
0384 }
0385 for (i = 1; i <= nbP2d; i++) {
0386 Poi2d.SetCoord(mypoints(myfirstp, i2), mypoints(myfirstp, i2+1));
0387 Pole1.SetPoint2d(i+nbP, Poi2d);
0388 i2 += 2;
0389 }
0390 for (i = 1; i <= mypoles.ColNumber(); i++)
0391 mypoles(1, i) = mypoints(myfirstp, i);
0392 }
0393
0394
0395
0396 if (LastConstraint == AppParCurves_PassPoint ||
0397 LastConstraint == AppParCurves_TangencyPoint ||
0398 FirstConstraint == AppParCurves_CurvaturePoint) {
0399 i2 = 1;
0400 for (i = 1; i <= nbP; i++) {
0401 Poi.SetCoord(mypoints(mylastp, i2),
0402 mypoints(mylastp, i2+1),
0403 mypoints(mylastp, i2+2));
0404 PoleN.SetPoint(i, Poi);
0405 i2 += 3;
0406 }
0407 for (i = 1; i <= nbP2d; i++) {
0408 Poi2d.SetCoord(mypoints(mylastp, i2),
0409 mypoints(mylastp, i2+1));
0410 PoleN.SetPoint2d(i+nbP, Poi2d);
0411 i2 += 2;
0412 }
0413
0414 for (i = 1; i <= mypoles.ColNumber(); i++)
0415 mypoles(nbpoles, i) = mypoints(mylastp, i);
0416 }
0417
0418
0419 if (FirstConstraint == AppParCurves_NoConstraint) {
0420 resinit = 1;
0421 SCU.SetValue(1, Pole1);
0422 if (LastConstraint == AppParCurves_NoConstraint) {
0423 resfin = nbpoles;
0424 }
0425 else if (LastConstraint == AppParCurves_PassPoint) {
0426 resfin = nbpoles-1;
0427 SCU.SetValue(nbpoles, PoleN);
0428 }
0429 else if (LastConstraint == AppParCurves_TangencyPoint) {
0430 resfin = nbpoles-2;
0431 SCU.SetValue(nbpoles, PoleN);
0432 }
0433 else if (LastConstraint == AppParCurves_CurvaturePoint) {
0434 resfin = nbpoles-3;
0435 SCU.SetValue(nbpoles, PoleN);
0436 }
0437 }
0438 else if (FirstConstraint == AppParCurves_PassPoint) {
0439 resinit = 2;
0440 SCU.SetValue(1, Pole1);
0441 if (LastConstraint == AppParCurves_NoConstraint) {
0442 resfin = nbpoles;
0443 }
0444 else if (LastConstraint == AppParCurves_PassPoint) {
0445 resfin = nbpoles-1;
0446 SCU.SetValue(nbpoles, PoleN);
0447 }
0448 else if (LastConstraint == AppParCurves_TangencyPoint) {
0449 resfin = nbpoles-2;
0450 SCU.SetValue(nbpoles, PoleN);
0451 }
0452 else if (LastConstraint == AppParCurves_CurvaturePoint) {
0453 resfin = nbpoles-3;
0454 SCU.SetValue(nbpoles, PoleN);
0455 }
0456 }
0457 else if (FirstConstraint == AppParCurves_TangencyPoint) {
0458 resinit = 3;
0459 SCU.SetValue(1, Pole1);
0460 if (LastConstraint == AppParCurves_NoConstraint) {
0461 resfin = nbpoles;
0462 }
0463 if (LastConstraint == AppParCurves_PassPoint) {
0464 resfin = nbpoles-1;
0465 SCU.SetValue(nbpoles, PoleN);
0466 }
0467 if (LastConstraint == AppParCurves_TangencyPoint) {
0468 resfin = nbpoles-2;
0469 SCU.SetValue(nbpoles, PoleN);
0470 }
0471 else if (LastConstraint == AppParCurves_CurvaturePoint) {
0472 resfin = nbpoles-3;
0473 SCU.SetValue(nbpoles, PoleN);
0474 }
0475 }
0476 else if (FirstConstraint == AppParCurves_CurvaturePoint) {
0477 resinit = 4;
0478 SCU.SetValue(1, Pole1);
0479 if (LastConstraint == AppParCurves_NoConstraint) {
0480 resfin = nbpoles;
0481 }
0482 if (LastConstraint == AppParCurves_PassPoint) {
0483 resfin = nbpoles-1;
0484 SCU.SetValue(nbpoles, PoleN);
0485 }
0486 if (LastConstraint == AppParCurves_TangencyPoint) {
0487 resfin = nbpoles-2;
0488 SCU.SetValue(nbpoles, PoleN);
0489 }
0490 else if (LastConstraint == AppParCurves_CurvaturePoint) {
0491 resfin = nbpoles-3;
0492 SCU.SetValue(nbpoles, PoleN);
0493 }
0494 }
0495
0496 Standard_Integer Nincx = resfin-resinit+1;
0497 if (Nincx<1) { //Impossible d'aller plus loin
0498 isready = Standard_False;
0499 return;
0500 }
0501 Standard_Integer Neq = LastP-FirstP+1;
0502
0503 NA = 3*nbP+2*nbP2d;
0504 Nlignes = NA*Neq;
0505 Ninc = NA*Nincx;
0506 if (FirstConstraint >= AppParCurves_TangencyPoint) Ninc++;
0507 if (LastConstraint >= AppParCurves_TangencyPoint) Ninc++;
0508 }
0509
0510
0511
0512
0513 void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters) {
0514
0515 done = Standard_False;
0516 if (!isready) {
0517 return;
0518 }
0519 Standard_Integer i, j, k, Ci, i2, k1, k2;
0520 Standard_Integer nbpol1 = nbpoles-1, Ninc1 = Ninc-1;
0521 Standard_Real AD1, A0;
0522 // gp_Pnt Pt;
0523 // gp_Pnt2d Pt2d;
0524 iscalculated = Standard_False;
0525
0526 // calcul de la matrice A et DA des fonctions d approximation:
0527 ComputeFunction(Parameters);
0528
0529 if (FirstConstraint != AppParCurves_TangencyPoint &&
0530 LastConstraint != AppParCurves_TangencyPoint) {
0531 if (FirstConstraint == AppParCurves_NoConstraint) {
0532 if (LastConstraint == AppParCurves_NoConstraint ) {
0533
0534 math_Householder HouResol(A, mypoints);
0535 if (!(HouResol.IsDone())) {
0536 done = Standard_False;
0537 return;
0538 }
0539 done = Standard_True;
0540 mypoles = HouResol.AllValues();
0541 return;
0542
0543 }
0544 else {
0545 for (j = FirstP; j <= LastP; j++) {
0546 AD1 = A(j, nbpoles);
0547 for (i = 1; i <= B2.ColNumber(); i++) {
0548 B2(j, i) = mypoints(j,i) - AD1*mypoles(nbpoles, i);
0549 }
0550 }
0551 }
0552 }
0553 else if (FirstConstraint == AppParCurves_PassPoint) {
0554 if (LastConstraint == AppParCurves_NoConstraint) {
0555 for (j = FirstP; j <= LastP; j++) {
0556 A0 = A(j, 1);
0557 for (i = 1; i <= B2.ColNumber(); i++) {
0558 B2(j, i) = mypoints(j, i)- A0*mypoles(1, i);
0559 }
0560 }
0561 }
0562 else if (LastConstraint == AppParCurves_PassPoint) {
0563 for (j = FirstP; j <= LastP; j++) {
0564 A0 = A(j, 1);
0565 AD1 = A(j, nbpoles);
0566 for (i = 1; i <= B2.ColNumber(); i++) {
0567 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0568 - AD1* mypoles(nbpoles, i);
0569 }
0570 }
0571 }
0572 }
0573
0574 // resolution:
0575
0576 Standard_Integer Nincx = resfin-resinit+1;
0577 if (Nincx < 1) {
0578 done = Standard_True;
0579 return;
0580 }
0581 math_IntegerVector Index(1, Nincx);
0582 SearchIndex(Index);
0583 math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0);
0584 math_Vector TheAA(1, Index(Nincx), 0.0);
0585 math_Vector myTABB(1, Nincx, 0.0);
0586
0587 MakeTAA(TheAA, mytab);
0588 DACTCL_Decompose(TheAA, Index);
0589
0590 Standard_Integer kk2;
0591 for (j = 1; j <= B2.ColNumber(); j++) {
0592 kk2 = 1;
0593 for (i = resinit; i <= resfin; i++) {
0594 myTABB(kk2) = mytab(i, j);
0595 kk2++;
0596 }
0597 DACTCL_Solve(TheAA, myTABB, Index);
0598
0599 i2 = 1;
0600 for (k = resinit; k <= resfin; k++) {
0601 mypoles(k, j) = myTABB.Value(i2);
0602 i2++;
0603 }
0604 }
0605 done = Standard_True;
0606 }
0607
0608 // ===========================================================
0609 // cas de tangence:
0610 // ===========================================================
0611
0612 Standard_Integer Nincx = resfin-resinit+1;
0613 Standard_Integer deport = 0, Nincx2 = 2*Nincx;
0614
0615 math_IntegerVector InternalIndex(1, Nincx);
0616 SearchIndex(InternalIndex);
0617 math_IntegerVector Index(1, Ninc);
0618
0619 Standard_Integer l = 1;
0620 if (resinit <= resfin) {
0621 for (j = 0; j <= NA-1; j++) {
0622 deport = j*InternalIndex(Nincx);
0623 for (i = 1; i <= Nincx; i++) {
0624 Index(l) = InternalIndex(i) + deport;
0625 l++;
0626 }
0627 }
0628 }
0629
0630 if (resinit > resfin) Index(1) = 1;
0631 if (Ninc1 > 1) {
0632 if (FirstConstraint >= AppParCurves_TangencyPoint &&
0633 LastConstraint >= AppParCurves_TangencyPoint)
0634 Index(Ninc1) = Index(Ninc1 - 1) + Ninc1;
0635 }
0636 if (FirstConstraint >= AppParCurves_TangencyPoint ||
0637 LastConstraint >= AppParCurves_TangencyPoint)
0638 Index(Ninc) = Index(Ninc-1) + Ninc;
0639
0640
0641 math_Vector TheA(1, Index(Ninc), 0.0);
0642 math_Vector myTAB(1, Ninc, 0.0);
0643
0644 MakeTAA(TheA, myTAB);
0645
0646 Standard_Integer Error = DACTCL_Decompose(TheA, Index);
0647 Error = DACTCL_Solve(TheA, myTAB, Index);
0648
0649 if (!Error) done = Standard_True;
0650
0651 if (FirstConstraint >= AppParCurves_TangencyPoint &&
0652 LastConstraint >= AppParCurves_TangencyPoint) {
0653 lambda1 = myTAB.Value(Ninc1);
0654 lambda2 = myTAB.Value(Ninc);
0655 }
0656 else if (FirstConstraint >= AppParCurves_TangencyPoint)
0657 lambda1 = myTAB.Value(Ninc);
0658 else if (LastConstraint >= AppParCurves_TangencyPoint)
0659 lambda2 = myTAB.Value(Ninc);
0660
0661
0662
0663 // Les resultats sont stockes dans mypoles.
0664 //=========================================
0665 k = 1;
0666 i2 = 1;
0667 for (Ci = 1; Ci <= nbP; Ci++) {
0668 k1 = k+1; k2 = k+2;
0669 for (j = resinit; j <= resfin; j++) {
0670 mypoles(j, k) = myTAB.Value(i2);
0671 mypoles(j, k1) = myTAB.Value(i2+Nincx);
0672 mypoles(j, k2) = myTAB.Value(i2+Nincx2);
0673 i2++;
0674 }
0675
0676 if (FirstConstraint >= AppParCurves_TangencyPoint) {
0677 mypoles(2, k) = mypoints(myfirstp, k) + lambda1*Vec1t(k);
0678 mypoles(2, k1) = mypoints(myfirstp, k1) + lambda1*Vec1t(k1);
0679 mypoles(2, k2) = mypoints(myfirstp, k2) + lambda1*Vec1t(k2);
0680 }
0681 if (LastConstraint >= AppParCurves_TangencyPoint) {
0682 mypoles(nbpol1, k) = mypoints(mylastp, k) - lambda2*Vec2t(k);
0683 mypoles(nbpol1, k1) = mypoints(mylastp, k1) - lambda2*Vec2t(k1);
0684 mypoles(nbpol1, k2) = mypoints(mylastp, k2) - lambda2*Vec2t(k2);
0685 }
0686 k += 3; i2 += Nincx2;
0687 }
0688
0689 for (Ci = 1; Ci <= nbP2d; Ci++) {
0690 k1 = k+1; k2 = k+2;
0691 for (j = resinit; j <= resfin; j++) {
0692 mypoles(j, k) = myTAB.Value(i2);
0693 mypoles(j, k1) = myTAB.Value(i2+Nincx);
0694 i2++;
0695 }
0696 if (FirstConstraint >= AppParCurves_TangencyPoint) {
0697 mypoles(2, k) = mypoints(myfirstp, k) + lambda1*Vec1t(k);
0698 mypoles(2, k1) = mypoints(myfirstp, k1) + lambda1*Vec1t(k1);
0699 }
0700 if (LastConstraint >= AppParCurves_TangencyPoint) {
0701 mypoles(nbpol1, k) = mypoints(mylastp, k) - lambda2*Vec2t(k);
0702 mypoles(nbpol1, k1) = mypoints(mylastp, k1) - lambda2*Vec2t(k1);
0703 }
0704 k += 2; i2 += Nincx;
0705 }
0706
0707 }
0708
0709 void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters,
0710 const math_Vector& V1t,
0711 const math_Vector& V2t,
0712 const Standard_Real l1,
0713 const Standard_Real l2)
0714 {
0715 done = Standard_False;
0716 if (!isready) {
0717 return;
0718 }
0719 Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower();
0720 resinit = 3; resfin = nbpoles-2;
0721 Standard_Integer Nincx = resfin-resinit+1;
0722 Ninc = NA*Nincx + 2;
0723 FirstConstraint = AppParCurves_TangencyPoint;
0724 LastConstraint = AppParCurves_TangencyPoint;
0725 for (i = 1; i <= Vec1t.Upper(); i++) {
0726 Vec1t(i) = V1t(i+lower1-1);
0727 Vec2t(i) = V2t(i+lower2-1);
0728 }
0729 Perform (Parameters, l1, l2);
0730 }
0731
0732
0733 void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters,
0734 const math_Vector& V1t,
0735 const math_Vector& V2t,
0736 const math_Vector& V1c,
0737 const math_Vector& V2c,
0738 const Standard_Real l1,
0739 const Standard_Real l2) {
0740 done = Standard_False;
0741 if (!isready) {
0742 return;
0743 }
0744 Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower();
0745 Standard_Integer lowc1 = V1c.Lower(), lowc2 = V2c.Lower();
0746 resinit = 4; resfin = nbpoles-3;
0747 Standard_Integer Nincx = resfin-resinit+1;
0748 Ninc = NA*Nincx + 2;
0749 FirstConstraint = AppParCurves_CurvaturePoint;
0750 LastConstraint = AppParCurves_CurvaturePoint;
0751
0752 for (i = 1; i <= Vec1t.Upper(); i++) {
0753 Vec1t(i) = V1t(i+lower1-1);
0754 Vec2t(i) = V2t(i+lower2-1);
0755 Vec1c(i) = V1c(i+lowc1-1);
0756 Vec2c(i) = V2c(i+lowc2-1);
0757 }
0758 Perform (Parameters, l1, l2);
0759 }
0760
0761
0762
0763 void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters,
0764 const Standard_Real l1,
0765 const Standard_Real l2) {
0766 done = Standard_False;
0767 if (!isready) {
0768 return;
0769 }
0770 if (FirstConstraint < AppParCurves_TangencyPoint &&
0771 LastConstraint < AppParCurves_TangencyPoint) {
0772 Perform(Parameters);
0773 return;
0774 }
0775 iscalculated = Standard_False;
0776
0777 lambda1 = l1;
0778 lambda2 = l2;
0779 Standard_Integer i, j, k, i2;
0780 Standard_Real AD0, AD1, AD2, A0, A1, A2;
0781 // gp_Pnt Pt, P1, P2;
0782 // gp_Pnt2d Pt2d, P12d, P22d;
0783 Standard_Real l11 = deg*l1, l22 = deg*l2;
0784
0785 ComputeFunction(Parameters);
0786
0787 if (FirstConstraint >= AppParCurves_TangencyPoint) {
0788 for (i = 1; i <= mypoles.ColNumber(); i++)
0789 mypoles(2, i) = mypoints(myfirstp, i) + l1*Vec1t(i);
0790 }
0791
0792
0793 if (FirstConstraint == AppParCurves_CurvaturePoint) {
0794 for (i = 1; i <= mypoles.ColNumber(); i++)
0795 mypoles(3, i) = 2*mypoles(2, i)-mypoles(1, i)
0796 + l11*l11*Vec1c(i)/(deg*(deg-1));
0797 }
0798
0799
0800 if (LastConstraint >= AppParCurves_TangencyPoint) {
0801 for (i = 1; i <= mypoles.ColNumber(); i++)
0802 mypoles(nbpoles-1, i) = mypoints(mylastp, i) - l2*Vec2t(i);
0803 }
0804
0805
0806 if (LastConstraint == AppParCurves_CurvaturePoint) {
0807 for (i = 1; i <= mypoles.ColNumber(); i++)
0808 mypoles(nbpoles-2, i) = 2*mypoles(nbpoles-1, i) - mypoles(nbpoles, i)
0809 + l22*l22*Vec2c(i)/(deg*(deg-1));
0810 }
0811
0812 if (resinit > resfin) {
0813 done = Standard_True;
0814 return;
0815 }
0816
0817 if (FirstConstraint == AppParCurves_NoConstraint) {
0818 if (LastConstraint == AppParCurves_TangencyPoint) {
0819 for (j = FirstP; j <= LastP; j++) {
0820 AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1);
0821 for (i = 1; i <= B2.ColNumber(); i++) {
0822 B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i)
0823 - AD1 * mypoles(nbpoles-1, i);
0824 }
0825 }
0826 }
0827 if (LastConstraint == AppParCurves_CurvaturePoint) {
0828 for (j = FirstP; j <= LastP; j++) {
0829 AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
0830 for (i = 1; i <= B2.ColNumber(); i++) {
0831 B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i)
0832 - AD1 * mypoles(nbpoles-1, i)
0833 - AD2 * mypoles(nbpoles-2, i);
0834 }
0835 }
0836 }
0837 }
0838 else if (FirstConstraint == AppParCurves_PassPoint) {
0839 if (LastConstraint == AppParCurves_TangencyPoint) {
0840 for (j = FirstP; j <= LastP; j++) {
0841 A0 = A(j, 1);
0842 AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1);
0843 for (i = 1; i <= B2.ColNumber(); i++) {
0844 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0845 - AD0 * mypoles(nbpoles, i)
0846 - AD1 * mypoles(nbpoles-1, i);
0847 }
0848 }
0849 }
0850 if (LastConstraint == AppParCurves_CurvaturePoint) {
0851 for (j = FirstP; j <= LastP; j++) {
0852 A0 = A(j, 1);
0853 AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
0854 for (i = 1; i <= B2.ColNumber(); i++) {
0855 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0856 - AD0 * mypoles(nbpoles, i)
0857 - AD1 * mypoles(nbpoles-1, i)
0858 - AD2 * mypoles(nbpoles-2, i);
0859 }
0860 }
0861 }
0862 }
0863 else if (FirstConstraint == AppParCurves_TangencyPoint) {
0864 if (LastConstraint == AppParCurves_NoConstraint) {
0865 for (j = FirstP; j <= LastP; j++) {
0866 A0 = A(j, 1); A1 = A(j, 2);
0867 for (i = 1; i <= B2.ColNumber(); i++) {
0868 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0869 - A1 * mypoles(2, i);
0870 }
0871 }
0872 }
0873 else if (LastConstraint == AppParCurves_PassPoint) {
0874 for (j = FirstP; j <= LastP; j++) {
0875 A0 = A(j, 1); AD0 = A(j, nbpoles); A1 = A(j, 2);
0876 for (i = 1; i <= B2.ColNumber(); i++) {
0877 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0878 - AD0 * mypoles(nbpoles, i)
0879 - A1 * mypoles(2, i);
0880 }
0881 }
0882 }
0883 else if (LastConstraint == AppParCurves_TangencyPoint) {
0884 for (j = FirstP; j <= LastP; j++) {
0885 A0 = A(j, 1); AD0 = A(j, nbpoles); A1 = A(j, 2); AD1 = A(j, nbpoles-1);
0886 for (i = 1; i <= B2.ColNumber(); i++) {
0887 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0888 - AD0 * mypoles(nbpoles, i)
0889 - A1 * mypoles(2, i)
0890 - AD1 * mypoles(nbpoles-1, i);
0891 }
0892 }
0893 }
0894 }
0895 else if (FirstConstraint == AppParCurves_CurvaturePoint) {
0896 if (LastConstraint == AppParCurves_NoConstraint) {
0897 for (j = FirstP; j <= LastP; j++) {
0898 A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3);
0899 for (i = 1; i <= B2.ColNumber(); i++) {
0900 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0901 - A1 * mypoles(2, i)
0902 - A2 * mypoles(3, i);
0903 }
0904 }
0905 }
0906 else if (LastConstraint == AppParCurves_PassPoint) {
0907 for (j = FirstP; j <= LastP; j++) {
0908 A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3); AD0 = A(j, nbpoles);
0909 for (i = 1; i <= B2.ColNumber(); i++) {
0910 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0911 - A1 * mypoles(2, i)
0912 - A2 * mypoles(3, i)
0913 - AD0 * mypoles(nbpoles, i);
0914 }
0915 }
0916 }
0917 else if (LastConstraint == AppParCurves_TangencyPoint) {
0918 for (j = FirstP; j <= LastP; j++) {
0919 A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3);
0920 AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1);
0921 for (i = 1; i <= B2.ColNumber(); i++) {
0922 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0923 - A1 * mypoles(2, i)
0924 - A2 * mypoles(3, i)
0925 - AD0 * mypoles(nbpoles, i)
0926 - AD1 * mypoles(nbpoles-1, i);
0927 }
0928 }
0929 }
0930 else if (LastConstraint == AppParCurves_CurvaturePoint) {
0931 for (j = FirstP; j <= LastP; j++) {
0932 A0 = A(j, 1); A1 = A(j, 2); A2 = A(j, 3);
0933 AD0 = A(j, nbpoles); AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
0934 for (i = 1; i <= B2.ColNumber(); i++) {
0935 B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
0936 - A1 * mypoles(2, i)
0937 - A2 * mypoles(3, i)
0938 - AD0 * mypoles(nbpoles, i)
0939 - AD1 * mypoles(nbpoles-1, i)
0940 - AD2 * mypoles(nbpoles-2, i);
0941 }
0942 }
0943 }
0944 }
0945
0946 Standard_Integer Nincx = resfin-resinit+1;
0947
0948 math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0);
0949 math_IntegerVector Index(1, Nincx);
0950 SearchIndex(Index);
0951 math_Vector AA(1, Index(Nincx), 0.0);
0952 MakeTAA(AA, mytab);
0953
0954 math_Vector myTABB(1, Nincx, 0.0);
0955
0956 DACTCL_Decompose(AA, Index);
0957
0958 Standard_Integer kk2;
0959 for (j = 1; j <= B2.ColNumber(); j++) {
0960 kk2 = 1;
0961 for (i = resinit; i <= resfin; i++) {
0962 myTABB(kk2) = mytab(i, j);
0963 kk2++;
0964 }
0965
0966 DACTCL_Solve(AA, myTABB, Index);
0967
0968 i2 = 1;
0969 for (k = resinit; k <= resfin; k++) {
0970 mypoles(k, j) = myTABB.Value(i2);
0971 i2++;
0972 }
0973
0974 }
0975
0976 done = Standard_True;
0977
0978 }
0979
0980
0981
0982 //=======================================================================
0983 //function : Affect
0984 //purpose : Index is an ID of the point in MultiLine. Every point is set of
0985 // several 3D- and 2D-points. E.g. every points of Walking-line,
0986 // obtained in intersection algorithm, is set of one 3D points
0987 // (nbP == 1) and two 2D-points (nbP2d == 2).
0988 //=======================================================================
0989 void AppParCurves_LeastSquare::Affect(const MultiLine& SSP,
0990 const Standard_Integer Index,
0991 AppParCurves_Constraint& Cons,
0992 math_Vector& Vt,
0993 math_Vector& Vc)
0994 {
0995 // Vt: vector of tangent, Vc: vector of curvature.
0996
0997 if (Cons >= AppParCurves_TangencyPoint) {
0998 Standard_Integer i, i2 = 1;
0999 Standard_Boolean Ok;
1000 Standard_Integer mynbP2d = nbP2d, mynbP = nbP;
1001 if (nbP2d == 0) mynbP2d = 1;
1002 if (nbP == 0) mynbP = 1;
1003 TColgp_Array1OfVec TabV(1, mynbP);
1004 TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
1005
1006 if (Cons == AppParCurves_CurvaturePoint)
1007 {
1008 if (nbP != 0 && nbP2d != 0)
1009 {
1010 Ok = ToolLine::Curvature(SSP, Index,TabV,TabV2d);
1011 if (!Ok) { Cons = AppParCurves_TangencyPoint;}
1012 }
1013 else if (nbP2d != 0)
1014 {
1015 Ok = ToolLine::Curvature(SSP, Index, TabV2d);
1016 if (!Ok) { Cons = AppParCurves_TangencyPoint;}
1017 }
1018 else {
1019 Ok = ToolLine::Curvature(SSP, Index, TabV);
1020 if (!Ok) { Cons = AppParCurves_TangencyPoint;}
1021 }
1022 if (Ok) {
1023 for (i = 1; i <= nbP; i++) {
1024 (TabV(i)).Coord(Vc(i2), Vc(i2+1), Vc(i2+2));
1025 i2 += 3;
1026 }
1027
1028 for (i = 1; i <= nbP2d; i++) {
1029 (TabV2d(i)).Coord(Vc(i2), Vc(i2+1));
1030 i2 += 2;
1031 }
1032 }
1033 }
1034
1035 i2 = 1;
1036 if (Cons >= AppParCurves_TangencyPoint) {
1037 if (nbP != 0 && nbP2d != 0) {
1038 Ok = ToolLine::Tangency(SSP, Index, TabV, TabV2d);
1039 if (!Ok) { Cons = AppParCurves_PassPoint;}
1040 }
1041 else if (nbP2d != 0) {
1042 Ok = ToolLine::Tangency(SSP, Index, TabV2d);
1043 if (!Ok) { Cons = AppParCurves_PassPoint;}
1044 }
1045 else {
1046 Ok = ToolLine::Tangency(SSP, Index, TabV);
1047 if (!Ok) { Cons = AppParCurves_PassPoint;}
1048 }
1049
1050 if (Ok)
1051 {
1052 TColgp_Array1OfPnt anArrPts3d1(1, mynbP), anArrPts3d2(1, mynbP);
1053
1054 if(nbP != 0)
1055 {
1056 if(Index < ToolLine::LastPoint(SSP))
1057 {
1058 ToolLine::Value(SSP, Index, anArrPts3d1);
1059 ToolLine::Value(SSP, Index+1, anArrPts3d2);
1060 }
1061 else
1062 {// (Index == ToolLine::LastPoint(theML))
1063 ToolLine::Value(SSP, Index-1, anArrPts3d1);
1064 ToolLine::Value(SSP, Index, anArrPts3d2);
1065 }
1066
1067 CheckTangents(anArrPts3d1, anArrPts3d2, TabV, TabV2d);
1068 }
1069 else if(nbP2d != 0)
1070 {
1071 TColgp_Array1OfPnt2d anArrPts2d1(1, mynbP2d), anArrPts2d2(1, mynbP2d);
1072
1073 if(Index < ToolLine::LastPoint(SSP))
1074 {
1075 ToolLine::Value(SSP, Index, anArrPts3d1, anArrPts2d1);
1076 ToolLine::Value(SSP, Index+1, anArrPts3d2, anArrPts2d2);
1077 }
1078 else
1079 {// (Index == ToolLine::LastPoint(theML))
1080 ToolLine::Value(SSP, Index-1, anArrPts3d1, anArrPts2d1);
1081 ToolLine::Value(SSP, Index, anArrPts3d2, anArrPts2d2);
1082 }
1083
1084 CheckTangents(anArrPts2d1, anArrPts2d2, TabV2d);
1085 }
1086
1087 for (i = 1; i <= nbP; i++) {
1088 (TabV(i)).Coord(Vt(i2), Vt(i2+1), Vt(i2+2));
1089 i2 += 3;
1090 }
1091
1092 for (i = 1; i <= nbP2d; i++) {
1093 (TabV2d(i)).Coord(Vt(i2), Vt(i2+1));
1094 i2 += 2;
1095 }
1096 }
1097 }
1098 }
1099 }
1100
1101
1102
1103 Standard_Integer AppParCurves_LeastSquare::NbBColumns(
1104 const MultiLine& SSP) const
1105 {
1106 Standard_Integer BCol;
1107 BCol = (ToolLine::NbP3d(SSP))*3 +
1108 (ToolLine::NbP2d(SSP))*2;
1109 return BCol;
1110 }
1111
1112
1113 void AppParCurves_LeastSquare::Error(Standard_Real& F,
1114 Standard_Real& MaxE3d,
1115 Standard_Real& MaxE2d)
1116 {
1117
1118 if (!done) {throw StdFail_NotDone();}
1119 Standard_Integer i, j, k, i2, indexdeb, indexfin;
1120 Standard_Integer i21, i22;
1121 Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ;
1122 MaxE3d = MaxE2d = 0.0;
1123 F = 0.0;
1124 i2 = 1;
1125 math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles);
1126
1127 for (k = 1 ; k <= nbP+nbP2d; k++) {
1128 i21 = i2+1; i22 = i2+2;
1129 for (i = 1; i <= nbpoles; i++) {
1130 Px(i) = mypoles(i, i2);
1131 Py(i) = mypoles(i, i21);
1132 if (k <= nbP) Pz(i) = mypoles(i, i22);
1133 }
1134 for (i = FirstP; i <= LastP; i++) {
1135 AA = 0.0; BB = 0.0; CC = 0.0;
1136 indexdeb = myindex(i) + 1;
1137 indexfin = indexdeb + deg;
1138 for (j = indexdeb; j <= indexfin; j++) {
1139 AIJ = A(i, j);
1140 AA += AIJ*Px(j);
1141 BB += AIJ*Py(j);
1142 if (k <= nbP) CC += AIJ*Pz(j);
1143 }
1144 FX = AA-mypoints(i, i2);
1145 FY = BB-mypoints(i, i21);
1146 Fi= FX*FX + FY*FY;
1147 if (k <= nbP) {
1148 FZ = CC-mypoints(i, i22);
1149 Fi += FZ*FZ;
1150 if (Fi > MaxE3d) MaxE3d = Fi;
1151 }
1152 else {
1153 if (Fi > MaxE2d) MaxE2d = Fi;
1154 }
1155 theError(i, k) = Fi;
1156 F += Fi;
1157 }
1158 if (k <= nbP) i2 += 3;
1159 else i2 += 2;
1160 }
1161 MaxE3d = Sqrt(MaxE3d);
1162 MaxE2d = Sqrt(MaxE2d);
1163 }
1164
1165
1166 void AppParCurves_LeastSquare::ErrorGradient(math_Vector& Grad,
1167 Standard_Real& F,
1168 Standard_Real& MaxE3d,
1169 Standard_Real& MaxE2d)
1170 {
1171 if (!done) {throw StdFail_NotDone();}
1172 Standard_Integer i, j, k, i2, indexdeb, indexfin;
1173 Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ;
1174 // Standard_Real DAIJ, DAA, DBB, DCC, Gr, gr1= 0.0, gr2= 0.0;
1175 Standard_Real DAIJ, DAA, DBB, DCC, Gr;
1176 MaxE3d = MaxE2d = 0.0;
1177 // Standard_Integer i21, i22, diff = (deg-1);
1178 Standard_Integer i21, i22;
1179 F = 0.0;
1180 i2 = 1;
1181 math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles);
1182
1183 for (k = Grad.Lower(); k <= Grad.Upper(); k++) Grad(k) = 0.0;
1184
1185 for (k = 1 ; k <= nbP+nbP2d; k++) {
1186 i21 = i2+1; i22 = i2+2;
1187 for (i = 1; i <= nbpoles; i++) {
1188 Px(i) = mypoles(i, i2);
1189 Py(i) = mypoles(i, i21);
1190 if (k <= nbP) Pz(i) = mypoles(i, i22);
1191 }
1192 for (i = FirstP; i <= LastP; i++) {
1193 AA = 0.0; BB = 0.0; CC = 0.0; DAA = 0.0; DBB = 0.0; DCC = 0.0;
1194 indexdeb = myindex(i) + 1;
1195 indexfin = indexdeb + deg;
1196 for (j = indexdeb; j <= indexfin; j++) {
1197 AIJ = A(i, j); DAIJ = DA(i, j);
1198 AA += AIJ*Px(j); DAA += DAIJ*Px(j);
1199 BB += AIJ*Py(j); DBB += DAIJ*Py(j);
1200 if (k <= nbP) {
1201 CC += AIJ*Pz(j);
1202 DCC += DAIJ*Pz(j);
1203 }
1204 }
1205 FX = AA-mypoints(i, i2);
1206 FY = BB-mypoints(i, i2+1);
1207 Fi = FX*FX + FY*FY;
1208 Gr = 2.0*(DAA*FX + DBB*FY);
1209
1210 if (k <= nbP) {
1211 FZ = CC-mypoints(i, i2+2);
1212 Fi += FZ*FZ;
1213 Gr += 2.0*DCC*FZ;
1214 if (Fi > MaxE3d) MaxE3d = Fi;
1215 }
1216 else {
1217 if (Fi > MaxE2d) MaxE2d = Fi;
1218 }
1219 theError(i, k) = Fi;
1220 Grad(i) += Gr;
1221 F += Fi;
1222 }
1223 if (k <= nbP) i2 += 3;
1224 else i2 += 2;
1225 }
1226 MaxE3d = Sqrt(MaxE3d);
1227 MaxE2d = Sqrt(MaxE2d);
1228
1229 }
1230
1231
1232 const math_Matrix& AppParCurves_LeastSquare::Distance()
1233 {
1234 if (!iscalculated) {
1235 for (Standard_Integer i = myfirstp; i <= mylastp; i++) {
1236 for (Standard_Integer j = 1; j <= nbP+nbP2d; j++) {
1237 theError(i, j) = Sqrt(theError(i, j));
1238 }
1239 }
1240 iscalculated = Standard_True;
1241 }
1242 return theError;
1243 }
1244
1245
1246 Standard_Real AppParCurves_LeastSquare::FirstLambda() const
1247 {
1248 return lambda1;
1249 }
1250
1251 Standard_Real AppParCurves_LeastSquare::LastLambda() const
1252 {
1253 return lambda2;
1254 }
1255
1256
1257
1258 Standard_Boolean AppParCurves_LeastSquare::IsDone() const
1259 {
1260 return done;
1261 }
1262
1263
1264 AppParCurves_MultiCurve AppParCurves_LeastSquare::BezierValue()
1265 {
1266 if (!myknots.IsNull()) throw Standard_NoSuchObject();
1267 return (AppParCurves_MultiCurve)(BSplineValue());
1268 }
1269
1270
1271 const AppParCurves_MultiBSpCurve& AppParCurves_LeastSquare::BSplineValue()
1272 {
1273 if (!done) {throw StdFail_NotDone();}
1274
1275 Standard_Integer i, j, j2, npoints = nbP+nbP2d;
1276 gp_Pnt Pt;
1277 gp_Pnt2d Pt2d;
1278 Standard_Integer ideb = resinit, ifin = resfin;
1279 if (ideb >= 2) ideb = 2;
1280 if (ifin <= nbpoles-1) ifin = nbpoles-1;
1281
1282 // On met le resultat dans les curves correspondantes
1283 for (i = ideb; i <= ifin; i++) {
1284 j2 = 1;
1285 AppParCurves_MultiPoint MPole(nbP, nbP2d);
1286 for (j = 1; j <= nbP; j++) {
1287 Pt.SetCoord(mypoles(i, j2), mypoles(i, j2+1), mypoles(i,j2+2));
1288 MPole.SetPoint(j, Pt);
1289 j2 += 3;
1290 }
1291 for (j = nbP+1;j <= npoints; j++) {
1292 Pt2d.SetCoord(mypoles(i, j2), mypoles(i, j2+1));
1293 MPole.SetPoint2d(j, Pt2d);
1294 j2 += 2;
1295 }
1296 SCU.SetValue(i, MPole);
1297 }
1298 return SCU;
1299 }
1300
1301
1302
1303 const math_Matrix& AppParCurves_LeastSquare::FunctionMatrix() const
1304 {
1305 if (!done) {throw StdFail_NotDone();}
1306 return A;
1307 }
1308
1309
1310 const math_Matrix& AppParCurves_LeastSquare::DerivativeFunctionMatrix() const
1311 {
1312 if (!done) {throw StdFail_NotDone();}
1313 return DA;
1314 }
1315
1316
1317
1318
1319 Standard_Integer AppParCurves_LeastSquare::
1320 TheFirstPoint(const AppParCurves_Constraint FirstCons,
1321 const Standard_Integer FirstPoint) const
1322 {
1323 if (FirstCons == AppParCurves_NoConstraint)
1324 return FirstPoint;
1325 else
1326 return FirstPoint + 1;
1327 }
1328
1329
1330
1331 Standard_Integer AppParCurves_LeastSquare::
1332 TheLastPoint(const AppParCurves_Constraint LastCons,
1333 const Standard_Integer LastPoint) const
1334 {
1335 if (LastCons == AppParCurves_NoConstraint)
1336 return LastPoint;
1337 else
1338 return LastPoint - 1;
1339 }
1340
1341
1342 void AppParCurves_LeastSquare::ComputeFunction(const math_Vector& Parameters)
1343 {
1344 if (myknots.IsNull()) {
1345 AppParCurves::Bernstein(nbpoles, Parameters, A, DA);
1346 }
1347 else {
1348 AppParCurves::SplineFunction(nbpoles, deg, Parameters,
1349 Vflatknots, A, DA, myindex);
1350 }
1351 }
1352
1353
1354
1355 const math_Matrix& AppParCurves_LeastSquare::Points() const
1356 {
1357 return mypoints;
1358 }
1359
1360 const math_Matrix& AppParCurves_LeastSquare::Poles() const
1361 {
1362 return mypoles;
1363 }
1364
1365
1366
1367 const math_IntegerVector& AppParCurves_LeastSquare::KIndex() const
1368 {
1369 return myindex;
1370 }
1371
1372
1373
1374
1375 void AppParCurves_LeastSquare::MakeTAA(math_Vector& TheA,
1376 math_Vector& myTAB)
1377 {
1378 Standard_Integer i, j, k, Ci, i2, i21, i22;
1379 Standard_Real xx = 0.0, yy = 0.0;
1380
1381 Standard_Integer Nincx = resfin-resinit+1;
1382 Standard_Integer Nrow, Neq = LastP-FirstP+1;
1383
1384 Standard_Integer Ninc1;
1385
1386 if (FirstConstraint >= AppParCurves_TangencyPoint &&
1387 LastConstraint >= AppParCurves_TangencyPoint) {
1388 Ninc1 = Ninc-1;
1389 }
1390 else Ninc1 = Ninc;
1391
1392 Standard_Integer myfirst = A.LowerRow();
1393 Standard_Integer ix, iy, iz;
1394 Standard_Integer mylast = myfirst+Nlignes-1;
1395 Standard_Integer k1;
1396 Standard_Real taf1 = 0.0, taf2 = 0.0, taf3 = 0.0,
1397 tab1 = 0.0, tab2 = 0.0;
1398 Standard_Integer nb, inc, jinit, jfin, u;
1399 Standard_Integer indexdeb, indexfin;
1400 Standard_Integer NA1 = NA-1;
1401 Standard_Real v1=0, v2=0, b;
1402 Standard_Real Ai2, Aid, Akj;
1403 math_Vector myB(myfirst, mylast, 0.0),
1404 myV1(myfirst, mylast, 0.0),
1405 myV2(myfirst, mylast, 0.0);
1406 math_Vector TheV1(1, Ninc, 0.0), TheV2(1, Ninc, 0.0);
1407
1408
1409 for (i = FirstP; i <= LastP; i++) {
1410 Ai2 = A(i, 2);
1411 Aid = A(i, nbpoles-1);
1412 if (FirstConstraint >= AppParCurves_PassPoint) xx = A(i, 1);
1413 if (FirstConstraint >= AppParCurves_TangencyPoint) xx += Ai2;
1414 if (LastConstraint >= AppParCurves_PassPoint) yy = A(i, nbpoles);
1415 if (LastConstraint >= AppParCurves_TangencyPoint) yy += Aid;
1416 i2 = 1;
1417 Nrow = myfirst-FirstP;
1418 for (Ci = 1; Ci <= nbP; Ci++) {
1419 i21 = i2+1; i22 = i2+2;
1420 ix = i+Nrow; iy = ix+Neq; iz = iy+Neq;
1421 if (FirstConstraint >= AppParCurves_TangencyPoint) {
1422 myV1(ix) = Ai2*Vec1t(i2);
1423 myV1(iy) = Ai2*Vec1t(i21);
1424 myV1(iz) = Ai2*Vec1t(i22);
1425 }
1426 if (LastConstraint >= AppParCurves_TangencyPoint) {
1427 myV2(ix) = -Aid*Vec2t(i2);
1428 myV2(iy) = -Aid*Vec2t(i21);
1429 myV2(iz) = -Aid*Vec2t(i22);
1430 }
1431 myB(ix) = mypoints(i, i2) - xx*mypoints(myfirstp, i2)
1432 - yy*mypoints(mylastp, i2);
1433 myB(iy) = mypoints(i, i21) - xx*mypoints(myfirstp, i21)
1434 - yy*mypoints(mylastp, i21);
1435 myB(iz) = mypoints(i, i22) - xx*mypoints(myfirstp, i22)
1436 - yy*mypoints(mylastp, i22);
1437 i2 += 3;
1438 Nrow = Nrow + 3*Neq;
1439 }
1440
1441 for (Ci = 1; Ci <= nbP2d; Ci++) {
1442 i21 = i2+1; i22 = i2+2;
1443 ix = i+Nrow; iy = ix+Neq;
1444 if (FirstConstraint >= AppParCurves_TangencyPoint) {
1445 myV1(ix) = Ai2*Vec1t(i2);
1446 myV1(iy) = Ai2*Vec1t(i21);
1447 }
1448 if (LastConstraint >= AppParCurves_TangencyPoint) {
1449 myV2(ix) = -Aid*Vec2t(i2);
1450 myV2(iy) = -Aid*Vec2t(i21);
1451 }
1452 myB(ix) = mypoints(i, i2) - xx*mypoints(myfirstp, i2)
1453 - yy*mypoints(mylastp, i2);
1454 myB(iy) = mypoints(i, i21) - xx*mypoints(myfirstp, i21)
1455 - yy*mypoints(mylastp, i21);
1456 Nrow = Nrow + 2*Neq;
1457 i2 += 2;
1458 }
1459 }
1460
1461
1462
1463 // Construction de TA*A et TA*B:
1464
1465 for (k = FirstP; k <= LastP; k++) {
1466 indexdeb = myindex(k)+1;
1467 indexfin = indexdeb + deg;
1468 jinit = Max(resinit, indexdeb);
1469 jfin = Min(resfin, indexfin);
1470 k1 = k + myfirst - FirstP;
1471 for (i = 0; i <= NA1; i++) {
1472 nb = i*Neq + k1;
1473 if (FirstConstraint >= AppParCurves_TangencyPoint)
1474 v1 = myV1(nb);
1475 if (LastConstraint >= AppParCurves_TangencyPoint)
1476 v2 = myV2(nb);
1477 b = myB(nb);
1478 inc = i*Nincx-resinit+1;
1479 for (j = jinit; j <= jfin; j++) {
1480 Akj = A(k, j);
1481 u = j+inc;
1482 if (FirstConstraint >= AppParCurves_TangencyPoint)
1483 TheV1(u) += Akj*v1;
1484 if (LastConstraint >= AppParCurves_TangencyPoint)
1485 TheV2(u) += Akj*v2;
1486 myTAB(u) += Akj*b;
1487 }
1488 if (FirstConstraint >= AppParCurves_TangencyPoint) {
1489 taf1 += v1*v1;
1490 tab1 += v1*b;
1491 }
1492 if (LastConstraint >= AppParCurves_TangencyPoint) {
1493 taf2 += v2*v2;
1494 tab2 += v2*b;
1495 }
1496 if (FirstConstraint >= AppParCurves_TangencyPoint &&
1497 LastConstraint >= AppParCurves_TangencyPoint) {
1498 taf3 += v1*v2;
1499 }
1500 }
1501 }
1502
1503
1504 if (FirstConstraint >= AppParCurves_TangencyPoint) {
1505 TheV1(Ninc1) = taf1;
1506 myTAB(Ninc1) = tab1;
1507 }
1508 if (LastConstraint >= AppParCurves_TangencyPoint) {
1509 TheV2(Ninc) = taf2;
1510 myTAB(Ninc) = tab2;
1511 }
1512 if (FirstConstraint >= AppParCurves_TangencyPoint &&
1513 LastConstraint >= AppParCurves_TangencyPoint) {
1514 TheV2(Ninc1) = taf3;
1515 }
1516
1517
1518 if (resinit <= resfin) {
1519 math_IntegerVector Index(1, Nincx);
1520 SearchIndex(Index);
1521 math_Vector AA(1, Index(Nincx));
1522 MakeTAA(AA);
1523
1524 Standard_Integer kk = 1;
1525 for (k = 1; k <= NA; k++) {
1526 for(i = 1; i <= AA.Length(); i++) {
1527 TheA(kk) = AA(i);
1528 kk++;
1529 }
1530 }
1531 }
1532
1533 Standard_Integer length = TheA.Length();
1534
1535 if (FirstConstraint >= AppParCurves_TangencyPoint &&
1536 LastConstraint >= AppParCurves_TangencyPoint) {
1537 for (j = 1; j <= Ninc1; j++)
1538 TheA(length- 2*Ninc+j+1) = TheV1(j);
1539 for (j = 1; j <= Ninc; j++)
1540 TheA(length- Ninc+j) = TheV2(j);
1541 }
1542
1543
1544 else if (FirstConstraint >= AppParCurves_TangencyPoint) {
1545 for (j = 1; j <= Ninc; j++)
1546 TheA(length- Ninc+j) = TheV1(j);
1547 }
1548
1549 else if (LastConstraint >= AppParCurves_TangencyPoint) {
1550 for (j = 1; j <= Ninc; j++)
1551 TheA(length- Ninc+j) = TheV2(j);
1552 }
1553 }
1554
1555
1556
1557
1558 void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA,
1559 math_Matrix& myTAB)
1560 {
1561 Standard_Integer i, j, k;
1562 Standard_Integer indexdeb, indexfin, jinit, jfin;
1563 Standard_Integer iinit, ifin;
1564 Standard_Real Akj;
1565 math_Matrix TheA(resinit, resfin, resinit, resfin);
1566 TheA.Init(0.0);
1567
1568 for (k = FirstP ; k <= LastP; k++) {
1569 indexdeb = myindex(k)+1;
1570 indexfin = indexdeb + deg;
1571 jinit = Max(resinit, indexdeb);
1572 jfin = Min(resfin, indexfin);
1573 for (i = jinit; i <= jfin; i++) {
1574 Akj = A(k, i);
1575 for (j = jinit; j <= i; j++) {
1576 TheA(i, j) += A(k, j) * Akj;
1577 }
1578 for (j = 1; j <= B2.ColNumber(); j++) {
1579 myTAB(i, j) += Akj*B2(k, j);
1580 }
1581 }
1582 }
1583
1584 Standard_Integer len;
1585 if (!myknots.IsNull()) len = myknots->Length();
1586 else len = 2;
1587 Standard_Integer d, i2 = 1;
1588 iinit = resinit;
1589 jinit = resinit;
1590 ifin = Min(resfin, deg+1);
1591 for (k = 2; k <= len; k++) {
1592 for (i = iinit; i <= ifin; i++) {
1593 for (j = jinit; j <= i; j++) {
1594 AA(i2) = TheA(i, j);
1595 i2++;
1596 }
1597 }
1598 if (!mymults.IsNull()) {
1599 iinit = ifin+1;
1600 d = ifin + mymults->Value(k);
1601 ifin = Min(d, resfin);
1602 jinit = Max(resinit, d - deg);
1603 }
1604 }
1605 }
1606
1607
1608 void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA)
1609 {
1610 Standard_Integer i, j, k;
1611 Standard_Integer indexdeb, indexfin, jinit, jfin;
1612 Standard_Integer iinit, ifin;
1613 Standard_Real Akj;
1614 math_Matrix TheA(resinit, resfin, resinit, resfin, 0.0);
1615
1616
1617 for (k = FirstP ; k <= LastP; k++) {
1618 indexdeb = myindex(k)+1;
1619 indexfin = indexdeb + deg;
1620 jinit = Max(resinit, indexdeb);
1621 jfin = Min(resfin, indexfin);
1622 for (i = jinit; i <= jfin; i++) {
1623 Akj = A(k, i);
1624 for (j = jinit; j <= i; j++) {
1625 TheA(i, j) += A(k, j) * Akj;
1626 }
1627 }
1628 }
1629
1630
1631 Standard_Integer i2 = 1;
1632 iinit = resinit;
1633 jinit = resinit;
1634 ifin = Min(resfin, deg+1);
1635 Standard_Integer len, d;
1636 if (!myknots.IsNull()) len = myknots->Length();
1637 else len = 2;
1638 for (k = 2; k <= len; k++) {
1639 for (i = iinit; i <= ifin; i++) {
1640 for (j = jinit; j <= i; j++) {
1641 AA(i2) = TheA(i, j);
1642 i2++;
1643 }
1644 }
1645 if (!mymults.IsNull()) {
1646 iinit = ifin+1;
1647 d = ifin + mymults->Value(k);
1648 ifin = Min(d, resfin);
1649 jinit = Max(resinit, d - deg);
1650 }
1651 }
1652
1653 }
1654
1655
1656
1657 void AppParCurves_LeastSquare::SearchIndex(math_IntegerVector& Index)
1658 {
1659 Standard_Integer iinit, jinit, ifin;
1660 Standard_Integer i, j, k, d, i2 = 1;
1661 Standard_Integer len;
1662 Standard_Integer Nincx = resfin-resinit+1;
1663 Index(1) = 1;
1664
1665 if (myknots.IsNull()) {
1666 if (resinit <= resfin) {
1667 Standard_Integer l = 1;
1668 for (i = 2; i <= Nincx; i++) {
1669 l++;
1670 Index(l) = Index(l-1)+i;
1671 }
1672 }
1673
1674 }
1675 else {
1676 iinit = resinit;
1677 jinit = resinit;
1678 ifin = Min(resfin, deg+1);
1679 len = myknots->Length();
1680
1681 for (k = 2; k <= len; k++) {
1682 for (i = iinit; i <= ifin; i++) {
1683 for (j = jinit; j <= i; j++) {
1684 if (i2 != 1)
1685 Index(i2) = Index(i2-1) + i-jinit+1;
1686 }
1687 i2++;
1688 }
1689 iinit = ifin+1;
1690 d = ifin + mymults->Value(k);
1691 ifin = Min(d, resfin);
1692 jinit = Max(resinit, d - deg);
1693 }
1694 }
1695 }
