Beispiel #1
0
//--------------------------------------------------------------------------
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();
}
Beispiel #2
0
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;
}
Beispiel #3
0
/* 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);
}
Beispiel #4
0
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));
}
Beispiel #6
0
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);
}
Beispiel #7
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);
}
Beispiel #8
0
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;
}
Beispiel #9
0
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;
}
Beispiel #10
0
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);
}
Beispiel #11
0
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);
}
Beispiel #12
0
//--------------------------------------------------------------------------
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;
}
Beispiel #13
0
//--------------------------------------------------------------------------
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);
    }
  }
}
Beispiel #14
0
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");
}
Beispiel #15
0
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;
    }
}
Beispiel #16
0
void
m_end(struct msg *m)
{
	if (m->pos != m->end)
		m_error("not at msg end");
}
Beispiel #17
0
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;
    }
}
Beispiel #18
0
// 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
}
Beispiel #19
0
 void if_method()
 {
   m_error(E_INPUT, "A::if_method");
 }
Beispiel #20
0
//--------------------------------------------------------------------------
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;  
}
Beispiel #21
0
//--------------------------------------------------------------------------
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;
}