[occt.git] / src / Geom2dEvaluator / Geom2dEvaluator_OffsetCurve.cxx

// Created on: 2015-09-21
// Copyright (c) 2015 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 <Geom2dEvaluator_OffsetCurve.hxx>

#include <Geom2dAdaptor_HCurve.hxx>
#include <Standard_NullValue.hxx>


IMPLEMENT_STANDARD_RTTIEXT(Geom2dEvaluator_OffsetCurve,Geom2dEvaluator_Curve)

Geom2dEvaluator_OffsetCurve::Geom2dEvaluator_OffsetCurve(
        const Handle(Geom2d_Curve)& theBase,
        const Standard_Real theOffset)
  : Geom2dEvaluator_Curve(),
    myBaseCurve(theBase),
    myOffset(theOffset)
{
}

Geom2dEvaluator_OffsetCurve::Geom2dEvaluator_OffsetCurve(
        const Handle(Geom2dAdaptor_HCurve)& theBase,
        const Standard_Real theOffset)
  : Geom2dEvaluator_Curve(),
    myBaseAdaptor(theBase),
    myOffset(theOffset)
{
}

void Geom2dEvaluator_OffsetCurve::D0(const Standard_Real theU,
                                           gp_Pnt2d& theValue) const
{
  gp_Vec2d aD1;
  BaseD1(theU, theValue, aD1);
  CalculateD0(theValue, aD1);
}

void Geom2dEvaluator_OffsetCurve::D1(const Standard_Real theU,
                                           gp_Pnt2d& theValue,
                                           gp_Vec2d& theD1) const
{
  gp_Vec2d aD2;
  BaseD2(theU, theValue, theD1, aD2);
  CalculateD1(theValue, theD1, aD2);
}

void Geom2dEvaluator_OffsetCurve::D2(const Standard_Real theU,
                                           gp_Pnt2d& theValue,
                                           gp_Vec2d& theD1,
                                           gp_Vec2d& theD2) const
{
  gp_Vec2d aD3;
  BaseD3(theU, theValue, theD1, theD2, aD3);

  Standard_Boolean isDirectionChange = Standard_False;
  if (theD1.SquareMagnitude() <= gp::Resolution())
  {
    gp_Vec2d aDummyD4;
    isDirectionChange = AdjustDerivative(3, theU, theD1, theD2, aD3, aDummyD4);
  }

  CalculateD2(theValue, theD1, theD2, aD3, isDirectionChange);
}

void Geom2dEvaluator_OffsetCurve::D3(const Standard_Real theU,
                                           gp_Pnt2d& theValue,
                                           gp_Vec2d& theD1,
                                           gp_Vec2d& theD2,
                                           gp_Vec2d& theD3) const
{
  gp_Vec2d aD4;
  BaseD4(theU, theValue, theD1, theD2, theD3, aD4);

  Standard_Boolean isDirectionChange = Standard_False;
  if (theD1.SquareMagnitude() <= gp::Resolution())
    isDirectionChange = AdjustDerivative(4, theU, theD1, theD2, theD3, aD4);

  CalculateD3(theValue, theD1, theD2, theD3, aD4, isDirectionChange);
}

gp_Vec2d Geom2dEvaluator_OffsetCurve::DN(const Standard_Real theU,
                                         const Standard_Integer theDeriv) const
{
  Standard_RangeError_Raise_if(theDeriv < 1, "Geom2dEvaluator_OffsetCurve::DN(): theDeriv < 1");

  gp_Pnt2d aPnt;
  gp_Vec2d aDummy, aDN;
  switch (theDeriv)
  {
  case 1:
    D1(theU, aPnt, aDN);
    break;
  case 2:
    D2(theU, aPnt, aDummy, aDN);
    break;
  case 3:
    D3(theU, aPnt, aDummy, aDummy, aDN);
    break;
  default:
    aDN = BaseDN(theU, theDeriv);
  }
  return aDN;
}


void Geom2dEvaluator_OffsetCurve::BaseD0(const Standard_Real theU,
                                               gp_Pnt2d& theValue) const
{
  if (!myBaseAdaptor.IsNull())
    myBaseAdaptor->D0(theU, theValue);
  else
    myBaseCurve->D0(theU, theValue);
}

void Geom2dEvaluator_OffsetCurve::BaseD1(const Standard_Real theU,
                                               gp_Pnt2d& theValue,
                                               gp_Vec2d& theD1) const
{
  if (!myBaseAdaptor.IsNull())
    myBaseAdaptor->D1(theU, theValue, theD1);
  else
    myBaseCurve->D1(theU, theValue, theD1);
}

