GSaha567/seq_level_training_data
052
1text,length,is_long_context,metric_val,label_metric2"/*This file is part of the FEBio source code and is licensed under the MIT license3listed below.4 5See Copyright-FEBio.txt for details.6 7Copyright (c) 2019 University of Utah, The Trustees of Columbia University in 8the City of New York, and others.9 10Permission is hereby granted, free of charge, to any person obtaining a copy11of this software and associated documentation files (the ""Software""), to deal12in the Software without restriction, including without limitation the rights13to use, copy, modify, merge, publish, distribute, sublicense, and/or sell14copies of the Software, and to permit persons to whom the Software is15furnished to do so, subject to the following conditions:16 17The above copyright notice and this permission notice shall be included in all18copies or substantial portions of the Software.19 20THE SOFTWARE IS PROVIDED ""AS IS"", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR21IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,22FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE23AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER24LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,25OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE26SOFTWARE.*/27 28 29#include ""stdafx.h""30#include ""FERigidSolver.h""31#include ""FESolidSolver2.h""32#include ""FERigidMaterial.h""33#include <FECore/FERigidSystem.h>34#include <FECore/FERigidBody.h>35#include <FECore/FEModel.h>36#include <FECore/RigidBC.h>37#include <FECore/FEAnalysis.h>38#include <FECore/SparseMatrix.h>39#include <FECore/log.h>40#include <FECore/FEMaterial.h>41#include <FECore/Archive.h>42 43FERigidSolver::FERigidSolver(FEModel* fem) : m_fem(fem)44{45 m_dofX = m_dofY = m_dofZ = -1;46 47 m_bAllowMixedBCs = false;48}49 50int FERigidSolver::InitEquations(int neq)51{52 // Next, we assign equation numbers to the rigid body degrees of freedom53 FERigidSystem& rigid = *m_fem->GetRigidSystem();54 int nrb = rigid.Objects();55 for (int i = 0; i<nrb; ++i)56 {57 FERigidBody& RB = *rigid.Object(i);58 for (int j = 0; j<6; ++j)59 {60 int bcj = RB.m_BC[j];61 int lmj = RB.m_LM[j];62 if (bcj == DOF_OPEN ) { RB.m_LM[j] = neq; neq++; }63 else if (bcj == DOF_PRESCRIBED) { RB.m_LM[j] = -neq - 2; neq++; }64 else if (bcj == DOF_FIXED ) { RB.m_LM[j] = -1; }65 else { assert(false); return -1; }66 }67 }68 69 // get the DOF indices70 m_dofX = m_fem->GetDOFIndex(""x"");71 m_dofY = m_fem->GetDOFIndex(""y"");72 m_dofZ = m_fem->GetDOFIndex(""z"");73 m_dofVX = m_fem->GetDOFIndex(""vx"");74 m_dofVY = m_fem->GetDOFIndex(""vy"");75 m_dofVZ = m_fem->GetDOFIndex(""vz"");76 m_dofU = m_fem->GetDOFIndex(""u"");77 m_dofV = m_fem->GetDOFIndex(""v"");78 m_dofW = m_fem->GetDOFIndex(""w"");79 m_dofSX = m_fem->GetDOFIndex(""sx"");80 m_dofSY = m_fem->GetDOFIndex(""sy"");81 m_dofSZ = m_fem->GetDOFIndex(""sz"");82 m_dofSVX = m_fem->GetDOFIndex(""svx"");83 m_dofSVY = m_fem->GetDOFIndex(""svy"");84 m_dofSVZ = m_fem->GetDOFIndex(""svz"");85 int dofRU = m_fem->GetDOFIndex(""Ru"");86 int dofRV = m_fem->GetDOFIndex(""Rv"");87 int dofRW = m_fem->GetDOFIndex(""Rw"");88 89 // we assign the rigid body equation number to90 // Also make sure that the nodes are NOT constrained!91 FEMesh& mesh = m_fem->GetMesh();92 for (int i = 0; i<mesh.Nodes(); ++i)93 {94 FENode& node = mesh.Node(i);95 if (node.m_rid >= 0)96 {97 FERigidBody& RB = *rigid.Object(node.m_rid);98 node.m_ID[m_dofX] = (RB.m_LM[0] >= 0 ? -RB.m_LM[0] - 2 : RB.m_LM[0]);99 node.m_ID[m_dofY] = (RB.m_LM[1] >= 0 ? -RB.m_LM[1] - 2 : RB.m_LM[1]);100 node.m_ID[m_dofZ] = (RB.m_LM[2] >= 0 ? -RB.m_LM[2] - 2 : RB.m_LM[2]);101 node.m_ID[dofRU] = (RB.m_LM[3] >= 0 ? -RB.m_LM[3] - 2 : RB.m_LM[3]);102 node.m_ID[dofRV] = (RB.m_LM[4] >= 0 ? -RB.m_LM[4] - 2 : RB.m_LM[4]);103 node.m_ID[dofRW] = (RB.m_LM[5] >= 0 ? -RB.m_LM[5] - 2 : RB.m_LM[5]);104 if (node.HasFlags(FENode::SHELL) && node.HasFlags(FENode::RIGID_CLAMP)) {105 node.m_ID[m_dofU] = (RB.m_LM[0] >= 0 ? -RB.m_LM[0] - 2 : RB.m_LM[0]);106 node.m_ID[m_dofV] = (RB.m_LM[1] >= 0 ? -RB.m_LM[1] - 2 : RB.m_LM[1]);107 node.m_ID[m_dofW] = (RB.m_LM[2] >= 0 ? -RB.m_LM[2] - 2 : RB.m_LM[2]);108 node.m_ID[m_dofSX] = (RB.m_LM[0] >= 0 ? -RB.m_LM[0] - 2 : RB.m_LM[0]);109 node.m_ID[m_dofSY] = (RB.m_LM[1] >= 0 ? -RB.m_LM[1] - 2 : RB.m_LM[1]);110 node.m_ID[m_dofSZ] = (RB.m_LM[2] >= 0 ? -RB.m_LM[2] - 2 : RB.m_LM[2]);111 }112 }113 }114 115 return neq;116}117 118//-----------------------------------------------------------------------------119//! Serialization120void FERigidSolver::Serialize(DumpStream& ar)121{122 if (ar.IsShallow()) return;123 124 if (ar.IsSaving())125 {126 ar << m_dofX << m_dofY << m_dofZ;127 ar << m_dofVX << m_dofVY << m_dofVZ;128 ar << m_dofU << m_dofV << m_dofW;129 ar << m_dofSX << m_dofSY << m_dofSZ;130 ar << m_dofSVX << m_dofSVY << m_dofSVZ;131 ar << m_bAllowMixedBCs;132 }133 else134 {135 ar >> m_dofX >> m_dofY >> m_dofZ;136 ar >> m_dofVX >> m_dofVY >> m_dofVZ;137 ar >> m_dofU >> m_dofV >> m_dofW;138 ar >> m_dofSX >> m_dofSY >> m_dofSZ;139 ar >> m_dofSVX >> m_dofSVY >> m_dofSVZ;140 ar >> m_bAllowMixedBCs;141 }142}143 144//-----------------------------------------------------------------------------145// \\todo: eliminate need for ui parameter146void FERigidSolver::PrepStep(const FETimeInfo& timeInfo, vector<double>& ui)147{148 FERigidSystem& rigid = *m_fem->GetRigidSystem();149 int NO = rigid.Objects();150 for (int i = 0; i<NO; ++i) rigid.Object(i)->Init();151 152 // calculate local rigid displacements153 for (int i = 0; i<rigid.PrescribedBCs(); ++i)154 {155 FERigidBodyDisplacement& DC = *rigid.PrescribedBC(i);156 FERigidBody& RB = *rigid.Object(DC.id);157 if (DC.IsActive())158 {159 int I = DC.bc;160 int lc = DC.lc;161 if (lc >= 0)162 {163 RB.m_dul[I] = DC.Value() - RB.m_Ut[DC.bc];164 }165 }166 }167 168 // calculate global rigid displacements169 for (int i = 0; i<NO; ++i)170 {171 FERigidBody* prb = rigid.Object(i);172 if (prb)173 {174 FERigidBody& RB = *prb;175 if (RB.m_prb == 0)176 {177 // if all rotation dofs are fixed or prescribed, set the flag178 if (m_bAllowMixedBCs==false)179 {180 if (RB.m_pDC[3] || RB.m_pDC[4] || RB.m_pDC[5])181 {182 bool bpofr[3] = { false };183 for (int j = 3; j<6; ++j) if (RB.m_pDC[j] || (RB.m_LM[j] < 0)) bpofr[j - 3] = true;184 if (bpofr[0] && bpofr[1] && bpofr[2]) RB.m_bpofr = true;185 else186 {187 felog.printf(""FATAL ERROR: Rigid body rotations cannot mix prescribed and free components.\\n"");188 felog.printf(""Rigid body: %d, Material: %d\\n"", RB.m_nID, RB.GetMaterialID());189 throw ""FATAL ERROR"";190 }191 }192 }193 194 for (int j = 0; j<6; ++j) RB.m_du[j] = RB.m_dul[j];195 }196 else197 {198 double* dul = RB.m_dul;199 vec3d dr = vec3d(dul[0], dul[1], dul[2]);200 201 vec3d v = vec3d(dul[3], dul[4], dul[5]);202 double w = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);203 quatd dq = quatd(w, v);204 205 FERigidBody* pprb = RB.m_prb;206 207 vec3d r0 = RB.m_rt;208 quatd Q0 = RB.GetRotation();209 210 dr = Q0*dr;211 dq = Q0*dq*Q0.Inverse();212 213 while (pprb)214 {215 vec3d r1 = pprb->m_rt;216 dul = pprb->m_dul;217 218 quatd Q1 = pprb->GetRotation();219 220 dr = r0 + dr - r1;221 222 // grab the parent's local displacements223 vec3d dR = vec3d(dul[0], dul[1], dul[2]);224 v = vec3d(dul[3], dul[4], dul[5]);225 w = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);226 quatd dQ = quatd(w, v);227 228 dQ = Q1*dQ*Q1.Inverse();229 230 // update global displacements231 quatd Qi = Q1.Inverse();232 dr = dR + r1 + dQ*dr - r0;233 dq = dQ*dq;234 235 // move up in the chain236 pprb = pprb->m_prb;237 Q0 = Q1;238 }239 240 // set global displacements241 double* du = RB.m_du;242 243 du[0] = dr.x;244 du[1] = dr.y;245 du[2] = dr.z;246 247 v = dq.GetVector();248 w = dq.GetAngle();249 du[3] = w*v.x;250 du[4] = w*v.y;251 du[5] = w*v.z;252 }253 }254 }255 256 // store rigid displacements in Ui vector257 for (int i = 0; i<NO; ++i)258 {259 FERigidBody& RB = *rigid.Object(i);260 for (int j = 0; j<6; ++j)261 {262 int I = -RB.m_LM[j] - 2;263 if (I >= 0) ui[I] = RB.m_du[j];264 }265 }266 267 FEAnalysis* pstep = m_fem->GetCurrentStep();268 if (pstep->m_nanalysis == FE_DYNAMIC)269 {270 FEMesh& mesh = m_fem->GetMesh();271 272 // set the initial velocities of all rigid nodes273 for (int i = 0; i<mesh.Nodes(); ++i)274 {275 FENode& n = mesh.Node(i);276 if (n.m_rid >= 0)277 {278 FERigidBody& rb = *rigid.Object(n.m_rid);279 vec3d V = rb.m_vt;280 vec3d W = rb.m_wt;281 vec3d r = n.m_rt - rb.m_rt;282 283 vec3d v = V + (W ^ r);284 n.m_vp = v;285 n.set_vec3d(m_dofVX, m_dofVY, m_dofVZ, v);286 287 vec3d a = (W ^ V)*2.0 + (W ^ (W ^ r));288 n.m_ap = n.m_at = a;289 }290 }291 }292 293 // store the current rigid body reaction forces294 for (int i = 0; i<rigid.Objects(); ++i)295 {296 FERigidBody& RB = *rigid.Object(i);297 RB.m_Fp = RB.m_Fr;298 RB.m_Mp = RB.m_Mr;299 }300}301 302//-----------------------------------------------------------------------------303//! This function calculates the rigid stiffness matrices304void FERigidSolver::RigidStiffness(SparseMatrix& K, vector<double>& ui, vector<double>& F, vector<int>& en, vector<int>& elm, matrix& ke, double alpha)305{306 int n = (int)en.size();307 FEMesh& mesh = m_fem->GetMesh();308 309 bool bclamped_shell = false;310 for (int j = 0; j<n; ++j) {311 if (mesh.Node(en[j]).HasFlags(FENode::SHELL) && mesh.Node(en[j]).HasFlags(FENode::RIGID_CLAMP)) {312 bclamped_shell = true;313 break;314 }315 }316 if (bclamped_shell)317 RigidStiffnessShell(K, ui, F, en, elm, ke, alpha);318 else319 RigidStiffnessSolid(K, ui, F, en, elm, ke, alpha);320 return;321}322 323//-----------------------------------------------------------------------------324//! This function calculates the rigid stiffness matrices325//! correct stiffness matrix for rigid-solid interfaces326void FERigidSolver::RigidStiffnessSolid(SparseMatrix& K, vector<double>& ui, vector<double>& F, vector<int>& en, vector<int>& elm, matrix& ke, double alpha)327{328 FERigidSystem& rigid = *m_fem->GetRigidSystem();329 if (rigid.Objects() == 0) return;330 331 int i, j, k, l, n = (int)en.size();332 333 // get nodal DOFS334 DOFS& fedofs = m_fem->GetDOFS();335 int MAX_NDOFS = fedofs.GetTotalDOFS();336 337 int ndof = ke.columns() / n;338 339 matrix kij(ndof, ndof);340 matrix KF(ndof, 6);341 342 double KR[6][6];343 344 int *lmi, *lmj;345 int I, J;346 347 vec3d zi, zj;348 mat3d Zi, Zj;349 350 351 FEMesh& mesh = m_fem->GetMesh();352 353 // loop over columns354 for (j = 0; j<n; ++j)355 {356 FENode& nodej = mesh.Node(en[j]);357 if (nodej.m_rid >= 0)358 {359 // this is a rigid interface node360 // get the rigid body this node is attached to361 FERigidBody& RBj = *rigid.Object(nodej.m_rid);362 363 // get the rigid body equation nrs.364 lmj = RBj.m_LM;365 366 // get the relative distance to the center of mass367 zj = nodej.m_rt - RBj.m_rt;368 Zj.skew(zj);369 370 // loop over rows371 for (i = 0; i<n; ++i)372 {373 // get the element sub-matrix374 for (k = 0; k<ndof; ++k)375 for (l = 0; l<ndof; ++l)376 kij[k][l] = ke[ndof*i + k][ndof*j + l];377 378 mat3d Kuu(kij[0][0], kij[0][1], kij[0][2],379 kij[1][0], kij[1][1], kij[1][2],380 kij[2][0], kij[2][1], kij[2][2]);381 382 FENode& nodei = mesh.Node(en[i]);383 384 if (nodei.m_rid >= 0)385 {386 // node i is also a rigid body node387 // get the rigid body this node is attached to388 FERigidBody& RBi = *rigid.Object(nodei.m_rid);389 390 lmi = RBi.m_LM;391 392 // get the relative distance (use alpha rule)393 zi = (nodei.m_rt - RBi.m_rt)*alpha + (nodei.m_rp - RBi.m_rp)*(1 - alpha);394 Zi.skew(zi);395 396 mat3d M;397 398 // Kuu transformation to Krr399 M = Kuu*alpha;400 KR[0][0] = M[0][0]; KR[0][1] = M[0][1]; KR[0][2] = M[0][2];401 KR[1][0] = M[1][0]; KR[1][1] = M[1][1]; KR[1][2] = M[1][2];402 KR[2][0] = M[2][0]; KR[2][1] = M[2][1]; KR[2][2] = M[2][2];403 404 405 // Kuu transformation to Krq406 M = Kuu*Zj*(-alpha);407 KR[0][3] = M[0][0]; KR[0][4] = M[0][1]; KR[0][5] = M[0][2];408 KR[1][3] = M[1][0]; KR[1][4] = M[1][1]; KR[1][5] = M[1][2];409 KR[2][3] = M[2][0]; KR[2][4] = M[2][1]; KR[2][5] = M[2][2];410 411 412 // Kuu transformation to Kqr413 M = Zi*Kuu*alpha;414 KR[3][0] = M[0][0]; KR[3][1] = M[0][1]; KR[3][2] = M[0][2];415 KR[4][0] = M[1][0]; KR[4][1] = M[1][1]; KR[4][2] = M[1][2];416 KR[5][0] = M[2][0]; KR[5][1] = M[2][1]; KR[5][2] = M[2][2];417 418 419 // Kuu transformation to Kqq420 M = Zi*Kuu*Zj*(-alpha);421 KR[3][3] = M[0][0]; KR[3][4] = M[0][1]; KR[3][5] = M[0][2];422 KR[4][3] = M[1][0]; KR[4][4] = M[1][1]; KR[4][5] = M[1][2];423 KR[5][3] = M[2][0]; KR[5][4] = M[2][1]; KR[5][5] = M[2][2];424 425 // add the stiffness components to the Krr matrix426 for (k = 0; k<6; ++k)427 for (l = 0; l<6; ++l)428 {429 J = lmj[k];430 I = lmi[l];431 432 if (I >= 0)433 {434 // multiply KR by alpha for alpha rule435 if (J < -1) F[I] -= KR[l][k]*ui[-J - 2];436 else if (J >= 0) K.add(I, J, KR[l][k]);437 }438 }439 440 // we still need to couple the non-rigid degrees of node i to the441 // rigid dofs of node j442 for (k = 3; k<ndof; ++k) {443 vec3d kpu(kij[k][0], kij[k][1], kij[k][2]);444 vec3d m = kpu*alpha;445 KF[k][0] = m.x; KF[k][1] = m.y; KF[k][2] = m.z;446 m = Zj*kpu*alpha;447 KF[k][3] = m.x; KF[k][4] = m.y; KF[k][5] = m.z;448 }449 450 for (k = 0; k<6; ++k)451 for (l = 3; l<ndof; ++l)452 {453 J = lmj[k];454 I = elm[ndof*i + l];455 456 if (I >= 0)457 {458 // multiply KF by alpha for alpha rule459 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];460 else if (J >= 0) K.add(I, J, KF[l][k]);461 }462 }463 464 // now the transpose location465 for (l = 3; l<ndof; ++l) {466 vec3d kup(kij[0][l], kij[1][l], kij[2][l]);467 vec3d m = Zi*kup;468 KF[l][0] = kup.x; KF[l][1] = kup.y; KF[l][2] = kup.z;469 KF[l][3] = m.x; KF[l][4] = m.y; KF[l][5] = m.z;470 }471 472 for (k = 0; k<6; ++k)473 for (l = 3; l<ndof; ++l)474 {475 J = elm[ndof*j + l];476 I = lmi[k];477 478 if (I >= 0)479 {480 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];481 else if (J >= 0) K.add(I, J, KF[l][k]);482 }483 }484 485 }486 else487 {488 // node i is not a rigid body node489 // add the stiffness components to the Kfr matrix490 491 // Kij492 for (k = 0; k<ndof; ++k) {493 vec3d kpu(kij[k][0], kij[k][1], kij[k][2]);494 vec3d m = kpu*alpha;495 KF[k][0] = m.x; KF[k][1] = m.y; KF[k][2] = m.z;496 m = Zj*kpu*alpha;497 KF[k][3] = m.x; KF[k][4] = m.y; KF[k][5] = m.z;498 }499 500 for (k = 0; k<6; ++k)501 for (l = 0; l<ndof; ++l)502 {503 J = lmj[k];504 I = elm[ndof*i + l];505 506 if (I >= 0)507 {508 // multiply KF by alpha for alpha rule509 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];510 else if (J >= 0) K.add(I, J, KF[l][k]);511 }512 }513 }514 }515 }516 else517 {518 // loop over rows519 for (i = 0; i<n; ++i)520 {521 FENode& nodei = mesh.Node(en[i]);522 if (nodei.m_rid >= 0)523 {524 // node i is a rigid body525 // get the rigid body this node is attached to526 FERigidBody& RBi = *rigid.Object(nodei.m_rid);527 528 // get the rigid body equation nrs.529 lmi = RBi.m_LM;530 531 // get the relative distance (use alpha rule)532 zi = (nodei.m_rt - RBi.m_rt)*alpha + (nodei.m_rp - RBi.m_rp)*(1 - alpha);533 Zi.skew(zi);534 535 // get the element sub-matrix536 for (k = 0; k<ndof; ++k)537 for (l = 0; l<ndof; ++l)538 kij[k][l] = ke[ndof*i + k][ndof*j + l];539 540 // add the stiffness components to the Krf matrix541 542 // Kij543 for (k = 0; k<ndof; ++k) {544 vec3d kup(kij[0][k], kij[1][k], kij[2][k]);545 vec3d m = Zi*kup;546 KF[k][0] = kup.x; KF[k][1] = kup.y; KF[k][2] = kup.z;547 KF[k][3] = m.x; KF[k][4] = m.y; KF[k][5] = m.z;548 }549 550 for (k = 0; k<6; ++k)551 for (l = 0; l<ndof; ++l)552 {553 I = lmi[k];554 J = elm[ndof*j + l];555 556 if (I >= 0)557 {558 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];559 else if (J >= 0) K.add(I, J, KF[l][k]);560 }561 }562 }563 }564 }565 }566}567 568//-----------------------------------------------------------------------------569//! This function calculates the rigid stiffness matrices570//! correct stiffness matrix for rigid bodies accounting for rigid-body-deformable-shell interfaces571void FERigidSolver::RigidStiffnessShell(SparseMatrix& K, vector<double>& ui, vector<double>& F, vector<int>& en, vector<int>& elm, matrix& ke, double alpha)572{573 FERigidSystem& rigid = *m_fem->GetRigidSystem();574 if (rigid.Objects() == 0) return;575 576 int i, j, k, l, n = (int)en.size();577 578 // get nodal DOFS579 DOFS& fedofs = m_fem->GetDOFS();580 int MAX_NDOFS = fedofs.GetTotalDOFS();581 582 vector< vector<double> > kij; kij.assign(MAX_NDOFS, vector<double>(MAX_NDOFS));583 584 vector< vector<double> > KF; KF.assign(MAX_NDOFS, vector<double>(6));585 double KR[6][6];586 587 int *lmi, *lmj;588 int I, J;589 590 vec3d ai, aj, bi, bj;591 mat3d Ai, Aj, Bi, Bj;592 593 int ndof = ke.columns() / n;594 595 FEMesh& mesh = m_fem->GetMesh();596 597 // loop over columns598 for (j = 0; j<n; ++j)599 {600 FENode& nodej = mesh.Node(en[j]);601 if (nodej.m_rid >= 0)602 {603 // this is a rigid interface node604 // get the rigid body this node is attached to605 FERigidBody& RBj = *rigid.Object(nodej.m_rid);606 607 // get the rigid body equation nrs.608 lmj = RBj.m_LM;609 610 // get the relative distance to the center of mass611 aj = nodej.m_rt - RBj.m_rt;612 Aj.skew(aj);613 614 // get the shell director615 bj = aj - (nodej.m_d0 + nodej.get_vec3d(m_dofX, m_dofY, m_dofZ) - nodej.get_vec3d(m_dofSX, m_dofSY, m_dofSZ));616 Bj.skew(bj);617 618 // loop over rows619 for (i = 0; i<n; ++i)620 {621 // get the element sub-matrix622 for (k = 0; k<ndof; ++k)623 for (l = 0; l<ndof; ++l)624 kij[k][l] = ke[ndof*i + k][ndof*j + l];625 626 mat3d Kuu(kij[0][0], kij[0][1], kij[0][2],627 kij[1][0], kij[1][1], kij[1][2],628 kij[2][0], kij[2][1], kij[2][2]);629 630 mat3d Kuw(kij[0][3], kij[0][4], kij[0][5],631 kij[1][3], kij[1][4], kij[1][5],632 kij[2][3], kij[2][4], kij[2][5]);633 634 mat3d Kwu(kij[3][0], kij[3][1], kij[3][2],635 kij[4][0], kij[4][1], kij[4][2],636 kij[5][0], kij[5][1], kij[5][2]);637 638 mat3d Kww(kij[3][3], kij[3][4], kij[3][5],639 kij[4][3], kij[4][4], kij[4][5],640 kij[5][3], kij[5][4], kij[5][5]);641 642 FENode& nodei = mesh.Node(en[i]);643 644 if (nodei.m_rid >= 0)645 {646 // node i is also a rigid body node647 // get the rigid body this node is attached to648 FERigidBody& RBi = *rigid.Object(nodei.m_rid);649 650 lmi = RBi.m_LM;651 652 // get the relative distance (use alpha rule)653 ai = (nodei.m_rt - RBi.m_rt)*alpha + (nodei.m_rp - RBi.m_rp)*(1 - alpha);654 Ai.skew(ai);655 656 // get the shell director657 bi = ai - (nodei.m_d0 + nodei.get_vec3d(m_dofX, m_dofY, m_dofZ) - nodei.get_vec3d(m_dofSX, m_dofSY, m_dofSZ));658 Bi.skew(bi);659 660 mat3d M;661 662 // Kuu transformation663 M = (Kuu + Kwu + Kuw + Kww)*alpha;664 KR[0][0] = M[0][0]; KR[0][1] = M[0][1]; KR[0][2] = M[0][2];665 KR[1][0] = M[1][0]; KR[1][1] = M[1][1]; KR[1][2] = M[1][2];666 KR[2][0] = M[2][0]; KR[2][1] = M[2][1]; KR[2][2] = M[2][2];667 668 669 // Kuw transformation670 M = ((Kuu + Kwu)*Aj + (Kuw + Kww)*Bj)*(-alpha);671 KR[0][3] = M[0][0]; KR[0][4] = M[0][1]; KR[0][5] = M[0][2];672 KR[1][3] = M[1][0]; KR[1][4] = M[1][1]; KR[1][5] = M[1][2];673 KR[2][3] = M[2][0]; KR[2][4] = M[2][1]; KR[2][5] = M[2][2];674 675 676 // Kwu transformation677 M = (Ai*(Kuu + Kuw) + Bi*(Kwu + Kww))*alpha;678 KR[3][0] = M[0][0]; KR[3][1] = M[0][1]; KR[3][2] = M[0][2];679 KR[4][0] = M[1][0]; KR[4][1] = M[1][1]; KR[4][2] = M[1][2];680 KR[5][0] = M[2][0]; KR[5][1] = M[2][1]; KR[5][2] = M[2][2];681 682 683 // Kww transformation684 M = ((Ai*Kuu + Bi*Kwu)*Aj + (Ai*Kuw + Bi*Kww)*Bj)*(-alpha);685 KR[3][3] = M[0][0]; KR[3][4] = M[0][1]; KR[3][5] = M[0][2];686 KR[4][3] = M[1][0]; KR[4][4] = M[1][1]; KR[4][5] = M[1][2];687 KR[5][3] = M[2][0]; KR[5][4] = M[2][1]; KR[5][5] = M[2][2];688 689 // add the stiffness components to the Krr matrix690 for (k = 0; k<6; ++k)691 for (l = 0; l<6; ++l)692 {693 J = lmj[k];694 I = lmi[l];695 696 if (I >= 0)697 {698 // multiply KR by alpha for alpha rule699 if (J < -1) F[I] -= KR[l][k]*ui[-J - 2];700 else if (J >= 0) K.add(I, J, KR[l][k]);701 }702 }703 704 // we still need to couple the non-rigid degrees of node i to the705 // rigid dofs of node j706 for (k = 6; k<ndof; ++k) {707 vec3d kpu(kij[k][0], kij[k][1], kij[k][2]);708 vec3d kpw(kij[k][3], kij[k][4], kij[k][5]);709 vec3d m = (kpu + kpw)*alpha;710 KF[k][0] = m.x; KF[k][1] = m.y; KF[k][2] = m.z;711 m = (Aj*kpu + Bj*kpw)*alpha;712 KF[k][3] = m.x; KF[k][4] = m.y; KF[k][5] = m.z;713 }714 715 for (k = 0; k<6; ++k)716 for (l = 6; l<ndof; ++l)717 {718 J = lmj[k];719 I = elm[ndof*i + l];720 721 if (I >= 0)722 {723 // multiply KF by alpha for alpha rule724 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];725 else if (J >= 0) K.add(I, J, KF[l][k]);726 }727 }728 729 // now the transpose location730 for (l = 6; l<ndof; ++l) {731 vec3d kup(kij[0][l], kij[1][l], kij[2][l]);732 vec3d kwp(kij[3][l], kij[4][l], kij[5][l]);733 vec3d m = kup + kwp;734 KF[l][0] = m.x; KF[l][1] = m.y; KF[l][2] = m.z;735 m = Ai*kup + Bi*kwp;736 KF[l][3] = m.x; KF[l][4] = m.y; KF[l][5] = m.z;737 }738 739 for (k = 0; k<6; ++k)740 for (l = 6; l<ndof; ++l)741 {742 J = elm[ndof*j + l];743 I = lmi[k];744 745 if (I >= 0)746 {747 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];748 else if (J >= 0) K.add(I, J, KF[l][k]);749 }750 }751 752 }753 else754 {755 // node i is not a rigid body node756 // add the stiffness components to the Kfr matrix757 758 // Kij759 for (k = 0; k<ndof; ++k) {760 vec3d kpu(kij[k][0], kij[k][1], kij[k][2]);761 vec3d kpw(kij[k][3], kij[k][4], kij[k][5]);762 vec3d m = (kpu + kpw)*alpha;763 KF[k][0] = m.x; KF[k][1] = m.y; KF[k][2] = m.z;764 m = (Aj*kpu + Bj*kpw)*alpha;765 KF[k][3] = m.x; KF[k][4] = m.y; KF[k][5] = m.z;766 }767 768 for (k = 0; k<6; ++k)769 for (l = 0; l<ndof; ++l)770 {771 J = lmj[k];772 I = elm[ndof*i + l];773 774 if (I >= 0)775 {776 // multiply KF by alpha for alpha rule777 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];778 else if (J >= 0) K.add(I, J, KF[l][k]);779 }780 }781 }782 }783 }784 else785 {786 // loop over rows787 for (i = 0; i<n; ++i)788 {789 FENode& nodei = mesh.Node(en[i]);790 if (nodei.m_rid >= 0)791 {792 // node i is a rigid body793 // get the rigid body this node is attached to794 FERigidBody& RBi = *rigid.Object(nodei.m_rid);795 796 // get the rigid body equation nrs.797 lmi = RBi.m_LM;798 799 // get the relative distance (use alpha rule)800 ai = (nodei.m_rt - RBi.m_rt)*alpha + (nodei.m_rp - RBi.m_rp)*(1 - alpha);801 Ai.skew(ai);802 803 // get the shell director804 bi = ai - (nodei.m_d0 + nodei.get_vec3d(m_dofX, m_dofY, m_dofZ) - nodei.get_vec3d(m_dofSX, m_dofSY, m_dofSZ));805 Bi.skew(bi);806 807 // get the element sub-matrix808 for (k = 0; k<ndof; ++k)809 for (l = 0; l<ndof; ++l)810 kij[k][l] = ke[ndof*i + k][ndof*j + l];811 812 // add the stiffness components to the Krf matrix813 814 // Kij815 for (k = 0; k<ndof; ++k) {816 vec3d kup(kij[0][k], kij[1][k], kij[2][k]);817 vec3d kwp(kij[3][k], kij[4][k], kij[5][k]);818 vec3d m = kup + kwp;819 KF[k][0] = m.x; KF[k][1] = m.y; KF[k][2] = m.z;820 m = Ai*kup + Bi*kwp;821 KF[k][3] = m.x; KF[k][4] = m.y; KF[k][5] = m.z;822 }823 824 for (k = 0; k<6; ++k)825 for (l = 0; l<ndof; ++l)826 {827 I = lmi[k];828 J = elm[ndof*j + l];829 830 if (I >= 0)831 {832 if (J < -1) F[I] -= KF[l][k] * ui[-J - 2];833 else if (J >= 0) K.add(I, J, KF[l][k]);834 }835 }836 }837 }838 }839 }840}841 842//-----------------------------------------------------------------------------843void FERigidSolver::AssembleResidual(int node_id, int dof, double f, vector<double>& R)844{845 FEMesh& mesh = m_fem->GetMesh();846 FERigidSystem& rigid = *m_fem->GetRigidSystem();847 848 // get the equation number849 FENode& node = mesh.Node(node_id);850 int n = node.m_ID[dof];851 852 // assemble into global vector853 if (n >= 0) R[n] += f;854 else if (node.m_rid >= 0)855 {856 // this is a rigid body node857 FERigidBody& RB = *rigid.Object(node.m_rid);858 859 // get the relative position860 vec3d a = node.m_rt - RB.m_rt;861 862 int* lm = RB.m_LM;863 if (dof == m_dofX)864 {865 if (lm[0] >= 0) R[lm[0]] += f;866 if (lm[4] >= 0) R[lm[4]] += a.z*f;867 if (lm[5] >= 0) R[lm[5]] += -a.y*f;868 }869 else if (dof == m_dofY)870 {871 if (lm[1] >= 0) R[lm[1]] += f;872 if (lm[3] >= 0) R[lm[3]] += -a.z*f;873 if (lm[5] >= 0) R[lm[5]] += a.x*f;874 }875 else if (dof == m_dofZ)876 {877 if (lm[2] >= 0) R[lm[2]] += f;878 if (lm[3] >= 0) R[lm[3]] += a.y*f;879 if (lm[4] >= 0) R[lm[4]] += -a.x*f;880 }881 if (node.HasFlags(FENode::SHELL) && node.HasFlags(FENode::RIGID_CLAMP)) {882 // get the shell director883 vec3d d = node.m_d0 + node.get_vec3d(m_dofX, m_dofY, m_dofZ) - node.get_vec3d(m_dofSX, m_dofSY, m_dofSZ);884 vec3d b = a - d;885 if (dof == m_dofSX)886 {887 if (lm[0] >= 0) R[lm[0]] += f;888 if (lm[4] >= 0) R[lm[4]] += b.z*f;889 if (lm[5] >= 0) R[lm[5]] += -b.y*f;890 }891 else if (dof == m_dofSY)892 {893 if (lm[1] >= 0) R[lm[1]] += f;894 if (lm[3] >= 0) R[lm[3]] += -b.z*f;895 if (lm[5] >= 0) R[lm[5]] += b.x*f;896 }897 else if (dof == m_dofSZ)898 {899 if (lm[2] >= 0) R[lm[2]] += f;900 if (lm[3] >= 0) R[lm[3]] += b.y*f;901 if (lm[4] >= 0) R[lm[4]] += -b.x*f;902 }903 }904 }905}906 907//-----------------------------------------------------------------------------908void FERigidSolver::Residual()909{910 FERigidSystem& rigid = *m_fem->GetRigidSystem();911 int NRB = rigid.Objects();912 for (int i = 0; i<NRB; ++i)913 {914 FERigidBody& RB = *rigid.Object(i);915 RB.m_Fr = RB.m_Mr = vec3d(0, 0, 0);916 }917}918 919//-----------------------------------------------------------------------------920void FERigidSolver::StiffnessMatrix(SparseMatrix& K, const FETimeInfo& tp)921{922 FERigidSystem& rigid = *m_fem->GetRigidSystem();923 924 // we still need to set the diagonal elements to 1925 // for the prescribed rigid body dofs.926 int NRB = rigid.Objects();927 for (int i = 0; i<NRB; ++i)928 {929 FERigidBody& rb = *rigid.Object(i);930 for (int j = 0; j<6; ++j)931 if (rb.m_LM[j] < -1)932 {933 int I = -rb.m_LM[j] - 2;934 K.set(I, I, 1);935 }936 }937}938 939//-----------------------------------------------------------------------------940//! This function calculates the contribution to the mass matrix from the rigid bodies941void FERigidSolver::RigidMassMatrix(FESolver* solver, const FETimeInfo& timeInfo)942{943 FERigidSystem& rigid = *m_fem->GetRigidSystem();944 945 // element stiffness matrix946 vector<int> lm;947 matrix ke;948 949 // 6 dofs per rigid body950 ke.resize(6, 6);951 952 // Newmark integration rule953 double dt = timeInfo.timeIncrement;954 double beta = timeInfo.beta;955 double gamma = timeInfo.gamma;956 double a = 1. / (beta*dt*dt);957 958 for (int i=0; i<rigid.Objects(); ++i)959 {960 FERigidBody& RB = *rigid.Object(i);961 962 // mass matrix963 double M = RB.m_mass*a;964 965 ke.zero();966 ke[0][0] = M;967 ke[1][1] = M;968 ke[2][2] = M;969 970 // evaluate mass moment of inertia at t971 mat3d Rt = RB.GetRotation().RotationMatrix();972 mat3ds Jt = (Rt*RB.m_moi*Rt.transpose()).sym();973 974 // incremental rotation in spatial frame975 quatd q = RB.GetRotation()*RB.m_qp.Inverse();976 q.MakeUnit(); // clean-up roundoff errors977 double theta = 2 * tan(q.GetAngle() / 2); // get theta from Cayley transform978 vec3d e = q.GetVector();979 980 // skew-symmetric tensor whose axial vector is the incremental rotation981 mat3d qhat;982 qhat.skew(e*theta);983 984 // generate tensor T(theta)985 mat3d T = mat3dd(1) + qhat / 2 + dyad(e*theta) / 4;986 987 // skew-symmetric of angular momentum988 mat3d Jw;989 Jw.skew(Jt*RB.m_wt);990 991 // rotational inertia stiffness992 mat3d K = (Jt*T)*a*gamma - Jw / dt;993 994 ke[3][3] = K(0, 0); ke[3][4] = K(0, 1); ke[3][5] = K(0, 2);995 ke[4][3] = K(1, 0); ke[4][4] = K(1, 1); ke[4][5] = K(1, 2);996 ke[5][3] = K(2, 0); ke[5][4] = K(2, 1); ke[5][5] = K(2, 2);997 998 lm.assign(RB.m_LM, RB.m_LM + 6);999 1000 solver->AssembleStiffness(lm, ke);1001 }1002}1003 1004//=================================================================================================1005// FERigidSolverOld1006//=================================================================================================1007 1008//-----------------------------------------------------------------------------1009//! This function updates the rigid body linear and angular velocity by solving1010//! an overdetermined system of linear equations using the least-square method. 1011void FERigidSolverOld::UpdateRigidKinematics()1012{1013 // get the model and mesh1014 FEModel& fem = *m_fem;1015 FEMesh& mesh = fem.GetMesh();1016 FERigidSystem& rigid = *fem.GetRigidSystem();1017 1018 // loop over all rigid bodies1019 int NRB = rigid.Objects();1020 for (int j = 0; j<NRB; ++j)1021 {1022 // get the rigid body1023 FERigidBody& rb = *rigid.Object(j);1024 1025 // right-hand side and least-square matrix1026 vector<double> r; r.assign(6, 0.0);1027 matrix m(6, 6); m.zero();1028 1029 // we need to loop over all domains that define this rigid body1030 int ncnt = 0;1031 int NDOM = mesh.Domains();1032 for (int n = 0; n<NDOM; ++n)1033 {1034 FEDomain& dom = mesh.Domain(n);1035 FEMaterial* pm = dom.GetMaterial();1036 if (pm->IsRigid())1037 {1038 FERigidMaterial* prm = static_cast<FERigidMaterial*>(pm);1039 if (prm->GetRigidBodyID() == j)1040 {1041 // now loop over all the nodes1042 int NN = dom.Nodes();1043 for (int i = 0; i<NN; ++i, ncnt++)1044 {1045 vec3d ri = dom.Node(i).m_rt - rb.m_rt;1046 vec3d vi = dom.Node(i).get_vec3d(m_dofVX, m_dofVY, m_dofVZ);1047 1048 vec3d wi = ri ^ vi;1049 1050 // right-hand side1051 r[0] += vi.x;1052 r[1] += vi.y;1053 r[2] += vi.z;1054 r[3] += wi.x;1055 r[4] += wi.y;1056 r[5] += wi.z;1057 1058 // least-squares matrix1059 m[0][0] += 1.0;1060 m[1][1] += 1.0;1061 m[2][2] += 1.0;1062 1063 m[0][4] += ri.z; m[0][5] += -ri.y;1064 m[1][3] += -ri.z; m[1][5] += ri.x;1065 m[2][3] += ri.y; m[2][4] += -ri.x;1066 1067 m[3][4] += -ri.z; m[3][5] += ri.y;1068 m[4][3] += ri.z; m[4][5] += -ri.x;1069 m[5][3] += -ri.y; m[5][4] += ri.x;1070 1071 m[3][3] += ri.y*ri.y + ri.z*ri.z; m[3][4] += -ri.x*ri.y; m[3][5] += -ri.x*ri.z;1072 m[4][4] += ri.x*ri.x + ri.z*ri.z; m[4][3] += -ri.x*ri.y; m[4][5] += -ri.y*ri.z;1073 m[5][5] += ri.x*ri.x + ri.y*ri.y; m[5][3] += -ri.x*ri.z; m[5][4] += -ri.y*ri.z;1074 }1075 }1076 }1077 }1078 1079 // solve for the rigid body velocity (if we have enough nodes)1080 if (ncnt > 2)1081 {1082 vector<double> VR = r / m;1083 rb.m_vt = vec3d(VR[0], VR[1], VR[2]);1084 rb.m_wt = vec3d(VR[3], VR[4], VR[5]);1085 }1086 }1087}1088 1089//-----------------------------------------------------------------------------1090//! Updates the rigid body data1091void FERigidSolverOld::UpdateRigidBodies(vector<double>& Ui, vector<double>& ui, bool bnewUpdate)1092{1093 // get the number of rigid bodies1094 FERigidSystem& rigid = *m_fem->GetRigidSystem();1095 const int NRB = rigid.Objects();1096 1097 // first calculate the rigid body displacement increments1098 for (int i = 0; i<NRB; ++i)1099 {1100 // get the rigid body1101 FERigidBody& RB = *rigid.Object(i);1102 int *lm = RB.m_LM;1103 double* du = RB.m_du;1104 1105 if (RB.m_prb == 0)1106 {1107 for (int j = 0; j<6; ++j)1108 {1109 du[j] = (lm[j] >= 0 ? Ui[lm[j]] + ui[lm[j]] : 0);1110 }1111 }1112 }1113 1114 // for prescribed displacements, the displacement increments are evaluated differently1115 // TODO: Is this really necessary? Why can't the ui vector contain the correct values?1116 const int NRD = rigid.PrescribedBCs();1117 for (int i = 0; i<NRD; ++i)1118 {1119 FERigidBodyDisplacement& dc = *rigid.PrescribedBC(i);1120 if (dc.IsActive())1121 {1122 FERigidBody& RB = *rigid.Object(dc.id);1123 if (RB.m_prb == 0)1124 {1125 RB.m_du[dc.bc] = (dc.lc < 0 ? 0 : dc.Value() - RB.m_Up[dc.bc]);1126 }1127 }1128 }1129 1130 // update the rigid bodies1131 for (int i = 0; i<NRB; ++i)1132 {1133 // get the rigid body1134 FERigidBody& RB = *rigid.Object(i);1135 double* du = RB.m_du;1136 1137 if (bnewUpdate)1138 {1139 // This is the ""new"" update algorithm which addressesses a couple issues1140 // with the old method, namely that prescribed rotational dofs aren't update correctly.1141 // Unfortunately, it seems to produce worse convergence in some cases, especially with line search1142 // and it doesn't work when rigid bodies are used in a hierarchy1143 if (RB.m_prb) du = RB.m_dul;1144 RB.m_Ut[0] = RB.m_Up[0] + du[0];1145 RB.m_Ut[1] = RB.m_Up[1] + du[1];1146 RB.m_Ut[2] = RB.m_Up[2] + du[2];1147 RB.m_Ut[3] = RB.m_Up[3] + du[3];1148 RB.m_Ut[4] = RB.m_Up[4] + du[4];1149 RB.m_Ut[5] = RB.m_Up[5] + du[5];1150 1151 RB.m_rt = RB.m_r0 + vec3d(RB.m_Ut[0], RB.m_Ut[1], RB.m_Ut[2]);1152 1153 vec3d Rt(RB.m_Ut[3], RB.m_Ut[4], RB.m_Ut[5]);1154 RB.SetRotation(quatd(Rt));1155 }1156 else1157 {1158 // This is the ""old"" update algorithm which has some issues. It does not produce the correct1159 // rigid body orientation when the rotational degrees of freedom are prescribed.1160 RB.m_rt.x = RB.m_rp.x + du[0];1161 RB.m_rt.y = RB.m_rp.y + du[1];1162 RB.m_rt.z = RB.m_rp.z + du[2];1163 1164 vec3d r = vec3d(du[3], du[4], du[5]);1165 double w = sqrt(r.x*r.x + r.y*r.y + r.z*r.z);1166 quatd dq = quatd(w, r);1167 1168 quatd Q = dq*RB.m_qp;1169 Q.MakeUnit();1170 RB.SetRotation(Q);1171 1172 if (RB.m_prb) du = RB.m_dul;1173 RB.m_Ut[0] = RB.m_Up[0] + du[0];1174 RB.m_Ut[1] = RB.m_Up[1] + du[1];1175 RB.m_Ut[2] = RB.m_Up[2] + du[2];1176 RB.m_Ut[3] = RB.m_Up[3] + du[3];1177 RB.m_Ut[4] = RB.m_Up[4] + du[4];1178 RB.m_Ut[5] = RB.m_Up[5] + du[5];1179 }1180 }1181 1182 // we need to update the position of rigid nodes1183 rigid.UpdateMesh();1184 1185 // Since the rigid nodes are repositioned we need to update the displacement DOFS1186 FEMesh& mesh = m_fem->GetMesh();1187 int N = mesh.Nodes();1188 for (int i = 0; i<N; ++i)1189 {1190 FENode& node = mesh.Node(i);1191 if (node.m_rid >= 0)1192 {1193 vec3d ut = node.m_rt - node.m_r0;1194 node.set_vec3d(m_dofX, m_dofY, m_dofZ, ut);1195 }1196 }1197}1198 1199//=================================================================================================1200// FERigidSolverNew