OCCT

// Copyright (c) 1996-1999 Matra Datavision
// 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.

// Jeannine PANCIATICI le 06/06/96
// Igor FEOKTISTOV 14/12/98 - correction of Approximate() and Init().
// Approximation d une MultiLine de points decrite par le tool MLineTool.
// avec criteres variationnels

#include <AppDef_MultiLine.hxx>
#include <AppDef_Variational.hxx>
#include <AppParCurves_MultiBSpCurve.hxx>
#include <Standard_ConstructionError.hxx>
#include <Standard_DomainError.hxx>

#define No_Standard_RangeError
#define No_Standard_OutOfRange
#define No_Standard_DimensionError
#define No_Standard_ConstructionError

#include <Standard_Stream.hxx>

#include <AppParCurves.hxx>
#include <AppParCurves_Constraint.hxx>
#include <AppParCurves_HArray1OfConstraintCouple.hxx>
#include <AppParCurves_Array1OfMultiPoint.hxx>
#include <AppParCurves_MultiPoint.hxx>
#include <AppDef_LinearCriteria.hxx>
#include <Convert_CompPolynomialToPoles.hxx>
#include <gp_Pnt.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec.hxx>
#include <gp_Vec2d.hxx>
#include <TColgp_Array1OfPnt.hxx>
#include <TColgp_Array1OfPnt2d.hxx>
#include <TColgp_Array1OfVec.hxx>
#include <TColgp_Array1OfVec2d.hxx>
#include <TColStd_HArray1OfInteger.hxx>
#include <TColStd_Array1OfReal.hxx>
#include <TColStd_HArray1OfReal.hxx>
#include <TColStd_HArray2OfReal.hxx>
#include <StdFail_NotDone.hxx>
#include <Precision.hxx>
#include <AppDef_MyLineTool.hxx>

#include <TColStd_HArray2OfInteger.hxx>
#include <TColStd_Array2OfReal.hxx>
#include <FEmTool_Assembly.hxx>
#include <FEmTool_Curve.hxx>
#include <math_Vector.hxx>
#include <PLib_HermitJacobi.hxx>
#include <FEmTool_HAssemblyTable.hxx>

// Add this line:
#include <algorithm>

#if defined(_MSC_VER)
  #include <stdio.h>
#endif /* _MSC_VER */

//
//=======================================================================
// function : AppDef_Variational
// purpose  : Initialization of   the   fields.
//=======================================================================
//
AppDef_Variational::AppDef_Variational(
  const AppDef_MultiLine&                               SSP,
  const Standard_Integer                                FirstPoint,
  const Standard_Integer                                LastPoint,
  const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
  const Standard_Integer                                MaxDegree,
  const Standard_Integer                                MaxSegment,
  const GeomAbs_Shape                                   Continuity,
  const Standard_Boolean                                WithMinMax,
  const Standard_Boolean                                WithCutting,
  const Standard_Real                                   Tolerance,
  const Standard_Integer                                NbIterations)
    : mySSP(SSP),
      myFirstPoint(FirstPoint),
      myLastPoint(LastPoint),
      myConstraints(TheConstraints),
      myMaxDegree(MaxDegree),
      myMaxSegment(MaxSegment),
      myNbIterations(NbIterations),
      myTolerance(Tolerance),
      myContinuity(Continuity),
      myWithMinMax(WithMinMax),
      myWithCutting(WithCutting)
{
  // Verifications:
  if (myMaxDegree < 1)
    throw Standard_DomainError();
  myMaxDegree = Min(30, myMaxDegree);
  //
  if (myMaxSegment < 1)
    throw Standard_DomainError();
  //
  if (myWithMinMax != 0 && myWithMinMax != 1)
    throw Standard_DomainError();
  if (myWithCutting != 0 && myWithCutting != 1)
    throw Standard_DomainError();
  //
  myIsOverConstr = Standard_False;
  myIsCreated    = Standard_False;
  myIsDone       = Standard_False;
  switch (myContinuity)
  {
    case GeomAbs_C0:
      myNivCont = 0;
      break;
    case GeomAbs_C1:
      myNivCont = 1;
      break;
    case GeomAbs_C2:
      myNivCont = 2;
      break;
    default:
      throw Standard_ConstructionError();
  }
  //
  myNbP2d     = AppDef_MyLineTool::NbP2d(SSP);
  myNbP3d     = AppDef_MyLineTool::NbP3d(SSP);
  myDimension = 2 * myNbP2d + 3 * myNbP3d;
  //
  myPercent[0] = 0.4;
  myPercent[1] = 0.2;
  myPercent[2] = 0.4;
  myKnots      = new TColStd_HArray1OfReal(1, 2);
  myKnots->SetValue(1, 0.);
  myKnots->SetValue(2, 1.);

  //  Declaration
  //
  mySmoothCriterion = new AppDef_LinearCriteria(mySSP, myFirstPoint, myLastPoint);
  myParameters      = new TColStd_HArray1OfReal(myFirstPoint, myLastPoint);
  myNbPoints        = myLastPoint - myFirstPoint + 1;
  if (myNbPoints <= 0)
    throw Standard_ConstructionError();
  //
  myTabPoints = new TColStd_HArray1OfReal(1, myDimension * myNbPoints);
  //
  //  Table of Points initialization
  //
  Standard_Integer     ipoint, jp2d, jp3d, index;
  TColgp_Array1OfPnt   TabP3d(1, Max(1, myNbP3d));
  TColgp_Array1OfPnt2d TabP2d(1, Max(1, myNbP2d));
  gp_Pnt2d             P2d;
  gp_Pnt               P3d;
  index = 1;

  for (ipoint = myFirstPoint; ipoint <= myLastPoint; ipoint++)
  {

    if (myNbP2d != 0 && myNbP3d == 0)
    {
      AppDef_MyLineTool::Value(mySSP, ipoint, TabP2d);

      for (jp2d = 1; jp2d <= myNbP2d; jp2d++)
      {
        P2d = TabP2d.Value(jp2d);

        myTabPoints->SetValue(index++, P2d.X());
        myTabPoints->SetValue(index++, P2d.Y());
      }
    }
    if (myNbP3d != 0 && myNbP2d == 0)
    {
      AppDef_MyLineTool::Value(mySSP, ipoint, TabP3d);

      for (jp3d = 1; jp3d <= myNbP3d; jp3d++)

      {
        P3d = TabP3d.Value(jp3d);

        myTabPoints->SetValue(index++, P3d.X());
        myTabPoints->SetValue(index++, P3d.Y());
        myTabPoints->SetValue(index++, P3d.Z());
      }
    }
    if (myNbP3d != 0 && myNbP2d != 0)
    {
      AppDef_MyLineTool::Value(mySSP, ipoint, TabP3d, TabP2d);

      for (jp3d = 1; jp3d <= myNbP3d; jp3d++)

      {
        P3d = TabP3d.Value(jp3d);

        myTabPoints->SetValue(index++, P3d.X());
        myTabPoints->SetValue(index++, P3d.Y());
        myTabPoints->SetValue(index++, P3d.Z());
      }
      for (jp2d = 1; jp2d <= myNbP2d; jp2d++)

      {
        P2d = TabP2d.Value(jp2d);

        myTabPoints->SetValue(index++, P2d.X());
        myTabPoints->SetValue(index++, P2d.Y());
      }
    }
  }
  Init();
}

//
//=======================================================================
// function : Init
// purpose  : Initializes the tables of constraints
//           and verifies if the problem is not over-constrained.
//           This method is used in the Create and the method SetConstraint.
//=======================================================================
//
void AppDef_Variational::Init()
{

  Standard_Integer     ipoint, jp2d, jp3d, index, jndex;
  Standard_Integer     CurMultyPoint;
  TColgp_Array1OfVec   TabV3d(1, Max(1, myNbP3d));
  TColgp_Array1OfVec2d TabV2d(1, Max(1, myNbP2d));
  TColgp_Array1OfVec   TabV3dcurv(1, Max(1, myNbP3d));
  TColgp_Array1OfVec2d TabV2dcurv(1, Max(1, myNbP2d));

  gp_Vec   Vt3d, Vc3d;
  gp_Vec2d Vt2d, Vc2d;

  myNbConstraints = myConstraints->Length();
  if (myNbConstraints < 0)
    throw Standard_ConstructionError();

  myTypConstraints = new TColStd_HArray1OfInteger(1, Max(1, 2 * myNbConstraints));
  myTabConstraints = new TColStd_HArray1OfReal(1, Max(1, 2 * myDimension * myNbConstraints));
  myTtheta = new TColStd_HArray1OfReal(1, Max(1, (2 * myNbP2d + 6 * myNbP3d) * myNbConstraints));
  myTfthet = new TColStd_HArray1OfReal(1, Max(1, (2 * myNbP2d + 6 * myNbP3d) * myNbConstraints));

  //
  // Table of types initialization
  Standard_Integer iconstr;
  index          = 1;
  jndex          = 1;
  CurMultyPoint  = 1;
  myNbPassPoints = 0;
  myNbTangPoints = 0;
  myNbCurvPoints = 0;
  AppParCurves_Constraint valcontr;

  for (iconstr = myConstraints->Lower(); iconstr <= myConstraints->Upper(); iconstr++)
  {
    ipoint = (myConstraints->Value(iconstr)).Index();

    valcontr = (myConstraints->Value(iconstr)).Constraint();
    switch (valcontr)
    {
      case AppParCurves_NoConstraint:
        CurMultyPoint -= myNbP3d * 6 + myNbP2d * 2;
        break;
      case AppParCurves_PassPoint:
        myTypConstraints->SetValue(index++, ipoint);
        myTypConstraints->SetValue(index++, 0);
        myNbPassPoints++;
        if (myNbP2d != 0)
          jndex = jndex + 4 * myNbP2d;
        if (myNbP3d != 0)
          jndex = jndex + 6 * myNbP3d;
        break;
      case AppParCurves_TangencyPoint:
        myTypConstraints->SetValue(index++, ipoint);
        myTypConstraints->SetValue(index++, 1);
        myNbTangPoints++;
        if (myNbP2d != 0 && myNbP3d == 0)
        {
          if (AppDef_MyLineTool::Tangency(mySSP, ipoint, TabV2d) == Standard_False)
            throw Standard_ConstructionError();
          for (jp2d = 1; jp2d <= myNbP2d; jp2d++)
          {
            Vt2d = TabV2d.Value(jp2d);
            Vt2d.Normalize();
            myTabConstraints->SetValue(jndex++, Vt2d.X());
            myTabConstraints->SetValue(jndex++, Vt2d.Y());
            jndex = jndex + 2;
            InitTthetaF(2, valcontr, CurMultyPoint + (jp2d - 1) * 2, jndex - 4);
          }
        }
        if (myNbP3d != 0 && myNbP2d == 0)
        {
          if (AppDef_MyLineTool::Tangency(mySSP, ipoint, TabV3d) == Standard_False)
            throw Standard_ConstructionError();
          for (jp3d = 1; jp3d <= myNbP3d; jp3d++)
          {
            Vt3d = TabV3d.Value(jp3d);
            Vt3d.Normalize();
            myTabConstraints->SetValue(jndex++, Vt3d.X());

            myTabConstraints->SetValue(jndex++, Vt3d.Y());

            myTabConstraints->SetValue(jndex++, Vt3d.Z());
            jndex = jndex + 3;
            InitTthetaF(3, valcontr, CurMultyPoint + (jp3d - 1) * 6, jndex - 6);
          }
        }
        if (myNbP3d != 0 && myNbP2d != 0)
        {
          if (AppDef_MyLineTool::Tangency(mySSP, ipoint, TabV3d, TabV2d) == Standard_False)
            throw Standard_ConstructionError();
          for (jp3d = 1; jp3d <= myNbP3d; jp3d++)
          {
            Vt3d = TabV3d.Value(jp3d);
            Vt3d.Normalize();
            myTabConstraints->SetValue(jndex++, Vt3d.X());
            myTabConstraints->SetValue(jndex++, Vt3d.Y());
            myTabConstraints->SetValue(jndex++, Vt3d.Z());
            jndex = jndex + 3;
            InitTthetaF(3, valcontr, CurMultyPoint + (jp3d - 1) * 6, jndex - 6);
          }

          for (jp2d = 1; jp2d <= myNbP2d; jp2d++)
          {
            Vt2d = TabV2d.Value(jp2d);
            Vt2d.Normalize();
            myTabConstraints->SetValue(jndex++, Vt2d.X());
            myTabConstraints->SetValue(jndex++, Vt2d.Y());
            jndex = jndex + 2;
            InitTthetaF(2, valcontr, CurMultyPoint + (jp2d - 1) * 2 + myNbP3d * 6, jndex - 4);
          }
        }

        break;

      case AppParCurves_CurvaturePoint:
        myTypConstraints->SetValue(index++, ipoint);
        myTypConstraints->SetValue(index++, 2);
        myNbCurvPoints++;
        if (myNbP2d != 0 && myNbP3d == 0)
        {
          if (AppDef_MyLineTool::Tangency(mySSP, ipoint, TabV2d) == Standard_False)
            throw Standard_ConstructionError();
          if (AppDef_MyLineTool::Curvature(mySSP, ipoint, TabV2dcurv) == Standard_False)
            throw Standard_ConstructionError();
          for (jp2d = 1; jp2d <= myNbP2d; jp2d++)
          {
            Vt2d = TabV2d.Value(jp2d);
            Vt2d.Normalize();
            Vc2d = TabV2dcurv.Value(jp2d);
            if (Abs(Abs(Vc2d.Angle(Vt2d)) - M_PI / 2.) > Precision::Angular())
              throw Standard_ConstructionError();
            myTabConstraints->SetValue(jndex++, Vt2d.X());
            myTabConstraints->SetValue(jndex++, Vt2d.Y());
            myTabConstraints->SetValue(jndex++, Vc2d.X());
            myTabConstraints->SetValue(jndex++, Vc2d.Y());
            InitTthetaF(2, valcontr, CurMultyPoint + (jp2d - 1) * 2, jndex - 4);
          }
        }

        if (myNbP3d != 0 && myNbP2d == 0)
        {
          if (AppDef_MyLineTool::Tangency(mySSP, ipoint, TabV3d) == Standard_False)
            throw Standard_ConstructionError();
          if (AppDef_MyLineTool::Curvature(mySSP, ipoint, TabV3dcurv) == Standard_False)
            throw Standard_ConstructionError();
          for (jp3d = 1; jp3d <= myNbP3d; jp3d++)
          {
            Vt3d = TabV3d.Value(jp3d);
            Vt3d.Normalize();
            Vc3d = TabV3dcurv.Value(jp3d);
            if ((Vc3d.Normalized()).IsNormal(Vt3d, Precision::Angular()) == Standard_False)
              throw Standard_ConstructionError();
            myTabConstraints->SetValue(jndex++, Vt3d.X());
            myTabConstraints->SetValue(jndex++, Vt3d.Y());
            myTabConstraints->SetValue(jndex++, Vt3d.Z());
            myTabConstraints->SetValue(jndex++, Vc3d.X());
            myTabConstraints->SetValue(jndex++, Vc3d.Y());
            myTabConstraints->SetValue(jndex++, Vc3d.Z());
            InitTthetaF(3, valcontr, CurMultyPoint + (jp3d - 1) * 6, jndex - 6);
          }
        }
        if (myNbP3d != 0 && myNbP2d != 0)
        {
          if (AppDef_MyLineTool::Tangency(mySSP, ipoint, TabV3d, TabV2d) == Standard_False)
            throw Standard_ConstructionError();
          if (AppDef_MyLineTool::Curvature(mySSP, ipoint, TabV3dcurv, TabV2dcurv) == Standard_False)
            throw Standard_ConstructionError();
          for (jp3d = 1; jp3d <= myNbP3d; jp3d++)
          {
            Vt3d = TabV3d.Value(jp3d);
            Vt3d.Normalize();
            Vc3d = TabV3dcurv.Value(jp3d);
            if ((Vc3d.Normalized()).IsNormal(Vt3d, Precision::Angular()) == Standard_False)
              throw Standard_ConstructionError();
            myTabConstraints->SetValue(jndex++, Vt3d.X());
            myTabConstraints->SetValue(jndex++, Vt3d.Y());
            myTabConstraints->SetValue(jndex++, Vt3d.Z());
            myTabConstraints->SetValue(jndex++, Vc3d.X());
            myTabConstraints->SetValue(jndex++, Vc3d.Y());
            myTabConstraints->SetValue(jndex++, Vc3d.Z());
            InitTthetaF(3, valcontr, CurMultyPoint + (jp3d - 1) * 6, jndex - 6);
          }
          for (jp2d = 1; jp2d <= myNbP2d; jp2d++)
          {
            Vt2d = TabV2d.Value(jp2d);
            Vt2d.Normalize();
            Vc2d = TabV2dcurv.Value(jp2d);
            if (Abs(Abs(Vc2d.Angle(Vt2d)) - M_PI / 2.) > Precision::Angular())
              throw Standard_ConstructionError();
            myTabConstraints->SetValue(jndex++, Vt2d.X());
            myTabConstraints->SetValue(jndex++, Vt2d.Y());
            myTabConstraints->SetValue(jndex++, Vc2d.X());
            myTabConstraints->SetValue(jndex++, Vc2d.Y());
            InitTthetaF(2, valcontr, CurMultyPoint + (jp2d - 1) * 2 + myNbP3d * 6, jndex - 4);
          }
        }
        break;
      default:
        throw Standard_ConstructionError();
    }
    CurMultyPoint += myNbP3d * 6 + myNbP2d * 2;
  }
  // OverConstraint Detection
  Standard_Integer MaxSeg;
  if (myWithCutting == Standard_True)
    MaxSeg = myMaxSegment;
  else
    MaxSeg = 1;
  if (((myMaxDegree - myNivCont) * MaxSeg - myNbPassPoints - 2 * myNbTangPoints
       - 3 * myNbCurvPoints)
      < 0)
  {
    myIsOverConstr = Standard_True;
    myIsCreated    = Standard_False;
  }
  else
  {
    InitSmoothCriterion();
    myIsCreated = Standard_True;
  }
}