void Geom2dEvaluator_OffsetCurve::BaseD2(const Standard_Real theU,
                                               gp_Pnt2d& theValue,
                                               gp_Vec2d& theD1,
                                               gp_Vec2d& theD2) const
{
  if (!myBaseAdaptor.IsNull())
    myBaseAdaptor->D2(theU, theValue, theD1, theD2);
  else
    myBaseCurve->D2(theU, theValue, theD1, theD2);
}

void Geom2dEvaluator_OffsetCurve::BaseD3(const Standard_Real theU,
                                               gp_Pnt2d& theValue,
                                               gp_Vec2d& theD1,
                                               gp_Vec2d& theD2,
                                               gp_Vec2d& theD3) const
{
  if (!myBaseAdaptor.IsNull())
    myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
  else
    myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
}

void Geom2dEvaluator_OffsetCurve::BaseD4(const Standard_Real theU,
                                               gp_Pnt2d& theValue,
                                               gp_Vec2d& theD1,
                                               gp_Vec2d& theD2,
                                               gp_Vec2d& theD3,
                                               gp_Vec2d& theD4) const
{
  if (!myBaseAdaptor.IsNull())
  {
    myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
    theD4 = myBaseAdaptor->DN(theU, 4);
  }
  else
  {
    myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
    theD4 = myBaseCurve->DN(theU, 4);
  }
}

gp_Vec2d Geom2dEvaluator_OffsetCurve::BaseDN(const Standard_Real theU,
                                             const Standard_Integer theDeriv) const
{
  if (!myBaseAdaptor.IsNull())
    return myBaseAdaptor->DN(theU, theDeriv);
  return myBaseCurve->DN(theU, theDeriv);
}


void Geom2dEvaluator_OffsetCurve::CalculateD0(      gp_Pnt2d& theValue,
                                              const gp_Vec2d& theD1) const
{
  if (theD1.SquareMagnitude() <= gp::Resolution())
    Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Undefined normal vector "
                              "because tangent vector has zero-magnitude!");

  gp_Dir2d aNormal(theD1.Y(), -theD1.X());
  theValue.ChangeCoord().Add(aNormal.XY() * myOffset);
}

void Geom2dEvaluator_OffsetCurve::CalculateD1(      gp_Pnt2d& theValue,
                                                    gp_Vec2d& theD1,
                                              const gp_Vec2d& theD2) const
{
  // P(u) = p(u) + Offset * Ndir / R
  // with R = || p' ^ Z|| and Ndir = P' ^ Z

  // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))

  gp_XY Ndir(theD1.Y(), -theD1.X());
  gp_XY DNdir(theD2.Y(), -theD2.X());
  Standard_Real R2 = Ndir.SquareModulus();
  Standard_Real R = Sqrt(R2);
  Standard_Real R3 = R * R2;
  Standard_Real Dr = Ndir.Dot(DNdir);
  if (R3 <= gp::Resolution())
  {
    if (R2 <= gp::Resolution())
      Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Null derivative");
    //We try another computation but the stability is not very good.
    DNdir.Multiply(R);
    DNdir.Subtract(Ndir.Multiplied(Dr / R));
    DNdir.Multiply(myOffset / R2);
  }
  else
  {
    // Same computation as IICURV in EUCLID-IS because the stability is better
    DNdir.Multiply(myOffset / R);
    DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
  }

  Ndir.Multiply(myOffset / R);
  // P(u)
  theValue.ChangeCoord().Add(Ndir);
  // P'(u)
  theD1.Add(gp_Vec2d(DNdir));
}

