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