void BPlusTree::remove(int val) { if (!root) { // tree is empty throw new std::exception(); } BTreeNode *tp = root; while (!tp->isLeaf) { for(ListNode* lp = tp->children.begin(); lp->next; lp = lp->next) { if (val < lp->index) { tp = lp->child; break; } } } tp->children.remove(val); // if less then A*D keys are in this Node take fuse if (tp->children.getLength() <= D*A) { if (tp->lbro && tp->lbro->children.getLength() > D*A+1) { share(tp, tp->lbro); } else if (tp->rbro && tp->rbro->children.getLength() > D*A+1) { share(tp, tp->rbro); } else if (tp->lbro && tp->lbro->children.getLength() <= D*A+1) { fuse(tp, tp->lbro); } else if (tp->rbro && tp->rbro->children.getLength() <= D*A+1) { fuse(tp, tp->rbro); } else { for(ListNode *lp = tp->children.begin(); lp; lp = lp->next) { tp->parent->children.insert(lp->index, lp->child); } } } }
/* * whgtchk: * See if the hero can carry his pack */ void wghtchk(void) { reg int dropchk, err = TRUE; reg int ch; inwhgt = TRUE; if (pstats.s_pack > pstats.s_carry) { ch = CCHAR( mvwinch(stdscr, hero.y, hero.x) ); if((ch != FLOOR && ch != PASSAGE)) { extinguish(wghtchk); fuse(wghtchk,(void *)TRUE,1,AFTER); inwhgt = FALSE; return; } extinguish(wghtchk); msg("Your pack is too heavy for you."); do { dropchk = drop(NULL); if(dropchk == 0) { mpos = 0; msg("You must drop something."); } if(dropchk == TRUE) err = FALSE; } while(err); } inwhgt = FALSE; }
FuelUses ErrorEstimation::getUses(const std::string &t_sourceName, const isomodel::UserModel &t_userModel, const isomodel::ISOResults &t_results) const { auto itr = m_confidences.find(t_sourceName); if (itr == m_confidences.end()) { throw std::runtime_error("Unknown source name, no registered confidence level"); } FuelUses retval(itr->second); std::vector<EndUseFuelType> fuelTypes = EndUses::fuelTypes(); for (const auto & fuelType : fuelTypes) { double value = 0; for (const auto & monthlyResult : t_results.monthlyResults) { value += monthlyResult.getEndUseByFuelType(fuelType); } LOG(Debug, "Read fuel use of " << value << " For " << fuelType.valueName()); try { // the fuel uses in this case seem to be stored in kWh/m2 FuelUse fuse(openstudio::FuelType(fuelType.valueName()), value * 3600000 * t_userModel.floorArea(), *openstudio::createUnit("J")); retval += fuse; } catch (const std::exception &e) { LOG(Error, "Unmatched fuel type " << e.what()); } } return retval; }
FuelUses ErrorEstimation::getUses(const std::string &t_sourceName, const SqlFile &t_sql) const { auto itr = m_confidences.find(t_sourceName); if (itr == m_confidences.end()) { throw std::runtime_error("Unknown source name, no registered confidence level"); } std::vector<SummaryData> sd = t_sql.getSummaryData(); FuelUses retval(itr->second); for (std::vector<SummaryData>::const_iterator itr = sd.begin(); itr != sd.end(); ++itr) { if (itr->reportingFrequency == openstudio::ReportingFrequency::Hourly) { FuelUse fuse(itr->fuelType, itr->value, itr->units); retval += fuse; } } return retval; }
void AHRSClass::update(float dt) { getSensors(); MerayoCalibClass::apply(&scaledSensorData.x, &scaledSensorData.y, &scaledSensorData.z, &accCalibData); fuse(dt); transformOrientation(); }
/* * main: * The main program, of course */ int main(int argc, char **argv) { md_init(); initJedi(); /* * get home and options from environment */ #ifdef __linux__ strcpy(file_name, "/var/lib/griffin/rogue.save"); #else strcpy(file_name, "rogue.save"); #endif open_score(); /* * check for print-score option */ if (argc == 2) { if (strcmp(argv[1], "-s") == 0) { score(0, -1, 0); exit(0); } } init_check(); /* check for legal startup */ restore("-r"); init_probs(); /* Set up prob tables for objects */ init_player(); /* Set up initial player stats */ init_names(); /* Set up names of scrolls */ init_colors(); /* Set up colors of potions */ init_stones(); /* Set up stone settings of rings */ init_materials(); /* Set up materials of wands */ setup(); displayMessage("WELCOME TO THE DUNGEONS OF DOOM"); new_level(); /* Draw current level */ /* * Start up daemons and fuses */ start_daemon(runners, 0, AFTER); start_daemon(doctor, 0, AFTER); fuse(swander, 0, WANDERTIME, AFTER); start_daemon(stomach, 0, AFTER); playit(); return(0); }
static void PointsImageCallback(const autoware_msgs::PointsImage& points_image) { sensor_header = points_image.header; setPointsImage(points_image); if (ready_) { fuse(); publishTopic(); ready_ = false; return; } ready_ = true; }
static void DetectedObjectsCallback(const autoware_msgs::ImageObj& image_object) { sensor_header = image_object.header; setDetectedObjects(image_object); if (ready_) { fuse(); publishTopic(); ready_ = false; return; } ready_ = true; }
rollwand() { if (++between >= 4) { if (roll(1, 6) == 4) { wanderer(); kill_daemon(rollwand); fuse(swander, 0, WANDERTIME, BEFORE); } between = 0; } }
void CommunicationInterface::handleMessage(std::shared_ptr<Message> message) { auto entity = message->getEntity(); _log->info("Received Message with id '%v' from %v", std::to_string(message->getId()), entity->toString()); entity->setActiveTimestamp(); switch (message->getId()) { case (SCMD_IDS_REQUEST): this->onRequestIds(entity); break; case (SCMD_IDS_RESPONSE): { entity->fuse(std::static_pointer_cast<IdMessage>(message)->getIds()); entity->checkIce(); break; } case (SCMD_ID_REQUEST): // TODO break; case (SCMD_ID_RESPONSE): // TODO break; case (SCMD_OFFERS_REQUEST): this->onRequestOffers(entity); break; case (SCMD_OFFERS_RESPONSE): entity->addOfferedInformation(std::static_pointer_cast<OffersMessage>(message)->getOfferes()); break; case (SCMD_INFORMATION_REQUEST): this->onRequestInformation(entity, std::static_pointer_cast<RequestMessage>(message)->getRequests()); break; case (SCMD_INFORMATION_RESPONSE): this->onInformation(entity, std::static_pointer_cast<InformationMessage>(message)->getInformations()); break; default: _log->error("Unknown command '%v', message will be skipped", std::to_string(message->getId())); break; } }
/* * rollwand: * Called to roll to see if a wandering monster starts up */ void rollwand(void) { if (++between >= 4) { /* Theives may not awaken a monster */ if ((roll(1, 6) == 4) && ((player.t_ctype != C_THIEF) || (rnd(30) >= pstats.s_dext))) { if (levtype != POSTLEV) wanderer(); kill_daemon(rollwand); fuse(swander, 0, WANDERTIME, BEFORE); } between = 0; } }
TopoDS_Shape Fuse::execute(const TopoDS_Shape& shape1, const TopoDS_Shape& shape2) const { try { BRepAlgoAPI_Fuse fuse(shape1, shape2); return fuse.Shape(); } catch(const StdFail_NotDone& ex) { throw Common::Exception(QObject::tr("Ошибка выполнения операции")); } catch(const Standard_ConstructionError&) { throw Common::Exception(QObject::tr("Невозможно создать фигуру")); } }
void IdentityRequest::onResponsIds(std::shared_ptr<IdMessage> const &message) { if (this->stateIR != IdentityRequestState::IRS_REQUEST_IDS && this->stateIR != IdentityRequestState::IRS_RESPONS_IDS) { _log->warn("Received ids message in wrong state '%v' from '%v'", this->stateIR, entity->toString()); return; } // duplicated message if (this->stateIR == IdentityRequestState::IRS_RESPONS_IDS && false == this->timeFactory->checkTimeout(this->timestampLastActive, 100)) { _log->debug("Received duplicated ids message from '%v'", entity->toString()); return; } _log->info("Received ids from %v", this->entity->toString()); this->updateActiveTime(); this->stateIR = IdentityRequestState::IRS_RESPONS_IDS; auto entity = message->getEntity(); entity->fuse(message->getIds()); entity->checkDirectory(); bool hasIri = entity->getId(EntityDirectory::ID_ONTOLOGY, this->iri); // if not an ice engine stop here if (false == hasIri) { this->finish(); return; } // ontology iris known? if (entity->getOntologyIds().size() > 0) { this->checkOntologyIris(); return; } // request ontology ids this->stateIR = IdentityRequestState::IRS_REQUEST_ONT_IRI; this->sendCommand(IceMessageIds::IMI_ONTOLOGY_IDS_REQUEST); this->state = CJState::CJ_WAITING; }
void do_pot(int type, bool knowit) { PACT *pp; int t; pp = &p_actions[type]; if (!pot_info[type].oi_know) pot_info[type].oi_know = knowit; t = spread(pp->pa_time); if (!on(player, pp->pa_flags)) { player.t_flags |= pp->pa_flags; fuse(pp->pa_daemon, 0, t, AFTER); look(FALSE); } else lengthen(pp->pa_daemon, t); msg(choose_str(pp->pa_high, pp->pa_straight)); }
bool ElevationMap::fuseArea(const Eigen::Vector2d& position, const Eigen::Array2d& length) { ROS_DEBUG("Requested to fuse an area of the elevation map with center at (%f, %f) and side lengths (%f, %f)", position[0], position[1], length[0], length[1]); Index topLeftIndex; Index submapBufferSize; // These parameters are not used in this function. Position submapPosition; Length submapLength; Index requestedIndexInSubmap; boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_); getSubmapInformation(topLeftIndex, submapBufferSize, submapPosition, submapLength, requestedIndexInSubmap, position, length, rawMap_.getLength(), rawMap_.getPosition(), rawMap_.getResolution(), rawMap_.getSize(), rawMap_.getStartIndex()); return fuse(topLeftIndex, submapBufferSize); }
/* * quaff: * Let the hero drink a potion */ int quaff() { struct object *obj; struct linked_list *item, *titem; struct thing *th; int wh; char buf[LINLEN]; bool bless, curse; /* * Make certain that it is somethings that we want to drink */ if ((item = get_item("quaff", POTION)) == NULL) return 0; obj = OBJPTR(item); if (obj->o_type != POTION) { msg("That's undrinkable!"); after = FALSE; return 0; } wh = obj->o_which; bless = o_on(obj, ISBLESS); curse = o_on(obj, ISCURSED); del_pack(item); /* get rid of it */ /* * Calculate the effect it has on the poor guy. */ switch(wh) { case P_CONFUSE: if (!bless) { if (pl_on(ISINVINC)) msg("You remain level-headed."); else { chg_abil(WIS,-1,TRUE); /* confuse his mind */ if (pl_off(ISHUH)) { msg("Wait, what's going on here. Huh? What? Who?"); if (pl_on(ISHUH)) lengthen(unconfuse,rnd(8)+HUHDURATION); else fuse(unconfuse,TRUE,rnd(8)+HUHDURATION); player.t_flags |= ISHUH; } } p_know[P_CONFUSE] = TRUE; } when P_POISON: if (!bless) { if (pl_off(ISINVINC) && !iswearing(R_SUSTSTR) && !iswearing(R_SUSAB)) { chg_abil(CON,-1,TRUE); chg_abil(STR,-(rnd(3)+1),TRUE); msg("You feel very sick now."); } else msg("You feel momentarily sick."); p_know[P_POISON] = TRUE; } when P_HEALING: if (!curse) { heal_self(4, TRUE); msg("You begin to feel better."); if (!iswearing(R_SLOW)) notslow(FALSE); sight(FALSE); p_know[P_HEALING] = TRUE; } when P_STRENGTH: if (!curse) { msg("You feel stronger, now. What bulging muscles!"); chg_abil(STR,1,TRUE); p_know[P_STRENGTH] = TRUE; } when P_MFIND: /* * Potion of monster detection - find all monsters */ if (mlist != NULL && !curse) { dispmons(); mpos = 0; msg("You begin to sense the presence of monsters--More--"); p_know[P_MFIND] = TRUE; wait_for(cw,' '); msg(""); /* clear line */ } else msg("You have a strange feeling for a moment, then it passes."); when P_TFIND: /* * Potion of magic detection. Show the potions and scrolls */ if (lvl_obj != NULL && !curse) { struct linked_list *mobj; struct object *tp; bool show; show = FALSE; wclear(hw); for (mobj = lvl_obj; mobj != NULL; mobj = next(mobj)) { tp = OBJPTR(mobj); if (is_magic(tp)) { show = TRUE; mvwaddch(hw, tp->o_pos.y, tp->o_pos.x, MAGIC); } } for(titem = mlist; titem != NULL; titem = next(titem)) { struct linked_list *pitem; th = THINGPTR(titem); for(pitem=th->t_pack;pitem!=NULL;pitem=next(pitem)) { if (is_magic(ldata(pitem))) { show = TRUE; mvwaddch(hw,th->t_pos.y, th->t_pos.x, MAGIC); } } } if (show) { msg("You begin to sense the presence of magic."); overlay(hw,cw); p_know[P_TFIND] = TRUE; break; } } msg("You have a strange feeling for a moment, then it passes."); when P_PARALYZE: if (!bless) { if (pl_on(ISINVINC)) msg("You feel numb for a moment."); else { msg("You can't move."); player.t_nocmd = HOLDTIME; } p_know[P_PARALYZE] = TRUE; } when P_SEEINVIS: if (!curse) { int invlen = roll(40,20); msg("This potion tastes like %s juice.", fruit); if (pl_off(CANSEE)) { player.t_flags |= CANSEE; fuse(unsee, TRUE, invlen); light(&hero); } else lengthen(unsee, invlen); sight(FALSE); } when P_RAISE: if (!curse) { msg("You suddenly feel much more skillful."); p_know[P_RAISE] = TRUE; chg_abil(DEX,1,TRUE); chg_abil(WIS,1,TRUE); chg_abil(CON,1,TRUE); raise_level(); } when P_XHEAL: if (!curse) { heal_self(8, TRUE); if (rnd(100) < 50) chg_abil(CON,1,TRUE); msg("You begin to feel much better."); p_know[P_XHEAL] = TRUE; if (!iswearing(R_SLOW)) notslow(FALSE); unconfuse(); extinguish(unconfuse); sight(FALSE); } when P_HASTE: if (!curse) { add_haste(TRUE); msg("You feel yourself moving much faster."); p_know[P_HASTE] = TRUE; } when P_INVINC: if (!curse) { int time = rnd(400) + 350; msg("You feel invincible."); if (player.t_flags & ISINVINC) lengthen(notinvinc,time); else fuse(notinvinc,TRUE,time); player.t_flags |= ISINVINC; p_know[P_INVINC] = TRUE; } when P_SMART: if (!curse) { msg("You feel more perceptive."); p_know[P_SMART] = TRUE; chg_abil(WIS,1,TRUE); } when P_RESTORE: if (!curse) { msg("Hey, this tastes great. You feel warm all over."); him->s_re = max_stats.s_re; him->s_ef = max_stats.s_re; ringabil(); /* add in rings */ updpack(); /* update weight */ p_know[P_RESTORE] = TRUE; extinguish(rchg_str); /* kill restore in from ulodyte */ } when P_BLIND: if (!bless) { if (pl_on(ISINVINC)) msg("The light dims for a moment."); else { chg_abil(WIS,-1,TRUE); msg("A cloak of darkness falls around you."); if (pl_off(ISBLIND)) { player.t_flags |= ISBLIND; fuse(sight, TRUE, rnd(400) + 450); light(&hero); } } p_know[P_BLIND] = TRUE; } when P_ETH: if (!curse) { int ethlen = roll(40,20); msg("You feel more vaporous."); if (pl_on(ISETHER)) lengthen(noteth,ethlen); else fuse(noteth,TRUE,ethlen); player.t_flags |= ISETHER; p_know[P_ETH] = TRUE; } when P_NOP: msg("This potion tastes extremely dull."); when P_DEX: if (!curse) { chg_abil(DEX,1,TRUE); /* increase dexterity */ p_know[P_DEX] = TRUE; msg("You feel much more agile."); } when P_REGEN: if (!curse) { int reglen = rnd(450) + 450; if (pl_on(ISREGEN)) lengthen(notregen, reglen); else fuse(notregen, TRUE, reglen); player.t_flags |= ISREGEN; msg("You feel yourself improved."); p_know[P_REGEN] = TRUE; } when P_DECREP: case P_SUPHERO: { int howmuch = rnd(3) + 1; if (wh == P_DECREP) { if (!bless) { if (iswearing(R_SUSAB) || pl_on(ISINVINC)) { msg("You feel momentarily woozy."); howmuch = 0; } else { msg("You feel crippled."); howmuch = -howmuch; if (!iswearing(R_SUSTSTR)) chg_abil(STR,howmuch,TRUE); } } else howmuch = 0; } else { /* potion of superhero */ if (curse) howmuch = 0; msg("You feel invigorated."); chg_abil(STR,howmuch,TRUE); } chg_abil(CON,howmuch,TRUE); chg_abil(DEX,howmuch,TRUE); chg_abil(WIS,howmuch,TRUE); /* change abilities */ p_know[wh] = TRUE; } otherwise: msg("What an odd tasting potion!"); return 0; } nochange = FALSE; if (p_know[wh] && p_guess[wh]) { free(p_guess[wh]); p_guess[wh] = NULL; } else if(!p_know[wh] && p_guess[wh] == NULL) { strcpy(buf, p_colors[wh]); msg(callit); if (get_str(buf, cw) == NORM) { p_guess[wh] = new(strlen(buf) + 1); strcpy(p_guess[wh], buf); }
bool ElevationMap::fuseAll() { ROS_DEBUG("Requested to fuse entire elevation map."); boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_); return fuse(Index(0, 0), fusedMap_.getSize()); }
void Ekf::fuseAirspeed() { // Initialize variables float vn; // Velocity in north direction float ve; // Velocity in east direction float vd; // Velocity in downwards direction float vwn; // Wind speed in north direction float vwe; // Wind speed in east direction float v_tas_pred; // Predicted measurement float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); // Variance for true airspeed measurement - (m/sec)^2 float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian float H_TAS[24] = {}; // Observation Jacobian float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector float Kfusion[24] = {}; // Kalman gain vector // Copy required states to local variable names vn = _state.vel(0); ve = _state.vel(1); vd = _state.vel(2); vwn = _state.wind_vel(0); vwe = _state.wind_vel(1); // Calculate the predicted airspeed v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd); // Perform fusion of True Airspeed measurement if (v_tas_pred > 1.0f) { // Calculate the observation jacobian // intermediate variable from algebraic optimisation SH_TAS[0] = 1.0f/v_tas_pred; SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; } H_TAS[4] = SH_TAS[2]; H_TAS[5] = SH_TAS[1]; H_TAS[6] = vd*SH_TAS[0]; H_TAS[22] = -SH_TAS[2]; H_TAS[23] = -SH_TAS[1]; // We don't want to update the innovation variance if the calculation is ill conditioned float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; _fault_status.flags.bad_airspeed = false; } else { // Reset the estimator covarinace matrix _fault_status.flags.bad_airspeed = true; initialiseCovariance(); ECL_ERR("EKF airspeed fusion numerical error - covariance reset"); return; } SK_TAS[1] = SH_TAS[1]; Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); // Calculate measurement innovation _airspeed_innov = v_tas_pred - _airspeed_sample_delayed.true_airspeed; // Calculate the innovation variance _airspeed_innov_var = 1.0f / SK_TAS[0]; // Compute the ratio of innovation to gate size _tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var); // If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health if (_tas_test_ratio > 1.0f) { _innov_check_fail_status.flags.reject_airspeed = true; return; } else { _innov_check_fail_status.flags.reject_airspeed = false; } // Airspeed measurement sample has passed check so record it _time_last_arsp_fuse = _time_last_imu; // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP for (unsigned row = 0; row < _k_num_states; row++) { KH[row][4] = Kfusion[row] * H_TAS[4]; KH[row][5] = Kfusion[row] * H_TAS[5]; KH[row][6] = Kfusion[row] * H_TAS[6]; KH[row][22] = Kfusion[row] * H_TAS[22]; KH[row][23] = Kfusion[row] * H_TAS[23]; } for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { float tmp = KH[row][4] * P[4][column]; tmp += KH[row][5] * P[5][column]; tmp += KH[row][6] * P[6][column]; tmp += KH[row][22] * P[22][column]; tmp += KH[row][23] * P[23][column]; KHP[row][column] = tmp; } } // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected bool healthy = true; _fault_status.flags.bad_airspeed = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns zeroRows(P,i,i); zeroCols(P,i,i); //flag as unhealthy healthy = false; // update individual measurement health status _fault_status.flags.bad_airspeed = true; } } // only apply covariance and state corrrections if healthy if (healthy) { // apply the covariance corrections for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { P[row][column] = P[row][column] - KHP[row][column]; } } // correct the covariance marix for gross errors fixCovarianceErrors(); // apply the state corrections fuse(Kfusion, _airspeed_innov); } } }
/* * wake_monster: * What to do when the hero steps next to a monster */ const THING * wake_monster(int y, int x) { THING *tp; struct room *rp; int ch; const char *mname; if ((tp = moat(y, x)) == NULL) { #ifdef MASTER msg("can't find monster in wake_monster"); #endif return NULL; } ch = tp->t_type; /* * Every time he sees mean monster, it might start chasing him */ if (!on(*tp, ISRUN) && rnd(3) != 0 && on(*tp, ISMEAN) && !on(*tp, ISHELD) && !ISWEARING(R_STEALTH) && !on(player, ISLEVIT)) { tp->t_dest = &hero; tp->t_flags |= ISRUN; } if (ch == 'M' && !on(player, ISBLIND) && !on(player, ISHALU) && !on(*tp, ISFOUND) && !on(*tp, ISCANC) && on(*tp, ISRUN)) { rp = proom; if ((rp != NULL && !(rp->r_flags & ISDARK)) || dist(y, x, hero.y, hero.x) < LAMPDIST) { tp->t_flags |= ISFOUND; if (!save(VS_MAGIC)) { if (on(player, ISHUH)) lengthen(unconfuse, spread(HUHDURATION)); else fuse(unconfuse, 0, spread(HUHDURATION), AFTER); player.t_flags |= ISHUH; mname = set_mname(tp); addmsg("%s", mname); if (strcmp(mname, "it") != 0) addmsg("'"); msg("s gaze has confused you"); } } } /* * Let greedy ones guard gold */ if (on(*tp, ISGREED) && !on(*tp, ISRUN)) { tp->t_flags |= ISRUN; if (proom->r_goldval) tp->t_dest = &proom->r_gold; else tp->t_dest = &hero; } return tp; }
constexpr operator To() { return fuse(typename boost::mpl::apply<Lam, To>::type())(std::tuple<Args &&...>(m_args)); }
void egg_example() { BOOST_CHECK( 1+2+3 == fuse(&unfused_plus)(boost::make_tuple(1,2,3)) ); }
void quaff() { THING *obj, *tp, *mp; bool discardit = FALSE; bool show, trip; obj = get_item("quaff", POTION); /* * Make certain that it is somethings that we want to drink */ if (obj == NULL) return; if (obj->o_type != POTION) { if (!terse) msg("yuk! Why would you want to drink that?"); else msg("that's undrinkable"); return; } if (obj == cur_weapon) cur_weapon = NULL; /* * Calculate the effect it has on the poor guy. */ trip = on(player, ISHALU); discardit = (bool)(obj->o_count == 1); leave_pack(obj, FALSE, FALSE); switch (obj->o_which) { case P_CONFUSE: do_pot(P_CONFUSE, !trip); when P_POISON: pot_info[P_POISON].oi_know = TRUE; if (ISWEARING(R_SUSTSTR)) msg("you feel momentarily sick"); else { chg_str(-(rnd(3) + 1)); msg("you feel very sick now"); come_down(); } when P_HEALING: pot_info[P_HEALING].oi_know = TRUE; if ((pstats.s_hpt += roll(pstats.s_lvl, 4)) > max_hp) pstats.s_hpt = ++max_hp; sight(); msg("you begin to feel better"); when P_STRENGTH: pot_info[P_STRENGTH].oi_know = TRUE; chg_str(1); msg("you feel stronger, now. What bulging muscles!"); when P_MFIND: player.t_flags |= SEEMONST; fuse((void(*)())turn_see, TRUE, HUHDURATION, AFTER); if (!turn_see(FALSE)) msg("you have a %s feeling for a moment, then it passes", choose_str("normal", "strange")); when P_TFIND: /* * Potion of magic detection. Show the potions and scrolls */ show = FALSE; if (lvl_obj != NULL) { wclear(hw); for (tp = lvl_obj; tp != NULL; tp = next(tp)) { if (is_magic(tp)) { show = TRUE; wmove(hw, tp->o_pos.y, tp->o_pos.x); waddch(hw, MAGIC); pot_info[P_TFIND].oi_know = TRUE; } } for (mp = mlist; mp != NULL; mp = next(mp)) { for (tp = mp->t_pack; tp != NULL; tp = next(tp)) { if (is_magic(tp)) { show = TRUE; wmove(hw, mp->t_pos.y, mp->t_pos.x); waddch(hw, MAGIC); } } } } if (show) { pot_info[P_TFIND].oi_know = TRUE; show_win("You sense the presence of magic on this level.--More--"); } else msg("you have a %s feeling for a moment, then it passes", choose_str("normal", "strange")); when P_LSD: if (!trip) { if (on(player, SEEMONST)) turn_see(FALSE); start_daemon(visuals, 0, BEFORE); seenstairs = seen_stairs(); } do_pot(P_LSD, TRUE); when P_SEEINVIS: sprintf(prbuf, "this potion tastes like %s juice", fruit); show = on(player, CANSEE); do_pot(P_SEEINVIS, FALSE); if (!show) invis_on(); sight(); when P_RAISE: pot_info[P_RAISE].oi_know = TRUE; msg("you suddenly feel much more skillful"); raise_level(); when P_XHEAL: pot_info[P_XHEAL].oi_know = TRUE; if ((pstats.s_hpt += roll(pstats.s_lvl, 8)) > max_hp) { if (pstats.s_hpt > max_hp + pstats.s_lvl + 1) ++max_hp; pstats.s_hpt = ++max_hp; } sight(); come_down(); msg("you begin to feel much better"); when P_HASTE: pot_info[P_HASTE].oi_know = TRUE; after = FALSE; if (add_haste(TRUE)) msg("you feel yourself moving much faster"); when P_RESTORE: if (ISRING(LEFT, R_ADDSTR)) add_str(&pstats.s_str, -cur_ring[LEFT]->o_arm); if (ISRING(RIGHT, R_ADDSTR)) add_str(&pstats.s_str, -cur_ring[RIGHT]->o_arm); if (pstats.s_str < max_stats.s_str) pstats.s_str = max_stats.s_str; if (ISRING(LEFT, R_ADDSTR)) add_str(&pstats.s_str, cur_ring[LEFT]->o_arm); if (ISRING(RIGHT, R_ADDSTR)) add_str(&pstats.s_str, cur_ring[RIGHT]->o_arm); msg("hey, this tastes great. It make you feel warm all over"); when P_BLIND: do_pot(P_BLIND, TRUE); when P_LEVIT: do_pot(P_LEVIT, TRUE); #ifdef MASTER otherwise: msg("what an odd tasting potion!"); return; #endif } status(); /* * Throw the item away */ call_it(&pot_info[obj->o_which]); if (discardit) discard(obj); return; }
/* * quaff: * Quaff a potion from the pack */ quaff() { register THING *obj, *th; register bool discardit = FALSE; obj = get_item("quaff", POTION); /* * Make certain that it is somethings that we want to drink */ if (obj == NULL) return; if (obj->o_type != POTION) { if (!terse) msg("yuk! Why would you want to drink that?"); else msg("that's undrinkable"); return; } if (obj == cur_weapon) cur_weapon = NULL; /* * Calculate the effect it has on the poor guy. */ switch (obj->o_which) { case P_CONFUSE: p_know[P_CONFUSE] = TRUE; if (!on(player, ISHUH)) { if (on(player, ISHUH)) lengthen(unconfuse, rnd(8)+HUHDURATION); else fuse(unconfuse, 0, rnd(8)+HUHDURATION, AFTER); player.t_flags |= ISHUH; msg("wait, what's going on here. Huh? What? Who?"); } when P_POISON: p_know[P_POISON] = TRUE; if (!ISWEARING(R_SUSTSTR)) { chg_str(-(rnd(3)+1)); msg("you feel very sick now"); } else msg("you feel momentarily sick"); when P_HEALING: p_know[P_HEALING] = TRUE; if ((pstats.s_hpt += roll(pstats.s_lvl, 4)) > max_hp) pstats.s_hpt = ++max_hp; sight(); msg("you begin to feel better"); when P_STRENGTH: p_know[P_STRENGTH] = TRUE; chg_str(1); msg("you feel stronger, now. What bulging muscles!"); when P_MFIND: player.t_flags |= SEEMONST; fuse(turn_see, TRUE, HUHDURATION, AFTER); if (mlist == NULL) msg("you have a strange feeling for a moment"); else p_know[P_MFIND] |= turn_see(FALSE); when P_TFIND: /* * Potion of magic detection. Show the potions and scrolls */ if (lvl_obj != NULL) { register THING *tp; register bool show; show = FALSE; wclear(hw); for (tp = lvl_obj; tp != NULL; tp = next(tp)) { if (is_magic(tp)) { show = TRUE; mvwaddch(hw, tp->o_pos.y, tp->o_pos.x, MAGIC); p_know[P_TFIND] = TRUE; } } for (th = mlist; th != NULL; th = next(th)) { for (tp = th->t_pack; tp != NULL; tp = next(tp)) { if (is_magic(tp)) { show = TRUE; mvwaddch(hw, th->t_pos.y, th->t_pos.x, MAGIC); p_know[P_TFIND] = TRUE; } } } if (show) { show_win(hw, "You sense the presence of magic on this level.--More--"); break; } } msg("you have a strange feeling for a moment, then it passes"); when P_PARALYZE: p_know[P_PARALYZE] = TRUE; no_command = HOLDTIME; player.t_flags &= ~ISRUN; msg("you can't move"); when P_SEEINVIS: if (!on(player, CANSEE)) { fuse(unsee, 0, SEEDURATION, AFTER); look(FALSE); invis_on(); } sight(); msg("this potion tastes like %s juice", fruit); when P_RAISE: p_know[P_RAISE] = TRUE; msg("you suddenly feel much more skillful"); raise_level(); when P_XHEAL: p_know[P_XHEAL] = TRUE; if ((pstats.s_hpt += roll(pstats.s_lvl, 8)) > max_hp) { if (pstats.s_hpt > max_hp + pstats.s_lvl + 1) ++max_hp; pstats.s_hpt = ++max_hp; } sight(); msg("you begin to feel much better"); when P_HASTE: p_know[P_HASTE] = TRUE; if (add_haste(TRUE)) msg("you feel yourself moving much faster"); when P_RESTORE: if (ISRING(LEFT, R_ADDSTR)) add_str(&pstats.s_str, -cur_ring[LEFT]->o_ac); if (ISRING(RIGHT, R_ADDSTR)) add_str(&pstats.s_str, -cur_ring[RIGHT]->o_ac); if (pstats.s_str < max_stats.s_str) pstats.s_str = max_stats.s_str; if (ISRING(LEFT, R_ADDSTR)) add_str(&pstats.s_str, cur_ring[LEFT]->o_ac); if (ISRING(RIGHT, R_ADDSTR)) add_str(&pstats.s_str, cur_ring[RIGHT]->o_ac); msg("hey, this tastes great. It make you feel warm all over"); when P_BLIND: p_know[P_BLIND] = TRUE; if (!on(player, ISBLIND)) { player.t_flags |= ISBLIND; fuse(sight, 0, SEEDURATION, AFTER); look(FALSE); } msg("a cloak of darkness falls around you"); when P_NOP: msg("this potion tastes extremely dull"); otherwise: msg("what an odd tasting potion!"); return; } status(); /* * Throw the item away */ inpack--; if (obj->o_count > 1) obj->o_count--; else { detach(pack, obj); discardit = TRUE; } call_it(p_know[obj->o_which], &p_guess[obj->o_which]); if (discardit) discard(obj); }
/* * ring_on: * Put on a ring */ int ring_on() { struct object *obj; struct linked_list *item; int ring, wh; char buf[LINLEN]; bool okring; if (cur_ring[LEFT] != NULL && cur_ring[RIGHT] != NULL) { msg("Already wearing two rings."); after = FALSE; return 0; } /* * Make certain that it is somethings that we want to wear */ if ((item = get_item("put on", RING)) == NULL) return 0; obj = OBJPTR(item); if (obj->o_type != RING) { msg("That won't fit on your finger."); return 0; } /* * find out which hand to put it on */ if (is_current(obj)) return 0; if (cur_ring[LEFT] == NULL && cur_ring[RIGHT] == NULL) { if ((ring = gethand(FALSE)) < 0) return 0; } else if (cur_ring[LEFT] == NULL) ring = LEFT; else ring = RIGHT; cur_ring[ring] = obj; wh = obj->o_which; /* * okring = FALSE when: * 1) ring is cursed and benefit = plus * 2) ring is blessed and benefit = minus */ okring = !((obj->o_ac > 0 && o_on(obj, ISCURSED)) || (obj->o_ac < 0 && o_on(obj, ISBLESS))); /* * Calculate the effect it has on the poor guy (if possible). */ if (okring) { switch (wh) { case R_SPEED: if (--obj->o_ac < 0) { obj->o_ac = 0; setoflg(obj, ISCURSED); } else { add_haste(FALSE); msg("You find yourself moving must faster."); } when R_GIANT : /* to 24 */ him->s_ef.a_str = MAXSTR; when R_ADDSTR : chg_abil(STR, obj->o_ac, FROMRING); when R_KNOW : chg_abil(WIS, obj->o_ac, FROMRING); when R_DEX : chg_abil(DEX, obj->o_ac, FROMRING); when R_CONST : chg_abil(CON, obj->o_ac, FROMRING); when R_SEEINVIS : player.t_flags |= CANSEE; light(&hero); mvwaddch(cw, hero.y, hero.x, PLAYER); when R_AGGR : aggravate(); when R_HEAVY : updpack(); /* new pack weight */ when R_BLIND : r_know[R_BLIND] = TRUE; player.t_flags |= ISBLIND; look(FALSE); when R_SLOW : player.t_flags |= ISSLOW; when R_SAPEM : fuse(sapem, TRUE, 150); when R_LIGHT: { struct room *rop; r_know[R_LIGHT] = TRUE; if ((rop = player.t_room) != NULL) { rop->r_flags &= ~ISDARK; light(&hero); mvwaddch(cw, hero.y, hero.x, PLAYER); } } } } if (r_know[wh] && r_guess[wh]) { free(r_guess[wh]); r_guess[wh] = NULL; } else if (!r_know[wh] && r_guess[wh] == NULL) { mpos = 0; strcpy(buf, r_stones[wh]); msg(callit); if (get_str(buf, cw) == NORM) { r_guess[wh] = new (strlen(buf) + 1); strcpy(r_guess[wh], buf); }
/* * main: * The main program, of course */ int main(int argc, char **argv) { char *env; time_t lowtime; md_init(); #ifdef MASTER /* * Check to see if he is a wizard */ if (argc >= 2 && argv[1][0] == '\0') if (strcmp(PASSWD, md_crypt(md_getpass("wizard's password: "******"mT")) == 0) { wizard = TRUE; player.t_flags |= SEEMONST; argv++; argc--; } #endif /* * get home and options from environment */ strcpy(home, md_gethomedir()); if (strlen(home) > MAXSTR - strlen("rogue.save") - 1) *home = 0; strcpy(file_name, home); strcat(file_name, "rogue.save"); if ((env = getenv("ROGUEOPTS")) != NULL) parse_opts(env); if (env == NULL || whoami[0] == '\0') strucpy(whoami, md_getusername(), strlen(md_getusername())); lowtime = time(NULL); if (getenv("SEED") != NULL) { dnum = atoi(getenv("SEED")); noscore = 1; } else dnum = (unsigned int) lowtime + md_getpid(); seed = dnum; open_score(); /* * Drop setuid/setgid after opening the scoreboard file. */ md_normaluser(); /* * check for print-score option */ md_normaluser(); /* we drop any setgid/setuid priveldges here */ if (argc == 2) { if (strcmp(argv[1], "-s") == 0) { noscore = TRUE; score(0, -1, 0); exit(0); } else if (strcmp(argv[1], "-d") == 0) { dnum = rnd(100); /* throw away some rnd()s to break patterns */ while (--dnum) rnd(100); purse = rnd(100) + 1; level = rnd(100) + 1; initscr(); getltchars(); death(death_monst()); exit(0); } } init_check(); /* check for legal startup */ if (argc == 2) if (!restore(argv[1])) /* Note: restore will never return */ my_exit(1); #ifdef MASTER if (wizard) printf("Hello %s, welcome to dungeon #%d", whoami, dnum); else #endif printf("Hello %s, just a moment while I dig the dungeon...", whoami); fflush(stdout); initscr(); /* Start up cursor package */ init_probs(); /* Set up prob tables for objects */ init_player(); /* Set up initial player stats */ init_names(); /* Set up names of scrolls */ init_colors(); /* Set up colors of potions */ init_stones(); /* Set up stone settings of rings */ init_materials(); /* Set up materials of wands */ setup(); /* * The screen must be at least NUMLINES x NUMCOLS */ if (LINES < NUMLINES || COLS < NUMCOLS) { printf("\nSorry, the screen must be at least %dx%d\n", NUMLINES, NUMCOLS); endwin(); my_exit(1); } /* * Set up windows */ hw = newwin(LINES, COLS, 0, 0); idlok(stdscr, TRUE); idlok(hw, TRUE); #ifdef MASTER noscore = wizard; #endif new_level(); /* Draw current level */ /* * Start up daemons and fuses */ start_daemon(runners, 0, AFTER); start_daemon(doctor, 0, AFTER); fuse(swander, 0, WANDERTIME, AFTER); start_daemon(stomach, 0, AFTER); playit(); return(0); }