0024002: Overall code and build procedure refactoring -- automatic
[occt.git] / src / BRepBlend / BRepBlend_SurfPointEvolRadInv.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
18 #include <Adaptor3d_HCurve.hxx>
19 #include <Adaptor3d_HSurface.hxx>
20 #include <BRepBlend_SurfPointEvolRadInv.hxx>
21 #include <gp_Pnt.hxx>
22 #include <Law_Function.hxx>
23 #include <math_Matrix.hxx>
24
25 //=======================================================================
26 //function : 
27 //purpose  : 
28 //=======================================================================
29 BRepBlend_SurfPointEvolRadInv::BRepBlend_SurfPointEvolRadInv
30 (const Handle(Adaptor3d_HSurface)& S,
31 const Handle(Adaptor3d_HCurve)& C,
32  const Handle(Law_Function)& Evol
33 ) : surf(S), curv(C)
34 { tevol=Evol;
35 }
36
37 //=======================================================================
38 //function : 
39 //purpose  : 
40 //=======================================================================
41  void BRepBlend_SurfPointEvolRadInv::Set(const Standard_Integer Choix) 
42 {  choix = Choix;
43   switch (choix) {
44   case 1 :
45   case 2 :
46     sg1 = -1;
47     break;
48   case 3 :
49   case 4 :
50     sg1 = 1;
51     break;
52   default :
53     sg1 = -1;
54     break;
55   }
56 }
57
58 //=======================================================================
59 //function : 
60 //purpose  : 
61 //=======================================================================
62  Standard_Integer BRepBlend_SurfPointEvolRadInv::NbEquations() const
63 {
64   return 3;
65 }
66
67 //=======================================================================
68 //function : 
69 //purpose  : 
70 //=======================================================================
71  Standard_Boolean BRepBlend_SurfPointEvolRadInv::Value(const math_Vector& X,math_Vector& F) 
72 {
73   Standard_Real theD,norm,unsurnorm;
74   gp_Pnt ptcur,pts;
75   gp_Vec d1cur,d1u,d1v;
76   gp_XYZ nplan(0.,0.,0.),ns(0.,0.,0.),ref(0.,0.,0.);
77   curv->D1(X(1),ptcur,d1cur);
78   ray = sg1*tevol->Value(X(1));
79   nplan = d1cur.Normalized().XYZ();
80   theD = -(nplan.Dot(ptcur.XYZ()));
81   surf->D1(X(2),X(3),pts,d1u,d1v);
82   F(1) = nplan.Dot(point.XYZ()) + theD;
83   F(2) = nplan.Dot(pts.XYZ()) + theD;
84   ns = d1u.Crossed(d1v).XYZ();
85   norm = nplan.Crossed(ns).Modulus();
86   unsurnorm = 1./norm;
87   ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
88   ns.Multiply(unsurnorm);
89   ref = pts.XYZ() - point.XYZ();
90   ref.SetLinearForm(ray,ns,ref);
91   F(3) = ref.SquareModulus() - ray*ray;
92   return Standard_True;
93 }
94
95 //=======================================================================
96 //function : 
97 //purpose  : 
98 //=======================================================================
99  Standard_Boolean BRepBlend_SurfPointEvolRadInv::Derivatives(const math_Vector& X,math_Matrix& D) 
100 {
101   gp_Pnt ptcur,pts;
102   gp_Vec d1cur,d2cur,nplan,dnplan,d1u,d1v,d2u,d2v,duv;
103   Standard_Real dtheD, normd1cur, unsurnormd1cur,dray;
104
105   curv->D2(X(1),ptcur,d1cur,d2cur);
106   tevol->D1(X(1),ray,dray);
107   ray=sg1*ray;
108   dray=sg1*dray;
109   normd1cur = d1cur.Magnitude();
110   unsurnormd1cur = 1./normd1cur;
111   nplan = unsurnormd1cur * d1cur;
112   dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
113   dnplan.Multiply(unsurnormd1cur);
114   dtheD = - nplan.XYZ().Dot(d1cur.XYZ()) - dnplan.XYZ().Dot(ptcur.XYZ());
115   D(1,1) = dnplan.XYZ().Dot(point.XYZ()) + dtheD;
116   D(1,2) = D(1,3) = 0.;
117   surf->D2(X(2),X(3),pts,d1u,d1v,d2u,d2v,duv);
118   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
119   D(2,2) = nplan.Dot(d1u);
120   D(2,3) = nplan.Dot(d1v);
121   
122   gp_Vec nsurf = d1u.Crossed(d1v);
123   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
124   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
125
126   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
127   gp_Vec dwnplancrosnsurf = dnplan.Crossed(nsurf);
128   gp_Vec dunplancrosnsurf = nplan.Crossed(dunsurf);
129   gp_Vec dvnplancrosnsurf = nplan.Crossed(dvnsurf);
130
131   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
132   Standard_Real norm       = sqrt(norm2);
133   Standard_Real unsurnorm  = 1./norm;
134   Standard_Real raysurnorm = ray*unsurnorm;
135   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
136   Standard_Real raysurnorm2 = ray*unsurnorm2;
137   Standard_Real dwnorm = unsurnorm*nplancrosnsurf.Dot(dwnplancrosnsurf);
138   Standard_Real dunorm = unsurnorm*nplancrosnsurf.Dot(dunplancrosnsurf);
139   Standard_Real dvnorm = unsurnorm*nplancrosnsurf.Dot(dvnplancrosnsurf);
140
141   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
142   Standard_Real dwnplandotnsurf = dnplan.Dot(nsurf);
143   Standard_Real dunplandotnsurf = nplan.Dot(dunsurf);
144   Standard_Real dvnplandotnsurf = nplan.Dot(dvnsurf);
145   
146   gp_Vec temp,dwtemp,dutemp,dvtemp;
147   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
148   dwtemp.SetLinearForm(nplandotnsurf,dnplan,dwnplandotnsurf,nplan);
149   dutemp.SetLinearForm(dunplandotnsurf,nplan,-1.,dunsurf);
150   dvtemp.SetLinearForm(dvnplandotnsurf,nplan,-1.,dvnsurf);
151
152   gp_Vec ref,dwref,duref,dvref,corde(point,pts);
153   ref.SetLinearForm(raysurnorm,temp,corde);
154   dwref.SetLinearForm(raysurnorm,dwtemp,-raysurnorm2*dwnorm,temp);
155   dwref.SetLinearForm(1.,dwref,dray*unsurnorm,temp);
156   duref.SetLinearForm(raysurnorm,dutemp,-raysurnorm2*dunorm,temp,d1u);
157   dvref.SetLinearForm(raysurnorm,dvtemp,-raysurnorm2*dvnorm,temp,d1v);
158   
159   ref.Add(ref);
160   D(3,1) = ref.Dot(dwref) - 2.*dray*ray;
161   D(3,2) = ref.Dot(duref);
162   D(3,3) = ref.Dot(dvref);
163
164   return Standard_True;
165 }
166
167 //=======================================================================
168 //function : 
169 //purpose  : 
170 //=======================================================================
171  Standard_Boolean BRepBlend_SurfPointEvolRadInv::Values(const math_Vector& X,math_Vector& F,math_Matrix& D) 
172 {
173   gp_Pnt ptcur,pts;
174   gp_Vec d1cur,d2cur,nplan,dnplan,d1u,d1v,d2u,d2v,duv;
175   Standard_Real theD, dtheD, normd1cur, unsurnormd1cur,dray;
176
177   curv->D2(X(1),ptcur,d1cur,d2cur);
178   tevol->D1(X(1),ray,dray);
179   ray=sg1*ray;
180   dray=sg1*dray;
181   surf->D2(X(2),X(3),pts,d1u,d1v,d2u,d2v,duv);
182   normd1cur = d1cur.Magnitude();
183   unsurnormd1cur = 1./normd1cur;
184   nplan = unsurnormd1cur * d1cur;
185   theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
186   F(1) = nplan.XYZ().Dot(point.XYZ()) + theD;
187   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
188
189   dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
190   dnplan.Multiply(unsurnormd1cur);
191   dtheD = - nplan.XYZ().Dot(d1cur.XYZ()) - dnplan.XYZ().Dot(ptcur.XYZ());
192   D(1,1) = dnplan.XYZ().Dot(point.XYZ()) + dtheD;
193   D(1,2) = D(1,3) = 0.;
194   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
195   D(2,2) = nplan.Dot(d1u);
196   D(2,3) = nplan.Dot(d1v);
197   
198   gp_Vec nsurf = d1u.Crossed(d1v);
199   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
200   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
201
202   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
203   gp_Vec dwnplancrosnsurf = dnplan.Crossed(nsurf);
204   gp_Vec dunplancrosnsurf = nplan.Crossed(dunsurf);
205   gp_Vec dvnplancrosnsurf = nplan.Crossed(dvnsurf);
206
207   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
208   Standard_Real norm       = sqrt(norm2);
209   Standard_Real unsurnorm  = 1./norm;
210   Standard_Real raysurnorm = ray*unsurnorm;
211   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
212   Standard_Real raysurnorm2 = ray*unsurnorm2;
213   Standard_Real dwnorm = unsurnorm*nplancrosnsurf.Dot(dwnplancrosnsurf);
214   Standard_Real dunorm = unsurnorm*nplancrosnsurf.Dot(dunplancrosnsurf);
215   Standard_Real dvnorm = unsurnorm*nplancrosnsurf.Dot(dvnplancrosnsurf);
216
217   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
218   Standard_Real dwnplandotnsurf = dnplan.Dot(nsurf);
219   Standard_Real dunplandotnsurf = nplan.Dot(dunsurf);
220   Standard_Real dvnplandotnsurf = nplan.Dot(dvnsurf);
221   
222   gp_Vec temp,dwtemp,dutemp,dvtemp;
223   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
224   dwtemp.SetLinearForm(nplandotnsurf,dnplan,dwnplandotnsurf,nplan);
225   dutemp.SetLinearForm(dunplandotnsurf,nplan,-1.,dunsurf);
226   dvtemp.SetLinearForm(dvnplandotnsurf,nplan,-1.,dvnsurf);
227
228   gp_Vec ref,dwref,duref,dvref,corde(point,pts);
229   ref.SetLinearForm(raysurnorm,temp,corde);
230   F(3) = ref.SquareMagnitude() - ray*ray;
231   dwref.SetLinearForm(raysurnorm,dwtemp,-raysurnorm2*dwnorm,temp);
232   dwref.SetLinearForm(1.,dwref,dray*unsurnorm,temp);
233   duref.SetLinearForm(raysurnorm,dutemp,-raysurnorm2*dunorm,temp,d1u);
234   dvref.SetLinearForm(raysurnorm,dvtemp,-raysurnorm2*dvnorm,temp,d1v);
235   
236   ref.Add(ref);
237   D(3,1) = ref.Dot(dwref) - 2.*dray*ray;
238   D(3,2) = ref.Dot(duref);
239   D(3,3) = ref.Dot(dvref);
240
241   return Standard_True;
242 }
243
244 //=======================================================================
245 //function : 
246 //purpose  : 
247 //=======================================================================
248  void BRepBlend_SurfPointEvolRadInv::Set(const gp_Pnt& P) 
249 {
250   point = P;
251 }
252
253 //=======================================================================
254 //function : 
255 //purpose  : 
256 //=======================================================================
257  void BRepBlend_SurfPointEvolRadInv::GetTolerance(math_Vector& Tolerance,const Standard_Real Tol) const
258 {
259   Tolerance(1) = curv->Resolution(Tol);
260   Tolerance(2) = surf->UResolution(Tol);
261   Tolerance(3) = surf->VResolution(Tol);
262 }
263
264 //=======================================================================
265 //function : 
266 //purpose  : 
267 //=======================================================================
268  void BRepBlend_SurfPointEvolRadInv::GetBounds(math_Vector& InfBound,math_Vector& SupBound) const
269 {
270   InfBound(1) = curv->FirstParameter();
271   SupBound(1) = curv->LastParameter();
272   InfBound(2) = surf->FirstUParameter();
273   SupBound(2) = surf->LastUParameter();
274   InfBound(3) = surf->FirstVParameter();
275   SupBound(3) = surf->LastVParameter();
276 }
277
278 //=======================================================================
279 //function : 
280 //purpose  : 
281 //=======================================================================
282  Standard_Boolean BRepBlend_SurfPointEvolRadInv::IsSolution(const math_Vector& Sol,const Standard_Real Tol) 
283 {
284   math_Vector valsol(1,3);
285   Value(Sol,valsol);
286   if (Abs(valsol(1)) <= Tol && 
287       Abs(valsol(2)) <= Tol &&
288       Abs(valsol(3)) <= 2*Tol*Abs(ray) ) {
289     return Standard_True;
290   }
291   return Standard_False;
292 }
293