//-------------------------------------------------------------------------- void Omu_IntODE::init(int k, const Omu_StateVec &xc, const Omu_Vec &q, const Omu_DependentVec &Fc, bool sa) { if (!(Fc.Jdx.is_scalar_constant())) { m_error(E_FORMAT, "Omu_IntODE::init, which was called for non scalar or non const dF/ddx"); } if (_na > 0) { m_error(E_FORMAT, "Omu_IntODE::init, which was called for algebraic states"); } resize(); }
wchar_t * m_mbstowcsdup(const char *s) { int n; wchar_t *w; n = strlen(s) + 1; if ((w = (wchar_t *)m_malloc(n * sizeof(wchar_t))) == NULL) { m_error(m_textmsg(3581, "!memory allocation failure", "E")); return(NULL); } if (mbstowcs(w, s, n) == -1) { m_error(m_textmsg(3642, "!multibyte string", "E")); return(NULL); } return w; }
/* Getachar returns the next character from the input stream */ int getachar(M_NOPAR) { int c; M_WCHAR wc,wnl; char mbyte[32]; /* bigger than any possible multibyte char */ int length; if (toundo) wc = (M_WCHAR) savechar[--toundo]; else { length = 0; if ((c = getc(ifile)) == EOF) return(EOF); while (1) { mbyte[length++] = c; mbyte[length] = 0; if (mblen(mbyte,length) != -1) break; /* hurray! */ if (length == MB_CUR_MAX) { /* reached max without a hit */ m_error("An invalid multi-byte character was found in the input"); c = ' '; length = 1; break; } if ((c = getc(ifile)) == EOF) { /* huh? */ m_error("End-of-file found in within a multi-byte character"); return(EOF); } } mbtowc(&wc,mbyte,length); } mbtowc(&wnl, "\n", 1); if (wc == wnl) m_line++; if (wc == 65535) return -1; return((int) wc); }
bool f_stack_push(t_stack *v_this, void *data) { t_stack_cell *push; push = D_STACK(create_cell)(data); if (push == NULL) return (m_error(false, "Bad alloc")); push->v_prev = v_this->v_last; v_this->v_last = push; v_this->v_size = v_this->v_size + 1; return (true); }
bool f_htable_add(t_htable *v_this, const char *str, void *data) { ui key; t_list *element; t_htable_cell *cell; key = D_HTABLE(generate_key)(v_this, str); element = D_ARRAY(at)(&v_this->v_array, key, t_list *); cell = D_HTABLE(create_cell)(str, data, v_this->f_delete); if (cell == NULL) return (m_error(false, "Bad alloc")); return (D_LIST(push_back)(element, cell)); }
int init_env(t_env *e, t_obj *obj) { if ((e->mlx = mlx_init()) == NULL) return (m_error("mlx_init(): fail")); if ((e->win = mlx_new_window(e->mlx, WIN_X, WIN_Y, "rtv1")) == NULL) return (m_error("mlx_new_window(): fail")); if ((e->img = mlx_new_image(e->mlx, WIN_X, WIN_Y)) == NULL) return (m_error("mlx_new_image(): fail")); if ((e->addr = mlx_get_data_addr(e->img, &(e->bpp), &(e->size_line), &(e->endian))) == NULL) return (m_error("mlx_get_data_addr(): fail")); if ((e->rgb_tab = (t_rgb *)malloc(sizeof(t_rgb) * (WIN_X * WIN_Y))) == NULL) return (m_error("rgb_tab_init(): fail")); e->eye_pos = vec_new(0, 0, 0); e->eye_dir = vec_new(0, 0, 1); e->right_vec = vec_new(1, 0, 0); e->up_vec = vec_new(0, -1, 0); e->view_plane_ori = vec_add(vec_add(e->eye_pos, vec_numb(e->eye_dir, VIEW_PLANE_DIST)), vec_sub(vec_numb(e->up_vec, VIEW_PLANE_HEIGHT / 2.0) , vec_numb(e->right_vec, VIEW_PLANE_WIDTH / 2.0))); init_obj(obj); return (0); }
//-------------------------------------------------------------------------- void Omu_Integrator::init_stage(int k, const Omu_VariableVec &x, const Omu_VariableVec &u, const Omu_DependentVec &Ft, bool sa) { if (k >= _K) { m_error(E_INTERN, "Omu_Integrator::init_stage" " that was called with wrong integrator setup"); } // initialize dimensions init_dims(k, x, u, Ft); _sa = sa; if ((int)(_Fcs[k]->dim) != _n) { m_error(E_INTERN, "Omu_Integrator::solve" " that was called with wrong integrator setup of stage"); } // initialize call arguments for sys->continuous _ut.resize(_nu); _dxt.resize(_nxt, _nx, _nu); v_zero(_dxt); // zero time derivative of discrete states // initialize call arguments for high-level integrator interface _xc.resize(_n, 0, 0, _nq); _dxc.resize(_n, 0, 0, _nq); _q.resize(_nq); // call high-level init init(k, _xc, _q, _Fcs[k], sa); // call depreciated init_stage init_stage(k, (Omu_SVarVec &)x, u, sa); }
void m_get_string(struct msg *m, const char **s) { uint8_t *end; char c; if (m->pos >= m->end) m_error("msg too short"); c = *m->pos++; if (c == '\0') { *s = NULL; return; } if (m->pos >= m->end) m_error("msg too short"); end = memchr(m->pos, 0, m->end - m->pos); if (end == NULL) m_error("unterminated string"); *s = m->pos; m->pos = end + 1; }
void m_get_data(struct msg *m, const void **data, size_t *sz) { m_get_size(m, sz); if (*sz == 0) { *data = NULL; return; } if (m->pos + *sz > m->end) m_error("msg too short"); *data = m->pos; m->pos += *sz; }
bool f_array_init(t_array *v_this, ui (*uf_realloc)(ui size), void (*uf_delete)(void *ptr), ui type_size) { v_this->v_size = 0; v_this->v_capacity = 0; v_this->f_realloc = uf_realloc; if (uf_realloc == NULL) v_this->f_realloc = uf_array_realloc; v_this->f_delete = uf_delete; if (uf_delete == NULL) v_this->f_delete = uf_array_delete; v_this->v_data = calloc(2, type_size); if (v_this->v_data == NULL) return (m_error(false, "Bad alloc")); v_this->v_capacity = 2; v_this->v_type_size = type_size; return (true); }
bool f_rbtree_init(t_rbtree *v_this, int (*f_cmp)(void *d1, void *d2), void (*f_del)(void *data)) { uf_memset(v_this, 0, sizeof(*v_this)); if (f_cmp == NULL) return (m_error(false, "f_cmp couldn't NULL")); v_this->f_delete = uf_rbtree_delete; if (f_del != NULL) v_this->f_delete = f_del; v_this->f_cmp = f_cmp; v_this->v_nil.v_left = &v_this->v_nil; v_this->v_nil.v_right = &v_this->v_nil; v_this->v_nil.v_parent = &v_this->v_nil; v_this->v_nil.v_color = e_black; v_this->v_root.v_left = &v_this->v_nil; v_this->v_root.v_right = &v_this->v_nil; v_this->v_root.v_parent = &v_this->v_nil; v_this->v_root.v_color = e_black; return (true); }
//-------------------------------------------------------------------------- void Omu_Integrator::setup_struct(int k, const Omu_VariableVec &x, const Omu_VariableVec &u, const Omu_DependentVec &Ft) { // initialize Jacobians for high-level integrator interface if (k >= _K) { m_error(E_INTERN, "Omu_Integrator::setup_struct" " that was called with wrong integrator setup"); } int i; Omu_DepVec &Fc = _Fcs[k]; init_dims(k, x, u, Ft); Fc.size(_n, _n, 0, _n, 0, _nx+_nu); m_move(Ft.Jx, _nd, _nd, _n, _n, Fc.Jx, 0, 0); m_move(Ft.Jdx, _nd, _nd, _n, _n, Fc.Jdx, 0, 0); m_zero(Fc.Jq); // zero Jq wrt. continuous states as Jx gets chained with Sx m_move(Ft.Jx, _nd, 0, _n, _nd, Fc.Jq, 0, 0); m_move(Ft.Ju, _nd, 0, _n, _nu, Fc.Jq, 0, _nx); Fc.c_setup = true; for (i = 0; i < _n; i++) { int wrt = 0; if (Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_x)) wrt |= Omu_Dependent::WRT_x; if (Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_dx)) wrt |= Omu_Dependent::WRT_dx; if ((_nd == 0 || Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_x)) && Ft.is_linear_element(_nd + i, Omu_Dependent::WRT_u)) wrt |= Omu_Dependent::WRT_q; Fc.set_linear_element(i, wrt); } Fc.analyze_struct(); Fc.c_setup = false; }
//-------------------------------------------------------------------------- void Omu_Integrator::residual(int kk, double t, const Omu_StateVec &xc, const Omu_StateVec &dxc, const Omu_Vec &q, Omu_DependentVec &Fc) { if (!_sys) { m_error(E_NULL, "Omu_Integrator::residual"); } // prepare call arguments Omu_DependentVec &Ft = *_Ft_ptr; Omu_StateVec &xt = *_xt_ptr; v_move(q, 0, _nd, xt, 0); v_move(xc, 0, _n, xt, _nd); v_move(dxc, 0, _n, _dxt, _nd); v_move(q, _nx, _nu, _ut, 0); Ft.set_required_J(Fc.is_required_J()); // note: treat seed derivatives xc.Sq and dxc.Sq to support forward mode // call continuous of Omu_Program _sys->continuous(kk, t, xt, _ut, _dxt, Ft); // return results v_move(Ft, _nd, _n, Fc, 0); if (Fc.is_required_J()) { if (!Fc.Jx.is_constant()) m_move(Ft.Jx, _nd, _nd, _n, _n, Fc.Jx, 0, 0); if (!Fc.Jdx.is_constant()) m_move(Ft.Jdx, _nd, _nd, _n, _n, Fc.Jdx, 0, 0); if (!Fc.Jq.is_constant()) { m_move(Ft.Jx, _nd, 0, _n, _nd, Fc.Jq, 0, 0); m_move(Ft.Ju, _nd, 0, _n, _nu, Fc.Jq, 0, _nx); } } }
void f_string_print_fd(const t_string *v_this, ui fd) { if (write(fd, v_this->v_str, v_this->v_size) != v_this->v_size) m_error(0, "Write : Fail"); }
void logger::init(const config_tree& a_cfg, const sigset_t* a_ignore_signals, bool a_install_finalizer) { if (m_initialized) throw std::runtime_error("Logger already initialized!"); std::lock_guard<std::mutex> guard(m_mutex); do_finalize(); try { m_show_location = a_cfg.get<bool> ("logger.show-location", m_show_location); m_show_fun_namespaces = a_cfg.get<int> ("logger.show-fun-namespaces", m_show_fun_namespaces); m_show_category = a_cfg.get<bool> ("logger.show-category", m_show_category); m_show_ident = a_cfg.get<bool> ("logger.show-ident", m_show_ident); m_show_thread = a_cfg.get<bool> ("logger.show-thread", m_show_thread); m_fatal_kill_signal = a_cfg.get<int> ("logger.fatal-kill-signal",m_fatal_kill_signal); m_ident = a_cfg.get<std::string>("logger.ident", m_ident); m_ident = replace_macros(m_ident); std::string ts = a_cfg.get<std::string>("logger.timestamp", "time-usec"); m_timestamp_type = parse_stamp_type(ts); std::string levs = a_cfg.get<std::string>("logger.levels", ""); if (!levs.empty()) set_level_filter(static_cast<log_level>(parse_log_levels(levs))); std::string ls = a_cfg.get<std::string>("logger.min-level-filter", "info"); if (!levs.empty() && !ls.empty()) std::runtime_error ("Either 'levels' or 'min-level-filter' option is permitted!"); set_min_level_filter(parse_log_level(ls)); long timeout_ms = a_cfg.get<int> ("logger.wait-timeout-ms", 1000); m_wait_timeout = timespec{timeout_ms / 1000, timeout_ms % 1000 * 1000000L}; m_sched_yield_us = a_cfg.get<long> ("logger.sched-yield-us", -1); m_silent_finish = a_cfg.get<bool> ("logger.silent-finish", false); m_block_signals = a_cfg.get<bool> ("logger.block-signals", true); if ((int)m_timestamp_type < 0) throw std::runtime_error("Invalid timestamp type: " + ts); // Install crash signal handlers // (SIGABRT, SIGFPE, SIGILL, SIGSEGV, SIGTERM) if (a_cfg.get("logger.handle-crash-signals", true)) { sigset_t sset = sig_members_parse (a_cfg.get("logger.handle-crash-signals.signals",""), UTXX_SRC); // Remove signals from the sset that are handled externally if (a_ignore_signals) for (uint i=1; i < sig_names_count(); ++i) if (sigismember(a_ignore_signals, i)) sigdelset(&sset, i); if (install_sighandler(true, &sset)) { auto old = m_crash_sigset.exchange(new sigset_t(sset)); if (!old) delete old; } } //logger_impl::msg_info info(NULL, 0); //query_timestamp(info); logger_impl_mgr& lim = logger_impl_mgr::instance(); std::lock_guard<std::mutex> guard(lim.mutex()); // Check the list of registered implementations. If corresponding // configuration section is found, initialize the implementation. for(logger_impl_mgr::impl_map_t::iterator it=lim.implementations().begin(); it != lim.implementations().end(); ++it) { std::string path = std::string("logger.") + it->first; if (a_cfg.get_child_optional(path)) { // Determine if implementation of this type is already // registered with the logger bool found = false; for(implementations_vector::iterator im = m_implementations.begin(), iend = m_implementations.end(); im != iend; ++im) if (it->first == (*im)->name()) { found = true; break; } if (found) throw badarg_error("Implementation '", it->first, "' is already registered with the logger!"); // A call to it->second() creates a logger_impl* pointer. // We need to call implementation's init function that may throw, // so use RAII to guarantee proper cleanup. logger_impl_mgr::impl_callback_t& f = it->second; m_implementations.emplace_back( f(it->first.c_str()) ); auto& i = m_implementations.back(); i->set_log_mgr(this); i->init(a_cfg); } } m_initialized = true; m_abort = false; m_thread.reset(new std::thread([this]() { this->run(); })); if (!m_finalizer_installed && a_install_finalizer) { atexit(&finalize_logger_at_exit); m_finalizer_installed = true; } } catch (std::runtime_error& e) { if (m_error) m_error(e.what()); else throw; } }
void m_end(struct msg *m) { if (m->pos != m->end) m_error("not at msg end"); }
void logger::dolog_msg(const logger::msg& a_msg) { try { switch (a_msg.m_type) { case payload_t::CHAR_FUN: { assert(a_msg.m_fun.cf); char buf[4096]; auto* end = buf + sizeof(buf); char* p = format_header(a_msg, buf, end); int n = (a_msg.m_fun.cf)(p, end - p); if (p[n-1] == '\n') --p; p = format_footer(a_msg, p+n, end); m_sig_slot[level_to_signal_slot(a_msg.level())]( on_msg_delegate_t::invoker_type(a_msg, buf, p - buf)); if (fatal_kill_signal() && a_msg.level() == LEVEL_FATAL) { m_abort = true; dolog_fatal_msg(buf, p - buf); } break; } case payload_t::STR_FUN: { assert(a_msg.m_fun.cf); char pfx[256], sfx[256]; char* p = format_header(a_msg, pfx, pfx + sizeof(pfx)); char* q = format_footer(a_msg, sfx, sfx + sizeof(sfx)); auto res = (a_msg.m_fun.sf)(pfx, p - pfx, sfx, q - sfx); m_sig_slot[level_to_signal_slot(a_msg.level())]( on_msg_delegate_t::invoker_type(a_msg, res.c_str(), res.size())); if (fatal_kill_signal() && a_msg.level() == LEVEL_FATAL) { m_abort = true; dolog_fatal_msg(res.c_str(), res.size()); } break; } case payload_t::STR: { detail::basic_buffered_print<1024> buf; char pfx[256], sfx[256]; char* p = format_header(a_msg, pfx, pfx + sizeof(pfx)); char* q = format_footer(a_msg, sfx, sfx + sizeof(sfx)); auto ps = p - pfx; auto qs = q - sfx; buf.reserve(a_msg.m_fun.str.size() + ps + qs + 1); buf.sprint(pfx, ps); auto& s = a_msg.m_fun.str; // Remove trailing new lines auto sz = int(s.size()); while (sz && s[sz-1] == '\n') --sz; buf.sprint(s.c_str(), sz); buf.sprint(sfx, qs); m_sig_slot[level_to_signal_slot(a_msg.level())]( on_msg_delegate_t::invoker_type(a_msg, buf.str(), buf.size())); if (fatal_kill_signal() && a_msg.level() == LEVEL_FATAL) { m_abort = true; dolog_fatal_msg(buf.c_str(), buf.size()); } break; } } } catch (std::runtime_error& e) { if (m_error) m_error(e.what()); else throw; } }
// alternative implementation calling high-level _sys->continuous //-------------------------------------------------------------------------- void Omu_IntODE::syseq_forward(double t, const VECP y, const VECP u, VECP f) { #ifdef OMU_WITH_ADOLC int i, j; Omu_DependentVec &Ft = *_Ft_ptr; // // form a vector of independent variables // for (i = 0; i < _nd; i++) { _x[i] = u[i]; } for (i = 0; i < _n; i++) { _x[_nd + i] = y[i]; _x[_nd + _n + i] = 0.0; // yprime[i] } for (i = 0; i < _nu; i++) { _x[_nd + 2 * _n + i] = u[_nd + i]; } // // evaluate residual // // adoublev ax(_nd + _n); static adoublev ax; ax.alloc(_nd + _n); // adoublev adx(_nd + _n); static adoublev adx; adx.alloc(_nd + _n); // adoublev au(_nu); static adoublev au; au.alloc(_nu); // adoublev aF(_nd + _n); static adoublev aF; aF.alloc(_nd + _n); for (i = 0; i < _nd; i++) adx[i] = 0.0; for (i = _nd; i < _nxt; i++) aF[i] = 0.0; if (_sa) trace_on(3); // tape 3 ax <<= _x->ve; for (i = 0; i < _n; i++) adx[_nd + i] <<= _x->ve[_nd + _n + i]; au <<= _x->ve + _nd + 2 * _n; _sys->continuous(_kk, t, ax, au, adx, aF); for (i = _nd; i < _nxt; i++) { aF[i] >>= f[i - _nd]; f[i - _nd] /= -Ft.Jdx[i][i]; } if (_sa) { trace_off(); int nindep = _nd + 2 * _n + _nu; int npar = _nx + _nu; m_zero(_X2); for (i = 0; i < _nd; i++) { _X2[i][i] = 1.0; } for (i = 0; i < _n; i++) { for (j = 0; j < npar; j++) { _X2[_nd + i][j] = y[(1 + j) * _n + i]; _X2[_nd + _n + i][j] = 0.0; // yprime[(1 + j) * _n + i]; } } for (i = 0; i < _nu; i++) { _X2[_nd + 2 * _n + i][_nd + _n + i] = 1.0; } forward(3, _n, nindep, npar, _x->ve, _X2->me, f->ve, _Y2->me); for (i = _nd; i < _nxt; i++) { f[i - _nd] /= -Ft.Jdx[i][i]; for (j = 0; j < npar; j++) { f[(1 + j) * _n + i - _nd] = _Y2[i - _nd][j] / -Ft.Jdx[i][i]; } } } _res_evals++; if (_sa) _sen_evals++; #else m_error(E_NULL, "Omu_IntODE::syseq_forward: was compiled without ADOL-C"); #endif }
void if_method() { m_error(E_INPUT, "A::if_method"); }
//-------------------------------------------------------------------------- SPMAT *Hqp_IpRedSpBKP::sub_CTC(const PERM *px, SPMAT *Q) // return Q - _CT * diag(_zw) * _CT' // read: _CT, _zw // write: _scale { int i, j, j_idx, j_end; int qi, qj, qj_idx; SPROW *crow, *qrow; Real sum, val; IVEC neigh_header; IVEC *neigh = &neigh_header; assert(Q->n == Q->m); assert((int)px->size == Q->m && Q->m >= _n); if (!Q->flag_diag) sp_diag_access(Q); neigh_header.max_dim = 0; crow = _CT->row; for (i=0; i<_n; i++, crow++) { qrow = Q->row + px->pe[i]; if (crow->len <= 0) { val = qrow->elt[qrow->diag].val; _scale->ve[i] = min(1.0, sqrt(-1.0 / val)); } else { // calculate diagonal entry sum = sprow_inprod(crow, _zw, crow); j_idx = qrow->diag; val = qrow->elt[j_idx].val -= sum; _scale->ve[i] = min(1.0, sqrt(-1.0 / val)); // calculate resting entries neigh->ive = _CTC_neighs->ive + _CTC_neigh_start->ive[i]; neigh->dim = _CTC_neigh_start->ive[i + 1] - _CTC_neigh_start->ive[i]; j_end = neigh->dim; for (j_idx = 0; j_idx < j_end; j_idx++) { j = neigh->ive[j_idx]; if (j < i) { sum = sprow_inprod(crow, _zw, _CT->row + j); qi = px->pe[i]; qj = px->pe[j]; // substract sum from Qij or Qji (entry from upper part only) if (qi < qj) { qrow = Q->row + qi; qj_idx = sprow_idx(qrow, qj); if (qj_idx < 0) { // the structure must already have been allocated in init() m_error(E_INTERN, "Hqp_IpRedSpBKP"); } else { qrow->elt[qj_idx].val -= sum; } } else { qrow = Q->row + qj; qj_idx = sprow_idx(qrow, qi); if (qj_idx < 0) { // the structure must already have been allocated in init() m_error(E_INTERN, "Hqp_IpRedSpBKP"); } else { qrow->elt[qj_idx].val -= sum; } } } } } } return Q; }
//-------------------------------------------------------------------------- void Omu_IntRKsuite::ode_solve(double tstart, VECP y, const VECP u, double tend) { if (_sa) _neq = y->dim; else _neq = _n; _npar = u->dim; v_resize(_work, 32 * _neq); v_resize(_thres, _neq); v_resize(_u, _npar); v_resize(_yp, _neq); _y_head.dim = _neq; _yp_head.dim = _neq; v_set(_thres, _atol); // exclude sensitivities from error test if (!_serr) { for (int i = _n; i < (int)_thres->dim; i++) _thres[i] = 1e128; } v_zero(_yp); v_copy(u, _u); integer NEQ = _neq; // a hack to reinitialize _hnext in each simulation if (tstart < _tlast) _hnext = 0.0; _tlast = tstart; dreal TNOW = tstart; integer CFLAG; //integer METHOD = (_rtol > 5e-4)? 1: (_rtol > 5e-6)? 2: 3; integer METHOD = _method; char TASK = 'c'; logical ERRASS = 0; dreal HSTART = _hnext; integer LENWRK = _work->dim; logical MESAGE = 0; setup_(&NEQ, &tstart, y->ve, &tend, &_rtol, _thres->ve, &METHOD, &TASK, &ERRASS, &HSTART, _work->ve, &LENWRK, &MESAGE); while (TNOW < tend) { ct_(&::F, &TNOW, y->ve, _yp->ve, _work->ve, &CFLAG); if (CFLAG > 1) fprintf(stderr, "RKsuite message %d at time %g\n", CFLAG, TNOW); if (CFLAG > 4) m_error(E_CONV, "Omu_IntRKsuite::step"); } integer TOTF, STPCST, STPSOK; dreal WASTE; stat_(&TOTF, &STPCST, &WASTE, &STPSOK, &HSTART); _hnext = HSTART; }