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