OCCT

// Created on: 1996-01-09
// Created by: Laurent PAINNOT
// 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.

#include <Adaptor3d_Surface.hxx>
#include <Extrema_FuncExtSS.hxx>
#include <Extrema_POnSurf.hxx>
#include <gp_Vec.hxx>
#include <math_Matrix.hxx>
#include <Standard_TypeMismatch.hxx>

/*----------------------------------------------------------------------------
 Si on note Du1s et Dv1s, les derivees en u1 et v1, les 2 fonctions a annuler sont:
 Si on note Du2s et Dv2s, les derivees en u2 et v2, les 2 fonctions a annuler sont:
 { F1(u1,v1,u2,v2) = (S1(u1,v1)-S2(u2,v2)).Du1s1(u1,v1) }
 { F2(u1,v1,u2,v2) = (S1(u1,v1)-S2(u2,v2)).Dv1s1(u1,v1) }
 { F3(u1,v1,u2,v2) = (S1(u1,v1)-S2(u2,v2)).Du2s2(u2,v2) }
 { F4(u1,v1,u2,v2) = (S1(u1,v1)-S2(u2,v2)).Dv2s2(u2,v2) }
 { du1f1(u1,v1,u2,v2) = Du1s1(u1,v1).Du1s1(u1,v1)+(S1(u1,v1)-S2(u2,v2)).Du1u1s1(u1,v1)
                      = ||Du1s1(u1,v1)||**2      +(S1(u1,v1)-S2(u2,v2)).Du1u1s1(u1,v1) }
 { dv1f1(u1,v1,u2,v2) = Dv1s1(u1,v1).Du1s1(u1,v1)+(S1(u1,v1)-S2(u2,v2)).Du1v1s1(u1,v1) }
 { du2f1(u1,v1,u2,v2) = -Du2s2(u2,v2).Du1s1(u1,v1) }
 { dv2f1(u1,v1,u2,v2) = -Dv2s2(u2,v2).Du1s1(u1,v1) }
 { du1f2(u1,v1,u2,v2) = Du1s1(u1,v1).Dv1s1(u1,v1)+(S1(u1,v1)-S2(u2,v2)).Du1v1s1(u1,v1) }
 { dv1f2(u1,v1,u2,v2) = Dv1s1(u1,v1).Dv1s1(u1,v1)+(S1(u1,v1)-S2(u2,v2)).Dv1v1s1(u1,v1)
                      = ||Dv1s1(u1,v1)||**2      +(S1(u1,v1)-S2(u2,v2)).Dv1v1s1(u1,v1) }
 { du2f2(u1,v1,u2,v2) = -Du2s2(u2,v2).Dv1s1(u1,v1) }
 { dv2f2(u1,v1,u2,v2) = -Dv2s2(u2,v2).Dv1s1(u1,v1) }
 { du1f3(u1,v1,u2,v2) = Du1s1(u1,v1).Du2s2(u2,v2) }
 { dv1f3(u1,v1,u2,v2) = Dv1s1(u1,v1).Du2s2(u2,v2) }
 { du2f3(u1,v1,u2,v2) = -Du2s2(u2,v2).Du2s2(u2,v2)+(S1(u1,v1)-S2(u2,v2)).Du2u2s2(u2,v2)
                      = -||Du2s2(u2,v2)||**2      +(S1(u1,v1)-S2(u2,v2)).Du2u2s2(u2,v2) }
 { dv2f3(u1,v1,u2,v2) = -Dv2s2(u2,v2).Du2s2(u2,v2)+(S1(u1,v1)-S2(u2,v2)).Dv2u2s2(u2,v2) }
 { du1f4(u1,v1,u2,v2) = Du1s1(u1,v1).Dv2s2(u2,v2) }
 { dv1f4(u1,v1,u2,v2) = Dv1s1(u1,v1).Dv2s2(u2,v2) }
 { du2f4(u1,v1,u2,v2) = -Du2s2(u2,v2).Dv2s2(u2,v2)+(S1(u1,v1)-S2(u2,v2)).Du2v2s2(u2,v2) }
 { dv2f4(u1,v1,u2,v2) = -Dv2s2(u2,v2).Dv2s2(u2,v2)+(S1(u1,v1)-S2(u2,v2)).Dv2v2s2(u2,v2)
                      = -||Dv2s2(u2,v2)||**2      +(S1(u1,v1)-S2(u2,v2)).Dv2v2s2(u2,v2) }
----------------------------------------------------------------------------*/
//=================================================================================================

Extrema_FuncExtSS::Extrema_FuncExtSS()
    : myS1(nullptr),
      myS2(nullptr),
      myU1(0.0),
      myV1(0.0),
      myU2(0.0),
      myV2(0.0)
{
  myS1init = false;
  myS2init = false;
}

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

Extrema_FuncExtSS::Extrema_FuncExtSS(const Adaptor3d_Surface& S1, const Adaptor3d_Surface& S2)
    : myU1(0.0),
      myV1(0.0),
      myU2(0.0),
      myV2(0.0)
{
  myS1     = &S1;
  myS2     = &S2;
  myS1init = true;
  myS2init = true;
}

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