//
//=======================================================================
// function : Approximate
// purpose  : Makes the approximation with the current fields.
//=======================================================================
//
void AppDef_Variational::Approximate()

{
  if (myIsCreated == Standard_False)
    throw StdFail_NotDone();

  Standard_Real WQuadratic, WQuality;

  TColStd_Array1OfReal Ecarts(myFirstPoint, myLastPoint);

  mySmoothCriterion->GetWeight(WQuadratic, WQuality);

  Handle(FEmTool_Curve) TheCurve;

  mySmoothCriterion->GetCurve(TheCurve);

  //---------------------------------------------------------------------

  TheMotor(mySmoothCriterion, WQuadratic, WQuality, TheCurve, Ecarts);

  if (myWithMinMax && myTolerance < myMaxError)
    Adjusting(mySmoothCriterion, WQuadratic, WQuality, TheCurve, Ecarts);

  //---------------------------------------------------------------------

  Standard_Integer jp2d, jp3d, ipole, NbElem = TheCurve->NbElements();

  TColgp_Array1OfPnt   TabP3d(1, Max(1, myNbP3d));
  TColgp_Array1OfPnt2d TabP2d(1, Max(1, myNbP2d));
  Standard_Real        debfin[2] = {-1., 1};

  gp_Pnt2d P2d;
  gp_Pnt   P3d;
  {
    Handle(TColStd_HArray2OfReal) PolynomialIntervalsPtr =
      new TColStd_HArray2OfReal(1, NbElem, 1, 2);

    Handle(TColStd_HArray1OfInteger) NbCoeffPtr = new TColStd_HArray1OfInteger(1, myMaxSegment);

    Standard_Integer              size     = myMaxSegment * (myMaxDegree + 1) * myDimension;
    Handle(TColStd_HArray1OfReal) CoeffPtr = new TColStd_HArray1OfReal(1, size);

    CoeffPtr->Init(0.);

    Handle(TColStd_HArray1OfReal) IntervallesPtr = new TColStd_HArray1OfReal(1, NbElem + 1);

    IntervallesPtr->ChangeArray1() = TheCurve->Knots();

    TheCurve->GetPolynom(CoeffPtr->ChangeArray1());

    Standard_Integer ii;

    for (ii = 1; ii <= NbElem; ii++)
      NbCoeffPtr->SetValue(ii, TheCurve->Degree(ii) + 1);

    for (ii = PolynomialIntervalsPtr->LowerRow(); ii <= PolynomialIntervalsPtr->UpperRow(); ii++)
    {
      PolynomialIntervalsPtr->SetValue(ii, 1, debfin[0]);
      PolynomialIntervalsPtr->SetValue(ii, 2, debfin[1]);
    }
    /*
    printf("\n =========== Parameters for converting\n");
    printf("nb_courbes : %d \n", NbElem);
    printf("tab_option[4] : %d \n", myNivCont);
    printf("myDimension : %d \n", myDimension);
    printf("myMaxDegree : %d \n", myMaxDegree);
    printf("\n NbcoeffPtr :\n");
    for(ii = 1; ii <= NbElem; ii++)  printf("NbCoeffPtr(%d) = %d \n", ii, NbCoeffPtr->Value(ii));
    size = NbElem*(myMaxDegree + 1) * myDimension;
    printf("\n CoeffPtr :\n");
    for(ii = 1; ii <= size; ii++)  printf("CoeffPtr(%d) = %.8e \n", ii, CoeffPtr->Value(ii));
    printf("\n PolinimialIntervalsPtr :\n");
    for (ii = PolynomialIntervalsPtr->LowerRow() ;
    ii <= PolynomialIntervalsPtr->UpperRow() ;ii++)
    {
    printf(" %d : [%f, %f] \n", ii, PolynomialIntervalsPtr->Value(ii,1),
    PolynomialIntervalsPtr->Value(ii,2)) ;
    }
    printf("\n IntervallesPtr :\n");
    for (ii = IntervallesPtr->Lower();
    ii <= IntervallesPtr->Upper() - 1; ii++)
    {
    printf(" %d : [%f, %f] \n", ii, IntervallesPtr->Value(ii),
    IntervallesPtr->Value(ii+1)) ;
    }
    */
    Convert_CompPolynomialToPoles AConverter(NbElem,
                                             myNivCont,
                                             myDimension,
                                             myMaxDegree,
                                             NbCoeffPtr,
                                             CoeffPtr,
                                             PolynomialIntervalsPtr,
                                             IntervallesPtr);
    if (AConverter.IsDone())
    {
      Handle(TColStd_HArray2OfReal)    PolesPtr;
      Handle(TColStd_HArray1OfInteger) Mults;
      Standard_Integer                 NbPoles = AConverter.NbPoles();
      //	Standard_Integer Deg=AConverter.Degree();
      AppParCurves_Array1OfMultiPoint TabMU(1, NbPoles);
      AConverter.Poles(PolesPtr);
      AConverter.Knots(myKnots);
      AConverter.Multiplicities(Mults);

      for (ipole = PolesPtr->LowerRow(); ipole <= PolesPtr->UpperRow(); ipole++)
      {
        Standard_Integer index = PolesPtr->LowerCol();
        /*	    if(myNbP2d !=0 )
        {
        for (jp2d=1;jp2d<=myNbP2d;jp2d++)
        {
        P2d.SetX(PolesPtr->Value(ipole,index++));
        P2d.SetY(PolesPtr->Value(ipole,index++));
        TabP2d.SetValue(jp2d,P2d);
        }
        }*/
        if (myNbP3d != 0)
        {
          for (jp3d = 1; jp3d <= myNbP3d; jp3d++)
          {
            //                       std::cout << "\n Poles(ipole,1)" <<
            //                       PolesPtr->Value(ipole,index);
            P3d.SetX(PolesPtr->Value(ipole, index++));
            //                       std::cout << "\n Poles(ipole,1)" <<
            //                       PolesPtr->Value(ipole,index);
            P3d.SetY(PolesPtr->Value(ipole, index++));
            //                       std::cout << "\n Poles(ipole,1)" <<
            //                       PolesPtr->Value(ipole,index);
            P3d.SetZ(PolesPtr->Value(ipole, index++));
            TabP3d.SetValue(jp3d, P3d);
          }
        }
        if (myNbP2d != 0)
        {
          for (jp2d = 1; jp2d <= myNbP2d; jp2d++)
          {
            P2d.SetX(PolesPtr->Value(ipole, index++));
            P2d.SetY(PolesPtr->Value(ipole, index++));
            TabP2d.SetValue(jp2d, P2d);
          }
        }
        if (myNbP2d != 0 && myNbP3d != 0)
        {
          AppParCurves_MultiPoint aMultiPoint(TabP3d, TabP2d);
          TabMU.SetValue(ipole, aMultiPoint);
        }
        else if (myNbP2d != 0)
        {
          AppParCurves_MultiPoint aMultiPoint(TabP2d);
          TabMU.SetValue(ipole, aMultiPoint);
        }
        else
        {

          AppParCurves_MultiPoint aMultiPoint(TabP3d);
          TabMU.SetValue(ipole, aMultiPoint);
        }
      }
      AppParCurves_MultiBSpCurve aCurve(TabMU, myKnots->Array1(), Mults->Array1());
      myMBSpCurve = aCurve;
      myIsDone    = Standard_True;
    }
  }
}

//
//=======================================================================
// function : IsCreated
// purpose  : returns True if the creation is done
//=======================================================================
//
Standard_Boolean AppDef_Variational::IsCreated() const
{
  return myIsCreated;
}

//
//=======================================================================
// function : IsDone
// purpose  : returns True if the  approximation is ok
//=======================================================================
//
Standard_Boolean AppDef_Variational::IsDone() const
{
  return myIsDone;
}

//
//=======================================================================
// function : IsOverConstrained
// purpose  : returns True if the problem is overconstrained
//           in this case, approximation cannot be done.
//=======================================================================
//
Standard_Boolean AppDef_Variational::IsOverConstrained() const
{
  return myIsOverConstr;
}

//
//=======================================================================
// function : Value
// purpose  : returns all the BSpline curves approximating the
//    	     MultiLine SSP after minimization of the parameter.

//=======================================================================
//
AppParCurves_MultiBSpCurve AppDef_Variational::Value() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myMBSpCurve;
}

//
//=======================================================================
// function : MaxError
// purpose  : returns the maximum of the distances between
//           the points of the multiline and the approximation
//           curves.
//=======================================================================
//
Standard_Real AppDef_Variational::MaxError() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myMaxError;
}

//
//=======================================================================
// function : MaxErrorIndex
// purpose  : returns the index of the MultiPoint of ErrorMax
//=======================================================================
//
Standard_Integer AppDef_Variational::MaxErrorIndex() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myMaxErrorIndex;
}

//
//=======================================================================
// function : QuadraticError
// purpose  :  returns the quadratic average of the distances between
//            the points of the multiline and the approximation
//            curves.
//=======================================================================
//
Standard_Real AppDef_Variational::QuadraticError() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myCriterium[0];
}

//
//=======================================================================
// function : Distance
// purpose  : returns the distances between the points of the
//           multiline and the approximation curves.
//=======================================================================
//
void AppDef_Variational::Distance(math_Matrix& mat)

