0023024: Update headers of OCCT 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-2012 OPEN CASCADE SAS
5 //
6 // The content of this file is subject to the Open CASCADE Technology Public
7 // License Version 6.5 (the "License"). You may not use the content of this file
8 // except in compliance with the License. Please obtain a copy of the License
9 // at http://www.opencascade.org and read it completely before using this file.
10 //
11 // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
12 // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
13 //
14 // The Original Code and all software distributed under the License is
15 // distributed on an "AS IS" basis, without warranty of any kind, and the
16 // Initial Developer hereby disclaims all such warranties, including without
17 // limitation, any warranties of merchantability, fitness for a particular
18 // purpose or non-infringement. Please see the License for the specific terms
19 // and conditions governing the rights and limitations under the License.
20
21
22
23 #include <BRepBlend_SurfCurvConstRadInv.ixx>
24
25 //=======================================================================
26 //function : BRepBlend_SurfCurvConstRadInv
27 //purpose  : 
28 //=======================================================================
29
30 BRepBlend_SurfCurvConstRadInv::BRepBlend_SurfCurvConstRadInv
31 (const Handle(Adaptor3d_HSurface)& S,
32  const Handle(Adaptor3d_HCurve)& C,
33  const Handle(Adaptor3d_HCurve)& Cg) : surf(S),curv(C),guide(Cg)
34 {
35 }
36
37 //=======================================================================
38 //function : Set
39 //purpose  : 
40 //=======================================================================
41
42 void BRepBlend_SurfCurvConstRadInv::Set(const Standard_Real R,
43                                         const Standard_Integer Choix) 
44 {
45   choix = Choix;
46   switch (choix) {
47   case 1:
48   case 2:
49     {
50       ray = -Abs(R);
51     }
52     break;
53   case 3:
54   case 4:
55     {
56       ray = Abs(R);
57     }
58     break;
59   default:
60     {
61       ray = -Abs(R);
62     }
63   }
64 }
65
66 //=======================================================================
67 //function : NbEquations
68 //purpose  : 
69 //=======================================================================
70
71 Standard_Integer BRepBlend_SurfCurvConstRadInv::NbEquations() const
72 {
73   return 3;
74 }
75
76 //=======================================================================
77 //function : Value
78 //purpose  : 
79 //=======================================================================
80
81 Standard_Boolean BRepBlend_SurfCurvConstRadInv::Value(const math_Vector& X,
82                                                       math_Vector& F) 
83 {
84   gp_Pnt ptgui;
85   gp_Vec d1gui;
86   guide->D1(X(1),ptgui,d1gui);
87   gp_Vec nplan = d1gui.Normalized();
88   Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
89   gp_Pnt ptcur = curv->Value(X(2));
90   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
91   gp_Pnt2d p2drst = rst->Value(X(3));
92   gp_Pnt pts;
93   gp_Vec du,dv;
94   surf->D1(p2drst.X(),p2drst.Y(),pts,du,dv);
95   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
96   gp_Vec ns = du.Crossed(dv);
97   Standard_Real norm = nplan.Crossed(ns).Magnitude();
98   Standard_Real unsurnorm = 1./norm;
99   ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
100   ns.Multiply(unsurnorm);
101   gp_Vec ref(ptcur,pts);
102   ref.SetLinearForm(ray,ns,ref);
103   F(3) = ref.SquareMagnitude() - ray*ray;
104   return Standard_True;
105 }
106
107 //=======================================================================
108 //function : Derivatives
109 //purpose  : 
110 //=======================================================================
111
112 Standard_Boolean BRepBlend_SurfCurvConstRadInv::Derivatives(const math_Vector& X,
113                                                             math_Matrix& D) 
114 {
115   gp_Pnt ptgui;
116   gp_Vec d1gui,d2gui;
117   guide->D2(X(1),ptgui,d1gui,d2gui);
118   Standard_Real normd1gui = d1gui.Magnitude();
119   Standard_Real unsurnormd1gui = 1./normd1gui;
120   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
121   gp_Vec dnplan;
122   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
123   dnplan.Multiply(unsurnormd1gui);
124   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
125   gp_Pnt ptcur;
126   gp_Vec d1cur;
127   curv->D1(X(2),ptcur,d1cur);
128   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
129   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
130   D(1,3) = 0.;
131
132   gp_Pnt2d p2drst;
133   gp_Vec2d d1rst;
134   rst->D1(X(3),p2drst,d1rst);
135   gp_Pnt pts;
136   gp_Vec d1u,d1v,d2u,d2v,duv;
137   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
138   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
139   D(2,2) = 0.;
140   gp_Vec dwrstpts;
141   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
142   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
143
144   gp_Vec nsurf = d1u.Crossed(d1v);
145   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
146   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
147   gp_Vec dwrstnsurf;
148   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
149
150   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
151   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
152   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
153
154   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
155   Standard_Real norm       = sqrt(norm2);
156   Standard_Real unsurnorm  = 1./norm;
157   Standard_Real raysurnorm = ray*unsurnorm;
158   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
159   Standard_Real raysurnorm2 = ray*unsurnorm2;
160   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
161   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
162
163   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
164   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
165   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
166
167   gp_Vec temp,dwguitemp,dwrsttemp;
168   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
169   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
170   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
171
172   gp_Vec corde(ptcur,pts);
173   gp_Vec ref,dwguiref,dwrstref;
174   ref.SetLinearForm(raysurnorm,temp,corde);
175   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
176   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
177
178   ref.Add(ref);
179   D(3,1) = ref.Dot(dwguiref);
180   D(3,2) = -ref.Dot(d1cur);
181   D(3,3) = ref.Dot(dwrstref);
182
183   return Standard_True;
184 }
185
186 //=======================================================================
187 //function : Values
188 //purpose  : 
189 //=======================================================================
190
191 Standard_Boolean BRepBlend_SurfCurvConstRadInv::Values(const math_Vector& X,
192                                                        math_Vector& F,
193                                                        math_Matrix& D) 
194 {
195   gp_Pnt ptgui;
196   gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.);
197   guide->D2(X(1),ptgui,d1gui,d2gui);
198   Standard_Real normd1gui = d1gui.Magnitude();
199   Standard_Real unsurnormd1gui = 1./normd1gui;
200   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
201   Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
202   gp_Vec dnplan;
203   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
204   dnplan.Multiply(unsurnormd1gui);
205   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
206   gp_Pnt ptcur;
207   gp_Vec d1cur;
208   curv->D1(X(2),ptcur,d1cur);
209   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
210   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
211   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
212   D(1,3) = 0.;
213
214   gp_Pnt2d p2drst;
215   gp_Vec2d d1rst;
216   rst->D1(X(3),p2drst,d1rst);
217   gp_Pnt pts;
218   gp_Vec d1u,d1v,d2u,d2v,duv;
219   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
220   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
221   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
222   D(2,2) = 0.;
223   gp_Vec dwrstpts;
224   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
225   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
226
227   gp_Vec nsurf = d1u.Crossed(d1v);
228   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
229   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
230   gp_Vec dwrstnsurf;
231   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
232
233   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
234   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
235   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
236
237   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
238   Standard_Real norm       = sqrt(norm2);
239   Standard_Real unsurnorm  = 1./norm;
240   Standard_Real raysurnorm = ray*unsurnorm;
241   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
242   Standard_Real raysurnorm2 = ray*unsurnorm2;
243   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
244   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
245
246   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
247   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
248   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
249
250   gp_Vec temp,dwguitemp,dwrsttemp;
251   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
252   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
253   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
254
255   gp_Vec corde(ptcur,pts);
256   gp_Vec ref,dwguiref,dwrstref;
257   ref.SetLinearForm(raysurnorm,temp,corde);
258   F(3) = ref.SquareMagnitude() - ray*ray;
259   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
260   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
261
262   ref.Add(ref);
263   D(3,1) = ref.Dot(dwguiref);
264   D(3,2) = -ref.Dot(d1cur);
265   D(3,3) = ref.Dot(dwrstref);
266   return Standard_True;
267 }
268
269 //=======================================================================
270 //function : Set
271 //purpose  : 
272 //=======================================================================
273
274 void BRepBlend_SurfCurvConstRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst) 
275 {
276   rst = Rst;
277 }
278
279 //=======================================================================
280 //function : GetTolerance
281 //purpose  : 
282 //=======================================================================
283
284 void BRepBlend_SurfCurvConstRadInv::GetTolerance(math_Vector& Tolerance,
285                                                  const Standard_Real Tol) const
286 {
287   Tolerance(1) = guide->Resolution(Tol);
288   Tolerance(2) = curv->Resolution(Tol);
289   Standard_Real ru,rv;
290   ru = surf->UResolution(Tol);
291   rv = surf->VResolution(Tol);
292   Tolerance(3) = rst->Resolution(Min(ru,rv)); 
293 }
294
295 //=======================================================================
296 //function : GetBounds
297 //purpose  : 
298 //=======================================================================
299
300 void BRepBlend_SurfCurvConstRadInv::GetBounds(math_Vector& InfBound,
301                                               math_Vector& SupBound) const
302 {
303   InfBound(1) = guide->FirstParameter();
304   SupBound(1) = guide->LastParameter();
305   InfBound(2) = curv->FirstParameter();
306   SupBound(2) = curv->LastParameter();
307   InfBound(3) = rst->FirstParameter();
308   SupBound(3) = rst->LastParameter();
309 }
310
311 //=======================================================================
312 //function : IsSolution
313 //purpose  : 
314 //=======================================================================
315
316 Standard_Boolean BRepBlend_SurfCurvConstRadInv::IsSolution(const math_Vector& Sol,
317                                                            const Standard_Real Tol) 
318 {
319   math_Vector valsol(1,3);
320   Value(Sol,valsol);
321   if (Abs(valsol(1)) <= Tol && 
322       Abs(valsol(2)) <= Tol &&
323       Abs(valsol(3)) <= 2*Tol*Abs(ray) ) {
324     return Standard_True;
325   }
326   return Standard_False;
327 }
328
329