[occt.git] / src / Convert / Convert_PolynomialCosAndSin.cxx

// 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 <TColgp_Array1OfPnt2d.hxx>
#include <gp_Trsf2d.hxx>
#include <gp_Pnt2d.hxx>
#include <gp_Vec2d.hxx>
#include <gp_XY.hxx>

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

#include <Standard_ConstructionError.hxx>

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


void BuildPolynomialCosAndSin
  (const Standard_Real UFirst,
   const Standard_Real ULast,
   const Standard_Integer num_poles,
   Handle(TColStd_HArray1OfReal)& CosNumeratorPtr,
   Handle(TColStd_HArray1OfReal)& SinNumeratorPtr,
   Handle(TColStd_HArray1OfReal)& DenominatorPtr)
{

  Standard_Real  Delta,
  locUFirst,
//  locULast,
//  temp_value,
  t_min,
  t_max,
  trim_min,
  trim_max,
  middle,
  Angle,
  PI2 = 2*M_PI ;
  Standard_Integer 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 
  // parametre 1/2 of Bezier was exactly a point of the bissectrice 
  // of the required angular sector.
  //
  Angle = middle - M_PI ;
  //
  // Circle of radius 1. See Euclid
  //

  TColgp_Array1OfPnt2d 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 = Max(t_min,0.0e0) ;
  t_max = 1.0e0 + (Delta * 1.3e0 / M_PI) ;
  t_max *= 0.5e0 ;
  t_max = 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 ;
  //
  Standard_Real knot_array[2] ;
  Standard_Integer  mults_array[2] ; 
  knot_array[0] = 0.0e0 ;
  knot_array[1] = 1.0e0 ;
  mults_array[0] = degree + 1 ;
  mults_array[1] = degree + 1 ;
  
  TColStd_Array1OfReal  the_knots(knot_array[0],1,2),
  the_new_knots(knot_array[0],1,2);
  TColStd_Array1OfInteger the_mults(mults_array[0],1,2),
  the_new_mults(mults_array[0],1,2) ;
  
  BSplCLib::Trimming(degree,
		     Standard_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
  Standard_Real SinD = Sin(Delta), CosD = Cos(Delta);
  gp_Pnt2d Pdeb(1., 0.);
  gp_Pnt2d Pfin(CosD, SinD);

  Standard_Real 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++) {
    CosNumeratorPtr->SetValue(ii,NewTPoles(ii).X());
    SinNumeratorPtr->SetValue(ii,NewTPoles(ii).Y());
    DenominatorPtr->SetValue(ii,1.);
  }
}

/*
void BuildHermitePolynomialCosAndSin
  (const Standard_Real UFirst,
   const Standard_Real ULast,
   const Standard_Integer num_poles,
   Handle(TColStd_HArray1OfReal)& CosNumeratorPtr,
   Handle(TColStd_HArray1OfReal)& SinNumeratorPtr,
   Handle(TColStd_HArray1OfReal)& DenominatorPtr)
{

  if (num_poles%2 != 0) {
    throw Standard_ConstructionError();
  }
  Standard_Integer ii;
  Standard_Integer ordre_deriv = num_poles/2;
  Standard_Real ang = ULast - UFirst;
  Standard_Real Cd = Cos(UFirst);
  Standard_Real Sd = Sin(UFirst);
  Standard_Real Cf = Cos(ULast);
  Standard_Real Sf = Sin(ULast);
  
  Standard_Integer Degree = num_poles-1;
  TColStd_Array1OfReal FlatKnots(1,2*num_poles);
  TColStd_Array1OfReal Parameters(1,num_poles);
  TColStd_Array1OfInteger ContactOrderArray(1,num_poles);
  TColgp_Array1OfPnt2d Poles(1,num_poles);
  TColgp_Array1OfPnt2d TPoles(1,num_poles);
 

  for (ii=1; ii<=num_poles; ii++) {
    FlatKnots(ii) = 0.;
    FlatKnots(ii+num_poles) = 1.;
  }

  Standard_Real coef = 1.;
  Standard_Real xd,yd,xf,yf; 

  for (ii=1; ii<=ordre_deriv; ii++) {
    Parameters(ii) = 0.;
    Parameters(ii+ordre_deriv) = 1.;

    ContactOrderArray(ii) = ContactOrderArray(num_poles-ii+1) = ii-1;

    switch ((ii-1)%4) {
    case 0:
      {
	xd = Cd*coef;
	yd = Sd*coef;
	xf = Cf*coef;
	yf = Sf*coef;
      }
      break;
    case 1:
      {
	xd = -Sd*coef;
	yd =  Cd*coef;
	xf = -Sf*coef;
	yf =  Cf*coef;
      }
      break;
    case 2:
      {
	xd = -Cd*coef;
	yd = -Sd*coef;
	xf = -Cf*coef;
	yf = -Sf*coef;
      }
      break;
    case 3:
      {
	xd =  Sd*coef;
	yd = -Cd*coef;
	xf =  Sf*coef;
	yf = -Cf*coef;
      }
      break;
    }

    Poles(ii).SetX(xd);
    Poles(ii).SetY(yd);
    Poles(num_poles-ii+1).SetX(xf);
    Poles(num_poles-ii+1).SetY(yf);

    coef *= ang;
  }

  Standard_Integer InversionPb;
  BSplCLib::Interpolate(Degree,FlatKnots,Parameters,
			ContactOrderArray,Poles,InversionPb);

  if (InversionPb !=0) {
    throw Standard_ConstructionError();
  }
  for (ii=1; ii<=num_poles; ii++) {
    CosNumeratorPtr->SetValue(ii,Poles(ii).X());
    SinNumeratorPtr->SetValue(ii,Poles(ii).Y());
    DenominatorPtr->SetValue(ii,1.);
  }

}
*/
Commit	Line	Data
b311480e	1	// Created on: 1995-10-10
	2	// Created by: Jacques GOUSSARD
	3	// Copyright (c) 1995-1999 Matra Datavision
