0024624: Lost word in license statement in source files
[occt.git] / src / BRepBlend / BRepBlend_SurfCurvEvolRadInv.cxx
1 // Created on: 1997-07-29
2 // Created by: Jerome LEMONIER
3 // Copyright (c) 1997-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17 #include <BRepBlend_SurfCurvEvolRadInv.ixx>
18
19 //=======================================================================
20 //function :
21 //purpose  : 
22 //=======================================================================
23 BRepBlend_SurfCurvEvolRadInv::BRepBlend_SurfCurvEvolRadInv
24 (const Handle(Adaptor3d_HSurface)& S,
25 const Handle(Adaptor3d_HCurve)& C,
26 const Handle(Adaptor3d_HCurve)& Cg,
27  const Handle(Law_Function)& Evol): surf(S),curv(C),guide(Cg)
28 { tevol=Evol;
29 }
30
31 //=======================================================================
32 //function :
33 //purpose  : 
34 //=======================================================================
35  void BRepBlend_SurfCurvEvolRadInv::Set(const Standard_Integer Choix) 
36 { choix = Choix;
37   switch (choix) {
38   case 1 :
39   case 2 :
40     sg1 = -1;
41     break;
42   case 3 :
43   case 4 :
44     sg1 = 1;
45     break;
46   default :
47     sg1 = -1;
48     break;
49   }
50
51 }
52
53 //=======================================================================
54 //function :
55 //purpose  : 
56 //=======================================================================
57  Standard_Integer BRepBlend_SurfCurvEvolRadInv::NbEquations() const
58 {
59   return 3;
60 }
61
62 //=======================================================================
63 //function :
64 //purpose  : 
65 //=======================================================================
66  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::Value(const math_Vector& X,math_Vector& F) 
67 {
68   gp_Pnt ptgui;
69   gp_Vec d1gui(0.,0.,0.);
70   guide->D1(X(1),ptgui,d1gui);
71   ray = sg1*tevol->Value(X(1));
72   gp_Vec nplan = d1gui.Normalized();
73 //  Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
74   gp_XYZ nplanXYZ(nplan.XYZ());
75   gp_XYZ ptguiXYZ(ptgui.XYZ());
76   Standard_Real theD =  nplanXYZ.Dot(ptguiXYZ)  ;
77   theD = theD  * (-1.) ;
78
79   gp_Pnt ptcur = curv->Value(X(2));
80   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
81   gp_Pnt2d p2drst = rst->Value(X(3));
82   gp_Pnt pts;
83   gp_Vec du,dv;
84   surf->D1(p2drst.X(),p2drst.Y(),pts,du,dv);
85   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
86   gp_Vec ns = du.Crossed(dv);
87   Standard_Real norm = nplan.Crossed(ns).Magnitude();
88   Standard_Real unsurnorm = 1./norm;
89   ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
90   ns.Multiply(unsurnorm);
91   gp_Vec ref(ptcur,pts);
92
93   ref.SetLinearForm(ray,ns,ref);
94   F(3) = ref.SquareMagnitude() - ray*ray;
95   return Standard_True;
96 }
97
98 //=======================================================================
99 //function :
100 //purpose  : 
101 //=======================================================================
102  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::Derivatives(const math_Vector& X,math_Matrix& D) 
103 {
104   gp_Pnt ptgui;
105   gp_Vec d1gui,d2gui;
106   guide->D2(X(1),ptgui,d1gui,d2gui);
107   Standard_Real normd1gui = d1gui.Magnitude(),dray;
108   Standard_Real unsurnormd1gui = 1./normd1gui;
109   tevol->D1(X(1),ray,dray);
110   ray=sg1*ray;
111   dray=sg1*dray;
112   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
113   gp_Vec dnplan;
114   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
115   dnplan.Multiply(unsurnormd1gui);
116   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
117   gp_Pnt ptcur;
118   gp_Vec d1cur;
119   curv->D1(X(2),ptcur,d1cur);
120   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
121   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
122   D(1,3) = 0.;
123
124   gp_Pnt2d p2drst;
125   gp_Vec2d d1rst;
126   rst->D1(X(3),p2drst,d1rst);
127   gp_Pnt pts;
128   gp_Vec d1u,d1v,d2u,d2v,duv;
129   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
130   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
131   D(2,2) = 0.;
132   gp_Vec dwrstpts;
133   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
134   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
135
136   gp_Vec nsurf = d1u.Crossed(d1v);
137   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
138   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
139   gp_Vec dwrstnsurf;
140   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
141
142   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
143   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
144   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
145
146   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
147   Standard_Real norm       = sqrt(norm2);
148   Standard_Real unsurnorm  = 1./norm;
149   Standard_Real raysurnorm = ray*unsurnorm;
150   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
151   Standard_Real raysurnorm2 = ray*unsurnorm2;
152   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
153   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
154
155   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
156   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
157   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
158
159   gp_Vec temp,dwguitemp,dwrsttemp;
160   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
161   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
162   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
163
164   gp_Vec corde(ptcur,pts);
165   gp_Vec ref,dwguiref,dwrstref;
166   ref.SetLinearForm(raysurnorm,temp,corde);
167   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
168   dwguiref.SetLinearForm(1.,dwguiref,dray*unsurnorm,temp);
169   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
170
171   ref.Add(ref);
172   D(3,1) = ref.Dot(dwguiref) - 2.*dray*ray;
173   D(3,2) = -ref.Dot(d1cur);
174   D(3,3) = ref.Dot(dwrstref);
175
176   return Standard_True;
177 }
178
179 //=======================================================================
180 //function :
181 //purpose  : 
182 //=======================================================================
183  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::Values(const math_Vector& X,math_Vector& F,math_Matrix& D) 
184 {
185   gp_Pnt ptgui;
186   gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.);
187   guide->D2(X(1),ptgui,d1gui,d2gui);
188   Standard_Real dray;
189   tevol->D1(X(1),ray,dray);
190   ray=sg1*ray;
191   dray=sg1*dray;
192   Standard_Real normd1gui = d1gui.Magnitude();
193   Standard_Real unsurnormd1gui = 1./normd1gui;
194   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
195 //  Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
196   gp_XYZ nplanXYZ(nplan.XYZ());
197   gp_XYZ ptcurXYZ(ptgui.XYZ());
198   Standard_Real theD =  nplanXYZ.Dot(ptcurXYZ)  ;
199   theD = theD  * (-1.) ;
200
201   gp_Vec dnplan;
202   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
203   dnplan.Multiply(unsurnormd1gui);
204   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
205   gp_Pnt ptcur;
206   gp_Vec d1cur;
207   curv->D1(X(2),ptcur,d1cur);
208   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
209   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
210   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
211   D(1,3) = 0.;
212
213   gp_Pnt2d p2drst;
214   gp_Vec2d d1rst;
215   rst->D1(X(3),p2drst,d1rst);
216   gp_Pnt pts;
217   gp_Vec d1u,d1v,d2u,d2v,duv;
218   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
219   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
220   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
221   D(2,2) = 0.;
222   gp_Vec dwrstpts;
223   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
224   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
225
226   gp_Vec nsurf = d1u.Crossed(d1v);
227   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
228   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
229   gp_Vec dwrstnsurf;
230   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
231
232   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
233   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
234   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
235
236   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
237   Standard_Real norm       = sqrt(norm2);
238   Standard_Real unsurnorm  = 1./norm;
239   Standard_Real raysurnorm = ray*unsurnorm;
240   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
241   Standard_Real raysurnorm2 = ray*unsurnorm2;
242   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
243   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
244
245   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
246   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
247   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
248
249   gp_Vec temp,dwguitemp,dwrsttemp;
250   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
251   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
252   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
253
254   gp_Vec corde(ptcur,pts);
255   gp_Vec ref,dwguiref,dwrstref;
256   ref.SetLinearForm(raysurnorm,temp,corde);
257   F(3) = ref.SquareMagnitude() - ray*ray;
258   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
259   dwguiref.SetLinearForm(1.,dwguiref,dray*unsurnorm,temp);
260   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
261
262   ref.Add(ref);
263   D(3,1) = ref.Dot(dwguiref) - 2.*dray*ray;
264   D(3,2) = -ref.Dot(d1cur);
265   D(3,3) = ref.Dot(dwrstref);
266   return Standard_True;
267 }
268
269 //=======================================================================
270 //function :
271 //purpose  : 
272 //=======================================================================
273  void BRepBlend_SurfCurvEvolRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst) 
274 {
275   rst = Rst;
276 }
277
278 //=======================================================================
279 //function :
280 //purpose  : 
281 //=======================================================================
282  void BRepBlend_SurfCurvEvolRadInv::GetTolerance(math_Vector& Tolerance,const Standard_Real Tol) const
283 {
284   Tolerance(1) = guide->Resolution(Tol);
285   Tolerance(2) = curv->Resolution(Tol);
286   Standard_Real ru,rv;
287   ru = surf->UResolution(Tol);
288   rv = surf->VResolution(Tol);
289   Tolerance(3) = rst->Resolution(Min(ru,rv)); 
290 }
291
292 //=======================================================================
293 //function :
294 //purpose  : 
295 //=======================================================================
296  void BRepBlend_SurfCurvEvolRadInv::GetBounds(math_Vector& InfBound,math_Vector& SupBound) const
297 {
298   InfBound(1) = guide->FirstParameter();
299   SupBound(1) = guide->LastParameter();
300   InfBound(2) = curv->FirstParameter();
301   SupBound(2) = curv->LastParameter();
302   InfBound(3) = rst->FirstParameter();
303   SupBound(3) = rst->LastParameter();
304 }
305
306 //=======================================================================
307 //function :
308 //purpose  : 
309 //=======================================================================
310  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::IsSolution(const math_Vector& Sol,const Standard_Real Tol) 
311 {
312   math_Vector valsol(1,3);
313   Value(Sol,valsol);
314   if (Abs(valsol(1)) <= Tol && 
315       Abs(valsol(2)) <= Tol &&
316       Abs(valsol(3)) <= 2*Tol*Abs(ray)) {
317     return Standard_True;
318   }
319   return Standard_False;
320 }
321