OCCT

// Created on: 1995-07-18
// 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.

#include <Adaptor3d_Surface.hxx>
#include <Extrema_GenExtSS.hxx>
#include <Extrema_POnSurf.hxx>
#include <GeomGridEval_Surface.hxx>
#include <math_BFGS.hxx>
#include <math_FunctionSetRoot.hxx>
#include <math_MultipleVarFunctionWithGradient.hxx>
#include <math_Vector.hxx>
#include <Standard_OutOfRange.hxx>
#include <StdFail_NotDone.hxx>
#include <NCollection_Array1.hxx>

//! This class represents distance objective function for surface / surface.
class Extrema_FuncDistSS : public math_MultipleVarFunctionWithGradient
{
public:
  DEFINE_STANDARD_ALLOC

  Standard_EXPORT Extrema_FuncDistSS(const Adaptor3d_Surface& S1, const Adaptor3d_Surface& S2)
      : myS1(&S1),
        myS2(&S2)
  {
  }

  Standard_EXPORT int NbVariables() const override { return 4; }

  Standard_EXPORT bool Value(const math_Vector& X, double& F) override
  {
    F = myS1->Value(X(1), X(2)).SquareDistance(myS2->Value(X(3), X(4)));
    return true;
  }

  Standard_EXPORT bool Gradient(const math_Vector& X, math_Vector& G) override
  {
    gp_Pnt P1, P2;
    gp_Vec Du1s1, Dv1s1;
    gp_Vec Du2s2, Dv2s2;
    myS1->D1(X(1), X(2), P1, Du1s1, Dv1s1);
    myS2->D1(X(3), X(4), P2, Du2s2, Dv2s2);

    gp_Vec P1P2(P2, P1);

    G(1) = P1P2.Dot(Du1s1);
    G(2) = P1P2.Dot(Dv1s1);
    G(3) = -P1P2.Dot(Du2s2);
    G(4) = -P1P2.Dot(Dv2s2);

    return true;
  }

  Standard_EXPORT bool Values(const math_Vector& X, double& F, math_Vector& G) override
  {
    F = myS1->Value(X(1), X(2)).SquareDistance(myS2->Value(X(3), X(4)));

    gp_Pnt P1, P2;
    gp_Vec Du1s1, Dv1s1;
    gp_Vec Du2s2, Dv2s2;
    myS1->D1(X(1), X(2), P1, Du1s1, Dv1s1);
    myS2->D1(X(3), X(4), P2, Du2s2, Dv2s2);

    gp_Vec P1P2(P2, P1);

    G(1) = P1P2.Dot(Du1s1);
    G(2) = P1P2.Dot(Dv1s1);
    G(3) = -P1P2.Dot(Du2s2);
    G(4) = -P1P2.Dot(Dv2s2);

    return true;
  }

private:
  const Adaptor3d_Surface* myS1;
  const Adaptor3d_Surface* myS2;
};

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

Extrema_GenExtSS::Extrema_GenExtSS()
    : myu1min(0.0),
      myu1sup(0.0),
      myv1min(0.0),
      myv1sup(0.0),
      myu2min(0.0),
      myu2sup(0.0),
      myv2min(0.0),
      myv2sup(0.0),
      myusample(0),
      myvsample(0),
      mytol1(0.0),
      mytol2(0.0),
      myS2(nullptr)
{
  myDone = false;
  myInit = false;
}

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

Extrema_GenExtSS::~Extrema_GenExtSS() = default;

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

Extrema_GenExtSS::Extrema_GenExtSS(const Adaptor3d_Surface& S1,
                                   const Adaptor3d_Surface& S2,
                                   const int                NbU,
                                   const int                NbV,
                                   const double             Tol1,
                                   const double             Tol2)
    : myF(S1, S2)
{
  Initialize(S2, NbU, NbV, Tol2);
  Perform(S1, Tol1);
}

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

Extrema_GenExtSS::Extrema_GenExtSS(const Adaptor3d_Surface& S1,
                                   const Adaptor3d_Surface& S2,
                                   const int                NbU,
                                   const int                NbV,
                                   const double             U1min,
                                   const double             U1sup,
                                   const double             V1min,
                                   const double             V1sup,
                                   const double             U2min,
                                   const double             U2sup,
                                   const double             V2min,
                                   const double             V2sup,
                                   const double             Tol1,
                                   const double             Tol2)
    : myF(S1, S2)
{
  Initialize(S2, NbU, NbV, U2min, U2sup, V2min, V2sup, Tol2);
  Perform(S1, U1min, U1sup, V1min, V1sup, Tol1);
}

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

void Extrema_GenExtSS::Initialize(const Adaptor3d_Surface& S2,
                                  const int                NbU,
                                  const int                NbV,
                                  const double             Tol2)
{
  myu2min = S2.FirstUParameter();
  myu2sup = S2.LastUParameter();
  myv2min = S2.FirstVParameter();
  myv2sup = S2.LastVParameter();
  Initialize(S2, NbU, NbV, myu2min, myu2sup, myv2min, myv2sup, Tol2);
}

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

