[occt.git] / src / ApproxInt / ApproxInt_KnotTools.cxx

// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <ApproxInt_KnotTools.hxx>
#include <TColgp_Array1OfPnt2d.hxx>
#include <TColStd_Array1OfReal.hxx>
#include <TColStd_HArray1OfReal.hxx>
#include <TColStd_HArray1OfInteger.hxx>
#include <math_Vector.hxx>
#include <Geom_BSplineCurve.hxx>
#include <Geom2d_BSplineCurve.hxx>
#include <GeomInt_TheMultiLineOfWLApprox.hxx>
#include <NCollection_Sequence.hxx>
#include <NCollection_List.hxx>
#include <PLib.hxx>
#include <Precision.hxx>
#include <NCollection_Vector.hxx>
#include <TColgp_Array1OfPnt.hxx>

// (Sqrt(5.0) - 1.0) / 4.0
//static const Standard_Real aSinCoeff = 0.30901699437494742410229341718282;
static const Standard_Real aSinCoeff2 = 0.09549150281252627; // aSinCoeff^2 = (3. - Sqrt(5.)) / 8.
static const Standard_Integer aMaxPntCoeff = 15;

//=======================================================================
//function : EvalCurv
//purpose  : Evaluate curvature in dim-dimension point.
//=======================================================================
static Standard_Real EvalCurv(const Standard_Real dim, 
                              const Standard_Real* V1,
                              const Standard_Real* V2)
{
  // Really V1 and V2 are arrays of size dim;
  // V1 is first derivative, V2 is second derivative
  // of n-dimension curve
  // Curvature is curv = |V1^V2|/|V1|^3
  // V1^V2 is outer product of two vectors:
  // P(i,j) = V1(i)*V2(j) - V1(j)*V2(i);
  Standard_Real mp = 0.;
  Standard_Integer i, j;
  Standard_Real p;
  for(i = 1; i < dim; ++i)
  {
    for(j = 0; j < i; ++j)
    {
      p = V1[i]*V2[j] - V1[j]*V2[i];
      mp += p*p;
    }
  }
  //
  Standard_Real q = 0.;
  for(i = 0; i < dim; ++i)
  {
    q += V1[i]*V1[i];
  }

  if (q < 1 / Precision::Infinite())
  {
    // Indeed, if q is small then we can
    // obtain equivocation of "0/0" type.
    // In this case, local curvature can be
    // not equal to 0 or Infinity.
    // However, it is good solution to insert
    // knot in the place with such singularity.
    // Therefore, we need imitation of curvature
    // jumping. Return of Precision::Infinite() is
    // enough for it.

    return Precision::Infinite();
  }

  q = Min(q, Precision::Infinite());
  q *= q*q;

  //
  Standard_Real curv = Sqrt(mp / q);

  return curv;
}

