0031303: Different calculation of offset direction in Adaptor2d_OffsetCurve and Geom2...
[occt.git] / src / Geom2dEvaluator / Geom2dEvaluator.cxx
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.hxx>
16 #include <gp_Pnt2d.hxx>
17 #include <gp_Vec2d.hxx>
18 #include <gp_XY.hxx>
19 #include <Standard_NullValue.hxx>
20
21 //=======================================================================
22 //function : CalculateD0
23 //purpose  : 
24 //=======================================================================
25 void Geom2dEvaluator::CalculateD0( gp_Pnt2d& theValue,
26                                    const gp_Vec2d& theD1, const Standard_Real theOffset)
27 {
28   if (theD1.SquareMagnitude() <= gp::Resolution())
29     throw Standard_NullValue("Geom2dEvaluator: Undefined normal vector "
30                              "because tangent vector has zero-magnitude!");
31
32   gp_Dir2d aNormal(theD1.Y(), -theD1.X());
33   theValue.ChangeCoord().Add(aNormal.XY() * theOffset);
34 }
35
36 //=======================================================================
37 //function : CalculateD1
38 //purpose  : 
39 //=======================================================================
40 void Geom2dEvaluator::CalculateD1(gp_Pnt2d& theValue,
41                                   gp_Vec2d& theD1,
42                                   const gp_Vec2d& theD2, const Standard_Real theOffset)
43 {
44   // P(u) = p(u) + Offset * Ndir / R
45   // with R = || p' ^ Z|| and Ndir = P' ^ Z
46
47   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
48
49   gp_XY Ndir(theD1.Y(), -theD1.X());
50   gp_XY DNdir(theD2.Y(), -theD2.X());
51   Standard_Real R2 = Ndir.SquareModulus();
52   Standard_Real R = Sqrt(R2);
53   Standard_Real R3 = R * R2;
54   Standard_Real Dr = Ndir.Dot(DNdir);
55   if (R3 <= gp::Resolution())
56   {
57     if (R2 <= gp::Resolution())
58       throw Standard_NullValue("Geom2dEvaluator_OffsetCurve: Null derivative");
59     //We try another computation but the stability is not very good.
60     DNdir.Multiply(R);
61     DNdir.Subtract(Ndir.Multiplied(Dr / R));
62     DNdir.Multiply(theOffset / R2);
63   }
64   else
65   {
66     // Same computation as IICURV in EUCLID-IS because the stability is better
67     DNdir.Multiply(theOffset / R);
68     DNdir.Subtract(Ndir.Multiplied(theOffset * Dr / R3));
69   }
70
71   Ndir.Multiply(theOffset / R);
72   // P(u)
73   theValue.ChangeCoord().Add(Ndir);
74   // P'(u)
75   theD1.Add(gp_Vec2d(DNdir));
76 }
77
78 //=======================================================================
79 //function : CalculateD2
80 //purpose  : 
81 //=======================================================================
82 void Geom2dEvaluator::CalculateD2( gp_Pnt2d& theValue,
83                                                     gp_Vec2d& theD1,
84                                                     gp_Vec2d& theD2,
85                                               const gp_Vec2d& theD3,
86                                               const Standard_Boolean theIsDirChange, const Standard_Real theOffset) 
87 {
88   // P(u) = p(u) + Offset * Ndir / R
89   // with R = || p' ^ Z|| and Ndir = P' ^ Z
90
91   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
92
93   // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
94   //         Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
95
96   gp_XY Ndir(theD1.Y(), -theD1.X());
97   gp_XY DNdir(theD2.Y(), -theD2.X());
98   gp_XY D2Ndir(theD3.Y(), -theD3.X());
99   Standard_Real R2 = Ndir.SquareModulus();
100   Standard_Real R = Sqrt(R2);
101   Standard_Real R3 = R2 * R;
102   Standard_Real R4 = R2 * R2;
103   Standard_Real R5 = R3 * R2;
104   Standard_Real Dr = Ndir.Dot(DNdir);
105   Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
106   if (R5 <= gp::Resolution())
107   {
108     if (R4 <= gp::Resolution())
109       throw Standard_NullValue("Geom2dEvaluator: Null derivative");
110     //We try another computation but the stability is not very good dixit ISG.
111     // V2 = P" (U) :
112     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
113     D2Ndir.Add(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
114     D2Ndir.Multiply(theOffset / R);
115
116     // V1 = P' (U) :
117     DNdir.Multiply(R);
118     DNdir.Subtract(Ndir.Multiplied(Dr / R));
119     DNdir.Multiply(theOffset / R2);
120   }
121   else
122   {
123     // Same computation as IICURV in EUCLID-IS because the stability is better.
124     // V2 = P" (U) :
125     D2Ndir.Multiply(theOffset / R);
126     D2Ndir.Subtract(DNdir.Multiplied(2.0 * theOffset * Dr / R3));
127     D2Ndir.Add(Ndir.Multiplied(theOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
128
129     // V1 = P' (U) 
130     DNdir.Multiply(theOffset / R);
131     DNdir.Subtract(Ndir.Multiplied(theOffset * Dr / R3));
132   }
133
134   Ndir.Multiply(theOffset / R);
135   // P(u)
136   theValue.ChangeCoord().Add(Ndir);
137   // P'(u) :
138   theD1.Add(gp_Vec2d(DNdir));
139   // P"(u) :
140   if (theIsDirChange)
141     theD2.Reverse();
142   theD2.Add(gp_Vec2d(D2Ndir));
143 }
144
145 //=======================================================================
146 //function : CalculateD3
147 //purpose  : 
148 //=======================================================================
149 void Geom2dEvaluator::CalculateD3(      gp_Pnt2d& theValue,
150                                                     gp_Vec2d& theD1,
151                                                     gp_Vec2d& theD2,
152                                                     gp_Vec2d& theD3,
153                                               const gp_Vec2d& theD4,
154                                               const Standard_Boolean theIsDirChange, const Standard_Real theOffset) 
155 {
156   // P(u) = p(u) + Offset * Ndir / R
157   // with R = || p' ^ Z|| and Ndir = P' ^ Z
158
159   // P'(u)  = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
160
161   // P"(u)  = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
162   //          Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
163
164   // P"'(u) = p"'(u) + (Offset / R) * (D3Ndir - (3.0 * Dr/R**2 ) * D2Ndir -
165   //          (3.0 * D2r / R2) * DNdir) + (3.0 * Dr * Dr / R4) * DNdir -
166   //          (D3r/R2) * Ndir + (6.0 * Dr * Dr / R4) * Ndir +
167   //          (6.0 * Dr * D2r / R4) * Ndir - (15.0 * Dr* Dr* Dr /R6) * Ndir
168
169   gp_XY Ndir(theD1.Y(), -theD1.X());
170   gp_XY DNdir(theD2.Y(), -theD2.X());
171   gp_XY D2Ndir(theD3.Y(), -theD3.X());
172   gp_XY D3Ndir(theD4.Y(), -theD4.X());
173   Standard_Real R2 = Ndir.SquareModulus();
174   Standard_Real R = Sqrt(R2);
175   Standard_Real R3 = R2 * R;
176   Standard_Real R4 = R2 * R2;
177   Standard_Real R5 = R3 * R2;
178   Standard_Real R6 = R3 * R3;
179   Standard_Real R7 = R5 * R2;
180   Standard_Real Dr = Ndir.Dot(DNdir);
181   Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
182   Standard_Real D3r = Ndir.Dot(D3Ndir) + 3.0 * DNdir.Dot(D2Ndir);
183
184   if (R7 <= gp::Resolution())
185   {
186     if (R6 <= gp::Resolution())
187       throw Standard_NullValue("Geom2dEvaluator: Null derivative");
188     //We try another computation but the stability is not very good dixit ISG.
189     // V3 = P"' (U) :
190     D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * theOffset * Dr / R2));
191     D3Ndir.Subtract(
192       (DNdir.Multiplied((3.0 * theOffset) * ((D2r / R2) + (Dr*Dr) / R4))));
193     D3Ndir.Add(Ndir.Multiplied(
194       (theOffset * (6.0*Dr*Dr / R4 + 6.0*Dr*D2r / R4 - 15.0*Dr*Dr*Dr / R6 - D3r))));
195     D3Ndir.Multiply(theOffset / R);
196     // V2 = P" (U) :
197     R4 = R2 * R2;
198     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
199     D2Ndir.Subtract(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
200     D2Ndir.Multiply(theOffset / R);
201     // V1 = P' (U) :
202     DNdir.Multiply(R);
203     DNdir.Subtract(Ndir.Multiplied(Dr / R));
204     DNdir.Multiply(theOffset / R2);
205   }
206   else
207   {
208     // Same computation as IICURV in EUCLID-IS because the stability is better.
209     // V3 = P"' (U) :
210     D3Ndir.Multiply(theOffset / R);
211     D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * theOffset * Dr / R3));
212     D3Ndir.Subtract(DNdir.Multiplied(
213       ((3.0 * theOffset) * ((D2r / R3) + (Dr*Dr) / R5))));
214     D3Ndir.Add(Ndir.Multiplied(
215       (theOffset * (6.0*Dr*Dr / R5 + 6.0*Dr*D2r / R5 - 15.0*Dr*Dr*Dr / R7 - D3r))));
216     // V2 = P" (U) :
217     D2Ndir.Multiply(theOffset / R);
218     D2Ndir.Subtract(DNdir.Multiplied(2.0 * theOffset * Dr / R3));
219     D2Ndir.Subtract(Ndir.Multiplied(
220       theOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
221     // V1 = P' (U) :
222     DNdir.Multiply(theOffset / R);
223     DNdir.Subtract(Ndir.Multiplied(theOffset * Dr / R3));
224   }
225
226   Ndir.Multiply(theOffset / R);
227   // P(u)
228   theValue.ChangeCoord().Add(Ndir);
229   // P'(u) :
230   theD1.Add(gp_Vec2d(DNdir));
231   // P"(u)
232   theD2.Add(gp_Vec2d(D2Ndir));
233   // P"'(u)
234   if (theIsDirChange)
235     theD3.Reverse();
236   theD3.Add(gp_Vec2d(D2Ndir));
237 }
238
239