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