typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
multi_gp_cholesky_log(const Eigen::Matrix
<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
const Eigen::Matrix
<T_covar, Eigen::Dynamic, Eigen::Dynamic>& L,
const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) {
static const char* function("multi_gp_cholesky_log");
typedef
typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type T_lp;
T_lp lp(0.0);
check_size_match(function,
"Size of random variable (rows y)", y.rows(),
"Size of kernel scales (w)", w.size());
check_size_match(function,
"Size of random variable", y.cols(),
"rows of covariance parameter", L.rows());
check_finite(function, "Kernel scales", w);
check_positive(function, "Kernel scales", w);
check_finite(function, "Random variable", y);
if (y.rows() == 0)
return lp;
if (include_summand<propto>::value) {
lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
}
if (include_summand<propto, T_covar>::value) {
lp -= L.diagonal().array().log().sum() * y.rows();
}
if (include_summand<propto, T_w>::value) {
lp += 0.5 * y.cols() * sum(log(w));
}
if (include_summand<propto, T_y, T_w, T_covar>::value) {
T_lp sum_lp_vec(0.0);
for (int i = 0; i < y.rows(); i++) {
Eigen::Matrix<T_y, Eigen::Dynamic, 1> y_row(y.row(i));
Eigen::Matrix<typename boost::math::tools::promote_args
<T_y, T_covar>::type,
Eigen::Dynamic, 1>
half(mdivide_left_tri_low(L, y_row));
sum_lp_vec += w(i) * dot_self(half);
}
lp -= 0.5*sum_lp_vec;
}
return lp;
}
typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
multi_gp_log(const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
const Eigen::Matrix
<T_covar, Eigen::Dynamic, Eigen::Dynamic>& Sigma,
const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) {
static const char* function("multi_gp_log");
typedef typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
T_lp;
T_lp lp(0.0);
check_positive(function, "Kernel rows", Sigma.rows());
check_finite(function, "Kernel", Sigma);
check_symmetric(function, "Kernel", Sigma);
LDLT_factor<T_covar, Eigen::Dynamic, Eigen::Dynamic> ldlt_Sigma(Sigma);
check_ldlt_factor(function, "LDLT_Factor of Sigma", ldlt_Sigma);
check_size_match(function,
"Size of random variable (rows y)", y.rows(),
"Size of kernel scales (w)", w.size());
check_size_match(function,
"Size of random variable", y.cols(),
"rows of covariance parameter", Sigma.rows());
check_positive_finite(function, "Kernel scales", w);
check_finite(function, "Random variable", y);
if (y.rows() == 0)
return lp;
if (include_summand<propto>::value) {
lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
}
if (include_summand<propto, T_covar>::value) {
lp -= 0.5 * log_determinant_ldlt(ldlt_Sigma) * y.rows();
}
if (include_summand<propto, T_w>::value) {
lp += (0.5 * y.cols()) * sum(log(w));
}
if (include_summand<propto, T_y, T_w, T_covar>::value) {
Eigen::Matrix<T_w, Eigen::Dynamic, Eigen::Dynamic>
w_mat(w.asDiagonal());
Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic> yT(y.transpose());
lp -= 0.5 * trace_gen_inv_quad_form_ldlt(w_mat, ldlt_Sigma, yT);
}
return lp;
}
// Driver function to find the limits of a slice containing 'x'.
// Logic varies according to whether the distribution is bounded
// above, below, both, or neither.
void SSS::find_limits(double x){
logp_slice_ = logf_(x) - rexp_mt(rng(), 1.0);
check_finite(x,logp_slice_);
bool limits_successfully_found = true;
if(doubly_bounded()){
lo_ = lower_bound_;
logplo_ = logf_(lo_);
hi_ = upper_bound_;
logphi_ = logf_(hi_);
}else if (lower_bounded()){
lo_ = lower_bound_;
logplo_ = logf_(lo_);
limits_successfully_found = find_upper_limit(x);
}else if(upper_bounded()){
limits_successfully_found = find_lower_limit(x);
hi_ = upper_bound_;
logphi_ = logf_(hi_);
}else{ // unbounded
limits_successfully_found = find_limits_unbounded(x);
}
check_slice(x);
if (limits_successfully_found) {
check_probs(x);
}
}
typename boost::math::tools::promote_args<T_prob>::type
categorical_logit_log(const std::vector<int>& ns,
const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>&
beta) {
static const char* function("categorical_logit_log");
for (size_t k = 0; k < ns.size(); ++k)
check_bounded(function, "categorical outcome out of support",
ns[k], 1, beta.size());
check_finite(function, "log odds parameter", beta);
if (!include_summand<propto, T_prob>::value)
return 0.0;
if (ns.size() == 0)
return 0.0;
Eigen::Matrix<T_prob, Eigen::Dynamic, 1> log_softmax_beta
= log_softmax(beta);
// FIXME: replace with more efficient sum()
Eigen::Matrix<typename boost::math::tools::promote_args<T_prob>::type,
Eigen::Dynamic, 1> results(ns.size());
for (size_t i = 0; i < ns.size(); ++i)
results[i] = log_softmax_beta(ns[i] - 1);
return sum(results);
}
int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
{
int ret = 0;
if (check_finite(kp)) {
pid->kp = kp;
} else {
ret = 1;
}
if (check_finite(ki)) {
pid->ki = ki;
} else {
ret = 1;
}
if (check_finite(kd)) {
pid->kd = kd;
} else {
ret = 1;
}
if (check_finite(limit_min)) {
pid->limit_min = limit_min;
} else {
ret = 1;
}
if (check_finite(limit_max)) {
pid->limit_max = limit_max;
} else {
ret = 1;
}
return ret;
}
typename boost::math::tools::promote_args<T_prob>::type
categorical_logit_log(int n,
const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>&
beta) {
static const char* function("categorical_logit_log");
check_bounded(function, "categorical outcome out of support", n,
1, beta.size());
check_finite(function, "log odds parameter", beta);
if (!include_summand<propto, T_prob>::value)
return 0.0;
// FIXME: wasteful vs. creating term (n-1) if not vectorized
return beta(n-1) - log_sum_exp(beta); // == log_softmax(beta)(n-1);
}
float _wrap_360(float bearing)
{
/* value is inf or NaN */
if (!check_finite(bearing)) {
return bearing;
}
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
}
while (bearing < 0.0f) {
bearing = bearing + 360.0f;
}
return bearing;
}
float _wrap_2pi(float bearing)
{
/* value is inf or NaN */
if (!check_finite(bearing)) {
return bearing;
}
while (bearing >= M_TWOPI) {
bearing = bearing - M_TWOPI;
}
while (bearing < 0.0f) {
bearing = bearing + M_TWOPI;
}
return bearing;
}
float _wrap_pi(float bearing)
{
/* value is inf or NaN */
if (!check_finite(bearing) || bearing == 0) {
return bearing;
}
int c = 0;
while (bearing > M_PI && c < 30) {
bearing -= M_TWOPI;
c++;
}
c = 0;
while (bearing <= -M_PI && c < 30) {
bearing += M_TWOPI;
c++;
}
return bearing;
}
float ECL_pitch_controller_control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
bool_t lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = absolute_elapsed_time(&epc._last_run);
epc._last_run = get_absolute_time();
float dt = dt_micros / 1000000;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = 1;
float k_roll_ff = max((epc._k_p - epc._k_i * epc._tc) * epc._tc - epc._k_d, 0.0f);
float k_i_rate = epc._k_i * epc._tc;
/* input conditioning */
//airspeed = constrain (airspeed, airspeed_min, airspeed_max);
if (!check_finite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* flying inverted (wings upside down) ? */
bool_t inverted = 0;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(roll) < radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
roll = constrain(roll, radians(-80.0f), radians(80.0f));
} else {
/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
if (roll > 0.0f) {
/* right hemisphere */
roll = constrain(roll, radians(100.0f), radians(180.0f));
} else {
/* left hemisphere */
roll = constrain(roll, radians(-100.0f), radians(-180.0f));
}
}
/* calculate the offset in the rate resulting from rolling */
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * epc._roll_ff;
if (inverted)
turn_offset = -turn_offset;
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
epc._rate_setpoint = pitch_error / epc._tc;
/* add turn offset */
epc._rate_setpoint += turn_offset;
epc._rate_error = epc._rate_setpoint - pitch_rate;
float ilimit_scaled = 0.0f;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = epc._rate_error * k_i_rate * dt * scaler;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
*/
if (epc._last_output < -epc._max_deflection_rad) {
/* only allow motion to center: increase value */
id = max(id, 0.0f);
} else if (epc._last_output > epc._max_deflection_rad) {
/* only allow motion to center: decrease value */
id = min(id, 0.0f);
}
epc._integrator += id;
}
/* integrator limit */
epc._integrator = constrain(epc._integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
epc._last_output = ((epc._rate_error * epc._k_d * scaler) + epc._integrator + (epc._rate_setpoint * k_roll_ff)) * scaler;
return constrain(epc._last_output, -epc._max_deflection_rad, epc._max_deflection_rad);
}
float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
{
/* Alternative integral component calculation
*
* start:
* error = setpoint - current_value
* integral = integral + (Ki * error * dt)
* derivative = (error - previous_error) / dt
* previous_error = error
* output = (Kp * error) + integral + (Kd * derivative)
* wait(dt)
* goto start
*/
if (!check_finite(sp) || !check_finite(val) || !check_finite(dt)) {
return pid->last_output;
}
float i, d, output, att_comp, error;
pid->sp = sp;
// Calculated current error value
error = pid->sp - val;
// Calculate or measured current error derivative
if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
} else {
d = 0.0f;
}
if (!check_finite(d)) {
d = 0.0f;
}
/* calculate the error integral */
i = pid->integral + (pid->ki * error * dt);
/* attitude-thrust compensation
* r22 is (2, 2) componet of rotation matrix for current attitude */
if (r22 > 0.8f)
att_comp = 1.0f / r22;
else if (r22 > 0.0f)
att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
else
att_comp = 1.0f;
/* calculate output */
output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
//fprintf (stderr, "output:%.3f\terror:%.3f\tpid_kp:%.3f\tpid_ki:%.3f\tpid_kd:%.3f\tvz_sp:%.3f\tcurr_vz:%.3f\tatt_comp:%.3f\n", output, error, pid->kp, i, pid->kd, sp, val, att_comp);
/* check for saturation */
if (output < pid->limit_min || output > pid->limit_max) {
/* saturated, recalculate output with old integral */
output = (error * pid->kp) + pid->integral + (d * pid->kd);
} else {
if (check_finite(i)) {
pid->integral = i;
}
}
if (check_finite(output)) {
if (output > pid->limit_max) {
output = pid->limit_max;
} else if (output < pid->limit_min) {
output = pid->limit_min;
}
pid->last_output = output;
}
return pid->last_output;
}
typename return_type<T_y, T_loc, T_covar>::type
multi_normal_cholesky_log(const T_y& y,
const T_loc& mu,
const T_covar& L) {
static const char* function("multi_normal_cholesky_log");
typedef typename scalar_type<T_covar>::type T_covar_elem;
typedef typename return_type<T_y, T_loc, T_covar>::type lp_type;
lp_type lp(0.0);
VectorViewMvt<const T_y> y_vec(y);
VectorViewMvt<const T_loc> mu_vec(mu);
size_t size_vec = max_size_mvt(y, mu);
int size_y = y_vec[0].size();
int size_mu = mu_vec[0].size();
if (size_vec > 1) {
int size_y_old = size_y;
int size_y_new;
for (size_t i = 1, size_ = length_mvt(y); i < size_; i++) {
int size_y_new = y_vec[i].size();
check_size_match(function,
"Size of one of the vectors of "
"the random variable", size_y_new,
"Size of another vector of the "
"random variable", size_y_old);
size_y_old = size_y_new;
}
int size_mu_old = size_mu;
int size_mu_new;
for (size_t i = 1, size_ = length_mvt(mu); i < size_; i++) {
int size_mu_new = mu_vec[i].size();
check_size_match(function,
"Size of one of the vectors of "
"the location variable", size_mu_new,
"Size of another vector of the "
"location variable", size_mu_old);
size_mu_old = size_mu_new;
}
(void) size_y_old;
(void) size_y_new;
(void) size_mu_old;
(void) size_mu_new;
}
check_size_match(function,
"Size of random variable", size_y,
"size of location parameter", size_mu);
check_size_match(function,
"Size of random variable", size_y,
"rows of covariance parameter", L.rows());
check_size_match(function,
"Size of random variable", size_y,
"columns of covariance parameter", L.cols());
for (size_t i = 0; i < size_vec; i++) {
check_finite(function, "Location parameter", mu_vec[i]);
check_not_nan(function, "Random variable", y_vec[i]);
}
if (size_y == 0)
return lp;
if (include_summand<propto>::value)
lp += NEG_LOG_SQRT_TWO_PI * size_y * size_vec;
if (include_summand<propto, T_covar_elem>::value)
lp -= L.diagonal().array().log().sum() * size_vec;
if (include_summand<propto, T_y, T_loc, T_covar_elem>::value) {
lp_type sum_lp_vec(0.0);
for (size_t i = 0; i < size_vec; i++) {
Eigen::Matrix<typename return_type<T_y, T_loc>::type,
Eigen::Dynamic, 1> y_minus_mu(size_y);
for (int j = 0; j < size_y; j++)
y_minus_mu(j) = y_vec[i](j)-mu_vec[i](j);
Eigen::Matrix<typename return_type<T_y, T_loc, T_covar>::type,
Eigen::Dynamic, 1>
half(mdivide_left_tri_low(L, y_minus_mu));
// FIXME: this code does not compile. revert after fixing subtract()
// Eigen::Matrix<typename
// boost::math::tools::promote_args<T_covar,
// typename value_type<T_loc>::type,
// typename value_type<T_y>::type>::type>::type,
// Eigen::Dynamic, 1>
// half(mdivide_left_tri_low(L, subtract(y, mu)));
sum_lp_vec += dot_self(half);
}
lp -= 0.5*sum_lp_vec;
}
return lp;
}
inline bool check_finite(const char* function,
const T_y& y,
const char* name,
T_result* result) {
return check_finite(function,y,name,result,default_policy());
}
SEXP hitrun(SEXP alpha, SEXP initial, SEXP nbatch, SEXP blen, SEXP nspac,
SEXP origin, SEXP basis, SEXP amat, SEXP bvec, SEXP outmat, SEXP debug)
{
if (! isReal(alpha))
error("argument \"alpha\" must be type double");
if (! isReal(initial))
error("argument \"initial\" must be type double");
if (! isInteger(nbatch))
error("argument \"nbatch\" must be type integer");
if (! isInteger(blen))
error("argument \"blen\" must be type integer");
if (! isInteger(nspac))
error("argument \"nspac\" must be type integer");
if (! isReal(origin))
error("argument \"origin\" must be type double");
if (! isReal(basis))
error("argument \"basis\" must be type double");
if (! isReal(amat))
error("argument \"amat\" must be type double");
if (! isReal(bvec))
error("argument \"bvec\" must be type double");
if (! (isNull(outmat) | isReal(outmat)))
error("argument \"outmat\" must be type double or NULL");
if (! isLogical(debug))
error("argument \"debug\" must be logical");
if (! isMatrix(basis))
error("argument \"basis\" must be matrix");
if (! isMatrix(amat))
error("argument \"amat\" must be matrix");
if (! (isNull(outmat) | isMatrix(outmat)))
error("argument \"outmat\" must be matrix or NULL");
int dim_oc = LENGTH(alpha);
int dim_nc = LENGTH(initial);
int ncons = nrows(amat);
if (LENGTH(nbatch) != 1)
error("argument \"nbatch\" must be scalar");
if (LENGTH(blen) != 1)
error("argument \"blen\" must be scalar");
if (LENGTH(nspac) != 1)
error("argument \"nspac\" must be scalar");
if (LENGTH(origin) != dim_oc)
error("length(origin) != length(alpha)");
if (nrows(basis) != dim_oc)
error("nrow(basis) != length(alpha)");
if (ncols(basis) != dim_nc)
error("ncol(basis) != length(initial)");
if (ncols(amat) != dim_nc)
error("ncol(amat) != length(initial)");
if (LENGTH(bvec) != ncons)
error("length(bvec) != nrow(amat)");
if (LENGTH(debug) != 1)
error("argument \"debug\" must be scalar");
int dim_out = dim_oc;
if (! isNull(outmat)) {
dim_out = nrows(outmat);
if (ncols(outmat) != dim_oc)
error("ncol(outmat) != length(alpha)");
}
int int_nbatch = INTEGER(nbatch)[0];
int int_blen = INTEGER(blen)[0];
int int_nspac = INTEGER(nspac)[0];
int int_debug = LOGICAL(debug)[0];
double *dbl_star_alpha = REAL(alpha);
double *dbl_star_initial = REAL(initial);
double *dbl_star_origin = REAL(origin);
double *dbl_star_basis = REAL(basis);
double *dbl_star_amat = REAL(amat);
double *dbl_star_bvec = REAL(bvec);
int has_outmat = isMatrix(outmat);
double *dbl_star_outmat = 0;
if (has_outmat)
dbl_star_outmat = REAL(outmat);
if (int_nbatch <= 0)
error("argument \"nbatch\" must be positive");
if (int_blen <= 0)
error("argument \"blen\" must be positive");
if (int_nspac <= 0)
error("argument \"nspac\" must be positive");
check_finite(dbl_star_alpha, dim_oc, "alpha");
check_positive(dbl_star_alpha, dim_oc, "alpha");
check_finite(dbl_star_initial, dim_nc, "initial");
check_finite(dbl_star_origin, dim_oc, "origin");
check_finite(dbl_star_basis, dim_oc * dim_nc, "basis");
check_finite(dbl_star_amat, ncons * dim_nc, "amat");
check_finite(dbl_star_bvec, ncons, "bvec");
if (has_outmat)
check_finite(dbl_star_outmat, dim_out * dim_oc, "outmat");
double *state = (double *) R_alloc(dim_nc, sizeof(double));
double *proposal = (double *) R_alloc(dim_nc, sizeof(double));
double *batch_buffer = (double *) R_alloc(dim_out, sizeof(double));
double *out_buffer = (double *) R_alloc(dim_out, sizeof(double));
memcpy(state, dbl_star_initial, dim_nc * sizeof(double));
logh_setup(dbl_star_alpha, dbl_star_origin, dbl_star_basis, dim_oc, dim_nc);
double current_log_dens = logh(state);
out_setup(dbl_star_origin, dbl_star_basis, dbl_star_outmat, dim_oc, dim_nc,
dim_out, has_outmat);
SEXP result, resultnames, path, save_initial, save_final;
if (! int_debug) {
PROTECT(result = allocVector(VECSXP, 3));
PROTECT(resultnames = allocVector(STRSXP, 3));
} else {
PROTECT(result = allocVector(VECSXP, 11));
PROTECT(resultnames = allocVector(STRSXP, 11));
}
PROTECT(path = allocMatrix(REALSXP, dim_out, int_nbatch));
SET_VECTOR_ELT(result, 0, path);
PROTECT(save_initial = duplicate(initial));
SET_VECTOR_ELT(result, 1, save_initial);
UNPROTECT(2);
SET_STRING_ELT(resultnames, 0, mkChar("batch"));
SET_STRING_ELT(resultnames, 1, mkChar("initial"));
SET_STRING_ELT(resultnames, 2, mkChar("final"));
if (int_debug) {
SEXP spath, ppath, zpath, u1path, u2path, s1path, s2path, gpath;
int nn = int_nbatch * int_blen * int_nspac;
PROTECT(spath = allocMatrix(REALSXP, dim_nc, nn));
SET_VECTOR_ELT(result, 3, spath);
PROTECT(ppath = allocMatrix(REALSXP, dim_nc, nn));
SET_VECTOR_ELT(result, 4, ppath);
PROTECT(zpath = allocMatrix(REALSXP, dim_nc, nn));
SET_VECTOR_ELT(result, 5, zpath);
PROTECT(u1path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 6, u1path);
PROTECT(u2path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 7, u2path);
PROTECT(s1path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 8, s1path);
PROTECT(s2path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 9, s2path);
PROTECT(gpath = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 10, gpath);
UNPROTECT(8);
SET_STRING_ELT(resultnames, 3, mkChar("current"));
SET_STRING_ELT(resultnames, 4, mkChar("proposal"));
SET_STRING_ELT(resultnames, 5, mkChar("z"));
SET_STRING_ELT(resultnames, 6, mkChar("u1"));
SET_STRING_ELT(resultnames, 7, mkChar("u2"));
SET_STRING_ELT(resultnames, 8, mkChar("s1"));
SET_STRING_ELT(resultnames, 9, mkChar("s2"));
SET_STRING_ELT(resultnames, 10, mkChar("log.green"));
}
namesgets(result, resultnames);
UNPROTECT(1);
GetRNGstate();
if (current_log_dens == R_NegInf)
error("log unnormalized density -Inf at initial state");
for (int ibatch = 0, k = 0; ibatch < int_nbatch; ibatch++) {
for (int i = 0; i < dim_out; i++)
batch_buffer[i] = 0.0;
for (int jbatch = 0; jbatch < int_blen; jbatch++) {
double proposal_log_dens;
for (int ispac = 0; ispac < int_nspac; ispac++) {
/* Note: should never happen! */
if (current_log_dens == R_NegInf)
error("log density -Inf at current state");
double u1 = R_NaReal;
double u2 = R_NaReal;
double smax = R_NaReal;
double smin = R_NaReal;
double z[dim_nc];
propose(state, proposal, dbl_star_amat, dbl_star_bvec,
dim_nc, ncons, z, &smax, &smin, &u1);
proposal_log_dens = logh(proposal);
int accept = FALSE;
if (proposal_log_dens != R_NegInf) {
if (proposal_log_dens >= current_log_dens) {
accept = TRUE;
} else {
double green = exp(proposal_log_dens
- current_log_dens);
u2 = unif_rand();
accept = u2 < green;
}
}
if (int_debug) {
int l = ispac + int_nspac * (jbatch + int_blen * ibatch);
int lbase = l * dim_nc;
SEXP spath = VECTOR_ELT(result, 3);
SEXP ppath = VECTOR_ELT(result, 4);
SEXP zpath = VECTOR_ELT(result, 5);
SEXP u1path = VECTOR_ELT(result, 6);
SEXP u2path = VECTOR_ELT(result, 7);
SEXP s1path = VECTOR_ELT(result, 8);
SEXP s2path = VECTOR_ELT(result, 9);
SEXP gpath = VECTOR_ELT(result, 10);
for (int lj = 0; lj < dim_nc; lj++) {
REAL(spath)[lbase + lj] = state[lj];
REAL(ppath)[lbase + lj] = proposal[lj];
REAL(zpath)[lbase + lj] = z[lj];
}
REAL(u1path)[l] = u1;
REAL(u2path)[l] = u2;
REAL(s1path)[l] = smin;
REAL(s2path)[l] = smax;
REAL(gpath)[l] = proposal_log_dens - current_log_dens;
}
if (accept) {
memcpy(state, proposal, dim_nc * sizeof(double));
current_log_dens = proposal_log_dens;
}
} /* end of inner loop (one iteration) */
outfun(state, out_buffer);
for (int j = 0; j < dim_out; j++)
batch_buffer[j] += out_buffer[j];
} /* end of middle loop (one batch) */
for (int j = 0; j < dim_out; j++, k++)
REAL(path)[k] = batch_buffer[j] / int_blen;
} /* end of outer loop */
PutRNGstate();
PROTECT(save_final = allocVector(REALSXP, dim_nc));
memcpy(REAL(save_final), state, dim_nc * sizeof(double));
SET_VECTOR_ELT(result, 2, save_final);
UNPROTECT(5);
return result;
}
