OCCT

// Created on: 1995-03-14
// Created by: Modelistation
// Copyright (c) 1995-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.

#ifndef OCCT_DEBUG
  #define No_Standard_OutOfRange
  #define No_Standard_RangeError
#endif
#include <AppCont_LeastSquare.hxx>

#include <math.hxx>
#include <AppParCurves_MultiPoint.hxx>
#include <AppCont_ContMatrices.pxx>
#include <PLib.hxx>

//=================================================================================================

void AppCont_LeastSquare::FixSingleBorderPoint(const AppCont_Function&       theSSP,
                                               const double                  theU,
                                               const double                  theU0,
                                               const double                  theU1,
                                               NCollection_Array1<gp_Pnt2d>& theFix2d,
                                               NCollection_Array1<gp_Pnt>&   theFix)
{
  int                          aMaxIter = 15;
  NCollection_Array1<gp_Pnt>   aTabP(1, std::max(myNbP, 1)), aPrevP(1, std::max(myNbP, 1));
  NCollection_Array1<gp_Pnt2d> aTabP2d(1, std::max(myNbP2d, 1)), aPrevP2d(1, std::max(myNbP2d, 1));
  double                       aMult       = ((theU - theU0) > (theU1 - theU)) ? 1.0 : -1.0;
  double                       aStartParam = theU, aCurrParam, aPrevDist = 1.0, aCurrDist = 1.0;

  double du  = -(theU1 - theU0) / 2.0 * aMult;
  double eps = Epsilon(1.);
  double dd = du, dec = .1;
  for (int anIter = 1; anIter < aMaxIter; anIter++)
  {
    dd *= dec;
    aCurrParam = aStartParam + dd;
    theSSP.Value(aCurrParam, aTabP2d, aTabP);

    // from second iteration
    if (anIter > 1)
    {
      aCurrDist = 0.0;

      int i2 = 1;
      for (int j = 1; j <= myNbP; j++)
      {
        aCurrDist += aTabP(j).Distance(aPrevP(j));
        i2 += 3;
      }
      for (int j = 1; j <= myNbP2d; j++)
      {
        aCurrDist += aTabP2d(j).Distance(aPrevP2d(j));
        i2 += 2;
      }
      (void)i2; // unused but set for debug

      // from the third iteration
      if (anIter > 2 && aCurrDist / aPrevDist > 10.0)
      {
        break;
      }
    }
    aPrevP    = aTabP;
    aPrevP2d  = aTabP2d;
    aPrevDist = aCurrDist;
    if (aPrevDist <= eps)
    {
      break;
    }
  }
  theFix2d = aPrevP2d;
  theFix   = aPrevP;
}

//=================================================================================================