{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  Standard_Integer     ipoint, jp2d, jp3d, index;
  TColgp_Array1OfPnt   TabP3d(1, Max(1, myNbP3d));
  TColgp_Array1OfPnt2d TabP2d(1, Max(1, myNbP2d));
  Standard_Integer     j0 = mat.LowerCol() - myFirstPoint;

  gp_Pnt2d P2d;
  gp_Pnt   P3d;

  gp_Pnt   Pt3d;
  gp_Pnt2d Pt2d;

  for (ipoint = myFirstPoint; ipoint <= myLastPoint; ipoint++)
  {
    index = 1;
    if (myNbP3d != 0)
    {
      AppDef_MyLineTool::Value(mySSP, ipoint, TabP3d);

      for (jp3d = 1; jp3d <= myNbP3d; jp3d++)

      {
        P3d = TabP3d.Value(jp3d);
        myMBSpCurve.Value(index, myParameters->Value(ipoint), Pt3d);
        mat(index++, j0 + ipoint) = P3d.Distance(Pt3d);
      }
    }
    if (myNbP2d != 0)
    {
      if (myNbP3d == 0)
        AppDef_MyLineTool::Value(mySSP, ipoint, TabP2d);
      else
        AppDef_MyLineTool::Value(mySSP, ipoint, TabP3d, TabP2d);
      for (jp2d = 1; jp2d <= myNbP2d; jp2d++)

      {
        P2d = TabP2d.Value(jp2d);
        myMBSpCurve.Value(index, myParameters->Value(ipoint), Pt2d);
        mat(index++, j0 + ipoint) = P2d.Distance(Pt2d);
      }
    }
  }
}

//
//=======================================================================
// function : AverageError
// purpose  : returns the average error between
//           the MultiLine and the approximation.
//=======================================================================
//
Standard_Real AppDef_Variational::AverageError() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myAverageError;
}

//
//=======================================================================
// function : Parameters
// purpose  : returns the parameters uses to the approximations
//=======================================================================
//
const Handle(TColStd_HArray1OfReal)& AppDef_Variational::Parameters() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myParameters;
}

//
//=======================================================================
// function : Knots
// purpose  : returns the knots uses to the approximations
//=======================================================================
//
const Handle(TColStd_HArray1OfReal)& AppDef_Variational::Knots() const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  return myKnots;
}

//
//=======================================================================
// function : Criterium
// purpose  : returns the values of the quality criterium.
//=======================================================================
//
void AppDef_Variational::Criterium(Standard_Real& VFirstOrder,
                                   Standard_Real& VSecondOrder,
                                   Standard_Real& VThirdOrder) const
{
  if (myIsDone == Standard_False)
    throw StdFail_NotDone();
  VFirstOrder  = myCriterium[1];
  VSecondOrder = myCriterium[2];
  VThirdOrder  = myCriterium[3];
}

//
//=======================================================================
// function : CriteriumWeight
// purpose  : returns the Weights (as percent) associed  to the criterium used in
//           the  optimization.
//=======================================================================
//
void AppDef_Variational::CriteriumWeight(Standard_Real& Percent1,
                                         Standard_Real& Percent2,
                                         Standard_Real& Percent3) const
{
  Percent1 = myPercent[0];
  Percent2 = myPercent[1];
  Percent3 = myPercent[2];
}

//
//=======================================================================
// function : MaxDegree
// purpose  : returns the Maximum Degree used in the approximation
//=======================================================================
//
Standard_Integer AppDef_Variational::MaxDegree() const
{
  return myMaxDegree;
}

//
//=======================================================================
// function : MaxSegment
// purpose  : returns the Maximum of segment used in the approximation
//=======================================================================
//
Standard_Integer AppDef_Variational::MaxSegment() const
{
  return myMaxSegment;
}

//
//=======================================================================
// function : Continuity
// purpose  : returns the Continuity used in the approximation
//=======================================================================
//
GeomAbs_Shape AppDef_Variational::Continuity() const
{
  return myContinuity;
}

//
//=======================================================================
// function : WithMinMax
// purpose  : returns if the  approximation  search to  minimize the
//           maximum Error or not.
//=======================================================================
//
Standard_Boolean AppDef_Variational::WithMinMax() const
{
  return myWithMinMax;
}

//
//=======================================================================
// function : WithCutting
// purpose  :  returns if the  approximation can insert new Knots or not.
//=======================================================================
//
Standard_Boolean AppDef_Variational::WithCutting() const
{
  return myWithCutting;
}

//
//=======================================================================
// function : Tolerance
// purpose  : returns the tolerance used in the approximation.
//=======================================================================
//
Standard_Real AppDef_Variational::Tolerance() const
{
  return myTolerance;
}

//
//=======================================================================
// function : NbIterations
// purpose  : returns the number of iterations used in the approximation.
//=======================================================================
//
Standard_Integer AppDef_Variational::NbIterations() const
{
  return myNbIterations;
}

//
//=======================================================================
// function : Dump
// purpose  : Prints on the stream o information on the current state
//           of the object.
//=======================================================================
//
void AppDef_Variational::Dump(Standard_OStream& o) const
{
  o << " \nVariational Smoothing " << std::endl;
  o << " Number of multipoints                   " << myNbPoints << std::endl;
  o << " Number of 2d par multipoint " << myNbP2d << std::endl;
  o << " Nombre of 3d par multipoint " << myNbP3d << std::endl;
  o << " Number of PassagePoint      " << myNbPassPoints << std::endl;
  o << " Number of TangencyPoints    " << myNbTangPoints << std::endl;
  o << " Number of CurvaturePoints   " << myNbCurvPoints << std::endl;
  o << " \nTolerance " << o.setf(std::ios::scientific) << std::setprecision(3) << std::setw(9)
    << myTolerance;
  if (WithMinMax())
  {
    o << "  as Max Error." << std::endl;
  }
  else
  {
    o << "  as size Error." << std::endl;
  }
  o << "CriteriumWeights : " << myPercent[0] << " , " << myPercent[1] << " , " << myPercent[2]
    << std::endl;

  if (myIsDone)
  {
    o << " MaxError             " << std::setprecision(3) << std::setw(9) << myMaxError
      << std::endl;
    o << " Index of  MaxError   " << myMaxErrorIndex << std::endl;
    o << " Average Error        " << std::setprecision(3) << std::setw(9) << myAverageError
      << std::endl;
    o << " Quadratic Error      " << std::setprecision(3) << std::setw(9) << myCriterium[0]
      << std::endl;
    o << " Tension Criterium    " << std::setprecision(3) << std::setw(9) << myCriterium[1]
      << std::endl;
    o << " Flexion  Criterium   " << std::setprecision(3) << std::setw(9) << myCriterium[2]
      << std::endl;
    o << " Jerk  Criterium      " << std::setprecision(3) << std::setw(9) << myCriterium[3]
      << std::endl;
    o << " NbSegments           " << myKnots->Length() - 1 << std::endl;
  }
  else
  {
    o << (myIsOverConstr ? " The problem is overconstraint" : " Error in approximation")
      << std::endl;
  }
}

//=======================================================================
// function : SetConstraints
// purpose  : Define the constraints to approximate
//           If this value is incompatible with the others fields
//           this method modify nothing and returns false
//=======================================================================
//
Standard_Boolean AppDef_Variational::SetConstraints(
  const Handle(AppParCurves_HArray1OfConstraintCouple)& aConstraint)

{
  myConstraints = aConstraint;
  Init();
  if (myIsOverConstr)
    return Standard_False;
  else
    return Standard_True;
}

//
//=======================================================================
// function : SetParameters
// purpose  : Defines the parameters used by the approximations.
//=======================================================================
//
void AppDef_Variational::SetParameters(const Handle(TColStd_HArray1OfReal)& param)
{
  myParameters->ChangeArray1() = param->Array1();
}

//
//=======================================================================
// function : SetKnots
// purpose  : Defines the knots used by the approximations
//    --          If this value is incompatible with the others fields
//    --          this method modify nothing and returns false
//=======================================================================
//
Standard_Boolean AppDef_Variational::SetKnots(const Handle(TColStd_HArray1OfReal)& knots)
{
  myKnots->ChangeArray1() = knots->Array1();
  return Standard_True;
}

//
//=======================================================================
// function : SetMaxDegree
// purpose  : Define the Maximum Degree used in the approximation
//           If this value is incompatible with the others fields
//           this method modify nothing and returns false
//=======================================================================
//
Standard_Boolean AppDef_Variational::SetMaxDegree(const Standard_Integer Degree)
{
  if (((Degree - myNivCont) * myMaxSegment - myNbPassPoints - 2 * myNbTangPoints
       - 3 * myNbCurvPoints)
      < 0)
    return Standard_False;
  else
  {
    myMaxDegree = Degree;

    InitSmoothCriterion();

    return Standard_True;
  }
}

//
//=======================================================================
// function : SetMaxSegment
// purpose  : Define the maximum number of segments used in the approximation
//           If this value is incompatible with the others fields
//           this method modify nothing and returns false
//=======================================================================
//
Standard_Boolean AppDef_Variational::SetMaxSegment(const Standard_Integer NbSegment)
{
  if (myWithCutting == Standard_True
      && ((myMaxDegree - myNivCont) * NbSegment - myNbPassPoints - 2 * myNbTangPoints
          - 3 * myNbCurvPoints)
           < 0)
    return Standard_False;
  else
  {
    myMaxSegment = NbSegment;
    return Standard_True;
  }
}

//
//=======================================================================
// function : SetContinuity
// purpose  : Define the Continuity used in the approximation
//           If this value is incompatible with the others fields
//           this method modify nothing and returns false
//=======================================================================
//
Standard_Boolean AppDef_Variational::SetContinuity(const GeomAbs_Shape C)
{
  Standard_Integer NivCont = 0;
  switch (C)
  {
    case GeomAbs_C0:
      NivCont = 0;
      break;
    case GeomAbs_C1:
      NivCont = 1;
      break;
    case GeomAbs_C2:
      NivCont = 2;
      break;
    default:
      throw Standard_ConstructionError();
  }
  if (((myMaxDegree - NivCont) * myMaxSegment - myNbPassPoints - 2 * myNbTangPoints
       - 3 * myNbCurvPoints)
      < 0)
    return Standard_False;
  else
  {
    myContinuity = C;
    myNivCont    = NivCont;

    InitSmoothCriterion();
    return Standard_True;
  }
}

//
//=======================================================================
// function : SetWithMinMax
// purpose  : Define if the  approximation  search to  minimize the
//           maximum Error or not.
//=======================================================================
//
void AppDef_Variational::SetWithMinMax(const Standard_Boolean MinMax)
{
  myWithMinMax = MinMax;

  InitSmoothCriterion();
}

//
//=======================================================================
// function : SetWithCutting
// purpose  : Define if the  approximation can insert new Knots or not.
//           If this value is incompatible with the others fields
//           this method modify nothing and returns false
//=======================================================================
//
Standard_Boolean AppDef_Variational::SetWithCutting(const Standard_Boolean Cutting)
{
  if (Cutting == Standard_False)
  {
    if (((myMaxDegree - myNivCont) * myKnots->Length() - myNbPassPoints - 2 * myNbTangPoints
         - 3 * myNbCurvPoints)
        < 0)
      return Standard_False;

    else
    {
      myWithCutting = Cutting;
      InitSmoothCriterion();
      return Standard_True;
    }
  }
  else
  {
    if (((myMaxDegree - myNivCont) * myMaxSegment - myNbPassPoints - 2 * myNbTangPoints
         - 3 * myNbCurvPoints)
        < 0)
      return Standard_False;

    else
    {
      myWithCutting = Cutting;
      InitSmoothCriterion();
      return Standard_True;
    }
  }
}

//
//=======================================================================
// function : SetCriteriumWeight
// purpose  : define the Weights (as percent) associed to the criterium used in
//           the  optimization.
//=======================================================================
//
void AppDef_Variational::SetCriteriumWeight(const Standard_Real Percent1,
                                            const Standard_Real Percent2,
                                            const Standard_Real Percent3)
{
  if (Percent1 < 0 || Percent2 < 0 || Percent3 < 0)
    throw Standard_DomainError();
  Standard_Real Total = Percent1 + Percent2 + Percent3;
  myPercent[0]        = Percent1 / Total;
  myPercent[1]        = Percent2 / Total;
  myPercent[2]        = Percent3 / Total;

  InitSmoothCriterion();
}

//
//=======================================================================
// function : SetCriteriumWeight
// purpose  :  define the  Weight   (as  percent)  associed  to   the
//            criterium   Order used in   the optimization  : Others
//            weights are updated.
//=======================================================================
//
void AppDef_Variational::SetCriteriumWeight(const Standard_Integer Order,
                                            const Standard_Real    Percent)
{
  if (Percent < 0)
    throw Standard_DomainError();
  if (Order < 1 || Order > 3)
    throw Standard_ConstructionError();
  myPercent[Order - 1] = Percent;
  Standard_Real Total  = myPercent[0] + myPercent[1] + myPercent[2];
  myPercent[0]         = myPercent[0] / Total;
  myPercent[1]         = myPercent[1] / Total;
  myPercent[2]         = myPercent[2] / Total;

  InitSmoothCriterion();
}

//
//=======================================================================
// function : SetTolerance
// purpose  : define the tolerance used in the approximation.
//=======================================================================
//
void AppDef_Variational::SetTolerance(const Standard_Real Tol)
{
  myTolerance = Tol;
  InitSmoothCriterion();
}

//
//=======================================================================
// function : SetNbIterations
// purpose  : define the number of iterations used in the approximation.
//=======================================================================
//
void AppDef_Variational::SetNbIterations(const Standard_Integer Iter)
{
  myNbIterations = Iter;
}