973c2be1	4	// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e	5	//
973c2be1	6	// This file is part of Open CASCADE Technology software library.
b311480e	7	//
d5f74e42	8	// This library is free software; you can redistribute it and/or modify it under
d5f74e42	9	// the terms of the GNU Lesser General Public License version 2.1 as published
973c2be1	10	// by the Free Software Foundation, with special exception defined in the file
	11	// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
	12	// distribution for complete text of the license and disclaimer of any warranty.
b311480e	13	//
973c2be1	14	// Alternatively, this file may be used under the terms of Open CASCADE
973c2be1	15	// commercial license or contractual agreement.
7fd59977	16
	17	#include <Convert_PolynomialCosAndSin.hxx>
	18
	19	#include <TColgp_Array1OfPnt2d.hxx>
	20	#include <gp_Trsf2d.hxx>
	21	#include <gp_Pnt2d.hxx>
	22	#include <gp_Vec2d.hxx>
	23	#include <gp_XY.hxx>
	24
	25	#include <gp.hxx>
	26	#include <Precision.hxx>
	27	#include <PLib.hxx>
	28	#include <BSplCLib.hxx>
	29
	30	#include <Standard_ConstructionError.hxx>
	31
	32	static Standard_Real Locate(const Standard_Real Angfin,
	33	const TColgp_Array1OfPnt2d& TPoles,
	34	const Standard_Real Umin,
	35	const Standard_Real Umax)
	36	{
	37	Standard_Real umin = Umin;
	38	Standard_Real umax = Umax;
	39	Standard_Real Ptol = Precision::Angular();
	40	Standard_Real Utol = Precision::PConfusion();
	41	while (Abs(umax-umin)>= Utol) {
	42	Standard_Real ptest = (umax+umin)/2.;
	43	gp_Pnt2d valP;
	44	BSplCLib::D0(ptest,TPoles,BSplCLib::NoWeights(),valP);
	45	Standard_Real theta = ATan2(valP.Y(),valP.X());
	46	if (theta < 0.) {
c6541a0c	47	theta +=2.*M_PI;
7fd59977	48	}
	49	if (Abs(theta - Angfin) < Ptol) {
	50	return ptest;
	51	}
	52	if (theta < Angfin) {
	53	umin = ptest;
	54	}
	55	else if (theta > Angfin) {
	56	umax = ptest;
	57	}
	58	}
	59	return (umin+umax)/2.;
	60	}
	61
	62
	63	void BuildPolynomialCosAndSin
	64	(const Standard_Real UFirst,
	65	const Standard_Real ULast,
	66	const Standard_Integer num_poles,
	67	Handle(TColStd_HArray1OfReal)& CosNumeratorPtr,
	68	Handle(TColStd_HArray1OfReal)& SinNumeratorPtr,
	69	Handle(TColStd_HArray1OfReal)& DenominatorPtr)
	70	{
	71
	72	Standard_Real Delta,
	73	locUFirst,
	74	// locULast,
	75	// temp_value,
	76	t_min,
	77	t_max,
	78	trim_min,
	79	trim_max,
	80	middle,
	81	Angle,
c6541a0c	82	PI2 = 2*M_PI ;
7fd59977	83	Standard_Integer ii, degree = num_poles -1 ;
	84	locUFirst = UFirst ;
	85
0d969553 Y	86	// Return UFirst in [-2PI; 2PI]
0d969553 Y	87	// to make rotations without risk
7fd59977	88	while (locUFirst > PI2) {
	89	locUFirst -= PI2;
	90	}
	91	while (locUFirst < - PI2) {
	92	locUFirst += PI2;
	93	}
	94
0d969553	95	// Return to the arc [0, Delta]
7fd59977	96	Delta = ULast - UFirst;
7fd59977	97	middle = 0.5e0 * Delta ;
0d969553 Y	98
	99	// coincide the required bisector of the angular sector with
	100	// axis -Ox definition of the circle in Bezier of degree 7 so that
	101	// parametre 1/2 of Bezier was exactly a point of the bissectrice
	102	// of the required angular sector.
7fd59977	103	//
c6541a0c	104	Angle = middle - M_PI ;
7fd59977	105	//
0d969553	106	// Circle of radius 1. See Euclid
7fd59977	107	//
	108
	109	TColgp_Array1OfPnt2d TPoles(1,8),
	110	NewTPoles(1,8) ;
	111	TPoles(1).SetCoord(1.,0.);
	112	TPoles(2).SetCoord(1.,1.013854);
	113	TPoles(3).SetCoord(-0.199043,1.871905);
	114	TPoles(4).SetCoord(-1.937729,1.057323);
	115	TPoles(5).SetCoord(-1.937729,-1.057323);
	116	TPoles(6).SetCoord(-0.199043,-1.871905);
	117	TPoles(7).SetCoord(1.,-1.013854);
	118	TPoles(8).SetCoord(1.,0.);
	119	gp_Trsf2d T;
	120	T.SetRotation(gp::Origin2d(),Angle);
	121	for (ii=1; ii<=num_poles; ii++) {
	122	TPoles(ii).Transform(T);
	123	}
	124
	125
c6541a0c	126	t_min = 1.0e0 - (Delta * 1.3e0 / M_PI) ;
7fd59977	127	t_min *= 0.5e0 ;
7fd59977	128	t_min = Max(t_min,0.0e0) ;
c6541a0c	129	t_max = 1.0e0 + (Delta * 1.3e0 / M_PI) ;
7fd59977	130	t_max *= 0.5e0 ;
	131	t_max = Min(t_max,1.0e0) ;
	132	trim_max = Locate(Delta,
	133	TPoles,
	134	t_min,
	135	t_max);
	136	//
0d969553 Y	137	// as Bezier is symmetric correspondingly to the bissector
0d969553 Y	138	// of the angular sector ...
7fd59977	139
	140	trim_min = 1.0e0 - trim_max ;
	141	//
	142	Standard_Real knot_array[2] ;
	143	Standard_Integer mults_array[2] ;
	144	knot_array[0] = 0.0e0 ;
	145	knot_array[1] = 1.0e0 ;
	146	mults_array[0] = degree + 1 ;
	147	mults_array[1] = degree + 1 ;
	148
	149	TColStd_Array1OfReal the_knots(knot_array[0],1,2),
	150	the_new_knots(knot_array[0],1,2);
	151	TColStd_Array1OfInteger the_mults(mults_array[0],1,2),
	152	the_new_mults(mults_array[0],1,2) ;
	153
	154	BSplCLib::Trimming(degree,
	155	Standard_False,
	156	the_knots,
	157	the_mults,
	158	TPoles,
	159	BSplCLib::NoWeights(),
	160	trim_min,
	161	trim_max,
	162	the_new_knots,
	163	the_new_mults,
	164	NewTPoles,
	165	BSplCLib::NoWeights());
	166
0d969553	167	// readjustment is obviously redundant
7fd59977	168	Standard_Real SinD = Sin(Delta), CosD = Cos(Delta);
	169	gp_Pnt2d Pdeb(1., 0.);
	170	gp_Pnt2d Pfin(CosD, SinD);
	171
	172	Standard_Real dtg = NewTPoles(1).Distance(NewTPoles(2));
	173	NewTPoles(1) = Pdeb;
	174	gp_XY theXY(0.,dtg);
	175	Pdeb.ChangeCoord() += theXY;
	176	NewTPoles(2) = Pdeb;
	177
0d969553	178	// readjustment to Euclid
7fd59977	179	dtg = NewTPoles(num_poles).Distance(NewTPoles(num_poles-1));
	180	NewTPoles(num_poles) = Pfin;
	181	theXY.SetCoord(dtgSinD,-dtgCosD);
	182	Pfin.ChangeCoord() += theXY;
	183	NewTPoles(num_poles-1) = Pfin;
	184
0d969553	185	// Rotation to return to the arc [LocUFirst, LocUFirst+Delta]
7fd59977	186	T.SetRotation(gp::Origin2d(), locUFirst);
	187	for (ii=1; ii<=num_poles; ii++) {
	188	NewTPoles(ii).Transform(T);
	189	}
	190
	191	for (ii=1; ii<=num_poles; ii++) {
	192	CosNumeratorPtr->SetValue(ii,NewTPoles(ii).X());
	193	SinNumeratorPtr->SetValue(ii,NewTPoles(ii).Y());
	194	DenominatorPtr->SetValue(ii,1.);
	195	}
	196	}
	197
	198	/*
	199	void BuildHermitePolynomialCosAndSin
	200	(const Standard_Real UFirst,
	201	const Standard_Real ULast,
	202	const Standard_Integer num_poles,
	203	Handle(TColStd_HArray1OfReal)& CosNumeratorPtr,
	204	Handle(TColStd_HArray1OfReal)& SinNumeratorPtr,
	205	Handle(TColStd_HArray1OfReal)& DenominatorPtr)
	206	{
	207
	208	if (num_poles%2 != 0) {
9775fa61	209	throw Standard_ConstructionError();
7fd59977	210	}
	211	Standard_Integer ii;
	212	Standard_Integer ordre_deriv = num_poles/2;
	213	Standard_Real ang = ULast - UFirst;
	214	Standard_Real Cd = Cos(UFirst);
	215	Standard_Real Sd = Sin(UFirst);
	216	Standard_Real Cf = Cos(ULast);
	217	Standard_Real Sf = Sin(ULast);
	218
	219	Standard_Integer Degree = num_poles-1;
	220	TColStd_Array1OfReal FlatKnots(1,2*num_poles);
	221	TColStd_Array1OfReal Parameters(1,num_poles);
	222	TColStd_Array1OfInteger ContactOrderArray(1,num_poles);
	223	TColgp_Array1OfPnt2d Poles(1,num_poles);
	224	TColgp_Array1OfPnt2d TPoles(1,num_poles);
	225
	226
	227	for (ii=1; ii<=num_poles; ii++) {
	228	FlatKnots(ii) = 0.;
	229	FlatKnots(ii+num_poles) = 1.;
	230	}
	231
	232	Standard_Real coef = 1.;
	233	Standard_Real xd,yd,xf,yf;
	234
	235	for (ii=1; ii<=ordre_deriv; ii++) {
	236	Parameters(ii) = 0.;
	237	Parameters(ii+ordre_deriv) = 1.;
	238
	239	ContactOrderArray(ii) = ContactOrderArray(num_poles-ii+1) = ii-1;
	240
	241	switch ((ii-1)%4) {
	242	case 0:
	243	{
	244	xd = Cd*coef;
	245	yd = Sd*coef;
	246	xf = Cf*coef;
	247	yf = Sf*coef;
	248	}
	249	break;
	250	case 1:
	251	{
	252	xd = -Sd*coef;
	253	yd = Cd*coef;
	254	xf = -Sf*coef;
	255	yf = Cf*coef;
	256	}
	257	break;
	258	case 2:
	259	{
	260	xd = -Cd*coef;
	261	yd = -Sd*coef;
	262	xf = -Cf*coef;
	263	yf = -Sf*coef;
	264	}
	265	break;
	266	case 3:
	267	{
	268	xd = Sd*coef;
	269	yd = -Cd*coef;
	270	xf = Sf*coef;
	271	yf = -Cf*coef;
	272	}
	273	break;
274	}
275
276	Poles(ii).SetX(xd);
277	Poles(ii).SetY(yd);
278	Poles(num_poles-ii+1).SetX(xf);
279	Poles(num_poles-ii+1).SetY(yf);
280
281	coef *= ang;
282	}
283
284	Standard_Integer InversionPb;
285	BSplCLib::Interpolate(Degree,FlatKnots,Parameters,
286	ContactOrderArray,Poles,InversionPb);
287
288	if (InversionPb !=0) {
9775fa61	289	throw Standard_ConstructionError();
7fd59977	290	}
	291	for (ii=1; ii<=num_poles; ii++) {
	292	CosNumeratorPtr->SetValue(ii,Poles(ii).X());
	293	SinNumeratorPtr->SetValue(ii,Poles(ii).Y());
	294	DenominatorPtr->SetValue(ii,1.);
	295	}
	296
	297	}
	298	*/