src/ApproxInt/ApproxInt_KnotTools.cxx

   1 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   2 //
   3 // This file is part of Open CASCADE Technology software library.
   4 //
   5 // This library is free software; you can redistribute it and/or modify it under
   6 // the terms of the GNU Lesser General Public License version 2.1 as published
   7 // by the Free Software Foundation, with special exception defined in the file
   8 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
   9 // distribution for complete text of the license and disclaimer of any warranty.
  10 //
  11 // Alternatively, this file may be used under the terms of Open CASCADE
  12 // commercial license or contractual agreement.
  13
  14 #include <ApproxInt_KnotTools.hxx>
  15 #include <TColgp_Array1OfPnt2d.hxx>
  16 #include <TColStd_Array1OfReal.hxx>
  17 #include <TColStd_HArray1OfReal.hxx>
  18 #include <TColStd_HArray1OfInteger.hxx>
  19 #include <math_Vector.hxx>
  20 #include <Geom_BSplineCurve.hxx>
  21 #include <Geom2d_BSplineCurve.hxx>
  22 #include <GeomInt_TheMultiLineOfWLApprox.hxx>
  23 #include <NCollection_Sequence.hxx>
  24 #include <NCollection_List.hxx>
  25 #include <PLib.hxx>
  26 #include <Precision.hxx>
  27 #include <NCollection_Vector.hxx>
  28 #include <TColgp_Array1OfPnt.hxx>
  29
  30 // (Sqrt(5.0) - 1.0) / 4.0
  31 //static const Standard_Real aSinCoeff = 0.30901699437494742410229341718282;
  32 static const Standard_Real aSinCoeff2 = 0.09549150281252627; // aSinCoeff^2 = (3. - Sqrt(5.)) / 8.
  33 static const Standard_Integer aMaxPntCoeff = 15;
  34
  35 //=======================================================================
  36 //function : EvalCurv
  37 //purpose  : Evaluate curvature in dim-dimension point.
  38 //=======================================================================
  39 static Standard_Real EvalCurv(const Standard_Real dim,
  40                               const Standard_Real* V1,
  41                               const Standard_Real* V2)
  42 {
  43   // Really V1 and V2 are arrays of size dim;
  44   // V1 is first derivative, V2 is second derivative
  45   // of n-dimension curve
  46   // Curvature is curv = |V1^V2|/|V1|^3
  47   // V1^V2 is outer product of two vectors:
  48   // P(i,j) = V1(i)*V2(j) - V1(j)*V2(i);
  49   Standard_Real mp = 0.;
  50   Standard_Integer i, j;
  51   Standard_Real p;
  52   for(i = 1; i < dim; ++i)
  53   {
  54     for(j = 0; j < i; ++j)
  55     {
  56       p = V1[i]*V2[j] - V1[j]*V2[i];
  57       mp += p*p;
  58     }
  59   }
  60   //
  61   Standard_Real q = 0.;
  62   for(i = 0; i < dim; ++i)
  63   {
  64     q += V1[i]*V1[i];
  65   }
  66
  67   if (q < 1 / Precision::Infinite())
  68   {
  69     // Indeed, if q is small then we can
  70     // obtain equivocation of "0/0" type.
  71     // In this case, local curvature can be
  72     // not equal to 0 or Infinity.
  73     // However, it is good solution to insert
  74     // knot in the place with such singularity.
  75     // Therefore, we need imitation of curvature
  76     // jumping. Return of Precision::Infinite() is
  77     // enough for it.
  78
  79     return Precision::Infinite();
  80   }
  81
  82   q = Min(q, Precision::Infinite());
  83   q *= q*q;
  84
  85   //
  86   Standard_Real curv = Sqrt(mp / q);
  87
  88   return curv;
  89 }
  90
  91 //=======================================================================
  92 //function : ComputeKnotInds
  93 //purpose  :
  94 //=======================================================================
  95 void ApproxInt_KnotTools::ComputeKnotInds(const NCollection_LocalArray<Standard_Real>& theCoords,
  96                                           const Standard_Integer theDim,
  97                                           const math_Vector& thePars,
  98                                           NCollection_Sequence<Standard_Integer>& theInds)
  99 {
 100   //I: Create discrete curvature.
 101   NCollection_Sequence<Standard_Integer> aFeatureInds;
 102   TColStd_Array1OfReal aCurv(thePars.Lower(), thePars.Upper());
 103   // Arrays are allocated for max theDim = 7: 1 3d curve + 2 2d curves.
 104   Standard_Real Val[21], Par[3], Res[21];
 105   Standard_Integer i, j, m, ic;
 106   Standard_Real aMaxCurv = 0.;
 107   Standard_Integer dim = theDim;
 108   //
 109   i = aCurv.Lower();
 110   for(j = 0; j < 3; ++j)
 111   {
 112     Standard_Integer k = i+j;
 113     ic = (k - aCurv.Lower()) * dim;
 114     Standard_Integer l = dim*j;
 115     for(m = 0; m < dim; ++m)
 116     {
 117       Val[l + m] = theCoords[ic + m];
 118     }
 119     Par[j] = thePars(k);
 120   }
 121   PLib::EvalLagrange(Par[0], 2, 2, dim, *Val, *Par, *Res);
 122   //
 123   aCurv(i) = EvalCurv(dim, &Res[dim], &Res[2*dim]);
 124   //
 125   if(aCurv(i) > aMaxCurv)
 126   {
 127     aMaxCurv = aCurv(i);
 128   }
 129   //
 130   for(i = aCurv.Lower()+1; i < aCurv.Upper(); ++i)
 131   {
 132     for(j = 0; j < 3; ++j)
 133     {
 134       Standard_Integer k = i+j-1;
 135       ic = (k - aCurv.Lower()) * dim;
 136       Standard_Integer l = dim*j;
 137       for(m = 0; m < dim; ++m)
 138       {
 139         Val[l + m] = theCoords[ic + m];
 140       }
 141       Par[j] = thePars(k);
 142     }
 143     PLib::EvalLagrange(Par[1], 2, 2, dim, *Val, *Par, *Res);
 144     //
 145     aCurv(i) = EvalCurv(dim, &Res[dim], &Res[2*dim]);
 146     if(aCurv(i) > aMaxCurv)
 147     {
 148       aMaxCurv = aCurv(i);
 149     }
 150   }
 151   //
 152   i = aCurv.Upper();
 153   for(j = 0; j < 3; ++j)
 154   {
 155     Standard_Integer k = i+j-2;
 156     ic = (k - aCurv.Lower()) * dim;
 157     Standard_Integer l = dim*j;
 158     for(m = 0; m < dim; ++m)
 159     {
 160       Val[l + m] = theCoords[ic + m];
 161     }
 162     Par[j] = thePars(k);
 163   }
 164   PLib::EvalLagrange(Par[2], 2, 2, dim, *Val, *Par, *Res);
 165   //
 166   aCurv(i) = EvalCurv(dim, &Res[dim], &Res[2*dim]);
 167   if(aCurv(i) > aMaxCurv)
 168   {
 169     aMaxCurv = aCurv(i);
 170   }
 171
 172 #ifdef APPROXINT_KNOTTOOLS_DEBUG
 173   cout << "Discrete curvature array is" << endl;
 174   for(i = aCurv.Lower(); i <= aCurv.Upper(); ++i)
 175   {
 176     cout << i << " " << aCurv(i) << endl;
 177   }
 178 #endif
 179
 180   theInds.Append(aCurv.Lower());
 181   if(aMaxCurv <= Precision::Confusion())
 182   {
 183     // Linear case.
 184     theInds.Append(aCurv.Upper());
 185     return;
 186   }
 187
 188   // II: Find extremas of curvature.
 189   // Not used Precision::PConfusion, by different from "param space" eps nature.
 190   Standard_Real eps  = 1.0e-9,
 191                 eps1 = 1.0e3 * eps;
 192   for(i = aCurv.Lower() + 1; i < aCurv.Upper(); ++i)
 193   {
 194     Standard_Real d1 = aCurv(i) - aCurv(i - 1),
 195                   d2 = aCurv(i) - aCurv(i + 1),
 196                   ad1 = Abs(d1), ad2 = Abs(d2);
 197
 198     if(d1*d2 > 0. && ad1 > eps && ad2 > eps)
 199     {
 200       if(i != theInds.Last())
 201       {
 202         theInds.Append(i);
 203         aFeatureInds.Append(i);
 204       }
 205     }
 206     else if((ad1 < eps && ad2 > eps1) || (ad1 > eps1 && ad2 < eps))
 207     {
 208       if(i != theInds.Last())
 209       {
 210         theInds.Append(i);
 211         aFeatureInds.Append(i);
 212       }
 213     }
 214   }
 215   if(aCurv.Upper() != theInds.Last())
 216   {
 217     theInds.Append(aCurv.Upper());
 218   }
 219
 220 #if defined(APPROXINT_KNOTTOOLS_DEBUG)
 221   {
 222     cout << "Feature indices new: " << endl;
 223     i;
 224     for(i = theInds.Lower(); i <= theInds.Upper();  ++i)
 225     {
 226       cout << i << " : " << theInds(i) << endl;
 227     }
 228   }
 229 #endif
 230
 231   //III: Put knots in monotone intervals of curvature.
 232   Standard_Boolean Ok;
 233   i = 1;
 234   do
 235   {
 236     i++;
 237     //
 238     Ok = InsKnotBefI(i, aCurv, theCoords, dim, theInds, Standard_True);
 239     if(Ok)
 240     {
 241       i--;
 242     }
 243   }
 244   while(i < theInds.Length());
 245
 246   //IV: Cheking feature points.
 247   j = 2;
 248   for(i = 1; i <= aFeatureInds.Length(); ++i)
 249   {
 250     Standard_Integer anInd = aFeatureInds(i);
 251     for(; j <= theInds.Length() - 1;)
 252     {
 253       if(theInds(j) == anInd)
 254       {
 255         Standard_Integer anIndPrev = theInds(j-1);
 256         Standard_Integer anIndNext = theInds(j+1);
 257         Standard_Integer ici = (anIndPrev - aCurv.Lower()) * theDim,
 258           ici1 = (anIndNext - aCurv.Lower()) * theDim,
 259           icm = (anInd - aCurv.Lower()) * theDim;
 260         NCollection_LocalArray<Standard_Real> V1(theDim), V2(theDim);
 261         Standard_Real mp = 0., m1 = 0., m2 = 0.;
 262         Standard_Real p;
 263         for(Standard_Integer k = 0; k < theDim; ++k)
 264         {
 265           V1[k] = theCoords[icm + k] - theCoords[ici + k];
 266           m1 += V1[k]*V1[k];
 267           V2[k] = theCoords[ici1 + k] - theCoords[icm + k];
 268           m2 += V2[k]*V2[k];
 269         }
 270         for(Standard_Integer k = 1; k < theDim; ++k)
 271         {
 272           for(Standard_Integer l = 0; l < k; ++l)
 273           {
 274             p = V1[k]*V2[l] - V1[l]*V2[k];
 275             mp += p*p;
 276           }
 277         }
 278         //mp *= 2.; //P(j,i) = -P(i,j);
 279         //
 280
 281         if(mp > aSinCoeff2 * m1 * m2) // Sqrt (mp/(m1*m2)) > aSinCoeff
 282         {
 283           //Insert new knots
 284           Standard_Real d1 = Abs(aCurv(anInd) - aCurv(anIndPrev));
 285           Standard_Real d2 = Abs(aCurv(anInd) - aCurv(anIndNext));
 286           if(d1 > d2)
 287           {
 288             Ok = InsKnotBefI(j, aCurv, theCoords, dim, theInds, Standard_False);
 289             if(Ok)
 290             {
 291               j++;
 292             }
 293             else
 294             {
 295               break;
 296             }
 297           }
 298           else
 299           {
 300             Ok = InsKnotBefI(j+1, aCurv, theCoords, dim, theInds, Standard_False);
 301             if(!Ok)
 302             {
 303               break;
 304             }
 305           }
 306         }
 307         else
 308         {
 309           j++;
 310           break;
 311         }
 312       }
 313       else
 314       {
 315         j++;
 316       }
 317     }
 318   }
 319   //
 320 }
 321
 322
 323 //=======================================================================
 324 //function : FilterKnots
 325 //purpose  :
 326 //=======================================================================
 327 void ApproxInt_KnotTools::FilterKnots(NCollection_Sequence<Standard_Integer>& theInds,
 328                                       const Standard_Integer theMinNbPnts,
 329                                       NCollection_Vector<Standard_Integer>& theLKnots)
 330 {
 331   // Maximum number of points per knot interval.
 332   Standard_Integer aMaxNbPnts = aMaxPntCoeff*theMinNbPnts;
 333   Standard_Integer i = 1;
 334   Standard_Integer aMinNbStep = theMinNbPnts / 2;
 335
 336   // I: Filter too big number of points per knot interval.
 337   while(i < theInds.Length())
 338   {
 339     Standard_Integer nbint = theInds(i + 1) - theInds(i) + 1;
 340     if(nbint <= aMaxNbPnts)
 341     {
 342       ++i;
 343       continue;
 344     }
 345     else
 346     {
 347       Standard_Integer ind = theInds(i) + nbint / 2;
 348       theInds.InsertAfter(i, ind);
 349     }
 350   }
 351
 352   // II: Filter poins with too small amount of points per knot interval.
 353   i = 1;
 354   theLKnots.Append(theInds(i));
 355   Standard_Integer anIndsPrev = theInds(i);
 356   for(i = 2; i <= theInds.Length(); ++i)
 357   {
 358     if(theInds(i) - anIndsPrev <= theMinNbPnts)
 359     {
 360       if (i != theInds.Length())
 361       {
 362         Standard_Integer anIdx = i + 1;
 363         for( ; anIdx <= theInds.Length(); ++anIdx)
 364         {
 365           if (theInds(anIdx) - anIndsPrev >= theMinNbPnts)
 366             break;
 367         }
 368         anIdx--;
 369
 370         Standard_Integer aMidIdx = (theInds(anIdx) + anIndsPrev) / 2;
 371         if (aMidIdx - anIndsPrev        < theMinNbPnts &&
 372             aMidIdx - theInds(anIdx)    < theMinNbPnts &&
 373             theInds(anIdx) - anIndsPrev >= aMinNbStep)
 374         {
 375           if (theInds(anIdx) - anIndsPrev > 2 * theMinNbPnts)
 376           {
 377             // Bad distribution points merge into one knot interval.
 378             theLKnots.Append(anIndsPrev + theMinNbPnts);
 379             anIndsPrev = anIndsPrev + theMinNbPnts;
 380             i = anIdx - 1;
 381           }
 382           else
 383           {
 384             if (theInds(anIdx - 1) - anIndsPrev >= theMinNbPnts / 2)
 385             {
 386               // Bad distribution points merge into one knot interval.
 387               theLKnots.Append(theInds(anIdx - 1));
 388               anIndsPrev = theInds(anIdx - 1);
 389               i = anIdx - 1;
 390               if (theInds(anIdx) - theInds(anIdx - 1) <= theMinNbPnts / 2)
 391               {
 392                 theLKnots.SetValue(theLKnots.Upper(), theInds(anIdx));
 393                 anIndsPrev = theInds(anIdx );
 394                 i = anIdx;
 395               }
 396             }
 397             else
 398             {
 399               // Bad distribution points merge into one knot interval.
 400               theLKnots.Append(theInds(anIdx));
 401               anIndsPrev = theInds(anIdx);
 402               i = anIdx;
 403             }
 404           }
 405         }
 406         else if (anIdx == theInds.Upper() && // Last point obtained.
 407                  theLKnots.Length() >= 2) // It is possible to modify last item.
 408         {
 409           // Current bad interval from i to last.
 410           // Trying to add knot to divide sequence on two parts:
 411           // Last good index -> Last index - theMinNbPnts -> Last index
 412           Standard_Integer aLastGoodIdx = theLKnots.Value(theLKnots.Upper() - 1);
 413           if ( theInds.Last() - 2 * theMinNbPnts >= aLastGoodIdx)
 414           {
 415             theLKnots(theLKnots.Upper()) = theInds.Last() - theMinNbPnts;
 416             theLKnots.Append(theInds.Last());
 417             anIndsPrev = theInds(anIdx);
 418             i = anIdx;
 419           }
 420         }
 421       } // if (i != theInds.Length())
 422       continue;
 423     }
 424     else
 425     {
 426       theLKnots.Append(theInds(i));
 427       anIndsPrev = theInds(i);
 428     }
 429   }
 430
 431   // III: Fill Last Knot.
 432   if(theLKnots.Length() < 2)
 433   {
 434     theLKnots.Append(theInds.Last());
 435   }
 436   else
 437   {
 438     if(theLKnots.Last() < theInds.Last())
 439     {
 440       theLKnots(theLKnots.Upper()) = theInds.Last();
 441     }
 442   }
 443 }
 444 //=======================================================================
 445 //function : InsKnotBefI
 446 //purpose  :
 447 //=======================================================================
 448 Standard_Boolean ApproxInt_KnotTools::InsKnotBefI(const Standard_Integer theI,
 449                                                   const TColStd_Array1OfReal& theCurv,
 450                                                   const NCollection_LocalArray<Standard_Real>& theCoords,
 451                                                   const Standard_Integer theDim,
 452                                                   NCollection_Sequence<Standard_Integer>& theInds,
 453                                                   const Standard_Boolean ChkCurv)
 454 {
 455   Standard_Integer anInd1 = theInds(theI);
 456   Standard_Integer anInd = theInds(theI - 1);
 457   //
 458   if((anInd1-anInd) == 1)
 459   {
 460     return Standard_False;
 461   }
 462   //
 463   Standard_Real curv = 0.5*(theCurv(anInd) + theCurv(anInd1));
 464   Standard_Integer mid = 0, j, jj;
 465   const Standard_Real aLimitCurvatureChange = 3.0;
 466   for(j = anInd+1; j < anInd1; ++j)
 467   {
 468     mid = 0;
 469
 470     // I: Curvature change criteria:
 471     // Non-null curvature.
 472     if (theCurv(j)     > Precision::Confusion() &&
 473         theCurv(anInd) > Precision::Confusion() )
 474     {
 475       if (theCurv(j) / theCurv(anInd) > aLimitCurvatureChange ||
 476           theCurv(j) / theCurv(anInd) < 1.0 / aLimitCurvatureChange)
 477       {
 478         // Curvature on current interval changed more than 3 times.
 479         mid = j;
 480         theInds.InsertBefore(theI, mid);
 481         return Standard_True;
 482       }
 483     }
 484
 485     // II: Angular criteria:
 486     Standard_Real ac = theCurv(j - 1), ac1 = theCurv(j);
 487     if((curv >= ac && curv <= ac1) || (curv >= ac1 && curv <= ac))
 488     {
 489       if(Abs(curv - ac) < Abs(curv - ac1))
 490       {
 491         mid = j - 1;
 492       }
 493       else
 494       {
 495         mid = j;
 496       }
 497     }
 498     if(mid == anInd)
 499     {
 500       mid++;
 501     }
 502     if(mid == anInd1)
 503     {
 504       mid--;
 505     }
 506     if(mid > 0)
 507     {
 508       if(ChkCurv)
 509       {
 510         Standard_Integer ici = (anInd - theCurv.Lower()) * theDim,
 511           ici1 = (anInd1 - theCurv.Lower()) * theDim,
 512           icm = (mid - theCurv.Lower()) * theDim;
 513         NCollection_LocalArray<Standard_Real> V1(theDim), V2(theDim);
 514         Standard_Integer i;
 515         Standard_Real mp = 0., m1 = 0., m2 = 0.;
 516         Standard_Real p;
 517         for(i = 0; i < theDim; ++i)
 518         {
 519           V1[i] = theCoords[icm + i] - theCoords[ici + i];
 520           m1 += V1[i]*V1[i];
 521           V2[i] = theCoords[ici1 + i] - theCoords[icm + i];
 522           m2 += V2[i]*V2[i];
 523         }
 524         for(i = 1; i < theDim; ++i)
 525         {
 526           for(jj = 0; jj < i; ++jj)
 527           {
 528             p = V1[i]*V2[jj] - V1[jj]*V2[i];
 529             mp += p*p;
 530           }
 531         }
 532         //mp *= 2.; //P(j,i) = -P(i,j);
 533         //
 534
 535         if (mp > aSinCoeff2 * m1 * m2) // Sqrt (mp / m1m2) > aSinCoeff
 536         {
 537           theInds.InsertBefore(theI, mid);
 538           return Standard_True;
 539         }
 540       }
 541       else
 542       {
 543         theInds.InsertBefore(theI, mid);
 544         return Standard_True;
 545       }
 546     }
 547   }
 548
 549   return Standard_False;
 550 }
 551
 552 //=======================================================================
 553 //function : BuildKnots
 554 //purpose  :
 555 //=======================================================================
 556 void ApproxInt_KnotTools::BuildKnots(const TColgp_Array1OfPnt& thePntsXYZ,
 557                                      const TColgp_Array1OfPnt2d& thePntsU1V1,
 558                                      const TColgp_Array1OfPnt2d& thePntsU2V2,
 559                                      const math_Vector& thePars,
 560                                      const Standard_Boolean theApproxXYZ,
 561                                      const Standard_Boolean theApproxU1V1,
 562                                      const Standard_Boolean theApproxU2V2,
 563                                      const Standard_Integer theMinNbPnts,
 564                                      NCollection_Vector<Standard_Integer>& theKnots)
 565 {
 566   NCollection_Sequence<Standard_Integer> aKnots;
 567   Standard_Integer aDim = 0;
 568
 569   // I: Convert input data to the corresponding format.
 570   if(theApproxXYZ)
 571     aDim += 3;
 572   if(theApproxU1V1)
 573     aDim += 2;
 574   if(theApproxU2V2)
 575     aDim += 2;
 576
 577   NCollection_LocalArray<Standard_Real> aCoords(thePars.Length()*aDim);
 578   Standard_Integer i, j;
 579   for(i = thePars.Lower(); i <= thePars.Upper(); ++i)
 580   {
 581     j = (i - thePars.Lower()) * aDim;
 582     if(theApproxXYZ)
 583     {
 584       aCoords[j] = thePntsXYZ.Value(i).X();
 585       ++j;
 586       aCoords[j] = thePntsXYZ.Value(i).Y();
 587       ++j;
 588       aCoords[j] = thePntsXYZ.Value(i).Z();
 589       ++j;
 590     }
 591     if(theApproxU1V1)
 592     {
 593       aCoords[j] = thePntsU1V1.Value(i).X();
 594       ++j;
 595       aCoords[j] = thePntsU1V1.Value(i).Y();
 596       ++j;
 597     }
 598     if(theApproxU2V2)
 599     {
 600       aCoords[j] = thePntsU2V2.Value(i).X();
 601       ++j;
 602       aCoords[j] = thePntsU2V2.Value(i).Y();
 603       ++j;
 604     }
 605   }
 606
 607   // II: Build draft knot sequence.
 608   ComputeKnotInds(aCoords, aDim, thePars, aKnots);
 609
 610 #if defined(APPROXINT_KNOTTOOLS_DEBUG)
 611     cout << "Draft knot sequence: " << endl;
 612     for(i = aKnots.Lower(); i <= aKnots.Upper();  ++i)
 613     {
 614       cout << i << " : " << aKnots(i) << endl;
 615     }
 616 #endif
 617
 618   // III: Build output knot sequence.
 619   FilterKnots(aKnots, theMinNbPnts, theKnots);
 620
 621 #if defined(APPROXINT_KNOTTOOLS_DEBUG)
 622     cout << "Result knot sequence: " << endl;
 623     for(i = theKnots.Lower(); i <= theKnots.Upper();  ++i)
 624     {
 625       cout << i << " : " << theKnots(i) << endl;
 626     }
 627 #endif
 628
 629 }