void Geom2dEvaluator_OffsetCurve::CalculateD2(      gp_Pnt2d& theValue,
                                                    gp_Vec2d& theD1,
                                                    gp_Vec2d& theD2,
                                              const gp_Vec2d& theD3,
                                              const Standard_Boolean theIsDirChange) const
{
  // P(u) = p(u) + Offset * Ndir / R
  // with R = || p' ^ Z|| and Ndir = P' ^ Z

  // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))

  // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
  //         Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))

  gp_XY Ndir(theD1.Y(), -theD1.X());
  gp_XY DNdir(theD2.Y(), -theD2.X());
  gp_XY D2Ndir(theD3.Y(), -theD3.X());
  Standard_Real R2 = Ndir.SquareModulus();
  Standard_Real R = Sqrt(R2);
  Standard_Real R3 = R2 * R;
  Standard_Real R4 = R2 * R2;
  Standard_Real R5 = R3 * R2;
  Standard_Real Dr = Ndir.Dot(DNdir);
  Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
  if (R5 <= gp::Resolution())
  {
    if (R4 <= gp::Resolution())
      Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Null derivative");
    //We try another computation but the stability is not very good dixit ISG.
    // V2 = P" (U) :
    D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
    D2Ndir.Add(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
    D2Ndir.Multiply(myOffset / R);

    // V1 = P' (U) :
    DNdir.Multiply(R);
    DNdir.Subtract(Ndir.Multiplied(Dr / R));
    DNdir.Multiply(myOffset / R2);
  }
  else
  {
    // Same computation as IICURV in EUCLID-IS because the stability is better.
    // V2 = P" (U) :
    D2Ndir.Multiply(myOffset / R);
    D2Ndir.Subtract(DNdir.Multiplied(2.0 * myOffset * Dr / R3));
    D2Ndir.Add(Ndir.Multiplied(myOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));

    // V1 = P' (U) 
    DNdir.Multiply(myOffset / R);
    DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
  }

  Ndir.Multiply(myOffset / R);
  // P(u)
  theValue.ChangeCoord().Add(Ndir);
  // P'(u) :
  theD1.Add(gp_Vec2d(DNdir));
  // P"(u) :
  if (theIsDirChange)
    theD2.Reverse();
  theD2.Add(gp_Vec2d(D2Ndir));
}

void Geom2dEvaluator_OffsetCurve::CalculateD3(      gp_Pnt2d& theValue,
                                                    gp_Vec2d& theD1,
                                                    gp_Vec2d& theD2,
                                                    gp_Vec2d& theD3,
                                              const gp_Vec2d& theD4,
                                              const Standard_Boolean theIsDirChange) const
{
  // P(u) = p(u) + Offset * Ndir / R
  // with R = || p' ^ Z|| and Ndir = P' ^ Z

  // P'(u)  = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))

  // P"(u)  = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
  //          Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))

  // P"'(u) = p"'(u) + (Offset / R) * (D3Ndir - (3.0 * Dr/R**2 ) * D2Ndir -
  //          (3.0 * D2r / R2) * DNdir) + (3.0 * Dr * Dr / R4) * DNdir -
  //          (D3r/R2) * Ndir + (6.0 * Dr * Dr / R4) * Ndir +
  //          (6.0 * Dr * D2r / R4) * Ndir - (15.0 * Dr* Dr* Dr /R6) * Ndir

  gp_XY Ndir(theD1.Y(), -theD1.X());
  gp_XY DNdir(theD2.Y(), -theD2.X());
  gp_XY D2Ndir(theD3.Y(), -theD3.X());
  gp_XY D3Ndir(theD4.Y(), -theD4.X());
  Standard_Real R2 = Ndir.SquareModulus();
  Standard_Real R = Sqrt(R2);
  Standard_Real R3 = R2 * R;
  Standard_Real R4 = R2 * R2;
  Standard_Real R5 = R3 * R2;
  Standard_Real R6 = R3 * R3;
  Standard_Real R7 = R5 * R2;
  Standard_Real Dr = Ndir.Dot(DNdir);
  Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
  Standard_Real D3r = Ndir.Dot(D3Ndir) + 3.0 * DNdir.Dot(D2Ndir);

  if (R7 <= gp::Resolution())
  {
    if (R6 <= gp::Resolution())
      Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Null derivative");
    //We try another computation but the stability is not very good dixit ISG.
    // V3 = P"' (U) :
    D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * myOffset * Dr / R2));
    D3Ndir.Subtract(
      (DNdir.Multiplied((3.0 * myOffset) * ((D2r / R2) + (Dr*Dr) / R4))));
    D3Ndir.Add(Ndir.Multiplied(
      (myOffset * (6.0*Dr*Dr / R4 + 6.0*Dr*D2r / R4 - 15.0*Dr*Dr*Dr / R6 - D3r))));
    D3Ndir.Multiply(myOffset / R);
    // V2 = P" (U) :
    R4 = R2 * R2;
    D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
    D2Ndir.Subtract(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
    D2Ndir.Multiply(myOffset / R);
    // V1 = P' (U) :
    DNdir.Multiply(R);
    DNdir.Subtract(Ndir.Multiplied(Dr / R));
    DNdir.Multiply(myOffset / R2);
  }
  else
  {
    // Same computation as IICURV in EUCLID-IS because the stability is better.
    // V3 = P"' (U) :
    D3Ndir.Multiply(myOffset / R);
    D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * myOffset * Dr / R3));
    D3Ndir.Subtract(DNdir.Multiplied(
      ((3.0 * myOffset) * ((D2r / R3) + (Dr*Dr) / R5))));
    D3Ndir.Add(Ndir.Multiplied(
      (myOffset * (6.0*Dr*Dr / R5 + 6.0*Dr*D2r / R5 - 15.0*Dr*Dr*Dr / R7 - D3r))));
    // V2 = P" (U) :
    D2Ndir.Multiply(myOffset / R);
    D2Ndir.Subtract(DNdir.Multiplied(2.0 * myOffset * Dr / R3));
    D2Ndir.Subtract(Ndir.Multiplied(
      myOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
    // V1 = P' (U) :
    DNdir.Multiply(myOffset / R);
    DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
  }

  Ndir.Multiply(myOffset / R);
  // P(u)
  theValue.ChangeCoord().Add(Ndir);
  // P'(u) :
  theD1.Add(gp_Vec2d(DNdir));
  // P"(u)
  theD2.Add(gp_Vec2d(D2Ndir));
  // P"'(u)
  if (theIsDirChange)
    theD3.Reverse();
  theD3.Add(gp_Vec2d(D2Ndir));
}


