Program Listing for File SubTree.h

↰ Return to documentation for file (include/Karana/SOADyn/SubTree.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/KCore/LockingBase.h"
#include"Karana/KCore/Tree.h"
#include"Karana/KCore/UsageTrackingMap.h"
#include"Karana/SOADyn/CoordData.h"
#include"Karana/SOADyn/PhysicalBody.h"
#include"Karana/SOADyn/SubhingeBase.h"

namespaceKarana::Dynamics{

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

classBodyBase;
classCompoundBody;
classMultibody;
classPhysicalBody;
classCoordData;

classSubTree:publickc::LockingBase{

/*foraccessto_unsorted_bodies_list*/
friendclassCompoundBody;
friendclassCompoundSubhinge;

/*foraccessto_body2parent_map*/
friendclassPhysicalBody;

/*foraccesstogetPsiMatrix...*/
friendclassHingePnode;

/*foraccessto_clearExternals*/
friendclassStatePropagator;

/*foraccessto_getMinvBlock*/
friendclassAlgorithms;

public:
SubTree(std::string_viewname,
constkc::ks_ptr<BodyBase>&root,
constkc::ks_ptr<SubTree>&parent_subtree=nullptr);

virtual~SubTree();

std::string_viewtypeString()constnoexceptoverride{
staticstd::stringtype_string="Karana::Dynamics::SubTree";
returntype_string;
}

virtualboolisSubGraph()const{returnfalse;}

virtualconstkc::ks_ptr<BodyBase>&virtualRoot()const{return_virtual_root_body;}

public:
constkc::ks_ptr<BodyBase>&parentBody(constBodyBase&body)const;

conststd::vector<kc::ks_ptr<BodyBase>>&baseBodies()const{return_base_bodies;}

conststd::vector<kc::ks_ptr<BodyBase>>&leafBodies()const{return_leaf_bodies;}

kc::ks_ptr<BodyBase>ancestorBody(constBodyBase&bd,constBodyBase&bd1)const;

boolcontainsBody(constBodyBase&bd)const;

boolhasBody(constBodyBase&bd)const;

kc::ks_ptr<BodyBase>getBody(std::string_viewname)const;

std::vector<kc::ks_ptr<BodyBase>>getBodies(std::string_viewname)const;

std::vector<kc::ks_ptr<BodyBase>>childrenBodies(constBodyBase&bd)const;

boolisBaseBody(constBodyBase&bd)const;

size_tnumBodies()const;

public:
virtualconststd::pair<kc::ks_ptr<CoordBase>,size_t>coordAt(size_tu_offset)const;

CoordData::CoordOffsetcoordOffsets(constCoordBase&c)const;

size_tnQ()const;

size_tnU()const;

km::VecgetQ()const;

voidsetQ(constEigen::Ref<constkm::Vec>&Q);

voidsetQ(doublefill_value);

km::VecgetQdot()const;

km::VecgetU()const;

voidsetU(constEigen::Ref<constkm::Vec>&U);

voidsetU(doublefill_value);

km::VecgetUdot()const;

void
setUdot(constEigen::Ref<constkm::Vec>&Udot);//NOLINT(readability-identifier-naming)

voidsetUdot(doublefill_value);

km::VecgetT()const;

voidsetT(constEigen::Ref<constkm::Vec>&T);

voidsetT(doublefill_value);

public:
statickc::ks_ptr<SubTree>
create(constkc::ks_ptr<SubTree>&parent_subtree,
std::string_viewname,
kc::ks_ptr<BodyBase>new_root,
conststd::vector<kc::ks_ptr<BodyBase>>&use_branches={},
conststd::vector<kc::ks_ptr<BodyBase>>&stop_at={});

constkc::ks_ptr<Multibody>&multibody()const{return_multibody;}

public:
km::MatbodyPnodesOSCM(conststd::vector<kc::ks_ptr<BodyBase>>&bodies);

km::MatbodyOSCM(conststd::vector<kc::ks_ptr<BodyBase>>&bodies);

km::MatframesOSCM(conststd::vector<kc::ks_ptr<kf::Frame>>&body_frames,
conststd::vector<kc::ks_ptr<PhysicalSubhinge>>&subhinges={});

public:
virtualvoidshowState(constkm::Vec&x);

virtualvoidenableSubhingeCharts(boolflag);

boolsubhingeChartsEnabled()const{return_enabled_subhinge_charts;};

public:
std::string
dumpString(std::string_viewprefix="",
constKarana::Core::Base::DumpOptions*options=nullptr)constoverride;

virtualvoiddisplayModel(std::string_viewprefix="")const;

structDumpTreeOptions{
//booltypeString=false;///<includethebody'stypeStringinfo
boolhinge_type=true;
boollock_status=false;
boolref_count=false;
boolhinge_ref_count=false;
boolid=false;
};

virtualvoiddumpTree(std::string_viewprefix="",
constDumpTreeOptionsoptions=
DumpTreeOptions(true,false,false,false,false))const;

constkc::ks_ptr<CoordData>&subhingeCoordData()const;

virtualconstkc::ks_ptr<CoordData>treeCoordData()const;

constkc::ks_ptr<CoordData>bodyCoordData()const{return_body_coord_data;}

boolhasCompoundBodies()const{return_has_compound_bodies;}

conststd::vector<kc::ks_ptr<PhysicalBody>>&sortedPhysicalBodiesList()const;

conststd::vector<kc::ks_ptr<BodyBase>>&sortedBodiesList()const;

voidenableAlgorithmicUse();

voiddisableAlgorithmicUse();

voidsetUniformGravAccel(constkm::Vec3&g,kc::ks_ptr<kf::Frame>ref_frame=nullptr);

voidaccumUniformGravAccel(constkm::Vec3&g,kc::ks_ptr<kf::Frame>ref_frame=nullptr);

constkc::ks_ptr<kf::Frame>&cmFrame()const{return_cm_frame;}

public:
voidarticulateSubhinge(SubhingeBase&shg,
size_tsubhinge_index,
doublerange_q=.5,
doubled_q=.01,
doublepause=.01,
std::function<void()>cb=nullptr)const;

voidarticulateBodies(doublerange_q=.5,doubled_q=.01,doublepause=.01)const;

boolsanitizedCoords();

virtualvoidresetData();

km::Mat66crossUpsilonMatrix(constHingePnode&left,constHingePnode&right);

km::MatgetCrossUpsilonMatrix(constPhysicalBody&lbd,constPhysicalBody&rbd);

public:
km::MatgetBodyPairPnodePsiMatrix(constBodyBase&left,constBodyBase&right);

km::MatgetBodyPairPsiMatrix(constBodyBase&left,constBodyBase&right);

km::MatgetBodyPairPhiMatrix(constBodyBase&left,constBodyBase&right);

constkm::Mat&getCoordBasePairPsiMatrix(constCoordBase&left,constCoordBase&right);

constkm::Mat&getCoordBasePairOframePsiMatrix(constCoordBase&left,
constCoordBase&right);

constkm::Mat&getCoordBasePairPhiMatrix(constCoordBase&left,constCoordBase&right);

voiddumpDynamics(std::string_viewprefix="")const;

boolenable_dump_dynamics=false;

protected:
virtualvoid_stateToDynamics(constkm::Vec&x,boolglobal=false);

virtualvoid_dynamicsToState(Eigen::Ref<km::Vec>x)const;

virtualkm::Vec_dynamicsToState()const;

virtualvoid_dynamicsToStateDeriv(Eigen::Ref<km::Vec>dx)const;

virtualkm::Vec_dynamicsToStateDeriv()const;

virtualstd::vector<kc::ks_ptr<CoordData>>_coordDataList()const;

void_localChartSetQ(constEigen::Ref<constkm::Vec>&val);

km::Vec_localChartGetQ()const;

virtualvoid_resetSubhingeCharts();

virtualCoordData::CoordOffset_coordOffsets(kc::id_tid)const;

km::Mat_getCrossUpsilonMatrix(CoordBase&lsh,CoordBase&rsh);

km::Mat_getMinvBlock(conststd::vector<kc::ks_ptr<CoordBase>>&cbs);

km::Mat_getMinvBlock(CoordBase&rcb,CoordBase&ccb);

km::Mat_getMinvBlock(CoordBase&rcb,HingePnode&pnode);

km::Mat_getMinvOSCMBlock(conststd::vector<kc::ks_ptr<kf::Frame>>&body_frames,
conststd::vector<kc::ks_ptr<Node>>&bdnds,
conststd::vector<kc::ks_ptr<PhysicalSubhinge>>&shgs);

virtualvoid_clearExternals();

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

constkc::ks_ptr<kc::DataCache<km::Mat>>&_lookupOrCreateCoordBasePairPhiCache(
constCoordBase&left,constCoordBase&right,constCoordData&cd);
void_trackUsageCoordBasePhiCache(conststd::pair<kc::id_t,kc::id_t>&key,
constkc::ks_ptr<kc::DataCache<km::Mat>>&phi_cache);

constkc::ks_ptr<kc::DataCache<km::Mat>>&_lookupOrCreateCoordBasePairPsiCache(
constCoordBase&left,constCoordBase&right,constCoordData&cd);
void_trackUsageCoordBasePsiCache(conststd::pair<kc::id_t,kc::id_t>&key,
constkc::ks_ptr<kc::DataCache<km::Mat>>&psi_cache);

constkc::ks_ptr<kc::DataCache<km::Mat>>&_lookupOrCreateCoordBasePairOframePsiCache(
constCoordBase&left,constCoordBase&right,constCoordData&cd);
void
_trackUsageCoordBaseOframePsiCache(conststd::pair<kc::id_t,kc::id_t>&key,
constkc::ks_ptr<kc::DataCache<km::Mat>>&psi_cache);

void_computeBodyPairPnodePsi(km::Mat&val,constBodyBase&left,constBodyBase&right);

void_computeBodyPairPsi(km::Mat&val,constBodyBase&left,constBodyBase&right);

void_computeBodyPairPhi(km::Mat&val,constBodyBase&left,constBodyBase&right);

void_computeCoordBasePairPhi(km::Mat&val,
constCoordBase&left,
constCoordBase&right,
constCoordData&cd);

void_computeCoordBasePairPsi(km::Mat&val,
constCoordBase&left,
constCoordBase&right,
constCoordData&cd);

void_computeCoordBasePairOframePsi(km::Mat&val,
constCoordBase&left,
constCoordBase&right,
constCoordData&cd);

void_dumpTree(
std::string_viewprefix="",
constDumpTreeOptionsoptions=DumpTreeOptions(false,false,false,false,false),
std::map<kc::id_t,std::string>extras={})const;

protected:
void_trackUsageCompoundBody(kc::ks_ptr<CompoundBody>bd);

void_setupTree();

void_setupCoordSanitizationSubhinges();

virtualvoid_makeStale()override;

void_discardAllSubTrees();

virtualvoid_discardCmFrame();

/*Returnafilteredlistofchildonodeswhoseparentbodies
belongtothissub-tree.Thislistwillbeusedinthegather
sweepsforthissub-tree*/
std::vector<kc::ks_ptr<HingeOnode>>
_filteredChildOnodes(constkc::RegistryList<Karana::Dynamics::HingeOnode>&onodes)const;

virtualvoid_registerBody(constkc::ks_ptr<BodyBase>&body);

virtualvoid_unregisterBody(constkc::ks_ptr<BodyBase>&body);

bool_areDisjoint(constSubTree&other)const;

void_populateNewChildTreeWithBodies(kc::ks_ptr<SubTree>child_subtree,
conststd::vector<kc::id_t>&branch_ids,
conststd::vector<kc::ks_ptr<BodyBase>>&stop_at);

/*helpermethodtocreatedsortedphysicalbodieslistin
_sorted_physical_bodies*/
void_setupSortedPhysicalBodiesList();

/*helpermethodtotopolgicallysortthesubtree'sbodiesto
populate_sorted_bodieslist*/
void_setupSortedBodiesList();

km::HomTran_computeCMPose();
km::SpatialVector_computeCMSpVel();
km::SpatialVector_computeCMSpAccel();

km::Mat_framesOSCM(conststd::vector<kc::ks_ptr<kf::Frame>>&body_frames,
conststd::vector<kc::ks_ptr<Node>>&bdnds,
conststd::vector<kc::ks_ptr<BodyBase>>&bbds);

km::Mat_framesSubhingesOSCM(conststd::vector<kc::ks_ptr<kf::Frame>>&body_frames,
conststd::vector<kc::ks_ptr<Node>>&bdnds,
conststd::vector<kc::ks_ptr<BodyBase>>&bbds,
conststd::vector<kc::ks_ptr<PhysicalSubhinge>>&shgs={},
size_tshg_dofs=0);

protected:
kc::ks_ptr<Multibody>_multibody;

kc::ks_ptr<BodyBase>_virtual_root_body=nullptr;

kc::RegistryList<BodyBase>_unsorted_bodies_list;

std::vector<kc::ks_ptr<BodyBase>>_sorted_bodies;

kc::ks_ptr<SubTree>_parent_subtree=nullptr;

kc::ks_ptr<kf::Frame>_cm_frame=nullptr;

kc::ks_ptr<CoordData>_subhinge_coord_data=nullptr;

kc::ks_ptr<CoordData>_body_coord_data=nullptr;

mutablekc::ks_ptr<CoordData>_tree_coord_data=nullptr;

protected:
kc::UsageTrackingMap<kc::id_t, SubTree>_child_subtrees_usage_map;

kc::UsageTrackingMap<kc::id_t, CompoundBody>_compound_bodies_usage_map;

kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>,kc::DataCache<km::Mat>>
_coordbase_phi_cache_usage_map;

kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>,kc::DataCache<km::Mat>>
_coordbase_psi_cache_usage_map;

kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>,kc::DataCache<km::Mat>>
_coordbase_oframe_psi_cache_usage_map;

std::map<kc::id_t,kc::ks_ptr<BodyBase>>_body2parent_map;

std::vector<kc::ks_ptr<BodyBase>>_base_bodies;
std::vector<kc::ks_ptr<BodyBase>>_leaf_bodies;

std::vector<kc::ks_ptr<PhysicalBody>>_sorted_physical_bodies;

std::vector<kc::ks_ptr<PhysicalSubhinge>>_coord_sanitization_subhinges;

mutablestd::unique_ptr<constkc::Tree<kc::id_t>>_tree;

bool_has_compound_bodies=false;

bool_enabled_algorthmic=false;

bool_enabled_subhinge_charts=true;

/*prefixusedbydumpDynamics()ifnoneisspecfied*/
std::string_dump_dynamics_prefix="";
};

}//namespaceKarana::Dynamics