0024750: Replace instantiations of TCollection generic classes by NCollection templat...
[occt.git] / src / GProp / GProp_VelGProps.cxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
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
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.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
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
24 GProp_VelGProps::GProp_VelGProps(){}
25
26 void GProp_VelGProps::SetLocation(const gp_Pnt& VLocation)
27 {
28   loc =VLocation;
29 }
30
31
32 GProp_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
43 GProp_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
54 GProp_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
66 GProp_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
77 void 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
144 void 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
224 void 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
304 void 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