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