Program Listing for File PhysicalBody.h

↰ Return to documentation for file (include/Karana/SOADyn/PhysicalBody.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/DataCache.h"
#include"Karana/Math/SpatialInertia.h"
#include"Karana/Math/SpatialVector.h"
#include"Karana/ProxyScene/ProxyScenePart.h"
#include"Karana/SOADyn/BodyBase.h"
#include"Karana/SOADyn/PhysicalHinge.h"
#include"Karana/Scene/ScenePartSpec.h"

namespaceKarana::Dynamics{

namespacekc=Karana::Core;
namespacekf=Karana::Frame;
namespaceks=Karana::Scene;

classNode;
classConstraintNode;
classHingePnode;
classHinge;
classMultibody;
classCompoundBody;
classLoopConstraintHinge;

structPhysicalBodyParams{
km::SpatialInertiasp_i=
km::SpatialInertia(2.0,
km::Vec3(0.3,0.2,0.1),
Eigen::DiagonalMatrix<double,3>(
3,2,1));
std::vector<km::Vec3>axes={{0,1,0}};
km::HomTranbody_to_joint_transform=
km::HomTran(km::UnitQuaternion(.8,.6,0,0),
km::Vec3(.2,.3,.4));
km::HomTraninb_to_joint_transform=km::HomTran(
km::UnitQuaternion(.5,.5,.5,.5),
km::Vec3(.4,.2,.3));
std::vector<ks::ScenePartSpec>scene_part_specs=
{};

PhysicalBodyParams&operator=(constPhysicalBodyParams&p){
if(this!=&p){
//AssignDerived-specificmembershere
sp_i=p.sp_i;
//htype=p.htype;
axes=p.axes;
body_to_joint_transform=p.body_to_joint_transform;
inb_to_joint_transform=p.inb_to_joint_transform;
scene_part_specs=p.scene_part_specs;
}

return*this;
}
};

classPhysicalBody:publickf::Frame,publicBodyBase{

/*socanaccess_newtonian2body_f2f*/
friendclassMultibody;

//foraccessto_invdyn_cache
friendclassPhysicalHinge;

//foraccessto_spIetcforinversedynamics
friendclassHingePnode;

//foraccessto_child_onodes_list
friendclassSubTree;
friendclassCompoundBody;

//foraccessto_constraint_nodes_list
friendclassSubGraph;

//foraccessto_get/_setBodyToNodeTransforms
friendclassNode;

//foraccessto_getOnodeATBISmootherAlphaPlus
friendclassHingeOnode;

//foraccessto_setupNode
friendclassConstraintNode;

//foraccessto_isInboardOf
friendclassCompoundSubhinge;

//foraccessto_atbi_filter_include_gravityetc
friendclassAlgorithms;

public:
PhysicalBody(std::string_viewname,kc::ks_ptr<Multibody>mb);

virtual~PhysicalBody();

statickc::ks_ptr<PhysicalBody>create(std::string_viewname,kc::ks_ptr<Multibody>mb);

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

constkc::id_t&id()constoverride{returnkf::Frame::id();}


std::string_viewname()constoverride{returnkf::Frame::name();};

boolisRootBody()constoverride;

virtualvoidsetParams(constPhysicalBodyParams&params);

staticstd::vector<kc::ks_ptr<PhysicalBody>>
addSerialChain(std::string_viewname,
size_tnbodies,
PhysicalBody&root,
HingeBase::HingeTypehtype=HingeBase::HingeType::PIN,
PhysicalBodyParams*params=nullptr);

staticstd::vector<kc::ks_ptr<PhysicalBody>>
addTree(std::string_viewname,
size_tbranch_length,
size_tnbranches,
size_tdepth,
PhysicalBody&root,
HingeBase::HingeTypehtype=HingeBase::HingeType::PIN,
PhysicalBodyParams*params=nullptr);

public:
voidsetSpatialInertia(constkm::SpatialInertia&sp_i,
constkc::ks_ptr<kf::Frame>&ref_frame=nullptr);

constkm::SpatialInertia&getSpatialInertia()const;

virtualvoidsetBodyToJointTransform(constkm::HomTran&T);

virtualkm::HomTrangetBodyToJointTransform()const;

constkm::SpatialVectorbodyObservedSpatialAccel()const;

km::SpatialVectorexternalSpatialForce(boolwith_constraints)const;

voidsplitSubhinges();

voidinsertDummyParentBody();

voidreattach(PhysicalBody&new_parent,
HingeBase::HingeTypehinge_type=HingeBase::HingeType::FULL6DOF);

voiddetach();

kc::ks_ptr<ks::ProxyScenePart>getScenePart(std::string_viewnm)const;

conststd::vector<kc::ks_ptr<ks::ProxyScenePart>>&getSceneParts()const;

conststd::vector<ks::ScenePartSpec>&getScenePartSpecs()const;

kc::ks_ptr<LoopConstraintHinge>toLoopConstraint();

voidshiftBaseBody();

constkc::ks_ptr<HingePnode>&pnode()const;

constkc::ks_ptr<HingeOnode>&onode()const;

constkc::ks_ptr<PhysicalBody>&physicalParentBody()constoverride;

kc::ks_ptr<HingeBase>parentHinge()constoverride;

kc::ks_ptr<kf::OrientedChainedFrameToFrame>newtonianToBodyFrameToFrame()const{
return_newtonian2body_f2f;
}

conststd::vector<kc::ks_ptr<Node>>&nodeList()const;

conststd::vector<kc::ks_ptr<ConstraintNode>>&constraintNodeList()const;

kc::ks_ptr<Node>getNode(std::string_viewnode_name);

kc::ks_ptr<ConstraintNode>getConstraintNode(std::string_viewnode_name);

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

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

constkm::Vec3&getGravAccel(){return_grav_accel_cache->get();}

voidsetGravityGradient(constkm::Vec3&grav_gradient,
constkc::ks_ptr<kf::Frame>&ref_frame=nullptr);

voidaccumGravityGradient(constkm::Vec3&grav_gradient,
constkc::ks_ptr<kf::Frame>&ref_frame=nullptr);

virtualkm::Mat66getNodeUpsilonFromPnode(constNode&nd);

virtualkm::Mat66getNodeUpsilonFromBody(constNode&node);

structDumpOptions:Karana::Frame::Frame::DumpOptions{
//Addingsuppression,asCodeCheckercomplainsthismethodhasthesamenameasa
//methodinthebaseversion.ThisisjustaCodeCheckerfalsepositive.
//codechecker_suppress[cppcheck-duplInheritedMember]
DumpOptions&operator=(constDumpOptions&p){
if(this!=&p){
Frame::DumpOptions::operator=(p);//Callbaseclassassignmentoperator
//AssignDerived-specificmembershere
cache_deps=p.cache_deps;
}

return*this;
}
};

std::stringdumpString(std::string_viewprefix,
constKarana::Core::Base::DumpOptions*options)constoverride;

boolisFinalized()constoverride;

virtualdoublekineticEnergy()const;

virtualkm::SpatialVectorspatialMomentum()const;

km::MatgetUpsilonMatrix()override;

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

voidaddScenePartSpec(constks::ScenePartSpec&scene_part);

voiddiscardAllNodes();

protected:
structInvDynVectors{
km::SpatialVectorf;
};

structGatherSweepFlags{
boolgravity=true;

boolexternal_forces=true;

boolconstraint_forces=true;

boolelastic_forces=true;

boolmodal_gen_forces=false;
};

protected:
GatherSweepFlags&_gatherSweepFlags(){return_gather_sweep_flags;}

void_trackUsageNode(constkc::ks_ptr<Node>&);

virtualvoid_moveNode(constkc::ks_ptr<Node>&node,constkm::HomTran&T);

kc::ks_ptr<PhysicalBody>_asSharedPtr();

void_realizeScenePart(constks::ScenePartSpec&scene_part);

virtualvoid_detachNode(kc::ks_ptr<Node>&nd);

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

virtualvoid_setupNode(Node&node,boolforce_node);

void_processScenePartSpecs();

virtualkm::Mat66_getPnodeMassMatrix()const;

virtualvoid_getPnodeCRBInertiaMatrix(km::Mat&R);

virtualvoid_setBodyToNodeTransform(Node&node,constkm::HomTran&t);
virtualkm::HomTran_getBodyToNodeTransform(constNode&node)const;

virtualvoid_computeGyroscopicForce(km::SpatialVector&)const;

//virtualkm::Mat66_psiToChildNode(kc::ks_ptr<Node>&node);

virtualkm::Mat66_pnodeToChildNodeTransform(constNode&node);

virtualkm::Mat_bodyToChildNodeTransform(constNode&node,
constkf::Frame&from_frame)const;

virtualkm::Mat_pnodeToBodyPsi()const;

void_clearExternals();

protected:
virtualkc::ks_ptr<HingePnode>_createPnode(boolfloating_frame);

virtualkc::ks_ptr<HingeOnode>_createOnode(std::string_viewname);

virtualvoid_discardHingePnode();
virtualvoid_discardHingeOnode(kc::ks_ptr<HingeOnode>&onode);

virtualvoid_registerChildOnode(kc::ks_ptr<HingeOnode>node);
virtualvoid_unregisterChildOnode(kc::ks_ptr<HingeOnode>node);

protected:
virtualkm::SpatialVector_getPnodeATBIFilterZ();
virtualkm::Mat66_getPnodeATBIMatricesP();
virtualkm::SpatialVector_getOnodeATBISmootherAlphaPlus(constHingeOnode&ond);

virtualvoid_processPnodeATBISmootherVectors();
virtualkm::Mat66_processPnodeUpsilonMatrices();

virtualkm::SpatialVector_getPnodeInvDynForcesF();

protected:
/*setupthecachedependenciesbetweenaphysicalbodyandits
childphysicalbody*/
void_setupChildBodyCacheDependencies(constBodyBase&child_body)override;
void_setupChildPhysicalBodyCacheDependencies(constPhysicalBody&child_body,
boolskip_scatter)override;

/*setupthecachedependenciesbetweenaphysicalbodyandits
childcompoundbody*/
void_setupChildCompoundBodyCacheDependencies(constCompoundBody&child_body)override;

void_setupBaseBodyCacheDependencies()override;

/*setupthecachedependenciesbetweenaphysicalbodyandits
childphysicalbody*/
void_teardownChildBodyCacheDependencies(constBodyBase&child_body)override;
void_teardownChildPhysicalBodyCacheDependencies(constPhysicalBody&child_body,
boolskip_scatter)override;

/*setupthecachedependenciesbetweenaphysicalbodyandits
childcompoundbody*/
void_teardownChildCompoundBodyCacheDependencies(constCompoundBody&child_body)override;

void_teardownBaseBodyCacheDependencies()override;

protected:
km::SpatialInertia_spI;

km::Vec3_grav_accel_body_frame;

km::Vec3_grav_gradient_body_frame;

kc::ks_ptr<HingePnode>_parent_pnode=nullptr;

//f2ffromthenewtonianframetothisbody'sframe
kc::ks_ptr<kf::OrientedChainedFrameToFrame>_newtonian2body_f2f=nullptr;

GatherSweepFlags_gather_sweep_flags;

std::vector<ks::ScenePartSpec>_scene_part_specs;

std::vector<kc::ks_ptr<ks::ProxyScenePart>>_scene_part_spec_parts;

protected:
kc::ks_ptr<kc::DataCache<km::SpatialInertia>>_spI_cache=nullptr;

kc::ks_ptr<kc::DataCache<km::Vec3>>_grav_accel_cache=nullptr;

kc::ks_ptr<kc::DataCache<km::SpatialVector>>_gyroscopic_force_cache=nullptr;

protected:
kc::RegistryList<HingeOnode>_child_onodes_list;

protected:
kc::RegistryList<Node>_nodes_list;
kc::RegistryList<Node>_external_force_nodes_list;
kc::RegistryList<Node>_contact_force_nodes_list;

size_t_active_contact_force_node_index=0;
kc::RegistryList<ConstraintNode>_constraint_nodes_list;
};

}//namespaceKarana::Dynamics