//====================== Private Methodes =============================//
//=======================================================================
// function : TheMotor
// purpose  : Smoothing's motor like STRIM routine "MOTLIS"
//=======================================================================
void AppDef_Variational::TheMotor(Handle(AppDef_SmoothCriterion)& J,
                                  //			 const Standard_Real WQuadratic,
                                  const Standard_Real,
                                  const Standard_Real    WQuality,
                                  Handle(FEmTool_Curve)& TheCurve,
                                  TColStd_Array1OfReal&  Ecarts)
{
  // ...

  const Standard_Real BigValue = 1.e37, SmallValue = 1.e-6, SmallestValue = 1.e-9;

  Handle(TColStd_HArray1OfReal)    CurrentTi, NewTi, OldTi;
  Handle(TColStd_HArray2OfInteger) Dependence;
  Standard_Boolean                 lestim, ToOptim, iscut;
  Standard_Boolean                 isnear = Standard_False, again = Standard_True;
  Standard_Integer                 NbEst, ICDANA, NumPnt, Iter;
  Standard_Integer                 MaxNbEst = 5;
  Standard_Real VOCRI[3] = {BigValue, BigValue, BigValue}, EROLD = BigValue, VALCRI[3],
                ERRMAX = BigValue, ERRMOY, ERRQUA;
  Standard_Real         CBLONG, LNOLD;
  Standard_Integer      NbrPnt        = myLastPoint - myFirstPoint + 1;
  Standard_Integer      NbrConstraint = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  Handle(FEmTool_Curve) CCurrent, COld, CNew;
  Standard_Real         EpsLength = SmallValue;
  Standard_Real         EpsDeg;

  Standard_Real    e1, e2, e3;
  Standard_Real    J1min, J2min, J3min;
  Standard_Integer iprog;

  // (0) Init

  J->GetEstimation(e1, e2, e3);
  J1min = 1.e-8;
  J2min = J3min = (e1 + 1.e-8) * 1.e-6;

  if (e1 < J1min)
    e1 = J1min; // Like in
  if (e2 < J2min)
    e2 = J2min; // MOTLIS
  if (e3 < J3min)
    e3 = J3min;

  J->SetEstimation(e1, e2, e3);

  CCurrent                  = TheCurve;
  CurrentTi                 = new TColStd_HArray1OfReal(1, myParameters->Length());
  CurrentTi->ChangeArray1() = myParameters->Array1();
  OldTi                     = new (TColStd_HArray1OfReal)(1, CurrentTi->Length());
  OldTi->ChangeArray1()     = CurrentTi->Array1();
  COld                      = CCurrent;
  LNOLD = CBLONG = J->EstLength();
  Dependence     = J->DependenceTable();

  J->SetCurve(CCurrent);
  FEmTool_Assembly* TheAssembly = new FEmTool_Assembly(Dependence->Array2(), J->AssemblyTable());

  //============        Optimization      ============================
  //  Standard_Integer inagain = 0;
  while (again)
  {

    // (1) Loop  Optimization / Estimation
    lestim                  = Standard_True;
    Standard_Boolean lconst = Standard_True;
    NbEst                   = 0;

    J->SetCurve(CCurrent);

    while (lestim)
    {

      //     (1.1) Curve's Optimization.
      EpsLength = SmallValue * CBLONG / NbrPnt;
      CNew      = new (
        FEmTool_Curve)(CCurrent->Dimension(), CCurrent->NbElements(), CCurrent->Base(), EpsLength);
      CNew->Knots() = CCurrent->Knots();

      J->SetParameters(CurrentTi);
      EpsDeg = Min(WQuality * .1, CBLONG * .001);

      Optimization(J, *TheAssembly, lconst, EpsDeg, CNew, CurrentTi->Array1());

      lconst = Standard_False;

      //        (1.2) calcul des criteres de qualites et amelioration
      //              des estimation.
      ICDANA = J->QualityValues(J1min, J2min, J3min, VALCRI[0], VALCRI[1], VALCRI[2]);

      if (ICDANA > 0)
        lconst = Standard_True;

      J->ErrorValues(ERRMAX, ERRQUA, ERRMOY);

      isnear = ((Sqrt(ERRQUA / NbrPnt) < 2 * WQuality) && (myNbIterations > 1));

      //       (1.3) Optimisation des ti par proj orthogonale
      //             et calcul de l'erreur aux points.

      if (isnear)
      {
        NewTi = new (TColStd_HArray1OfReal)(1, CurrentTi->Length());
        Project(CNew,
                CurrentTi->Array1(),
                NewTi->ChangeArray1(),
                Ecarts,
                NumPnt,
                ERRMAX,
                ERRQUA,
                ERRMOY,
                2);
      }
      else
        NewTi = CurrentTi;

      //        (1.4) Progression's test
      iprog = 0;
      if ((EROLD > WQuality) && (ERRMAX < 0.95 * EROLD))
        iprog++;
      if ((EROLD > WQuality) && (ERRMAX < 0.8 * EROLD))
        iprog++;
      if ((EROLD > WQuality) && (ERRMAX < WQuality))
        iprog++;
      if ((EROLD > WQuality) && (ERRMAX < 0.99 * EROLD) && (ERRMAX < 1.1 * WQuality))
        iprog++;
      if (VALCRI[0] < 0.975 * VOCRI[0])
        iprog++;
      if (VALCRI[0] < 0.9 * VOCRI[0])
        iprog++;
      if (VALCRI[1] < 0.95 * VOCRI[1])
        iprog++;
      if (VALCRI[1] < 0.8 * VOCRI[1])
        iprog++;
      if (VALCRI[2] < 0.95 * VOCRI[2])
        iprog++;
      if (VALCRI[2] < 0.8 * VOCRI[2])
        iprog++;
      if ((VOCRI[1] > SmallestValue) && (VOCRI[2] > SmallestValue))
      {
        if ((VALCRI[1] / VOCRI[1] + 2 * VALCRI[2] / VOCRI[2]) < 2.8)
          iprog++;
      }

      if (iprog < 2 && NbEst == 0)
      {
        //             (1.5) Invalidation of new knots.
        VALCRI[0] = VOCRI[0];
        VALCRI[1] = VOCRI[1];
        VALCRI[2] = VOCRI[2];
        ERRMAX    = EROLD;
        CBLONG    = LNOLD;
        CCurrent  = COld;
        CurrentTi = OldTi;

        goto L8000; // exit
      }

      VOCRI[0] = VALCRI[0];
      VOCRI[1] = VALCRI[1];
      VOCRI[2] = VALCRI[2];
      LNOLD    = CBLONG;
      EROLD    = ERRMAX;

      CCurrent  = CNew;
      CurrentTi = NewTi;

      //       (1.6) Test if the Estimations seems  OK, else repeat
      NbEst++;
      lestim = ((NbEst < MaxNbEst) && (ICDANA == 2) && (iprog > 0));

      if (lestim && isnear)
      {
        //           (1.7) Optimization of ti by ACR.

        std::stable_sort(CurrentTi->begin(), CurrentTi->end());

        Standard_Integer Decima = 4;

        CCurrent->Length(0., 1., CBLONG);
        J->EstLength() = CBLONG;

        ACR(CCurrent, CurrentTi->ChangeArray1(), Decima);
        lconst = Standard_True;
      }
    }

    //     (2) loop of parametric / geometric optimization

    Iter    = 1;
    ToOptim = ((Iter < myNbIterations) && (isnear));

    while (ToOptim)
    {
      Iter++;
      //     (2.1) Save current results
      VOCRI[0]              = VALCRI[0];
      VOCRI[1]              = VALCRI[1];
      VOCRI[2]              = VALCRI[2];
      EROLD                 = ERRMAX;
      LNOLD                 = CBLONG;
      COld                  = CCurrent;
      OldTi->ChangeArray1() = CurrentTi->Array1();

      //     (2.2) Optimization des ti by ACR.

      std::stable_sort(CurrentTi->begin(), CurrentTi->end());

      Standard_Integer Decima = 4;

      CCurrent->Length(0., 1., CBLONG);
      J->EstLength() = CBLONG;

      ACR(CCurrent, CurrentTi->ChangeArray1(), Decima);
      lconst = Standard_True;

      //      (2.3) Optimisation des courbes
      EpsLength = SmallValue * CBLONG / NbrPnt;

      CNew = new (
        FEmTool_Curve)(CCurrent->Dimension(), CCurrent->NbElements(), CCurrent->Base(), EpsLength);
      CNew->Knots() = CCurrent->Knots();

      J->SetParameters(CurrentTi);

      EpsDeg = Min(WQuality * .1, CBLONG * .001);
      Optimization(J, *TheAssembly, lconst, EpsDeg, CNew, CurrentTi->Array1());

      CCurrent = CNew;

      //      (2.4) calcul des criteres de qualites et amelioration
      //             des estimation.

      ICDANA = J->QualityValues(J1min, J2min, J3min, VALCRI[0], VALCRI[1], VALCRI[2]);
      if (ICDANA > 0)
        lconst = Standard_True;

      J->GetEstimation(e1, e2, e3);
      //       (2.5) Optimisation des ti par proj orthogonale

      NewTi = new (TColStd_HArray1OfReal)(1, CurrentTi->Length());
      Project(CCurrent,
              CurrentTi->Array1(),
              NewTi->ChangeArray1(),
              Ecarts,
              NumPnt,
              ERRMAX,
              ERRQUA,
              ERRMOY,
              2);

      //       (2.6)  Test de non regression

      Standard_Integer iregre = 0;
      if (NbrConstraint < NbrPnt)
      {
        if ((ERRMAX > WQuality) && (ERRMAX > 1.05 * EROLD))
          iregre++;
        if ((ERRMAX > WQuality) && (ERRMAX > 2 * EROLD))
          iregre++;
        if ((EROLD > WQuality) && (ERRMAX <= 0.5 * EROLD))
          iregre--;
      }
      Standard_Real E1, E2, E3;
      J->GetEstimation(E1, E2, E3);
      if ((VALCRI[0] > E1) && (VALCRI[0] > 1.1 * VOCRI[0]))
        iregre++;
      if ((VALCRI[1] > E2) && (VALCRI[1] > 1.1 * VOCRI[1]))
        iregre++;
      if ((VALCRI[2] > E3) && (VALCRI[2] > 1.1 * VOCRI[2]))
        iregre++;

      if (iregre >= 2)
      {
        //      if (iregre >= 1) {
        // (2.7) on restaure l'iteration precedente
        VALCRI[0]                 = VOCRI[0];
        VALCRI[1]                 = VOCRI[1];
        VALCRI[2]                 = VOCRI[2];
        ERRMAX                    = EROLD;
        CBLONG                    = LNOLD;
        CCurrent                  = COld;
        CurrentTi->ChangeArray1() = OldTi->Array1();
        ToOptim                   = Standard_False;
      }
      else
      { // Iteration is Ok.
        CCurrent  = CNew;
        CurrentTi = NewTi;
      }
      if (Iter >= myNbIterations)
        ToOptim = Standard_False;
    }

    // (3) Decoupe eventuelle

    if ((CCurrent->NbElements() < myMaxSegment) && myWithCutting)
    {

      //    (3.1) Sauvgarde de l'etat precedent
      VOCRI[0]              = VALCRI[0];
      VOCRI[1]              = VALCRI[1];
      VOCRI[2]              = VALCRI[2];
      EROLD                 = ERRMAX;
      COld                  = CCurrent;
      OldTi->ChangeArray1() = CurrentTi->Array1();

      //       (3.2) On arrange les ti : Trie + recadrage sur (0,1)
      //         ---> On trie, afin d'assurer l'ordre par la suite.

      std::stable_sort(CurrentTi->begin(), CurrentTi->end());

      if ((CurrentTi->Value(1) != 0.) || (CurrentTi->Value(NbrPnt) != 1.))
      {
        Standard_Real t, DelatT = 1.0 / (CurrentTi->Value(NbrPnt) - CurrentTi->Value(1));
        for (Standard_Integer ii = 2; ii < NbrPnt; ii++)
        {
          t = (CurrentTi->Value(ii) - CurrentTi->Value(1)) * DelatT;
          CurrentTi->SetValue(ii, t);
        }
        CurrentTi->SetValue(1, 0.);
        CurrentTi->SetValue(NbrPnt, 1.);
      }

      //       (3.3) Insert new Knots

      SplitCurve(CCurrent, CurrentTi->Array1(), EpsLength, CNew, iscut);
      if (!iscut)
        again = Standard_False;
      else
      {
        CCurrent = CNew;
        // New Knots => New Assembly.
        J->SetCurve(CNew);
        delete TheAssembly;
        TheAssembly = new FEmTool_Assembly(Dependence->Array2(), J->AssemblyTable());
      }
    }
    else
    {
      again = Standard_False;
    }
  }

  //    ================   Great loop end   ===================

L8000:

  // (4) Compute the best Error.
  NewTi = new (TColStd_HArray1OfReal)(1, CurrentTi->Length());
  Project(CCurrent,
          CurrentTi->Array1(),
          NewTi->ChangeArray1(),
          Ecarts,
          NumPnt,
          ERRMAX,
          ERRQUA,
          ERRMOY,
          10);

  // (5) field's update

  TheCurve                     = CCurrent;
  J->EstLength()               = CBLONG;
  myParameters->ChangeArray1() = NewTi->Array1();
  myCriterium[0]               = ERRQUA;
  myCriterium[1]               = Sqrt(VALCRI[0]);
  myCriterium[2]               = Sqrt(VALCRI[1]);
  myCriterium[3]               = Sqrt(VALCRI[2]);
  myMaxError                   = ERRMAX;
  myMaxErrorIndex              = NumPnt;
  if (NbrPnt > NbrConstraint)
    myAverageError = ERRMOY / (NbrPnt - NbrConstraint);
  else
    myAverageError = ERRMOY / NbrConstraint;

  delete TheAssembly;
}