//=======================================================================
//function : ComputeKnotInds
//purpose  : 
//=======================================================================
void ApproxInt_KnotTools::ComputeKnotInds(const NCollection_LocalArray<Standard_Real>& theCoords,
                                          const Standard_Integer theDim, 
                                          const math_Vector& thePars,
                                          NCollection_Sequence<Standard_Integer>& theInds)
{
  //I: Create discrete curvature.
  NCollection_Sequence<Standard_Integer> aFeatureInds;
  TColStd_Array1OfReal aCurv(thePars.Lower(), thePars.Upper());
  // Arrays are allocated for max theDim = 7: 1 3d curve + 2 2d curves.
  Standard_Real Val[21], Par[3], Res[21];
  Standard_Integer i, j, m, ic;
  Standard_Real aMaxCurv = 0.;
  Standard_Integer dim = theDim;
  //
  i = aCurv.Lower();
  for(j = 0; j < 3; ++j)
  {
    Standard_Integer k = i+j;
    ic = (k - aCurv.Lower()) * dim;
    Standard_Integer l = dim*j;
    for(m = 0; m < dim; ++m)
    {
      Val[l + m] = theCoords[ic + m];
    }
    Par[j] = thePars(k);
  }
  PLib::EvalLagrange(Par[0], 2, 2, dim, *Val, *Par, *Res);
  //
  aCurv(i) = EvalCurv(dim, &Res[dim], &Res[2*dim]);
  //
  if(aCurv(i) > aMaxCurv)
  {
    aMaxCurv = aCurv(i);
  }
  //
  for(i = aCurv.Lower()+1; i < aCurv.Upper(); ++i)
  {
    for(j = 0; j < 3; ++j)
    {
      Standard_Integer k = i+j-1;
      ic = (k - aCurv.Lower()) * dim;
      Standard_Integer l = dim*j;
      for(m = 0; m < dim; ++m)
      {
        Val[l + m] = theCoords[ic + m];
      }
      Par[j] = thePars(k);
    }
    PLib::EvalLagrange(Par[1], 2, 2, dim, *Val, *Par, *Res);
    //
    aCurv(i) = EvalCurv(dim, &Res[dim], &Res[2*dim]);
    if(aCurv(i) > aMaxCurv)
    {
      aMaxCurv = aCurv(i);
    }
  }
  //
  i = aCurv.Upper();
  for(j = 0; j < 3; ++j)
  {
    Standard_Integer k = i+j-2;
    ic = (k - aCurv.Lower()) * dim;
    Standard_Integer l = dim*j;
    for(m = 0; m < dim; ++m)
    {
      Val[l + m] = theCoords[ic + m];
    }
    Par[j] = thePars(k);
  }
  PLib::EvalLagrange(Par[2], 2, 2, dim, *Val, *Par, *Res);
  //
  aCurv(i) = EvalCurv(dim, &Res[dim], &Res[2*dim]);
  if(aCurv(i) > aMaxCurv)
  {
    aMaxCurv = aCurv(i);
  }

#ifdef APPROXINT_KNOTTOOLS_DEBUG
  std::cout << "Discrete curvature array is" << std::endl;
  for(i = aCurv.Lower(); i <= aCurv.Upper(); ++i)
  {
    std::cout << i << " " << aCurv(i) << std::endl;
  }
#endif

  theInds.Append(aCurv.Lower());
  if(aMaxCurv <= Precision::Confusion())
  {
    // Linear case.
    theInds.Append(aCurv.Upper());
    return;
  }

  // II: Find extremas of curvature.
  // Not used Precision::PConfusion, by different from "param space" eps nature.
  Standard_Real eps  = 1.0e-9,
                eps1 = 1.0e3 * eps;
  for(i = aCurv.Lower() + 1; i < aCurv.Upper(); ++i)
  {
    Standard_Real d1 = aCurv(i) - aCurv(i - 1),
                  d2 = aCurv(i) - aCurv(i + 1),
                  ad1 = Abs(d1), ad2 = Abs(d2);

    if(d1*d2 > 0. && ad1 > eps && ad2 > eps)
    {
      if(i != theInds.Last())
      {
        theInds.Append(i);
        aFeatureInds.Append(i);
      }
    }
    else if((ad1 < eps && ad2 > eps1) || (ad1 > eps1 && ad2 < eps))
    {
      if(i != theInds.Last())
      {
        theInds.Append(i);
        aFeatureInds.Append(i);
      }
    }
  }
  if(aCurv.Upper() != theInds.Last())
  {
    theInds.Append(aCurv.Upper());
  }

#if defined(APPROXINT_KNOTTOOLS_DEBUG)
  {
    std::cout << "Feature indices new: " << std::endl;
    i;
    for(i = theInds.Lower(); i <= theInds.Upper();  ++i)
    {
      std::cout << i << " : " << theInds(i) << std::endl;
    }
  }
#endif

  //III: Put knots in monotone intervals of curvature.
  Standard_Boolean Ok;
  i = 1;
  do
  {
    i++;
    //
    Ok = InsKnotBefI(i, aCurv, theCoords, dim, theInds, Standard_True); 
    if(Ok)
    {
      i--;
    }
  }
  while(i < theInds.Length());

  //IV: Checking feature points.
  j = 2;
  for(i = 1; i <= aFeatureInds.Length(); ++i)
  {
    Standard_Integer anInd = aFeatureInds(i);
    for(; j <= theInds.Length() - 1;)
    {
      if(theInds(j) == anInd)
      {
        Standard_Integer anIndPrev = theInds(j-1);
        Standard_Integer anIndNext = theInds(j+1);
        Standard_Integer ici = (anIndPrev - aCurv.Lower()) * theDim,
          ici1 = (anIndNext - aCurv.Lower()) * theDim,
          icm = (anInd - aCurv.Lower()) * theDim;
        NCollection_LocalArray<Standard_Real> V1(theDim), V2(theDim);
        Standard_Real mp = 0., m1 = 0., m2 = 0.;
        Standard_Real p;
        for(Standard_Integer k = 0; k < theDim; ++k)
        {
          V1[k] = theCoords[icm + k] - theCoords[ici + k];
          m1 += V1[k]*V1[k];
          V2[k] = theCoords[ici1 + k] - theCoords[icm + k];
          m2 += V2[k]*V2[k];
        }
        for(Standard_Integer k = 1; k < theDim; ++k)
        {
          for(Standard_Integer l = 0; l < k; ++l)
          {
            p = V1[k]*V2[l] - V1[l]*V2[k];
            mp += p*p;
          }
        }
        //mp *= 2.; //P(j,i) = -P(i,j);
        //

        if(mp > aSinCoeff2 * m1 * m2) // Sqrt (mp/(m1*m2)) > aSinCoeff
        {
          //Insert new knots
          Standard_Real d1 = Abs(aCurv(anInd) - aCurv(anIndPrev));
          Standard_Real d2 = Abs(aCurv(anInd) - aCurv(anIndNext));
          if(d1 > d2)
          {
            Ok = InsKnotBefI(j, aCurv, theCoords, dim, theInds, Standard_False);
            if(Ok)
            {
              j++;
            }
            else
            {
              break;
            }
          }
          else
          {
            Ok = InsKnotBefI(j+1, aCurv, theCoords, dim, theInds, Standard_False);
            if(!Ok)
            {
              break;
            }
          }
        }
        else
        {
          j++;
          break;
        }
      }
      else
      {
        j++;
      }
    }
  }
  //
}


