src/Law/Law_Interpolate.cxx

   1 // File:        Law_Interpolate.cxx
   2 // Created:     Thu Nov 16 09:40:28 1995
   3 // Author:      Laurent BOURESCHE
   4 //              <lbo@phylox>
   5 // Programme cree
   6
   7 #include <Law_Interpolate.ixx>
   8 #include <Standard_ConstructionError.hxx>
   9 #include <PLib.hxx>
  10 #include <BSplCLib.hxx>
  11 #include <TColStd_Array1OfBoolean.hxx>
  12 #include <TColStd_Array1OfInteger.hxx>
  13 #include <gp_Pnt.hxx>
  14
  15 //=======================================================================
  16 //function : CheckParameters
  17 //purpose  :
  18 //=======================================================================
  19
  20 static Standard_Boolean CheckParameters
  21 (const TColStd_Array1OfReal&  Parameters)
  22 {
  23   Standard_Integer ii;
  24   Standard_Real distance;
  25   Standard_Boolean result = Standard_True;
  26   for (ii = Parameters.Lower() ; result && ii < Parameters.Upper() ; ii++) {
  27     distance = Parameters.Value(ii+1) - Parameters.Value(ii);
  28     result = (distance >= RealSmall());
  29   }
  30   return result;
  31 }
  32
  33 //=======================================================================
  34 //function : BuildParameters
  35 //purpose  :
  36 //=======================================================================
  37 static void  BuildParameters(const Standard_Boolean         PeriodicFlag,
  38                              const TColStd_Array1OfReal&    PointsArray,
  39                              Handle(TColStd_HArray1OfReal)& ParametersPtr)
  40 {
  41   Standard_Integer ii, index = 2;
  42   Standard_Real distance;
  43   Standard_Integer num_parameters = PointsArray.Length();
  44   if (PeriodicFlag) {
  45     num_parameters += 1;
  46   }
  47   ParametersPtr = new TColStd_HArray1OfReal(1, num_parameters);
  48   ParametersPtr->SetValue(1,0.);
  49   for (ii = PointsArray.Lower(); ii < PointsArray.Upper(); ii++) {
  50     distance = Abs(PointsArray.Value(ii) - PointsArray.Value(ii+1));
  51     ParametersPtr->SetValue(index, ParametersPtr->Value(ii) + distance);
  52     index += 1 ;
  53   }
  54   if (PeriodicFlag) {
  55     distance = Abs(PointsArray.Value(PointsArray.Upper()) -
  56                    PointsArray.Value(PointsArray.Lower()));
  57     ParametersPtr->SetValue(index, ParametersPtr->Value(ii) + distance);
  58   }
  59 }
  60
  61 //=======================================================================
  62 //function : BuildPeriodicTangents
  63 //purpose  :
  64 //=======================================================================
  65
  66 static void BuildPeriodicTangent
  67 (const TColStd_Array1OfReal& PointsArray,
  68  TColStd_Array1OfReal&       TangentsArray,
  69  TColStd_Array1OfBoolean&    TangentFlags,
  70  const TColStd_Array1OfReal& ParametersArray)
  71 {
  72   Standard_Real point_array[3], parameter_array[3], eval_result[2];
  73
  74   if (PointsArray.Length() < 2) {
  75     TangentFlags.SetValue(1,Standard_True);
  76     TangentsArray.SetValue(1,0.);
  77   }
  78   else if (!TangentFlags.Value(1)){
  79     //Pour les periodiques on evalue la tangente du point de fermeture
  80     //par une interpolation de degre 2 entre le dernier point, le point
  81     //de fermeture et le deuxieme point.
  82     Standard_Integer degree = 2;
  83     Standard_Real period = (ParametersArray.Value(ParametersArray.Upper()) -
  84                             ParametersArray.Value(ParametersArray.Lower()));
  85     point_array [0] = PointsArray.Value(PointsArray.Upper());
  86     point_array [1] = PointsArray.Value(PointsArray.Lower());
  87     point_array [2] = PointsArray.Value(PointsArray.Lower()+1);
  88     parameter_array[0] = ParametersArray.Value(ParametersArray.Upper() - 1) - period;
  89     parameter_array[1] = ParametersArray.Value(ParametersArray.Lower());
  90     parameter_array[2] = ParametersArray.Value(ParametersArray.Lower() + 1);
  91     TangentFlags.SetValue(1,Standard_True);
  92     PLib::EvalLagrange(parameter_array[1],
  93                        1,
  94                        degree,
  95                        1,
  96                        point_array[0],
  97                        parameter_array[0],
  98                        eval_result[0]);
  99     TangentsArray.SetValue(1,eval_result[1]);
 100   }
 101 }
 102
 103 //=======================================================================
 104 //function : BuildTangents
 105 //purpose  :
 106 //=======================================================================
 107
 108 static void BuildTangents(const TColStd_Array1OfReal&  PointsArray,
 109                           TColStd_Array1OfReal&        TangentsArray,
 110                           TColStd_Array1OfBoolean&     TangentFlags,
 111                           const TColStd_Array1OfReal&  ParametersArray)
 112 {
 113   Standard_Integer  degree = 3;//,ii;
 114   Standard_Real *point_array, *parameter_array, eval_result[2];
 115
 116   if ( PointsArray.Length() < 3) {
 117     Standard_ConstructionError::Raise();
 118   }
 119   if (PointsArray.Length() == 3) {
 120     degree = 2;
 121   }
 122   if (!TangentFlags.Value(1)) {
 123     point_array = (Standard_Real *) &PointsArray.Value(PointsArray.Lower());
 124     parameter_array = (Standard_Real *) &ParametersArray.Value(1);
 125     TangentFlags.SetValue(1,Standard_True);
 126     PLib::EvalLagrange(ParametersArray.Value(1),
 127                        1,
 128                        degree,
 129                        1,
 130                        point_array[0],
 131                        parameter_array[0],
 132                        eval_result[0]);
 133     TangentsArray.SetValue(1,eval_result[1]);
 134   }
 135   if (! TangentFlags.Value(TangentFlags.Upper())) {
 136     point_array =
 137       (Standard_Real *) &PointsArray.Value(PointsArray.Upper() - degree);
 138     TangentFlags.SetValue(TangentFlags.Upper(),Standard_True);
 139     Standard_Integer iup = ParametersArray.Upper() - degree;
 140     parameter_array = (Standard_Real *) &ParametersArray.Value(iup);
 141     PLib::EvalLagrange(ParametersArray.Value(ParametersArray.Upper()),
 142                        1,
 143                        degree,
 144                        1,
 145                        point_array[0],
 146                        parameter_array[0],
 147                        eval_result[0]);
 148     TangentsArray.SetValue(TangentsArray.Upper(),eval_result[1]);
 149   }
 150 }
 151
 152 //=======================================================================
 153 //function : Law_Interpolate
 154 //purpose  :
 155 //=======================================================================
 156
 157 Law_Interpolate::Law_Interpolate
 158 (const Handle(TColStd_HArray1OfReal)& PointsPtr,
 159  const Standard_Boolean               PeriodicFlag,
 160  const Standard_Real                  Tolerance) :
 161  myTolerance(Tolerance),
 162  myPoints(PointsPtr),
 163  myIsDone(Standard_False),
 164  myPeriodic(PeriodicFlag),
 165  myTangentRequest(Standard_False)
 166
 167 {
 168 //Standard_Integer ii;
 169   myTangents = new TColStd_HArray1OfReal (myPoints->Lower(),
 170                                           myPoints->Upper());
 171   myTangentFlags = new TColStd_HArray1OfBoolean(myPoints->Lower(),
 172                                                 myPoints->Upper());
 173
 174   BuildParameters(PeriodicFlag,
 175                   PointsPtr->Array1(),
 176                   myParameters);
 177   myTangentFlags->Init(Standard_False);
 178 }
 179
 180 //=======================================================================
 181 //function : Law_Interpolate
 182 //purpose  :
 183 //=======================================================================
 184
 185 Law_Interpolate::Law_Interpolate
 186 (const Handle(TColStd_HArray1OfReal)& PointsPtr,
 187  const Handle(TColStd_HArray1OfReal)& ParametersPtr,
 188  const Standard_Boolean               PeriodicFlag,
 189  const Standard_Real                  Tolerance) :
 190  myTolerance(Tolerance),
 191  myPoints(PointsPtr),
 192  myIsDone(Standard_False),
 193  myParameters(ParametersPtr),
 194  myPeriodic(PeriodicFlag),
 195  myTangentRequest(Standard_False)
 196 {
 197 //Standard_Integer ii;
 198   if (PeriodicFlag) {
 199     if ((PointsPtr->Length()) + 1 != ParametersPtr->Length()) {
 200       Standard_ConstructionError::Raise();
 201     }
 202   }
 203   myTangents = new TColStd_HArray1OfReal(myPoints->Lower(),
 204                                        myPoints->Upper());
 205   myTangentFlags = new TColStd_HArray1OfBoolean(myPoints->Lower(),
 206                                                 myPoints->Upper());
 207   Standard_Boolean result = CheckParameters(ParametersPtr->Array1());
 208   if (!result) {
 209     Standard_ConstructionError::Raise();
 210   }
 211   myTangentFlags->Init(Standard_False);
 212 }
 213
 214 //=======================================================================
 215 //function : Load
 216 //purpose  :
 217 //=======================================================================
 218
 219 void Law_Interpolate::Load
 220 (const TColStd_Array1OfReal&             Tangents,
 221  const Handle(TColStd_HArray1OfBoolean)& TangentFlagsPtr)
 222 {
 223 //Standard_Boolean result;
 224   Standard_Integer ii;
 225   myTangentRequest = Standard_True;
 226   myTangentFlags = TangentFlagsPtr;
 227   if (Tangents.Length() != myPoints->Length() ||
 228       TangentFlagsPtr->Length() != myPoints->Length()) {
 229     Standard_ConstructionError::Raise();
 230   }
 231   myTangents = new TColStd_HArray1OfReal(Tangents.Lower(),Tangents.Upper());
 232   for (ii = Tangents.Lower() ; ii <= Tangents.Upper() ; ii++ ) {
 233     myTangents->SetValue(ii,Tangents.Value(ii));
 234   }
 235 }
 236
 237 //=======================================================================
 238 //function : Load
 239 //purpose  :
 240 //=======================================================================
 241
 242 void Law_Interpolate::Load(const Standard_Real InitialTangent,
 243                            const Standard_Real FinalTangent)
 244 {
 245 //Standard_Boolean result;
 246   myTangentRequest = Standard_True;
 247   myTangentFlags->SetValue(1,Standard_True);
 248   myTangents->SetValue(1,InitialTangent);
 249   myTangentFlags->SetValue(myPoints->Length(),Standard_True);
 250   myTangents->SetValue(myPoints->Length(),FinalTangent) ;
 251 }
 252
 253 //=======================================================================
 254 //function : Perform
 255 //purpose  :
 256 //=======================================================================
 257
 258 void Law_Interpolate::Perform()
 259 {
 260   if (myPeriodic) {
 261     PerformPeriodic();
 262   }
 263   else {
 264     PerformNonPeriodic();
 265   }
 266 }
 267
 268 //=======================================================================
 269 //function : PerformPeriodic
 270 //purpose  :
 271 //=======================================================================
 272
 273 void Law_Interpolate::PerformPeriodic()
 274 {
 275   Standard_Integer degree,
 276   ii,
 277 //jj,
 278   index,
 279   index1,
 280 //index2,
 281   mult_index,
 282   half_order,
 283   inversion_problem,
 284   num_points,
 285   num_distinct_knots,
 286   num_poles;
 287
 288   Standard_Real period;
 289
 290 //gp_Pnt a_point;
 291
 292   num_points = myPoints->Length();
 293   period = myParameters->Value(myParameters->Upper()) -
 294     myParameters->Value(myParameters->Lower())  ;
 295   num_poles = num_points + 1 ;
 296   num_distinct_knots = num_points + 1;
 297   half_order = 2;
 298   degree = 3;
 299   num_poles += 2;
 300   if (myTangentRequest) {
 301     for (ii = myTangentFlags->Lower() + 1;
 302          ii <= myTangentFlags->Upper(); ii++) {
 303       if (myTangentFlags->Value(ii)) {
 304         num_poles += 1;
 305       }
 306     }
 307   }
 308   TColStd_Array1OfReal     parameters(1,num_poles);
 309   TColStd_Array1OfReal     flatknots(1,num_poles + degree + 1);
 310   TColStd_Array1OfInteger  mults(1,num_distinct_knots);
 311   TColStd_Array1OfInteger  contact_order_array(1, num_poles);
 312   TColStd_Array1OfReal     poles(1,num_poles);
 313
 314   for (ii = 1 ; ii <= half_order ; ii++) {
 315     flatknots.SetValue(ii,myParameters->Value(myParameters->Upper() -1) -
 316                        period);
 317     flatknots.SetValue(ii + half_order,myParameters->
 318                        Value(myParameters->Lower()));
 319     flatknots.SetValue(num_poles + ii,
 320                        myParameters->Value(myParameters->Upper()));
 321     flatknots.SetValue(num_poles + half_order + ii,
 322                        myParameters->Value(half_order) + period);
 323   }
 324   for (ii = 1 ; ii <= num_poles ; ii++) {
 325     contact_order_array.SetValue(ii,0) ;
 326   }
 327   for (ii = 2; ii < num_distinct_knots; ii++) {
 328     mults.SetValue(ii,1);
 329   }
 330   mults.SetValue(1,half_order);
 331   mults.SetValue(num_distinct_knots ,half_order);
 332
 333   BuildPeriodicTangent(myPoints->Array1(),
 334                        myTangents->ChangeArray1(),
 335                        myTangentFlags->ChangeArray1(),
 336                        myParameters->Array1());
 337
 338     contact_order_array.SetValue(2,1);
 339   parameters.SetValue(1,myParameters->Value(1));
 340   parameters.SetValue(2,myParameters->Value(1));
 341   poles.SetValue(1,myPoints->Value(1));
 342   poles.SetValue(2,myTangents->Value(1));
 343   mult_index = 2;
 344   index = 3;
 345   index1 = degree + 2;
 346   if (myTangentRequest) {
 347     for (ii = myTangentFlags->Lower() + 1;
 348          ii <= myTangentFlags->Upper(); ii++) {
 349       parameters.SetValue(index,myParameters->Value(ii));
 350       flatknots.SetValue(index1,myParameters->Value(ii));
 351       poles.SetValue(index,myPoints->Value(ii));
 352       index += 1;
 353       index1 += 1;
 354       if (myTangentFlags->Value(ii)) {
 355         mults.SetValue(mult_index,mults.Value(mult_index)  + 1);
 356         contact_order_array(index) = 1;
 357
 358         parameters.SetValue(index,
 359                             myParameters->Value(ii));
 360         flatknots.SetValue(index1,myParameters->Value(ii));
 361         poles.SetValue(index,myTangents->Value(ii));
 362         index  += 1;
 363         index1 += 1;
 364       }
 365       mult_index += 1;
 366     }
 367   }
 368   else {
 369     index = degree + 1;
 370     index1 = 2 ;
 371     for(ii = myParameters->Lower(); ii <= myParameters->Upper(); ii++) {
 372       parameters.SetValue(index1, myParameters->Value(ii));
 373       flatknots.SetValue(index, myParameters->Value(ii));
 374       index += 1;
 375       index1 += 1;
 376     }
 377     index = 3;
 378     for (ii = myPoints->Lower() + 1; ii <= myPoints->Upper(); ii++) {
 379       //
 380       // copy all the given points since the last one will be initialized
 381       // below by the first point in the array myPoints
 382       //
 383       poles.SetValue(index, myPoints->Value(ii));
 384       index += 1;
 385     }
 386   }
 387   contact_order_array.SetValue(num_poles - 1, 1);
 388   parameters.SetValue(num_poles-1,
 389                       myParameters->Value(myParameters->Upper()));
 390   //
 391   // for the periodic curve ONLY  the tangent of the first point
 392   // will be used since the curve should close itself at the first
 393   // point See BuildPeriodicTangent
 394   //
 395   poles.SetValue(num_poles-1,myTangents->Value(1));
 396   parameters.SetValue(num_poles,
 397                       myParameters->Value(myParameters->Upper()));
 398   poles.SetValue(num_poles,
 399                  myPoints->Value(1));
 400
 401   BSplCLib::Interpolate(degree,
 402                         flatknots,
 403                         parameters,
 404                         contact_order_array,
 405                         1,
 406                         poles(1),
 407                         inversion_problem);
 408   if (!inversion_problem) {
 409     TColStd_Array1OfReal  newpoles(poles.Value(1),
 410                                    1,
 411                                    num_poles - 2) ;
 412     myCurve =   new Law_BSpline(newpoles,
 413                                 myParameters->Array1(),
 414                                 mults,
 415                                 degree,
 416                                 myPeriodic);
 417     myIsDone = Standard_True;
 418   }
 419 }
 420
 421
 422 //=======================================================================
 423 //function : PerformNonPeriodic
 424 //purpose  :
 425 //=======================================================================
 426
 427 void Law_Interpolate::PerformNonPeriodic()
 428 {
 429   Standard_Integer degree,
 430   ii,
 431 //jj,
 432   index,
 433   index1,
 434   index2,
 435   index3,
 436   mult_index,
 437   inversion_problem,
 438   num_points,
 439   num_distinct_knots,
 440   num_poles;
 441
 442   num_points =
 443     num_distinct_knots =
 444       num_poles = myPoints->Length();
 445   if (num_poles == 2 &&   !myTangentRequest)  {
 446     degree = 1;
 447   }
 448   else if (num_poles == 3 && !myTangentRequest) {
 449     degree = 2;
 450     num_distinct_knots = 2;
 451   }
 452   else {
 453     degree = 3;
 454     num_poles += 2;
 455     if (myTangentRequest) {
 456       for (ii = myTangentFlags->Lower() + 1;
 457            ii < myTangentFlags->Upper(); ii++) {
 458         if (myTangentFlags->Value(ii)) {
 459           num_poles += 1;
 460         }
 461       }
 462     }
 463   }
 464   TColStd_Array1OfReal     parameters(1,num_poles) ;
 465   TColStd_Array1OfReal     flatknots(1,num_poles + degree + 1) ;
 466   TColStd_Array1OfInteger  mults(1,num_distinct_knots) ;
 467   TColStd_Array1OfReal     knots(1,num_distinct_knots) ;
 468   TColStd_Array1OfInteger  contact_order_array(1, num_poles) ;
 469   TColStd_Array1OfReal     poles(1,num_poles) ;
 470
 471   for (ii = 1 ; ii <= degree + 1 ; ii++) {
 472     flatknots.SetValue(ii,myParameters->Value(1));
 473     flatknots.SetValue(ii + num_poles,
 474                        myParameters->Value(num_points));
 475   }
 476   for (ii = 1 ; ii <= num_poles ; ii++) {
 477     contact_order_array.SetValue(ii,0);
 478   }
 479   for (ii = 2 ; ii < num_distinct_knots ; ii++) {
 480     mults.SetValue(ii,1);
 481   }
 482   mults.SetValue(1,degree + 1);
 483   mults.SetValue(num_distinct_knots ,degree + 1);
 484
 485   switch (degree) {
 486   case 1:
 487     for (ii = 1 ; ii <= num_poles ; ii++) {
 488       poles.SetValue(ii ,myPoints->Value(ii));
 489     }
 490     myCurve =
 491       new Law_BSpline(poles,
 492                       myParameters->Array1(),
 493                       mults,
 494                       degree) ;
 495     myIsDone = Standard_True ;
 496     break ;
 497   case 2:
 498     knots.SetValue(1,myParameters->Value(1)) ;
 499     knots.SetValue(2,myParameters->Value(num_poles)) ;
 500     for (ii = 1 ; ii <= num_poles ; ii++) {
 501       poles.SetValue(ii,myPoints->Value(ii)) ;
 502
 503     }
 504     BSplCLib::Interpolate(degree,
 505                           flatknots,
 506                           myParameters->Array1(),
 507                           contact_order_array,
 508                           1,
 509                           poles(1),
 510                           inversion_problem) ;
 511     if (!inversion_problem) {
 512       myCurve = new Law_BSpline(poles,
 513                                 knots,
 514                                 mults,
 515                                 degree);
 516       myIsDone = Standard_True;
 517     }
 518     break;
 519   case 3:
 520     //
 521     // check if the boundary conditions are set
 522     //
 523     if (num_points >= 3) {
 524       //
 525       // cannot build the tangents with degree 3 with only 2 points
 526       // if those where not given in advance
 527       //
 528       BuildTangents(myPoints->Array1(),
 529                     myTangents->ChangeArray1(),
 530                     myTangentFlags->ChangeArray1(),
 531                     myParameters->Array1()) ;
 532     }
 533     contact_order_array.SetValue(2,1);
 534     parameters.SetValue(1,myParameters->Value(1));
 535     parameters.SetValue(2,myParameters->Value(1));
 536     poles.SetValue(1,myPoints->Value(1));
 537     poles.SetValue(2,myTangents->Value(1));
 538     mult_index = 2 ;
 539     index = 3 ;
 540     index1 = 2 ;
 541     index2 = myPoints->Lower() + 1 ;
 542     index3 = degree + 2 ;
 543     if (myTangentRequest) {
 544       for (ii = myParameters->Lower() + 1 ;
 545            ii < myParameters->Upper() ; ii++) {
 546         parameters.SetValue(index,myParameters->Value(ii)) ;
 547         poles.SetValue(index,myPoints->Value(index2)) ;
 548         flatknots.SetValue(index3,myParameters->Value(ii)) ;
 549         index += 1 ;
 550         index3 += 1 ;
 551         if (myTangentFlags->Value(index1)) {
 552           //
 553           // set the multiplicities, the order of the contact, the
 554           // the flatknots,
 555           //
 556           mults.SetValue(mult_index,mults.Value(mult_index) + 1) ;
 557           contact_order_array(index) = 1 ;
 558           flatknots.SetValue(index3, myParameters->Value(ii)) ;
 559           parameters.SetValue(index,
 560                               myParameters->Value(ii));
 561           poles.SetValue(index,myTangents->Value(ii));
 562           index += 1  ;
 563           index3 += 1 ;
 564         }
 565         mult_index += 1 ;
 566         index1 += 1 ;
 567         index2 += 1 ;
 568       }
 569     }
 570     else {
 571       index1 = 2 ;
 572       for(ii = myParameters->Lower(); ii <= myParameters->Upper(); ii++) {
 573         parameters.SetValue(index1, myParameters->Value(ii));
 574         index1 += 1;
 575       }
 576       index = 3 ;
 577       for (ii = myPoints->Lower() + 1; ii <= myPoints->Upper() - 1; ii++){
 578         poles.SetValue(index, myPoints->Value(ii));
 579         index += 1 ;
 580       }
 581       index = degree + 1;
 582       for(ii = myParameters->Lower(); ii <= myParameters->Upper(); ii++){
 583         flatknots.SetValue(index, myParameters->Value(ii));
 584         index += 1 ;
 585       }
 586     }
 587     poles.SetValue(num_poles-1, myTangents->Value(num_points));
 588
 589     contact_order_array.SetValue(num_poles - 1, 1);
 590     parameters.SetValue(num_poles,
 591                         myParameters->Value(myParameters->Upper())) ;
 592     parameters.SetValue(num_poles -1,
 593                         myParameters->Value(myParameters->Upper())) ;
 594
 595     poles.SetValue(num_poles,
 596                    myPoints->Value(num_points)) ;
 597
 598     BSplCLib::Interpolate(degree,
 599                           flatknots,
 600                           parameters,
 601                           contact_order_array,
 602                           1,
 603                           poles(1),
 604                           inversion_problem) ;
 605     if (!inversion_problem) {
 606       myCurve = new Law_BSpline(poles,
 607                                 myParameters->Array1(),
 608                                 mults,
 609                                 degree) ;
 610       myIsDone = Standard_True;
 611     }
 612     break ;
 613   }
 614 }
 615
 616 //=======================================================================
 617 //function : Handle_Geom_BSplineCurve&
 618 //purpose  :
 619 //=======================================================================
 620
 621 const Handle(Law_BSpline)& Law_Interpolate::Curve() const
 622 {
 623   if ( !myIsDone)
 624     StdFail_NotDone::Raise(" ");
 625   return myCurve;
 626 }
 627
 628 //=======================================================================
 629 //function : IsDone
 630 //purpose  :
 631 //=======================================================================
 632
 633 Standard_Boolean Law_Interpolate::IsDone() const
 634 {
 635   return myIsDone;
 636 }