0024750: Replace instantiations of TCollection generic classes by NCollection templat...
[occt.git] / src / GProp / GProp_VelGProps.cxx
CommitLineData
b311480e 1// Copyright (c) 1995-1999 Matra Datavision
973c2be1 2// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e 3//
973c2be1 4// This file is part of Open CASCADE Technology software library.
b311480e 5//
d5f74e42 6// This library is free software; you can redistribute it and/or modify it under
7// the terms of the GNU Lesser General Public License version 2.1 as published
973c2be1 8// by the Free Software Foundation, with special exception defined in the file
9// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10// distribution for complete text of the license and disclaimer of any warranty.
b311480e 11//
973c2be1 12// Alternatively, this file may be used under the terms of Open CASCADE
13// commercial license or contractual agreement.
b311480e 14
7fd59977 15#include <GProp_VelGProps.ixx>
16#include <Standard_NotImplemented.hxx>
17#include <gp.hxx>
18#include <GProp.hxx>
19#include <math_Matrix.hxx>
20#include <math_Vector.hxx>
21#include <math_Jacobi.hxx>
22
23
24GProp_VelGProps::GProp_VelGProps(){}
25
26void GProp_VelGProps::SetLocation(const gp_Pnt& VLocation)
27{
28 loc =VLocation;
29}
30
31
32GProp_VelGProps::GProp_VelGProps(const gp_Cylinder& S,
33 const Standard_Real Alpha1,
34 const Standard_Real Alpha2,
35 const Standard_Real Z1,
36 const Standard_Real Z2,
37 const gp_Pnt& VLocation)
38{
39 SetLocation(VLocation);
40 Perform(S,Alpha1,Alpha2,Z1,Z2);
41}
42
43GProp_VelGProps::GProp_VelGProps(const gp_Cone& S,
44 const Standard_Real Alpha1,
45 const Standard_Real Alpha2,
46 const Standard_Real Z1,
47 const Standard_Real Z2,
48 const gp_Pnt& VLocation)
49{
50 SetLocation(VLocation);
51 Perform(S,Alpha1,Alpha2,Z1,Z2);
52}
53
54GProp_VelGProps::GProp_VelGProps(const gp_Sphere& S,
55 const Standard_Real Teta1,
56 const Standard_Real Teta2,
57 const Standard_Real Alpha1,
58 const Standard_Real Alpha2,
59 const gp_Pnt& VLocation)
60{
61 SetLocation(VLocation);
62 Perform(S,Teta1,Teta2,Alpha1,Alpha2);
63}
64
65
66GProp_VelGProps::GProp_VelGProps(const gp_Torus& S,
67 const Standard_Real Teta1,
68 const Standard_Real Teta2,
69 const Standard_Real Alpha1,
70 const Standard_Real Alpha2,
71 const gp_Pnt& VLocation)
72{
73 SetLocation(VLocation);
74 Perform(S,Teta1,Teta2,Alpha1,Alpha2);
75}
76
77void GProp_VelGProps::Perform(const gp_Cylinder& S,
78 const Standard_Real Alpha1,
79 const Standard_Real Alpha2,
80 const Standard_Real Z1,
81 const Standard_Real Z2)
82{
83 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
84 S.Location().Coord(X0,Y0,Z0);
85 Standard_Real Rayon = S.Radius();
86 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
87 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
88 S.Position().Direction().Coord(Xa3,Ya3,Za3);
89 dim = Rayon*Rayon*(Z2-Z1)/2.;
90 Standard_Real SA2 = Sin(Alpha2);
91 Standard_Real SA1 = Sin(Alpha1);
92 Standard_Real CA2 = Cos(Alpha2);
93 Standard_Real CA1 = Cos(Alpha1);
94 Standard_Real Dsin = SA2-SA1;
95 Standard_Real Dcos = CA1-CA2;
96 Standard_Real Coef = Rayon/(Alpha2-Alpha1);
97
98 g.SetCoord(X0+(Coef*(Xa1*Dsin+Xa2*Dcos) ) + (Xa3*(Z2+Z1)/2.),
99 Y0+(Coef*(Ya1*Dsin+Ya2*Dcos) ) + (Ya3*(Z2+Z1)/2.),
100 Z0+(Coef*(Za1*Dsin+Za2*Dcos) ) + (Za3*(Z2+Z1)/2.) );
101
102 Standard_Real ICn2 = dim/2. *( Alpha2-Alpha1 + SA2*CA2 - SA1*CA1 );
103 Standard_Real ISn2 = dim/2. *( Alpha2-Alpha1 - SA2*CA2 + SA1*CA1 );
104 Standard_Real IZ2 = dim * (Alpha2-Alpha1)*(Z2*Z2+Z1*Z2+Z1*Z1);
105 Standard_Real ICnSn = dim *(CA2*CA2-CA1*CA1)/2.;
106 Standard_Real ICnz = dim *(Z2+Z1)/2.*Dsin;
107 Standard_Real ISnz = dim *(Z2+Z1)/2.*Dcos;
108 dim =(Alpha2-Alpha1)*dim;
109
110 math_Matrix Dm(1,3,1,3);
111
112 Dm(1,1) = Rayon*Rayon*ISn2 + IZ2;
113 Dm(2,2) = Rayon*Rayon*ICn2 + IZ2;
114 Dm(3,3) = Rayon*Rayon*dim;
115 Dm(1,2) = Dm(2,1) = -Rayon*Rayon*ICnSn;
116 Dm(1,3) = Dm(3,1) = -Rayon*ICnz;
117 Dm(3,2) = Dm(2,3) = -Rayon*ISnz;
118
119 math_Matrix Passage (1,3,1,3);
120 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
121 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
122 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
123
124 math_Jacobi J(Dm);
125 math_Vector V1(1,3),V2(1,3),V3(1,3);
126 J.Vector(1,V1);
127 V1.Multiply(Passage,V1);
128 V1.Multiply(J.Value(1));
129 J.Vector(2,V2);
130 V2.Multiply(Passage,V2);
131 V2.Multiply(J.Value(2));
132 J.Vector(3,V3);
133 V3.Multiply(Passage,V3);
134 V3.Multiply(J.Value(3));
135
136 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
137 gp_XYZ(V1(2),V2(2),V3(2)),
138 gp_XYZ(V1(3),V2(3),V3(3)));
139 gp_Mat Hop;
140 GProp::HOperator(g,loc,dim,Hop);
141 inertia = inertia+Hop;
142}
143
144void GProp_VelGProps::Perform(const gp_Cone& S,
145 const Standard_Real Alpha1,
146 const Standard_Real Alpha2,
147 const Standard_Real Z1,
148 const Standard_Real Z2)
149{
150 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
151 S.Location().Coord(X0,Y0,Z0);
152 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
153 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
154 S.Position().Direction().Coord(Xa3,Ya3,Za3);
155 Standard_Real t =S.SemiAngle();
156 Standard_Real Cnt = Cos(t);
157 Standard_Real Snt = Sin(t);
158 Standard_Real R = S.RefRadius();
159 Standard_Real Sn2 = Sin(Alpha2);
160 Standard_Real Sn1 = Sin(Alpha1);
161 Standard_Real Cn2 = Cos(Alpha2);
162 Standard_Real Cn1 = Cos(Alpha1);
163 Standard_Real ZZ = (Z2-Z1)*(Z2-Z1)*Cnt*Snt;
164 Standard_Real Auxi1= 2*R +(Z2+Z1)*Snt;
165
166 dim = ZZ*(Alpha2-Alpha1)*Auxi1/2.;
167
168 Standard_Real R1 = R + Z1*Snt;
169 Standard_Real R2 = R + Z2*Snt;
170 Standard_Real Coef0 = (R1*R1+R1*R2+R2*R2);
171 Standard_Real Iz = Cnt*(R*(Z2+Z1) + 2*Snt*(Z1*Z1+Z1*Z2+Z2*Z2)/3.)/Auxi1;
172 Standard_Real Ix = Coef0*(Sn2-Sn1)/(Alpha2-Alpha1)/Auxi1;
173 Standard_Real Iy = Coef0*(Cn1-Cn2)/(Alpha2-Alpha1)/Auxi1;
174
175 g.SetCoord(X0 + Xa1*Ix + Xa2*Iy + Xa3*Iz,
176 Y0 + Ya1*Ix + Ya2*Iy + Ya3*Iz,
177 Z0 + Za1*Ix + Za2*Iy + Za3*Iz);
178
179 Standard_Real IR2 = ZZ*(R2*R2*R2+R2*R2*R1+R1*R1*R2+R1*R1*R1)/4.;
180 Standard_Real ICn2 = IR2*(Alpha2-Alpha1+Cn2*Sn2-Cn1*Sn1)/2.;
181 Standard_Real ISn2 = IR2*(Alpha2-Alpha1+Cn2*Sn2-Cn1*Sn1)/2.;
182 Standard_Real IZ2 = ZZ*Cnt*Cnt*(Alpha2-Alpha1)*
183 (Z1*Z1*(R/3 + Z1*Snt/4) +
184 Z2*Z2*(R/3 + Z2*Snt/4) +
185 Z1*Z2*(R/3 +Z1*Snt/4 +Z2*Snt/4));
186 Standard_Real ICnSn = IR2*(Cn2*Cn2-Cn1*Cn1);
187 Standard_Real ICnz = (Z1+Z2)*ZZ*Coef0*(Sn2-Sn1)/3;
188 Standard_Real ISnz = (Z1+Z2)*ZZ*Coef0*(Cn1-Cn2)/3;
189
190
191 math_Matrix Dm(1,3,1,3);
192 Dm(1,1) = ISn2 + IZ2;
193 Dm(2,2) = ICn2 + IZ2;
194 Dm(3,3) = IR2*(Alpha2-Alpha1);
195 Dm(1,2) = Dm(2,1) = -ICnSn;
196 Dm(1,3) = Dm(3,1) = -ICnz;
197 Dm(3,2) = Dm(2,3) = -ISnz;
198
199 math_Matrix Passage (1,3,1,3);
200 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
201 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
202 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
203
204 math_Jacobi J(Dm);
205 math_Vector V1(1,3),V2(1,3),V3(1,3);
206 J.Vector(1,V1);
207 V1.Multiply(Passage,V1);
208 V1.Multiply(J.Value(1));
209 J.Vector(2,V2);
210 V2.Multiply(Passage,V2);
211 V2.Multiply(J.Value(2));
212 J.Vector(3,V3);
213 V3.Multiply(Passage,V3);
214 V3.Multiply(J.Value(3));
215
216 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
217 gp_XYZ(V1(2),V2(2),V3(2)),
218 gp_XYZ(V1(3),V2(3),V3(3)));
219 gp_Mat Hop;
220 GProp::HOperator(g,loc,dim,Hop);
221 inertia = inertia+Hop;
222}
223
224void GProp_VelGProps::Perform(const gp_Sphere& S,
225 const Standard_Real Teta1,
226 const Standard_Real Teta2,
227 const Standard_Real Alpha1,
228 const Standard_Real Alpha2)
229{
230 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
231 S.Location().Coord(X0,Y0,Z0);
232 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
233 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
234 S.Position().Direction().Coord(Xa3,Ya3,Za3);
235 Standard_Real R = S.Radius();
236 Standard_Real Cnt1 = Cos(Teta1);
237 Standard_Real Snt1 = Sin(Teta1);
238 Standard_Real Cnt2 = Cos(Teta2);
239 Standard_Real Snt2 = Sin(Teta2);
240 Standard_Real Cnf1 = Cos(Alpha1);
241 Standard_Real Snf1 = Sin(Alpha1);
242 Standard_Real Cnf2 = Cos(Alpha2);
243 Standard_Real Snf2 = Sin(Alpha2);
244
245 dim = (Teta2-Teta1)*R*R*R*(Snf2-Snf1)/3.;
246
247 Standard_Real Ix =
248 R*(Snt2-Snt1)/(Teta2-Teta1)*
249 (Alpha2-Alpha1+Snf2*Cnf2-Snf1*Cnf1)/(Snf2-Snf1)/2.;
250 Standard_Real Iy =
251 R*(Cnt1-Cnt2)/(Teta2-Teta1)*
252 (Alpha2-Alpha1+Snf2*Cnf2-Snf1*Cnf1)/(Snf2-Snf1)/2.;
253 Standard_Real Iz = R*(Snf2+Snf1)/2.;
254 g.SetCoord(
255 X0 + Ix*Xa1 + Iy*Xa2 + Iz*Xa3,
256 Y0 + Ix*Ya1 + Iy*Ya2 + Iz*Ya3,
257 Z0 + Ix*Za1 + Iy*Za2 + Iz*Za3);
258
259 Standard_Real IR2 = ( Cnf2*Snf2*(Cnf2+1.)- Cnf1*Snf1*(Cnf1+1.) +
260 Alpha2-Alpha1 )/9.;
261 Standard_Real ICn2 = (Teta2-Teta1+ Cnt2*Snt2-Cnt1*Snt1)*IR2/2.;
262 Standard_Real ISn2 = (Teta2-Teta1-Cnt2*Snt2+Cnt1*Snt1)*IR2/2.;
263 Standard_Real ICnSn = ( Snt2*Snt2-Snt1*Snt1)*IR2/2.;
264 Standard_Real IZ2 = (Teta2-Teta1)*(Snf2*Snf2*Snf2-Snf1*Snf1*Snf1)/9.;
265 Standard_Real ICnz =(Snt2-Snt1)*(Cnf1*Cnf1*Cnf1-Cnf2*Cnf2*Cnf2)/9.;
266 Standard_Real ISnz =(Cnt1-Cnt2)*(Cnf1*Cnf1*Cnf1-Cnf2*Cnf2*Cnf2)/9.;
267
268 math_Matrix Dm(1,3,1,3);
269 Dm(1,1) = ISn2 +IZ2;
270 Dm(2,2) = ICn2 +IZ2;
271 Dm(3,3) = IR2*(Teta2-Teta1);
272 Dm(1,2) = Dm(2,1) = -ICnSn;
273 Dm(1,3) = Dm(3,1) = -ICnz;
274 Dm(3,2) = Dm(2,3) = -ISnz;
275
276 math_Matrix Passage (1,3,1,3);
277 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
278 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
279 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
280
281 math_Jacobi J(Dm);
282 R = R*R*R*R*R;
283 math_Vector V1(1,3), V2(1,3), V3(1,3);
284 J.Vector(1,V1);
285 V1.Multiply(Passage,V1);
286 V1.Multiply(R*J.Value(1));
287 J.Vector(2,V2);
288 V2.Multiply(Passage,V2);
289 V2.Multiply(R*J.Value(2));
290 J.Vector(3,V3);
291 V3.Multiply(Passage,V3);
292 V3.Multiply(R*J.Value(3));
293
294 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
295 gp_XYZ(V1(2),V2(2),V3(2)),
296 gp_XYZ(V1(3),V2(3),V3(3)));
297 gp_Mat Hop;
298 GProp::HOperator(g,loc,dim,Hop);
299 inertia = inertia+Hop;
300
301}
302
303
304void GProp_VelGProps::Perform(const gp_Torus& S,
305 const Standard_Real Teta1,
306 const Standard_Real Teta2,
307 const Standard_Real Alpha1,
308 const Standard_Real Alpha2)
309{
310 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
311 S.Location().Coord(X0,Y0,Z0);
312 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
313 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
314 S.Position().Direction().Coord(Xa3,Ya3,Za3);
315 Standard_Real RMax = S.MajorRadius();
316 Standard_Real Rmin = S.MinorRadius();
317 Standard_Real Cnt1 = Cos(Teta1);
318 Standard_Real Snt1 = Sin(Teta1);
319 Standard_Real Cnt2 = Cos(Alpha2);
320 Standard_Real Snt2 = Sin(Alpha2);
321 Standard_Real Cnf1 = Cos(Alpha1);
322 Standard_Real Snf1 = Sin(Alpha1);
323 Standard_Real Cnf2 = Cos(Alpha2);
324 Standard_Real Snf2 = Sin(Alpha2);
325
326 dim = RMax*Rmin*Rmin*(Teta2-Teta1)*(Alpha2-Alpha1)/2.;
327 Standard_Real Ix =
328 (Snt2-Snt1)/(Teta2-Teta1)*(Rmin*(Snf2-Snf1)/(Alpha2-Alpha1) + RMax);
329 Standard_Real Iy =
330 (Cnt1-Cnt2)/(Teta2-Teta1)*(Rmin*(Snf2-Snf1)/(Alpha2-Alpha1) + RMax);
331 Standard_Real Iz = Rmin*(Cnf1-Cnf2)/(Alpha2-Alpha1);
332
333 g.SetCoord(
334 X0+Ix*Xa1+Iy*Xa2+Iz*Xa3,
335 Y0+Ix*Ya1+Iy*Ya2+Iz*Ya3,
336 Z0+Ix*Za1+Iy*Za2+Iz*Za3);
337
338 Standard_Real IR2 = RMax*RMax+Rmin*Rmin/2. +2.*RMax*Rmin*(Snf2-Snf1) +
339 Rmin*Rmin/2.*(Snf2*Cnf2-Snf1*Cnf1);
340 Standard_Real ICn2 = IR2*(Teta2-Teta1 +Snt2*Cnt2-Snt1*Cnt1)/2.;
341 Standard_Real ISn2 = IR2*(Teta2-Teta1 -Snt2*Cnt2+Snt1*Cnt1)/2.;
342 Standard_Real ICnSn = IR2*(Snt2*Snt2-Snt1*Snt1)/2.;
343 Standard_Real IZ2 =
344 (Teta2-Teta1)*Rmin*Rmin*(Alpha2-Alpha1-Snf2*Cnf2+Snf1*Cnf1)/2.;
345 Standard_Real ICnz = Rmin*(Snt2-Snt1)*(Cnf1-Cnf2)*(RMax+Rmin*(Cnf1+Cnf2)/2.);
346 Standard_Real ISnz = Rmin*(Cnt2-Cnt1)*(Cnf1-Cnf2)*(RMax+Rmin*(Cnf1+Cnf2)/2.);
347
348 math_Matrix Dm(1,3,1,3);
349 Dm(1,1) = ISn2 + IZ2;
350 Dm(2,2) = ICn2 + IZ2;
351 Dm(3,3) = IR2*(Teta2-Teta1);
352 Dm(1,2) = Dm(2,1) = -ICnSn;
353 Dm(1,3) = Dm(3,1) = -ICnz;
354 Dm(3,2) = Dm(2,3) = -ISnz;
355
356 math_Matrix Passage (1,3,1,3);
357 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
358 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
359 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
360
361 math_Jacobi J(Dm);
362 RMax = RMax*Rmin*Rmin/2.;
363 math_Vector V1(1,3), V2(1,3), V3(1,3);
364 J.Vector(1,V1);
365 V1.Multiply(Passage,V1);
366 V1.Multiply(RMax*J.Value(1));
367 J.Vector(2,V2);
368 V2.Multiply(Passage,V2);
369 V2.Multiply(RMax*J.Value(2));
370 J.Vector(3,V3);
371 V3.Multiply(Passage,V3);
372 V3.Multiply(RMax*J.Value(3));
373
374 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
375 gp_XYZ(V1(2),V2(2),V3(2)),
376 gp_XYZ(V1(3),V2(3),V3(3)));
377 gp_Mat Hop;
378 GProp::HOperator(g,loc,dim,Hop);
379 inertia = inertia+Hop;
380}
381
382
383