Program Listing for File CompoundBody.h

↰ Return to documentation for file (include/Karana/SOADyn/CompoundBody.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"Karana/SOADyn/BodyBase.h"
#include<cassert>
#include<map>
#include<vector>

namespaceKarana::Dynamics{

namespacekc=Karana::Core;

classPhysicalBody;
classHingePnode;
classCompoundHinge;
classCompoundSubhinge;

classCompoundBody:publickc::LockingBase,publicBodyBase{

/*foraccessto_physical_bodies_tree*/
friendSubTree;

/*foraccessto_pnodes_list*/
friendCompoundSubhinge;

/*foraccessto_base_onodes_list*/
friendPhysicalBody;

/*foraccesstoinverseDynamicsVectors()*/
friendclassAlgorithms;

public:
statickc::ks_ptr<CompoundBody>create(kc::ks_ptr<SubTree>parent_subtree,
conststd::string&name,
kc::ks_ptr<SubTree>component_bodies);

//FIXME:makeprotectedandmarkallocatorfunctionusedbyMultibodyasfriend
CompoundBody(conststd::string&name,
kc::ks_ptr<SubTree>parent_subtree,
kc::ks_ptr<SubTree>component_bodies);

virtual~CompoundBody();

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

#if1
constkc::id_t&id()constoverride{returnKarana::Core::LockingBase::id();}

conststd::string&name()constoverride{returnKarana::Core::LockingBase::name();}
#endif

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

constkc::ks_ptr<SubTree>&bodiesTree()const{return_bodies_tree;}

kc::ks_ptr<SubTree>physicalBodiesTree()const{
return_physical_bodies_tree?_physical_bodies_tree:_bodies_tree;
}

public:
kc::ks_ptr<BodyBase>getPtr()override{
returnkc::static_pointer_cast<CompoundBody>(Karana::Core::LockingBase::getPtr());
}

kc::ks_ptr<constBodyBase>getPtr()constoverride{
returnkc::static_pointer_cast<constCompoundBody>(Karana::Core::LockingBase::getPtr());
}

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

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

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

/*Returnthecompoundhingeforthiscompoundbody*/
kc::ks_ptr<HingeBase>parentHinge()constoverride;

virtualconstkc::ks_ptr<PhysicalBody>&physicalParentBody()constoverride{
return_physical_parent_body;
}

structdumpOptions:Karana::Core::LockingBase::dumpOptions{
boolcacheDeps=true;

dumpOptions&operator=(constdumpOptions&p){
if(this!=&p){
kc::LockingBase::dumpOptions::operator=(
p);//Callbaseclassassignmentoperator
//AssignDerived-specificmembershere
cacheDeps=p.cacheDeps;
}

return*this;
}
};

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

public:
constkc::ks_ptr<kc::DataCache<bool>>atbiMatrixCache()const{
return_atbi_data_caches.matrix_cache;
}

constkc::ks_ptr<kc::DataCache<bool>>atbiFilterCache()const{
return_atbi_data_caches.filter_cache;
}

constkc::ks_ptr<kc::DataCache<bool>>atbiSmootherCache()const{
return_atbi_data_caches.smoother_cache;
}

constkc::ks_ptr<kc::DataCache<bool>>upsilonMatrixCache()const{
return_atbi_data_caches.upsilon_matrix_cache;
}

protected:
structInvDynVectors{

km::Vecf;
};

structATBIDataCaches{
kc::ks_ptr<kc::DataCache<bool>>matrix_cache=nullptr;
kc::ks_ptr<kc::DataCache<bool>>filter_cache=nullptr;
kc::ks_ptr<kc::DataCache<bool>>smoother_cache=nullptr;
kc::ks_ptr<kc::DataCache<bool>>upsilon_matrix_cache=nullptr;
};

//public:

constkc::ks_ptr<kc::DataCache<InvDynVectors>>inverseDynamicsCache()const{
return_invdyn_data_cache;
}

constInvDynVectors&inverseDynamicsVectors()const{
returninverseDynamicsCache()->get();
}

protected:
virtualkc::ks_ptr<CompoundSubhinge>_createSubhinge(kc::ks_ptr<CompoundHinge>hinge);

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

std::vector<kc::ks_ptr<PhysicalBody>>_physicalBodiesList()const;

protected:
void_setupChildBodyCacheDependencies(constBodyBase&child_body)override;
void_setupChildPhysicalBodyCacheDependencies(constPhysicalBody&child_body,
boolskip_scatter)override;

void_setupChildCompoundBodyCacheDependencies(constCompoundBody&child_body)override;

void_setupBaseBodyCacheDependencies()override;

void_teardownChildBodyCacheDependencies(constBodyBase&child_body)override;
void_teardownChildPhysicalBodyCacheDependencies(constPhysicalBody&child_body,
boolskip_scatter)override;

void_teardownChildCompoundBodyCacheDependencies(constCompoundBody&child_body)override;

void_teardownBaseBodyCacheDependencies()override;

protected:
void_initialSetup();
void_postSetup();

void_oneTimeSetupDataCaches();

void_oneTimeTeardownDataCaches();

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

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

void_computeInvDynForces(InvDynVectors&val);

km::SpatialVector_getBaseOnodeInvDynForces(kc::ks_ptr<HingeOnode>base_onode);

protected:
kc::ks_ptr<PhysicalBody>_physical_parent_body=nullptr;

kc::RegistryList<HingeOnode>_base_onodes_list;

kc::ks_ptr<SubTree>_bodies_tree=nullptr;

kc::ks_ptr<SubTree>_physical_bodies_tree=nullptr;

kc::ks_ptr<CompoundHinge>_hinge=nullptr;

std::map<kc::ks_ptr<PhysicalBody>,kc::RegistryList<HingeOnode>>_child_onodes_map;

kc::ks_ptr<kc::DataCache<InvDynVectors>>_invdyn_data_cache=nullptr;
ATBIDataCaches_atbi_data_caches;

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

mutablekm::Vec_getQ_cache;

mutablekm::Vec_getU_cache;

mutablekm::Vec_getUdot_cache;

mutablekm::Vec_getT_cache;
};

}//namespaceKarana::Dynamics