src/GProp/GProp_SelGProps.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
  16 #include <gp_Cone.hxx>
  17 #include <gp_Cylinder.hxx>
  18 #include <gp_Pnt.hxx>
  19 #include <gp_Sphere.hxx>
  20 #include <gp_Torus.hxx>
  21 #include <GProp.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>
  27
  28 GProp_SelGProps::GProp_SelGProps(){}
  29
  30 void GProp_SelGProps::SetLocation(const gp_Pnt& SLocation )
  31 {
  32   loc = SLocation;
  33 }
  34
  35
  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)
  41  {
  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.;
  64
  65   math_Matrix Dm(1,3,1,3);
  66
  67   Dm(1,1) = ISn2 + IZ2;
  68   Dm(2,2) = ICn2 + IZ2;
  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;
  73
  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;
  78
  79   math_Jacobi J(Dm);
  80   R = R*(Z2-Z1);
  81   math_Vector V1(1,3), V2(1,3), V3(1,3);
  82   J.Vector(1,V1);
  83   V1.Multiply(Passage,V1);
  84   V1.Multiply(R*J.Value(1));
  85   J.Vector(2,V2);
  86   V2.Multiply(Passage,V2);
  87   V2.Multiply(R*J.Value(2));
  88   J.Vector(3,V3);
  89   V3.Multiply(Passage,V3);
  90   V3.Multiply(R*J.Value(3));
  91
  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)));
  95   gp_Mat Hop;
  96   GProp::HOperator(g,loc,dim,Hop);
  97   inertia = inertia+Hop;
  98 }
  99
 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)
 105
 106 {
 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);
 120
 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;
 124
 125   Standard_Real Ix  =
 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;
 130
 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);
 134
 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);
 146
 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;
 154
 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;
 159
 160   math_Jacobi J(Dm);
 161   math_Vector V1(1,3),V2(1,3),V3(1,3);
 162   J.Vector(1,V1);
 163   V1.Multiply(Passage,V1);
 164   V1.Multiply(J.Value(1));
 165   J.Vector(2,V2);
 166   V2.Multiply(Passage,V2);
 167   V2.Multiply(J.Value(2));
 168   J.Vector(3,V3);
 169   V3.Multiply(Passage,V3);
 170   V3.Multiply(J.Value(3));
 171
 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)));
 175   gp_Mat Hop;
 176   GProp::HOperator(g,loc,dim,Hop);
 177   inertia = inertia+Hop;
 178 }
 179
 180
 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)
 186 {
 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);
 202   Standard_Real Ix =
 203     R*(Snt2-Snt1)/(Teta2-Teta1)*
 204     (Alpha2-Alpha1+Snf2*Cnf2-Snf1*Cnf1)/(Snf2-Snf1)/2.;
 205   Standard_Real Iy =
 206     R*(Cnt1-Cnt2)/(Teta2-Teta1)*
 207     (Alpha2-Alpha1+Snf2*Cnf2-Snf1*Cnf1)/(Snf2-Snf1)/2.;
 208   Standard_Real Iz = R*(Snf2+Snf1)/2.;
 209   g.SetCoord(
 210    X0 + Ix*Xa1 + Iy*Xa2 + Iz*Xa3,
 211    Y0 + Ix*Ya1 + Iy*Ya2 + Iz*Ya3,
 212    Z0 + Ix*Za1 + Iy*Za2 + Iz*Za3);
 213
 214   Standard_Real IR2 = ( Cnf2*Snf2*(Cnf2+1.)- Cnf1*Snf1*(Cnf1+1.) +
 215                        Alpha2-Alpha1 )/3.;
 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.;
 222
 223   math_Matrix Dm(1,3,1,3);
 224   Dm(1,1) = ISn2 +IZ2;
 225   Dm(2,2) = ICn2 +IZ2;
 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;
 230
 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;
 235
 236   math_Jacobi J(Dm);
 237   R = R*R*R*R;
 238   math_Vector V1(1,3), V2(1,3), V3(1,3);
 239   J.Vector(1,V1);
 240   V1.Multiply(Passage,V1);
 241   V1.Multiply(R*J.Value(1));
 242   J.Vector(2,V2);
 243   V2.Multiply(Passage,V2);
 244   V2.Multiply(R*J.Value(2));
 245   J.Vector(3,V3);
 246   V3.Multiply(Passage,V3);
 247   V3.Multiply(R*J.Value(3));
 248
 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)));
 252   gp_Mat Hop;
 253   GProp::HOperator(g,loc,dim,Hop);
 254   inertia = inertia+Hop;
 255
 256
 257 }
 258
 259
 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)
 265 {
 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);
 281
 282
 283   dim = RMax*Rmin*(Teta2-Teta1)*(Alpha2-Alpha1);
 284   Standard_Real Ix =
 285     (Snt2-Snt1)/(Teta2-Teta1)*(Rmin*(Snf2-Snf1)/(Alpha2-Alpha1) + RMax);
 286   Standard_Real Iy =
 287     (Cnt1-Cnt2)/(Teta2-Teta1)*(Rmin*(Snf2-Snf1)/(Alpha2-Alpha1) + RMax);
 288    Standard_Real Iz = Rmin*(Cnf1-Cnf2)/(Alpha2-Alpha1);
 289
 290    g.SetCoord(
 291              X0+Ix*Xa1+Iy*Xa2+Iz*Xa3,
 292              Y0+Ix*Ya1+Iy*Ya2+Iz*Ya3,
 293              Z0+Ix*Za1+Iy*Za2+Iz*Za3);
 294
 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.;
 300   Standard_Real IZ2 =
 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.);
 304
 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;
 312
 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;
 317
 318   math_Jacobi J(Dm);
 319   RMax = RMax*Rmin;
 320   math_Vector V1(1,3), V2(1,3), V3(1,3);
 321   J.Vector(1,V1);
 322   V1.Multiply(Passage,V1);
 323   V1.Multiply(RMax*J.Value(1));
 324   J.Vector(2,V2);
 325   V2.Multiply(Passage,V2);
 326   V2.Multiply(RMax*J.Value(2));
 327   J.Vector(3,V3);
 328   V3.Multiply(Passage,V3);
 329   V3.Multiply(RMax*J.Value(3));
 330
 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)));
 334   gp_Mat Hop;
 335   GProp::HOperator(g,loc,dim,Hop);
 336   inertia = inertia+Hop;
 337
 338
 339 }
 340
 341
 342
 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 )
 349 {
 350   SetLocation(SLocation);
 351   Perform(S,Alpha1,Alpha2,Z1,Z2);
 352 }
 353
 354
 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)
 361  {
 362   SetLocation(SLocation);
 363   Perform(S,Alpha1,Alpha2,Z1,Z2);
 364 }
 365
 366
 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)
 373  {
 374   SetLocation(SLocation);
 375   Perform(S,Teta1,Teta2,Alpha1,Alpha2);
 376 }
 377
 378
 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)
 385  {
 386   SetLocation(SLocation);
 387   Perform(S,Teta1,Teta2,Alpha1,Alpha2);
 388 }
 389
 390
 391