1 // ============================================================================= 2 // PROJECT CHRONO - http://projectchrono.org 3 // 4 // Copyright (c) 2014 projectchrono.org 5 // All rights reserved. 6 // 7 // Use of this source code is governed by a BSD-style license that can be found 8 // in the LICENSE file at the top level of the distribution and at 9 // http://projectchrono.org/license-chrono.txt. 10 // 11 // ============================================================================= 12 // Authors: Alessandro Tasora 13 // ============================================================================= 14 15 16 #include "chrono/fea/ChBeamSectionEuler.h" 17 #include "chrono/core/ChMatrixMBD.h" 18 #include "chrono/core/ChMatrix33.h" 19 20 21 namespace chrono { 22 namespace fea { 23 24 ComputeInertiaDampingMatrix(ChMatrixNM<double,6,6> & Ri,const ChVector<> & mW)25 void ChBeamSectionEuler::ComputeInertiaDampingMatrix(ChMatrixNM<double, 6, 6>& Ri, ///< 6x6 sectional inertial-damping (gyroscopic damping) matrix values here 26 const ChVector<>& mW ///< current angular velocity of section, in material frame 27 ) { 28 double Delta = 1e-8; // magic number, todo: parametrize or #define 29 Ri.setZero(); 30 31 if (compute_inertia_damping_matrix == false) 32 return; 33 34 // Fi=Fia+Fiv, where Fia depends on acceleration only, so restrict to Fiv quadratic terms for numerical differentiation. 35 // Also we assume first three columns of Ri are null because Fiv does not depend on linear velocity. 36 // Quadratic terms (gyro, centrifugal) at current state: 37 ChVectorN<double,6> Fi0; 38 ChVector<> mF, mT; 39 this->ComputeQuadraticTerms(mF, mT, mW); 40 Fi0.segment(0, 3) = mF.eigen(); 41 Fi0.segment(3, 3) = mT.eigen(); 42 // dw_x 43 ChVectorN<double,6> Fi_dw; 44 this->ComputeQuadraticTerms(mF, mT, mW + ChVector<>(Delta,0,0)); 45 Fi_dw.segment(0, 3) = mF.eigen(); 46 Fi_dw.segment(3, 3) = mT.eigen(); 47 Ri.block(0,3, 6,1) = (Fi_dw - Fi0) * (1.0 / Delta); 48 // dw_y 49 this->ComputeQuadraticTerms(mF, mT, mW + ChVector<>(0,Delta,0)); 50 Fi_dw.segment(0, 3) = mF.eigen(); 51 Fi_dw.segment(3, 3) = mT.eigen(); 52 Ri.block(0,4, 6,1) = (Fi_dw - Fi0) * (1.0 / Delta); 53 // dw_z 54 this->ComputeQuadraticTerms(mF, mT, mW + ChVector<>(0,0,Delta)); 55 Fi_dw.segment(0, 3) = mF.eigen(); 56 Fi_dw.segment(3, 3) = mT.eigen(); 57 Ri.block(0,5, 6,1) = (Fi_dw - Fi0) * (1.0 / Delta); 58 } 59 60 ComputeInertiaStiffnessMatrix(ChMatrixNM<double,6,6> & Ki,const ChVector<> & mWvel,const ChVector<> & mWacc,const ChVector<> & mXacc)61 void ChBeamSectionEuler::ComputeInertiaStiffnessMatrix(ChMatrixNM<double, 6, 6>& Ki, ///< 6x6 sectional inertial-stiffness matrix values here 62 const ChVector<>& mWvel, ///< current angular velocity of section, in material frame 63 const ChVector<>& mWacc, ///< current angular acceleration of section, in material frame 64 const ChVector<>& mXacc ///< current acceleration of section, in material frame (not absolute!) 65 ){ 66 double Delta = 1e-8; // magic number, todo: parametrize or #define 67 Ki.setZero(); 68 69 if (compute_inertia_stiffness_matrix == false) 70 return; 71 72 // We assume first three columns of Ki are null because Fi does not depend on displacement. 73 // We compute Ki by numerical differentiation. 74 75 76 ChVector<> mF, mT; 77 this->ComputeInertialForce(mF, mT, mWvel, mWacc, mXacc); 78 ChVectorN<double,6> Fi0; 79 Fi0.segment(0, 3) = mF.eigen(); 80 Fi0.segment(3, 3) = mT.eigen(); 81 82 83 ChVectorN<double,6> Fi_dr; 84 ChVectorN<double, 6> drFi; 85 86 // dr_x 87 ChStarMatrix33<> rot_lx(ChVector<>(Delta, 0, 0)); 88 rot_lx.diagonal().setOnes(); 89 this->ComputeInertialForce(mF, mT, 90 mWvel, // or rot_lx.transpose()*mWvel, if abs. ang.vel is constant during rot.increments 91 mWacc, // or rot_lx.transpose()*mWacc, if abs. ang.vel is constant during rot.increments 92 rot_lx.transpose()*mXacc); 93 Fi_dr.segment(0, 3) = mF.eigen(); 94 Fi_dr.segment(3, 3) = mT.eigen(); 95 drFi.segment(0 , 3) = rot_lx * Fi0.segment(0, 3); 96 drFi.segment(3 , 3) = Fi0.segment(3, 3); 97 Ki.block(0, 3, 6, 1) = (Fi_dr - Fi0) * (1.0 / Delta) +(drFi - Fi0) * (1.0 / Delta); 98 99 // dr_y 100 ChStarMatrix33<> rot_ly(ChVector<>(0, Delta, 0)); 101 rot_ly.diagonal().setOnes(); 102 this->ComputeInertialForce(mF, mT, 103 mWvel, 104 mWacc, 105 rot_ly.transpose()*mXacc); 106 Fi_dr.segment(0, 3) = mF.eigen(); 107 Fi_dr.segment(3, 3) = mT.eigen(); 108 drFi.segment(0 , 3) = rot_ly * Fi0.segment(0, 3); 109 drFi.segment(3 , 3) = Fi0.segment(3, 3); 110 Ki.block(0, 4, 6, 1) = (Fi_dr - Fi0) * (1.0 / Delta) +(drFi - Fi0) * (1.0 / Delta); 111 112 // dr_z 113 ChStarMatrix33<> rot_lz(ChVector<>(0, 0, Delta)); 114 rot_lz.diagonal().setOnes(); 115 this->ComputeInertialForce(mF, mT, 116 mWvel, 117 mWacc, 118 rot_lz.transpose()*mXacc); 119 Fi_dr.segment(0, 3) = mF.eigen(); 120 Fi_dr.segment(3, 3) = mT.eigen(); 121 drFi.segment(0 , 3) = rot_lz * Fi0.segment(0, 3); 122 drFi.segment(3 , 3) = Fi0.segment(3, 3); 123 Ki.block(0, 5, 6, 1) = (Fi_dr - Fi0) * (1.0 / Delta) +(drFi - Fi0) * (1.0 / Delta); 124 } 125 ComputeInertialForce(ChVector<> & mFi,ChVector<> & mTi,const ChVector<> & mWvel,const ChVector<> & mWacc,const ChVector<> & mXacc)126 void ChBeamSectionEuler::ComputeInertialForce( 127 ChVector<>& mFi, ///< total inertial force returned here 128 ChVector<>& mTi, ///< total inertial torque returned here 129 const ChVector<>& mWvel, ///< current angular velocity of section, in material frame 130 const ChVector<>& mWacc, ///< current angular acceleration of section, in material frame 131 const ChVector<>& mXacc ///< current acceleration of section, in material frame (not absolute!) 132 ) { 133 // Default implementation as Fi = [Mi]*{xacc,wacc}+{mF_quadratic,mT_quadratic} 134 // but if possible implement it in children classes with ad-hoc faster formulas. 135 ChMatrixNM<double, 6, 6> Mi; 136 this->ComputeInertiaMatrix(Mi); 137 ChVectorN<double, 6> xpp; 138 xpp.segment(0 , 3) = mXacc.eigen(); 139 xpp.segment(3 , 3) = mWacc.eigen(); 140 ChVectorN<double, 6> Fipp = Mi * xpp; // [Mi]*{xacc,wacc} 141 ChVector<> mF_quadratic; 142 ChVector<> mT_quadratic; 143 this->ComputeQuadraticTerms(mF_quadratic, mT_quadratic, mWvel); // {mF_quadratic,mT_quadratic} 144 mFi = ChVector<>(Fipp.segment(0, 3)) + mF_quadratic; 145 mTi = ChVector<>(Fipp.segment(3, 3)) + mT_quadratic; 146 } 147 148 149 150 151 152 153 ComputeInertiaMatrix(ChMatrixNM<double,6,6> & M)154 void ChBeamSectionEulerSimple::ComputeInertiaMatrix(ChMatrixNM<double, 6, 6>& M) { 155 M.setZero(); 156 M(0, 0) = this->Area * this->density; 157 M(1, 1) = this->Area * this->density; 158 M(2, 2) = this->Area * this->density; 159 160 M(3, 3) = (this->Iyy + this->Izz) * this->density; 161 // M(4, 4) M(5, 5) are zero in Euler theory, as Jzz = 0 Jyy = 0 162 // .. but just make them a tiny nonzero value to avoid singularity, if small JzzJyy_factor 163 M(4, 4) = JzzJyy_factor * M(0, 0); 164 M(5, 5) = JzzJyy_factor * M(0, 0); 165 // .. but Rayleigh beam theory may add it, etc 166 //M(4, 4) = this->Jyy; 167 //M(5, 5) = this->Jzz; 168 //M(4, 5) = -this->Jyz; 169 //M(5, 4) = -this->Jyz; 170 } 171 ComputeInertiaDampingMatrix(ChMatrixNM<double,6,6> & Ri,const ChVector<> & mW)172 void ChBeamSectionEulerSimple::ComputeInertiaDampingMatrix(ChMatrixNM<double, 6, 6>& Ri, ///< 6x6 sectional inertial-damping (gyroscopic damping) matrix values here 173 const ChVector<>& mW ///< current angular velocity of section, in material frame 174 ) { 175 Ri.setZero(); 176 if (compute_inertia_damping_matrix == false) 177 return; 178 if (this->compute_Ri_Ki_by_num_diff) 179 return ChBeamSectionEuler::ComputeInertiaDampingMatrix(Ri, mW); 180 181 ChStarMatrix33<> wtilde(mW); // [w~] 182 // [I] here a diagonal inertia, in Euler it should be Jzz = 0 Jyy = 0, but a tiny nonzero value avoids singularity: 183 ChMatrix33<> mI(ChVector<>( 184 (this->Iyy + this->Izz) * this->density, 185 JzzJyy_factor * this->Area * this->density, 186 JzzJyy_factor * this->Area * this->density)); 187 Ri.block<3, 3>(3, 3) = wtilde * mI - ChStarMatrix33<>(mI * mW); // Ri = [0, 0; 0, [w~][I] - [([I]*w)~] ] 188 } 189 ComputeInertiaStiffnessMatrix(ChMatrixNM<double,6,6> & Ki,const ChVector<> & mWvel,const ChVector<> & mWacc,const ChVector<> & mXacc)190 void ChBeamSectionEulerSimple::ComputeInertiaStiffnessMatrix(ChMatrixNM<double, 6, 6>& Ki, ///< 6x6 sectional inertial-stiffness matrix values here 191 const ChVector<>& mWvel, ///< current angular velocity of section, in material frame 192 const ChVector<>& mWacc, ///< current angular acceleration of section, in material frame 193 const ChVector<>& mXacc ///< current acceleration of section, in material frame 194 ) { 195 Ki.setZero(); 196 if (compute_inertia_stiffness_matrix == false) 197 return; 198 if (this->compute_Ri_Ki_by_num_diff) 199 return ChBeamSectionEuler::ComputeInertiaStiffnessMatrix(Ki, mWvel, mWacc, mXacc); 200 // null [Ki^] (but only for the case where angular speeds and accelerations are assumed to corotate with local frames). 201 } 202 203 ComputeQuadraticTerms(ChVector<> & mF,ChVector<> & mT,const ChVector<> & mW)204 void ChBeamSectionEulerSimple::ComputeQuadraticTerms(ChVector<>& mF, ChVector<>& mT, const ChVector<>& mW) { 205 mF = VNULL; 206 mT = VNULL; 207 } 208 209 ComputeInertiaMatrix(ChMatrixNM<double,6,6> & M)210 void ChBeamSectionEulerAdvancedGeneric::ComputeInertiaMatrix(ChMatrixNM<double, 6, 6>& M) { 211 M.setZero(); 212 M(0, 0) = this->mu; 213 M(1, 1) = this->mu; 214 M(2, 2) = this->mu; 215 216 M(3, 1) = - this->mu * this->Mz; 217 M(3, 2) = this->mu * this->My; 218 M(4, 0) = this->mu * this->Mz; 219 M(5, 0) = - this->mu * this->My; 220 221 M(1, 3) = - this->mu * this->Mz; 222 M(2, 3) = this->mu * this->My; 223 M(0, 4) = this->mu * this->Mz; 224 M(0, 5) = - this->mu * this->My; 225 226 M(3, 3) = this->Jxx; 227 // M(4, 4) M(5, 5) are zero in Euler theory, as Jzz = 0 Jyy = 0 (for no My Mz offsets) 228 // .. but just make them a tiny nonzero value to avoid singularity, if small JzzJyy_factor 229 M(4, 4) = JzzJyy_factor * M(0, 0) + this->mu * this->Mz * this->Mz; 230 M(5, 5) = JzzJyy_factor * M(0, 0) + this->mu * this->My * this->My; 231 M(4, 5) = - this->mu * this->My * this->Mz; 232 M(5, 4) = - this->mu * this->My * this->Mz; 233 // .. but Rayleigh beam theory may add it as: 234 //M(4, 4) = this->Jyy; 235 //M(5, 5) = this->Jzz; 236 //M(4, 5) = -this->Jyz; 237 //M(5, 4) = -this->Jyz; 238 } 239 ComputeInertiaDampingMatrix(ChMatrixNM<double,6,6> & Ri,const ChVector<> & mW)240 void ChBeamSectionEulerAdvancedGeneric::ComputeInertiaDampingMatrix(ChMatrixNM<double, 6, 6>& Ri, ///< 6x6 sectional inertial-damping (gyroscopic damping) matrix values here 241 const ChVector<>& mW ///< current angular velocity of section, in material frame 242 ) { 243 Ri.setZero(); 244 if (compute_inertia_damping_matrix == false) 245 return; 246 if (this->compute_Ri_Ki_by_num_diff) 247 return ChBeamSectionEuler::ComputeInertiaDampingMatrix(Ri, mW); 248 249 ChStarMatrix33<> wtilde(mW); // [w~] 250 ChVector<> mC(0, this->My, this->Mz); 251 ChStarMatrix33<> ctilde(mC); // [c~] 252 ChMatrix33<> mI; 253 mI << this->Jxx , 0, 0, 254 0, JzzJyy_factor * this->mu + this->mu * this->Mz * this->Mz, - this->mu * this->My * this->Mz, 255 0, - this->mu * this->My * this->Mz, JzzJyy_factor * this->mu + this->mu * this->My * this->My; 256 // Ri = [0, m*[w~][c~]' + m*[([w~]*c)~]' ; 0 , [w~][I] - [([I]*w)~] ] 257 Ri.block<3, 3>(0, 3) = this->mu * (wtilde * ctilde.transpose() + (ChStarMatrix33<>( wtilde * mC )).transpose() ); 258 Ri.block<3, 3>(3, 3) = wtilde * mI - ChStarMatrix33<>(mI * mW); 259 } 260 ComputeInertiaStiffnessMatrix(ChMatrixNM<double,6,6> & Ki,const ChVector<> & mWvel,const ChVector<> & mWacc,const ChVector<> & mXacc)261 void ChBeamSectionEulerAdvancedGeneric::ComputeInertiaStiffnessMatrix(ChMatrixNM<double, 6, 6>& Ki, ///< 6x6 sectional inertial-stiffness matrix values here 262 const ChVector<>& mWvel, ///< current angular velocity of section, in material frame 263 const ChVector<>& mWacc, ///< current angular acceleration of section, in material frame 264 const ChVector<>& mXacc ///< current acceleration of section, in material frame 265 ) { 266 Ki.setZero(); 267 if (compute_inertia_stiffness_matrix == false) 268 return; 269 if (this->compute_Ri_Ki_by_num_diff) 270 return ChBeamSectionEuler::ComputeInertiaStiffnessMatrix(Ki, mWvel, mWacc, mXacc); 271 272 ChStarMatrix33<> wtilde(mWvel); // [w~] 273 ChStarMatrix33<> atilde(mWacc); // [a~] 274 ChVector<> mC(0, this->My, this->Mz); 275 ChStarMatrix33<> ctilde(mC); // [c~] 276 ChMatrix33<> mI; 277 mI << this->Jxx , 0, 0, 278 0, JzzJyy_factor * this->mu + this->mu * this->Mz * this->Mz, - this->mu * this->My * this->Mz, 279 0, - this->mu * this->My * this->Mz, JzzJyy_factor * this->mu + this->mu * this->My * this->My; 280 // Ki_al = [0, 0; -m*[([a~]c)~] -m*[([w~][w~]c)~] , m*[c~][xpp~] ] 281 Ki.block<3, 3>(0, 3) = -this->mu * ChStarMatrix33<>(atilde * mC) 282 -this->mu * ChStarMatrix33<>(wtilde*(wtilde*mC)); 283 Ki.block<3, 3>(3, 3) = this->mu * ctilde * ChStarMatrix33<>(mXacc); 284 } 285 ComputeQuadraticTerms(ChVector<> & mF,ChVector<> & mT,const ChVector<> & mW)286 void ChBeamSectionEulerAdvancedGeneric::ComputeQuadraticTerms(ChVector<>& mF, ///< centrifugal term (if any) returned here 287 ChVector<>& mT, ///< gyroscopic term returned here 288 const ChVector<>& mW ///< current angular velocity of section, in material frame 289 ) { 290 // F_centrifugal = density_per_unit_length w X w X c 291 mF = this->mu * Vcross(mW,Vcross(mW,ChVector<>(0,My,Mz))); 292 293 // unroll the product [J] * w in the expression w X [J] * w as 8 values of [J] are zero anyway 294 mT = Vcross(mW, ChVector<>( this->GetInertiaJxxPerUnitLength()*mW.x(), 295 0, 296 0 ) ); 297 } 298 299 ChBeamSectionEulerEasyRectangular(double width_y,double width_z,double myE,double myG,double mydensity)300 ChBeamSectionEulerEasyRectangular::ChBeamSectionEulerEasyRectangular(double width_y, double width_z, double myE, double myG, double mydensity) 301 { 302 this->SetYoungModulus(myE); 303 this->SetGshearModulus(myG); 304 this->SetDensity(mydensity); 305 this->SetAsRectangularSection(width_y, width_z); 306 } 307 ChBeamSectionEulerEasyCircular(double diameter,double myE,double myG,double mydensity)308 ChBeamSectionEulerEasyCircular::ChBeamSectionEulerEasyCircular(double diameter, double myE, double myG, double mydensity) 309 { 310 this->SetYoungModulus(myE); 311 this->SetGshearModulus(myG); 312 this->SetDensity(mydensity); 313 this->SetAsCircularSection(diameter); 314 } 315 316 317 318 ComputeInertiaMatrix(ChMatrixNM<double,6,6> & M)319 void ChBeamSectionRayleighSimple::ComputeInertiaMatrix(ChMatrixNM<double, 6, 6>& M) 320 { 321 // inherit 322 ChBeamSectionEulerSimple::ComputeInertiaMatrix(M); 323 324 // add Rayleigh terms 325 M(4, 4) += this->Iyy * this->density; 326 M(5, 5) += this->Izz * this->density; 327 } 328 ComputeInertiaDampingMatrix(ChMatrixNM<double,6,6> & Ri,const ChVector<> & mW)329 void ChBeamSectionRayleighSimple::ComputeInertiaDampingMatrix(ChMatrixNM<double, 6, 6>& Ri, ///< 6x6 sectional inertial-damping (gyroscopic damping) matrix values here 330 const ChVector<>& mW ///< current angular velocity of section, in material frame 331 ) { 332 Ri.setZero(); 333 if (compute_inertia_damping_matrix == false) 334 return; 335 if (this->compute_Ri_Ki_by_num_diff) 336 return ChBeamSectionEuler::ComputeInertiaDampingMatrix(Ri, mW); 337 338 ChStarMatrix33<> wtilde(mW); // [w~] 339 // [I] here a diagonal inertia, in Euler it should be Jzz = 0 Jyy = 0, but a tiny nonzero value avoids singularity: 340 ChMatrix33<> mI(ChVector<>( 341 (this->Iyy + this->Izz) * this->density, 342 JzzJyy_factor * this->Area * this->density + this->Iyy * this->density, 343 JzzJyy_factor * this->Area * this->density + this->Izz * this->density)); 344 Ri.block<3, 3>(3, 3) = wtilde * mI - ChStarMatrix33<>(mI * mW); // Ri = [0, 0; 0, [w~][I] - [([I]*w)~] ] 345 } 346 ComputeInertiaStiffnessMatrix(ChMatrixNM<double,6,6> & Ki,const ChVector<> & mWvel,const ChVector<> & mWacc,const ChVector<> & mXacc)347 void ChBeamSectionRayleighSimple::ComputeInertiaStiffnessMatrix(ChMatrixNM<double, 6, 6>& Ki, ///< 6x6 sectional inertial-stiffness matrix values here 348 const ChVector<>& mWvel, ///< current angular velocity of section, in material frame 349 const ChVector<>& mWacc, ///< current angular acceleration of section, in material frame 350 const ChVector<>& mXacc ///< current acceleration of section, in material frame 351 ) { 352 Ki.setZero(); 353 if (compute_inertia_stiffness_matrix == false) 354 return; 355 if (this->compute_Ri_Ki_by_num_diff) 356 return ChBeamSectionEuler::ComputeInertiaStiffnessMatrix(Ki, mWvel, mWacc, mXacc); 357 // null [Ki^] (but only for the case where angular speeds and accelerations are assumed to corotate with local frames. 358 } 359 ComputeQuadraticTerms(ChVector<> & mF,ChVector<> & mT,const ChVector<> & mW)360 void ChBeamSectionRayleighSimple::ComputeQuadraticTerms(ChVector<>& mF, ///< centrifugal term (if any) returned here 361 ChVector<>& mT, ///< gyroscopic term returned here 362 const ChVector<>& mW ///< current angular velocity of section, in material frame 363 ) { 364 // F_centrifugal = density_per_unit_length w X w X c 365 mF = VNULL; 366 367 // unroll the product [J] * w in the expression w X [J] * w as 8 values of [J] are zero anyway 368 mT = Vcross(mW, ChVector<>( this->GetInertiaJxxPerUnitLength()*mW.x(), 369 (JzzJyy_factor * this->Area * this->density + this->Iyy * this->density) * mW.y(), 370 (JzzJyy_factor * this->Area * this->density + this->Izz * this->density) * mW.z()) ); 371 } 372 ChBeamSectionRayleighEasyRectangular(double mwidth_y,double mwidth_z,double mE,double mG,double mdensity)373 ChBeamSectionRayleighEasyRectangular::ChBeamSectionRayleighEasyRectangular(double mwidth_y, double mwidth_z, double mE, double mG, double mdensity) 374 { 375 this->SetYoungModulus(mE); 376 this->SetGshearModulus(mG); 377 this->SetDensity(mdensity); 378 this->SetAsRectangularSection(mwidth_y,mwidth_z); 379 } 380 ChBeamSectionRayleighEasyCircular(double diameter,double mE,double mG,double mdensity)381 ChBeamSectionRayleighEasyCircular::ChBeamSectionRayleighEasyCircular(double diameter, double mE, double mG, double mdensity) 382 { 383 this->SetYoungModulus(mE); 384 this->SetGshearModulus(mG); 385 this->SetDensity(mdensity); 386 this->SetAsCircularSection(diameter); 387 } 388 389 390 391 392 393 394 395 SetInertiasPerUnitLength(const double mJyy,const double mJzz,const double mJyz)396 void ChBeamSectionRayleighAdvancedGeneric::SetInertiasPerUnitLength(const double mJyy, const double mJzz, const double mJyz) { 397 this->Jyy = mJyy; 398 this->Jzz = mJzz; 399 this->Jyz = mJyz; 400 // automatically set parent Jxx value 401 this->Jxx = (this->Jyy + this->Jzz); 402 } 403 404 SetMainInertiasInMassReference(double Jmyy,double Jmzz,double phi)405 void ChBeamSectionRayleighAdvancedGeneric::SetMainInertiasInMassReference(double Jmyy, double Jmzz, double phi) { 406 double cc = pow(cos(-phi), 2); 407 double ss = pow(sin(-phi), 2); 408 double cs = cos(-phi) * sin(-phi); 409 // generic 2x2 tensor rotation 410 double Tyy_rot = cc * Jmyy + ss * Jmzz; // + 2 * Jmyz * cs; //TODO: it seems the commented term has an opposite sign 411 double Tzz_rot = ss * Jmyy + cc * Jmzz; // - 2 * Jmyz * cs; //TODO: it seems the commented term has an opposite sign 412 double Tyz_rot = (Jmzz - Jmyy) * cs; // +Jmyz * (cc - ss); //TODO: it seems the commented term has an opposite sign 413 // add inertia transport 414 this->Jyy = Tyy_rot + this->mu * this->Mz * this->Mz; 415 this->Jzz = Tzz_rot + this->mu * this->My * this->My; 416 this->Jyz = -(Tyz_rot - this->mu * this->Mz * this->My); // note minus, per definition of Jyz 417 // automatically set parent Jxx value 418 this->Jxx = (this->Jyy + this->Jzz); 419 } 420 GetMainInertiasInMassReference(double & Jmyy,double & Jmzz,double & phi)421 void ChBeamSectionRayleighAdvancedGeneric::GetMainInertiasInMassReference(double& Jmyy, double& Jmzz, double& phi) { 422 // remove inertia transport 423 double Tyy_rot = this->Jyy - this->mu * this->Mz * this->Mz; 424 double Tzz_rot = this->Jzz - this->mu * this->My * this->My; 425 double Tyz_rot = -this->Jyz + this->mu * this->Mz * this->My; 426 // tensor de-rotation up to principal axes 427 double argum = pow((Tyy_rot - Tzz_rot) * 0.5, 2) + pow(Tyz_rot, 2); 428 if (argum <= 0) { 429 phi = 0; 430 Jmyy = 0.5 * (Tzz_rot + Tyy_rot); 431 Jmzz = 0.5 * (Tzz_rot + Tyy_rot); 432 return; 433 } 434 double discr = sqrt( pow((Tyy_rot - Tzz_rot)*0.5,2) + pow(Tyz_rot, 2) ); 435 phi = - 0.5* atan2(Tyz_rot / discr, (Tzz_rot - Tyy_rot) / (2. * discr)); 436 Jmyy = 0.5 * (Tzz_rot + Tyy_rot) - discr; 437 Jmzz = 0.5 * (Tzz_rot + Tyy_rot) + discr; 438 } 439 440 ComputeInertiaMatrix(ChMatrixNM<double,6,6> & M)441 void ChBeamSectionRayleighAdvancedGeneric::ComputeInertiaMatrix(ChMatrixNM<double, 6, 6>& M) 442 { 443 // inherit 444 ChBeamSectionEulerAdvancedGeneric::ComputeInertiaMatrix(M); 445 446 // overwrite rotational part using Rayleigh terms, similarly to the Cosserat beam. 447 //Also add the JzzJyy_factor*M(0,0) term as in Euler beams - a term that avoids zero on diagonal and that can be turned off. 448 M(3, 3) = this->Jyy+this->Jzz; 449 M(4, 4) = this->Jyy + JzzJyy_factor * M(0, 0); 450 M(5, 5) = this->Jzz + JzzJyy_factor * M(0, 0); 451 M(4, 5) = -this->Jyz; 452 M(5, 4) = -this->Jyz; 453 } 454 ComputeInertiaDampingMatrix(ChMatrixNM<double,6,6> & Ri,const ChVector<> & mW)455 void ChBeamSectionRayleighAdvancedGeneric::ComputeInertiaDampingMatrix(ChMatrixNM<double, 6, 6>& Ri, ///< 6x6 sectional inertial-damping (gyroscopic damping) matrix values here 456 const ChVector<>& mW ///< current angular velocity of section, in material frame 457 ) { 458 Ri.setZero(); 459 if (compute_inertia_damping_matrix == false) 460 return; 461 if (this->compute_Ri_Ki_by_num_diff) 462 return ChBeamSectionEuler::ComputeInertiaDampingMatrix(Ri, mW); 463 464 ChStarMatrix33<> wtilde(mW); // [w~] 465 ChVector<> mC(0, this->My, this->Mz); 466 ChStarMatrix33<> ctilde(mC); // [c~] 467 ChMatrix33<> mI; 468 mI << this->Jyy+this->Jzz , 0, 0, 469 0, this->Jyy + JzzJyy_factor * this->mu, -this->Jyz, 470 0, -this->Jyz, this->Jzz + JzzJyy_factor * this->mu; 471 // Ri = [0, m*[w~][c~]' + m*[([w~]*c)~]' ; 0 , [w~][I] - [([I]*w)~] ] 472 Ri.block<3, 3>(0, 3) = this->mu * (wtilde * ctilde.transpose() + (ChStarMatrix33<>( wtilde * mC )).transpose() ); 473 Ri.block<3, 3>(3, 3) = wtilde * mI - ChStarMatrix33<>(mI * mW); 474 } 475 ComputeInertiaStiffnessMatrix(ChMatrixNM<double,6,6> & Ki,const ChVector<> & mWvel,const ChVector<> & mWacc,const ChVector<> & mXacc)476 void ChBeamSectionRayleighAdvancedGeneric::ComputeInertiaStiffnessMatrix(ChMatrixNM<double, 6, 6>& Ki, ///< 6x6 sectional inertial-stiffness matrix values here 477 const ChVector<>& mWvel, ///< current angular velocity of section, in material frame 478 const ChVector<>& mWacc, ///< current angular acceleration of section, in material frame 479 const ChVector<>& mXacc ///< current acceleration of section, in material frame 480 ) { 481 Ki.setZero(); 482 if (compute_inertia_stiffness_matrix == false) 483 return; 484 if (this->compute_Ri_Ki_by_num_diff) 485 return ChBeamSectionEuler::ComputeInertiaStiffnessMatrix(Ki, mWvel, mWacc, mXacc); 486 487 ChStarMatrix33<> wtilde(mWvel); // [w~] 488 ChStarMatrix33<> atilde(mWacc); // [a~] 489 ChVector<> mC(0, this->My, this->Mz); 490 ChStarMatrix33<> ctilde(mC); // [c~] 491 ChMatrix33<> mI; 492 mI << this->Jyy+this->Jzz , 0, 0, 493 0, this->Jyy + JzzJyy_factor * this->mu, -this->Jyz, 494 0, -this->Jyz, this->Jzz + JzzJyy_factor * this->mu; 495 // Ki_al = [0, 0; -m*[([a~]c)~] -m*[([w~][w~]c)~] , m*[c~][xpp~] ] 496 Ki.block<3, 3>(0, 3) = -this->mu * ChStarMatrix33<>(atilde * mC) 497 -this->mu * ChStarMatrix33<>(wtilde*(wtilde*mC)); 498 Ki.block<3, 3>(3, 3) = this->mu * ctilde * ChStarMatrix33<>(mXacc); 499 } 500 ComputeQuadraticTerms(ChVector<> & mF,ChVector<> & mT,const ChVector<> & mW)501 void ChBeamSectionRayleighAdvancedGeneric::ComputeQuadraticTerms(ChVector<>& mF, ///< centrifugal term (if any) returned here 502 ChVector<>& mT, ///< gyroscopic term returned here 503 const ChVector<>& mW ///< current angular velocity of section, in material frame 504 ) { 505 // F_centrifugal = density_per_unit_length w X w X c 506 mF = this->mu * Vcross(mW,Vcross(mW,ChVector<>(0,this->My,this->Mz))); 507 508 // unroll the product [J] * w in the expression w X [J] * w as 4 values of [J] are zero anyway 509 mT = Vcross(mW, ChVector<>( this->GetInertiaJxxPerUnitLength()*mW.x(), 510 (this->Jyy + JzzJyy_factor * this->mu)*mW.y() - this->Jyz*mW.z(), 511 (this->Jzz + JzzJyy_factor * this->mu)*mW.z() - this->Jyz*mW.y() ) ); 512 } 513 514 515 516 } // end namespace fea 517 } // end namespace chrono 518 519