1 // Created on: 2015-09-21
2 // Copyright (c) 2015 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
15 #include <Geom2dEvaluator_OffsetCurve.hxx>
17 #include <Geom2dAdaptor_HCurve.hxx>
18 #include <Standard_NullValue.hxx>
21 IMPLEMENT_STANDARD_RTTIEXT(Geom2dEvaluator_OffsetCurve,Geom2dEvaluator_Curve)
23 Geom2dEvaluator_OffsetCurve::Geom2dEvaluator_OffsetCurve(
24 const Handle(Geom2d_Curve)& theBase,
25 const Standard_Real theOffset)
26 : Geom2dEvaluator_Curve(),
32 Geom2dEvaluator_OffsetCurve::Geom2dEvaluator_OffsetCurve(
33 const Handle(Geom2dAdaptor_HCurve)& theBase,
34 const Standard_Real theOffset)
35 : Geom2dEvaluator_Curve(),
36 myBaseAdaptor(theBase),
41 void Geom2dEvaluator_OffsetCurve::D0(const Standard_Real theU,
42 gp_Pnt2d& theValue) const
45 BaseD1(theU, theValue, aD1);
46 CalculateD0(theValue, aD1);
49 void Geom2dEvaluator_OffsetCurve::D1(const Standard_Real theU,
51 gp_Vec2d& theD1) const
54 BaseD2(theU, theValue, theD1, aD2);
55 CalculateD1(theValue, theD1, aD2);
58 void Geom2dEvaluator_OffsetCurve::D2(const Standard_Real theU,
61 gp_Vec2d& theD2) const
64 BaseD3(theU, theValue, theD1, theD2, aD3);
66 Standard_Boolean isDirectionChange = Standard_False;
67 if (theD1.SquareMagnitude() <= gp::Resolution())
70 isDirectionChange = AdjustDerivative(3, theU, theD1, theD2, aD3, aDummyD4);
73 CalculateD2(theValue, theD1, theD2, aD3, isDirectionChange);
76 void Geom2dEvaluator_OffsetCurve::D3(const Standard_Real theU,
80 gp_Vec2d& theD3) const
83 BaseD4(theU, theValue, theD1, theD2, theD3, aD4);
85 Standard_Boolean isDirectionChange = Standard_False;
86 if (theD1.SquareMagnitude() <= gp::Resolution())
87 isDirectionChange = AdjustDerivative(4, theU, theD1, theD2, theD3, aD4);
89 CalculateD3(theValue, theD1, theD2, theD3, aD4, isDirectionChange);
92 gp_Vec2d Geom2dEvaluator_OffsetCurve::DN(const Standard_Real theU,
93 const Standard_Integer theDeriv) const
95 Standard_RangeError_Raise_if(theDeriv < 1, "Geom2dEvaluator_OffsetCurve::DN(): theDeriv < 1");
105 D2(theU, aPnt, aDummy, aDN);
108 D3(theU, aPnt, aDummy, aDummy, aDN);
111 aDN = BaseDN(theU, theDeriv);
117 void Geom2dEvaluator_OffsetCurve::BaseD0(const Standard_Real theU,
118 gp_Pnt2d& theValue) const
120 if (!myBaseAdaptor.IsNull())
121 myBaseAdaptor->D0(theU, theValue);
123 myBaseCurve->D0(theU, theValue);
126 void Geom2dEvaluator_OffsetCurve::BaseD1(const Standard_Real theU,
128 gp_Vec2d& theD1) const
130 if (!myBaseAdaptor.IsNull())
131 myBaseAdaptor->D1(theU, theValue, theD1);
133 myBaseCurve->D1(theU, theValue, theD1);
136 void Geom2dEvaluator_OffsetCurve::BaseD2(const Standard_Real theU,
139 gp_Vec2d& theD2) const
141 if (!myBaseAdaptor.IsNull())
142 myBaseAdaptor->D2(theU, theValue, theD1, theD2);
144 myBaseCurve->D2(theU, theValue, theD1, theD2);
147 void Geom2dEvaluator_OffsetCurve::BaseD3(const Standard_Real theU,
151 gp_Vec2d& theD3) const
153 if (!myBaseAdaptor.IsNull())
154 myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
156 myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
159 void Geom2dEvaluator_OffsetCurve::BaseD4(const Standard_Real theU,
164 gp_Vec2d& theD4) const
166 if (!myBaseAdaptor.IsNull())
168 myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
169 theD4 = myBaseAdaptor->DN(theU, 4);
173 myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
174 theD4 = myBaseCurve->DN(theU, 4);
178 gp_Vec2d Geom2dEvaluator_OffsetCurve::BaseDN(const Standard_Real theU,
179 const Standard_Integer theDeriv) const
181 if (!myBaseAdaptor.IsNull())
182 return myBaseAdaptor->DN(theU, theDeriv);
183 return myBaseCurve->DN(theU, theDeriv);
187 void Geom2dEvaluator_OffsetCurve::CalculateD0( gp_Pnt2d& theValue,
188 const gp_Vec2d& theD1) const
190 if (theD1.SquareMagnitude() <= gp::Resolution())
191 throw Standard_NullValue("Geom2dEvaluator_OffsetCurve: Undefined normal vector "
192 "because tangent vector has zero-magnitude!");
194 gp_Dir2d aNormal(theD1.Y(), -theD1.X());
195 theValue.ChangeCoord().Add(aNormal.XY() * myOffset);
198 void Geom2dEvaluator_OffsetCurve::CalculateD1( gp_Pnt2d& theValue,
200 const gp_Vec2d& theD2) const
202 // P(u) = p(u) + Offset * Ndir / R
203 // with R = || p' ^ Z|| and Ndir = P' ^ Z
205 // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R - Ndir * (DR/R))
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())
215 if (R2 <= gp::Resolution())
216 throw Standard_NullValue("Geom2dEvaluator_OffsetCurve: Null derivative");
217 //We try another computation but the stability is not very good.
219 DNdir.Subtract(Ndir.Multiplied(Dr / R));
220 DNdir.Multiply(myOffset / R2);
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));
229 Ndir.Multiply(myOffset / R);
231 theValue.ChangeCoord().Add(Ndir);
233 theD1.Add(gp_Vec2d(DNdir));
236 void Geom2dEvaluator_OffsetCurve::CalculateD2( gp_Pnt2d& theValue,
239 const gp_Vec2d& theD3,
240 const Standard_Boolean theIsDirChange) const
242 // P(u) = p(u) + Offset * Ndir / R
243 // with R = || p' ^ Z|| and Ndir = P' ^ Z
245 // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R - Ndir * (DR/R))
247 // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
248 // Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
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())
262 if (R4 <= gp::Resolution())
263 throw Standard_NullValue("Geom2dEvaluator_OffsetCurve: Null derivative");
264 //We try another computation but the stability is not very good dixit ISG.
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);
272 DNdir.Subtract(Ndir.Multiplied(Dr / R));
273 DNdir.Multiply(myOffset / R2);
277 // Same computation as IICURV in EUCLID-IS because the stability is better.
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))));
284 DNdir.Multiply(myOffset / R);
285 DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
288 Ndir.Multiply(myOffset / R);
290 theValue.ChangeCoord().Add(Ndir);
292 theD1.Add(gp_Vec2d(DNdir));
296 theD2.Add(gp_Vec2d(D2Ndir));
299 void Geom2dEvaluator_OffsetCurve::CalculateD3( gp_Pnt2d& theValue,
303 const gp_Vec2d& theD4,
304 const Standard_Boolean theIsDirChange) const
306 // P(u) = p(u) + Offset * Ndir / R
307 // with R = || p' ^ Z|| and Ndir = P' ^ Z
309 // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R - Ndir * (DR/R))
311 // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
312 // Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
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
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);
334 if (R7 <= gp::Resolution())
336 if (R6 <= gp::Resolution())
337 throw Standard_NullValue("Geom2dEvaluator_OffsetCurve: Null derivative");
338 //We try another computation but the stability is not very good dixit ISG.
340 D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * myOffset * Dr / R2));
342 (DNdir.Multiplied((3.0 * myOffset) * ((D2r / R2) + (Dr*Dr) / R4))));
343 D3Ndir.Add(Ndir.Multiplied(
344 (myOffset * (6.0*Dr*Dr / R4 + 6.0*Dr*D2r / R4 - 15.0*Dr*Dr*Dr / R6 - D3r))));
345 D3Ndir.Multiply(myOffset / R);
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);
353 DNdir.Subtract(Ndir.Multiplied(Dr / R));
354 DNdir.Multiply(myOffset / R2);
358 // Same computation as IICURV in EUCLID-IS because the stability is better.
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.0*Dr*Dr / R5 + 6.0*Dr*D2r / R5 - 15.0*Dr*Dr*Dr / R7 - D3r))));
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))));
372 DNdir.Multiply(myOffset / R);
373 DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
376 Ndir.Multiply(myOffset / R);
378 theValue.ChangeCoord().Add(Ndir);
380 theD1.Add(gp_Vec2d(DNdir));
382 theD2.Add(gp_Vec2d(D2Ndir));
386 theD3.Add(gp_Vec2d(D2Ndir));
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
394 static const Standard_Real aTol = gp::Resolution();
395 static const Standard_Real aMinStep = 1e-7;
396 static const Standard_Integer aMaxDerivOrder = 3;
398 Standard_Boolean isDirectionChange = Standard_False;
399 Standard_Real anUinfium;
400 Standard_Real anUsupremum;
401 if (!myBaseAdaptor.IsNull())
403 anUinfium = myBaseAdaptor->FirstParameter();
404 anUsupremum = myBaseAdaptor->LastParameter();
408 anUinfium = myBaseCurve->FirstParameter();
409 anUsupremum = myBaseCurve->LastParameter();
412 static const Standard_Real DivisionFactor = 1.e-3;
414 if ((anUsupremum >= RealLast()) || (anUinfium <= RealFirst()))
417 du = anUsupremum - anUinfium;
419 const Standard_Real aDelta = Max(du * DivisionFactor, aMinStep);
421 //Derivative is approximated by Taylor-series
422 Standard_Integer anIndex = 1; //Derivative order
427 V = BaseDN(theU, ++anIndex);
428 } while ((V.SquareMagnitude() <= aTol) && anIndex < aMaxDerivOrder);
432 if (theU - anUinfium < aDelta)
438 BaseD0(Min(theU, u), P1);
439 BaseD0(Max(theU, u), P2);
442 isDirectionChange = V.Dot(V1) < 0.0;
443 Standard_Real aSign = isDirectionChange ? -1.0 : 1.0;
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;
450 return isDirectionChange;