Program Listing for File CompoundSubhinge.h

↰ Return to documentation for file (include/Karana/SOADyn/CompoundSubhinge.h)

/*
*Copyright(c)2024-2025KaranaDynamicsPtyLtd.Allrightsreserved.
*
*NOTICETOUSER:
*
*Thissourcecodeand/ordocumentation(the"LicensedMaterials")is
*theconfidentialandproprietaryinformationofKaranaDynamicsInc.
*UseoftheseLicensedMaterialsisgovernedbythetermsandconditions
*ofaseparatesoftwarelicenseagreementbetweenKaranaDynamicsandthe
*Licensee("LicenseAgreement").Unlessexpresslypermittedunderthat
*agreement,anyreproduction,modification,distribution,ordisclosure
*oftheLicensedMaterials,inwholeorinpart,toanythirdparty
*withoutthepriorwrittenconsentofKaranaDynamicsisstrictlyprohibited.
*
*THELICENSEDMATERIALSAREPROVIDED"ASIS"WITHOUTWARRANTYOFANYKIND.
*KARANADYNAMICSDISCLAIMSALLWARRANTIES,EXPRESSORIMPLIED,INCLUDING
*BUTNOTLIMITEDTOWARRANTIESOFMERCHANTABILITY,NON-INFRINGEMENT,AND
*FITNESSFORAPARTICULARPURPOSE.
*
*INNOEVENTSHALLKARANADYNAMICSBELIABLEFORANYDAMAGESWHATSOEVER,
*INCLUDINGBUTNOTLIMITEDTOLOSSOFPROFITS,DATA,ORUSE,EVENIF
*ADVISEDOFTHEPOSSIBILITYOFSUCHDAMAGES,WHETHERINCONTRACT,TORT,
*OROTHERWISEARISINGOUTOFORINCONNECTIONWITHTHELICENSEDMATERIALS.
*
*U.S.GovernmentEndUsers:TheLicensedMaterialsarea"commercialitem"
*asdefinedat48C.F.R.2.101,andareprovidedtotheU.S.Government
*onlyasacommercialenditemunderthetermsofthislicense.
*
*AnyuseoftheLicensedMaterialsinindividualorcommercialsoftwaremust
*include,intheuserdocumentationandinternalsourcecodecomments,
*thisNotice,Disclaimer,andU.S.GovernmentUseProvision.
*/


#pragmaonce

#include<cassert>
#include<map>
#include<unordered_map>
#include<utility>

#include"Karana/KCore/LockingBase.h"
#include"Karana/Math/Defs.h"
#include"Karana/SOADyn/SubhingeBase.h"

