1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
16 #include <gp_Cone.hxx>
17 #include <gp_Cylinder.hxx>
19 #include <gp_Sphere.hxx>
20 #include <gp_Torus.hxx>
22 #include <GProp_SelGProps.hxx>
23 #include <math_Jacobi.hxx>
24 #include <math_Matrix.hxx>
25 #include <math_Vector.hxx>
26 #include <Standard_NotImplemented.hxx>
28 GProp_SelGProps::GProp_SelGProps(){}
30 void GProp_SelGProps::SetLocation(const gp_Pnt& SLocation )
36 void GProp_SelGProps::Perform(const gp_Cylinder& S,
37 const Standard_Real Alpha1,
38 const Standard_Real Alpha2,
39 const Standard_Real Z1,
40 const Standard_Real Z2)
42 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
43 S.Location().Coord(X0,Y0,Z0);
44 Standard_Real R = S.Radius();
45 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
46 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
47 S.Position().Direction().Coord(Xa3,Ya3,Za3);
48 dim = R*(Z2-Z1)*(Alpha2-Alpha1);
49 Standard_Real SA2 = Sin(Alpha2);
50 Standard_Real SA1 = Sin(Alpha1);
51 Standard_Real CA2 = Cos(Alpha2);
52 Standard_Real CA1 = Cos(Alpha1);
53 Standard_Real Ix = R*(SA2-SA1)/(Alpha2-Alpha1);
54 Standard_Real Iy = R*(CA1-CA2)/(Alpha2-Alpha1);
55 g.SetCoord( X0 + Ix*Xa1+Iy*Xa2 + Xa3*(Z2+Z1)/2.,
56 Y0 + Ix*Ya1+Iy*Ya2 + Ya3*(Z2+Z1)/2.,
57 Z0 + Ix*Za1+Iy*Za2 + Za3*(Z2+Z1)/2.);
58 Standard_Real ICn2 =R*R*( Alpha2-Alpha1 + SA2*CA2 - SA1*CA1 )/2.;
59 Standard_Real ISn2 =R*R*( Alpha2-Alpha1 - SA2*CA2 + SA1*CA1 )/2.;
60 Standard_Real IZ2 = (Alpha2-Alpha1)*(Z2*Z2+Z2*Z1+Z1*Z1)/3.;
61 Standard_Real ICnSn= R*R*( SA2*SA2-SA1*SA1)/2.;
62 Standard_Real ICnz = (Z2+Z1)*(SA2-SA1)/2.;
63 Standard_Real ISnz = (Z2+Z1)*(CA1-CA2)/2.;
65 math_Matrix Dm(1,3,1,3);
69 Dm(3,3) = Alpha2-Alpha1;
70 Dm(1,2) = Dm(2,1) = -ICnSn;
71 Dm(1,3) = Dm(3,1) = -ICnz;
72 Dm(3,2) = Dm(2,3) = -ISnz;
74 math_Matrix Passage (1,3,1,3);
75 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
76 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
77 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
81 math_Vector V1(1,3), V2(1,3), V3(1,3);
83 V1.Multiply(Passage,V1);
84 V1.Multiply(R*J.Value(1));
86 V2.Multiply(Passage,V2);
87 V2.Multiply(R*J.Value(2));
89 V3.Multiply(Passage,V3);
90 V3.Multiply(R*J.Value(3));
92 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
93 gp_XYZ(V1(2),V2(2),V3(2)),
94 gp_XYZ(V1(3),V2(3),V3(3)));
96 GProp::HOperator(g,loc,dim,Hop);
97 inertia = inertia+Hop;
100 void GProp_SelGProps::Perform (const gp_Cone& S,
101 const Standard_Real Alpha1,
102 const Standard_Real Alpha2,
103 const Standard_Real Z1,
104 const Standard_Real Z2)
107 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
108 S.Location().Coord(X0,Y0,Z0);
109 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
110 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
111 S.Position().Direction().Coord(Xa3,Ya3,Za3);
112 Standard_Real t =S.SemiAngle();
113 Standard_Real Cnt = Cos(t);
114 Standard_Real Snt = Sin(t);
115 Standard_Real R = S.RefRadius();
116 Standard_Real Sn2 = Sin(Alpha2);
117 Standard_Real Sn1 = Sin(Alpha1);
118 Standard_Real Cn2 = Cos(Alpha2);
119 Standard_Real Cn1 = Cos(Alpha1);
121 Standard_Real Auxi1 = R + (Z2+Z1)*Snt/2.;
122 Standard_Real Auxi2 = (Z2*Z2+Z1*Z2+Z1*Z1)/3.;
123 dim = (Alpha2-Alpha1)*Cnt*(Z2-Z1)*Auxi1;
126 (R*R+R*(Z2+Z1)*Snt + Snt*Auxi2)/Auxi1;
127 Standard_Real Iy = Ix*(Cn1-Cn2)/(Alpha2-Alpha1);
128 Ix = Ix*(Sn2-Sn1)/(Alpha2-Alpha1);
129 Standard_Real Iz = Cnt*(R*(Z2+Z1)/2.+Snt*Auxi2)/Auxi1;
131 g.SetCoord(X0 + Xa1*Ix+ Xa2*Iy+ Xa3*Iz,
132 Y0 + Ya1*Ix+ Ya2*Iy+ Ya3*Iz,
133 Z0 + Za1*Ix+ Za2*Iy+ Za3*Iz);
135 Standard_Real R1 = R+Z1*Snt;
136 Standard_Real R2 = R+Z2*Snt;
137 Standard_Real ZZ = (Z2-Z1)*Cnt;
138 Standard_Real IR2 = ZZ*Snt*(R1*R1*R1+R1*R1*R2+R1*R2*R2+R2*R2*R2)/4.;
139 Standard_Real ICn2 = IR2*(Alpha2-Alpha1+Cn2*Sn2-Cn1*Sn1)/2.;
140 Standard_Real ISn2 = IR2*(Alpha2-Alpha1+Cn2*Sn2-Cn1*Sn1)/2.;
141 Standard_Real IZ2 = ZZ*Cnt*Cnt*(Z2-Z1)*(Alpha2-Alpha1)*
142 (R*Auxi2 +Snt*(Z2*Z2*Z2+Z2*Z2*Z1+Z2*Z1*Z1+Z1*Z1*Z1))/4.;
143 Standard_Real ICnSn = IR2*(Cn2*Cn2-Cn1*Cn1);
144 Standard_Real ICnz = Cnt*Snt*ZZ*(R*(Z1+Z2)/2.+Auxi2)*(Sn2-Sn1);
145 Standard_Real ISnz = Cnt*Snt*ZZ*(R*(Z1+Z2)/2.+Auxi2)*(Cn1-Cn2);
147 math_Matrix Dm(1,3,1,3);
148 Dm(1,1) = ISn2 + IZ2;
149 Dm(2,2) = ICn2 + IZ2;
150 Dm(3,3) = IR2*(Alpha2-Alpha1);
151 Dm(1,2) = Dm(2,1) = -ICnSn;
152 Dm(1,3) = Dm(3,1) = -ICnz;
153 Dm(3,2) = Dm(2,3) = -ISnz;
155 math_Matrix Passage (1,3,1,3);
156 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
157 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
158 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
161 math_Vector V1(1,3),V2(1,3),V3(1,3);
163 V1.Multiply(Passage,V1);
164 V1.Multiply(J.Value(1));
166 V2.Multiply(Passage,V2);
167 V2.Multiply(J.Value(2));
169 V3.Multiply(Passage,V3);
170 V3.Multiply(J.Value(3));
172 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
173 gp_XYZ(V1(2),V2(2),V3(2)),
174 gp_XYZ(V1(3),V2(3),V3(3)));
176 GProp::HOperator(g,loc,dim,Hop);
177 inertia = inertia+Hop;
181 void GProp_SelGProps::Perform(const gp_Sphere& S,
182 const Standard_Real Teta1,
183 const Standard_Real Teta2,
184 const Standard_Real Alpha1,
185 const Standard_Real Alpha2)
187 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
188 S.Location().Coord(X0,Y0,Z0);
189 S.Position().XDirection().Coord(Xa1,Ya1,Za1);
190 S.Position().YDirection().Coord(Xa2,Ya2,Za2);
191 S.Position().Direction().Coord(Xa3,Ya3,Za3);
192 Standard_Real R = S.Radius();
193 Standard_Real Cnt1 = Cos(Teta1);
194 Standard_Real Snt1 = Sin(Teta1);
195 Standard_Real Cnt2 = Cos(Teta2);
196 Standard_Real Snt2 = Sin(Teta2);
197 Standard_Real Cnf1 = Cos(Alpha1);
198 Standard_Real Snf1 = Sin(Alpha1);
199 Standard_Real Cnf2 = Cos(Alpha2);
200 Standard_Real Snf2 = Sin(Alpha2);
201 dim = R*R*(Teta2-Teta1)*(Snf2-Snf1);
203 R*(Snt2-Snt1)/(Teta2-Teta1)*
204 (Alpha2-Alpha1+Snf2*Cnf2-Snf1*Cnf1)/(Snf2-Snf1)/2.;
206 R*(Cnt1-Cnt2)/(Teta2-Teta1)*
207 (Alpha2-Alpha1+Snf2*Cnf2-Snf1*Cnf1)/(Snf2-Snf1)/2.;
208 Standard_Real Iz = R*(Snf2+Snf1)/2.;
210 X0 + Ix*Xa1 + Iy*Xa2 + Iz*Xa3,
211 Y0 + Ix*Ya1 + Iy*Ya2 + Iz*Ya3,
212 Z0 + Ix*Za1 + Iy*Za2 + Iz*Za3);
214 Standard_Real IR2 = ( Cnf2*Snf2*(Cnf2+1.)- Cnf1*Snf1*(Cnf1+1.) +
216 Standard_Real ICn2 = (Teta2-Teta1+ Cnt2*Snt2-Cnt1*Snt1)*IR2/2.;
217 Standard_Real ISn2 = (Teta2-Teta1-Cnt2*Snt2+Cnt1*Snt1)*IR2/2.;
218 Standard_Real ICnSn = ( Snt2*Snt2-Snt1*Snt1)*IR2/2.;
219 Standard_Real IZ2 = (Teta2-Teta1)*(Snf2*Snf2*Snf2-Snf1*Snf1*Snf1)/3.;
220 Standard_Real ICnz =(Snt2-Snt1)*(Cnf1*Cnf1*Cnf1-Cnf2*Cnf2*Cnf2)/3.;
221 Standard_Real ISnz =(Cnt1-Cnt2)*(Cnf1*Cnf1*Cnf1-Cnf2*Cnf2*Cnf2)/3.;
223 math_Matrix Dm(1,3,1,3);
226 Dm(3,3) = IR2*(Teta2-Teta1);
227 Dm(1,2) = Dm(2,1) = -ICnSn;
228 Dm(1,3) = Dm(3,1) = -ICnz;
229 Dm(3,2) = Dm(2,3) = -ISnz;
231 math_Matrix Passage (1,3,1,3);
232 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
233 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
234 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
238 math_Vector V1(1,3), V2(1,3), V3(1,3);
240 V1.Multiply(Passage,V1);
241 V1.Multiply(R*J.Value(1));
243 V2.Multiply(Passage,V2);
244 V2.Multiply(R*J.Value(2));
246 V3.Multiply(Passage,V3);
247 V3.Multiply(R*J.Value(3));
249 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
250 gp_XYZ(V1(2),V2(2),V3(2)),
251 gp_XYZ(V1(3),V2(3),V3(3)));
253 GProp::HOperator(g,loc,dim,Hop);
254 inertia = inertia+Hop;
260 void GProp_SelGProps::Perform (const gp_Torus& S,
261 const Standard_Real Teta1,
262 const Standard_Real Teta2,
263 const Standard_Real Alpha1,
264 const Standard_Real Alpha2)
266 Standard_Real X0,Y0,Z0,Xa1,Ya1,Za1,Xa2,Ya2,Za2,Xa3,Ya3,Za3;
267 S.Location().Coord(X0,Y0,Z0);
268 S.XAxis().Direction().Coord(Xa1,Ya1,Za1);
269 S.YAxis().Direction().Coord(Xa2,Ya2,Za2);
270 S.Axis().Direction().Coord(Xa3,Ya3,Za3);
271 Standard_Real RMax = S.MajorRadius();
272 Standard_Real Rmin = S.MinorRadius();
273 Standard_Real Cnt1 = Cos(Teta1);
274 Standard_Real Snt1 = Sin(Teta1);
275 Standard_Real Cnt2 = Cos(Teta2);
276 Standard_Real Snt2 = Sin(Teta2);
277 Standard_Real Cnf1 = Cos(Alpha1);
278 Standard_Real Snf1 = Sin(Alpha1);
279 Standard_Real Cnf2 = Cos(Alpha2);
280 Standard_Real Snf2 = Sin(Alpha2);
283 dim = RMax*Rmin*(Teta2-Teta1)*(Alpha2-Alpha1);
285 (Snt2-Snt1)/(Teta2-Teta1)*(Rmin*(Snf2-Snf1)/(Alpha2-Alpha1) + RMax);
287 (Cnt1-Cnt2)/(Teta2-Teta1)*(Rmin*(Snf2-Snf1)/(Alpha2-Alpha1) + RMax);
288 Standard_Real Iz = Rmin*(Cnf1-Cnf2)/(Alpha2-Alpha1);
291 X0+Ix*Xa1+Iy*Xa2+Iz*Xa3,
292 Y0+Ix*Ya1+Iy*Ya2+Iz*Ya3,
293 Z0+Ix*Za1+Iy*Za2+Iz*Za3);
295 Standard_Real IR2 = RMax*RMax + 2.*RMax*Rmin*(Snf2-Snf1) +
296 Rmin*Rmin/2.*(Snf2*Cnf2-Snf1*Cnf1);
297 Standard_Real ICn2 = IR2*(Teta2-Teta1 +Snt2*Cnt2-Snt1*Cnt1)/2.;
298 Standard_Real ISn2 = IR2*(Teta2-Teta1 -Snt2*Cnt2+Snt1*Cnt1)/2.;
299 Standard_Real ICnSn = IR2*(Snt2*Snt2-Snt1*Snt1)/2.;
301 (Teta2-Teta1)*Rmin*Rmin*(Alpha2-Alpha1-Snf2*Cnf2+Snf1*Cnf1)/2.;
302 Standard_Real ICnz = Rmin*(Snt2-Snt1)*(Cnf1-Cnf2)*(RMax+Rmin*(Cnf1+Cnf2)/2.);
303 Standard_Real ISnz = Rmin*(Cnt2-Cnt1)*(Cnf1-Cnf2)*(RMax+Rmin*(Cnf1+Cnf2)/2.);
305 math_Matrix Dm(1,3,1,3);
306 Dm(1,1) = ISn2 + IZ2;
307 Dm(2,2) = ICn2 + IZ2;
308 Dm(3,3) = IR2*(Teta2-Teta1);
309 Dm(1,2) = Dm(2,1) = -ICnSn;
310 Dm(1,3) = Dm(3,1) = -ICnz;
311 Dm(3,2) = Dm(2,3) = -ISnz;
313 math_Matrix Passage (1,3,1,3);
314 Passage(1,1) = Xa1; Passage(1,2) = Xa2 ;Passage(1,3) = Xa3;
315 Passage(2,1) = Ya1; Passage(2,2) = Ya2 ;Passage(2,3) = Ya3;
316 Passage(3,1) = Za1; Passage(3,2) = Za2 ;Passage(3,3) = Za3;
320 math_Vector V1(1,3), V2(1,3), V3(1,3);
322 V1.Multiply(Passage,V1);
323 V1.Multiply(RMax*J.Value(1));
325 V2.Multiply(Passage,V2);
326 V2.Multiply(RMax*J.Value(2));
328 V3.Multiply(Passage,V3);
329 V3.Multiply(RMax*J.Value(3));
331 inertia = gp_Mat (gp_XYZ(V1(1),V2(1),V3(1)),
332 gp_XYZ(V1(2),V2(2),V3(2)),
333 gp_XYZ(V1(3),V2(3),V3(3)));
335 GProp::HOperator(g,loc,dim,Hop);
336 inertia = inertia+Hop;
343 GProp_SelGProps::GProp_SelGProps (const gp_Cone& S,
344 const Standard_Real Alpha1,
345 const Standard_Real Alpha2,
346 const Standard_Real Z1,
347 const Standard_Real Z2,
348 const gp_Pnt& SLocation )
350 SetLocation(SLocation);
351 Perform(S,Alpha1,Alpha2,Z1,Z2);
355 GProp_SelGProps::GProp_SelGProps (const gp_Cylinder& S,
356 const Standard_Real Alpha1,
357 const Standard_Real Alpha2,
358 const Standard_Real Z1,
359 const Standard_Real Z2,
360 const gp_Pnt& SLocation)
362 SetLocation(SLocation);
363 Perform(S,Alpha1,Alpha2,Z1,Z2);
367 GProp_SelGProps::GProp_SelGProps (const gp_Sphere& S,
368 const Standard_Real Teta1,
369 const Standard_Real Teta2,
370 const Standard_Real Alpha1,
371 const Standard_Real Alpha2,
372 const gp_Pnt& SLocation)
374 SetLocation(SLocation);
375 Perform(S,Teta1,Teta2,Alpha1,Alpha2);
379 GProp_SelGProps::GProp_SelGProps (const gp_Torus& S,
380 const Standard_Real Teta1,
381 const Standard_Real Teta2,
382 const Standard_Real Alpha1,
383 const Standard_Real Alpha2,
384 const gp_Pnt& SLocation)
386 SetLocation(SLocation);
387 Perform(S,Teta1,Teta2,Alpha1,Alpha2);