void Extrema_GenExtSS::Initialize(const Adaptor3d_Surface& S2,
                                  const int                NbU,
                                  const int                NbV,
                                  const double             U2min,
                                  const double             U2sup,
                                  const double             V2min,
                                  const double             V2sup,
                                  const double             Tol2)
{
  myS2      = &S2;
  mypoints1 = new NCollection_HArray2<gp_Pnt>(0, NbU + 1, 0, NbV + 1);
  mypoints2 = new NCollection_HArray2<gp_Pnt>(0, NbU + 1, 0, NbV + 1);
  myusample = NbU;
  myvsample = NbV;
  myu2min   = U2min;
  myu2sup   = U2sup;
  myv2min   = V2min;
  myv2sup   = V2sup;
  mytol2    = Tol2;

  // Parametrage de l echantillon sur S2

  double PasU = myu2sup - myu2min;
  double PasV = myv2sup - myv2min;
  double U0   = PasU / myusample / 100.;
  double V0   = PasV / myvsample / 100.;
  PasU        = (PasU - U0) / (myusample - 1);
  PasV        = (PasV - V0) / (myvsample - 1);
  U0          = myu2min + U0 / 2.;
  V0          = myv2min + V0 / 2.;

  // Build UV parameter arrays for batch evaluation
  NCollection_Array1<double> aUParams(1, myusample);
  NCollection_Array1<double> aVParams(1, myvsample);

  double U = U0;
  for (int NoU = 1; NoU <= myusample; NoU++, U += PasU)
  {
    aUParams.SetValue(NoU, U);
  }
  double V = V0;
  for (int NoV = 1; NoV <= myvsample; NoV++, V += PasV)
  {
    aVParams.SetValue(NoV, V);
  }

  // Use batch grid evaluation for optimized surface point computation
  GeomGridEval_Surface anEvaluator(*myS2);

  const NCollection_Array2<gp_Pnt> aGrid = anEvaluator.EvaluateGrid(aUParams, aVParams);

  for (int NoU = 1; NoU <= myusample; NoU++)
  {
    for (int NoV = 1; NoV <= myvsample; NoV++)
    {
      mypoints2->SetValue(NoU, NoV, aGrid.Value(NoU, NoV));
    }
  }
}

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

void Extrema_GenExtSS::Perform(const Adaptor3d_Surface& S1, const double Tol1)
{
  myu1min = S1.FirstUParameter();
  myu1sup = S1.LastUParameter();
  myv1min = S1.FirstVParameter();
  myv1sup = S1.LastVParameter();
  Perform(S1, myu1min, myu1sup, myv1min, myv1sup, Tol1);
}

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

