7fd59977 |
1 | // File: BRepBlend_SurfCurvConstRadInv.cxx |
2 | // Created: Fri Feb 21 14:21:35 1997 |
3 | // Author: Laurent BOURESCHE |
4 | // <lbo@pomalox.paris1.matra-dtv.fr> |
5 | |
6 | |
7 | #include <BRepBlend_SurfCurvConstRadInv.ixx> |
8 | |
9 | //======================================================================= |
10 | //function : BRepBlend_SurfCurvConstRadInv |
11 | //purpose : |
12 | //======================================================================= |
13 | |
14 | BRepBlend_SurfCurvConstRadInv::BRepBlend_SurfCurvConstRadInv |
15 | (const Handle(Adaptor3d_HSurface)& S, |
16 | const Handle(Adaptor3d_HCurve)& C, |
17 | const Handle(Adaptor3d_HCurve)& Cg) : surf(S),curv(C),guide(Cg) |
18 | { |
19 | } |
20 | |
21 | //======================================================================= |
22 | //function : Set |
23 | //purpose : |
24 | //======================================================================= |
25 | |
26 | void BRepBlend_SurfCurvConstRadInv::Set(const Standard_Real R, |
27 | const Standard_Integer Choix) |
28 | { |
29 | choix = Choix; |
30 | switch (choix) { |
31 | case 1: |
32 | case 2: |
33 | { |
34 | ray = -Abs(R); |
35 | } |
36 | break; |
37 | case 3: |
38 | case 4: |
39 | { |
40 | ray = Abs(R); |
41 | } |
42 | break; |
43 | default: |
44 | { |
45 | ray = -Abs(R); |
46 | } |
47 | } |
48 | } |
49 | |
50 | //======================================================================= |
51 | //function : NbEquations |
52 | //purpose : |
53 | //======================================================================= |
54 | |
55 | Standard_Integer BRepBlend_SurfCurvConstRadInv::NbEquations() const |
56 | { |
57 | return 3; |
58 | } |
59 | |
60 | //======================================================================= |
61 | //function : Value |
62 | //purpose : |
63 | //======================================================================= |
64 | |
65 | Standard_Boolean BRepBlend_SurfCurvConstRadInv::Value(const math_Vector& X, |
66 | math_Vector& F) |
67 | { |
68 | gp_Pnt ptgui; |
69 | gp_Vec d1gui; |
70 | guide->D1(X(1),ptgui,d1gui); |
71 | gp_Vec nplan = d1gui.Normalized(); |
72 | Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ())); |
73 | gp_Pnt ptcur = curv->Value(X(2)); |
74 | F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD; |
75 | gp_Pnt2d p2drst = rst->Value(X(3)); |
76 | gp_Pnt pts; |
77 | gp_Vec du,dv; |
78 | surf->D1(p2drst.X(),p2drst.Y(),pts,du,dv); |
79 | F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD; |
80 | gp_Vec ns = du.Crossed(dv); |
81 | Standard_Real norm = nplan.Crossed(ns).Magnitude(); |
82 | Standard_Real unsurnorm = 1./norm; |
83 | ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns); |
84 | ns.Multiply(unsurnorm); |
85 | gp_Vec ref(ptcur,pts); |
86 | ref.SetLinearForm(ray,ns,ref); |
87 | F(3) = ref.SquareMagnitude() - ray*ray; |
88 | return Standard_True; |
89 | } |
90 | |
91 | //======================================================================= |
92 | //function : Derivatives |
93 | //purpose : |
94 | //======================================================================= |
95 | |
96 | Standard_Boolean BRepBlend_SurfCurvConstRadInv::Derivatives(const math_Vector& X, |
97 | math_Matrix& D) |
98 | { |
99 | gp_Pnt ptgui; |
100 | gp_Vec d1gui,d2gui; |
101 | guide->D2(X(1),ptgui,d1gui,d2gui); |
102 | Standard_Real normd1gui = d1gui.Magnitude(); |
103 | Standard_Real unsurnormd1gui = 1./normd1gui; |
104 | gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui); |
105 | #ifdef DEB |
106 | Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ())); |
107 | #endif |
108 | gp_Vec dnplan; |
109 | dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui); |
110 | dnplan.Multiply(unsurnormd1gui); |
111 | Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ()); |
112 | gp_Pnt ptcur; |
113 | gp_Vec d1cur; |
114 | curv->D1(X(2),ptcur,d1cur); |
115 | D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD; |
116 | D(1,2) = nplan.XYZ().Dot(d1cur.XYZ()); |
117 | D(1,3) = 0.; |
118 | |
119 | gp_Pnt2d p2drst; |
120 | gp_Vec2d d1rst; |
121 | rst->D1(X(3),p2drst,d1rst); |
122 | gp_Pnt pts; |
123 | gp_Vec d1u,d1v,d2u,d2v,duv; |
124 | surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv); |
125 | D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD; |
126 | D(2,2) = 0.; |
127 | gp_Vec dwrstpts; |
128 | dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v); |
129 | D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ()); |
130 | |
131 | gp_Vec nsurf = d1u.Crossed(d1v); |
132 | gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv)); |
133 | gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v)); |
134 | gp_Vec dwrstnsurf; |
135 | dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf); |
136 | |
137 | gp_Vec nplancrosnsurf = nplan.Crossed(nsurf); |
138 | gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf); |
139 | gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf); |
140 | |
141 | Standard_Real norm2 = nplancrosnsurf.SquareMagnitude(); |
142 | Standard_Real norm = sqrt(norm2); |
143 | Standard_Real unsurnorm = 1./norm; |
144 | Standard_Real raysurnorm = ray*unsurnorm; |
145 | Standard_Real unsurnorm2 = unsurnorm * unsurnorm; |
146 | Standard_Real raysurnorm2 = ray*unsurnorm2; |
147 | Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf); |
148 | Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf); |
149 | |
150 | Standard_Real nplandotnsurf = nplan.Dot(nsurf); |
151 | Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf); |
152 | Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf); |
153 | |
154 | gp_Vec temp,dwguitemp,dwrsttemp; |
155 | temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf); |
156 | dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan); |
157 | dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf); |
158 | |
159 | gp_Vec corde(ptcur,pts); |
160 | gp_Vec ref,dwguiref,dwrstref; |
161 | ref.SetLinearForm(raysurnorm,temp,corde); |
162 | dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp); |
163 | dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts); |
164 | |
165 | ref.Add(ref); |
166 | D(3,1) = ref.Dot(dwguiref); |
167 | D(3,2) = -ref.Dot(d1cur); |
168 | D(3,3) = ref.Dot(dwrstref); |
169 | |
170 | return Standard_True; |
171 | } |
172 | |
173 | //======================================================================= |
174 | //function : Values |
175 | //purpose : |
176 | //======================================================================= |
177 | |
178 | Standard_Boolean BRepBlend_SurfCurvConstRadInv::Values(const math_Vector& X, |
179 | math_Vector& F, |
180 | math_Matrix& D) |
181 | { |
182 | gp_Pnt ptgui; |
183 | gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.); |
184 | guide->D2(X(1),ptgui,d1gui,d2gui); |
185 | Standard_Real normd1gui = d1gui.Magnitude(); |
186 | Standard_Real unsurnormd1gui = 1./normd1gui; |
187 | gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui); |
188 | Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ())); |
189 | gp_Vec dnplan; |
190 | dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui); |
191 | dnplan.Multiply(unsurnormd1gui); |
192 | Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ()); |
193 | gp_Pnt ptcur; |
194 | gp_Vec d1cur; |
195 | curv->D1(X(2),ptcur,d1cur); |
196 | F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD; |
197 | D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD; |
198 | D(1,2) = nplan.XYZ().Dot(d1cur.XYZ()); |
199 | D(1,3) = 0.; |
200 | |
201 | gp_Pnt2d p2drst; |
202 | gp_Vec2d d1rst; |
203 | rst->D1(X(3),p2drst,d1rst); |
204 | gp_Pnt pts; |
205 | gp_Vec d1u,d1v,d2u,d2v,duv; |
206 | surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv); |
207 | F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD; |
208 | D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD; |
209 | D(2,2) = 0.; |
210 | gp_Vec dwrstpts; |
211 | dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v); |
212 | D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ()); |
213 | |
214 | gp_Vec nsurf = d1u.Crossed(d1v); |
215 | gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv)); |
216 | gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v)); |
217 | gp_Vec dwrstnsurf; |
218 | dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf); |
219 | |
220 | gp_Vec nplancrosnsurf = nplan.Crossed(nsurf); |
221 | gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf); |
222 | gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf); |
223 | |
224 | Standard_Real norm2 = nplancrosnsurf.SquareMagnitude(); |
225 | Standard_Real norm = sqrt(norm2); |
226 | Standard_Real unsurnorm = 1./norm; |
227 | Standard_Real raysurnorm = ray*unsurnorm; |
228 | Standard_Real unsurnorm2 = unsurnorm * unsurnorm; |
229 | Standard_Real raysurnorm2 = ray*unsurnorm2; |
230 | Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf); |
231 | Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf); |
232 | |
233 | Standard_Real nplandotnsurf = nplan.Dot(nsurf); |
234 | Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf); |
235 | Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf); |
236 | |
237 | gp_Vec temp,dwguitemp,dwrsttemp; |
238 | temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf); |
239 | dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan); |
240 | dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf); |
241 | |
242 | gp_Vec corde(ptcur,pts); |
243 | gp_Vec ref,dwguiref,dwrstref; |
244 | ref.SetLinearForm(raysurnorm,temp,corde); |
245 | F(3) = ref.SquareMagnitude() - ray*ray; |
246 | dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp); |
247 | dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts); |
248 | |
249 | ref.Add(ref); |
250 | D(3,1) = ref.Dot(dwguiref); |
251 | D(3,2) = -ref.Dot(d1cur); |
252 | D(3,3) = ref.Dot(dwrstref); |
253 | return Standard_True; |
254 | } |
255 | |
256 | //======================================================================= |
257 | //function : Set |
258 | //purpose : |
259 | //======================================================================= |
260 | |
261 | void BRepBlend_SurfCurvConstRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst) |
262 | { |
263 | rst = Rst; |
264 | } |
265 | |
266 | //======================================================================= |
267 | //function : GetTolerance |
268 | //purpose : |
269 | //======================================================================= |
270 | |
271 | void BRepBlend_SurfCurvConstRadInv::GetTolerance(math_Vector& Tolerance, |
272 | const Standard_Real Tol) const |
273 | { |
274 | Tolerance(1) = guide->Resolution(Tol); |
275 | Tolerance(2) = curv->Resolution(Tol); |
276 | Standard_Real ru,rv; |
277 | ru = surf->UResolution(Tol); |
278 | rv = surf->VResolution(Tol); |
279 | Tolerance(3) = rst->Resolution(Min(ru,rv)); |
280 | } |
281 | |
282 | //======================================================================= |
283 | //function : GetBounds |
284 | //purpose : |
285 | //======================================================================= |
286 | |
287 | void BRepBlend_SurfCurvConstRadInv::GetBounds(math_Vector& InfBound, |
288 | math_Vector& SupBound) const |
289 | { |
290 | InfBound(1) = guide->FirstParameter(); |
291 | SupBound(1) = guide->LastParameter(); |
292 | InfBound(2) = curv->FirstParameter(); |
293 | SupBound(2) = curv->LastParameter(); |
294 | InfBound(3) = rst->FirstParameter(); |
295 | SupBound(3) = rst->LastParameter(); |
296 | } |
297 | |
298 | //======================================================================= |
299 | //function : IsSolution |
300 | //purpose : |
301 | //======================================================================= |
302 | |
303 | Standard_Boolean BRepBlend_SurfCurvConstRadInv::IsSolution(const math_Vector& Sol, |
304 | const Standard_Real Tol) |
305 | { |
306 | math_Vector valsol(1,3); |
307 | Value(Sol,valsol); |
308 | if (Abs(valsol(1)) <= Tol && |
309 | Abs(valsol(2)) <= Tol && |
310 | Abs(valsol(3)) <= 2*Tol*Abs(ray) ) { |
311 | return Standard_True; |
312 | } |
313 | return Standard_False; |
314 | } |
315 | |
316 | |