void
consume(const IMC::LblConfig* msg)
{
if (msg->op == IMC::LblConfig::OP_SET_CFG)
{
// Save message to cache.
IMC::CacheControl cop;
cop.op = IMC::CacheControl::COP_STORE;
cop.message.set(*msg);
dispatch(cop);
Memory::replace(m_lbl_cfg, new IMC::LblConfig(*msg));
if (hasOrigin())
updateBeacons();
else
setEntityState(IMC::EntityState::ESTA_BOOT, Status::CODE_WAIT_GPS_FIX);
}
else if (msg->op == IMC::LblConfig::OP_GET_CFG)
{
IMC::LblConfig cfg;
cfg.op = IMC::LblConfig::OP_CUR_CFG;
if (m_lbl_cfg != NULL)
cfg.beacons = m_lbl_cfg->beacons;
dispatch(cfg);
}
}
bool SteerLib::GJK_EPA::GJK(const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB, std::vector<Util::Vector>& simplex)
{
Util::Vector d(1, 0, 0);
simplex.push_back(Support(_shapeA, _shapeB, d));
// Negates the vector
d = -d;
while (true)
{
simplex.push_back(Support(_shapeA, _shapeB, d));
//simplex.add
if ((simplex[simplex.size()-1]*d <= 0.0f)) // vector did not pass the origin, it is impossible for the origin to be in the mink. diff.
{
return false;
} else {
if (hasOrigin(simplex,d))
{
return true;
}
}
}
}
bool
isInitialized(void) const
{
return hasOrigin() && hasLBLConfig() && (m_beacons.size() > 0);
}
