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