Standard_Boolean Geom2dEvaluator_OffsetCurve::AdjustDerivative(
    const Standard_Integer theMaxDerivative, const Standard_Real theU,
    gp_Vec2d& theD1, gp_Vec2d& theD2, gp_Vec2d& theD3, gp_Vec2d& theD4) const
{
  static const Standard_Real aTol = gp::Resolution();
  static const Standard_Real aMinStep = 1e-7;
  static const Standard_Integer aMaxDerivOrder = 3;

  Standard_Boolean isDirectionChange = Standard_False;
  Standard_Real anUinfium;
  Standard_Real anUsupremum;
  if (!myBaseAdaptor.IsNull())
  {
    anUinfium = myBaseAdaptor->FirstParameter();
    anUsupremum = myBaseAdaptor->LastParameter();
  }
  else
  {
    anUinfium = myBaseCurve->FirstParameter();
    anUsupremum = myBaseCurve->LastParameter();
  }

  static const Standard_Real DivisionFactor = 1.e-3;
  Standard_Real du;
  if ((anUsupremum >= RealLast()) || (anUinfium <= RealFirst()))
    du = 0.0;
  else
    du = anUsupremum - anUinfium;

  const Standard_Real aDelta = Max(du * DivisionFactor, aMinStep);

  //Derivative is approximated by Taylor-series
  Standard_Integer anIndex = 1; //Derivative order
  gp_Vec2d V;

  do
  {
    V = BaseDN(theU, ++anIndex);
  } while ((V.SquareMagnitude() <= aTol) && anIndex < aMaxDerivOrder);

  Standard_Real u;

  if (theU - anUinfium < aDelta)
    u = theU + aDelta;
  else
    u = theU - aDelta;

  gp_Pnt2d P1, P2;
  BaseD0(Min(theU, u), P1);
  BaseD0(Max(theU, u), P2);

  gp_Vec2d V1(P1, P2);
  isDirectionChange = V.Dot(V1) < 0.0;
  Standard_Real aSign = isDirectionChange ? -1.0 : 1.0;

  theD1 = V * aSign;
  gp_Vec2d* aDeriv[3] = { &theD2, &theD3, &theD4 };
  for (Standard_Integer i = 1; i < theMaxDerivative; i++)
    *(aDeriv[i - 1]) = BaseDN(theU, anIndex + i) * aSign;

  return isDirectionChange;
}
Commit	Line	Data
d660a72a	1	// Created on: 2015-09-21
	2	// Copyright (c) 2015 OPEN CASCADE SAS
	3	//
	4	// This file is part of Open CASCADE Technology software library.
	5	//
	6	// This library is free software; you can redistribute it and/or modify it under
	7	// the terms of the GNU Lesser General Public License version 2.1 as published
	8	// by the Free Software Foundation, with special exception defined in the file
	9	// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
	10	// distribution for complete text of the license and disclaimer of any warranty.
	11	//
	12	// Alternatively, this file may be used under the terms of Open CASCADE
	13	// commercial license or contractual agreement.
	14
	15	#include <Geom2dEvaluator_OffsetCurve.hxx>
	16
	17	#include <Geom2dAdaptor_HCurve.hxx>
	18	#include <Standard_NullValue.hxx>
	19
	20