//=======================================================================
// function : Optimization
// purpose  :  (like FORTRAN subroutine MINIMI)
//=======================================================================
void AppDef_Variational::Optimization(Handle(AppDef_SmoothCriterion)& J,
                                      FEmTool_Assembly&               A,
                                      const Standard_Boolean          ToAssemble,
                                      const Standard_Real             EpsDeg,
                                      Handle(FEmTool_Curve)&          Curve,
                                      const TColStd_Array1OfReal&     Parameters) const
{
  Standard_Integer MxDeg = Curve->Base()->WorkDegree(), NbElm = Curve->NbElements(),
                   NbDim = Curve->Dimension();

  Handle(FEmTool_HAssemblyTable) AssTable;

  math_Matrix H(0, MxDeg, 0, MxDeg);
  math_Vector G(0, MxDeg), Sol(1, A.NbGlobVar());

  Standard_Integer el, dim;

  A.GetAssemblyTable(AssTable);
  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;

  Standard_Real CBLONG = J->EstLength();

  // Updating Assembly
  if (ToAssemble)
    A.NullifyMatrix();
  A.NullifyVector();

  for (el = 1; el <= NbElm; el++)
  {
    if (ToAssemble)
    {
      J->Hessian(el, 1, 1, H);
      for (dim = 1; dim <= NbDim; dim++)
        A.AddMatrix(el, dim, dim, H);
    }

    for (dim = 1; dim <= NbDim; dim++)
    {
      J->Gradient(el, dim, G);
      A.AddVector(el, dim, G);
    }
  }

  // Solution of system
  if (ToAssemble)
  {
    if (NbConstr != 0)
    { // Treatment of constraints
      AssemblingConstraints(Curve, Parameters, CBLONG, A);
    }
    A.Solve();
  }
  A.Solution(Sol);

  // Updating J
  J->SetCurve(Curve);
  J->InputVector(Sol, AssTable);

  // Updating Curve and reduction of degree

  Standard_Integer Newdeg;
  Standard_Real    MaxError;

  if (NbConstr == 0)
  {
    for (el = 1; el <= NbElm; el++)
    {
      Curve->ReduceDegree(el, EpsDeg, Newdeg, MaxError);
    }
  }
  else
  {

    TColStd_Array1OfReal& TabInt = Curve->Knots();
    Standard_Integer      Icnt = 1, p0 = Parameters.Lower() - myFirstPoint, point;
    for (el = 1; el <= NbElm; el++)
    {
      while ((Icnt < NbConstr)
             && (Parameters(p0 + myTypConstraints->Value(2 * Icnt - 1)) <= TabInt(el)))
        Icnt++;
      point = p0 + myTypConstraints->Value(2 * Icnt - 1);
      if (Parameters(point) <= TabInt(el) || Parameters(point) >= TabInt(el + 1))
        Curve->ReduceDegree(el, EpsDeg, Newdeg, MaxError);
      else if (Curve->Degree(el) < MxDeg)
        Curve->SetDegree(el, MxDeg);
    }
  }
}

void AppDef_Variational::Project(const Handle(FEmTool_Curve)& C,
                                 const TColStd_Array1OfReal&  Ti,
                                 TColStd_Array1OfReal&        ProjTi,
                                 TColStd_Array1OfReal&        Distance,
                                 Standard_Integer&            NumPoints,
                                 Standard_Real&               MaxErr,
                                 Standard_Real&               QuaErr,
                                 Standard_Real&               AveErr,
                                 const Standard_Integer       NbIterations) const

{
  // Initialisation

  const Standard_Real Seuil = 1.e-9, Eps = 1.e-12;

  MaxErr = QuaErr = AveErr = 0.;

  Standard_Integer Ipnt, NItCv, Iter, i, i0 = -myDimension, d0 = Distance.Lower() - 1;

  Standard_Real TNew, Dist, T0, Dist0, F1, F2, Aux, DF, Ecart;

  Standard_Boolean EnCour;

  TColStd_Array1OfReal ValOfC(1, myDimension), FirstDerOfC(1, myDimension),
    SecndDerOfC(1, myDimension);

  for (Ipnt = 1; Ipnt <= ProjTi.Length(); Ipnt++)
  {

    i0 += myDimension;

    TNew = Ti(Ipnt);

    EnCour = Standard_True;
    NItCv  = 0;
    Iter   = 0;
    C->D0(TNew, ValOfC);

    Dist = 0;
    for (i = 1; i <= myDimension; i++)
    {
      Aux = ValOfC(i) - myTabPoints->Value(i0 + i);
      Dist += Aux * Aux;
    }
    Dist = Sqrt(Dist);

    // ------- Newton's method for solving (C'(t),C(t) - P) = 0

    while (EnCour)
    {

      Iter++;
      T0    = TNew;
      Dist0 = Dist;

      C->D2(TNew, SecndDerOfC);
      C->D1(TNew, FirstDerOfC);

      F1 = F2 = 0.;
      for (i = 1; i <= myDimension; i++)
      {
        Aux = ValOfC(i) - myTabPoints->Value(i0 + i);
        DF  = FirstDerOfC(i);
        F1 += Aux * DF;                       // (C'(t),C(t) - P)
        F2 += DF * DF + Aux * SecndDerOfC(i); // ((C'(t),C(t) - P))'
      }

      if (Abs(F2) < Eps)
        EnCour = Standard_False;
      else
      {
        // Formula of Newton x(k+1) = x(k) - F(x(k))/F'(x(k))
        TNew -= F1 / F2;
        if (TNew < 0.)
          TNew = 0.;
        if (TNew > 1.)
          TNew = 1.;

        // Analysis of result

        C->D0(TNew, ValOfC);

        Dist = 0;
        for (i = 1; i <= myDimension; i++)
        {
          Aux = ValOfC(i) - myTabPoints->Value(i0 + i);
          Dist += Aux * Aux;
        }
        Dist = Sqrt(Dist);

        Ecart = Dist0 - Dist;

        if (Ecart <= -Seuil)
        {
          // Pas d'amelioration on s'arrete
          EnCour = Standard_False;
          TNew   = T0;
          Dist   = Dist0;
        }
        else if (Ecart <= Seuil)
          // Convergence
          NItCv++;
        else
          NItCv = 0;

        if ((NItCv >= 2) || (Iter >= NbIterations))
          EnCour = Standard_False;
      }
    }

    ProjTi(Ipnt)        = TNew;
    Distance(d0 + Ipnt) = Dist;
    if (Dist > MaxErr)
    {
      MaxErr    = Dist;
      NumPoints = Ipnt;
    }
    QuaErr += Dist * Dist;
    AveErr += Dist;
  }

  // clang-format off
  NumPoints = NumPoints + myFirstPoint - 1;// Setting NumPoints to interval [myFirstPoint, myLastPoint]
  // clang-format on
}

void AppDef_Variational::ACR(Handle(FEmTool_Curve)& Curve,
                             TColStd_Array1OfReal&  Ti,
                             const Standard_Integer Decima) const
{

  const Standard_Real Eps = 1.e-8;

  TColStd_Array1OfReal& Knots  = Curve->Knots();
  Standard_Integer      NbrPnt = Ti.Length(), TiFirst = Ti.Lower(), TiLast = Ti.Upper(),
                   KFirst = Knots.Lower(), KLast = Knots.Upper();

  Standard_Real    CbLong, DeltaT, VTest, UNew, UOld, DU, TPara, TOld, DTInv, Ratio;
  Standard_Integer ipnt, ii, IElm, IOld, POld, PCnt, ICnt = 0;
  Standard_Integer NbCntr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;

  //     (1) Calcul de la longueur de courbe

  Curve->Length(Ti(TiFirst), Ti(TiLast), CbLong);

  //     (2)  Mise de l'acr dans Ti

  if (NbrPnt >= 2)
  {

    //     (2.0) Initialisation
    DeltaT = (Ti(TiLast) - Ti(TiFirst)) / Decima;
    VTest  = Ti(TiFirst) + DeltaT;

    if (NbCntr > 0)
    {
      PCnt = myTypConstraints->Value(1) - myFirstPoint + TiFirst;
      ICnt = 1;
    }
    else
      PCnt = TiLast + 1;

    UOld = 0.;

    TOld = Ti(TiFirst);
    POld = TiFirst;

    IElm = KFirst;
    IOld = IElm;

    Ti(TiFirst) = 0.;

    for (ipnt = TiFirst + 1; ipnt <= TiLast; ipnt++)
    {

      while ((ICnt <= NbCntr) && (PCnt < ipnt))
      {
        ICnt++;
        PCnt = myTypConstraints->Value(2 * ICnt - 1) - myFirstPoint + TiFirst;
      }

      TPara = Ti(ipnt);

      if (TPara >= VTest || PCnt == ipnt)
      {

        if (Ti(TiLast) - TPara <= 1.e-2 * DeltaT)
        {
          ipnt  = TiLast;
          TPara = Ti(ipnt);
        }
        //        (2.2), (2.3) Cacul la longueur de courbe
        Curve->Length(Ti(TiFirst), TPara, UNew);

        UNew /= CbLong;

        while (Knots(IElm + 1) < TPara && IElm < KLast - 1)
          IElm++;

        //         (2.4) Mise a jours des parametres de decoupe
        DTInv = 1. / (TPara - TOld);
        DU    = UNew - UOld;

        for (ii = IOld + 1; ii <= IElm; ii++)
        {
          Ratio     = (Knots(ii) - TOld) * DTInv;
          Knots(ii) = UOld + Ratio * DU;
        }

        //           (2.5) Mise a jours des parametres de points.

        // Very strange loop, because it never works (POld+1 > ipnt-1)
        for (ii = POld + 1; ii <= ipnt - 1; ii++)
        {
          Ratio  = (Ti(ii) - TOld) * DTInv;
          Ti(ii) = UOld + Ratio * DU;
        }

        Ti(ipnt) = UNew;

        UOld = UNew;
        IOld = IElm;
        TOld = TPara;
        POld = ipnt;
      }
      //        --> Nouveau seuil parametrique pour le decimage

      if (TPara >= VTest)
      {
        //	ii = RealToInt((TPara - VTest + Eps) / DeltaT);
        //	VTest += (ii + 1) * DeltaT;
        VTest += Ceiling((TPara - VTest + Eps) / DeltaT) * DeltaT;
        if (VTest > 1. - Eps)
          VTest = 1.;
      }
    }
  }

  //     --- On ajuste les valeurs extremes

  Ti(TiFirst) = 0.;
  Ti(TiLast)  = 1.;
  ii          = TiLast - 1;
  while (Ti(ii) > Knots(KLast))
  {
    Ti(ii) = 1.;
    --ii;
  }
  Knots(KFirst) = 0.;
  Knots(KLast)  = 1.;
}

//----------------------------------------------------------//
// Standard_Integer NearIndex                               //
// Purpose: searching nearest index of TabPar corresponding //
// given value T (is similar to fortran routine MSRRE2)     //
//----------------------------------------------------------//

static Standard_Integer NearIndex(const Standard_Real         T,
                                  const TColStd_Array1OfReal& TabPar,
                                  const Standard_Real         Eps,
                                  Standard_Integer&           Flag)
{
  Standard_Integer Loi = TabPar.Lower(), Upi = TabPar.Upper();

  Flag = 0;

  if (T < TabPar(Loi))
  {
    Flag = -1;
    return Loi;
  }
  if (T > TabPar(Upi))
  {
    Flag = 1;
    return Upi;
  }

  Standard_Integer Ibeg = Loi, Ifin = Upi, Imidl;

  while (Ibeg + 1 != Ifin)
  {
    Imidl = (Ibeg + Ifin) / 2;
    if ((T >= TabPar(Ibeg)) && (T <= TabPar(Imidl)))
      Ifin = Imidl;
    else
      Ibeg = Imidl;
  }

  if (Abs(T - TabPar(Ifin)) < Eps)
    return Ifin;

  return Ibeg;
}

//----------------------------------------------------------//
// void GettingKnots                                        //
// Purpose: calculating values of new Knots for elements    //
// with degree that is equal Deg                            //
//----------------------------------------------------------//

static void GettingKnots(const TColStd_Array1OfReal&  TabPar,
                         const Handle(FEmTool_Curve)& InCurve,
                         const Standard_Integer       Deg,
                         Standard_Integer&            NbElm,
                         TColStd_Array1OfReal&        NewKnots)

{

  const Standard_Real Eps = 1.e-12;

  TColStd_Array1OfReal& OldKnots = InCurve->Knots();
  Standard_Integer      NbMaxOld = InCurve->NbElements();
  Standard_Integer      NbMax    = NewKnots.Upper(), Ipt, Ipt1, Ipt2;
  Standard_Integer      el = 0, i1 = OldKnots.Lower(), i0 = i1 - 1, Flag;
  Standard_Real         TPar;

  while ((NbElm < NbMax) && (el < NbMaxOld))
  {

    el++;
    i0++;
    i1++; // i0, i1 are indexes of left and right knots of element el

    if (InCurve->Degree(el) == Deg)
    {

      NbElm++;

      Ipt1 = NearIndex(OldKnots(i0), TabPar, Eps, Flag);
      if (Flag != 0)
        Ipt1 = TabPar.Lower();
      Ipt2 = NearIndex(OldKnots(i1), TabPar, Eps, Flag);
      if (Flag != 0)
        Ipt2 = TabPar.Upper();

      if (Ipt2 - Ipt1 >= 1)
      {

        Ipt = (Ipt1 + Ipt2) / 2;
        if (2 * Ipt == Ipt1 + Ipt2)
          TPar = 2. * TabPar(Ipt);
        else
          TPar = TabPar(Ipt) + TabPar(Ipt + 1);

        NewKnots(NbElm) = (OldKnots(i0) + OldKnots(i1) + TPar) / 4.;
      }
      else
        NewKnots(NbElm) = (OldKnots(i0) + OldKnots(i1)) / 2.;
    }
  }
}

void AppDef_Variational::SplitCurve(const Handle(FEmTool_Curve)& InCurve,
                                    const TColStd_Array1OfReal&  Ti,
                                    const Standard_Real          CurveTol,
                                    Handle(FEmTool_Curve)&       OutCurve,
                                    Standard_Boolean&            iscut) const
{
  Standard_Integer NbElmOld = InCurve->NbElements();

  if (NbElmOld >= myMaxSegment)
  {
    iscut = Standard_False;
    return;
  }
#ifdef OCCT_DEBUG
  Standard_Integer MaxDegree =
#endif
    InCurve->Base()->WorkDegree();
  Standard_Integer     NbElm = NbElmOld;
  TColStd_Array1OfReal NewKnots(NbElm + 1, myMaxSegment);
#ifndef OCCT_DEBUG
  GettingKnots(Ti, InCurve, InCurve->Base()->WorkDegree(), NbElm, NewKnots);
  GettingKnots(Ti, InCurve, InCurve->Base()->WorkDegree() - 1, NbElm, NewKnots);
#else
  GettingKnots(Ti, InCurve, MaxDegree, NbElm, NewKnots);
  GettingKnots(Ti, InCurve, MaxDegree - 1, NbElm, NewKnots);

#endif
  if (NbElm > NbElmOld)
  {

    iscut = Standard_True;

    OutCurve = new FEmTool_Curve(InCurve->Dimension(), NbElm, InCurve->Base(), CurveTol);
    TColStd_Array1OfReal& OutKnots = OutCurve->Knots();
    TColStd_Array1OfReal& InKnots  = InCurve->Knots();

    Standard_Integer i, i0 = OutKnots.Lower();
    for (i = InKnots.Lower(); i <= InKnots.Upper(); i++)
      OutKnots(i) = InKnots(i);
    for (i = NbElmOld + 1; i <= NbElm; i++)
      OutKnots(i + i0) = NewKnots(i);

    std::sort(OutKnots.begin(), OutKnots.end());
  }
  else
    iscut = Standard_False;
}

