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::StatePropagator>&sp,
constkc::ks_ptr<kd::Node>nd1,
constkc::ks_ptr<kd::Node>nd2)
:KModel<SpringDamper,SpringDamperParams,SpringDamperScratch>(name,sp)
,_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::StatePropagator>&sp,
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,sp,nd1,nd2);
sp->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();

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

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

//Forceonnode1duetosampingasexpressedintheinertialframe.
scratch->damping_force=params->d*scratch->velocity_error;

//Forceonnode1duetothespringasexpressedintheinertialframe.
scratch->total_force=
scratch->stiffness_force*scratch->offset/l+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::uninitializedNaN;
k=km::uninitializedNaN;
d=km::uninitializedNaN;
}

boolSpringDamperParams::isFinalized()const{
boolflag=true;
if(km::isUninitializedNaN(k)){
kc::warn("Parameterkisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(d)){
kc::warn("Parameterdisuninitialized.");
flag=false;
}
if(km::isUninitializedNaN(unsprung_length)){
kc::warn("Parameterunsprung_lengthisuninitialized.");
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::uninitialize(offset);
km::uninitialize(velocity_error);
km::uninitialize(damping_force);
km::uninitialize(total_force);
}

}//namespaceKarana::Models