//=======================================================================
//function : FilterKnots
//purpose  :
//=======================================================================
void ApproxInt_KnotTools::FilterKnots(NCollection_Sequence<Standard_Integer>& theInds, 
                                      const Standard_Integer theMinNbPnts,
                                      NCollection_Vector<Standard_Integer>& theLKnots)
{
  // Maximum number of points per knot interval.
  Standard_Integer aMaxNbPnts = aMaxPntCoeff*theMinNbPnts;
  Standard_Integer i = 1;
  Standard_Integer aMinNbStep = theMinNbPnts / 2;

  // I: Filter too big number of points per knot interval.
  while(i < theInds.Length())
  {
    Standard_Integer nbint = theInds(i + 1) - theInds(i) + 1;
    if(nbint <= aMaxNbPnts)
    {
      ++i;
      continue;
    }
    else
    {
      Standard_Integer ind = theInds(i) + nbint / 2;
      theInds.InsertAfter(i, ind);     
    }
  }

  // II: Filter points with too small amount of points per knot interval.
  i = 1;
  theLKnots.Append(theInds(i));
  Standard_Integer anIndsPrev = theInds(i);
  for(i = 2; i <= theInds.Length(); ++i)
  {
    if(theInds(i) - anIndsPrev <= theMinNbPnts)
    {
      if (i != theInds.Length())
      {
        Standard_Integer anIdx = i + 1;
        for( ; anIdx <= theInds.Length(); ++anIdx)
        {
          if (theInds(anIdx) - anIndsPrev >= theMinNbPnts)
            break;
        }
        anIdx--;

        Standard_Integer aMidIdx = (theInds(anIdx) + anIndsPrev) / 2;
        if (aMidIdx - anIndsPrev        < theMinNbPnts &&
            aMidIdx - theInds(anIdx)    < theMinNbPnts &&
            theInds(anIdx) - anIndsPrev >= aMinNbStep)
        {
          if (theInds(anIdx) - anIndsPrev > 2 * theMinNbPnts)
          {
            // Bad distribution points merge into one knot interval.
            theLKnots.Append(anIndsPrev + theMinNbPnts);
            anIndsPrev = anIndsPrev + theMinNbPnts;
            i = anIdx - 1;
          }
          else
          {
            if (theInds(anIdx - 1) - anIndsPrev >= theMinNbPnts / 2)
            {
              // Bad distribution points merge into one knot interval.
              theLKnots.Append(theInds(anIdx - 1));
              anIndsPrev = theInds(anIdx - 1);
              i = anIdx - 1;
              if (theInds(anIdx) - theInds(anIdx - 1) <= theMinNbPnts / 2)
              {
                theLKnots.SetValue(theLKnots.Upper(), theInds(anIdx));
                anIndsPrev = theInds(anIdx );
                i = anIdx;
              }
            }
            else
            {
              // Bad distribution points merge into one knot interval.
              theLKnots.Append(theInds(anIdx));
              anIndsPrev = theInds(anIdx);
              i = anIdx;
            }
          }
        }
        else if (anIdx == theInds.Upper() && // Last point obtained.
                 theLKnots.Length() >= 2) // It is possible to modify last item.
        {
          // Current bad interval from i to last.
          // Trying to add knot to divide sequence on two parts:
          // Last good index -> Last index - theMinNbPnts -> Last index
          Standard_Integer aLastGoodIdx = theLKnots.Value(theLKnots.Upper() - 1);
          if ( theInds.Last() - 2 * theMinNbPnts >= aLastGoodIdx)
          {
            theLKnots(theLKnots.Upper()) = theInds.Last() - theMinNbPnts;
            theLKnots.Append(theInds.Last());
            anIndsPrev = theInds(anIdx);
            i = anIdx;
          }
        }
      } // if (i != theInds.Length())
      continue;
    }
    else
    {
      theLKnots.Append(theInds(i));
      anIndsPrev = theInds(i);
    }
  }

  // III: Fill Last Knot.
  if(theLKnots.Length() < 2)
  {
    theLKnots.Append(theInds.Last());
  }
  else
  {
    if(theLKnots.Last() < theInds.Last())
    {
      theLKnots(theLKnots.Upper()) = theInds.Last();
    }
  }
}
//=======================================================================
//function : InsKnotBefI
//purpose  :
//=======================================================================
Standard_Boolean ApproxInt_KnotTools::InsKnotBefI(const Standard_Integer theI,
                                                  const TColStd_Array1OfReal& theCurv,
                                                  const NCollection_LocalArray<Standard_Real>& theCoords,
                                                  const Standard_Integer theDim, 
                                                  NCollection_Sequence<Standard_Integer>& theInds,
                                                  const Standard_Boolean ChkCurv)
{
  Standard_Integer anInd1 = theInds(theI);
  Standard_Integer anInd = theInds(theI - 1);
  //
  if((anInd1-anInd) == 1)
  {
    return Standard_False;
  }
  //
  Standard_Real curv = 0.5*(theCurv(anInd) + theCurv(anInd1));
  Standard_Integer mid = 0, j, jj;
  const Standard_Real aLimitCurvatureChange = 3.0;
  for(j = anInd+1; j < anInd1; ++j)
  {
    mid = 0;

    // I: Curvature change criteria:
    // Non-null curvature.
    if (theCurv(j)     > Precision::Confusion() && 
        theCurv(anInd) > Precision::Confusion() )
    {
      if (theCurv(j) / theCurv(anInd) > aLimitCurvatureChange || 
          theCurv(j) / theCurv(anInd) < 1.0 / aLimitCurvatureChange)
      {
        // Curvature on current interval changed more than 3 times.
        mid = j;
        theInds.InsertBefore(theI, mid);
        return Standard_True;
      }
    }

    // II: Angular criteria:
    Standard_Real ac = theCurv(j - 1), ac1 = theCurv(j);
    if((curv >= ac && curv <= ac1) || (curv >= ac1 && curv <= ac))
    {
      if(Abs(curv - ac) < Abs(curv - ac1))
      {
        mid = j - 1;
      }
      else
      {
        mid = j;
      }
    }
    if(mid == anInd)
    {
      mid++;
    }
    if(mid == anInd1)
    {
      mid--;
    }
    if(mid > 0)
    {
      if(ChkCurv)
      {
        Standard_Integer ici = (anInd - theCurv.Lower()) * theDim,
          ici1 = (anInd1 - theCurv.Lower()) * theDim,
          icm = (mid - theCurv.Lower()) * theDim;
        NCollection_LocalArray<Standard_Real> V1(theDim), V2(theDim);
        Standard_Integer i;
        Standard_Real mp = 0., m1 = 0., m2 = 0.;
        Standard_Real p;
        for(i = 0; i < theDim; ++i)
        {
          V1[i] = theCoords[icm + i] - theCoords[ici + i];
          m1 += V1[i]*V1[i];
          V2[i] = theCoords[ici1 + i] - theCoords[icm + i];
          m2 += V2[i]*V2[i];
        }
        for(i = 1; i < theDim; ++i)
        {
          for(jj = 0; jj < i; ++jj)
          {
            p = V1[i]*V2[jj] - V1[jj]*V2[i];
            mp += p*p;
          }
        }
        //mp *= 2.; //P(j,i) = -P(i,j);
        //

        if (mp > aSinCoeff2 * m1 * m2) // Sqrt (mp / m1m2) > aSinCoeff
        {
          theInds.InsertBefore(theI, mid);
          return Standard_True;
        }
      }
      else
      {
        theInds.InsertBefore(theI, mid);
        return Standard_True;
      }
    }
  }

  return Standard_False;
}

