OCCT

// Created on: 1997-02-12
// Created by: Laurent BOURESCHE
// Copyright (c) 1997-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_Curve.hxx>
#include <BRepBlend_CurvPointRadInv.hxx>
#include <gp_Pnt.hxx>
#include <math_Matrix.hxx>

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

BRepBlend_CurvPointRadInv::BRepBlend_CurvPointRadInv(const occ::handle<Adaptor3d_Curve>& C1,
                                                     const occ::handle<Adaptor3d_Curve>& C2)
    : curv1(C1),
      curv2(C2),
      choix(0)
{
}

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

void BRepBlend_CurvPointRadInv::Set(const int Choix)
{
  choix = Choix;
}

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

int BRepBlend_CurvPointRadInv::NbEquations() const
{
  return 2;
}

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

bool BRepBlend_CurvPointRadInv::Value(const math_Vector& X, math_Vector& F)
{
  double theD;
  gp_Pnt ptcur1, ptcur2;
  gp_Vec d1cur1, d1cur2;
  gp_XYZ nplan; //, ref;
  curv1->D1(X(1), ptcur1, d1cur1);
  nplan = d1cur1.Normalized().XYZ();
  theD  = -(nplan.Dot(ptcur1.XYZ()));
  curv2->D1(X(2), ptcur2, d1cur2);
  F(1) = nplan.Dot(point.XYZ()) + theD;
  F(2) = nplan.Dot(ptcur2.XYZ()) + theD;
  return true;
}

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

bool BRepBlend_CurvPointRadInv::Derivatives(const math_Vector& X, math_Matrix& D)
{
  gp_Pnt ptcur1, ptcur2;
  gp_Vec d1cur1, d2cur1, d1cur2, nplan, dnplan;
  double dtheD, normd1cur1, unsurnormd1cur1;

  curv1->D2(X(1), ptcur1, d1cur1, d2cur1);

  normd1cur1      = d1cur1.Magnitude();
  unsurnormd1cur1 = 1. / normd1cur1;
  nplan           = unsurnormd1cur1 * d1cur1;
  dnplan.SetLinearForm(-nplan.Dot(d2cur1), nplan, d2cur1);
  dnplan.Multiply(unsurnormd1cur1);
  dtheD   = -nplan.XYZ().Dot(d1cur1.XYZ()) - dnplan.XYZ().Dot(ptcur1.XYZ());
  D(1, 1) = dnplan.XYZ().Dot(point.XYZ()) + dtheD;
  D(1, 2) = 0.;
  curv2->D1(X(2), ptcur2, d1cur2);
  D(2, 1) = dnplan.XYZ().Dot(ptcur2.XYZ()) + dtheD;
  D(2, 2) = nplan.Dot(d1cur2);

  return true;
}

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

bool BRepBlend_CurvPointRadInv::Values(const math_Vector& X, math_Vector& F, math_Matrix& D)
{
  Value(X, F);
  Derivatives(X, D);

  return true;
}

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

void BRepBlend_CurvPointRadInv::Set(const gp_Pnt& P)
{
  point = P;
}

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

void BRepBlend_CurvPointRadInv::GetTolerance(math_Vector& Tolerance, const double Tol) const
{
  Tolerance(1) = curv1->Resolution(Tol);
  Tolerance(2) = curv2->Resolution(Tol);
}

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

void BRepBlend_CurvPointRadInv::GetBounds(math_Vector& InfBound, math_Vector& SupBound) const
{
  InfBound(1) = curv1->FirstParameter();
  SupBound(1) = curv1->LastParameter();
  InfBound(2) = curv2->FirstParameter();
  SupBound(2) = curv2->LastParameter();
}

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

bool BRepBlend_CurvPointRadInv::IsSolution(const math_Vector& Sol, const double Tol)
{
  math_Vector valsol(1, 2);
  Value(Sol, valsol);
  return std::abs(valsol(1)) <= Tol && std::abs(valsol(2)) <= Tol;
}