92efcf78	21	IMPLEMENT_STANDARD_RTTIEXT(Geom2dEvaluator_OffsetCurve,Geom2dEvaluator_Curve)
92efcf78	22
d660a72a	23	Geom2dEvaluator_OffsetCurve::Geom2dEvaluator_OffsetCurve(
	24	const Handle(Geom2d_Curve)& theBase,
	25	const Standard_Real theOffset)
	26	: Geom2dEvaluator_Curve(),
	27	myBaseCurve(theBase),
	28	myOffset(theOffset)
	29	{
	30	}
	31
	32	Geom2dEvaluator_OffsetCurve::Geom2dEvaluator_OffsetCurve(
	33	const Handle(Geom2dAdaptor_HCurve)& theBase,
	34	const Standard_Real theOffset)
	35	: Geom2dEvaluator_Curve(),
	36	myBaseAdaptor(theBase),
	37	myOffset(theOffset)
	38	{
	39	}
	40
	41	void Geom2dEvaluator_OffsetCurve::D0(const Standard_Real theU,
	42	gp_Pnt2d& theValue) const
	43	{
	44	gp_Vec2d aD1;
	45	BaseD1(theU, theValue, aD1);
	46	CalculateD0(theValue, aD1);
	47	}
	48
	49	void Geom2dEvaluator_OffsetCurve::D1(const Standard_Real theU,
	50	gp_Pnt2d& theValue,
	51	gp_Vec2d& theD1) const
	52	{
	53	gp_Vec2d aD2;
	54	BaseD2(theU, theValue, theD1, aD2);
	55	CalculateD1(theValue, theD1, aD2);
	56	}
	57
	58	void Geom2dEvaluator_OffsetCurve::D2(const Standard_Real theU,
	59	gp_Pnt2d& theValue,
	60	gp_Vec2d& theD1,
	61	gp_Vec2d& theD2) const
	62	{
	63	gp_Vec2d aD3;
	64	BaseD3(theU, theValue, theD1, theD2, aD3);
	65
	66	Standard_Boolean isDirectionChange = Standard_False;
	67	if (theD1.SquareMagnitude() <= gp::Resolution())
	68	{
	69	gp_Vec2d aDummyD4;
	70	isDirectionChange = AdjustDerivative(3, theU, theD1, theD2, aD3, aDummyD4);
	71	}
	72
	73	CalculateD2(theValue, theD1, theD2, aD3, isDirectionChange);
	74	}
	75
	76	void Geom2dEvaluator_OffsetCurve::D3(const Standard_Real theU,
	77	gp_Pnt2d& theValue,
	78	gp_Vec2d& theD1,
	79	gp_Vec2d& theD2,
	80	gp_Vec2d& theD3) const
	81	{
	82	gp_Vec2d aD4;
	83	BaseD4(theU, theValue, theD1, theD2, theD3, aD4);
	84
	85	Standard_Boolean isDirectionChange = Standard_False;
	86	if (theD1.SquareMagnitude() <= gp::Resolution())
87	isDirectionChange = AdjustDerivative(4, theU, theD1, theD2, theD3, aD4);
88
89	CalculateD3(theValue, theD1, theD2, theD3, aD4, isDirectionChange);
90	}
91
92	gp_Vec2d Geom2dEvaluator_OffsetCurve::DN(const Standard_Real theU,
93	const Standard_Integer theDeriv) const
94	{
95	Standard_RangeError_Raise_if(theDeriv < 1, "Geom2dEvaluator_OffsetCurve::DN(): theDeriv < 1");
96
97	gp_Pnt2d aPnt;
98	gp_Vec2d aDummy, aDN;
99	switch (theDeriv)
100	{
101	case 1:
102	D1(theU, aPnt, aDN);
103	break;
104	case 2:
105	D2(theU, aPnt, aDummy, aDN);
106	break;
107	case 3:
108	D3(theU, aPnt, aDummy, aDummy, aDN);
109	break;
110	default:
111	aDN = BaseDN(theU, theDeriv);
112	}
113	return aDN;
114	}
115
116
117	void Geom2dEvaluator_OffsetCurve::BaseD0(const Standard_Real theU,
118	gp_Pnt2d& theValue) const
119	{
120	if (!myBaseAdaptor.IsNull())
121	myBaseAdaptor->D0(theU, theValue);
122	else
123	myBaseCurve->D0(theU, theValue);
124	}
125
126	void Geom2dEvaluator_OffsetCurve::BaseD1(const Standard_Real theU,
127	gp_Pnt2d& theValue,
128	gp_Vec2d& theD1) const
129	{
130	if (!myBaseAdaptor.IsNull())
131	myBaseAdaptor->D1(theU, theValue, theD1);
132	else
133	myBaseCurve->D1(theU, theValue, theD1);
134	}
135
136	void Geom2dEvaluator_OffsetCurve::BaseD2(const Standard_Real theU,
137	gp_Pnt2d& theValue,
138	gp_Vec2d& theD1,
139	gp_Vec2d& theD2) const
140	{
141	if (!myBaseAdaptor.IsNull())
142	myBaseAdaptor->D2(theU, theValue, theD1, theD2);
143	else
144	myBaseCurve->D2(theU, theValue, theD1, theD2);
145	}
146
147	void Geom2dEvaluator_OffsetCurve::BaseD3(const Standard_Real theU,
148	gp_Pnt2d& theValue,
149	gp_Vec2d& theD1,
150	gp_Vec2d& theD2,
151	gp_Vec2d& theD3) const
152	{
153	if (!myBaseAdaptor.IsNull())
154	myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
155	else
156	myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
157	}
158
159	void Geom2dEvaluator_OffsetCurve::BaseD4(const Standard_Real theU,
160	gp_Pnt2d& theValue,
161	gp_Vec2d& theD1,
162	gp_Vec2d& theD2,
163	gp_Vec2d& theD3,
164	gp_Vec2d& theD4) const
165	{
166	if (!myBaseAdaptor.IsNull())
167	{
168	myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
169	theD4 = myBaseAdaptor->DN(theU, 4);
170	}
171	else
172	{
173	myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
174	theD4 = myBaseCurve->DN(theU, 4);
175	}
176	}
177
178	gp_Vec2d Geom2dEvaluator_OffsetCurve::BaseDN(const Standard_Real theU,
179	const Standard_Integer theDeriv) const
180	{
181	if (!myBaseAdaptor.IsNull())
182	return myBaseAdaptor->DN(theU, theDeriv);
183	return myBaseCurve->DN(theU, theDeriv);
184	}
185
186
187	void Geom2dEvaluator_OffsetCurve::CalculateD0( gp_Pnt2d& theValue,
188	const gp_Vec2d& theD1) const
189	{
190	if (theD1.SquareMagnitude() <= gp::Resolution())
191	Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Undefined normal vector "
192	"because tangent vector has zero-magnitude!");
193
194	gp_Dir2d aNormal(theD1.Y(), -theD1.X());
195	theValue.ChangeCoord().Add(aNormal.XY() * myOffset);
196	}
197
198	void Geom2dEvaluator_OffsetCurve::CalculateD1( gp_Pnt2d& theValue,
199	gp_Vec2d& theD1,
200	const gp_Vec2d& theD2) const
201	{
202	// P(u) = p(u) + Offset * Ndir / R
203	// with R = \|\| p' ^ Z\|\| and Ndir = P' ^ Z
204
205	// P'(u) = p'(u) + (Offset / R*2) (DNdir/DU * R - Ndir * (DR/R))
206
207	gp_XY Ndir(theD1.Y(), -theD1.X());
208	gp_XY DNdir(theD2.Y(), -theD2.X());
209	Standard_Real R2 = Ndir.SquareModulus();
210	Standard_Real R = Sqrt(R2);
211	Standard_Real R3 = R * R2;
212	Standard_Real Dr = Ndir.Dot(DNdir);
213	if (R3 <= gp::Resolution())
214	{
215	if (R2 <= gp::Resolution())
216	Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Null derivative");
217	//We try another computation but the stability is not very good.
218	DNdir.Multiply(R);
219	DNdir.Subtract(Ndir.Multiplied(Dr / R));
220	DNdir.Multiply(myOffset / R2);
221	}
222	else
223	{
224	// Same computation as IICURV in EUCLID-IS because the stability is better
225	DNdir.Multiply(myOffset / R);
226	DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
227	}
228
229	Ndir.Multiply(myOffset / R);
230	// P(u)
231	theValue.ChangeCoord().Add(Ndir);
232	// P'(u)
233	theD1.Add(gp_Vec2d(DNdir));
234	}
235
236	void Geom2dEvaluator_OffsetCurve::CalculateD2( gp_Pnt2d& theValue,
237	gp_Vec2d& theD1,
238	gp_Vec2d& theD2,
239	const gp_Vec2d& theD3,
240	const Standard_Boolean theIsDirChange) const
241	{
242	// P(u) = p(u) + Offset * Ndir / R
243	// with R = \|\| p' ^ Z\|\| and Ndir = P' ^ Z
244
245	// P'(u) = p'(u) + (Offset / R*2) (DNdir/DU * R - Ndir * (DR/R))
246
247	// P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
248	// Ndir * ( (3.0 * Dr2 / R4) - (D2r / R**2)))
249
250	gp_XY Ndir(theD1.Y(), -theD1.X());
251	gp_XY DNdir(theD2.Y(), -theD2.X());
252	gp_XY D2Ndir(theD3.Y(), -theD3.X());
253	Standard_Real R2 = Ndir.SquareModulus();
254	Standard_Real R = Sqrt(R2);
255	Standard_Real R3 = R2 * R;
256	Standard_Real R4 = R2 * R2;
257	Standard_Real R5 = R3 * R2;
258	Standard_Real Dr = Ndir.Dot(DNdir);
259	Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
260	if (R5 <= gp::Resolution())
261	{
262	if (R4 <= gp::Resolution())
263	Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Null derivative");
264	//We try another computation but the stability is not very good dixit ISG.
265	// V2 = P" (U) :
266	D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
267	D2Ndir.Add(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
268	D2Ndir.Multiply(myOffset / R);
269
270	// V1 = P' (U) :
271	DNdir.Multiply(R);
272	DNdir.Subtract(Ndir.Multiplied(Dr / R));
273	DNdir.Multiply(myOffset / R2);
274	}
275	else
276	{
277	// Same computation as IICURV in EUCLID-IS because the stability is better.
278	// V2 = P" (U) :
279	D2Ndir.Multiply(myOffset / R);
280	D2Ndir.Subtract(DNdir.Multiplied(2.0 * myOffset * Dr / R3));
281	D2Ndir.Add(Ndir.Multiplied(myOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
282
283	// V1 = P' (U)
284	DNdir.Multiply(myOffset / R);
285	DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
286	}
287
288	Ndir.Multiply(myOffset / R);
289	// P(u)
290	theValue.ChangeCoord().Add(Ndir);
291	// P'(u) :
292	theD1.Add(gp_Vec2d(DNdir));
293	// P"(u) :
294	if (theIsDirChange)
295	theD2.Reverse();
296	theD2.Add(gp_Vec2d(D2Ndir));
297	}
298
299	void Geom2dEvaluator_OffsetCurve::CalculateD3( gp_Pnt2d& theValue,
300	gp_Vec2d& theD1,
301	gp_Vec2d& theD2,
302	gp_Vec2d& theD3,
303	const gp_Vec2d& theD4,
304	const Standard_Boolean theIsDirChange) const
305	{
306	// P(u) = p(u) + Offset * Ndir / R
307	// with R = \|\| p' ^ Z\|\| and Ndir = P' ^ Z
308
309	// P'(u) = p'(u) + (Offset / R*2) (DNdir/DU * R - Ndir * (DR/R))
310
311	// P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
312	// Ndir * ( (3.0 * Dr2 / R4) - (D2r / R**2)))
313
314	// P"'(u) = p"'(u) + (Offset / R) * (D3Ndir - (3.0 * Dr/R*2 ) D2Ndir -
315	// (3.0 * D2r / R2) * DNdir) + (3.0 * Dr * Dr / R4) * DNdir -
316	// (D3r/R2) * Ndir + (6.0 * Dr * Dr / R4) * Ndir +
317	// (6.0 * Dr * D2r / R4) * Ndir - (15.0 * Dr* Dr* Dr /R6) * Ndir
318
319	gp_XY Ndir(theD1.Y(), -theD1.X());
320	gp_XY DNdir(theD2.Y(), -theD2.X());
321	gp_XY D2Ndir(theD3.Y(), -theD3.X());
322	gp_XY D3Ndir(theD4.Y(), -theD4.X());
323	Standard_Real R2 = Ndir.SquareModulus();
324	Standard_Real R = Sqrt(R2);
325	Standard_Real R3 = R2 * R;
326	Standard_Real R4 = R2 * R2;
327	Standard_Real R5 = R3 * R2;
328	Standard_Real R6 = R3 * R3;
329	Standard_Real R7 = R5 * R2;
330	Standard_Real Dr = Ndir.Dot(DNdir);
331	Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
332	Standard_Real D3r = Ndir.Dot(D3Ndir) + 3.0 * DNdir.Dot(D2Ndir);
333
334	if (R7 <= gp::Resolution())
335	{
336	if (R6 <= gp::Resolution())
337	Standard_NullValue::Raise("Geom2dEvaluator_OffsetCurve: Null derivative");
338	//We try another computation but the stability is not very good dixit ISG.
339	// V3 = P"' (U) :
340	D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * myOffset * Dr / R2));
341	D3Ndir.Subtract(
342	(DNdir.Multiplied((3.0 * myOffset) * ((D2r / R2) + (Dr*Dr) / R4))));
343	D3Ndir.Add(Ndir.Multiplied(
344	(myOffset * (6.0DrDr / R4 + 6.0DrD2r / R4 - 15.0DrDr*Dr / R6 - D3r))));
345	D3Ndir.Multiply(myOffset / R);
346	// V2 = P" (U) :
347	R4 = R2 * R2;
348	D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
349	D2Ndir.Subtract(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
350	D2Ndir.Multiply(myOffset / R);
351	// V1 = P' (U) :
352	DNdir.Multiply(R);
353	DNdir.Subtract(Ndir.Multiplied(Dr / R));
354	DNdir.Multiply(myOffset / R2);
355	}
356	else
357	{
358	// Same computation as IICURV in EUCLID-IS because the stability is better.
359	// V3 = P"' (U) :
360	D3Ndir.Multiply(myOffset / R);
361	D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * myOffset * Dr / R3));
362	D3Ndir.Subtract(DNdir.Multiplied(
363	((3.0 * myOffset) * ((D2r / R3) + (Dr*Dr) / R5))));
364	D3Ndir.Add(Ndir.Multiplied(
365	(myOffset * (6.0DrDr / R5 + 6.0DrD2r / R5 - 15.0DrDr*Dr / R7 - D3r))));
366	// V2 = P" (U) :
367	D2Ndir.Multiply(myOffset / R);
368	D2Ndir.Subtract(DNdir.Multiplied(2.0 * myOffset * Dr / R3));
369	D2Ndir.Subtract(Ndir.Multiplied(
370	myOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
371	// V1 = P' (U) :
372	DNdir.Multiply(myOffset / R);
373	DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
374	}
375
376	Ndir.Multiply(myOffset / R);
377	// P(u)
378	theValue.ChangeCoord().Add(Ndir);
379	// P'(u) :
380	theD1.Add(gp_Vec2d(DNdir));
381	// P"(u)
382	theD2.Add(gp_Vec2d(D2Ndir));
383	// P"'(u)
384	if (theIsDirChange)
385	theD3.Reverse();
386	theD3.Add(gp_Vec2d(D2Ndir));
387	}
388
389
390	Standard_Boolean Geom2dEvaluator_OffsetCurve::AdjustDerivative(
391	const Standard_Integer theMaxDerivative, const Standard_Real theU,
392	gp_Vec2d& theD1, gp_Vec2d& theD2, gp_Vec2d& theD3, gp_Vec2d& theD4) const
393	{
394	static const Standard_Real aTol = gp::Resolution();
395	static const Standard_Real aMinStep = 1e-7;
396	static const Standard_Integer aMaxDerivOrder = 3;
397
398	Standard_Boolean isDirectionChange = Standard_False;
399	Standard_Real anUinfium;
400	Standard_Real anUsupremum;
401	if (!myBaseAdaptor.IsNull())
402	{
403	anUinfium = myBaseAdaptor->FirstParameter();
404	anUsupremum = myBaseAdaptor->LastParameter();
405	}
406	else
407	{
408	anUinfium = myBaseCurve->FirstParameter();
409	anUsupremum = myBaseCurve->LastParameter();
410	}
411
412	static const Standard_Real DivisionFactor = 1.e-3;
413	Standard_Real du;
414	if ((anUsupremum >= RealLast()) \|\| (anUinfium <= RealFirst()))
415	du = 0.0;
416	else
417	du = anUsupremum - anUinfium;
418
419	const Standard_Real aDelta = Max(du * DivisionFactor, aMinStep);
420
421	//Derivative is approximated by Taylor-series
422	Standard_Integer anIndex = 1; //Derivative order
423	gp_Vec2d V;
424
425	do
426	{
427	V = BaseDN(theU, ++anIndex);
428	} while ((V.SquareMagnitude() <= aTol) && anIndex < aMaxDerivOrder);
429
430	Standard_Real u;
431
432	if (theU - anUinfium < aDelta)
433	u = theU + aDelta;
434	else
435	u = theU - aDelta;
436
437	gp_Pnt2d P1, P2;
438	BaseD0(Min(theU, u), P1);
439	BaseD0(Max(theU, u), P2);
440
441	gp_Vec2d V1(P1, P2);
442	isDirectionChange = V.Dot(V1) < 0.0;
443	Standard_Real aSign = isDirectionChange ? -1.0 : 1.0;
444
445	theD1 = V * aSign;
446	gp_Vec2d* aDeriv[3] = { &theD2, &theD3, &theD4 };
447	for (Standard_Integer i = 1; i < theMaxDerivative; i++)
448	(aDeriv[i - 1]) = BaseDN(theU, anIndex + i) aSign;
449
450	return isDirectionChange;
451	}
452