void CRABEquippedEntity::Update() {
CVector3 cPos = m_cPosOffset;
cPos.Rotate(m_psAnchor->Orientation);
cPos += m_psAnchor->Position;
SetPosition(cPos);
SetOrientation(m_psAnchor->Orientation * m_cRotOffset);
}
CRABEquippedEntity::CRABEquippedEntity(CComposableEntity* pc_parent,
const std::string& str_id,
size_t un_msg_size,
Real f_range,
const SAnchor& s_anchor,
CEmbodiedEntity& c_entity_body,
const CVector3& c_pos_offset,
const CQuaternion& c_rot_offset) :
CPositionalEntity(pc_parent,
str_id),
m_psAnchor(&s_anchor),
m_cPosOffset(c_pos_offset),
m_cRotOffset(c_rot_offset),
m_cData(un_msg_size),
m_fRange(f_range),
m_pcEntityBody(&c_entity_body) {
Disable();
SetCanBeEnabledIfDisabled(false);
CVector3 cPos = c_pos_offset;
cPos.Rotate(s_anchor.Orientation);
cPos += s_anchor.Position;
SetInitPosition(cPos);
SetPosition(cPos);
SetInitOrientation(s_anchor.Orientation * c_rot_offset);
SetOrientation(GetInitOrientation());
}
void CCylinderEntity::UpdateComponents() {
/* Set LED position and update led equipped entity */
CVector3 cLEDPosition;
for(UInt32 i = 0; i < m_pcLEDEquippedEntity->GetAllLeds().size(); ++i) {
cLEDPosition = m_vecBaseLEDPositions[i];
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
cLEDPosition += GetEmbodiedEntity().GetPosition();
m_pcLEDEquippedEntity->SetLedPosition(i, cLEDPosition);
}
m_pcEmbodiedEntity->UpdateBoundingBox();
}
void CTagEquippedEntity::UpdateComponents() {
/* Tag position wrt global reference frame */
CVector3 cTagPosition;
CQuaternion cTagOrientation;
for(SInstance& s_instance : m_vecInstances) {
if(s_instance.Tag.IsEnabled()) {
cTagPosition = s_instance.PositionOffset;
cTagPosition.Rotate(s_instance.Anchor.Orientation);
cTagPosition += s_instance.Anchor.Position;
cTagOrientation = s_instance.Anchor.Orientation *
s_instance.OrientationOffset;
s_instance.Tag.MoveTo(cTagPosition, cTagOrientation);
}
}
}
void CDirectionalLEDEquippedEntity::UpdateComponents() {
/* LED position wrt global reference frame */
CVector3 cLEDPosition;
CQuaternion cLEDOrientation;
for(SInstance& s_instance : m_vecInstances) {
if(s_instance.LED.IsEnabled()) {
cLEDPosition = s_instance.PositionOffset;
cLEDPosition.Rotate(s_instance.Anchor.Orientation);
cLEDPosition += s_instance.Anchor.Position;
cLEDOrientation = s_instance.Anchor.Orientation *
s_instance.OrientationOffset;
s_instance.LED.MoveTo(cLEDPosition, cLEDOrientation);
}
}
}
SAnchor& CEmbodiedEntity::AddAnchor(const std::string& str_id,
const CVector3& c_offset_position,
const CQuaternion& c_offset_orientation) {
/* Make sure the anchor id is unique */
if(m_mapAnchors.count(str_id) > 0 ) {
THROW_ARGOSEXCEPTION("Embodied entity \"" << GetContext() + GetId() << "\" already has an anchor with id " << str_id);
}
/* Calculate anchor position */
CVector3 cPos = c_offset_position;
cPos.Rotate(m_psOriginAnchor->Orientation);
cPos += m_psOriginAnchor->Position;
/* Calculate anchor orientation */
CQuaternion cOrient = m_psOriginAnchor->Orientation * c_offset_orientation;
/* Create anchor */
SAnchor* psAnchor = new SAnchor(str_id,
m_mapAnchors.size(),
c_offset_position,
c_offset_orientation,
cPos,
cOrient);
/* Add anchor to map */
m_mapAnchors[str_id] = psAnchor;
return *psAnchor;
}
void CFootBotEntity::SetLedPosition() {
/* Set LED positions */
const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
CVector3 cLEDPosition;
CRadians cLEDAnglePhase = FOOTBOT_HALF_LED_ANGLE_SLICE + m_cTurretRotation;
CRadians cLEDAngle;
SET_RING_LED_POSITION(0);
SET_RING_LED_POSITION(1);
SET_RING_LED_POSITION(2);
SET_RING_LED_POSITION(3);
SET_RING_LED_POSITION(4);
SET_RING_LED_POSITION(5);
SET_RING_LED_POSITION(6);
SET_RING_LED_POSITION(7);
SET_RING_LED_POSITION(8);
SET_RING_LED_POSITION(9);
SET_RING_LED_POSITION(10);
SET_RING_LED_POSITION(11);
/* Set beacon position */
cLEDPosition.Set(0.0f, 0.0f, FOOTBOT_BEACON_ELEVATION);
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
cLEDPosition += cEntityPosition;
m_pcLEDEquippedEntity->SetLedPosition(12, cLEDPosition);
}
