diff --git a/src/chrono/fea/ChBuilderBeam.cpp b/src/chrono/fea/ChBuilderBeam.cpp index 4d280529a3..10632d08fe 100644 --- a/src/chrono/fea/ChBuilderBeam.cpp +++ b/src/chrono/fea/ChBuilderBeam.cpp @@ -54,6 +54,8 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodes(beam_nodes[i - 1], beam_nodes[i]); element->SetSection(sect); + + element->SetRefRotation(mrot.GetQuaternion()); } } @@ -92,10 +94,12 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodes(beam_nodes[i - 1], beam_nodes[i]); ChQuaternion<> elrot = mrot.GetQuaternion(); - element->SetNodeAreferenceRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); - element->SetNodeBreferenceRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); + element->SetRefRotation(elrot); + element->SetElemToNodeArefRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); + element->SetElemToNodeBrefRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); element->SetSection(sect); + } } @@ -129,12 +133,12 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodes(beam_nodes[i - 1], beam_nodes[i]); ChQuaternion<> elrot = mrot.GetQuaternion(); - element->SetNodeAreferenceRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); - element->SetNodeBreferenceRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); - // std::cout << "Element n." << i << " with rotations:" << std::endl; - // std::cout << " Qa=" << element->GetNodeAreferenceRot() << std::endl; - // std::cout << " Qb=" << element->GetNodeBreferenceRot() << std::endl << std::endl; + element->SetRefRotation(elrot); + element->SetElemToNodeArefRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); + element->SetElemToNodeBrefRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); + element->SetSection(sect); + } } diff --git a/src/chrono/fea/ChElementBar.cpp b/src/chrono/fea/ChElementBar.cpp index 52f263b7a8..28225c09e7 100644 --- a/src/chrono/fea/ChElementBar.cpp +++ b/src/chrono/fea/ChElementBar.cpp @@ -17,7 +17,7 @@ namespace chrono { namespace fea { -ChElementBar::ChElementBar() : area(0.01 * 0.01), density(1000), E(0.01e9), rdamping(0.01), mass(0), length(0) { +ChElementBar::ChElementBar() : area(0.01 * 0.01), density(1000), E(0.01e9), rdamping(0.01), kstiff(0), mass(0), length(0) { nodes.resize(2); } @@ -54,10 +54,8 @@ void ChElementBar::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, doubl ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); ChVectorN dircolumn = dir.eigen(); - double Kstiffness = ((this->area * this->E) / this->length); - double Rdamping = this->rdamping * Kstiffness; // note that stiffness and damping matrices are the same, so join stuff here - double commonfactor = Kstiffness * Kfactor + Rdamping * Rfactor; + double commonfactor = this->kstiff * Kfactor + (this->rdamping * this->kstiff) * Rfactor; // TODO: the damping part depends on velocity, is 1/dt passed to Rfactor ? Or should 1/dt be inside the damping matrix calculation here? ChMatrix33<> V = dircolumn * dircolumn.transpose(); ChMatrix33<> keV = commonfactor * V; @@ -69,10 +67,9 @@ void ChElementBar::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, doubl // add geometric stiffness - in future it might become an option to switch off if not needed. // See for ex. http://shodhbhagirathi.iitr.ac.in:8081/jspui/handle/123456789/8433 pag. 14-15 if (true) { - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); + double L_ref = this->length; double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double Kstiffness1 = ((this->area * this->E) / this->length); - double internal_Kforce_local = Kstiffness1 * (L - L_ref); + double internal_Kforce_local = this->kstiff * (L - L_ref); ChMatrix33<> kgV = Kfactor * (internal_Kforce_local / L_ref) * (ChMatrix33<>(1) - V); H.block(0, 0, 3, 3) += kgV; @@ -94,29 +91,34 @@ void ChElementBar::SetupInitial(ChSystem* system) { // Compute rest length, mass: this->length = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); this->mass = this->length * this->area * this->density; + this->kstiff = this->area * this->E / this->length; } void ChElementBar::ComputeInternalForces(ChVectorDynamic<>& Fi) { assert(Fi.size() == 6); - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double internal_force_local = GetCurrentForce(); - ChVector3d int_forceA = dir * internal_force_local; - ChVector3d int_forceB = -dir * internal_force_local; - + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); + double internal_Kforce_local = this->kstiff * (L - L_ref); + double internal_Rforce_local = (this->rdamping * this->kstiff) * Vdot(dV, dPos) * Linv; + double internal_force_local = (internal_Kforce_local + internal_Rforce_local) * Linv; + ChVector3d int_forceA = dPos * internal_force_local; + ChVector3d int_forceB = -dPos * internal_force_local; Fi.segment(0, 3) = int_forceA.eigen(); Fi.segment(3, 3) = int_forceB.eigen(); } double ChElementBar::GetCurrentForce() { - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); - double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double L_dt = Vdot((nodes[1]->GetPosDt() - nodes[0]->GetPosDt()), dir); - double Kstiffness = ((this->area * this->E) / this->length); - double Rdamping = this->rdamping * Kstiffness; - double internal_Kforce_local = Kstiffness * (L - L_ref); - double internal_Rforce_local = Rdamping * L_dt; + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); + double internal_Kforce_local = this->kstiff * (L - L_ref); + double internal_Rforce_local = (this->rdamping * this->kstiff) * Vdot(dV, dPos) * Linv; return internal_Kforce_local + internal_Rforce_local; } diff --git a/src/chrono/fea/ChElementBar.h b/src/chrono/fea/ChElementBar.h index 8f7fb49bbd..acb6540ba4 100644 --- a/src/chrono/fea/ChElementBar.h +++ b/src/chrono/fea/ChElementBar.h @@ -71,7 +71,7 @@ class ChApi ChElementBar : public ChElementGeneric { // Custom properties functions // - /// Set the cross sectional area of the bar (m^2) (also changes stiffness keeping same E modulus) + /// Set the cross sectional area of the bar (m^2) void SetArea(double ma) { this->area = ma; } double GetArea() { return this->area; } @@ -79,7 +79,7 @@ class ChApi ChElementBar : public ChElementGeneric { void SetDensity(double md) { this->density = md; } double GetDensity() { return this->density; } - /// Set the Young elastic modulus (N/m^2) (also sets stiffness) + /// Set the Young elastic modulus (N/m^2) void SetYoungModulus(double mE) { this->E = mE; } double GetYoungModulus() { return this->E; } @@ -111,7 +111,7 @@ class ChApi ChElementBar : public ChElementGeneric { // (***not needed, thank to bookkeeping in parent class ChElementGeneric) private: - /// Initial setup. Precompute mass and matrices that do not change during the simulation, such as the local tangent + /// Initial setup. Precompute mass, rest length, axial stiffness and matrices that do not change during the simulation, such as the local tangent /// stiffness Kl of each element, if needed, etc. virtual void SetupInitial(ChSystem* system) override; @@ -120,6 +120,7 @@ class ChApi ChElementBar : public ChElementGeneric { double density; double E; double rdamping; + double kstiff; double mass; double length; }; diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index b49a4c1151..699234488e 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -15,6 +15,10 @@ // #define BEAM_VERBOSE #include +#include +#include "chrono/core/ChQuaternion.h" +#include "chrono/core/ChVector3.h" +#include "chrono/fea/ChNodeFEAxyzrot.h" #include "chrono/fea/ChElementBeamEuler.h" @@ -22,8 +26,10 @@ namespace chrono { namespace fea { ChElementBeamEuler::ChElementBeamEuler() - : q_refrotA(QUNIT), - q_refrotB(QUNIT), + : q_elemref_to_nodeAref_conj(QUNIT), + q_elemref_to_nodeBref_conj(QUNIT), + yabs_in_elemref_to_nodeAref_frame(0, 1, 0), + yabs_in_elemref_to_nodeBref_frame(0, 1, 0), q_element_abs_rot(QUNIT), q_element_ref_rot(QUNIT), disable_corotate(false), @@ -88,71 +94,65 @@ void ChElementBeamEuler::Update() { // parent class update: ChElementGeneric::Update(); - // always keep updated the rotation matrix A: - this->UpdateRotation(); + // Update the rotation matrix A when necessary: + if (!this->disable_corotate) + this->UpdateRotation(); } void ChElementBeamEuler::UpdateRotation() { - ChMatrix33<> A0(this->q_element_ref_rot); - ChMatrix33<> Aabs; - if (this->disable_corotate) { - Aabs = A0; - q_element_abs_rot = q_element_ref_rot; - } else { - ChVector3d mXele_w = nodes[1]->Frame().GetPos() - nodes[0]->Frame().GetPos(); - // propose Y_w as absolute dir of the Y axis of A node, removing the effect of Aref-to-A rotation if any: - // Y_w = [R Aref->w ]*[R Aref->A ]'*{0,1,0} - ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(q_refrotA.RotateBack(ChVector3d(0, 1, 0))); - // propose Y_w as absolute dir of the Y axis of B node, removing the effect of Bref-to-B rotation if any: - // Y_w = [R Bref->w ]*[R Bref->B ]'*{0,1,0} - ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(q_refrotB.RotateBack(ChVector3d(0, 1, 0))); - // Average the two Y directions to have midpoint torsion (ex -30?torsion A and +30?torsion B= 0? - ChVector3d myele_w = (myele_wA + myele_wB).GetNormalized(); - Aabs.SetFromAxisX(mXele_w, myele_w); - q_element_abs_rot = Aabs.GetQuaternion(); - } - this->A = A0.transpose() * Aabs; + ChVector3d mXele_w = nodes[1]->Frame().GetPos() - nodes[0]->Frame().GetPos(); + // propose Y_w as absolute dir of the Y axis of A node, removing the effect of Aref-to-A rotation if any: + // Y_w = [R Aref->w ]*[R Aref->A ]'*{0,1,0} + ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeAref_frame); + // propose Y_w as absolute dir of the Y axis of B node, removing the effect of Bref-to-B rotation if any: + // Y_w = [R Bref->w ]*[R Bref->B ]'*{0,1,0} + ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeBref_frame); + // Average the two Y directions to have midpoint torsion (ex -30?torsion A and +30?torsion B= 0? + ChVector3d myele_w = (myele_wA + myele_wB).GetNormalized(); + Aabs.SetFromAxisX(mXele_w, myele_w); + q_element_abs_rot = Aabs.GetQuaternion(); + + this->A.SetFromQuaternion(q_element_ref_rot.GetConjugate() * q_element_abs_rot); } void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { mD.resize(12); - ChVector3d delta_rot_dir; - double delta_rot_angle; - - // Node 0, displacement (in local element frame, corotated back) - // d = [Atw]' Xt - [A0w]'X0 - ChVector3d displ = this->q_element_abs_rot.RotateBack(nodes[0]->Frame().GetPos()) - - this->q_element_ref_rot.RotateBack(nodes[0]->GetX0().GetPos()); - mD.segment(0, 3) = displ.eigen(); - - // Node 0, x,y,z small rotations (in local element frame) - ChQuaternion<> q_delta0 = q_element_abs_rot.GetConjugate() * nodes[0]->Frame().GetRot() * q_refrotA.GetConjugate(); - // note, for small incremental rotations this is opposite of ChNodeFEAxyzrot::VariablesQbIncrementPosition - q_delta0.GetAngleAxis(delta_rot_angle, delta_rot_dir); - - if (delta_rot_angle > CH_PI) - delta_rot_angle -= CH_2PI; // no 0..360 range, use -180..+180 - - mD.segment(3, 3) = delta_rot_angle * delta_rot_dir.eigen(); - - // Node 1, displacement (in local element frame, corotated back) - // d = [Atw]' Xt - [A0w]'X0 - displ = this->q_element_abs_rot.RotateBack(nodes[1]->Frame().GetPos()) - - this->q_element_ref_rot.RotateBack(nodes[1]->GetX0().GetPos()); - mD.segment(6, 3) = displ.eigen(); - - // Node 1, x,y,z small rotations (in local element frame) - ChQuaternion<> q_delta1 = q_element_abs_rot.GetConjugate() * nodes[1]->Frame().GetRot() * q_refrotB.GetConjugate(); + // Node displacement in local element frame + // d = [Atw]' Xt - [A0w]'X0 + // If corotational disabled, [Atw] == [A0w] and we can do a single rotation. + auto getDisplacement = [&](std::shared_ptr node) { + ChVector3d local_disp; + if (disable_corotate) + local_disp = q_element_ref_rot.RotateBack(node->Frame().GetPos() - node->GetX0().GetPos()); + else + local_disp = q_element_abs_rot.RotateBack(node->Frame().GetPos()) - + q_element_ref_rot.RotateBack(node->GetX0().GetPos()); + return local_disp; + }; + + // Node small rotations in local element frame // note, for small incremental rotations this is opposite of ChNodeFEAxyzrot::VariablesQbIncrementPosition - q_delta1.GetAngleAxis(delta_rot_angle, delta_rot_dir); - - if (delta_rot_angle > CH_PI) - delta_rot_angle -= CH_2PI; // no 0..360 range, use -180..+180 - - mD.segment(9, 3) = delta_rot_angle * delta_rot_dir.eigen(); + auto getRotation = [&](std::shared_ptr node, ChQuaternion<> q_elemref_to_noderef_conj) { + ChVector3d delta_rot_dir; + double delta_rot_angle; + // If corotational disabled, no efficiency gain by rotating qt * q0^-1 into local frame. Use corotational formula + ChQuaternion<> q_delta = q_element_abs_rot.GetConjugate() * node->Frame().GetRot() * q_elemref_to_noderef_conj; + // TODO: GetAngleAxis already returns in the range [-PI..PI], no need to change range + // TODO: the previous range was only shifted if delta_rot_angle > CH_PI. How about delta_rot_angle < - CH_PI ? Was that a bug? + // TODO: We may want to use GetRotVec directly, but the angle is not within [-PI .. PI] + // TODO: Consider changing GetRotVec() to return within [-PI .. PI] + q_delta.GetAngleAxis(delta_rot_angle, delta_rot_dir); + return delta_rot_angle * delta_rot_dir; + }; + + mD.segment(0, 3) = getDisplacement(nodes[0]).eigen(); + mD.segment(3, 3) = getRotation(nodes[0], q_elemref_to_nodeAref_conj).eigen(); + + mD.segment(6, 3) = getDisplacement(nodes[1]).eigen(); + mD.segment(9, 3) = getRotation(nodes[1], q_elemref_to_nodeBref_conj).eigen(); } void ChElementBeamEuler::GetFieldDt(ChVectorDynamic<>& mD_dt) { @@ -444,13 +444,8 @@ void ChElementBeamEuler::SetupInitial(ChSystem* system) { this->length = (nodes[1]->GetX0().GetPos() - nodes[0]->GetX0().GetPos()).Length(); this->mass = this->length * this->section->GetMassPerUnitLength(); - // Compute initial rotation - ChMatrix33<> A0; - ChVector3d mXele = nodes[1]->GetX0().GetPos() - nodes[0]->GetX0().GetPos(); - ChVector3d myele = - (nodes[0]->GetX0().GetRotMat().GetAxisY() + nodes[1]->GetX0().GetRotMat().GetAxisY()).GetNormalized(); - A0.SetFromAxisX(mXele, myele); - q_element_ref_rot = A0.GetQuaternion(); + // Set initial rotation + q_element_abs_rot = q_element_ref_rot; // Compute local stiffness matrix: ComputeStiffnessMatrix(); diff --git a/src/chrono/fea/ChElementBeamEuler.h b/src/chrono/fea/ChElementBeamEuler.h index d302c57389..0605830d54 100644 --- a/src/chrono/fea/ChElementBeamEuler.h +++ b/src/chrono/fea/ChElementBeamEuler.h @@ -15,6 +15,7 @@ #ifndef CHELEMENTBEAMEULER_H #define CHELEMENTBEAMEULER_H +#include "chrono/core/ChQuaternion.h" #include "chrono/fea/ChBeamSectionEuler.h" #include "chrono/fea/ChElementBeam.h" #include "chrono/fea/ChElementCorotational.h" @@ -71,19 +72,26 @@ class ChApi ChElementBeamEuler : public ChElementBeam, /// Get the second node (ending) std::shared_ptr GetNodeB() { return nodes[1]; } - /// Set the reference rotation of nodeA respect to the element rotation. - void SetNodeAreferenceRot(ChQuaternion<> mrot) { q_refrotA = mrot; } - ChQuaternion<> GetNodeAreferenceRot() { return q_refrotA; } + /// Set the reference rotation of nodeA with respect to the element rotation. + void SetElemToNodeArefRot(ChQuaternion<> mrot) { + q_elemref_to_nodeAref_conj = mrot.GetConjugate(); + yabs_in_elemref_to_nodeAref_frame = mrot.RotateBack(ChVector3d(0, 1, 0)); + } - /// Set the reference rotation of nodeB respect to the element rotation. - void SetNodeBreferenceRot(ChQuaternion<> mrot) { q_refrotB = mrot; } - ChQuaternion<> GetNodeBreferenceRot() { return q_refrotB; } + /// Set the reference rotation of nodeB with respect to the element rotation. + void SetElemToNodeBrefRot(ChQuaternion<> mrot) { + q_elemref_to_nodeBref_conj = mrot.GetConjugate(); + yabs_in_elemref_to_nodeBref_frame = mrot.RotateBack(ChVector3d(0, 1, 0)); + } /// Get the absolute rotation of element in space /// This is not the same of Rotation() , that expresses /// the accumulated rotation from starting point. ChQuaternion<> GetAbsoluteRotation() { return q_element_abs_rot; } + /// Set the original reference rotation of element in space + void SetRefRotation(ChQuaternion<> q_initial) { q_element_ref_rot = q_initial; } + /// Get the original reference rotation of element in space ChQuaternion<> GetRefRotation() { return q_element_ref_rot; } @@ -282,8 +290,10 @@ class ChApi ChElementBeamEuler : public ChElementBeam, ChMatrixDynamic<> Km; ///< local material stiffness matrix ChMatrixDynamic<> Kg; ///< local geometric stiffness matrix NORMALIZED by P - ChQuaternion<> q_refrotA; - ChQuaternion<> q_refrotB; + ChQuaternion<> q_elemref_to_nodeAref_conj; + ChQuaternion<> q_elemref_to_nodeBref_conj; + ChVector3d yabs_in_elemref_to_nodeAref_frame; + ChVector3d yabs_in_elemref_to_nodeBref_frame; ChQuaternion<> q_element_abs_rot; ChQuaternion<> q_element_ref_rot; diff --git a/src/chrono/fea/ChElementSpring.cpp b/src/chrono/fea/ChElementSpring.cpp index 213f1cfa9c..ebc9b9a4db 100644 --- a/src/chrono/fea/ChElementSpring.cpp +++ b/src/chrono/fea/ChElementSpring.cpp @@ -13,11 +13,12 @@ // ============================================================================= #include "chrono/fea/ChElementSpring.h" +#include "chrono/core/ChVector3.h" namespace chrono { namespace fea { -ChElementSpring::ChElementSpring() : spring_k(1.0), damper_r(0.01) { +ChElementSpring::ChElementSpring() : spring_k(1.0), damper_r(0.01), length(0) { nodes.resize(2); } @@ -38,6 +39,10 @@ void ChElementSpring::GetStateBlock(ChVectorDynamic<>& mD) { mD.segment(3, 3) = this->nodes[1]->GetPos().eigen(); } +void ChElementSpring::SetupInitial(ChSystem* system) { + this->length = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); +} + void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, double Rfactor, double Mfactor) { assert((H.rows() == 6) && (H.cols() == 6)); @@ -47,7 +52,7 @@ void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, do ChVectorN dircolumn = dir.eigen(); // note that stiffness and damping matrices are the same, so join stuff here - double commonfactor = this->spring_k * Kfactor + this->damper_r * Rfactor; + double commonfactor = this->spring_k * Kfactor + this->damper_r * Rfactor; // TODO: the damping part depends on velocity, is 1/dt passed to Rfactor ? Or should 1/dt be inside the damping matrix calculation here? ChMatrix33<> V = dircolumn * dircolumn.transpose(); ChMatrix33<> keV = commonfactor * V; @@ -59,7 +64,7 @@ void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, do // add geometric stiffness - in future it might become an option to switch off if not needed. // See for ex. http://shodhbhagirathi.iitr.ac.in:8081/jspui/handle/123456789/8433 pag. 14-15 if (true) { - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); + double L_ref = length; double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); double internal_Kforce_local = this->spring_k * (L - L_ref); @@ -76,26 +81,30 @@ void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, do void ChElementSpring::ComputeInternalForces(ChVectorDynamic<>& Fi) { assert(Fi.size() == 6); - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); - double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double L_dt = Vdot((nodes[1]->GetPosDt() - nodes[0]->GetPosDt()), dir); + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); double internal_Kforce_local = this->spring_k * (L - L_ref); - double internal_Rforce_local = this->damper_r * L_dt; - double internal_force_local = internal_Kforce_local + internal_Rforce_local; - ChVector3d int_forceA = dir * internal_force_local; - ChVector3d int_forceB = -dir * internal_force_local; + double internal_Rforce_local = this->damper_r * Vdot(dV, dPos) * Linv; + double internal_force_local = (internal_Kforce_local + internal_Rforce_local) * Linv; + ChVector3d int_forceA = dPos * internal_force_local; + ChVector3d int_forceB = -dPos * internal_force_local; Fi.segment(0, 3) = int_forceA.eigen(); Fi.segment(3, 3) = int_forceB.eigen(); } double ChElementSpring::GetCurrentForce() { - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); - double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double L_dt = Vdot((nodes[1]->GetPosDt() - nodes[0]->GetPosDt()), dir); + // TODO: This is only used in demo_FEA_truss.cpp to mimick failure when the force it too high + // Otherwise not necessary. Think of implementing failure mechanisms / max stress/strain criterion + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); double internal_Kforce_local = this->spring_k * (L - L_ref); - double internal_Rforce_local = this->damper_r * L_dt; + double internal_Rforce_local = this->damper_r * Vdot(dV, dPos) * Linv; return internal_Kforce_local + internal_Rforce_local; } diff --git a/src/chrono/fea/ChElementSpring.h b/src/chrono/fea/ChElementSpring.h index a1daf13c61..bea4f468c1 100644 --- a/src/chrono/fea/ChElementSpring.h +++ b/src/chrono/fea/ChElementSpring.h @@ -81,14 +81,15 @@ class ChApi ChElementSpring : public ChElementGeneric { // (***not needed, thank to bookkeeping in parent class ChElementGeneric) private: - /// Initial setup. + /// Initial setup. Precompute rest length /// No override needed for the spring element because global K is computed on-the-fly in /// ComputeAddKRmatricesGlobal() - ////virtual void SetupInitial(ChSystem* system) override {} + virtual void SetupInitial(ChSystem* system) override; std::vector > nodes; double spring_k; double damper_r; + double length; }; /// @} fea_elements