//=======================================================================
//function : BuildKnots
//purpose  :
//=======================================================================
void ApproxInt_KnotTools::BuildKnots(const TColgp_Array1OfPnt& thePntsXYZ,
                                     const TColgp_Array1OfPnt2d& thePntsU1V1,
                                     const TColgp_Array1OfPnt2d& thePntsU2V2,
                                     const math_Vector& thePars,
                                     const Standard_Boolean theApproxXYZ,
                                     const Standard_Boolean theApproxU1V1,
                                     const Standard_Boolean theApproxU2V2,
                                     const Standard_Integer theMinNbPnts,
                                     NCollection_Vector<Standard_Integer>& theKnots)
{
  NCollection_Sequence<Standard_Integer> aKnots;
  Standard_Integer aDim = 0;

  // I: Convert input data to the corresponding format.
  if(theApproxXYZ)
    aDim += 3;
  if(theApproxU1V1)
    aDim += 2;
  if(theApproxU2V2)
    aDim += 2;

  NCollection_LocalArray<Standard_Real> aCoords(thePars.Length()*aDim);
  Standard_Integer i, j;
  for(i = thePars.Lower(); i <= thePars.Upper(); ++i)
  {
    j = (i - thePars.Lower()) * aDim;
    if(theApproxXYZ)
    {
      aCoords[j] = thePntsXYZ.Value(i).X();
      ++j;
      aCoords[j] = thePntsXYZ.Value(i).Y();
      ++j;
      aCoords[j] = thePntsXYZ.Value(i).Z();
      ++j;
    }
    if(theApproxU1V1)
    {
      aCoords[j] = thePntsU1V1.Value(i).X();
      ++j;
      aCoords[j] = thePntsU1V1.Value(i).Y();
      ++j;
    }
    if(theApproxU2V2)
    {
      aCoords[j] = thePntsU2V2.Value(i).X();
      ++j;
      aCoords[j] = thePntsU2V2.Value(i).Y();
      ++j;
    }
  }

  // II: Build draft knot sequence.
  ComputeKnotInds(aCoords, aDim, thePars, aKnots);

#if defined(APPROXINT_KNOTTOOLS_DEBUG)
    std::cout << "Draft knot sequence: " << std::endl;
    for(i = aKnots.Lower(); i <= aKnots.Upper();  ++i)
    {
      std::cout << i << " : " << aKnots(i) << std::endl;
    }
#endif

  // III: Build output knot sequence.
  FilterKnots(aKnots, theMinNbPnts, theKnots);

#if defined(APPROXINT_KNOTTOOLS_DEBUG)
    std::cout << "Result knot sequence: " << std::endl;
    for(i = theKnots.Lower(); i <= theKnots.Upper();  ++i)
    {
      std::cout << i << " : " << theKnots(i) << std::endl;
    }
#endif

}
Commit	Line	Data
f44aa197	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
f4dee9bb	31	//static const Standard_Real aSinCoeff = 0.30901699437494742410229341718282;
f4dee9bb	32	static const Standard_Real aSinCoeff2 = 0.09549150281252627; // aSinCoeff^2 = (3. - Sqrt(5.)) / 8.
f44aa197	33	static const Standard_Integer aMaxPntCoeff = 15;
f44aa197	34
f44aa197	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	}
f44aa197	60	//
	61	Standard_Real q = 0.;
	62	for(i = 0; i < dim; ++i)
	63	{
	64	q += V1[i]*V1[i];
	65	}