void Extrema_FuncExtSS::Initialize(const Adaptor3d_Surface& S1, const Adaptor3d_Surface& S2)
{
  myS1     = &S1;
  myS2     = &S2;
  myS1init = true;
  myS2init = true;
  myPoint1.Clear();
  myPoint2.Clear();
  mySqDist.Clear();
}

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

int Extrema_FuncExtSS::NbVariables() const
{
  return 4;
}

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

int Extrema_FuncExtSS::NbEquations() const
{
  return 4;
}

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

bool Extrema_FuncExtSS::Value(const math_Vector& UV, math_Vector& F)
{
  if (!myS1init || !myS2init)
  {
    throw Standard_TypeMismatch();
  }
  myU1 = UV(1);
  myV1 = UV(2);
  myU2 = UV(3);
  myV2 = UV(4);
  gp_Vec Du1s1, Dv1s1;
  gp_Vec Du2s2, Dv2s2;
  myS1->D1(myU1, myV1, myP1, Du1s1, Dv1s1);
  myS2->D1(myU2, myV2, myP2, Du2s2, Dv2s2);

  gp_Vec P1P2(myP2, myP1);

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

  return true;
}

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

bool Extrema_FuncExtSS::Derivatives(const math_Vector& UV, math_Matrix& Df)
{
  math_Vector F(1, 4);
  return Values(UV, F, Df);
}

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

bool Extrema_FuncExtSS::Values(const math_Vector& UV, math_Vector& F, math_Matrix& Df)
{
  if (!myS1init || !myS2init)
  {
    throw Standard_TypeMismatch();
  }
  myU1 = UV(1);
  myV1 = UV(2);
  myU2 = UV(3);
  myV2 = UV(4);
  gp_Vec Du1s1, Dv1s1, Du1u1s1, Dv1v1s1, Du1v1s1;
  gp_Vec Du2s2, Dv2s2, Du2u2s2, Dv2v2s2, Du2v2s2;
  myS1->D2(myU1, myV1, myP1, Du1s1, Dv1s1, Du1u1s1, Dv1v1s1, Du1v1s1);
  myS2->D2(myU2, myV2, myP2, Du2s2, Dv2s2, Du2u2s2, Dv2v2s2, Du2v2s2);

  gp_Vec P1P2(myP2, myP1);

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

  Df(1, 1) = Du1s1.SquareMagnitude() + P1P2.Dot(Du1u1s1);
  Df(1, 2) = Dv1s1.Dot(Du1s1) + P1P2.Dot(Du1v1s1);
  Df(1, 3) = -Du2s2.Dot(Du1s1);
  Df(1, 4) = -Dv2s2.Dot(Du1s1);

  Df(2, 1) = Df(1, 2); // Du1s1.Dot(Dv1s1) + P1P2.Dot(Du1v1s1);
  Df(2, 2) = Dv1s1.SquareMagnitude() + P1P2.Dot(Dv1v1s1);
  Df(2, 3) = -Du2s2.Dot(Dv1s1);
  Df(2, 4) = -Dv2s2.Dot(Dv1s1);

  Df(3, 1) = -Df(1, 3); // Du1s1.Dot(Du2s2);
  Df(3, 2) = -Df(2, 3); // Dv1s1.Dot(Du2s2);
  Df(3, 3) = -Du2s2.SquareMagnitude() + P1P2.Dot(Du2u2s2);
  Df(3, 4) = -Dv2s2.Dot(Du2s2) + P1P2.Dot(Du2v2s2);

  Df(4, 1) = -Df(1, 4); // Du1s1.Dot(Dv2s2);
  Df(4, 2) = -Df(2, 4); // Dv1s1.Dot(Dv2s2);
  Df(4, 3) = Df(3, 4);  // -Du2s2.Dot(Dv2s2) + P1P2.Dot(Du2v2s2);
  Df(4, 4) = -Dv2s2.SquareMagnitude() + P1P2.Dot(Dv2v2s2);

  return true;
}

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

int Extrema_FuncExtSS::GetStateNumber()
{
  if (!myS1init || !myS2init)
  {
    throw Standard_TypeMismatch();
  }

  mySqDist.Append(myP1.SquareDistance(myP2));
  myPoint1.Append(Extrema_POnSurf(myU1, myV1, myP1));
  myPoint2.Append(Extrema_POnSurf(myU2, myV2, myP2));
  return 0;
}

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

int Extrema_FuncExtSS::NbExt() const
{
  return mySqDist.Length();
}

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

double Extrema_FuncExtSS::SquareDistance(const int N) const
{
  if (!myS1init || !myS2init)
  {
    throw Standard_TypeMismatch();
  }
  return mySqDist.Value(N);
}

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

const Extrema_POnSurf& Extrema_FuncExtSS::PointOnS1(const int N) const
{
  if (!myS1init || !myS2init)
  {
    throw Standard_TypeMismatch();
  }
  return myPoint1.Value(N);
}

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

const Extrema_POnSurf& Extrema_FuncExtSS::PointOnS2(const int N) const
{
  if (!myS1init || !myS2init)
  {
    throw Standard_TypeMismatch();
  }
  return myPoint2.Value(N);
}