src/GProp/GProp_SelGProps.cxx

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