326b3e28	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 = qq;
	84
f44aa197	85	//
326b3e28	86	Standard_Real curv = Sqrt(mp / q);
f44aa197	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];
c7854818	105	Standard_Integer i, j, m, ic;
f44aa197	106	Standard_Real aMaxCurv = 0.;
	107	Standard_Integer dim = theDim;
	108	//
	109	i = aCurv.Lower();
	110	for(j = 0; j < 3; ++j)
	111	{
c7854818	112	Standard_Integer k = i+j;
f44aa197	113	ic = (k - aCurv.Lower()) * dim;
c7854818	114	Standard_Integer l = dim*j;
f44aa197	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	{
c7854818	134	Standard_Integer k = i+j-1;
f44aa197	135	ic = (k - aCurv.Lower()) * dim;
c7854818	136	Standard_Integer l = dim*j;
f44aa197	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	{
c7854818	155	Standard_Integer k = i+j-2;
f44aa197	156	ic = (k - aCurv.Lower()) * dim;
c7854818	157	Standard_Integer l = dim*j;
f44aa197	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
77dbd1f1	172	#ifdef APPROXINT_KNOTTOOLS_DEBUG
04232180	173	std::cout << "Discrete curvature array is" << std::endl;
f44aa197	174	for(i = aCurv.Lower(); i <= aCurv.Upper(); ++i)
f44aa197	175	{
04232180	176	std::cout << i << " " << aCurv(i) << std::endl;
f44aa197	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	}
f44aa197	214	}
	215	if(aCurv.Upper() != theInds.Last())
	216	{
	217	theInds.Append(aCurv.Upper());
	218	}
	219
6dc83e21	220	#if defined(APPROXINT_KNOTTOOLS_DEBUG)
f44aa197	221	{
04232180	222	std::cout << "Feature indices new: " << std::endl;
f44aa197	223	i;
	224	for(i = theInds.Lower(); i <= theInds.Upper(); ++i)
	225	{
04232180	226	std::cout << i << " : " << theInds(i) << std::endl;
f44aa197	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
b81b237f	246	//IV: Checking feature points.
f44aa197	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);
f44aa197	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);
f44aa197	261	Standard_Real mp = 0., m1 = 0., m2 = 0.;
f44aa197	262	Standard_Real p;
c7854818	263	for(Standard_Integer k = 0; k < theDim; ++k)
f44aa197	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	}
c7854818	270	for(Standard_Integer k = 1; k < theDim; ++k)
f44aa197	271	{
c7854818	272	for(Standard_Integer l = 0; l < k; ++l)
f44aa197	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	//
f44aa197	280
f4dee9bb	281	if(mp > aSinCoeff2 * m1 * m2) // Sqrt (mp/(m1*m2)) > aSinCoeff
f44aa197	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
316ea293	352	// II: Filter points with too small amount of points per knot interval.
f44aa197	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	{
4e14c88f	365	if (theInds(anIdx) - anIndsPrev >= theMinNbPnts)
f44aa197	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	{
4e14c88f	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	}
f44aa197	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	{
f44aa197	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	//
f44aa197	534
f4dee9bb	535	if (mp > aSinCoeff2 * m1 * m2) // Sqrt (mp / m1m2) > aSinCoeff
f44aa197	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
6dc83e21	610	#if defined(APPROXINT_KNOTTOOLS_DEBUG)
04232180	611	std::cout << "Draft knot sequence: " << std::endl;
f44aa197	612	for(i = aKnots.Lower(); i <= aKnots.Upper(); ++i)
f44aa197	613	{
04232180	614	std::cout << i << " : " << aKnots(i) << std::endl;
f44aa197	615	}
	616	#endif
	617
	618	// III: Build output knot sequence.
	619	FilterKnots(aKnots, theMinNbPnts, theKnots);
	620
6dc83e21	621	#if defined(APPROXINT_KNOTTOOLS_DEBUG)
04232180	622	std::cout << "Result knot sequence: " << std::endl;
f44aa197	623	for(i = theKnots.Lower(); i <= theKnots.Upper(); ++i)
f44aa197	624	{
04232180	625	std::cout << i << " : " << theKnots(i) << std::endl;
f44aa197	626	}
	627	#endif
	628
	629	}