AppCont_LeastSquare::AppCont_LeastSquare(const AppCont_Function&       SSP,
                                         const double                  U0,
                                         const double                  U1,
                                         const AppParCurves_Constraint FirstCons,
                                         const AppParCurves_Constraint LastCons,
                                         const int                     Deg,
                                         const int                     myNbPoints)
    : mySCU(Deg + 1),
      myPoints(1, myNbPoints, 1, 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints()),
      myPoles(1, Deg + 1, 1, 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints(), 0.0),
      myParam(1, myNbPoints),
      myVB(1, Deg + 1, 1, myNbPoints),
      myPerInfo(1, 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints())
{
  myDone  = false;
  myDegre = Deg;
  int    i, j, k, c, i2;
  int    classe = Deg + 1, cl1 = Deg;
  double U, dU, Coeff, Coeff2;
  double IBij, IBPij;

  int                     FirstP = 1, LastP = myNbPoints;
  int                     nbcol = 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints();
  math_Matrix             B(1, classe, 1, nbcol, 0.0);
  int                     bdeb = 1, bfin = classe;
  AppParCurves_Constraint myFirstC = FirstCons, myLastC = LastCons;
  SSP.GetNumberOfPoints(myNbP, myNbP2d);

  int i2plus1, i2plus2;
  myNbdiscret = myNbPoints;
  NCollection_Array1<gp_Pnt>   aTabP(1, std::max(myNbP, 1));
  NCollection_Array1<gp_Pnt2d> aTabP2d(1, std::max(myNbP2d, 1));
  NCollection_Array1<gp_Vec>   aTabV(1, std::max(myNbP, 1));
  NCollection_Array1<gp_Vec2d> aTabV2d(1, std::max(myNbP2d, 1));

  for (int aDimIdx = 1; aDimIdx <= myNbP * 3 + myNbP2d * 2; aDimIdx++)
  {
    SSP.PeriodInformation(aDimIdx, myPerInfo(aDimIdx).isPeriodic, myPerInfo(aDimIdx).myPeriod);
  }

  bool Ok;
  if (myFirstC == AppParCurves_TangencyPoint)
  {
    Ok = SSP.D1(U0, aTabV2d, aTabV);
    if (!Ok)
    {
      myFirstC = AppParCurves_PassPoint;
    }
  }

  if (myLastC == AppParCurves_TangencyPoint)
  {
    Ok = SSP.D1(U1, aTabV2d, aTabV);
    if (!Ok)
    {
      myLastC = AppParCurves_PassPoint;
    }
  }

  // Compute control points params on which approximation will be built.
  math_Vector GaussP(1, myNbPoints), GaussW(1, myNbPoints);
  math::GaussPoints(myNbPoints, GaussP);
  math::GaussWeights(myNbPoints, GaussW);
  math_Vector TheWeights(1, myNbPoints), VBParam(1, myNbPoints);
  dU = 0.5 * (U1 - U0);
  for (i = FirstP; i <= LastP; i++)
  {
    U = 0.5 * (U1 + U0) + dU * GaussP(i);
    if (i <= (myNbPoints + 1) / 2)
    {
      myParam(LastP - i + 1)    = U;
      VBParam(LastP - i + 1)    = 0.5 * (1 + GaussP(i));
      TheWeights(LastP - i + 1) = 0.5 * GaussW(i);
    }
    else
    {
      VBParam(i - (myNbPoints + 1) / 2)    = 0.5 * (1 + GaussP(i));
      myParam(i - (myNbPoints + 1) / 2)    = U;
      TheWeights(i - (myNbPoints + 1) / 2) = 0.5 * GaussW(i);
    }
  }

  // Compute control points.
  for (i = FirstP; i <= LastP; i++)
  {
    U = myParam(i);
    SSP.Value(U, aTabP2d, aTabP);

    i2 = 1;
    for (j = 1; j <= myNbP; j++)
    {
      (aTabP(j)).Coord(myPoints(i, i2), myPoints(i, i2 + 1), myPoints(i, i2 + 2));
      i2 += 3;
    }
    for (j = 1; j <= myNbP2d; j++)
    {
      (aTabP2d(j)).Coord(myPoints(i, i2), myPoints(i, i2 + 1));
      i2 += 2;
    }
  }

  // Fix possible "period jump".
  int aMaxDim = 3 * myNbP + 2 * myNbP2d;
  for (int aDimIdx = 1; aDimIdx <= aMaxDim; aDimIdx++)
  {
    if (myPerInfo(aDimIdx).isPeriodic
        && std::abs(myPoints(1, aDimIdx) - myPoints(2, aDimIdx))
             > myPerInfo(aDimIdx).myPeriod / 2.01
        && std::abs(myPoints(2, aDimIdx) - myPoints(3, aDimIdx))
             < myPerInfo(aDimIdx).myPeriod / 2.01)
    {
      double aPeriodMult   = (myPoints(1, aDimIdx) < myPoints(2, aDimIdx)) ? 1.0 : -1.0;
      double aNewParam     = myPoints(1, aDimIdx) + aPeriodMult * myPerInfo(aDimIdx).myPeriod;
      myPoints(1, aDimIdx) = aNewParam;
    }
  }
  for (int aPntIdx = 1; aPntIdx < myNbPoints; aPntIdx++)
  {
    for (int aDimIdx = 1; aDimIdx <= aMaxDim; aDimIdx++)
    {
      if (myPerInfo(aDimIdx).isPeriodic
          && std::abs(myPoints(aPntIdx, aDimIdx) - myPoints(aPntIdx + 1, aDimIdx))
               > myPerInfo(aDimIdx).myPeriod / 2.01)
      {
        double aPeriodMult =
          (myPoints(aPntIdx, aDimIdx) > myPoints(aPntIdx + 1, aDimIdx)) ? 1.0 : -1.0;
        double aNewParam =
          myPoints(aPntIdx + 1, aDimIdx) + aPeriodMult * myPerInfo(aDimIdx).myPeriod;
        myPoints(aPntIdx + 1, aDimIdx) = aNewParam;
      }
    }
  }

  AppCont_ContMatrices::VBernstein(classe, myNbPoints, myVB);

  // Processing of the right-hand side:
  NCollection_Array1<double> tmppoints(1, nbcol);

  for (c = 1; c <= classe; c++)
  {
    tmppoints.Init(0.0);
    for (i = 1; i <= myNbPoints; i++)
    {
      Coeff = TheWeights(i) * myVB(c, i);
      for (j = 1; j <= nbcol; j++)
      {
        tmppoints(j) += myPoints(i, j) * Coeff;
      }
    }
    for (k = 1; k <= nbcol; k++)
    {
      B(c, k) += tmppoints(k);
    }
  }

  if (myFirstC == AppParCurves_NoConstraint && myLastC == AppParCurves_NoConstraint)
  {

    math_Matrix InvM(1, classe, 1, classe);
    AppCont_ContMatrices::InvMMatrix(classe, InvM);
    // Direct computation of poles:

    for (i = 1; i <= classe; i++)
    {
      for (j = 1; j <= classe; j++)
      {
        IBij = InvM(i, j);
        for (k = 1; k <= nbcol; k++)
        {
          myPoles(i, k) += IBij * B(j, k);
        }
      }
    }
  }

  else
  {
    math_Matrix M(1, classe, 1, classe);
    AppCont_ContMatrices::MMatrix(classe, M);
    NCollection_Array1<gp_Pnt2d> aFixP2d(1, std::max(myNbP2d, 1));
    NCollection_Array1<gp_Pnt>   aFixP(1, std::max(myNbP, 1));

    if (myFirstC == AppParCurves_PassPoint || myFirstC == AppParCurves_TangencyPoint)
    {
      SSP.Value(U0, aTabP2d, aTabP);
      FixSingleBorderPoint(SSP, U0, U0, U1, aFixP2d, aFixP);

      i2 = 1;
      for (k = 1; k <= myNbP; k++)
      {
        if (aFixP(k).Distance(aTabP(k)) > 0.1)
        {
          (aFixP(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1), myPoles(1, i2 + 2));
        }
        else
        {
          (aTabP(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1), myPoles(1, i2 + 2));
        }
        i2 += 3;
      }
      for (k = 1; k <= myNbP2d; k++)
      {
        if (aFixP2d(k).Distance(aTabP2d(k)) > 0.1)
        {
          (aFixP2d(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1));
        }
        else
        {
          (aTabP2d(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1));
        }
        i2 += 2;
      }

      for (int aDimIdx = 1; aDimIdx <= aMaxDim; aDimIdx++)
      {
        if (myPerInfo(aDimIdx).isPeriodic
            && std::abs(myPoles(1, aDimIdx) - myPoints(1, aDimIdx))
                 > myPerInfo(aDimIdx).myPeriod / 2.01)
        {
          double aMult = myPoles(1, aDimIdx) < myPoints(1, aDimIdx) ? 1.0 : -1.0;
          myPoles(1, aDimIdx) += aMult * myPerInfo(aDimIdx).myPeriod;
        }
      }
    }

    if (myLastC == AppParCurves_PassPoint || myLastC == AppParCurves_TangencyPoint)
    {
      SSP.Value(U1, aTabP2d, aTabP);
      FixSingleBorderPoint(SSP, U1, U0, U1, aFixP2d, aFixP);

      i2 = 1;
      for (k = 1; k <= myNbP; k++)
      {
        if (aFixP(k).Distance(aTabP(k)) > 0.1)
        {
          (aFixP(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1), myPoles(classe, i2 + 2));
        }
        else
        {
          (aTabP(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1), myPoles(classe, i2 + 2));
        }
        i2 += 3;
      }
      for (k = 1; k <= myNbP2d; k++)
      {
        if (aFixP2d(k).Distance(aTabP2d(k)) > 0.1)
        {
          (aFixP2d(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1));
        }
        else
        {
          (aTabP2d(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1));
        }
        i2 += 2;
      }

      for (int aDimIdx = 1; aDimIdx <= 2; aDimIdx++)
      {
        if (myPerInfo(aDimIdx).isPeriodic
            && std::abs(myPoles(classe, aDimIdx) - myPoints(myNbPoints, aDimIdx))
                 > myPerInfo(aDimIdx).myPeriod / 2.01)
        {
          double aMult = myPoles(classe, aDimIdx) < myPoints(myNbPoints, aDimIdx) ? 1.0 : -1.0;
          myPoles(classe, aDimIdx) += aMult * myPerInfo(aDimIdx).myPeriod;
        }
      }
    }

    if (myFirstC == AppParCurves_PassPoint)
    {
      bdeb = 2;
      // Update the right-hand side:
      for (i = 1; i <= classe; i++)
      {
        Coeff = M(i, 1);
        for (k = 1; k <= nbcol; k++)
        {
          B(i, k) -= myPoles(1, k) * Coeff;
        }
      }
    }

    if (myLastC == AppParCurves_PassPoint)
    {
      bfin = cl1;
      for (i = 1; i <= classe; i++)
      {
        Coeff = M(i, classe);
        for (k = 1; k <= nbcol; k++)
        {
          B(i, k) -= myPoles(classe, k) * Coeff;
        }
      }
    }

    if (myFirstC == AppParCurves_TangencyPoint)
    {
      // Fix the second pole:
      bdeb = 3;
      SSP.D1(U0, aTabV2d, aTabV);

      i2    = 1;
      Coeff = (U1 - U0) / myDegre;
      for (k = 1; k <= myNbP; k++)
      {
        i2plus1             = i2 + 1;
        i2plus2             = i2 + 2;
        myPoles(2, i2)      = myPoles(1, i2) + aTabV(k).X() * Coeff;
        myPoles(2, i2plus1) = myPoles(1, i2plus1) + aTabV(k).Y() * Coeff;
        myPoles(2, i2plus2) = myPoles(1, i2plus2) + aTabV(k).Z() * Coeff;
        i2 += 3;
      }
      for (k = 1; k <= myNbP2d; k++)
      {
        i2plus1             = i2 + 1;
        myPoles(2, i2)      = myPoles(1, i2) + aTabV2d(k).X() * Coeff;
        myPoles(2, i2plus1) = myPoles(1, i2plus1) + aTabV2d(k).Y() * Coeff;
        i2 += 2;
      }

      for (i = 1; i <= classe; i++)
      {
        Coeff  = M(i, 1);
        Coeff2 = M(i, 2);
        for (k = 1; k <= nbcol; k++)
        {
          B(i, k) -= myPoles(1, k) * Coeff + myPoles(2, k) * Coeff2;
        }
      }
    }

    if (myLastC == AppParCurves_TangencyPoint)
    {
      bfin = classe - 2;
      SSP.D1(U1, aTabV2d, aTabV);
      i2    = 1;
      Coeff = (U1 - U0) / myDegre;
      for (k = 1; k <= myNbP; k++)
      {
        i2plus1               = i2 + 1;
        i2plus2               = i2 + 2;
        myPoles(cl1, i2)      = myPoles(classe, i2) - aTabV(k).X() * Coeff;
        myPoles(cl1, i2plus1) = myPoles(classe, i2plus1) - aTabV(k).Y() * Coeff;
        myPoles(cl1, i2plus2) = myPoles(classe, i2plus2) - aTabV(k).Z() * Coeff;
        i2 += 3;
      }
      for (k = 1; k <= myNbP2d; k++)
      {
        i2plus1               = i2 + 1;
        myPoles(cl1, i2)      = myPoles(classe, i2) - aTabV2d(k).X() * Coeff;
        myPoles(cl1, i2plus1) = myPoles(classe, i2plus1) - aTabV2d(k).Y() * Coeff;
        i2 += 2;
      }

      for (i = 1; i <= classe; i++)
      {
        Coeff  = M(i, classe);
        Coeff2 = M(i, cl1);
        for (k = 1; k <= nbcol; k++)
        {
          B(i, k) -= myPoles(classe, k) * Coeff + myPoles(cl1, k) * Coeff2;
        }
      }
    }

    if (bdeb <= bfin)
    {
      math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);

      for (i = bdeb; i <= bfin; i++)
      {
        for (j = 1; j <= classe; j++)
        {
          Coeff = M(i, j);
          for (k = 1; k <= nbcol; k++)
          {
            B2(i, k) += B(j, k) * Coeff;
          }
        }
      }

      // Resolution:
      // ===========
      math_Matrix IBP(bdeb, bfin, bdeb, bfin);

      // In IBPMatrix and IBTMatrix only results for a degree
      // less than or equal to 26 are stored (for now at least).

      if (bdeb == 2 && bfin == classe - 1 && classe <= 26)
      {
        AppCont_ContMatrices::IBPMatrix(classe, IBP);
      }
      else if (bdeb == 3 && bfin == classe - 2 && classe <= 26)
      {
        AppCont_ContMatrices::IBTMatrix(classe, IBP);
      }
      else
      {
        math_Matrix MP(1, classe, bdeb, bfin);
        for (i = 1; i <= classe; i++)
        {
          for (j = bdeb; j <= bfin; j++)
          {
            MP(i, j) = M(i, j);
          }
        }
        math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
        IBP1 = MP.Transposed() * MP;
        IBP  = IBP1.Inverse();
      }

      myDone = true;
      for (i = bdeb; i <= bfin; i++)
      {
        for (j = bdeb; j <= bfin; j++)
        {
          IBPij = IBP(i, j);
          for (k = 1; k <= nbcol; k++)
          {
            myPoles(i, k) += IBPij * B2(j, k);
          }
        }
      }
    }
  }
}

