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) |
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 * Dr**2 / R**4) - (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 * Dr**2 / R**4) - (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.0*Dr*Dr / R4 + 6.0*Dr*D2r / R4 - 15.0*Dr*Dr*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.0*Dr*Dr / R5 + 6.0*Dr*D2r / R5 - 15.0*Dr*Dr*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 | |