OCCT

// Created on: 1995-10-10
// Created by: Jacques GOUSSARD
// 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 <Convert_PolynomialCosAndSin.hxx>

#include <gp_Trsf2d.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_XY.hxx>

#include <BSplCLib.hxx>
#include <gp.hxx>
#include <Precision.hxx>

#include <array>

static double Locate(const double                        Angfin,
                     const NCollection_Array1<gp_Pnt2d>& TPoles,
                     const double                        Umin,
                     const double                        Umax)
{
  double umin = Umin;
  double umax = Umax;
  double Ptol = Precision::Angular();
  double Utol = Precision::PConfusion();
  while (std::abs(umax - umin) >= Utol)
  {
    double   ptest = (umax + umin) / 2.;
    gp_Pnt2d valP;
    BSplCLib::D0(ptest, TPoles, BSplCLib::NoWeights(), valP);
    double theta = std::atan2(valP.Y(), valP.X());
    if (theta < 0.)
    {
      theta += 2. * M_PI;
    }
    if (std::abs(theta - Angfin) < Ptol)
    {
      return ptest;
    }
    if (theta < Angfin)
    {
      umin = ptest;
    }
    else if (theta > Angfin)
    {
      umax = ptest;
    }
  }
  return (umin + umax) / 2.;
}

void BuildPolynomialCosAndSin(const double                UFirst,
                              const double                ULast,
                              const int                   num_poles,
                              NCollection_Array1<double>& CosNumerator,
                              NCollection_Array1<double>& SinNumerator,
                              NCollection_Array1<double>& Denominator)
{

  double Delta, locUFirst,
    //  locULast,
    //  temp_value,
    t_min, t_max, trim_min, trim_max, middle, Angle, PI2 = 2 * M_PI;
  int ii, degree = num_poles - 1;
  locUFirst = UFirst;

  // Return UFirst in [-2PI; 2PI]
  // to make rotations without risk
  while (locUFirst > PI2)
  {
    locUFirst -= PI2;
  }
  while (locUFirst < -PI2)
  {
    locUFirst += PI2;
  }

  // Return to the arc [0, Delta]
  Delta  = ULast - UFirst;
  middle = 0.5e0 * Delta;

  // Coincide the required bisector of the angular sector with
  // axis -Ox. Definition of the circle in Bezier of degree 7 so that
  // parameter 1/2 of Bezier is exactly a point of the bisectrix
  // of the required angular sector.
  Angle = middle - M_PI;

  // Bezier control points for a unit circle (degree-7 polynomial approximation).
  // Coefficients from Tiller's algorithm for polynomial cos/sin representation.
  NCollection_Array1<gp_Pnt2d> TPoles(1, 8), NewTPoles(1, 8);
  TPoles(1).SetCoord(1., 0.);
  TPoles(2).SetCoord(1., 1.013854);
  TPoles(3).SetCoord(-0.199043, 1.871905);
  TPoles(4).SetCoord(-1.937729, 1.057323);
  TPoles(5).SetCoord(-1.937729, -1.057323);
  TPoles(6).SetCoord(-0.199043, -1.871905);
  TPoles(7).SetCoord(1., -1.013854);
  TPoles(8).SetCoord(1., 0.);
  gp_Trsf2d T;
  T.SetRotation(gp::Origin2d(), Angle);
  for (ii = 1; ii <= num_poles; ii++)
  {
    TPoles(ii).Transform(T);
  }

  t_min = 1.0e0 - (Delta * 1.3e0 / M_PI);
  t_min *= 0.5e0;
  t_min = std::max(t_min, 0.0e0);
  t_max = 1.0e0 + (Delta * 1.3e0 / M_PI);
  t_max *= 0.5e0;
  t_max    = std::min(t_max, 1.0e0);
  trim_max = Locate(Delta, TPoles, t_min, t_max);
  //
  // as Bezier is symmetric correspondingly to the bissector
  // of the angular sector ...

  trim_min = 1.0e0 - trim_max;
  //
  std::array<double, 2> knot_array  = {0.0e0, 1.0e0};
  std::array<int, 2>    mults_array = {degree + 1, degree + 1};

  NCollection_Array1<double> the_knots(knot_array[0], 1, 2), the_new_knots(knot_array[0], 1, 2);
  NCollection_Array1<int>    the_mults(mults_array[0], 1, 2), the_new_mults(mults_array[0], 1, 2);

  BSplCLib::Trimming(degree,
                     false,
                     the_knots,
                     the_mults,
                     TPoles,
                     BSplCLib::NoWeights(),
                     trim_min,
                     trim_max,
                     the_new_knots,
                     the_new_mults,
                     NewTPoles,
                     BSplCLib::NoWeights());

  // readjustment is obviously redundant
  double   SinD = std::sin(Delta), CosD = std::cos(Delta);
  gp_Pnt2d Pdeb(1., 0.);
  gp_Pnt2d Pfin(CosD, SinD);

  double dtg   = NewTPoles(1).Distance(NewTPoles(2));
  NewTPoles(1) = Pdeb;
  gp_XY theXY(0., dtg);
  Pdeb.ChangeCoord() += theXY;
  NewTPoles(2) = Pdeb;

  // readjustment to Euclid
  dtg                  = NewTPoles(num_poles).Distance(NewTPoles(num_poles - 1));
  NewTPoles(num_poles) = Pfin;
  theXY.SetCoord(dtg * SinD, -dtg * CosD);
  Pfin.ChangeCoord() += theXY;
  NewTPoles(num_poles - 1) = Pfin;

  // Rotation to return to the arc [LocUFirst, LocUFirst+Delta]
  T.SetRotation(gp::Origin2d(), locUFirst);
  for (ii = 1; ii <= num_poles; ii++)
  {
    NewTPoles(ii).Transform(T);
  }

  for (ii = 1; ii <= num_poles; ii++)
  {
    CosNumerator(ii) = NewTPoles(ii).X();
    SinNumerator(ii) = NewTPoles(ii).Y();
    Denominator(ii)  = 1.;
  }
}