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