void Extrema_GenExtSS::Perform(const Adaptor3d_Surface& S1,
                               const double             U1min,
                               const double             U1sup,
                               const double             V1min,
                               const double             V1sup,
                               const double             Tol1)
{
  myF.Initialize(S1, *myS2);
  myu1min = U1min;
  myu1sup = U1sup;
  myv1min = V1min;
  myv1sup = V1sup;
  mytol1  = Tol1;

  double U1, V1, U2, V2;
  int    NoU1, NoV1, NoU2, NoV2;
  gp_Pnt P1, P2;

  // Parametrage de l echantillon sur S1

  double PasU1 = myu1sup - myu1min;
  double PasV1 = myv1sup - myv1min;
  double U10   = PasU1 / myusample / 100.;
  double V10   = PasV1 / myvsample / 100.;
  PasU1        = (PasU1 - U10) / (myusample - 1);
  PasV1        = (PasV1 - V10) / (myvsample - 1);
  U10          = myu1min + U10 / 2.;
  V10          = myv1min + V10 / 2.;

  double PasU2 = myu2sup - myu2min;
  double PasV2 = myv2sup - myv2min;
  double U20   = PasU2 / myusample / 100.;
  double V20   = PasV2 / myvsample / 100.;
  PasU2        = (PasU2 - U20) / (myusample - 1);
  PasV2        = (PasV2 - V20) / (myvsample - 1);
  U20          = myu2min + U20 / 2.;
  V20          = myv2min + V20 / 2.;

  // Build UV parameter arrays for batch evaluation of S1
  NCollection_Array1<double> aU1Params(1, myusample);
  NCollection_Array1<double> aV1Params(1, myvsample);

  U1 = U10;
  for (NoU1 = 1; NoU1 <= myusample; NoU1++, U1 += PasU1)
  {
    aU1Params.SetValue(NoU1, U1);
  }
  V1 = V10;
  for (NoV1 = 1; NoV1 <= myvsample; NoV1++, V1 += PasV1)
  {
    aV1Params.SetValue(NoV1, V1);
  }

  // Use batch grid evaluation for optimized surface point computation
  GeomGridEval_Surface anEvaluator(S1);

  const NCollection_Array2<gp_Pnt> aGrid = anEvaluator.EvaluateGrid(aU1Params, aV1Params);

  for (NoU1 = 1; NoU1 <= myusample; NoU1++)
  {
    for (NoV1 = 1; NoV1 <= myvsample; NoV1++)
    {
      mypoints1->SetValue(NoU1, NoV1, aGrid.Value(NoU1, NoV1));
    }
  }

  /*
  b- Calcul des minima:
     -----------------
     b.a) Initialisations:
  */

  math_Vector Tol(1, 4);
  Tol(1) = mytol1;
  Tol(2) = mytol1;
  Tol(3) = mytol2;
  Tol(4) = mytol2;
  math_Vector UV(1, 4), UVinf(1, 4), UVsup(1, 4);
  UVinf(1) = myu1min;
  UVinf(2) = myv1min;
  UVinf(3) = myu2min;
  UVinf(4) = myv2min;
  UVsup(1) = myu1sup;
  UVsup(2) = myv1sup;
  UVsup(3) = myu2sup;
  UVsup(4) = myv2sup;

  double distmin = RealLast(), distmax = 0.0, TheDist;

  int    N1Umin = 0, N1Vmin = 0, N2Umin = 0, N2Vmin = 0;
  gp_Pnt PP1min, PP2min;
  int    N1Umax = 0, N1Vmax = 0, N2Umax = 0, N2Vmax = 0;
  gp_Pnt PP1max, PP2max;

  for (NoU1 = 1, U1 = U10; NoU1 <= myusample; NoU1++, U1 += PasU1)
  {
    for (NoV1 = 1, V1 = V10; NoV1 <= myvsample; NoV1++, V1 += PasV1)
    {
      P1 = mypoints1->Value(NoU1, NoV1);
      for (NoU2 = 1, U2 = U20; NoU2 <= myusample; NoU2++, U2 += PasU2)
      {
        for (NoV2 = 1, V2 = V20; NoV2 <= myvsample; NoV2++, V2 += PasV2)
        {
          P2      = mypoints2->Value(NoU2, NoV2);
          TheDist = P1.SquareDistance(P2);
          if (TheDist < distmin)
          {
            distmin = TheDist;
            N1Umin  = NoU1;
            N1Vmin  = NoV1;
            N2Umin  = NoU2;
            N2Vmin  = NoV2;
            PP1min  = P1;
            PP2min  = P2;
          }
          if (TheDist > distmax)
          {
            distmax = TheDist;
            N1Umax  = NoU1;
            N1Vmax  = NoV1;
            N2Umax  = NoU2;
            N2Vmax  = NoV2;
            PP1max  = P1;
            PP2max  = P2;
          }
        }
      }
    }
  }

  UV(1) = U10 + (N1Umin - 1) * PasU1;
  UV(2) = V10 + (N1Vmin - 1) * PasV1;
  UV(3) = U20 + (N2Umin - 1) * PasU2;
  UV(4) = V20 + (N2Vmin - 1) * PasV2;

  Extrema_FuncDistSS aGFSS(S1, *myS2);
  math_BFGS          aBFGSSolver(4);
  aBFGSSolver.Perform(aGFSS, UV);
  if (aBFGSSolver.IsDone())
  {
    aBFGSSolver.Location(UV);

    //  Store result in myF.
    myF.Value(UV, UV);
    myF.GetStateNumber();
  }
  else
  {
    // If optimum is not computed successfully then compute by old approach.

    // Restore initial point.
    UV(1) = U10 + (N1Umin - 1) * PasU1;
    UV(2) = V10 + (N1Vmin - 1) * PasV1;
    UV(3) = U20 + (N2Umin - 1) * PasU2;
    UV(4) = V20 + (N2Vmin - 1) * PasV2;

    math_FunctionSetRoot SR1(myF, Tol);
    SR1.Perform(myF, UV, UVinf, UVsup);
  }

  // math_FunctionSetRoot SR1(myF, Tol);
  // SR1.Perform(myF, UV, UVinf, UVsup);

  UV(1) = U10 + (N1Umax - 1) * PasU1;
  UV(2) = V10 + (N1Vmax - 1) * PasV1;
  UV(3) = U20 + (N2Umax - 1) * PasU2;
  UV(4) = V20 + (N2Vmax - 1) * PasV2;

  // It is impossible to compute max distance in the same manner,
  // since for the distance functional for max have bad definition.
  // So, for max computation old approach is used.
  math_FunctionSetRoot SR2(myF, Tol);
  SR2.Perform(myF, UV, UVinf, UVsup);

  myDone = true;
}

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

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

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

int Extrema_GenExtSS::NbExt() const
{
  if (!IsDone())
  {
    throw StdFail_NotDone();
  }
  return myF.NbExt();
}

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

double Extrema_GenExtSS::SquareDistance(const int N) const
{
  if (N < 1 || N > NbExt())
  {
    throw Standard_OutOfRange();
  }

  return myF.SquareDistance(N);
}

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

const Extrema_POnSurf& Extrema_GenExtSS::PointOnS1(const int N) const
{
  if (N < 1 || N > NbExt())
  {
    throw Standard_OutOfRange();
  }

  return myF.PointOnS1(N);
}

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

const Extrema_POnSurf& Extrema_GenExtSS::PointOnS2(const int N) const
{
  if (N < 1 || N > NbExt())
  {
    throw Standard_OutOfRange();
  }

  return myF.PointOnS2(N);
}