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