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/KCore/UsageTrackingMap.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::SpatialInertiaspI=
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
spI=p.spI;
//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:
//FIXME:makeprotectedandmarkallocatorfunctionusedby
//Multibodyasfriend
PhysicalBody(conststd::string&name,kc::ks_ptr<Multibody>mb);

virtual~PhysicalBody();

statickc::ks_ptr<PhysicalBody>create(conststd::string&name,kc::ks_ptr<Multibody>mb);

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

#if1
/*cannotuseoverridebecauseofdiamondinheritancefromkc::Base*/
constkc::id_t&id()constoverride{returnkf::Frame::id();}


conststd::string&name()constoverride{returnkf::Frame::name();};

//voiddiscard();

#endif

boolisRootBody()const;

virtualvoidsetParams(constPhysicalBodyParams&params);

staticstd::vector<kc::ks_ptr<PhysicalBody>>
addSerialChain(conststd::string&name,
size_tnbodies,
PhysicalBody&root,
HingeBase::HINGE_TYPEhtype=HingeBase::HINGE_TYPE::PIN,
PhysicalBodyParams*params=nullptr);

staticstd::vector<kc::ks_ptr<PhysicalBody>>
addTree(conststd::string&name,
size_tbranch_length,
size_tnbranches,
size_tdepth,
PhysicalBody&root,
HingeBase::HINGE_TYPEhtype=HingeBase::HINGE_TYPE::PIN,
PhysicalBodyParams*params=nullptr);

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

constkm::SpatialInertia&getSpatialInertia()const;

virtualvoidsetBodyToJointTransform(constkm::HomTran&t);

virtualkm::HomTrangetBodyToJointTransform()const;

constkm::SpatialVectorbodyObservedSpatialAccel()const;

voidsplitSubhinges();

voidreattach(PhysicalBody&new_parent,
HingeBase::HINGE_TYPEhinge_type=HingeBase::HINGE_TYPE::FULL6DOF);

voiddetach();

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::OrientedChainedFrame2Frame>newtonian2BodyF2F()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{
boolcacheDeps=true;

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

return*this;
}
};

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

voiddump(conststd::string&prefix="",constdumpOptions*options=nullptr)const{
std::cout<<dumpString(prefix,options);
}

boolisFinalized()constoverride;

virtualdoublekineticEnergy()const;

virtualkm::SpatialVectorspatialMomentum()const;

km::MatgetUpsilonMatrix()override;

//virtualkm::MatgetCrossUpsilonMatrix(constPhysicalBody&other,SubTree&st);

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

kc::ks_ptr<kf::Frame2Frame>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;}

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

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

kc::ks_ptr<PhysicalBody>asSharedPtr();

kc::ks_ptr<BodyBase>getPtr()override;
#if0
{
returnkc::static_pointer_cast<PhysicalBody>(kf::Frame::getPtr());
}
#endif

kc::ks_ptr<constBodyBase>getPtr()constoverride;
#if0
{
returnkc::static_pointer_cast<constPhysicalBody>(kf::Frame::getPtr());
}
#endif

void_realizeScenePart(constks::ScenePartSpec&scene_part);

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

/*cannotuseoverridebecauseofdiamondinheritancefromkc::Base*/
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);

//virtualkm::Mat_pnodeCRBInertiaMarix()const;

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_pnode2BodyPsi()const;

void_clearExternals();

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

virtualkc::ks_ptr<HingeOnode>_createOnode(conststd::string&name);

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::OrientedChainedFrame2Frame>_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;
};

//externtemplate<PhysicalBody>voiddiscard(ks_ptr<PhysicalBody>&obj);

}//namespaceKarana::Dynamics