namespaceKarana::Dynamics{

namespacekc=Karana::Core;
namespacekm=Karana::Math;

classCompoundHinge;
classCompoundBody;
classPhysicalBody;

classCompoundSubhinge://publicCoordBase_T<Eigen::Dynamic,Eigen::Dynamic,Eigen::Dynamic>,
//publicCoordBase,
publickc::LockingBase,
publicSubhingeBase{

/*foraccesstothe_computeATBI*methods*/
friendclassCompoundBody;

public:
virtual~CompoundSubhinge();

conststd::string&name()constoverride{returnkc::LockingBase::name();}
constkc::id_t&id()constoverride{returnkc::LockingBase::id();}

//size_tnQ()constoverride{returnCoordBase_T<>::nQ();}

//size_tnU()constoverride{returnCoordBase_T<>::nU();};

virtualconststd::string&typeString()constnoexceptoverride{
staticstd::stringtype_string="Karana::Dynamics::CompoundSubhinge";
returntype_string;
}

#if0
voidaddDependent(kc::ks_ptr<kc::LockingBase>dependent){
kc::LockingBase::addDependent(dependent);
}
#endif

#if1
constkm::MatatbiCoordMapMatrix()constoverride{
return_atbi_coord_map_matrix_cache->get();
}
#endif

SubhingeBase::SUBHINGE_TYPEsubhingeType()constoverride;
kc::ks_ptr<kf::Frame2Frame>f2f()constoverride;

std::stringdumpString(conststd::string&prefix,
constKarana::Core::Base::dumpOptions*options)constoverride;

voidsetQ(constEigen::Ref<constkm::Vec>&val)override;
voidsetQ(doublefill_value)override;
constkm::Vec&getQ()constoverride;

voidsetU(constEigen::Ref<constkm::Vec>&val)override;
voidsetU(doublefill_value)override;
constkm::Vec&getU()constoverride;

voidsetT(constEigen::Ref<constkm::Vec>&val)override;
voidsetT(doublefill_value)override;
constkm::Vec&getT()constoverride;

voidsetUdot(constEigen::Ref<constkm::Vec>&val)override;
voidsetUdot(doublefill_value)override;
constkm::Vec&getUdot()constoverride;

constkm::Vec&getQdot()constoverride;

//std::optional<bool>isOriented(constkf::Frame2Frame&f2f)constoverride;

//constkm::Mat&coordMapMatrix()constoverride;

CompoundSubhinge(kc::ks_ptr<CompoundHinge>hge);

protected:
//structATBIMatrices:publicCoordBase_T<>::ATBIMatrices{
structATBIMatrices:publicCoordBase::ATBIMatrices{

//std::map<kc::id_t,km::Mat>P;
km::MatP;

km::MatPplus;

km::MatHP;

km::MatD;

km::MatDinv;

km::MatG;

km::Mattauper;

km::Matpsi;

voidresize(size_thge_nU,size_tn_bodies){
//CoordBase_T<>::ATBIMatrices::resize(hge_nU,6*n_bodies);
size_tbody_nU=6*n_bodies;
P.resize(body_nU,body_nU);
P.setZero();
Pplus.resize(body_nU,body_nU);

HP.resize(hge_nU,body_nU);
D.resize(hge_nU,hge_nU);
Dinv.resize(hge_nU,hge_nU);
G.resize(body_nU,hge_nU);
tauper.resize(body_nU,body_nU);
psi.resize(body_nU,body_nU);
km::uninitialize(HP);
km::uninitialize(D);
km::uninitialize(Dinv);
km::uninitialize(G);
km::uninitialize(tauper);
km::uninitialize(psi);
}
};

//structATBIFilterVectors:publicCoordBase_T<>::ATBIFilterVectors{
structATBIFilterVectors:publicCoordBase::ATBIFilterVectors{

public:
km::Vecz;

km::Veczplus;

km::Vecepsilon;

km::Vecnu;

voidresize(size_thge_nU,size_tn_bodies){//,size_tn_base_onodes){
//CoordBase_T<>::ATBIFilterVectors::resize(hge_nU);
size_tbody_nU=6*n_bodies;
z.resize(body_nU);
zplus.resize(body_nU);//n_base_onodes);
epsilon.resize(hge_nU);
nu.resize(hge_nU);
km::uninitialize(epsilon);
km::uninitialize(nu);
}
};

protected:
virtualkm::MatgetUpsilonMatrix()override{returnkm::Mat();};

km::SpatialVectoraprimeAccel(kc::ks_ptr<PhysicalBody>bd)const;

/*TODO-implementthismethod-usethecoorddata'smethod*/
km::Matjacobian(constkf::Frame&/*target*/,bool/*oriented*/)constoverride{
assert(0);
returnkm::Mat(6,nU());
}

/*TODO-implementthismethod-usethecoorddata'smethod*/
km::MatjacobianDot(constkf::Frame&/*target*/,bool/*oriented*/)constoverride{
assert(0);
returnkm::Mat(6,nU());
}

constkm::Mat&getE_Phi_G()const{return_E_Phi_G_cache->get();}

void_computeATBICoordMapMatrix(km::Mat&val)const;
//virtualvoid_computeCoordMapMatrix(km::Mat&val);

void_discard(kc::ks_ptr<kc::Base>&base)override;

/*Foracompoundhingethisisthesameasthecoordmapmatrix*/
//void_computeATBICoordMapMatrix(km::Mat&val)const;

km::Mat66_getVrootPnodeATBIMatrixP();

km::SpatialVector_getVrootPnodeATBIFilterZ();

km::SpatialVector_getVrootPnodeInvDynF();

//km::Mat66_getBaseOnodeATBIMatrixPlus(kc::ks_ptr<HingeOnode>base_ond);
km::SpatialVector_getBaseOnodeATBIFilterZplus(kc::ks_ptr<HingeOnode>base_ond);

void_computeATBIMatrices();
void_computeATBIFilterVectors();
void_computeATBISmootherVectors();
void_computeUpsilonMatrices();

/*TODO-checkifwereallyneedthisorcanwegetridofit*/
//constkc::ks_ptr<CoordData>&_coordData()const;

km::SpatialVector_computeAprimeAccel(kc::ks_ptr<PhysicalBody>bd,
kc::ks_ptr<PhysicalBody>prev_bd)const;

km::MatgetATBIMatPsi()constoverride{returnkm::Mat();};

km::MatgetATBID()constoverride;
km::MatgetATBIDinv()constoverride;
km::MatgetATBIG()constoverride{returnkm::Mat();};
km::MatgetATBITauper()constoverride{returnkm::Mat();};

protected:
void_computeInvDynGenForce();

kc::ks_ptr<CompoundBody>_parent_body=nullptr;

void_computeE_Phi_G(km::Mat&val);

km::Mat_oframe2pframePsi()constoverride;
km::Mat_oframe2pframePhi()constoverride;

km::Mat_pframe2otherPhi(constkf::Frame&other)constoverride;

protected:
std::map<std::pair<kc::id_t,kc::id_t>,bool>_body_pnodes_orientation_map;

std::unordered_map<kc::id_t,kc::ks_ptr<kc::DataCache<km::SpatialVector>>>_aprime_caches;

kc::ks_ptr<kc::DataCache<km::Mat>>_E_Phi_G_cache=nullptr;

//kc::ks_ptr<CoordData>_coord_data=nullptr;

size_t_coord_map_rows=0;

kc::ks_ptr<kc::DataCache<km::Mat>>_atbi_coord_map_matrix_cache=nullptr;

protected:
//private:
/*
*Wewanttoreturnconstkm::Vec&forthe
*gettermethods.However,weneedtouseCoordData
*forCompoundSubhine.Therefore,weaddtheseasmutable
*variables(sincetheyneedtochangeintheconstfunctions)
*andreturnreferencestothem.
*/

#if1

mutablekm::Vec_getQ_cache;

mutablekm::Vec_getQdot_cache;

mutablekm::Vec_getU_cache;

mutablekm::Vec_getUdot_cache;

mutablekm::Vec_getT_cache;

#endif

private:
constkm::Mat&_atbiCoordMapMatrix()const{return_atbi_coord_map_matrix_cache->get();}
};

}//namespaceKarana::Dynamics