Integration of OCCT 6.5.0 from SVN
[occt.git] / src / BRepBlend / BRepBlend_SurfCurvConstRadInv.cxx
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