Integration of OCCT 6.5.0 from SVN
[occt.git] / src / BRepBlend / BRepBlend_SurfCurvConstRadInv.cxx
CommitLineData
7fd59977 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
14BRepBlend_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
26void 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
55Standard_Integer BRepBlend_SurfCurvConstRadInv::NbEquations() const
56{
57 return 3;
58}
59
60//=======================================================================
61//function : Value
62//purpose :
63//=======================================================================
64
65Standard_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
96Standard_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
178Standard_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
261void BRepBlend_SurfCurvConstRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst)
262{
263 rst = Rst;
264}
265
266//=======================================================================
267//function : GetTolerance
268//purpose :
269//=======================================================================
270
271void 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
287void 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
303Standard_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