//=======================================================================
// function : InitSmoothCriterion
// purpose  : Initializes the SmoothCriterion
//=======================================================================
void AppDef_Variational::InitSmoothCriterion()
{

  const Standard_Real Eps2 = 1.e-6, Eps3 = 1.e-9;
  //  const Standard_Real J1 = .01, J2 = .001, J3 = .001;

  Standard_Real Length;

  InitParameters(Length);

  mySmoothCriterion->SetParameters(myParameters);

  Standard_Real E1, E2, E3;

  InitCriterionEstimations(Length, E1, E2, E3);
  /*
  J1 = 1.e-8; J2 = J3 = (E1 + 1.e-8) * 1.e-6;

  if(E1 < J1) E1 = J1;
  if(E2 < J2) E2 = J2;
  if(E3 < J3) E3 = J3;
  */
  mySmoothCriterion->EstLength() = Length;
  mySmoothCriterion->SetEstimation(E1, E2, E3);

  Standard_Real WQuadratic, WQuality;

  if (!myWithMinMax && myTolerance != 0.)
    WQuality = myTolerance;
  else if (myTolerance == 0.)
    WQuality = 1.;
  else
    WQuality = Max(myTolerance, Eps2 * Length);

  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  WQuadratic                = Sqrt((Standard_Real)(myNbPoints - NbConstr)) * WQuality;
  if (WQuadratic > Eps3)
    WQuadratic = 1. / WQuadratic;

  if (WQuadratic == 0.)
    WQuadratic = Max(Sqrt(E1), 1.);

  mySmoothCriterion->SetWeight(WQuadratic, WQuality, myPercent[0], myPercent[1], myPercent[2]);

  Handle(PLib_Base)     TheBase = new PLib_HermitJacobi(myMaxDegree, myContinuity);
  Handle(FEmTool_Curve) TheCurve;
  Standard_Integer      NbElem;
  Standard_Real         CurvTol = Eps2 * Length / myNbPoints;

  // Decoupe de l'intervalle en fonction des contraintes
  if (myWithCutting == Standard_True && NbConstr != 0)
  {

    InitCutting(TheBase, CurvTol, TheCurve);
  }
  else
  {

    NbElem   = 1;
    TheCurve = new FEmTool_Curve(myDimension, NbElem, TheBase, CurvTol);
    TheCurve->Knots().SetValue(TheCurve->Knots().Lower(), myParameters->Value(myFirstPoint));
    TheCurve->Knots().SetValue(TheCurve->Knots().Upper(), myParameters->Value(myLastPoint));
  }

  mySmoothCriterion->SetCurve(TheCurve);

  return;
}

//
//=======================================================================
// function : InitParameters
// purpose  : Calculation of initial estimation of the multicurve's length
//           and parameters for multipoints.
//=======================================================================
//
void AppDef_Variational::InitParameters(Standard_Real& Length)
{

  constexpr Standard_Real Eps1 = Precision::Confusion() * .01;

  Standard_Real    aux, dist;
  Standard_Integer i, i0, i1 = 0, ipoint;

  Length = 0.;
  myParameters->SetValue(myFirstPoint, Length);

  for (ipoint = myFirstPoint + 1; ipoint <= myLastPoint; ipoint++)
  {
    i0 = i1;
    i1 += myDimension;
    dist = 0;
    for (i = 1; i <= myDimension; i++)
    {
      aux = myTabPoints->Value(i1 + i) - myTabPoints->Value(i0 + i);
      dist += aux * aux;
    }
    Length += Sqrt(dist);
    myParameters->SetValue(ipoint, Length);
  }

  if (Length <= Eps1)
    throw Standard_ConstructionError("AppDef_Variational::InitParameters");

  for (ipoint = myFirstPoint + 1; ipoint <= myLastPoint - 1; ipoint++)
    myParameters->SetValue(ipoint, myParameters->Value(ipoint) / Length);

  myParameters->SetValue(myLastPoint, 1.);

  // Avec peu de point il y a sans doute sous estimation ...
  if (myNbPoints < 10)
    Length *= (1. + 0.1 / (myNbPoints - 1));
}

//
//=======================================================================
// function : InitCriterionEstimations
// purpose  : Calculation of initial estimation of the linear criteria
//
//=======================================================================
//
void AppDef_Variational::InitCriterionEstimations(const Standard_Real Length,
                                                  Standard_Real&      E1,
                                                  Standard_Real&      E2,
                                                  Standard_Real&      E3) const
{
  E1 = Length * Length;

  constexpr Standard_Real Eps1 = Precision::Confusion() * .01;

  math_Vector VTang1(1, myDimension), VTang2(1, myDimension), VTang3(1, myDimension),
    VScnd1(1, myDimension), VScnd2(1, myDimension), VScnd3(1, myDimension);

  // ========== Treatment of first point =================

  Standard_Integer ipnt = myFirstPoint;

  EstTangent(ipnt, VTang1);
  ipnt++;
  EstTangent(ipnt, VTang2);
  ipnt++;
  EstTangent(ipnt, VTang3);

  ipnt = myFirstPoint;
  EstSecnd(ipnt, VTang1, VTang2, Length, VScnd1);
  ipnt++;
  EstSecnd(ipnt, VTang1, VTang3, Length, VScnd2);

  //  Modified by skv - Fri Apr  8 14:58:12 2005 OCC8559 Begin
  //   Standard_Real Delta = .5 * (myParameters->Value(ipnt) - myParameters->Value(--ipnt));
  Standard_Integer anInd = ipnt;
  Standard_Real    Delta = .5 * (myParameters->Value(anInd) - myParameters->Value(--ipnt));
  //  Modified by skv - Fri Apr  8 14:58:12 2005 OCC8559 End

  if (Delta <= Eps1)
    Delta = 1.;

  E2 = VScnd1.Norm2() * Delta;

  E3 = (Delta > Eps1) ? VScnd2.Subtracted(VScnd1).Norm2() / (4. * Delta) : 0.;
  // ========== Treatment of internal points =================

  Standard_Integer CurrPoint = 2;

  for (ipnt = myFirstPoint + 1; ipnt < myLastPoint; ipnt++)
  {

    Delta = .5 * (myParameters->Value(ipnt + 1) - myParameters->Value(ipnt - 1));

    if (CurrPoint == 1)
    {
      if (ipnt + 1 != myLastPoint)
      {
        EstTangent(ipnt + 2, VTang3);
        EstSecnd(ipnt + 1, VTang1, VTang3, Length, VScnd2);
      }
      else
        EstSecnd(ipnt + 1, VTang1, VTang2, Length, VScnd2);

      E2 += VScnd1.Norm2() * Delta;
      E3 += (Delta > Eps1) ? VScnd2.Subtracted(VScnd3).Norm2() / (4. * Delta) : 0.;
    }
    else if (CurrPoint == 2)
    {
      if (ipnt + 1 != myLastPoint)
      {
        EstTangent(ipnt + 2, VTang1);
        EstSecnd(ipnt + 1, VTang2, VTang1, Length, VScnd3);
      }
      else
        EstSecnd(ipnt + 1, VTang2, VTang3, Length, VScnd3);

      E2 += VScnd2.Norm2() * Delta;
      E3 += (Delta > Eps1) ? VScnd3.Subtracted(VScnd1).Norm2() / (4. * Delta) : 0.;
    }
    else
    {
      if (ipnt + 1 != myLastPoint)
      {
        EstTangent(ipnt + 2, VTang2);
        EstSecnd(ipnt + 1, VTang3, VTang2, Length, VScnd1);
      }
      else
        EstSecnd(ipnt + 1, VTang3, VTang1, Length, VScnd1);

      E2 += VScnd3.Norm2() * Delta;
      E3 += (Delta > Eps1) ? VScnd1.Subtracted(VScnd2).Norm2() / (4. * Delta) : 0.;
    }

    CurrPoint++;
    if (CurrPoint == 4)
      CurrPoint = 1;
  }

  // ========== Treatment of last point =================

  Delta = .5 * (myParameters->Value(myLastPoint) - myParameters->Value(myLastPoint - 1));
  if (Delta <= Eps1)
    Delta = 1.;

  Standard_Real aux;

  if (CurrPoint == 1)
  {

    E2 += VScnd1.Norm2() * Delta;
    aux = VScnd1.Subtracted(VScnd3).Norm2();
    E3 += (Delta > Eps1) ? aux / (4. * Delta) : aux;
  }
  else if (CurrPoint == 2)
  {

    E2 += VScnd2.Norm2() * Delta;
    aux = VScnd2.Subtracted(VScnd1).Norm2();
    E3 += (Delta > Eps1) ? aux / (4. * Delta) : aux;
  }
  else
  {

    E2 += VScnd3.Norm2() * Delta;
    aux = VScnd3.Subtracted(VScnd2).Norm2();
    E3 += (Delta > Eps1) ? aux / (4. * Delta) : aux;
  }

  aux = Length * Length;

  E2 *= aux;
  E3 *= aux;
}

//
//=======================================================================
// function : EstTangent
// purpose  : Calculation of  estimation of the Tangent
//           (see fortran routine MMLIPRI)
//=======================================================================
//

void AppDef_Variational::EstTangent(const Standard_Integer ipnt, math_Vector& VTang) const

{
  Standard_Integer        i;
  constexpr Standard_Real Eps1    = Precision::Confusion() * .01;
  const Standard_Real     EpsNorm = 1.e-9;

  Standard_Real Wpnt = 1.;

  if (ipnt == myFirstPoint)
  {
    // Estimation at first point
    if (myNbPoints < 3)
      Wpnt = 0.;
    else
    {

      Standard_Integer adr1 = 1, adr2 = adr1 + myDimension, adr3 = adr2 + myDimension;

      math_Vector Pnt1((Standard_Real*)&myTabPoints->Value(adr1), 1, myDimension);
      math_Vector Pnt2((Standard_Real*)&myTabPoints->Value(adr2), 1, myDimension);
      math_Vector Pnt3((Standard_Real*)&myTabPoints->Value(adr3), 1, myDimension);

      // Parabolic interpolation
      // if we have parabolic interpolation: F(t) = A0 + A1*t + A2*t*t,
      // first derivative for t=0 is A1 = ((d2-1)*P1 + P2 - d2*P3)/(d*(1-d))
      //       d= |P2-P1|/(|P2-P1|+|P3-P2|), d2 = d*d
      Standard_Real V1 = Pnt2.Subtracted(Pnt1).Norm();
      Standard_Real V2 = 0.;
      if (V1 > Eps1)
        V2 = Pnt3.Subtracted(Pnt2).Norm();
      if (V2 > Eps1)
      {
        Standard_Real d = V1 / (V1 + V2), d1;
        d1              = 1. / (d * (1 - d));
        d *= d;
        VTang = ((d - 1.) * Pnt1 + Pnt2 - d * Pnt3) * d1;
      }
      else
      {
        // Simple 2-point estimation

        VTang = Pnt2 - Pnt1;
      }
    }
  }
  else if (ipnt == myLastPoint)
  {
    // Estimation at last point
    if (myNbPoints < 3)
      Wpnt = 0.;
    else
    {

      Standard_Integer adr1 = (myLastPoint - 3) * myDimension + 1, adr2 = adr1 + myDimension,
                       adr3 = adr2 + myDimension;

      math_Vector Pnt1((Standard_Real*)&myTabPoints->Value(adr1), 1, myDimension);
      math_Vector Pnt2((Standard_Real*)&myTabPoints->Value(adr2), 1, myDimension);
      math_Vector Pnt3((Standard_Real*)&myTabPoints->Value(adr3), 1, myDimension);

      // Parabolic interpolation
      // if we have parabolic interpolation: F(t) = A0 + A1*t + A2*t*t,
      // first derivative for t=1 is 2*A2 + A1 = ((d2+1)*P1 - P2 - d2*P3)/(d*(1-d))
      //       d= |P2-P1|/(|P2-P1|+|P3-P2|), d2 = d*(d-2)
      Standard_Real V1 = Pnt2.Subtracted(Pnt1).Norm();
      Standard_Real V2 = 0.;
      if (V1 > Eps1)
        V2 = Pnt3.Subtracted(Pnt2).Norm();
      if (V2 > Eps1)
      {
        Standard_Real d = V1 / (V1 + V2), d1;
        d1              = 1. / (d * (1 - d));
        d *= d - 2;
        VTang = ((d + 1.) * Pnt1 - Pnt2 - d * Pnt3) * d1;
      }
      else
      {
        // Simple 2-point estimation

        VTang = Pnt3 - Pnt2;
      }
    }
  }
  else
  {

    Standard_Integer adr1 = (ipnt - myFirstPoint - 1) * myDimension + 1,
                     adr2 = adr1 + 2 * myDimension;

    math_Vector Pnt1((Standard_Real*)&myTabPoints->Value(adr1), 1, myDimension);
    math_Vector Pnt2((Standard_Real*)&myTabPoints->Value(adr2), 1, myDimension);

    VTang = Pnt2 - Pnt1;
  }

  Standard_Real Vnorm = VTang.Norm();

  if (Vnorm <= EpsNorm)
    VTang.Init(0.);
  else
    VTang /= Vnorm;

  // Estimation with constraints

  Standard_Real    Wcnt  = 0.;
  Standard_Integer IdCnt = 1;

  // Warning!! Here it is suppoused that all points are in range [myFirstPoint, myLastPoint]

  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  math_Vector      VCnt(1, myDimension, 0.);

  if (NbConstr > 0)
  {

    while (myTypConstraints->Value(2 * IdCnt - 1) < ipnt && IdCnt <= NbConstr)
      IdCnt++;
    if ((myTypConstraints->Value(2 * IdCnt - 1) == ipnt)
        && (myTypConstraints->Value(2 * IdCnt) >= 1))
    {
      Wcnt                = 1.;
      Standard_Integer i0 = 2 * myDimension * (IdCnt - 1), k = 0;
      for (i = 1; i <= myNbP3d; i++)
      {
        for (Standard_Integer j = 1; j <= 3; j++)
          VCnt(++k) = myTabConstraints->Value(++i0);
        i0 += 3;
      }
      for (i = 1; i <= myNbP2d; i++)
      {
        for (Standard_Integer j = 1; j <= 2; j++)
          VCnt(++k) = myTabConstraints->Value(++i0);
        i0 += 2;
      }
    }
  }

  // Averaging of estimation

  Standard_Real Denom = Wpnt + Wcnt;
  if (Denom == 0.)
    Denom = 1.;
  else
    Denom = 1. / Denom;

  VTang = (Wpnt * VTang + Wcnt * VCnt) * Denom;

  Vnorm = VTang.Norm();

  if (Vnorm <= EpsNorm)
    VTang.Init(0.);
  else
    VTang /= Vnorm;
}

