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
   7 // under the terms of the GNU Lesser General Public 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