0022048: Visualization, AIS_InteractiveContext - single object selection should alway...
[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_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 }