//
//=======================================================================
// function : EstSecnd
// purpose  : Calculation of  estimation of the second derivative
//           (see fortran routine MLIMSCN)
//=======================================================================
//
void AppDef_Variational::EstSecnd(const Standard_Integer ipnt,
                                  const math_Vector&     VTang1,
                                  const math_Vector&     VTang2,
                                  const Standard_Real    Length,
                                  math_Vector&           VScnd) const
{
  Standard_Integer i;

  const Standard_Real Eps = 1.e-9;

  Standard_Real Wpnt = 1.;

  Standard_Real aux;

  if (ipnt == myFirstPoint)
    aux = myParameters->Value(ipnt + 1) - myParameters->Value(ipnt);
  else if (ipnt == myLastPoint)
    aux = myParameters->Value(ipnt) - myParameters->Value(ipnt - 1);
  else
    aux = myParameters->Value(ipnt + 1) - myParameters->Value(ipnt - 1);

  if (aux <= Eps)
    aux = 1.;
  else
    aux = 1. / aux;

  VScnd = (VTang2 - VTang1) * aux;

  // Estimation with constraints

  Standard_Real    Wcnt  = 0.;
  Standard_Integer IdCnt = 1;

  // Warning!! Here it is suppoused that all points are in range [myFirstPoint, myLastPoint]

  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  math_Vector      VCnt(1, myDimension, 0.);

  if (NbConstr > 0)
  {

    while (myTypConstraints->Value(2 * IdCnt - 1) < ipnt && IdCnt <= NbConstr)
      IdCnt++;

    if ((myTypConstraints->Value(2 * IdCnt - 1) == ipnt)
        && (myTypConstraints->Value(2 * IdCnt) >= 2))
    {
      Wcnt                = 1.;
      Standard_Integer i0 = 2 * myDimension * (IdCnt - 1) + 3, k = 0;
      for (i = 1; i <= myNbP3d; i++)
      {
        for (Standard_Integer j = 1; j <= 3; j++)
          VCnt(++k) = myTabConstraints->Value(++i0);
        i0 += 3;
      }
      i0--;
      for (i = 1; i <= myNbP2d; i++)
      {
        for (Standard_Integer j = 1; j <= 2; j++)
          VCnt(++k) = myTabConstraints->Value(++i0);
        i0 += 2;
      }
    }
  }

  // Averaging of estimation

  Standard_Real Denom = Wpnt + Wcnt;
  if (Denom == 0.)
    Denom = 1.;
  else
    Denom = 1. / Denom;

  VScnd = (Wpnt * VScnd + (Wcnt * Length) * VCnt) * Denom;
}

//
//=======================================================================
// function : InitCutting
// purpose  : Realisation of curve's cutting a priory accordingly to
//           constraints (see fortran routine MLICUT)
//=======================================================================
//
void AppDef_Variational::InitCutting(const Handle(PLib_Base)& aBase,
                                     const Standard_Real      CurvTol,
                                     Handle(FEmTool_Curve)&   aCurve) const
{

  // Definition of number of elements
  Standard_Integer ORCMx = -1, NCont = 0, i, kk, NbElem;
  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;

  for (i = 1; i <= NbConstr; i++)
  {
    kk    = Abs(myTypConstraints->Value(2 * i)) + 1;
    ORCMx = Max(ORCMx, kk);
    NCont += kk;
  }

  if (ORCMx > myMaxDegree - myNivCont)
    throw Standard_ConstructionError("AppDef_Variational::InitCutting");

  Standard_Integer NLibre = Max(myMaxDegree - myNivCont - (myMaxDegree + 1) / 4, myNivCont + 1);

  NbElem = (NCont % NLibre == 0) ? NCont / NLibre : NCont / NLibre + 1;

  while ((NbElem > myMaxSegment) && (NLibre < myMaxDegree - myNivCont))
  {

    NLibre++;
    NbElem = (NCont % NLibre == 0) ? NCont / NLibre : NCont / NLibre + 1;
  }

  if (NbElem > myMaxSegment)
    throw Standard_ConstructionError("AppDef_Variational::InitCutting");

  aCurve = new FEmTool_Curve(myDimension, NbElem, aBase, CurvTol);

  Standard_Integer NCnt  = (NCont - 1) / NbElem + 1;
  Standard_Integer NPlus = NbElem - (NCnt * NbElem - NCont);

  TColStd_Array1OfReal& Knot = aCurve->Knots();

  Standard_Integer IDeb = 0, IFin = NbConstr + 1, NDeb = 0, NFin = 0, IndEl = Knot.Lower(),
                   IUpper = Knot.Upper(), NbEl = 0;

  Knot(IndEl)  = myParameters->Value(myFirstPoint);
  Knot(IUpper) = myParameters->Value(myLastPoint);

  while (NbElem - NbEl > 1)
  {

    IndEl++;
    NbEl++;
    if (NPlus == 0)
      NCnt--;

    while (NDeb < NCnt && IDeb < IFin)
    {
      IDeb++;
      NDeb += Abs(myTypConstraints->Value(2 * IDeb)) + 1;
    }

    if (NDeb == NCnt)
    {
      NDeb = 0;
      if (NPlus == 1
          && myParameters->Value(myTypConstraints->Value(2 * IDeb - 1)) > Knot(IndEl - 1))

        Knot(IndEl) = myParameters->Value(myTypConstraints->Value(2 * IDeb - 1));
      else
        Knot(IndEl) = (myParameters->Value(myTypConstraints->Value(2 * IDeb - 1))
                       + myParameters->Value(myTypConstraints->Value(2 * IDeb + 1)))
                      / 2;
    }
    else
    {
      NDeb -= NCnt;
      Knot(IndEl) = myParameters->Value(myTypConstraints->Value(2 * IDeb - 1));
    }

    NPlus--;
    if (NPlus == 0)
      NCnt--;

    if (NbElem - NbEl == 1)
      break;

    NbEl++;

    while (NFin < NCnt && IDeb < IFin)
    {
      IFin--;
      NFin += Abs(myTypConstraints->Value(2 * IFin)) + 1;
    }

    if (NFin == NCnt)
    {
      NFin                     = 0;
      Knot(IUpper + 1 - IndEl) = (myParameters->Value(myTypConstraints->Value(2 * IFin - 1))
                                  + myParameters->Value(myTypConstraints->Value(2 * IFin - 3)))
                                 / 2;
    }
    else
    {
      NFin -= NCnt;
      if (myParameters->Value(myTypConstraints->Value(2 * IFin - 1)) < Knot(IUpper - IndEl + 1))
        Knot(IUpper + 1 - IndEl) = myParameters->Value(myTypConstraints->Value(2 * IFin - 1));
      else
        Knot(IUpper + 1 - IndEl) = (myParameters->Value(myTypConstraints->Value(2 * IFin - 1))
                                    + myParameters->Value(myTypConstraints->Value(2 * IFin - 3)))
                                   / 2;
    }
  }
}

//=======================================================================
// function : Adjusting
// purpose  : Smoothing's  adjusting like STRIM routine "MAJLIS"
//=======================================================================
void AppDef_Variational::Adjusting(Handle(AppDef_SmoothCriterion)& J,
                                   Standard_Real&                  WQuadratic,
                                   Standard_Real&                  WQuality,
                                   Handle(FEmTool_Curve)&          TheCurve,
                                   TColStd_Array1OfReal&           Ecarts)
{

  //  std::cout << "=========== Adjusting =============" << std::endl;

  /* Initialized data */

  const Standard_Integer mxiter        = 2;
  const Standard_Real    eps1          = 1e-6;
  Standard_Integer       NbrPnt        = myLastPoint - myFirstPoint + 1;
  Standard_Integer       NbrConstraint = myNbPassPoints + myNbTangPoints + myNbCurvPoints;
  Standard_Real          CurvTol       = eps1 * J->EstLength() / NbrPnt;

  /* Local variables */
  Standard_Integer               iter, ipnt;
  Standard_Real                  ecart, emold, erold, tpara;
  Standard_Real                  vocri[4], j1cibl, vtest, vseuil;
  Standard_Integer               i, numint, flag;
  TColStd_Array1OfReal           tbpoid(myFirstPoint, myLastPoint);
  Standard_Boolean               loptim, lrejet;
  Handle(AppDef_SmoothCriterion) JNew;
  Handle(FEmTool_Curve)          CNew;
  Standard_Real                  E1, E2, E3;

  /* (0.b) Initialisations */

  loptim = Standard_True;
  iter   = 0;
  tbpoid.Init(1.);

  /* ============   boucle sur le moteur de lissage  ============== */

  vtest  = WQuality * .9;
  j1cibl = Sqrt(myCriterium[0] / (NbrPnt - NbrConstraint));

  while (loptim)
  {

    ++iter;

    /*     (1) Sauvegarde de l'etat precedents */

    vocri[0] = myCriterium[0];
    vocri[1] = myCriterium[1];
    vocri[2] = myCriterium[2];
    vocri[3] = myCriterium[3];
    erold    = myMaxError;
    emold    = myAverageError;

    /*     (2) Augmentation du poids des moindre carre */

    if (j1cibl > vtest)
    {
      WQuadratic = j1cibl / vtest * WQuadratic;
    }

    /*     (3) Augmentation du poid associe aux points a problemes */

    vseuil = WQuality * .88;

    for (ipnt = myFirstPoint; ipnt <= myLastPoint; ++ipnt)
    {
      if (Ecarts(ipnt) > vtest)
      {
        ecart        = (Ecarts(ipnt) - vseuil) / WQuality;
        tbpoid(ipnt) = (ecart * 3 + 1.) * tbpoid(ipnt);
      }
    }

    /*     (4) Decoupe force */

    if (TheCurve->NbElements() < myMaxSegment && myWithCutting)
    {

      numint = NearIndex(myParameters->Value(myMaxErrorIndex), TheCurve->Knots(), 0, flag);

      tpara = (TheCurve->Knots()(numint) + TheCurve->Knots()(numint + 1)
               + myParameters->Value(myMaxErrorIndex) * 2)
              / 4;

      CNew = new FEmTool_Curve(myDimension, TheCurve->NbElements() + 1, TheCurve->Base(), CurvTol);

      for (i = 1; i <= numint; i++)
        CNew->Knots()(i) = TheCurve->Knots()(i);
      for (i = numint + 1; i <= TheCurve->Knots().Length(); i++)
        CNew->Knots()(i + 1) = TheCurve->Knots()(i);

      CNew->Knots()(numint + 1) = tpara;
    }
    else
    {

      CNew = new FEmTool_Curve(myDimension, TheCurve->NbElements(), TheCurve->Base(), CurvTol);

      CNew->Knots() = TheCurve->Knots();
    }

    JNew = new AppDef_LinearCriteria(mySSP, myFirstPoint, myLastPoint);

    JNew->EstLength() = J->EstLength();

    J->GetEstimation(E1, E2, E3);

    JNew->SetEstimation(E1, E2, E3);

    JNew->SetParameters(myParameters);

    JNew->SetWeight(WQuadratic, WQuality, myPercent[0], myPercent[1], myPercent[2]);

    JNew->SetWeight(tbpoid);

    JNew->SetCurve(CNew);

    /*     (5) Relissage */

    TheMotor(JNew, WQuadratic, WQuality, CNew, Ecarts);

    /*     (6) Tests de rejet */

    j1cibl = Sqrt(myCriterium[0] / (NbrPnt - NbrConstraint));
    vseuil = Sqrt(vocri[1]) + (erold - myMaxError) * 4;

    lrejet = ((myMaxError > WQuality) && (myMaxError > erold * 1.01))
             || (Sqrt(myCriterium[1]) > vseuil * 1.05);

    if (lrejet)
    {
      myCriterium[0] = vocri[0];
      myCriterium[1] = vocri[1];
      myCriterium[2] = vocri[2];
      myCriterium[3] = vocri[3];
      myMaxError     = erold;
      myAverageError = emold;

      loptim = Standard_False;
    }
    else
    {
      J        = JNew;
      TheCurve = CNew;
      J->SetCurve(TheCurve);
    }

    /*     (7) Test de convergence */

    if (((iter >= mxiter) && (myMaxSegment == CNew->NbElements())) || myMaxError < WQuality)
    {
      loptim = Standard_False;
    }
  }
}

