/**
* Reset this link back to its original state
*/
void CMultibodyLinkEntity::Reset()
{
// Call through to our super reset method
CComposableEntity::Reset();
// And reset
embodiedEntity->Reset();
// Restore our state
currentState = defaultState;
// Get our reset position (relative to our parent)
CVector3 pos{currentState.originX, currentState.originY, currentState.originZ};
// Get our orientation (relative to our parent)
CRadians roll{currentState.roll};
CRadians pitch{currentState.pitch};
CRadians yaw{currentState.yaw};
// And wrap it in a quaternion
CQuaternion orientation;
orientation.FromEulerAngles(yaw, pitch, roll);
// Set our position
embodiedEntity->GetOriginAnchor().Position = pos;
embodiedEntity->GetOriginAnchor().Orientation = orientation;
UpdateComponents();
}
CMultibodyLinkEntity::CMultibodyLinkEntity(CMultibodyEntity* parent, const Link &linkDef)
: CComposableEntity(parent, parent->GetId() + "_" + linkDef.name), defaultState(linkDef), currentState(linkDef)
{
// Get our orientation relative to our parent
CRadians roll{linkDef.roll};
CRadians pitch{linkDef.pitch};
CRadians yaw{linkDef.yaw};
// And wrap it in a quaternion
CQuaternion orientation;
orientation.FromEulerAngles(yaw, pitch, roll);
// Get our position relative to our parent
CVector3 pos{linkDef.originX, linkDef.originY, linkDef.originZ};
// Create our embodied entity
embodiedEntity = new CEmbodiedEntity{this, GetId(), pos, orientation};
embodiedEntity->SetEnabled(true);
embodiedEntity->SetMovable(true);
// And add the embodied component
AddComponent(*embodiedEntity);
Reset();
}