//=================================================================================================

const AppParCurves_MultiCurve& AppCont_LeastSquare::Value()
{

  int      i, j, j2;
  gp_Pnt   Pt;
  gp_Pnt2d Pt2d;
  int      ideb = 1, ifin = myDegre + 1;

  // Store the result in the corresponding curves
  for (i = ideb; i <= ifin; i++)
  {
    j2 = 1;
    AppParCurves_MultiPoint MPole(myNbP, myNbP2d);
    for (j = 1; j <= myNbP; j++)
    {
      Pt.SetCoord(myPoles(i, j2), myPoles(i, j2 + 1), myPoles(i, j2 + 2));
      MPole.SetPoint(j, Pt);
      j2 += 3;
    }
    for (j = myNbP + 1; j <= myNbP + myNbP2d; j++)
    {
      Pt2d.SetCoord(myPoles(i, j2), myPoles(i, j2 + 1));
      MPole.SetPoint2d(j, Pt2d);
      j2 += 2;
    }
    mySCU.SetValue(i, MPole);
  }
  return mySCU;
}

//=================================================================================================

void AppCont_LeastSquare::Error(double& F, double& MaxE3d, double& MaxE2d) const
{
  int    i, j, k, c, i2, classe = myDegre + 1;
  double Coeff, err3d = 0.0, err2d = 0.0;
  int    ncol = myPoints.UpperCol() - myPoints.LowerCol() + 1;

  math_Matrix MyPoints(1, myNbdiscret, 1, ncol);
  MyPoints = myPoints;

  MaxE3d = MaxE2d = F = 0.0;

  NCollection_Array1<double> tmppoles(1, ncol);

  for (c = 1; c <= classe; c++)
  {
    for (k = 1; k <= ncol; k++)
    {
      tmppoles(k) = myPoles(c, k);
    }
    for (i = 1; i <= myNbdiscret; i++)
    {
      Coeff = myVB(c, i);
      for (j = 1; j <= ncol; j++)
      {
        MyPoints(i, j) -= tmppoles(j) * Coeff;
      }
    }
  }

  double e1, e2, e3;
  for (i = 1; i <= myNbdiscret; i++)
  {
    i2 = 1;
    for (j = 1; j <= myNbP; j++)
    {
      e1     = MyPoints(i, i2);
      e2     = MyPoints(i, i2 + 1);
      e3     = MyPoints(i, i2 + 2);
      err3d  = e1 * e1 + e2 * e2 + e3 * e3;
      MaxE3d = std::max(MaxE3d, err3d);
      F += err3d;
      i2 += 3;
    }
    for (j = 1; j <= myNbP2d; j++)
    {
      e1     = MyPoints(i, i2);
      e2     = MyPoints(i, i2 + 1);
      err2d  = e1 * e1 + e2 * e2;
      MaxE2d = std::max(MaxE2d, err2d);
      F += err2d;
      i2 += 2;
    }
  }

  MaxE3d = std::sqrt(MaxE3d);
  MaxE2d = std::sqrt(MaxE2d);
}

//=================================================================================================

bool AppCont_LeastSquare::IsDone() const
{
  return myDone;
}