static Standard_Boolean NotParallel(gp_Vec& T, gp_Vec& V)
{
  V = T;
  V.SetX(V.X() + 1.);
  if (V.CrossMagnitude(T) > 1.e-12)
    return Standard_True;
  V.SetY(V.Y() + 1.);
  if (V.CrossMagnitude(T) > 1.e-12)
    return Standard_True;
  V.SetZ(V.Z() + 1.);
  if (V.CrossMagnitude(T) > 1.e-12)
    return Standard_True;
  return Standard_False;
}

void AppDef_Variational::AssemblingConstraints(const Handle(FEmTool_Curve)& Curve,
                                               const TColStd_Array1OfReal&  Parameters,
                                               const Standard_Real          CBLONG,
                                               FEmTool_Assembly&            A) const
{

  Standard_Integer MxDeg = Curve->Base()->WorkDegree(), NbElm = Curve->NbElements(),
                   NbDim = Curve->Dimension();

  TColStd_Array1OfReal G0(0, MxDeg), G1(0, MxDeg), G2(0, MxDeg);
  math_Vector          V0((Standard_Real*)&G0(0), 0, MxDeg), V1((Standard_Real*)&G1(0), 0, MxDeg),
    V2((Standard_Real*)&G2(0), 0, MxDeg);

  Standard_Integer IndexOfConstraint, Ng3d, Ng2d, NBeg2d, NPass, NgPC1, NTang3d, NTang2d, Point,
    TypOfConstr, p0 = Parameters.Lower() - myFirstPoint, curel = 1, el, i, ipnt, ityp, j, k, pnt,
                 curdim, jt, Ntheta                            = 6 * myNbP3d + 2 * myNbP2d;
  Standard_Integer NbConstr = myNbPassPoints + myNbTangPoints + myNbCurvPoints;

  //  Ng3d = 3 * NbConstr + 2 * myNbTangPoints + 5 * myNbCurvPoints;
  //  Ng2d = 2 * NbConstr + 1 * myNbTangPoints + 3 * myNbCurvPoints;
  Ng3d   = 3 * NbConstr + 3 * myNbTangPoints + 5 * myNbCurvPoints;
  Ng2d   = 2 * NbConstr + 2 * myNbTangPoints + 3 * myNbCurvPoints;
  NBeg2d = Ng3d * myNbP3d;
  //  NgPC1 = NbConstr + myNbCurvPoints;
  NgPC1   = NbConstr + myNbTangPoints + myNbCurvPoints;
  NPass   = 0;
  NTang3d = 3 * NgPC1;
  NTang2d = 2 * NgPC1;

  TColStd_Array1OfReal& Intervals = Curve->Knots();

  Standard_Real t, R1, R2;

  Handle(PLib_Base)         myBase         = Curve->Base();
  Handle(PLib_HermitJacobi) myHermitJacobi = Handle(PLib_HermitJacobi)::DownCast(myBase);
  Standard_Integer          Order          = myHermitJacobi->NivConstr() + 1;

  Standard_Real UFirst, ULast, coeff, c0, mfact, mfact1;

  A.NullifyConstraint();

  ipnt = -1;
  ityp = 0;
  for (i = 1; i <= NbConstr; i++)
  {

    ipnt += 2;
    ityp += 2;

    Point       = myTypConstraints->Value(ipnt);
    TypOfConstr = myTypConstraints->Value(ityp);

    t = Parameters(p0 + Point);

    for (el = curel; el <= NbElm;)
    {
      if (t <= Intervals(++el))
      {
        curel = el - 1;
        break;
      }
    }

    UFirst = Intervals(curel);
    ULast  = Intervals(curel + 1);
    coeff  = (ULast - UFirst) / 2.;
    c0     = (ULast + UFirst) / 2.;

    t = (t - c0) / coeff;

    if (TypOfConstr == 0)
    {
      myBase->D0(t, G0);
      for (k = 1; k < Order; k++)
      {
        mfact = Pow(coeff, k);
        G0(k) *= mfact;
        G0(k + Order) *= mfact;
      }
    }
    else if (TypOfConstr == 1)
    {
      myBase->D1(t, G0, G1);
      for (k = 1; k < Order; k++)
      {
        mfact = Pow(coeff, k);
        G0(k) *= mfact;
        G0(k + Order) *= mfact;
        G1(k) *= mfact;
        G1(k + Order) *= mfact;
      }
      mfact = 1. / coeff;
      for (k = 0; k <= MxDeg; k++)
      {
        G1(k) *= mfact;
      }
    }
    else
    {
      myBase->D2(t, G0, G1, G2);
      for (k = 1; k < Order; k++)
      {
        mfact = Pow(coeff, k);
        G0(k) *= mfact;
        G0(k + Order) *= mfact;
        G1(k) *= mfact;
        G1(k + Order) *= mfact;
        G2(k) *= mfact;
        G2(k + Order) *= mfact;
      }
      mfact  = 1. / coeff;
      mfact1 = mfact / coeff;
      for (k = 0; k <= MxDeg; k++)
      {
        G1(k) *= mfact;
        G2(k) *= mfact1;
      }
    }

    NPass++;

    j                   = NbDim * (Point - myFirstPoint);
    Standard_Integer n0 = NPass;
    curdim              = 0;
    for (pnt = 1; pnt <= myNbP3d; pnt++)
    {
      IndexOfConstraint = n0;
      for (k = 1; k <= 3; k++)
      {
        curdim++;
        A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
        IndexOfConstraint += NgPC1;
      }
      j += 3;
      n0 += Ng3d;
    }

    n0 = NPass + NBeg2d;
    for (pnt = 1; pnt <= myNbP2d; pnt++)
    {
      IndexOfConstraint = n0;
      for (k = 1; k <= 2; k++)
      {
        curdim++;
        A.AddConstraint(IndexOfConstraint, curel, curdim, V0, myTabPoints->Value(j + k));
        IndexOfConstraint += NgPC1;
      }
      j += 2;
      n0 += Ng2d;
    }

    /*    if(TypOfConstr == 1) {

    IndexOfConstraint = NTang3d + 1;
    jt = Ntheta * (i - 1);
    for(pnt = 1; pnt <= myNbP3d; pnt++) {
    for(k = 1; k <= 3; k++) {
    A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) *  V1, 0.);
    A.AddConstraint(IndexOfConstraint + 1, curel, k, myTtheta->Value(jt + 3 + k) *  V1, 0.);
    }
    IndexOfConstraint += Ng3d;
    jt += 6;
    }

    IndexOfConstraint = NBeg2d + NTang2d + 1;
    for(pnt = 1; pnt <= myNbP2d; pnt++) {
    for(k = 1; k <= 2; k++) {
    A.AddConstraint(IndexOfConstraint, curel, k, myTtheta->Value(jt + k) * V1, 0.);
    }
    IndexOfConstraint += Ng2d;
    jt += 2;
    }

    NTang3d += 2;
    NTang2d += 1;
    } */
    if (TypOfConstr == 1)
    {

      NPass++;
      n0     = NPass;
      j      = 2 * NbDim * (i - 1);
      curdim = 0;
      for (pnt = 1; pnt <= myNbP3d; pnt++)
      {
        IndexOfConstraint = n0;
        for (k = 1; k <= 3; k++)
        {
          curdim++;
          A.AddConstraint(IndexOfConstraint,
                          curel,
                          curdim,
                          V1,
                          CBLONG * myTabConstraints->Value(j + k));
          IndexOfConstraint += NgPC1;
        }
        n0 += Ng3d;
        j += 6;
      }

      n0 = NPass + NBeg2d;
      for (pnt = 1; pnt <= myNbP2d; pnt++)
      {
        IndexOfConstraint = n0;
        for (k = 1; k <= 2; k++)
        {
          curdim++;
          A.AddConstraint(IndexOfConstraint,
                          curel,
                          curdim,
                          V1,
                          CBLONG * myTabConstraints->Value(j + k));
          IndexOfConstraint += NgPC1;
        }
        n0 += Ng2d;
        j += 4;
      }
    }
    if (TypOfConstr == 2)
    {

      NPass++;
      n0     = NPass;
      j      = 2 * NbDim * (i - 1);
      curdim = 0;
      for (pnt = 1; pnt <= myNbP3d; pnt++)
      {
        IndexOfConstraint = n0;
        for (k = 1; k <= 3; k++)
        {
          curdim++;
          A.AddConstraint(IndexOfConstraint,
                          curel,
                          curdim,
                          V1,
                          CBLONG * myTabConstraints->Value(j + k));
          IndexOfConstraint += NgPC1;
        }
        n0 += Ng3d;
        j += 6;
      }

      n0 = NPass + NBeg2d;
      for (pnt = 1; pnt <= myNbP2d; pnt++)
      {
        IndexOfConstraint = n0;
        for (k = 1; k <= 2; k++)
        {
          curdim++;
          A.AddConstraint(IndexOfConstraint,
                          curel,
                          curdim,
                          V1,
                          CBLONG * myTabConstraints->Value(j + k));
          IndexOfConstraint += NgPC1;
        }
        n0 += Ng2d;
        j += 4;
      }

      j                 = 2 * NbDim * (i - 1) + 3;
      jt                = Ntheta * (i - 1);
      IndexOfConstraint = NTang3d + 1;
      curdim            = 0;
      for (pnt = 1; pnt <= myNbP3d; pnt++)
      {
        R1 = 0.;
        R2 = 0.;
        for (k = 1; k <= 3; k++)
        {
          R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
          R2 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + 3 + k);
        }
        R1 *= CBLONG * CBLONG;
        R2 *= CBLONG * CBLONG;
        for (k = 1; k <= 3; k++)
        {
          curdim++;
          if (k > 1)
            R1 = R2 = 0.;
          A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
          A.AddConstraint(IndexOfConstraint + 1,
                          curel,
                          curdim,
                          myTfthet->Value(jt + 3 + k) * V2,
                          R2);
        }
        IndexOfConstraint += Ng3d;
        j += 6;
        jt += 6;
      }

      j--;
      IndexOfConstraint = NBeg2d + NTang2d + 1;
      for (pnt = 1; pnt <= myNbP2d; pnt++)
      {
        R1 = 0.;
        for (k = 1; k <= 2; k++)
        {
          R1 += myTabConstraints->Value(j + k) * myTtheta->Value(jt + k);
        }
        R1 *= CBLONG * CBLONG;
        for (k = 1; k <= 2; k++)
        {
          curdim++;
          if (k > 1)
            R1 = 0.;
          A.AddConstraint(IndexOfConstraint, curel, curdim, myTfthet->Value(jt + k) * V2, R1);
        }
        IndexOfConstraint += Ng2d;
        j += 4;
        jt += 2;
      }

      NTang3d += 2;
      NTang2d += 1;
    }
  }
}

Standard_Boolean AppDef_Variational::InitTthetaF(const Standard_Integer        ndimen,
                                                 const AppParCurves_Constraint typcon,
                                                 const Standard_Integer        begin,
                                                 const Standard_Integer        jndex)
{
  if ((ndimen < 2) || (ndimen > 3))
    return Standard_False;
  gp_Vec        T, V;
  gp_Vec        theta1, theta2;
  gp_Vec        F;
  Standard_Real XX, XY, YY, XZ, YZ, ZZ;

  if ((typcon == AppParCurves_TangencyPoint) || (typcon == AppParCurves_CurvaturePoint))
  {
    T.SetX(myTabConstraints->Value(jndex));
    T.SetY(myTabConstraints->Value(jndex + 1));
    if (ndimen == 3)
      T.SetZ(myTabConstraints->Value(jndex + 2));
    else
      T.SetZ(0.);
    if (ndimen == 2)
    {
      V.SetX(0.);
      V.SetY(0.);
      V.SetZ(1.);
    }
    if (ndimen == 3)
      if (!NotParallel(T, V))
        return Standard_False;
    theta1 = V ^ T;
    theta1.Normalize();
    myTtheta->SetValue(begin, theta1.X());
    myTtheta->SetValue(begin + 1, theta1.Y());
    if (ndimen == 3)
    {
      theta2 = T ^ theta1;
      theta2.Normalize();
      myTtheta->SetValue(begin + 2, theta1.Z());
      myTtheta->SetValue(begin + 3, theta2.X());
      myTtheta->SetValue(begin + 4, theta2.Y());
      myTtheta->SetValue(begin + 5, theta2.Z());
    }

    // Calculation of myTfthet
    if (typcon == AppParCurves_CurvaturePoint)
    {
      XX = Pow(T.X(), 2);
      XY = T.X() * T.Y();
      YY = Pow(T.Y(), 2);
      if (ndimen == 2)
      {
        F.SetX(YY * theta1.X() - XY * theta1.Y());
        F.SetY(XX * theta1.Y() - XY * theta1.X());
        myTfthet->SetValue(begin, F.X());
        myTfthet->SetValue(begin + 1, F.Y());
      }
      if (ndimen == 3)
      {
        XZ = T.X() * T.Z();
        YZ = T.Y() * T.Z();
        ZZ = Pow(T.Z(), 2);

        F.SetX((ZZ + YY) * theta1.X() - XY * theta1.Y() - XZ * theta1.Z());
        F.SetY((XX + ZZ) * theta1.Y() - XY * theta1.X() - YZ * theta1.Z());
        F.SetZ((XX + YY) * theta1.Z() - XZ * theta1.X() - YZ * theta1.Y());
        myTfthet->SetValue(begin, F.X());
        myTfthet->SetValue(begin + 1, F.Y());
        myTfthet->SetValue(begin + 2, F.Z());
        F.SetX((ZZ + YY) * theta2.X() - XY * theta2.Y() - XZ * theta2.Z());
        F.SetY((XX + ZZ) * theta2.Y() - XY * theta2.X() - YZ * theta2.Z());
        F.SetZ((XX + YY) * theta2.Z() - XZ * theta2.X() - YZ * theta2.Y());
        myTfthet->SetValue(begin + 3, F.X());
        myTfthet->SetValue(begin + 4, F.Y());
        myTfthet->SetValue(begin + 5, F.Z());
      }
    }
  }
  return Standard_True;
}