Program Listing for File SpringDamper.cc

↰ Return to documentation for file (doxygen_docs/GeneralKModels/SpringDamper.cc)

#include"Karana/GeneralKModels/SpringDamper.h"
#include"Karana/KCore/Allocator.h"
#include"Karana/SOADyn/Multibody.h"


namespaceKarana::Models{

namespacekd=Karana::Dynamics;

SpringDamper::SpringDamper(std::string_viewname,
constkc::ks_ptr<kd::ModelManager>&mm,
constkc::ks_ptr<kd::Node>nd1,
constkc::ks_ptr<kd::Node>nd2)
:KModel<SpringDamper,SpringDamperParams,SpringDamperScratch>(name,mm)
,_nd1(nd1)
,_nd2(nd2){
params=std::allocate_shared<SpringDamperParams>(kc::Allocator<SpringDamperParams>{},
std::format("{}_params",name));
scratch=std::allocate_shared<SpringDamperScratch>(kc::Allocator<SpringDamperScratch>{},
std::format("{}_scratch",name));

if(not_nd1->isExternalForceNode()){
throwstd::invalid_argument("Node1isnotaforcenode.");
}
if(not_nd2->isExternalForceNode()){
throwstd::invalid_argument("Node2isnotaforcenode.");
}

_newton=_nd1->parentBody()->multibody()->getNewtonianFrame();
_f2f1=_newton->frameToFrame(_nd1);
_f2f2=_newton->frameToFrame(_nd2);
};

kc::ks_ptr<SpringDamper>SpringDamper::create(std::string_viewname,
constkc::ks_ptr<kd::ModelManager>&mm,
constkc::ks_ptr<kd::Node>nd1,
constkc::ks_ptr<kd::Node>nd2){
kc::ks_ptr<SpringDamper>sd=
std::allocate_shared<SpringDamper>(kc::Allocator<SpringDamper>{},name,mm,nd1,nd2);
mm->registerModel(sd);
returnsd;
}

voidSpringDamper::preDeriv(constkm::Ktime&,constkm::Vec&){
//Thevectorfromnode1tonode2asmeasuredinthenewtonianframe
scratch->offset=
_f2f2->relTransform().getTranslation()-_f2f1->relTransform().getTranslation();

//Thevelocityofnode2relativetonode1asexpressedintheinertialframe.
scratch->velocity_error=_f2f2->relSpVel().getv()-_f2f1->relSpVel().getv();

//Thecurrentlengthbetweenthenodes
doublel=scratch->offset.norm();
km::Vec3dir=scratch->offset/l;

//Signedscalarlengtherror
scratch->position_error=l-params->unsprung_length;

//Forceonnode1towardnode2duetospringstiffness,asexpressedin
//theinertialframe.
scratch->stiffness_force=params->k*scratch->position_error;

/*Forceonnode1duetosampingasexpressedintheinertial
frame.Basedonusingtheaxial/radialvelocitycomponentof
therelativevelocity:
https://nexus.hexagon.com/documentationcenter/tr-TR/bundle/Adams_2022.4_Adams_View_Function_Builder_User_Guide/resource/Adams_2022.4_Adams_View_Function_Builder_User_Guide.pdf
*/
//scratch->damping_force=params->d*scratch->velocity_error;
scratch->damping_force=(params->d*scratch->velocity_error.dot(dir))*dir;

//Forceonnode1duetothespringasexpressedintheinertialframe.
scratch->total_force=scratch->stiffness_force*dir+scratch->damping_force;
//km::Vec3f=(l-params->unsprung_length)*params->k*t/l-params->d*v;

//Applytheequalandopposingforcestothenodes
_nd1->accumExternalSpForce(km::SpatialVector(km::Vec3{0,0,0},scratch->total_force),
_newton);
_nd2->accumExternalSpForce(km::SpatialVector(km::Vec3{0,0,0},-scratch->total_force),
_newton);
}

std::vector<kc::id_t>SpringDamper::getNodeIds(){
returnstd::vector<kc::id_t>{_nd1->id(),_nd2->id()};
}

constkc::ks_ptr<kd::Node>&SpringDamper::sourceNode()const{return_nd1;}

constkc::ks_ptr<kd::Node>&SpringDamper::targetNode()const{return_nd2;}

SpringDamperParams::SpringDamperParams(std::string_viewname)
:KModelParams(name){
unsprung_length=km::notReadyNaN;
k=km::notReadyNaN;
d=km::notReadyNaN;
}

boolSpringDamperParams::isReady()const{
boolflag=true;
if(km::isNotReadyNaN(k)){
kc::warn("Parameterkisnotready.");
flag=false;
}
if(km::isNotReadyNaN(d)){
kc::warn("Parameterdisnotready.");
flag=false;
}
if(km::isNotReadyNaN(unsprung_length)){
kc::warn("Parameterunsprung_lengthisnotready.");
flag=false;
}
returnflag;
}

std::string
SpringDamperParams::dumpString(std::string_viewprefix,
constKarana::Core::Base::DumpOptions*/*options*/)const{
//std::stringresult;
//std::cout<<"JJJJJ<"<<prefix<<">KKK\n";
autoresult=
std::format("{}k={},d={},unsprung_length={}\n",prefix,k,d,unsprung_length);
returnresult;
}

std::string
SpringDamperScratch::dumpString(std::string_viewprefix,
constKarana::Core::Base::DumpOptions*/*options*/)const{
//std::stringresult;
//std::cout<<"JJJJJ<"<<prefix<<">KKK\n";
autoresult=std::format("{}offset:{}\n",prefix,km::dumpString(offset));
result+=std::format("{}position_error:{}\n",prefix,position_error);

result+=std::format("{}velocity_error:{}\n",prefix,km::dumpString(velocity_error));
result+=std::format("{}stiffness_force:{}\n",prefix,stiffness_force);
result+=std::format("{}damping_force:{}\n",prefix,km::dumpString(damping_force));
result+=std::format("{}total_force:{}\n",prefix,km::dumpString(total_force));
returnresult;
}

//DestructorincludedforMacOSbuilds.Musthaveakey-functionout-of-linetoavoiddulpicate
//symbols.
SpringDamper::~SpringDamper(){};

SpringDamperScratch::SpringDamperScratch(std::string_viewname)
:KModelScratch(name){
km::makeNotReady(offset);
km::makeNotReady(velocity_error);
km::makeNotReady(damping_force);
km::makeNotReady(total_force);
}

}//namespaceKarana::Models