0026431: Can't cut a sphere from a cylinder
[occt.git] / 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   }
199   if(aCurv.Upper() != theInds.Last())
200   {
201     theInds.Append(aCurv.Upper());
202   }
203
204 #if defined(APPROXINT_KNOTTOOLS_DEBUG) || defined(OCCT_DEBUG)
205   {
206     cout << "Feature indices new: " << endl;
207     i;
208     for(i = theInds.Lower(); i <= theInds.Upper();  ++i)
209     {
210       cout << i << " : " << theInds(i) << endl;
211     }
212   }
213 #endif
214
215   //III: Put knots in monotone intervals of curvature.
216   Standard_Boolean Ok;
217   i = 1;
218   do
219   {
220     i++;
221     //
222     Ok = InsKnotBefI(i, aCurv, theCoords, dim, theInds, Standard_True); 
223     if(Ok)
224     {
225       i--;
226     }
227   }
228   while(i < theInds.Length());
229
230   //IV: Cheking feature points.
231   j = 2;
232   for(i = 1; i <= aFeatureInds.Length(); ++i)
233   {
234     Standard_Integer anInd = aFeatureInds(i);
235     for(; j <= theInds.Length() - 1;)
236     {
237       if(theInds(j) == anInd)
238       {
239         Standard_Integer anIndPrev = theInds(j-1);
240         Standard_Integer anIndNext = theInds(j+1);
241         Standard_Real sina;
242         Standard_Integer ici = (anIndPrev - aCurv.Lower()) * theDim,
243           ici1 = (anIndNext - aCurv.Lower()) * theDim,
244           icm = (anInd - aCurv.Lower()) * theDim;
245         NCollection_LocalArray<Standard_Real> V1(theDim), V2(theDim);
246         Standard_Integer k,l;
247         Standard_Real mp = 0., m1 = 0., m2 = 0.;
248         Standard_Real p;
249         for(k = 0; k < theDim; ++k)
250         {
251           V1[k] = theCoords[icm + k] - theCoords[ici + k];
252           m1 += V1[k]*V1[k];
253           V2[k] = theCoords[ici1 + k] - theCoords[icm + k];
254           m2 += V2[k]*V2[k];
255         }
256         for(k = 1; k < theDim; ++k)
257         {
258           for(l = 0; l < k; ++l)
259           {
260             p = V1[k]*V2[l] - V1[l]*V2[k];
261             mp += p*p;
262           }
263         }
264         //mp *= 2.; //P(j,i) = -P(i,j);
265         //
266         sina = mp/(m1*m2);
267         sina = Sqrt(sina);
268
269         if(sina  > aSinCoeff) 
270         {
271           //Insert new knots
272           Standard_Real d1 = Abs(aCurv(anInd) - aCurv(anIndPrev));
273           Standard_Real d2 = Abs(aCurv(anInd) - aCurv(anIndNext));
274           if(d1 > d2)
275           {
276             Ok = InsKnotBefI(j, aCurv, theCoords, dim, theInds, Standard_False);
277             if(Ok)
278             {
279               j++;
280             }
281             else
282             {
283               break;
284             }
285           }
286           else
287           {
288             Ok = InsKnotBefI(j+1, aCurv, theCoords, dim, theInds, Standard_False);
289             if(!Ok)
290             {
291               break;
292             }
293           }
294         }
295         else
296         {
297           j++;
298           break;
299         }
300       }
301       else
302       {
303         j++;
304       }
305     }
306   }
307   //
308 }
309
310
311 //=======================================================================
312 //function : FilterKnots
313 //purpose  :
314 //=======================================================================
315 void ApproxInt_KnotTools::FilterKnots(NCollection_Sequence<Standard_Integer>& theInds, 
316                                       const Standard_Integer theMinNbPnts,
317                                       NCollection_Vector<Standard_Integer>& theLKnots)
318 {
319   // Maximum number of points per knot interval.
320   Standard_Integer aMaxNbPnts = aMaxPntCoeff*theMinNbPnts;
321   Standard_Integer i = 1;
322   Standard_Integer aMinNbStep = theMinNbPnts / 2;
323
324   // I: Filter too big number of points per knot interval.
325   while(i < theInds.Length())
326   {
327     Standard_Integer nbint = theInds(i + 1) - theInds(i) + 1;
328     if(nbint <= aMaxNbPnts)
329     {
330       ++i;
331       continue;
332     }
333     else
334     {
335       Standard_Integer ind = theInds(i) + nbint / 2;
336       theInds.InsertAfter(i, ind);     
337     }
338   }
339
340   // II: Filter poins with too small amount of points per knot interval.
341   i = 1;
342   theLKnots.Append(theInds(i));
343   Standard_Integer anIndsPrev = theInds(i);
344   for(i = 2; i <= theInds.Length(); ++i)
345   {
346     if(theInds(i) - anIndsPrev <= theMinNbPnts)
347     {
348       if (i != theInds.Length())
349       {
350         Standard_Integer anIdx = i + 1;
351         for( ; anIdx <= theInds.Length(); ++anIdx)
352         {
353           if (theInds(anIdx) - anIndsPrev >= theMinNbPnts)
354             break;
355         }
356         anIdx--;
357
358         Standard_Integer aMidIdx = (theInds(anIdx) + anIndsPrev) / 2;
359         if (aMidIdx - anIndsPrev        < theMinNbPnts &&
360             aMidIdx - theInds(anIdx)    < theMinNbPnts &&
361             theInds(anIdx) - anIndsPrev >= aMinNbStep)
362         {
363           if (theInds(anIdx) - anIndsPrev > 2 * theMinNbPnts)
364           {
365             // Bad distribution points merge into one knot interval.
366             theLKnots.Append(anIndsPrev + theMinNbPnts);
367             anIndsPrev = anIndsPrev + theMinNbPnts;
368             i = anIdx - 1;
369           }
370           else
371           {
372             if (theInds(anIdx - 1) - anIndsPrev >= theMinNbPnts / 2)
373             {
374               // Bad distribution points merge into one knot interval.
375               theLKnots.Append(theInds(anIdx - 1));
376               anIndsPrev = theInds(anIdx - 1);
377               i = anIdx - 1;
378               if (theInds(anIdx) - theInds(anIdx - 1) <= theMinNbPnts / 2)
379               {
380                 theLKnots.SetValue(theLKnots.Upper(), theInds(anIdx));
381                 anIndsPrev = theInds(anIdx );
382                 i = anIdx;
383               }
384             }
385             else
386             {
387               // Bad distribution points merge into one knot interval.
388               theLKnots.Append(theInds(anIdx));
389               anIndsPrev = theInds(anIdx);
390               i = anIdx;
391             }
392           }
393         }
394         else if (anIdx == theInds.Upper() && // Last point obtained.
395                  theLKnots.Length() >= 2) // It is possible to modify last item.
396         {
397           // Current bad interval from i to last.
398           // Trying to add knot to divide sequence on two parts:
399           // Last good index -> Last index - theMinNbPnts -> Last index
400           Standard_Integer aLastGoodIdx = theLKnots.Value(theLKnots.Upper() - 1);
401           if ( theInds.Last() - 2 * theMinNbPnts >= aLastGoodIdx)
402           {
403             theLKnots(theLKnots.Upper()) = theInds.Last() - theMinNbPnts;
404             theLKnots.Append(theInds.Last());
405             anIndsPrev = theInds(anIdx);
406             i = anIdx;
407           }
408         }
409       } // if (i != theInds.Length())
410       continue;
411     }
412     else
413     {
414       theLKnots.Append(theInds(i));
415       anIndsPrev = theInds(i);
416     }
417   }
418
419   // III: Fill Last Knot.
420   if(theLKnots.Length() < 2)
421   {
422     theLKnots.Append(theInds.Last());
423   }
424   else
425   {
426     if(theLKnots.Last() < theInds.Last())
427     {
428       theLKnots(theLKnots.Upper()) = theInds.Last();
429     }
430   }
431 }
432 //=======================================================================
433 //function : InsKnotBefI
434 //purpose  :
435 //=======================================================================
436 Standard_Boolean ApproxInt_KnotTools::InsKnotBefI(const Standard_Integer theI,
437                                                   const TColStd_Array1OfReal& theCurv,
438                                                   const NCollection_LocalArray<Standard_Real>& theCoords,
439                                                   const Standard_Integer theDim, 
440                                                   NCollection_Sequence<Standard_Integer>& theInds,
441                                                   const Standard_Boolean ChkCurv)
442 {
443   Standard_Integer anInd1 = theInds(theI);
444   Standard_Integer anInd = theInds(theI - 1);
445   //
446   if((anInd1-anInd) == 1)
447   {
448     return Standard_False;
449   }
450   //
451   Standard_Real curv = 0.5*(theCurv(anInd) + theCurv(anInd1));
452   Standard_Integer mid = 0, j, jj;
453   const Standard_Real aLimitCurvatureChange = 3.0;
454   for(j = anInd+1; j < anInd1; ++j)
455   {
456     mid = 0;
457
458     // I: Curvature change criteria:
459     // Non-null curvature.
460     if (theCurv(j)     > Precision::Confusion() && 
461         theCurv(anInd) > Precision::Confusion() )
462     {
463       if (theCurv(j) / theCurv(anInd) > aLimitCurvatureChange || 
464           theCurv(j) / theCurv(anInd) < 1.0 / aLimitCurvatureChange)
465       {
466         // Curvature on current interval changed more than 3 times.
467         mid = j;
468         theInds.InsertBefore(theI, mid);
469         return Standard_True;
470       }
471     }
472
473     // II: Angular criteria:
474     Standard_Real ac = theCurv(j - 1), ac1 = theCurv(j);
475     if((curv >= ac && curv <= ac1) || (curv >= ac1 && curv <= ac))
476     {
477       if(Abs(curv - ac) < Abs(curv - ac1))
478       {
479         mid = j - 1;
480       }
481       else
482       {
483         mid = j;
484       }
485     }
486     if(mid == anInd)
487     {
488       mid++;
489     }
490     if(mid == anInd1)
491     {
492       mid--;
493     }
494     if(mid > 0)
495     {
496       if(ChkCurv)
497       {
498         Standard_Real sina;
499         Standard_Integer ici = (anInd - theCurv.Lower()) * theDim,
500           ici1 = (anInd1 - theCurv.Lower()) * theDim,
501           icm = (mid - theCurv.Lower()) * theDim;
502         NCollection_LocalArray<Standard_Real> V1(theDim), V2(theDim);
503         Standard_Integer i;
504         Standard_Real mp = 0., m1 = 0., m2 = 0.;
505         Standard_Real p;
506         for(i = 0; i < theDim; ++i)
507         {
508           V1[i] = theCoords[icm + i] - theCoords[ici + i];
509           m1 += V1[i]*V1[i];
510           V2[i] = theCoords[ici1 + i] - theCoords[icm + i];
511           m2 += V2[i]*V2[i];
512         }
513         for(i = 1; i < theDim; ++i)
514         {
515           for(jj = 0; jj < i; ++jj)
516           {
517             p = V1[i]*V2[jj] - V1[jj]*V2[i];
518             mp += p*p;
519           }
520         }
521         //mp *= 2.; //P(j,i) = -P(i,j);
522         //
523         sina = mp/(m1*m2);
524         sina = Sqrt(sina);
525
526         if(sina > aSinCoeff)
527         {
528           theInds.InsertBefore(theI, mid);
529           return Standard_True;
530         }
531       }
532       else
533       {
534         theInds.InsertBefore(theI, mid);
535         return Standard_True;
536       }
537     }
538   }
539
540   return Standard_False;
541 }
542
543 //=======================================================================
544 //function : BuildKnots
545 //purpose  :
546 //=======================================================================
547 void ApproxInt_KnotTools::BuildKnots(const TColgp_Array1OfPnt& thePntsXYZ,
548                                      const TColgp_Array1OfPnt2d& thePntsU1V1,
549                                      const TColgp_Array1OfPnt2d& thePntsU2V2,
550                                      const math_Vector& thePars,
551                                      const Standard_Boolean theApproxXYZ,
552                                      const Standard_Boolean theApproxU1V1,
553                                      const Standard_Boolean theApproxU2V2,
554                                      const Standard_Integer theMinNbPnts,
555                                      NCollection_Vector<Standard_Integer>& theKnots)
556 {
557   NCollection_Sequence<Standard_Integer> aKnots;
558   Standard_Integer aDim = 0;
559
560   // I: Convert input data to the corresponding format.
561   if(theApproxXYZ)
562     aDim += 3;
563   if(theApproxU1V1)
564     aDim += 2;
565   if(theApproxU2V2)
566     aDim += 2;
567
568   NCollection_LocalArray<Standard_Real> aCoords(thePars.Length()*aDim);
569   Standard_Integer i, j;
570   for(i = thePars.Lower(); i <= thePars.Upper(); ++i)
571   {
572     j = (i - thePars.Lower()) * aDim;
573     if(theApproxXYZ)
574     {
575       aCoords[j] = thePntsXYZ.Value(i).X();
576       ++j;
577       aCoords[j] = thePntsXYZ.Value(i).Y();
578       ++j;
579       aCoords[j] = thePntsXYZ.Value(i).Z();
580       ++j;
581     }
582     if(theApproxU1V1)
583     {
584       aCoords[j] = thePntsU1V1.Value(i).X();
585       ++j;
586       aCoords[j] = thePntsU1V1.Value(i).Y();
587       ++j;
588     }
589     if(theApproxU2V2)
590     {
591       aCoords[j] = thePntsU2V2.Value(i).X();
592       ++j;
593       aCoords[j] = thePntsU2V2.Value(i).Y();
594       ++j;
595     }
596   }
597
598   // II: Build draft knot sequence.
599   ComputeKnotInds(aCoords, aDim, thePars, aKnots);
600
601 #if defined(APPROXINT_KNOTTOOLS_DEBUG) || defined(OCCT_DEBUG)
602     cout << "Draft knot sequence: " << endl;
603     for(i = aKnots.Lower(); i <= aKnots.Upper();  ++i)
604     {
605       cout << i << " : " << aKnots(i) << endl;
606     }
607 #endif
608
609   // III: Build output knot sequence.
610   FilterKnots(aKnots, theMinNbPnts, theKnots);
611
612 #if defined(APPROXINT_KNOTTOOLS_DEBUG) || defined(OCCT_DEBUG)
613     cout << "Result knot sequence: " << endl;
614     for(i = theKnots.Lower(); i <= theKnots.Upper();  ++i)
615     {
616       cout << i << " : " << theKnots(i) << endl;
617     }
618 #endif
619
620 }