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>(name,sp)
,_nd1(nd1)
,_nd2(nd2){
params=std::allocate_shared<SpringDamperParams>(kc::Allocator<SpringDamperParams>{},
std::format("{}_params",name));
if(not_nd1->isExternalForceNode()){
throwstd::invalid_argument("Node1isnotaforcenode.");
}
if(not_nd2->isExternalForceNode()){
throwstd::invalid_argument("Node2isnotaforcenode.");
}
_newton=_nd1->parentBody()->multibody()->getNewtonianFrame();
_f2f1=_newton->frame2Frame(_nd1);
_f2f2=_newton->frame2Frame(_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
_scratchOffset=
_f2f2->relTransform().getTranslation()-_f2f1->relTransform().getTranslation();

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

//Thecurrentlengthbetweenthenodes
doublel=_scratchOffset.norm();

//Signedscalarlengtherror
_scratchPositionError=l-params->unsprung_length;

//Forceonnode1towardnode2duetospringstiffness,asexpressedin
//theinertialframe.
_scratchStiffnessForce=params->k*_scratchPositionError;

//Forceonnode1duetosampingasexpressedintheinertialframe.
_scratchDampingForce=params->d*_scratchVelocityError;

//Forceonnode1duetothespringasexpressedintheinertialframe.
_scratchTotalForce=_scratchStiffnessForce*_scratchOffset/l+_scratchDampingForce;
//km::Vec3f=(l-params->unsprung_length)*params->k*t/l-params->d*v;

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

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

doubleSpringDamper::getPositionError()const{return_scratchPositionError;}

constkm::Vec3&SpringDamper::getVelocityError()const{return_scratchVelocityError;}

doubleSpringDamper::getStiffnessForce()const{return_scratchStiffnessForce;}

constkm::Vec3&SpringDamper::getDampingForce()const{return_scratchDampingForce;}

constkm::Vec3&SpringDamper::getTotalForce()const{return_scratchTotalForce;}

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;
}

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

}//namespaceKarana::Models