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); }