void MulticopterAttitudeControl::task_main() { /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); /* wakeup source: vehicle attitude */ struct pollfd fds[1]; fds[0].fd = _v_att_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude topic */ orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } if (_v_control_mode.flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if(_controller_status_pub > 0) { orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } } perf_end(_loop_perf); } warnx("exit"); _control_task = -1; _exit(0); }
__complex__ long double __ctanhl (__complex__ long double x) { __complex__ long double res; if (!isfinite (__real__ x) || !isfinite (__imag__ x)) { if (isinf (__real__ x)) { __real__ res = __copysignl (1.0L, __real__ x); __imag__ res = __copysignl (0.0L, __imag__ x); } else if (__imag__ x == 0.0) { res = x; } else { __real__ res = __nanl (""); __imag__ res = __nanl (""); #ifdef FE_INVALID if (isinf (__imag__ x)) feraiseexcept (FE_INVALID); #endif } } else { long double sinix, cosix; long double den; const int t = (int) ((LDBL_MAX_EXP - 1) * M_LN2l / 2.0L); /* tanh(x+iy) = (sinh(2x) + i*sin(2y))/(cosh(2x) + cos(2y)) = (sinh(x)*cosh(x) + i*sin(y)*cos(y))/(sinh(x)^2 + cos(y)^2). */ __sincosl (__imag__ x, &sinix, &cosix); if (fabsl (__real__ x) > t) { /* Avoid intermediate overflow when the imaginary part of the result may be subnormal. Ignoring negligible terms, the real part is +/- 1, the imaginary part is sin(y)*cos(y)/sinh(x)^2 = 4*sin(y)*cos(y)/exp(2x). */ long double exp_2t = __ieee754_expl (2 * t); __real__ res = __copysignl (1.0L, __real__ x); __imag__ res = 4 * sinix * cosix; __real__ x = fabsl (__real__ x); __real__ x -= t; __imag__ res /= exp_2t; if (__real__ x > t) { /* Underflow (original real part of x has absolute value > 2t). */ __imag__ res /= exp_2t; } else __imag__ res /= __ieee754_expl (2.0L * __real__ x); } else { long double sinhrx, coshrx; if (fabs (__real__ x) > LDBL_MIN) { sinhrx = __ieee754_sinhl (__real__ x); coshrx = __ieee754_coshl (__real__ x); } else { sinhrx = __real__ x; coshrx = 1.0L; } if (fabsl (sinhrx) > fabsl (cosix) * ldbl_eps) den = sinhrx * sinhrx + cosix * cosix; else den = cosix * cosix; __real__ res = sinhrx * (coshrx / den); __imag__ res = sinix * (cosix / den); } /* __gcc_qmul does not respect -0.0 so we need the following fixup. */ if ((__real__ res == 0.0L) && (__real__ x == 0.0L)) __real__ res = __real__ x; if ((__real__ res == 0.0L) && (__imag__ x == 0.0L)) __imag__ res = __imag__ x; } return res; }
int tJSolve::solveGS(const double eps, const int maxIterations) { // Gauss Seidel Verfahren int i,j,k,iteration; double err,s,xx,relax; double **a, *b, *x0; if (eps <= 0.) return -1; relax = 1.3; // Speicher fuer eine Kopie von MATRIX und RHS anlegen a = (double**)malloc(N*sizeof(double*)); b = (double*)malloc(N*nRHS*sizeof(double)); x0 = (double*)malloc(N*nRHS*sizeof(double)); for (i=0;i<N;i++) a[i] = (double*)malloc(N*sizeof(double)); for (i=0;i<N;i++){ //Kopie von MATRIX und RHS erzeugen if (MATRIX[i][i] != 0.){ xx = MATRIX[i][i]; for (j=0;j<N;j++){ a[i][j] = MATRIX[i][j]/xx; } for (j=0;j<nRHS;j++){ b[j*N+i] = RHS[j*N+i]/xx; } } else { //Ein Element auf der Hauptdiagonalen ist null for (k=0; MATRIX[k][i]== 0. && k<N;k++); //Eine Zeile wird subtrahiert, so dass die Hauptdiagonale nicht mehr null ist. if (k<N){ xx = MATRIX[k][i]; for (j=0;j<N;j++){ a[i][j] = (MATRIX[i][j] + MATRIX[k][j])/xx; } for (j=0;j<nRHS;j++){ b[j*N+i] = (RHS[j*N+i] + RHS[j*N+k])/xx; } } else { return -1; } } } iteration = 0; do { err = 0.; for (k=0;k<nRHS*N;k+=N){ for (i=0;i<N;i++){ s = 0.; for (j=0; j<i; j++) s += a[i][j]*x[k+j]; for (j=i+1; j<N; j++) s += a[i][j]*x[k+j]; x0[k+i] = x[k+i]; x[k+i] = b[k+i]-s; err = max(fabs(x[k+i]-x0[k+i]),double(err)); } } iteration++; if (err > eps){ for (k=0;k<nRHS*N;k+=N){ for (i=0;i<N;i++){ x[k+i] = (x[k+i]-x0[k+i])*relax + x0[k+i]; if (x[k+i]*x0[k+i] < 0){ x[k+i] = (x[k+i]+x0[k+i])*0.5; } } } } } while (err > eps && isfinite(err) && (iteration<maxIterations || maxIterations<=0)); for (i=0;i<N;i++) free(a[i]); free(a); free(b); free(x0); if (!isfinite(err)){ return -1; } else { return iteration; } }
long double __kernel_standard_l (long double x, long double y, int type) { double dx, dy; struct exception exc; if (isfinite (x)) { long double ax = fabsl (x); if (ax > DBL_MAX) dx = __copysignl (DBL_MAX, x); else if (ax > 0 && ax < DBL_MIN) dx = __copysignl (DBL_MIN, x); else dx = x; } else dx = x; if (isfinite (y)) { long double ay = fabsl (y); if (ay > DBL_MAX) dy = __copysignl (DBL_MAX, y); else if (ay > 0 && ay < DBL_MIN) dy = __copysignl (DBL_MIN, y); else dy = y; } else dy = y; switch (type) { case 221: /* powl (x, y) overflow. */ exc.arg1 = dx; exc.arg2 = dy; exc.type = OVERFLOW; exc.name = "powl"; if (_LIB_VERSION == _SVID_) { exc.retval = HUGE; y *= 0.5; if (x < zero && __rintl (y) != y) exc.retval = -HUGE; } else { exc.retval = HUGE_VAL; y *= 0.5; if (x < zero && __rintl (y) != y) exc.retval = -HUGE_VAL; } if (_LIB_VERSION == _POSIX_) __set_errno (ERANGE); else if (!matherr (&exc)) __set_errno (ERANGE); return exc.retval; case 222: /* powl (x, y) underflow. */ exc.arg1 = dx; exc.arg2 = dy; exc.type = UNDERFLOW; exc.name = "powl"; exc.retval = zero; y *= 0.5; if (x < zero && __rintl (y) != y) exc.retval = -zero; if (_LIB_VERSION == _POSIX_) __set_errno (ERANGE); else if (!matherr (&exc)) __set_errno (ERANGE); return exc.retval; default: return __kernel_standard (dx, dy, type); } }
static void debug_print_flonum(lref_t object, lref_t port, bool machine_readable) { _TCHAR buf[STACK_STRBUF_LEN]; UNREFERENCED(machine_readable); assert(FLONUMP(object)); if (isnan(FLONM(object))) { _sntprintf(buf, STACK_STRBUF_LEN, _T("#inan")); } else if (!isfinite(FLONM(object))) { if (FLONM(object) > 0) _sntprintf(buf, STACK_STRBUF_LEN, _T("#iposinf")); else _sntprintf(buf, STACK_STRBUF_LEN, _T("#ineginf")); } else { int digits = DEBUG_FLONUM_PRINT_PRECISION; assert((digits >= 0) && (digits <= 16)); /* Nothing is as easy as it seems... * * The sprintf 'g' format code will drop the decimal * point if all following digits are zero. That causes * the reader to read such numbers as exact, rather than * inexact. As a result, we need to implement our own * switching between scientific and conventional notation. */ double scale = 0.0; if (FLONM(object) != 0.0) scale = log10(fabs(FLONM(object))); if (fabs(scale) >= digits) _sntprintf(buf, STACK_STRBUF_LEN, _T("%.*e"), digits, FLONM(object)); else { /* Prevent numbers on the left of the decimal point from * adding to the number of digits we print. */ if ((scale > 0) && (scale <= digits)) digits -= (int) scale; _sntprintf(buf, STACK_STRBUF_LEN, _T("%.*f"), digits, FLONM(object)); } } write_text(port, buf, _tcslen(buf)); if (COMPLEXP(object)) { if (CMPLXIM(object) >= 0.0) write_text(port, _T("+"), 1); debug_print_flonum(FLOIM(object), port, machine_readable); write_text(port, _T("i"), 1); } }
static floatval_t l2sgd_calibration( encoder_t *gm, dataset_t *ds, floatval_t *w, logging_t *lg, const training_option_t* opt ) { int i, s; int dec = 0, ok, trials = 1; int num = opt->calibration_candidates; clock_t clk_begin = clock(); floatval_t loss = 0.; floatval_t init_loss = 0.; floatval_t best_loss = DBL_MAX; floatval_t eta = opt->calibration_eta; floatval_t best_eta = opt->calibration_eta; const int N = ds->num_instances; const int S = MIN(N, opt->calibration_samples); const int K = gm->num_features; const floatval_t init_eta = opt->calibration_eta; const floatval_t rate = opt->calibration_rate; const floatval_t lambda = opt->lambda; logging(lg, "Calibrating the learning rate (eta)\n"); logging(lg, "calibration.eta: %f\n", eta); logging(lg, "calibration.rate: %f\n", rate); logging(lg, "calibration.samples: %d\n", S); logging(lg, "calibration.candidates: %d\n", num); logging(lg, "calibration.max_trials: %d\n", opt->calibration_max_trials); /* Initialize a permutation that shuffles the instances. */ dataset_shuffle(ds); /* Initialize feature weights as zero. */ vecset(w, 0, K); /* Compute the initial loss. */ gm->set_weights(gm, w, 1.); init_loss = 0; for (i = 0;i < S;++i) { floatval_t score; const crfsuite_instance_t *inst = dataset_get(ds, i); gm->set_instance(gm, inst); gm->score(gm, inst->labels, &score); init_loss -= score; gm->partition_factor(gm, &score); init_loss += score; } init_loss += 0.5 * lambda * vecdot(w, w, K) * N; logging(lg, "Initial loss: %f\n", init_loss); while (num > 0 || !dec) { logging(lg, "Trial #%d (eta = %f): ", trials, eta); /* Perform SGD for one epoch. */ l2sgd( gm, ds, NULL, w, lg, S, 1.0 / (lambda * eta), lambda, 1, 1, 1, 0., &loss); /* Make sure that the learning rate decreases the log-likelihood. */ ok = isfinite(loss) && (loss < init_loss); if (ok) { logging(lg, "%f\n", loss); --num; } else { logging(lg, "%f (worse)\n", loss); } if (isfinite(loss) && loss < best_loss) { best_loss = loss; best_eta = eta; } if (!dec) { if (ok && 0 < num) { eta *= rate; } else { dec = 1; num = opt->calibration_candidates; eta = init_eta / rate; } } else { eta /= rate; } ++trials; if (opt->calibration_max_trials <= trials) { break; } } eta = best_eta; logging(lg, "Best learning rate (eta): %f\n", eta); logging(lg, "Seconds required: %.3f\n", (clock() - clk_begin) / (double)CLOCKS_PER_SEC); logging(lg, "\n"); return 1.0 / (lambda * eta); }
static bool Tuning_table_build_pitch_map(Tuning_table* tt) { assert(tt != NULL); AAtree* pitch_map = new_AAtree( (int (*)(const void*, const void*))pitch_index_cmp, memory_free); if (pitch_map == NULL) return false; for (int octave = 0; octave < KQT_TUNING_TABLE_OCTAVES; ++octave) { for (int note = 0; note < tt->note_count; ++note) { pitch_index* pi = &(pitch_index){ .cents = 0 }; pi->cents = tt->ref_pitch + tt->note_offsets[note] + tt->octave_offsets[octave]; pi->note = note; pi->octave = octave; if (!AAtree_contains(pitch_map, pi)) { pitch_index* pi_entry = memory_alloc_item(pitch_index); if (pi_entry == NULL) { del_AAtree(pitch_map); return false; } *pi_entry = *pi; if (!AAtree_ins(pitch_map, pi_entry)) { del_AAtree(pitch_map); return false; } } } } if (tt->pitch_map != NULL) del_AAtree(tt->pitch_map); tt->pitch_map = pitch_map; return true; } /** * Set a new note in the Tuning table using cents. * * Any existing note at the target index will be replaced. * The note will be set at no further than the first unoccupied index. * * \param tt The Tuning table -- must not be \c NULL. * \param index The index of the note to be set -- must be >= \c 0 and * < \c KQT_TUNING_TABLE_NOTES_MAX. * \param cents The pitch ratio between the new note and reference pitch * in cents -- must be a finite value. * * \return The index that was actually set. This is never larger than * \a index. */ static void Tuning_table_set_note_cents(Tuning_table* tt, int index, double cents); void Tuning_table_set_octave_width(Tuning_table* tt, double octave_width); Tuning_table* new_Tuning_table(double ref_pitch, double octave_width) { assert(ref_pitch > 0); assert(isfinite(octave_width)); assert(octave_width > 0); Tuning_table* tt = memory_alloc_item(Tuning_table); if (tt == NULL) return NULL; tt->pitch_map = NULL; tt->note_count = 0; tt->ref_note = 0; tt->ref_pitch = ref_pitch; tt->global_offset = 0; tt->center_octave = 4; Tuning_table_set_octave_width(tt, octave_width); for (int i = 0; i < KQT_TUNING_TABLE_NOTES_MAX; ++i) tt->note_offsets[i] = 0; if (!Tuning_table_build_pitch_map(tt)) { memory_free(tt); return NULL; } return tt; }
static bool LastseenMigrationVersion0(DBHandle *db) { bool errors = false; DBCursor *cursor; if (!NewDBCursor(db, &cursor)) { return false; } char *key; void *value; int ksize, vsize; while (NextDB(cursor, &key, &ksize, &value, &vsize)) { if (ksize == 0) { Log(LOG_LEVEL_INFO, "LastseenMigrationVersion0: Database structure error -- zero-length key."); continue; } /* Only look for old [+-]kH -> IP<QPoint> entries */ if ((key[0] != '+') && (key[0] != '-')) { /* Warn about completely unexpected keys */ if ((key[0] != 'q') && (key[0] != 'k') && (key[0] != 'a')) { Log(LOG_LEVEL_INFO, "LastseenMigrationVersion0: Malformed key found '%s'", key); } continue; } bool incoming = key[0] == '-'; const char *hostkey = key + 1; /* Only migrate sane data */ if (vsize != QPOINT0_OFFSET + sizeof(QPoint0)) { Log(LOG_LEVEL_INFO, "LastseenMigrationVersion0: invalid value size for key '%s', entry is deleted", key); DBCursorDeleteEntry(cursor); continue; } /* Properly align the data */ const char *old_data_address = (const char *) value; QPoint0 old_data_q; memcpy(&old_data_q, (const char *) value + QPOINT0_OFFSET, sizeof(QPoint0)); char hostkey_key[CF_BUFSIZE]; snprintf(hostkey_key, CF_BUFSIZE, "k%s", hostkey); if (!WriteDB(db, hostkey_key, old_data_address, strlen(old_data_address) + 1)) { Log(LOG_LEVEL_INFO, "Unable to write version 1 lastseen entry for '%s'", key); errors = true; continue; } char address_key[CF_BUFSIZE]; snprintf(address_key, CF_BUFSIZE, "a%s", old_data_address); if (!WriteDB(db, address_key, hostkey, strlen(hostkey) + 1)) { Log(LOG_LEVEL_INFO, "Unable to write version 1 reverse lastseen entry for '%s'", key); errors = true; continue; } char quality_key[CF_BUFSIZE]; snprintf(quality_key, CF_BUFSIZE, "q%c%s", incoming ? 'i' : 'o', hostkey); /* Ignore malformed connection quality data */ if ((!isfinite(old_data_q.q)) || (old_data_q.q < 0) || (!isfinite(old_data_q.expect)) || (!isfinite(old_data_q.var))) { Log(LOG_LEVEL_INFO, "Ignoring malformed connection quality data for '%s'", key); DBCursorDeleteEntry(cursor); continue; } KeyHostSeen data = { .lastseen = (time_t)old_data_q.q, .Q = { /* Previously .q wasn't stored in database, but was calculated every time as a difference between previous timestamp and a new timestamp. Given we don't have this information during the database upgrade, just assume that last reading is an average one. */ .q = old_data_q.expect, .dq = 0, .expect = old_data_q.expect, .var = old_data_q.var, } }; if (!WriteDB(db, quality_key, &data, sizeof(data))) { Log(LOG_LEVEL_INFO, "Unable to write version 1 connection quality key for '%s'", key); errors = true; continue; } if (!DBCursorDeleteEntry(cursor)) { Log(LOG_LEVEL_INFO, "Unable to delete version 0 lastseen entry for '%s'", key); errors = true; } } if (DeleteDBCursor(cursor) == false) { Log(LOG_LEVEL_ERR, "LastseenMigrationVersion0: Unable to close cursor"); errors = true; } if ((!errors) && (!WriteDB(db, "version", "1", sizeof("1")))) { errors = true; } return !errors; }
void f (double *v, size_t n) { const double complex mik = -I * k; double vtmp; double tinf = -rho * (kappa * theta * tau + V0) / omega + log (F / K); double scaledv; if (!cbuf_initialized) { cbuf = (double complex *) malloc (sizeof (double complex) * n); if (!cbuf) { abort (); } else { cbuf_len = n; } cbuf_initialized = true; } else { if (cbuf_len < n) { cbuf_len = 2 * cbuf_len > n ? 2 * cbuf_len : n; free (cbuf); cbuf = (double complex *) malloc (sizeof (double complex) * cbuf_len); if (!cbuf) { abort (); } } } for (size_t i = 0; i < n; i++) { cbuf[i] = -log (v[i]) / Cinf - ialphap1; } _bal_heston_characteristic_lord2006 (cbuf, n, kappa, omega, rho, tau, theta, log (F), V0); for (size_t i = 0; i < n; i++) { scaledv = -log (v[i]) / Cinf; ctmp = -exp (scaledv * mik) * cbuf[i] / (v[i] - ialpha) / (v[i] - ialphap1); vtmp = creal (ctmp) / v[i]; if (isfinite (vtmp) & isfinite (cimag (ctmp))) { v[i] = vtmp; } else { if (isfinite (scaledv) & isfinite (scaledv)) { vtmp = -psizero * exp (-scaledv * Cinf) * cos (scaledv * tinf) / scaledv / scaledv / v[i]; if (isfinite (vtmp)) { v[i] = vtmp; } else { v[i] = 0.; } } else { v[i] = 0.; } } } }
__complex__ double __ctan (__complex__ double x) { __complex__ double res; if (__glibc_unlikely (!isfinite (__real__ x) || !isfinite (__imag__ x))) { if (__isinf_ns (__imag__ x)) { __real__ res = __copysign (0.0, __real__ x); __imag__ res = __copysign (1.0, __imag__ x); } else if (__real__ x == 0.0) { res = x; } else { __real__ res = __nan (""); __imag__ res = __nan (""); if (__isinf_ns (__real__ x)) feraiseexcept (FE_INVALID); } } else { double sinrx, cosrx; double den; const int t = (int) ((DBL_MAX_EXP - 1) * M_LN2 / 2); int rcls = fpclassify (__real__ x); /* tan(x+iy) = (sin(2x) + i*sinh(2y))/(cos(2x) + cosh(2y)) = (sin(x)*cos(x) + i*sinh(y)*cosh(y)/(cos(x)^2 + sinh(y)^2). */ if (__glibc_likely (rcls != FP_SUBNORMAL)) { __sincos (__real__ x, &sinrx, &cosrx); } else { sinrx = __real__ x; cosrx = 1.0; } if (fabs (__imag__ x) > t) { /* Avoid intermediate overflow when the real part of the result may be subnormal. Ignoring negligible terms, the imaginary part is +/- 1, the real part is sin(x)*cos(x)/sinh(y)^2 = 4*sin(x)*cos(x)/exp(2y). */ double exp_2t = __ieee754_exp (2 * t); __imag__ res = __copysign (1.0, __imag__ x); __real__ res = 4 * sinrx * cosrx; __imag__ x = fabs (__imag__ x); __imag__ x -= t; __real__ res /= exp_2t; if (__imag__ x > t) { /* Underflow (original imaginary part of x has absolute value > 2t). */ __real__ res /= exp_2t; } else __real__ res /= __ieee754_exp (2 * __imag__ x); } else { double sinhix, coshix; if (fabs (__imag__ x) > DBL_MIN) { sinhix = __ieee754_sinh (__imag__ x); coshix = __ieee754_cosh (__imag__ x); } else { sinhix = __imag__ x; coshix = 1.0; } if (fabs (sinhix) > fabs (cosrx) * DBL_EPSILON) den = cosrx * cosrx + sinhix * sinhix; else den = cosrx * cosrx; __real__ res = sinrx * cosrx / den; __imag__ res = sinhix * coshix / den; } } return res; }
double fma(double x, double y, double z) { static const double split = 0x1p27 + 1.0; double xs, ys, zs; double c, cc, hx, hy, p, q, tx, ty; double r, rr, s; int oround; int ex, ey, ez; int spread; if (z == 0.0) return (x * y); if (x == 0.0 || y == 0.0) return (x * y + z); /* Results of frexp() are undefined for these cases. */ if (!isfinite(x) || !isfinite(y) || !isfinite(z)) return (x * y + z); xs = frexp(x, &ex); ys = frexp(y, &ey); zs = frexp(z, &ez); oround = fegetround(); spread = ex + ey - ez; /* * If x * y and z are many orders of magnitude apart, the scaling * will overflow, so we handle these cases specially. Rounding * modes other than FE_TONEAREST are painful. */ if (spread > DBL_MANT_DIG * 2) { fenv_t env; feraiseexcept(FE_INEXACT); switch(oround) { case FE_TONEAREST: return (x * y); case FE_TOWARDZERO: if (x > 0.0 ^ y < 0.0 ^ z < 0.0) return (x * y); feholdexcept(&env); r = x * y; if (!fetestexcept(FE_INEXACT)) r = nextafter(r, 0); feupdateenv(&env); return (r); case FE_DOWNWARD: if (z > 0.0) return (x * y); feholdexcept(&env); r = x * y; if (!fetestexcept(FE_INEXACT)) r = nextafter(r, -INFINITY); feupdateenv(&env); return (r); default: /* FE_UPWARD */ if (z < 0.0) return (x * y); feholdexcept(&env); r = x * y; if (!fetestexcept(FE_INEXACT)) r = nextafter(r, INFINITY); feupdateenv(&env); return (r); } } if (spread < -DBL_MANT_DIG) { feraiseexcept(FE_INEXACT); if (!isnormal(z)) feraiseexcept(FE_UNDERFLOW); switch (oround) { case FE_TONEAREST: return (z); case FE_TOWARDZERO: if (x > 0.0 ^ y < 0.0 ^ z < 0.0) return (z); else return (nextafter(z, 0)); case FE_DOWNWARD: if (x > 0.0 ^ y < 0.0) return (z); else return (nextafter(z, -INFINITY)); default: /* FE_UPWARD */ if (x > 0.0 ^ y < 0.0) return (nextafter(z, INFINITY)); else return (z); } } /* * Use Dekker's algorithm to perform the multiplication and * subsequent addition in twice the machine precision. * Arrange so that x * y = c + cc, and x * y + z = r + rr. */ fesetround(FE_TONEAREST); p = xs * split; hx = xs - p; hx += p; tx = xs - hx; p = ys * split; hy = ys - p; hy += p; ty = ys - hy; p = hx * hy; q = hx * ty + tx * hy; c = p + q; cc = p - c + q + tx * ty; zs = ldexp(zs, -spread); r = c + zs; s = r - c; rr = (c - (r - s)) + (zs - s) + cc; spread = ex + ey; if (spread + ilogb(r) > -1023) { fesetround(oround); r = r + rr; } else { /* * The result is subnormal, so we round before scaling to * avoid double rounding. */ p = ldexp(copysign(0x1p-1022, r), -spread); c = r + p; s = c - r; cc = (r - (c - s)) + (p - s) + rr; fesetround(oround); r = (c + cc) - p; } return (ldexp(r, spread)); }
/** * Axis calculation taking the view into account, correcting view-aligned axis. */ static void axisProjection(const TransInfo *t, const float axis[3], const float in[3], float out[3]) { float norm[3], vec[3], factor, angle; float t_con_center[3]; if (is_zero_v3(in)) { return; } copy_v3_v3(t_con_center, t->center_global); /* checks for center being too close to the view center */ viewAxisCorrectCenter(t, t_con_center); angle = fabsf(angle_v3v3(axis, t->viewinv[2])); if (angle > (float)M_PI_2) { angle = (float)M_PI - angle; } /* For when view is parallel to constraint... will cause NaNs otherwise * So we take vertical motion in 3D space and apply it to the * constraint axis. Nice for camera grab + MMB */ if (angle < DEG2RADF(5.0f)) { project_v3_v3v3(vec, in, t->viewinv[1]); factor = dot_v3v3(t->viewinv[1], vec) * 2.0f; /* Since camera distance is quite relative, use quadratic relationship. * holding shift can compensate. */ if (factor < 0.0f) { factor *= -factor; } else { factor *= factor; } /* -factor makes move down going backwards */ normalize_v3_v3_length(out, axis, -factor); } else { float v[3], i1[3], i2[3]; float v2[3], v4[3]; float norm_center[3]; float plane[3]; getViewVector(t, t_con_center, norm_center); cross_v3_v3v3(plane, norm_center, axis); project_v3_v3v3(vec, in, plane); sub_v3_v3v3(vec, in, vec); add_v3_v3v3(v, vec, t_con_center); getViewVector(t, v, norm); /* give arbitrary large value if projection is impossible */ factor = dot_v3v3(axis, norm); if (1.0f - fabsf(factor) < 0.0002f) { copy_v3_v3(out, axis); if (factor > 0) { mul_v3_fl(out, 1000000000.0f); } else { mul_v3_fl(out, -1000000000.0f); } } else { add_v3_v3v3(v2, t_con_center, axis); add_v3_v3v3(v4, v, norm); isect_line_line_v3(t_con_center, v2, v, v4, i1, i2); sub_v3_v3v3(v, i2, v); sub_v3_v3v3(out, i1, t_con_center); /* possible some values become nan when * viewpoint and object are both zero */ if (!isfinite(out[0])) { out[0] = 0.0f; } if (!isfinite(out[1])) { out[1] = 0.0f; } if (!isfinite(out[2])) { out[2] = 0.0f; } } } }
/** Compare two REAL4 vectors using various different comparison metrics */ int XLALCompareREAL4Vectors ( VectorComparison *result, ///< [out] return comparison results const REAL4Vector *x, ///< [in] first input vector const REAL4Vector *y, ///< [in] second input vector const VectorComparison *tol ///< [in] accepted tolerances on comparisons, or NULL for no check ) { XLAL_CHECK ( result != NULL, XLAL_EINVAL ); XLAL_CHECK ( x != NULL, XLAL_EINVAL ); XLAL_CHECK ( y != NULL, XLAL_EINVAL ); XLAL_CHECK ( x->data != NULL, XLAL_EINVAL ); XLAL_CHECK ( y->data != NULL, XLAL_EINVAL ); XLAL_CHECK ( x->length > 0, XLAL_EINVAL ); XLAL_CHECK ( x->length == y->length, XLAL_EINVAL ); REAL8 x_L1 = 0, x_L2 = 0; REAL8 y_L1 = 0, y_L2 = 0; REAL8 diff_L1 = 0, diff_L2 = 0; REAL8 scalar = 0; REAL4 maxAbsx = 0, maxAbsy = 0; REAL4 x_atMaxAbsx = 0, y_atMaxAbsx = 0; REAL4 x_atMaxAbsy = 0, y_atMaxAbsy = 0; UINT4 numSamples = x->length; for ( UINT4 i = 0; i < numSamples; i ++ ) { REAL4 x_i = x->data[i]; REAL4 y_i = y->data[i]; XLAL_CHECK ( isfinite ( x_i ), XLAL_EFPINVAL, "non-finite element: x(%d) = %g\n", i, x_i ); XLAL_CHECK ( isfinite ( y_i ), XLAL_EFPINVAL, "non-finite element: y(%d) = %g\n", i, y_i ); REAL4 xAbs_i = fabs ( x_i ); REAL4 yAbs_i = fabs ( y_i ); REAL8 absdiff = fabs ( x_i - y_i ); diff_L1 += absdiff; diff_L2 += SQ(absdiff); x_L1 += xAbs_i; y_L1 += yAbs_i; x_L2 += SQ(xAbs_i); y_L2 += SQ(yAbs_i); scalar += x_i * y_i; if ( xAbs_i > maxAbsx ) { maxAbsx = xAbs_i; x_atMaxAbsx = x_i; y_atMaxAbsx = y_i; } if ( yAbs_i > maxAbsy ) { maxAbsy = yAbs_i; x_atMaxAbsy = x_i; y_atMaxAbsy = y_i; } } // for i < numSamples // complete L2 norms by taking sqrt x_L2 = sqrt ( x_L2 ); y_L2 = sqrt ( y_L2 ); diff_L2 = sqrt ( diff_L2 ); // compute and return comparison results result->relErr_L1 = diff_L1 / ( 0.5 * (x_L1 + y_L1 ) ); result->relErr_L2 = diff_L2 / ( 0.5 * (x_L2 + y_L2 ) ); REAL8 cosTheta = fmin ( 1, scalar / (x_L2 * y_L2) ); result->angleV = acos ( cosTheta ); result->relErr_atMaxAbsx = fRELERR ( x_atMaxAbsx, y_atMaxAbsx ); result->relErr_atMaxAbsy = fRELERR ( x_atMaxAbsy, y_atMaxAbsy );; XLAL_CHECK ( XLALCheckVectorComparisonTolerances ( result, tol ) == XLAL_SUCCESS, XLAL_EFUNC ); return XLAL_SUCCESS; } // XLALCompareREAL4Vectors()
/* * Fused multiply-add: Compute x * y + z with a single rounding error. * * We use scaling to avoid overflow/underflow, along with the * canonical precision-doubling technique adapted from: * * Dekker, T. A Floating-Point Technique for Extending the * Available Precision. Numer. Math. 18, 224-242 (1971). */ long double fmal(long double x, long double y, long double z) { long double xs, ys, zs, adj; struct dd xy, r; int oround; int ex, ey, ez; int spread; /* * Handle special cases. The order of operations and the particular * return values here are crucial in handling special cases involving * infinities, NaNs, overflows, and signed zeroes correctly. */ if (x == 0.0 || y == 0.0) return (x * y + z); if (z == 0.0) return (x * y); if (!isfinite(x) || !isfinite(y)) return (x * y + z); if (!isfinite(z)) return (z); xs = frexpl(x, &ex); ys = frexpl(y, &ey); zs = frexpl(z, &ez); oround = fegetround(); spread = ex + ey - ez; /* * If x * y and z are many orders of magnitude apart, the scaling * will overflow, so we handle these cases specially. Rounding * modes other than FE_TONEAREST are painful. */ if (spread < -LDBL_MANT_DIG) { feraiseexcept(FE_INEXACT); if (!isnormal(z)) feraiseexcept(FE_UNDERFLOW); switch (oround) { case FE_TONEAREST: return (z); case FE_TOWARDZERO: if (x > 0.0 ^ y < 0.0 ^ z < 0.0) return (z); else return (nextafterl(z, 0)); case FE_DOWNWARD: if (x > 0.0 ^ y < 0.0) return (z); else return (nextafterl(z, -INFINITY)); default: /* FE_UPWARD */ if (x > 0.0 ^ y < 0.0) return (nextafterl(z, INFINITY)); else return (z); } } if (spread <= LDBL_MANT_DIG * 2) zs = ldexpl(zs, -spread); else zs = copysignl(LDBL_MIN, zs); fesetround(FE_TONEAREST); /* work around clang bug 8100 */ volatile long double vxs = xs; /* * Basic approach for round-to-nearest: * * (xy.hi, xy.lo) = x * y (exact) * (r.hi, r.lo) = xy.hi + z (exact) * adj = xy.lo + r.lo (inexact; low bit is sticky) * result = r.hi + adj (correctly rounded) */ xy = dd_mul(vxs, ys); r = dd_add(xy.hi, zs); spread = ex + ey; if (r.hi == 0.0) { /* * When the addends cancel to 0, ensure that the result has * the correct sign. */ fesetround(oround); volatile long double vzs = zs; /* XXX gcc CSE bug workaround */ return (xy.hi + vzs + ldexpl(xy.lo, spread)); } if (oround != FE_TONEAREST) { /* * There is no need to worry about double rounding in directed * rounding modes. */ fesetround(oround); /* work around clang bug 8100 */ volatile long double vrlo = r.lo; adj = vrlo + xy.lo; return (ldexpl(r.hi + adj, spread)); } adj = add_adjusted(r.lo, xy.lo); if (spread + ilogbl(r.hi) > -16383) return (ldexpl(r.hi + adj, spread)); else return (add_and_denormalize(r.hi, adj, spread)); }
void Path::addArc(const FloatPoint& p, float r, float sa, float ea, bool clockwise) { // Workaround for <rdar://problem/5189233> CGPathAddArc hangs or crashes when passed inf as start or end angle if (isfinite(sa) && isfinite(ea)) CGPathAddArc(m_path, 0, p.x(), p.y(), r, sa, ea, clockwise); }
void das_calcinnandspread(dasystem* das) { observations* obs = das->obs; int e, o; if (obs->nobs == 0) goto finish; if (das->s_mode == S_MODE_HE_f) { if (das->s_f == NULL) { das->s_f = calloc(obs->nobs, sizeof(double)); assert(das->std_f == NULL); das->std_f = calloc(obs->nobs, sizeof(double)); } else { memset(das->s_f, 0, obs->nobs * sizeof(double)); memset(das->std_f, 0, obs->nobs * sizeof(double)); } /* * calculate ensemble mean observations */ for (e = 0; e < das->nmem; ++e) { ENSOBSTYPE* Se = das->S[e]; for (o = 0; o < obs->nobs; ++o) das->s_f[o] += (double) Se[o]; } for (o = 0; o < obs->nobs; ++o) das->s_f[o] /= (double) das->nmem; /* * calculate ensemble spread and innovation */ for (e = 0; e < das->nmem; ++e) { ENSOBSTYPE* Se = das->S[e]; for (o = 0; o < obs->nobs; ++o) { Se[o] -= (ENSOBSTYPE) das->s_f[o]; das->std_f[o] += (double) (Se[o] * Se[o]); } } for (o = 0; o < obs->nobs; ++o) { observation* m = &obs->data[o]; if (m->status != STATUS_OK) continue; das->std_f[o] = sqrt(das->std_f[o] / (double) (das->nmem - 1)); das->s_f[o] = m->value - das->s_f[o]; if (!isfinite(das->s_f[o]) || fabs(das->s_f[o]) > STATE_BIGNUM) enkf_quit("obs # %d: Hx = %d, no point to continue", o); } das->s_mode = S_MODE_HA_f; } else if (das->s_mode == S_MODE_HE_a) { if (das->s_a == NULL) { das->s_a = calloc(obs->nobs, sizeof(double)); assert(das->std_a == NULL); das->std_a = calloc(obs->nobs, sizeof(double)); } else { memset(das->s_a, 0, obs->nobs * sizeof(double)); memset(das->std_a, 0, obs->nobs * sizeof(double)); } /* * calculate ensemble mean observations */ for (e = 0; e < das->nmem; ++e) { ENSOBSTYPE* Se = das->S[e]; for (o = 0; o < obs->nobs; ++o) das->s_a[o] += (double) Se[o]; } for (o = 0; o < obs->nobs; ++o) das->s_a[o] /= (double) das->nmem; /* * calculate ensemble spread and innovation */ for (e = 0; e < das->nmem; ++e) { ENSOBSTYPE* Se = das->S[e]; for (o = 0; o < obs->nobs; ++o) { Se[o] -= (ENSOBSTYPE) das->s_a[o]; das->std_a[o] += (double) (Se[o] * Se[o]); } } for (o = 0; o < obs->nobs; ++o) { observation* m = &obs->data[o]; if (m->status != STATUS_OK) continue; das->std_a[o] = sqrt(das->std_a[o] / (double) (das->nmem - 1)); das->s_a[o] = m->value - das->s_a[o]; if (!isfinite(das->s_a[o]) || fabs(das->s_a[o]) > STATE_BIGNUM) enkf_quit("obs # %d: Hx = %d, no point to continue", o); } das->s_mode = S_MODE_HA_a; } else enkf_quit("programming error"); finish: if (das->s_mode == S_MODE_HE_f) das->s_mode = S_MODE_HA_f; else if (das->s_mode == S_MODE_HE_a) das->s_mode = S_MODE_HA_a; }
static int l2sgd( encoder_t *gm, dataset_t *trainset, dataset_t *testset, floatval_t *w, logging_t *lg, const int N, const floatval_t t0, const floatval_t lambda, const int num_epochs, int calibration, int period, const floatval_t epsilon, floatval_t *ptr_loss ) { int i, epoch, ret = 0; floatval_t t = 0; floatval_t loss = 0, sum_loss = 0; floatval_t best_sum_loss = DBL_MAX; floatval_t eta, gain, decay = 1.; floatval_t improvement = 0.; floatval_t norm2 = 0.; floatval_t *pf = NULL; floatval_t *best_w = NULL; clock_t clk_prev, clk_begin = clock(); const int K = gm->num_features; if (!calibration) { pf = (floatval_t*)malloc(sizeof(floatval_t) * period); best_w = (floatval_t*)calloc(K, sizeof(floatval_t)); if (pf == NULL || best_w == NULL) { ret = CRFSUITEERR_OUTOFMEMORY; goto error_exit; } } /* Initialize the feature weights. */ vecset(w, 0, K); /* Loop for epochs. */ for (epoch = 1;epoch <= num_epochs;++epoch) { clk_prev = clock(); if (!calibration) { logging(lg, "***** Epoch #%d *****\n", epoch); /* Shuffle the training instances. */ dataset_shuffle(trainset); } /* Loop for instances. */ sum_loss = 0.; for (i = 0;i < N;++i) { const crfsuite_instance_t *inst = dataset_get(trainset, i); /* Update various factors. */ eta = 1 / (lambda * (t0 + t)); decay *= (1.0 - eta * lambda); gain = eta / decay; /* Compute the loss and gradients for the instance. */ gm->set_weights(gm, w, decay); gm->set_instance(gm, inst); gm->objective_and_gradients(gm, &loss, w, gain); sum_loss += loss; ++t; } /* Terminate when the loss is abnormal (NaN, -Inf, +Inf). */ if (!isfinite(loss)) { logging(lg, "ERROR: overflow loss\n"); ret = CRFSUITEERR_OVERFLOW; sum_loss = loss; goto error_exit; } /* Scale the feature weights. */ vecscale(w, decay, K); decay = 1.; /* Include the L2 norm of feature weights to the objective. */ /* The factor N is necessary because lambda = 2 * C / N. */ norm2 = vecdot(w, w, K); sum_loss += 0.5 * lambda * norm2 * N; /* One epoch finished. */ if (!calibration) { /* Check if the current epoch is the best. */ if (sum_loss < best_sum_loss) { /* Store the feature weights to best_w. */ best_sum_loss = sum_loss; veccopy(best_w, w, K); } /* We don't test the stopping criterion while period < epoch. */ if (period < epoch) { improvement = (pf[(epoch-1) % period] - sum_loss) / sum_loss; } else { improvement = epsilon; } /* Store the current value of the objective function. */ pf[(epoch-1) % period] = sum_loss; logging(lg, "Loss: %f\n", sum_loss); if (period < epoch) { logging(lg, "Improvement ratio: %f\n", improvement); } logging(lg, "Feature L2-norm: %f\n", sqrt(norm2)); logging(lg, "Learning rate (eta): %f\n", eta); logging(lg, "Total number of feature updates: %.0f\n", t); logging(lg, "Seconds required for this iteration: %.3f\n", (clock() - clk_prev) / (double)CLOCKS_PER_SEC); /* Holdout evaluation if necessary. */ if (testset != NULL) { holdout_evaluation(gm, testset, w, lg); } logging(lg, "\n"); /* Check for the stopping criterion. */ if (improvement < epsilon) { ret = 0; break; } } } /* Output the optimization result. */ if (!calibration) { if (ret == 0) { if (epoch < num_epochs) { logging(lg, "SGD terminated with the stopping criteria\n"); } else { logging(lg, "SGD terminated with the maximum number of iterations\n"); } } else { logging(lg, "SGD terminated with error code (%d)\n", ret); } } /* Restore the best weights. */ if (best_w != NULL) { sum_loss = best_sum_loss; veccopy(w, best_w, K); } error_exit: free(best_w); free(pf); if (ptr_loss != NULL) { *ptr_loss = sum_loss; } return ret; }
void FixedwingAttitudeControl::task_main() { /* * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* do not limit the attitude updates in order to minimize latency. * actuator outputs are still limited by the individual drivers * properly to not saturate IO or physical limitations */ parameters_update(); /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _att_sub; fds[1].events = POLLIN; _task_running = true; while (!_task_should_exit) { static int loop_counter = 0; /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around * the pitch axis compared to the neutral position of the vehicle in multicopter mode * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. * Additionally, in order to get the correct sign of the pitch, we need to multiply * the new x axis of the rotation matrix with -1 * * original: modified: * * Rxx Ryx Rzx -Rzx Ryx Rxx * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ math::Matrix<3, 3> R; //original rotation matrix math::Matrix<3, 3> R_adapted; //modified rotation matrix R.set(_att.R); R_adapted.set(_att.R); /* move z to x */ R_adapted(0, 0) = R(0, 2); R_adapted(1, 0) = R(1, 2); R_adapted(2, 0) = R(2, 2); /* move x to z */ R_adapted(0, 2) = R(0, 0); R_adapted(1, 2) = R(1, 0); R_adapted(2, 2) = R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation euler_angles = R_adapted.to_euler(); /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); PX4_R(_att.R, 0, 0) = R_adapted(0, 0); PX4_R(_att.R, 0, 1) = R_adapted(0, 1); PX4_R(_att.R, 0, 2) = R_adapted(0, 2); PX4_R(_att.R, 1, 0) = R_adapted(1, 0); PX4_R(_att.R, 1, 1) = R_adapted(1, 1); PX4_R(_att.R, 1, 2) = R_adapted(1, 2); PX4_R(_att.R, 2, 0) = R_adapted(2, 0); PX4_R(_att.R, 2, 1) = R_adapted(2, 1); PX4_R(_att.R, 2, 2) = R_adapted(2, 2); /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; } vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); vehicle_status_poll(); /* lock integrator until control is started */ bool lock_integrator; if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) { lock_integrator = false; } else { lock_integrator = true; } /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; //warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[7] = 0.0f; //warnx("_actuators_airframe.control[1] = -1.0f;"); } /* if we are in rotary wing mode, do nothing */ if (_vehicle_status.is_rotary_wing) { continue; } /* default flaps to center */ float flaps_control = 0.0f; /* map flaps by default to manual if valid */ if (isfinite(_manual.flaps)) { flaps_control = _manual.flaps; } /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ float airspeed; /* if airspeed is not updating, we assume the normal average speed */ if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we * want to do in flight and its the baseline a human pilot would choose. * * Forcing the scaling to this value allows reasonable handheld tests. */ float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if * - velocity control or position control is enabled (pos controller is running) * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } } else { /* * Scale down roll and pitch as the setpoints are radians * and a typical remote can only do around 45 degrees, the mapping is * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; /* * in manual mode no external source should / does emit attitude setpoints. * emit the manual setpoint here to allow attitude controller tuning * in attitude control mode. */ struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ if (!_vehicle_status.is_rotary_wing) { if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); } else { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } } /* If the aircraft is on ground reset the integrators */ if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; } else { if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; control_input.roll = _att.roll; control_input.pitch = _att.pitch; control_input.yaw = _att.yaw; control_input.roll_rate = _att.rollspeed; control_input.pitch_rate = _att.pitchspeed; control_input.yaw_rate = _att.yawspeed; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; control_input.acc_body_x = _accel.x; control_input.acc_body_y = _accel.y; control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," " _roll_ctrl.get_desired_rate() %.4f," " _pitch_ctrl.get_desired_rate() %.4f" " att_sp.roll_body %.4f", (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } } float yaw_u = _yaw_ctrl.control_bodyrate(control_input); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (isfinite(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ _rates_sp.roll = _roll_ctrl.get_desired_rate(); _rates_sp.pitch = _pitch_ctrl.get_desired_rate(); _rates_sp.yaw = _yaw_ctrl.get_desired_rate(); _rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } } else { /* manual/direct control */ _actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll; _actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch; _actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; _actuators.control[6] = _manual.aux2; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _att.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || _vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); } else if (_actuators_id) { _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub != nullptr) { /* publish the actuator controls*/ orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); } else { /* advertise and publish */ _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); } } } loop_counter++; perf_end(_loop_perf); } warnx("exiting.\n"); _control_task = -1; _task_running = false; _exit(0); }
static bool read_tuning_table_item(Streader* sr, const char* key, void* userdata) { assert(sr != NULL); assert(key != NULL); assert(userdata != NULL); Tuning_table* tt = userdata; if (string_eq(key, "ref_note")) { int64_t num = 0; if (!Streader_read_int(sr, &num)) return false; if (num < 0 || num >= KQT_TUNING_TABLE_NOTES_MAX) { Streader_set_error( sr, "Invalid reference note number: %" PRId64, num); return false; } tt->ref_note = num; } else if (string_eq(key, "ref_pitch")) { double num = NAN; if (!Streader_read_float(sr, &num)) return false; if (!isfinite(num)) { Streader_set_error(sr, "Invalid reference pitch: %f", num); return false; } tt->ref_pitch = num; } else if (string_eq(key, "pitch_offset")) { double cents = NAN; if (!Streader_read_float(sr, ¢s)) return false; if (!isfinite(cents)) { Streader_set_error(sr, "Pitch offset is not finite"); return false; } tt->global_offset = cents; } else if (string_eq(key, "octave_width")) { double cents = NAN; if (!Streader_read_tuning(sr, ¢s)) return false; if (cents <= 0) { Streader_set_error(sr, "Octave width must be positive"); return false; } Tuning_table_set_octave_width(tt, cents); } else if (string_eq(key, "center_octave")) { int64_t octave = -1; if (!Streader_read_int(sr, &octave)) return false; if (octave < 0 || octave >= KQT_TUNING_TABLE_OCTAVES) { Streader_set_error(sr, "Invalid center octave: %" PRId64, octave); return false; } tt->center_octave = octave; Tuning_table_set_octave_width(tt, tt->octave_width); } else if (string_eq(key, "notes")) { for (int i = 0; i < KQT_TUNING_TABLE_NOTES_MAX; ++i) tt->note_offsets[i] = NAN; if (!Streader_read_list(sr, read_note, tt)) return false; } else { Streader_set_error(sr, "Unrecognised key in tuning table: %s", key); return false; } return true; }
mrb_value mrb_str_format(mrb_state *mrb, int argc, const mrb_value *argv, mrb_value fmt) { const char *p, *end; char *buf; mrb_int blen; mrb_int bsiz; mrb_value result; mrb_int n; mrb_int width; mrb_int prec; int flags = FNONE; int nextarg = 1; int posarg = 0; mrb_value nextvalue; mrb_value str; mrb_value hash = mrb_undef_value(); #define CHECK_FOR_WIDTH(f) \ if ((f) & FWIDTH) { \ mrb_raise(mrb, E_ARGUMENT_ERROR, "width given twice"); \ } \ if ((f) & FPREC0) { \ mrb_raise(mrb, E_ARGUMENT_ERROR, "width after precision"); \ } #define CHECK_FOR_FLAGS(f) \ if ((f) & FWIDTH) { \ mrb_raise(mrb, E_ARGUMENT_ERROR, "flag after width"); \ } \ if ((f) & FPREC0) { \ mrb_raise(mrb, E_ARGUMENT_ERROR, "flag after precision"); \ } ++argc; --argv; fmt = mrb_str_to_str(mrb, fmt); p = RSTRING_PTR(fmt); end = p + RSTRING_LEN(fmt); blen = 0; bsiz = 120; result = mrb_str_buf_new(mrb, bsiz); buf = RSTRING_PTR(result); memset(buf, 0, bsiz); for (; p < end; p++) { const char *t; mrb_sym id = 0; for (t = p; t < end && *t != '%'; t++) ; PUSH(p, t - p); if (t >= end) goto sprint_exit; /* end of fmt string */ p = t + 1; /* skip '%' */ width = prec = -1; nextvalue = mrb_undef_value(); retry: switch (*p) { default: mrb_raisef(mrb, E_ARGUMENT_ERROR, "malformed format string - \\%%S", mrb_str_new(mrb, p, 1)); break; case ' ': CHECK_FOR_FLAGS(flags); flags |= FSPACE; p++; goto retry; case '#': CHECK_FOR_FLAGS(flags); flags |= FSHARP; p++; goto retry; case '+': CHECK_FOR_FLAGS(flags); flags |= FPLUS; p++; goto retry; case '-': CHECK_FOR_FLAGS(flags); flags |= FMINUS; p++; goto retry; case '0': CHECK_FOR_FLAGS(flags); flags |= FZERO; p++; goto retry; case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': n = 0; GETNUM(n, width); if (*p == '$') { if (!mrb_undef_p(nextvalue)) { mrb_raisef(mrb, E_ARGUMENT_ERROR, "value given twice - %S$", mrb_fixnum_value(n)); } nextvalue = GETPOSARG(n); p++; goto retry; } CHECK_FOR_WIDTH(flags); width = n; flags |= FWIDTH; goto retry; case '<': case '{': { const char *start = p; char term = (*p == '<') ? '>' : '}'; mrb_value symname; for (; p < end && *p != term; ) p++; if (id) { mrb_raisef(mrb, E_ARGUMENT_ERROR, "name%S after <%S>", mrb_str_new(mrb, start, p - start + 1), mrb_sym2str(mrb, id)); } symname = mrb_str_new(mrb, start + 1, p - start - 1); id = mrb_intern_str(mrb, symname); nextvalue = GETNAMEARG(mrb_symbol_value(id), start, (int)(p - start + 1)); if (mrb_undef_p(nextvalue)) { mrb_raisef(mrb, E_KEY_ERROR, "key%S not found", mrb_str_new(mrb, start, p - start + 1)); } if (term == '}') goto format_s; p++; goto retry; } case '*': CHECK_FOR_WIDTH(flags); flags |= FWIDTH; GETASTER(width); if (width < 0) { flags |= FMINUS; width = -width; } p++; goto retry; case '.': if (flags & FPREC0) { mrb_raise(mrb, E_ARGUMENT_ERROR, "precision given twice"); } flags |= FPREC|FPREC0; prec = 0; p++; if (*p == '*') { GETASTER(prec); if (prec < 0) { /* ignore negative precision */ flags &= ~FPREC; } p++; goto retry; } GETNUM(prec, precision); goto retry; case '\n': case '\0': p--; /* fallthrough */ case '%': if (flags != FNONE) { mrb_raise(mrb, E_ARGUMENT_ERROR, "invalid format character - %"); } PUSH("%", 1); break; case 'c': { mrb_value val = GETARG(); mrb_value tmp; char *c; tmp = mrb_check_string_type(mrb, val); if (!mrb_nil_p(tmp)) { if (mrb_fixnum(mrb_funcall(mrb, tmp, "size", 0)) != 1 ) { mrb_raise(mrb, E_ARGUMENT_ERROR, "%c requires a character"); } } else if (mrb_fixnum_p(val)) { tmp = mrb_funcall(mrb, val, "chr", 0); } else { mrb_raise(mrb, E_ARGUMENT_ERROR, "invalid character"); } c = RSTRING_PTR(tmp); n = RSTRING_LEN(tmp); if (!(flags & FWIDTH)) { CHECK(n); memcpy(buf+blen, c, n); blen += n; } else if ((flags & FMINUS)) { CHECK(n); memcpy(buf+blen, c, n); blen += n; FILL(' ', width-1); } else { FILL(' ', width-1); CHECK(n); memcpy(buf+blen, c, n); blen += n; } } break; case 's': case 'p': format_s: { mrb_value arg = GETARG(); mrb_int len; mrb_int slen; if (*p == 'p') arg = mrb_inspect(mrb, arg); str = mrb_obj_as_string(mrb, arg); len = RSTRING_LEN(str); if (RSTRING(result)->flags & MRB_STR_EMBED) { mrb_int tmp_n = len; RSTRING(result)->flags &= ~MRB_STR_EMBED_LEN_MASK; RSTRING(result)->flags |= tmp_n << MRB_STR_EMBED_LEN_SHIFT; } else { RSTRING(result)->as.heap.len = blen; } if (flags&(FPREC|FWIDTH)) { slen = RSTRING_LEN(str); if (slen < 0) { mrb_raise(mrb, E_ARGUMENT_ERROR, "invalid mbstring sequence"); } if ((flags&FPREC) && (prec < slen)) { char *p = RSTRING_PTR(str) + prec; slen = prec; len = p - RSTRING_PTR(str); } /* need to adjust multi-byte string pos */ if ((flags&FWIDTH) && (width > slen)) { width -= (int)slen; if (!(flags&FMINUS)) { CHECK(width); while (width--) { buf[blen++] = ' '; } } CHECK(len); memcpy(&buf[blen], RSTRING_PTR(str), len); blen += len; if (flags&FMINUS) { CHECK(width); while (width--) { buf[blen++] = ' '; } } break; } } PUSH(RSTRING_PTR(str), len); } break; case 'd': case 'i': case 'o': case 'x': case 'X': case 'b': case 'B': case 'u': { mrb_value val = GETARG(); char fbuf[32], nbuf[68], *s; const char *prefix = NULL; int sign = 0, dots = 0; char sc = 0; mrb_int v = 0; int base; mrb_int len; switch (*p) { case 'd': case 'i': case 'u': sign = 1; break; case 'o': case 'x': case 'X': case 'b': case 'B': if (flags&(FPLUS|FSPACE)) sign = 1; break; default: break; } if (flags & FSHARP) { switch (*p) { case 'o': prefix = "0"; break; case 'x': prefix = "0x"; break; case 'X': prefix = "0X"; break; case 'b': prefix = "0b"; break; case 'B': prefix = "0B"; break; default: break; } } bin_retry: switch (mrb_type(val)) { case MRB_TT_FLOAT: val = mrb_flo_to_fixnum(mrb, val); if (mrb_fixnum_p(val)) goto bin_retry; break; case MRB_TT_STRING: val = mrb_str_to_inum(mrb, val, 0, TRUE); goto bin_retry; case MRB_TT_FIXNUM: v = mrb_fixnum(val); break; default: val = mrb_Integer(mrb, val); goto bin_retry; } switch (*p) { case 'o': base = 8; break; case 'x': case 'X': base = 16; break; case 'b': case 'B': base = 2; break; case 'u': case 'd': case 'i': default: base = 10; break; } if (base == 2) { if (v < 0 && !sign) { val = mrb_fix2binstr(mrb, mrb_fixnum_value(v), base); dots = 1; } else { val = mrb_fixnum_to_str(mrb, mrb_fixnum_value(v), base); } } if (sign) { char c = *p; if (c == 'i') c = 'd'; /* %d and %i are identical */ if (v < 0) { v = -v; sc = '-'; width--; } else if (flags & FPLUS) { sc = '+'; width--; } else if (flags & FSPACE) { sc = ' '; width--; } if (base == 2) { snprintf(nbuf, sizeof(nbuf), "%s", RSTRING_PTR(val)); } else { snprintf(fbuf, sizeof(fbuf), "%%l%c", c); snprintf(nbuf, sizeof(nbuf), fbuf, v); } s = nbuf; } else { char c = *p; if (c == 'X') c = 'x'; s = nbuf; if (v < 0) { dots = 1; } if (base == 2) { snprintf(++s, sizeof(nbuf) - 1, "%s", RSTRING_PTR(val)); } else { snprintf(fbuf, sizeof(fbuf), "%%l%c", c); snprintf(++s, sizeof(nbuf) - 1, fbuf, v); } if (v < 0) { char d; s = remove_sign_bits(s, base); switch (base) { case 16: d = 'f'; break; case 8: d = '7'; break; case 2: d = '1'; break; default: d = 0; break; } if (d && *s != d) { *--s = d; } } } { size_t size; size = strlen(s); /* PARANOID: assert(size <= MRB_INT_MAX) */ len = (mrb_int)size; } if (dots) { prec -= 2; width -= 2; } if (*p == 'X') { char *pp = s; int c; while ((c = (int)(unsigned char)*pp) != 0) { *pp = toupper(c); pp++; } } if (prefix && !prefix[1]) { /* octal */ if (dots) { prefix = NULL; } else if (len == 1 && *s == '0') { len = 0; if (flags & FPREC) prec--; } else if ((flags & FPREC) && (prec > len)) { prefix = NULL; } } else if (len == 1 && *s == '0') { prefix = NULL; } if (prefix) { size_t size; size = strlen(prefix); /* PARANOID: assert(size <= MRB_INT_MAX). * this check is absolutely paranoid. */ width -= (mrb_int)size; } if ((flags & (FZERO|FMINUS|FPREC)) == FZERO) { prec = width; width = 0; } else { if (prec < len) { if (!prefix && prec == 0 && len == 1 && *s == '0') len = 0; prec = len; } width -= prec; } if (!(flags&FMINUS)) { CHECK(width); while (width-- > 0) { buf[blen++] = ' '; } } if (sc) PUSH(&sc, 1); if (prefix) { int plen = (int)strlen(prefix); PUSH(prefix, plen); } CHECK(prec - len); if (dots) PUSH("..", 2); if (v < 0) { char c = sign_bits(base, p); while (len < prec--) { buf[blen++] = c; } } else if ((flags & (FMINUS|FPREC)) != FMINUS) { char c = '0'; while (len < prec--) { buf[blen++] = c; } } PUSH(s, len); CHECK(width); while (width-- > 0) { buf[blen++] = ' '; } } break; case 'f': case 'g': case 'G': case 'e': case 'E': case 'a': case 'A': { mrb_value val = GETARG(); double fval; int i, need = 6; char fbuf[32]; fval = mrb_float(mrb_Float(mrb, val)); if (!isfinite(fval)) { const char *expr; const int elen = 3; if (isnan(fval)) { expr = "NaN"; } else { expr = "Inf"; } need = elen; if ((!isnan(fval) && fval < 0.0) || (flags & FPLUS)) need++; if ((flags & FWIDTH) && need < width) need = width; CHECK(need + 1); snprintf(&buf[blen], need + 1, "%*s", need, ""); if (flags & FMINUS) { if (!isnan(fval) && fval < 0.0) buf[blen++] = '-'; else if (flags & FPLUS) buf[blen++] = '+'; else if (flags & FSPACE) blen++; memcpy(&buf[blen], expr, elen); } else { if (!isnan(fval) && fval < 0.0) buf[blen + need - elen - 1] = '-'; else if (flags & FPLUS) buf[blen + need - elen - 1] = '+'; else if ((flags & FSPACE) && need > width) blen++; memcpy(&buf[blen + need - elen], expr, elen); } blen += strlen(&buf[blen]); break; } fmt_setup(fbuf, sizeof(fbuf), *p, flags, width, prec); need = 0; if (*p != 'e' && *p != 'E') { i = INT_MIN; frexp(fval, &i); if (i > 0) need = BIT_DIGITS(i); } need += (flags&FPREC) ? prec : 6; if ((flags&FWIDTH) && need < width) need = width; need += 20; CHECK(need); n = snprintf(&buf[blen], need, fbuf, fval); blen += n; } break; } flags = FNONE; } sprint_exit: #if 0 /* XXX - We cannot validate the number of arguments if (digit)$ style used. */ if (posarg >= 0 && nextarg < argc) { const char *mesg = "too many arguments for format string"; if (mrb_test(ruby_debug)) mrb_raise(mrb, E_ARGUMENT_ERROR, mesg); if (mrb_test(ruby_verbose)) mrb_warn(mrb, "%S", mrb_str_new_cstr(mrb, mesg)); } #endif mrb_str_resize(mrb, result, blen); return result; }
void WorldInternal::Culling(const Matrix44& cameraProjMat, bool isOpenGL) { objs.clear(); #ifdef _WIN32 if (_finite(cameraProjMat.Values[2][2]) != 0 && cameraProjMat.Values[0][0] != 0.0f && cameraProjMat.Values[1][1] != 0.0f) { #else if (isfinite(cameraProjMat.Values[2][2]) != 0 && cameraProjMat.Values[0][0] != 0.0f && cameraProjMat.Values[1][1] != 0.0f) { #endif Matrix44 cameraProjMatInv = cameraProjMat; cameraProjMatInv.SetInverted(); float maxx = 1.0f; float minx = -1.0f; float maxy = 1.0f; float miny = -1.0f; float maxz = 1.0f; float minz = 0.0f; if (isOpenGL) minz = -1.0f; Vector3DF eyebox[8]; eyebox[0 + 0] = Vector3DF(minx, miny, maxz); eyebox[1 + 0] = Vector3DF(maxx, miny, maxz); eyebox[2 + 0] = Vector3DF(minx, maxy, maxz); eyebox[3 + 0] = Vector3DF(maxx, maxy, maxz); eyebox[0 + 4] = Vector3DF(minx, miny, minz); eyebox[1 + 4] = Vector3DF(maxx, miny, minz); eyebox[2 + 4] = Vector3DF(minx, maxy, minz); eyebox[3 + 4] = Vector3DF(maxx, maxy, minz); for (int32_t i = 0; i < 8; i++) { eyebox[i] = cameraProjMatInv.Transform3D(eyebox[i]); } // 0-right 1-left 2-top 3-bottom 4-front 5-back Vector3DF facePositions[6]; facePositions[0] = eyebox[5]; facePositions[1] = eyebox[4]; facePositions[2] = eyebox[6]; facePositions[3] = eyebox[4]; facePositions[4] = eyebox[4]; facePositions[5] = eyebox[0]; Vector3DF faceDir[6]; faceDir[0] = Vector3DF::Cross(eyebox[1] - eyebox[5], eyebox[7] - eyebox[5]); faceDir[1] = Vector3DF::Cross(eyebox[6] - eyebox[4], eyebox[0] - eyebox[4]); faceDir[2] = Vector3DF::Cross(eyebox[7] - eyebox[6], eyebox[2] - eyebox[6]); faceDir[3] = Vector3DF::Cross(eyebox[0] - eyebox[4], eyebox[5] - eyebox[4]); faceDir[4] = Vector3DF::Cross(eyebox[5] - eyebox[4], eyebox[6] - eyebox[4]); faceDir[5] = Vector3DF::Cross(eyebox[2] - eyebox[0], eyebox[1] - eyebox[5]); for (int32_t i = 0; i < 6; i++) { faceDir[i].Normalize(); } for (int32_t z = 0; z < viewCullingZDiv; z++) { for (int32_t y = 0; y < viewCullingYDiv; y++) { for (int32_t x = 0; x < viewCullingXDiv; x++) { Vector3DF eyebox_[8]; float xsize = 1.0f / (float) viewCullingXDiv; float ysize = 1.0f / (float) viewCullingYDiv; float zsize = 1.0f / (float) viewCullingZDiv; for (int32_t e = 0; e < 8; e++) { float x_, y_, z_; if (e == 0){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * z; } if (e == 1){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * z; } if (e == 2){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 3){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 4){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 5){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 6){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * (z + 1); } if (e == 7){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * (z + 1); } Vector3DF yzMid[4]; yzMid[0] = eyebox[0] * x_ + eyebox[1] * (1.0f - x_); yzMid[1] = eyebox[2] * x_ + eyebox[3] * (1.0f - x_); yzMid[2] = eyebox[4] * x_ + eyebox[5] * (1.0f - x_); yzMid[3] = eyebox[6] * x_ + eyebox[7] * (1.0f - x_); Vector3DF zMid[2]; zMid[0] = yzMid[0] * y_ + yzMid[1] * (1.0f - y_); zMid[1] = yzMid[2] * y_ + yzMid[3] * (1.0f - y_); eyebox_[e] = zMid[0] * z_ + zMid[1] * (1.0f - z_); } Vector3DF max_(-FLT_MAX, -FLT_MAX, -FLT_MAX); Vector3DF min_(FLT_MAX, FLT_MAX, FLT_MAX); for (int32_t i = 0; i < 8; i++) { if (eyebox_[i].X > max_.X) max_.X = eyebox_[i].X; if (eyebox_[i].Y > max_.Y) max_.Y = eyebox_[i].Y; if (eyebox_[i].Z > max_.Z) max_.Z = eyebox_[i].Z; if (eyebox_[i].X < min_.X) min_.X = eyebox_[i].X; if (eyebox_[i].Y < min_.Y) min_.Y = eyebox_[i].Y; if (eyebox_[i].Z < min_.Z) min_.Z = eyebox_[i].Z; } /* 範囲内に含まれるグリッドを取得 */ for (size_t i = 0; i < layers.size(); i++) { layers[i]->AddGrids(max_, min_, grids); } } } } /* 外領域追加 */ grids.push_back(&outofLayers); grids.push_back(&allLayers); /* グリッドからオブジェクト取得 */ for (size_t i = 0; i < grids.size(); i++) { for (size_t j = 0; j < grids[i]->GetObjects().size(); j++) { Object* o = grids[i]->GetObjects()[j]; ObjectInternal* o_ = (ObjectInternal*) o; if ( o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL || IsInView(o_->GetPosition(), o_->GetNextStatus().CalcRadius(), facePositions, faceDir)) { objs.push_back(o); } } } /* 取得したグリッドを破棄 */ for (size_t i = 0; i < grids.size(); i++) { grids[i]->IsScanned = false; } grids.clear(); } else { grids.push_back(&allLayers); /* グリッドからオブジェクト取得 */ for (size_t i = 0; i < grids.size(); i++) { for (size_t j = 0; j < grids[i]->GetObjects().size(); j++) { Object* o = grids[i]->GetObjects()[j]; ObjectInternal* o_ = (ObjectInternal*) o; if (o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL) { objs.push_back(o); } } } /* 取得したグリッドを破棄 */ for (size_t i = 0; i < grids.size(); i++) { grids[i]->IsScanned = false; } grids.clear(); } } bool WorldInternal::Reassign() { /* 数が少ない */ if (outofLayers.GetObjects().size() < 10) return false; objs.clear(); for (size_t i = 0; i < layers.size(); i++) { delete layers[i]; } layers.clear(); outofLayers.GetObjects().clear(); allLayers.GetObjects().clear(); outofLayers.IsScanned = false; allLayers.IsScanned = false; for (auto& it : containedObjects) { auto o = (ObjectInternal*) (it); o->ObjectIndex = -1; } float xmin = FLT_MAX; float xmax = -FLT_MAX; float ymin = FLT_MAX; float ymax = -FLT_MAX; float zmin = FLT_MAX; float zmax = -FLT_MAX; for (auto& it : containedObjects) { ObjectInternal* o_ = (ObjectInternal*) it; if (o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL) continue; if (xmin > o_->GetNextStatus().Position.X) xmin = o_->GetNextStatus().Position.X; if (xmax < o_->GetNextStatus().Position.X) xmax = o_->GetNextStatus().Position.X; if (ymin > o_->GetNextStatus().Position.Y) ymin = o_->GetNextStatus().Position.Y; if (ymax < o_->GetNextStatus().Position.Y) ymax = o_->GetNextStatus().Position.Y; if (zmin > o_->GetNextStatus().Position.Z) zmin = o_->GetNextStatus().Position.Z; if (zmax < o_->GetNextStatus().Position.Z) zmax = o_->GetNextStatus().Position.Z; } auto xlen = Max(abs(xmax), abs(xmin)) * 2.0f; auto ylen = Max(abs(ymax), abs(ymin)) * 2.0f; auto zlen = Max(abs(zmax), abs(zmin)) * 2.0f; WorldInternal(xlen, ylen, zlen, this->layerCount); for (auto& it: containedObjects) { ObjectInternal* o_ = (ObjectInternal*) (it); AddObjectInternal(o_); } return true; } void WorldInternal::Dump(const char* path, const Matrix44& cameraProjMat, bool isOpenGL) { std::ofstream ofs(path); /* カメラ情報出力 */ Matrix44 cameraProjMatInv = cameraProjMat; cameraProjMatInv.SetInverted(); float maxx = 1.0f; float minx = -1.0f; float maxy = 1.0f; float miny = -1.0f; float maxz = 1.0f; float minz = 0.0f; if (isOpenGL) minz = -1.0f; Vector3DF eyebox[8]; eyebox[0 + 0] = Vector3DF(minx, miny, maxz); eyebox[1 + 0] = Vector3DF(maxx, miny, maxz); eyebox[2 + 0] = Vector3DF(minx, maxy, maxz); eyebox[3 + 0] = Vector3DF(maxx, maxy, maxz); eyebox[0 + 4] = Vector3DF(minx, miny, minz); eyebox[1 + 4] = Vector3DF(maxx, miny, minz); eyebox[2 + 4] = Vector3DF(minx, maxy, minz); eyebox[3 + 4] = Vector3DF(maxx, maxy, minz); for (int32_t i = 0; i < 8; i++) { eyebox[i] = cameraProjMatInv.Transform3D(eyebox[i]); } ofs << viewCullingXDiv << "," << viewCullingYDiv << "," << viewCullingZDiv << std::endl; for (int32_t i = 0; i < 8; i++) { ofs << eyebox[i].X << "," << eyebox[i].Y << "," << eyebox[i].Z << std::endl; } ofs << std::endl; for (int32_t z = 0; z < viewCullingZDiv; z++) { for (int32_t y = 0; y < viewCullingYDiv; y++) { for (int32_t x = 0; x < viewCullingXDiv; x++) { Vector3DF eyebox_[8]; float xsize = 1.0f / (float) viewCullingXDiv; float ysize = 1.0f / (float) viewCullingYDiv; float zsize = 1.0f / (float) viewCullingZDiv; for (int32_t e = 0; e < 8; e++) { float x_, y_, z_; if (e == 0){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * z; } if (e == 1){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * z; } if (e == 2){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 3){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * z; } if (e == 4){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 5){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * (z + 1); } if (e == 6){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * (z + 1); } if (e == 7){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * (z + 1); } Vector3DF yzMid[4]; yzMid[0] = eyebox[0] * x_ + eyebox[1] * (1.0f - x_); yzMid[1] = eyebox[2] * x_ + eyebox[3] * (1.0f - x_); yzMid[2] = eyebox[4] * x_ + eyebox[5] * (1.0f - x_); yzMid[3] = eyebox[6] * x_ + eyebox[7] * (1.0f - x_); Vector3DF zMid[2]; zMid[0] = yzMid[0] * y_ + yzMid[1] * (1.0f - y_); zMid[1] = yzMid[2] * y_ + yzMid[3] * (1.0f - y_); eyebox_[e] = zMid[0] * z_ + zMid[1] * (1.0f - z_); } Vector3DF max_(-FLT_MAX, -FLT_MAX, -FLT_MAX); Vector3DF min_(FLT_MAX, FLT_MAX, FLT_MAX); for (int32_t i = 0; i < 8; i++) { if (eyebox_[i].X > max_.X) max_.X = eyebox_[i].X; if (eyebox_[i].Y > max_.Y) max_.Y = eyebox_[i].Y; if (eyebox_[i].Z > max_.Z) max_.Z = eyebox_[i].Z; if (eyebox_[i].X < min_.X) min_.X = eyebox_[i].X; if (eyebox_[i].Y < min_.Y) min_.Y = eyebox_[i].Y; if (eyebox_[i].Z < min_.Z) min_.Z = eyebox_[i].Z; } ofs << x << "," << y << "," << z << std::endl; for (int32_t i = 0; i < 8; i++) { ofs << eyebox_[i].X << "," << eyebox_[i].Y << "," << eyebox_[i].Z << std::endl; } ofs << max_.X << "," << max_.Y << "," << max_.Z << std::endl; ofs << min_.X << "," << min_.Y << "," << min_.Z << std::endl; ofs << std::endl; } } } ofs << std::endl; /* レイヤー情報 */ ofs << layers.size() << std::endl; for (size_t i = 0; i < layers.size(); i++) { auto& layer = layers[i]; ofs << layer->GetGridXCount() << "," << layer->GetGridYCount() << "," << layer->GetGridZCount() << "," << layer->GetOffsetX() << "," << layer->GetOffsetY() << "," << layer->GetOffsetZ() << "," << layer->GetGridSize() << std::endl; for (size_t j = 0; j < layer->GetGrids().size(); j++) { auto& grid = layer->GetGrids()[j]; if (grid.GetObjects().size() > 0) { ofs << j << "," << grid.GetObjects().size() << std::endl; } } } Culling(cameraProjMat, isOpenGL); }
/* Check for a timesteps etc. which will actually finish. */ int mwCheckNormalPosNumEps(real n) { return !isfinite(n) || n <= REAL_EPSILON; }
// Modèle Numérique d'Élévation Horn Schunck (cas affine) void mnehs_affine(float *out_h, float *init_h, int ow, int oh, float *a, int wa, int ha, float *b, int wb, int hb, double PA[8], double PB[8], float alpha2, int niter) { // allocate temporary images float *h = xmalloc(ow * oh * sizeof*h); // h-increment float *Q = xmalloc(ow * oh * sizeof*Q); // Q float *amb = xmalloc(ow * oh * sizeof*amb); // A-B (warped) float *ga = xmalloc(2 * wa * ha * sizeof*ga); // grad(A) float *gb = xmalloc(2 * wb * hb * sizeof*gb); // grad(B) // gradient of A and B fill_gradient(ga, a, wa, ha); fill_gradient(gb, b, wb, hb); // fill images q and amb (a minus b) for (int j = 0; j < oh; j++) for (int i = 0; i < ow; i++) { float h0 = getsample_nan(init_h, ow, oh, 1, i, j, 0); double ijh[3] = {i, j, h0}, paijh[3], pbijh[3]; apply_projection(paijh, PA, ijh); apply_projection(pbijh, PB, ijh); float va, vb, vga[2], vgb[2]; bicubic_interpolation_nans(&va, a, wa, ha, 1, paijh[0], paijh[1]); bicubic_interpolation_nans(&vb, b, wb, hb, 1, pbijh[0], pbijh[1]); bicubic_interpolation(vga, ga, wa, ha, 2, paijh[0], paijh[1]); bicubic_interpolation(vgb, gb, wb, hb, 2, pbijh[0], pbijh[1]); float gapa = vga[0] * PA[2] + vga[1] * PA[6]; float gbpb = vgb[0] * PB[2] + vgb[1] * PB[6]; Q [j*ow+i] = gapa - gbpb; amb[j*ow+i] = va - vb; } nusavec("Q_%d.tiff", global_scale, Q, ow, oh, 1); nusavec("amb_%d.tiff", global_scale, amb, ow, oh, 1); // initialize h for (int i = 0; i < ow * oh; i++) h[i] = 0; // run the iterations (without warps) for (int iter = 0; iter < niter; iter++) { for (int j = 0; j < oh; j++) for (int i = 0; i < ow; i++) { int ij = j * ow + i; if (!isfinite(amb[ij])) continue; float ax = laplacian_at(h, ow, oh, i, j); ax -= Q[ij] * (Q[ij] * h[ij] + amb[ij]) / alpha2; h[ij] += TAU() * ax; } } nusavec("h_%d.tiff", global_scale, h, ow, oh, 1); // update result for (int i = 0; i < ow * oh; i++) out_h[i] = init_h[i] + h[i]; // cleanup and exit free(h); free(Q); free(amb); free(ga); free(gb); }
/* Check for positive, real numbers that can be any size */ int mwCheckNormalPosNum(real n) { return !isfinite(n) || n <= 0.0; }
/* implements the Markov chain */ int mc(system_t *system) { int j, msgsize; double initial_energy, final_energy, current_energy; double rot_partfunc; observables_t *observables_mpi; avg_nodestats_t *avg_nodestats_mpi; sorbateInfo_t *sinfo_mpi = 0; double *temperature_mpi = 0; char *snd_strct = 0, *rcv_strct = 0; system->count_autorejects = 0; // initialize counter for skipped close (unphysical) contacts // char linebuf[MAXLINE]; (unused variable) #ifdef MPI MPI_Datatype msgtype; #endif /* MPI */ /* allocate the statistics structures */ observables_mpi = calloc(1, sizeof(observables_t)); memnullcheck(observables_mpi, sizeof(observables_t), __LINE__ - 1, __FILE__); avg_nodestats_mpi = calloc(1, sizeof(avg_nodestats_t)); memnullcheck(avg_nodestats_mpi, sizeof(avg_nodestats_t), __LINE__ - 1, __FILE__); // if multiple-sorbates, allocate sorb statistics struct if (system->sorbateCount > 1) { sinfo_mpi = calloc(system->sorbateCount, sizeof(sorbateInfo_t)); memnullcheck(sinfo_mpi, sizeof(sorbateInfo_t), __LINE__ - 1, __FILE__); system->sorbateGlobal = calloc(system->sorbateCount, sizeof(sorbateAverages_t)); memnullcheck(system->sorbateGlobal, sizeof(sorbateAverages_t), __LINE__ - 1, __FILE__); } // compute message size msgsize = sizeof(observables_t) + sizeof(avg_nodestats_t); if (system->calc_hist) msgsize += system->n_histogram_bins * sizeof(int); if (system->sorbateCount > 1) msgsize += system->sorbateCount * sizeof(sorbateInfo_t); #ifdef MPI MPI_Type_contiguous(msgsize, MPI_BYTE, &msgtype); MPI_Type_commit(&msgtype); #endif /* MPI */ /* allocate MPI structures */ snd_strct = calloc(msgsize, 1); memnullcheck(snd_strct, sizeof(msgsize), __LINE__ - 1, __FILE__); if (!rank) { rcv_strct = calloc(size, msgsize); memnullcheck(rcv_strct, size * sizeof(msgsize), __LINE__ - 1, __FILE__); temperature_mpi = calloc(size, sizeof(double)); //temperature list for parallel tempering memnullcheck(temperature_mpi, size * sizeof(double), __LINE__ - 1, __FILE__); } /* update the grid for the first time */ if (system->cavity_bias) cavity_update_grid(system); /* set volume observable */ system->observables->volume = system->pbc->volume; /* get the initial energy of the system */ initial_energy = energy(system); #ifdef QM_ROTATION /* solve for the rotational energy levels */ if (system->quantum_rotation) quantum_system_rotational_energies(system); #endif /* QM_ROTATION */ /* be a bit forgiving of the initial state */ if (!isfinite(initial_energy)) initial_energy = system->observables->energy = MAXVALUE; /* if root, open necessary output files */ if (!rank) if (open_files(system) < 0) { error( "MC: could not open files\n"); return (-1); } // write initial observables to stdout and logs if (!rank) { calc_system_mass(system); // average in the initial values once (we don't want to double-count the initial state when using MPI) update_root_averages(system, system->observables, system->avg_observables); // average in the initial sorbate values if (system->sorbateCount > 1) { update_sorbate_info(system); //local update update_root_sorb_averages(system, system->sorbateInfo); //global update } // write initial observables exactly once if (system->file_pointers.fp_energy) write_observables(system->file_pointers.fp_energy, system, system->observables, system->temperature); if (system->file_pointers.fp_energy_csv) write_observables_csv(system->file_pointers.fp_energy_csv, system, system->observables, system->temperature); output( "MC: initial values:\n"); write_averages(system); } /* save the initial state */ checkpoint(system); /* main MC loop */ for (system->step = 1; system->step <= system->numsteps; (system->step)++) { /* restore the last accepted energy */ initial_energy = system->observables->energy; /* perturb the system */ make_move(system); /* calculate the energy change */ final_energy = energy(system); #ifdef QM_ROTATION /* solve for the rotational energy levels */ if (system->quantum_rotation && (system->checkpoint->movetype == MOVETYPE_SPINFLIP)) quantum_system_rotational_energies(system); #endif /* QM_ROTATION */ if (system->checkpoint->movetype != MOVETYPE_REMOVE) rot_partfunc = system->checkpoint->molecule_altered->rot_partfunc; else rot_partfunc = system->checkpoint->molecule_backup->rot_partfunc; /* treat a bad contact as a reject */ if (!isfinite(final_energy)){ system->observables->energy = MAXVALUE; system->nodestats->boltzmann_factor = 0; } else boltzmann_factor(system, initial_energy, final_energy, rot_partfunc); /* Metropolis function */ if ((get_rand(system) < system->nodestats->boltzmann_factor) && (system->iter_success == 0)) { /////////// ACCEPT current_energy = final_energy; /* checkpoint */ checkpoint(system); register_accept(system); /* SA */ if (system->simulated_annealing) { if (system->simulated_annealing_linear == 1) { system->temperature = system->temperature + (system->simulated_annealing_target - system->temperature) / (system->numsteps - system->step); if (system->numsteps - system->step == 0) system->temperature = system->simulated_annealing_target; } else system->temperature = system->simulated_annealing_target + (system->temperature - system->simulated_annealing_target) * system->simulated_annealing_schedule; } } else { /////////////// REJECT current_energy = initial_energy; //used in parallel tempering //reset the polar iterative failure flag system->iter_success = 0; /* restore from last checkpoint */ restore(system); register_reject(system); } // END REJECT // perform parallel_tempering if ((system->parallel_tempering) && (system->step % system->ptemp_freq == 0)) temper_system(system, current_energy); /* track the acceptance_rate */ track_ar(system->nodestats); /* each node calculates its stats */ update_nodestats(system->nodestats, system->avg_nodestats); /* do this every correlation time, and at the very end */ if (!(system->step % system->corrtime) || (system->step == system->numsteps)) { /* copy observables and avgs to the mpi send buffer */ /* histogram array is at the end of the message */ if (system->calc_hist) { zero_grid(system->grids->histogram->grid, system); population_histogram(system); } // update frozen and total system mass calc_system_mass(system); // update sorbate info on each node if (system->sorbateCount > 1) update_sorbate_info(system); /*write trajectory files for each node -> one at a time to avoid disk congestion*/ #ifdef MPI for (j = 0; j < size; j++) { MPI_Barrier(MPI_COMM_WORLD); if (j == rank) write_states(system); } #else write_states(system); #endif /*restart files for each node -> one at a time to avoid disk congestion*/ if (write_molecules_wrapper(system, system->pqr_restart) < 0) { error( "MC: could not write restart state to disk\n"); return (-1); } /*dipole/field data for each node -> one at a time to avoid disk congestion*/ #ifdef MPI if (system->polarization) { for (j = 0; j < size; j++) { MPI_Barrier(MPI_COMM_WORLD); if (j == rank) { write_dipole(system); write_field(system); } } } #else if (system->polarization) { write_dipole(system); write_field(system); } #endif /* zero the send buffer */ memset(snd_strct, 0, msgsize); memcpy(snd_strct, system->observables, sizeof(observables_t)); memcpy(snd_strct + sizeof(observables_t), system->avg_nodestats, sizeof(avg_nodestats_t)); if (system->calc_hist) mpi_copy_histogram_to_sendbuffer(snd_strct + sizeof(observables_t) + sizeof(avg_nodestats_t), system->grids->histogram->grid, system); if (system->sorbateCount > 1) memcpy(snd_strct + sizeof(observables_t) + sizeof(avg_nodestats_t) + (system->calc_hist) * system->n_histogram_bins * sizeof(int), //compensate for the size of hist data, if neccessary system->sorbateInfo, system->sorbateCount * sizeof(sorbateInfo_t)); if (!rank) memset(rcv_strct, 0, size * msgsize); #ifdef MPI MPI_Gather(snd_strct, 1, msgtype, rcv_strct, 1, msgtype, 0, MPI_COMM_WORLD); MPI_Gather(&(system->temperature), 1, MPI_DOUBLE, temperature_mpi, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD); //need to gather shit for sorbate stats also #else memcpy(rcv_strct, snd_strct, msgsize); temperature_mpi[0] = system->temperature; #endif /* MPI */ /* head node collects all observables and averages */ if (!rank) { /* clear avg_nodestats to avoid double-counting */ clear_avg_nodestats(system); //loop for each core -> shift data into variable_mpi, then average into avg_observables for (j = 0; j < size; j++) { /* copy from the mpi buffer */ memcpy(observables_mpi, rcv_strct + j * msgsize, sizeof(observables_t)); memcpy(avg_nodestats_mpi, rcv_strct + j * msgsize + sizeof(observables_t), sizeof(avg_nodestats_t)); if (system->calc_hist) mpi_copy_rcv_histogram_to_data(rcv_strct + j * msgsize + sizeof(observables_t) + sizeof(avg_nodestats_t), system->grids->histogram->grid, system); if (system->sorbateCount > 1) memcpy(sinfo_mpi, rcv_strct + j * msgsize + sizeof(observables_t) + sizeof(avg_nodestats_t) + (system->calc_hist) * system->n_histogram_bins * sizeof(int), //compensate for the size of hist data, if neccessary system->sorbateCount * sizeof(sorbateInfo_t)); /* write observables */ if (system->file_pointers.fp_energy) write_observables(system->file_pointers.fp_energy, system, observables_mpi, temperature_mpi[j]); if (system->file_pointers.fp_energy_csv) write_observables_csv(system->file_pointers.fp_energy_csv, system, observables_mpi, temperature_mpi[j]); if (system->file_pointers.fp_xyz) { write_molecules_xyz(system, system->file_pointers.fp_xyz); //L } /* collect the averages */ /* if parallel tempering, we will collect obserables from the coldest bath. this can't be done for * nodestats though, since nodestats are averaged over each corrtime, rather than based on a single * taken at the corrtime */ update_root_nodestats(system, avg_nodestats_mpi, system->avg_observables); if (!system->parallel_tempering) { update_root_averages(system, observables_mpi, system->avg_observables); if (system->calc_hist) update_root_histogram(system); if (system->sorbateCount > 1) update_root_sorb_averages(system, sinfo_mpi); } else if (system->ptemp->index[j] == 0) { update_root_averages(system, observables_mpi, system->avg_observables); if (system->calc_hist) update_root_histogram(system); if (system->sorbateCount > 1) update_root_sorb_averages(system, sinfo_mpi); } } /* write the averages to stdout */ if (system->file_pointers.fp_histogram) write_histogram(system->file_pointers.fp_histogram, system->grids->avg_histogram->grid, system); if (write_performance(system->step, system) < 0) { error( "MC: could not write performance data to stdout\n"); return (-1); } if (write_averages(system) < 0) { error( "MC: could not write statistics to stdout\n"); return (-1); } } /* !rank */ } /* corrtime */ } /* main loop */ /* write output, close any open files */ free(snd_strct); // restart files for each node if (write_molecules_wrapper(system, system->pqr_output) < 0) { error( "MC: could not write final state to disk\n"); return (-1); } if (!rank) { close_files(system); free(rcv_strct); free(temperature_mpi); } if (system->sorbateCount > 1) { free(system->sorbateGlobal); free(sinfo_mpi); } free(observables_mpi); free(avg_nodestats_mpi); printf( "MC: Total auto-rejected moves: %i\n", system->count_autorejects); return (0); }
long double __ieee754_j1l (long double x) { long double xx, xinv, z, p, q, c, s, cc, ss; if (! isfinite (x)) { if (x != x) return x; else return 0.0L; } if (x == 0.0L) return x; xx = fabsl (x); if (xx <= 0x1p-58L) { long double ret = x * 0.5L; if (fabsl (ret) < LDBL_MIN) { long double force_underflow = ret * ret; math_force_eval (force_underflow); } return ret; } if (xx <= 2.0L) { /* 0 <= x <= 2 */ z = xx * xx; p = xx * z * neval (z, J0_2N, NJ0_2N) / deval (z, J0_2D, NJ0_2D); p += 0.5L * xx; if (x < 0) p = -p; return p; } /* X = x - 3 pi/4 cos(X) = cos(x) cos(3 pi/4) + sin(x) sin(3 pi/4) = 1/sqrt(2) * (-cos(x) + sin(x)) sin(X) = sin(x) cos(3 pi/4) - cos(x) sin(3 pi/4) = -1/sqrt(2) * (sin(x) + cos(x)) cf. Fdlibm. */ __sincosl (xx, &s, &c); ss = -s - c; cc = s - c; if (xx <= LDBL_MAX / 2.0L) { z = __cosl (xx + xx); if ((s * c) > 0) cc = z / ss; else ss = z / cc; } if (xx > 0x1p256L) { z = ONEOSQPI * cc / __ieee754_sqrtl (xx); if (x < 0) z = -z; return z; } xinv = 1.0L / xx; z = xinv * xinv; if (xinv <= 0.25) { if (xinv <= 0.125) { if (xinv <= 0.0625) { p = neval (z, P16_IN, NP16_IN) / deval (z, P16_ID, NP16_ID); q = neval (z, Q16_IN, NQ16_IN) / deval (z, Q16_ID, NQ16_ID); } else { p = neval (z, P8_16N, NP8_16N) / deval (z, P8_16D, NP8_16D); q = neval (z, Q8_16N, NQ8_16N) / deval (z, Q8_16D, NQ8_16D); } } else if (xinv <= 0.1875) { p = neval (z, P5_8N, NP5_8N) / deval (z, P5_8D, NP5_8D); q = neval (z, Q5_8N, NQ5_8N) / deval (z, Q5_8D, NQ5_8D); } else { p = neval (z, P4_5N, NP4_5N) / deval (z, P4_5D, NP4_5D); q = neval (z, Q4_5N, NQ4_5N) / deval (z, Q4_5D, NQ4_5D); } } /* .25 */ else /* if (xinv <= 0.5) */ { if (xinv <= 0.375) { if (xinv <= 0.3125) { p = neval (z, P3r2_4N, NP3r2_4N) / deval (z, P3r2_4D, NP3r2_4D); q = neval (z, Q3r2_4N, NQ3r2_4N) / deval (z, Q3r2_4D, NQ3r2_4D); } else { p = neval (z, P2r7_3r2N, NP2r7_3r2N) / deval (z, P2r7_3r2D, NP2r7_3r2D); q = neval (z, Q2r7_3r2N, NQ2r7_3r2N) / deval (z, Q2r7_3r2D, NQ2r7_3r2D); } } else if (xinv <= 0.4375) { p = neval (z, P2r3_2r7N, NP2r3_2r7N) / deval (z, P2r3_2r7D, NP2r3_2r7D); q = neval (z, Q2r3_2r7N, NQ2r3_2r7N) / deval (z, Q2r3_2r7D, NQ2r3_2r7D); } else { p = neval (z, P2_2r3N, NP2_2r3N) / deval (z, P2_2r3D, NP2_2r3D); q = neval (z, Q2_2r3N, NQ2_2r3N) / deval (z, Q2_2r3D, NQ2_2r3D); } } p = 1.0L + z * p; q = z * q; q = q * xinv + 0.375L * xinv; z = ONEOSQPI * (p * cc - q * ss) / __ieee754_sqrtl (xx); if (x < 0) z = -z; return z; }
float ECL_RollController::control_bodyrate(float pitch, float roll_rate, float yaw_rate, float yaw_rate_setpoint, float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) lock_integrator = true; /* input conditioning */ // warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(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; } /* Transform setpoint to body angular rates */ _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian /* Transform estimation to body angular rates */ float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } _integrator += id; } /* integrator limit */ //xxx: until start detection is available: integral part in control signal is limited here float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); }
long double __ieee754_y1l (long double x) { long double xx, xinv, z, p, q, c, s, cc, ss; if (! isfinite (x)) { if (x != x) return x; else return 0.0L; } if (x <= 0.0L) { if (x < 0.0L) return (zero / (zero * x)); return -HUGE_VALL + x; } xx = fabsl (x); if (xx <= 0x1p-114) { z = -TWOOPI / x; if (isinf (z)) __set_errno (ERANGE); return z; } if (xx <= 2.0L) { /* 0 <= x <= 2 */ SET_RESTORE_ROUNDL (FE_TONEAREST); z = xx * xx; p = xx * neval (z, Y0_2N, NY0_2N) / deval (z, Y0_2D, NY0_2D); p = -TWOOPI / xx + p; p = TWOOPI * __ieee754_logl (x) * __ieee754_j1l (x) + p; return p; } /* X = x - 3 pi/4 cos(X) = cos(x) cos(3 pi/4) + sin(x) sin(3 pi/4) = 1/sqrt(2) * (-cos(x) + sin(x)) sin(X) = sin(x) cos(3 pi/4) - cos(x) sin(3 pi/4) = -1/sqrt(2) * (sin(x) + cos(x)) cf. Fdlibm. */ __sincosl (xx, &s, &c); ss = -s - c; cc = s - c; if (xx <= LDBL_MAX / 2.0L) { z = __cosl (xx + xx); if ((s * c) > 0) cc = z / ss; else ss = z / cc; } if (xx > 0x1p256L) return ONEOSQPI * ss / __ieee754_sqrtl (xx); xinv = 1.0L / xx; z = xinv * xinv; if (xinv <= 0.25) { if (xinv <= 0.125) { if (xinv <= 0.0625) { p = neval (z, P16_IN, NP16_IN) / deval (z, P16_ID, NP16_ID); q = neval (z, Q16_IN, NQ16_IN) / deval (z, Q16_ID, NQ16_ID); } else { p = neval (z, P8_16N, NP8_16N) / deval (z, P8_16D, NP8_16D); q = neval (z, Q8_16N, NQ8_16N) / deval (z, Q8_16D, NQ8_16D); } } else if (xinv <= 0.1875) { p = neval (z, P5_8N, NP5_8N) / deval (z, P5_8D, NP5_8D); q = neval (z, Q5_8N, NQ5_8N) / deval (z, Q5_8D, NQ5_8D); } else { p = neval (z, P4_5N, NP4_5N) / deval (z, P4_5D, NP4_5D); q = neval (z, Q4_5N, NQ4_5N) / deval (z, Q4_5D, NQ4_5D); } } /* .25 */ else /* if (xinv <= 0.5) */ { if (xinv <= 0.375) { if (xinv <= 0.3125) { p = neval (z, P3r2_4N, NP3r2_4N) / deval (z, P3r2_4D, NP3r2_4D); q = neval (z, Q3r2_4N, NQ3r2_4N) / deval (z, Q3r2_4D, NQ3r2_4D); } else { p = neval (z, P2r7_3r2N, NP2r7_3r2N) / deval (z, P2r7_3r2D, NP2r7_3r2D); q = neval (z, Q2r7_3r2N, NQ2r7_3r2N) / deval (z, Q2r7_3r2D, NQ2r7_3r2D); } } else if (xinv <= 0.4375) { p = neval (z, P2r3_2r7N, NP2r3_2r7N) / deval (z, P2r3_2r7D, NP2r3_2r7D); q = neval (z, Q2r3_2r7N, NQ2r3_2r7N) / deval (z, Q2r3_2r7D, NQ2r3_2r7D); } else { p = neval (z, P2_2r3N, NP2_2r3N) / deval (z, P2_2r3D, NP2_2r3D); q = neval (z, Q2_2r3N, NQ2_2r3N) / deval (z, Q2_2r3D, NQ2_2r3D); } } p = 1.0L + z * p; q = z * q; q = q * xinv + 0.375L * xinv; z = ONEOSQPI * (p * ss + q * cc) / __ieee754_sqrtl (xx); return z; }
static inline yana_complex_t c_let(yana_complex_t l) { assert( isfinite(creal(l)) ); assert( isfinite(cimag(l)) ); return l; }
int init_star(int *whichvel, int*whichcoord, int i, int j, int k, FTYPE *pr, FTYPE *pstag) { int set_zamo_velocity(int whichvel, struct of_geom *ptrgeom, FTYPE *pr); int set_phi_velocity_grb2(FTYPE *V, FTYPE *prstellar, FTYPE *pr); FTYPE X[NDIM],V[NDIM]; FTYPE dxdxp[NDIM][NDIM]; FTYPE r,th; void set_stellar_solution(int ii, int jj, int kk,FTYPE *pr, FTYPE *hcmsingle, FTYPE *ynu0single, FTYPE *ynusingle); FTYPE prstellar[NPR]; FTYPE przamobl[NPR]; FTYPE przamo[NPR]; FTYPE pratm[NPR]; struct of_geom geom; struct of_geom *ptrgeom; struct of_geom geombl; int pl,pliter; FTYPE hcmsingle,ynu0single,ynusingle; FTYPE parlist[MAXPARLIST]; int numparms; ////////////////////////////////// // // set free parameters // beta=1E6; Rbeta=1000.0*2.0*G*Mcgs/(C*C)/Lunit; // 1000R_S where Mcgs is the mass // DEBUG beta=1E30; // DEBUG // DEBUG ////////////////////////////////// // // Interpolate read-in data to computational grid set_stellar_solution(i,j,k,prstellar,&hcmsingle,&ynu0single,&ynusingle); // prstellar has 3-velocity in prstellar[U1], need to convert ///////////////////////////// // // Go ahead and set quantities that need no conversion of any kind // ///////////////////////////// if(!isfinite(hcmsingle)){ dualfprintf(fail_file,"read-in or interpolated bad hcmsingle: %d %d %d\n",i,j,k); } if(!isfinite(ynu0single)){ dualfprintf(fail_file,"read-in or interpolated bad ynu0single: %d %d %d\n",i,j,k); } if(!isfinite(ynusingle)){ dualfprintf(fail_file,"read-in or interpolated bad ynusingle: %d %d %d\n",i,j,k); } #if(DOYL!=DONOYL) pr[YL] = prstellar[YL]; if(!isfinite(pr[YL])){ dualfprintf(fail_file,"read-in or interpolated bad YL: %d %d %d\n",i,j,k); } #endif #if(DOYNU!=DONOYNU) // already accounted for WHICHEVOLVEYNU pr[YNU] = prstellar[YNU]; if(!isfinite(pr[YNU])){ dualfprintf(fail_file,"read-in or interpolated bad YNU: %d %d %d\n",i,j,k); } #endif // dualfprintf(fail_file,"YLYNU: i=%d yl=%21.15g ynu=%21.15g\n",i,pr[YL],pr[YNU]); //////////////////////////////////// // // Setup EOSextra for kazfulleos.c // parlist starts its index at 0 with EOSextra["TDYNORYEGLOBAL"] // // why not just call to compute EOSglobal things? (only because of H) // only matters for Kaz EOS and should be in correct order and type of quantity parlist[TDYNORYEGLOBAL-FIRSTEOSGLOBAL]=pr[YE]; // now using YE as primitive and YL as conserved ///pr[YL]-ynusingle; // Y_e for any WHICHEVOLVEYNU parlist[YNU0OLDGLOBAL-FIRSTEOSGLOBAL]=parlist[YNU0GLOBAL-FIRSTEOSGLOBAL]=ynu0single; // Y^0_\nu parlist[YNUOLDGLOBAL-FIRSTEOSGLOBAL]=ynusingle; // for WHICHEVOLVEYNU, no older yet, so indicate that by using same value int hi; for(hi=0;hi<NUMHDIRECTIONS;hi++){ parlist[HGLOBAL-FIRSTEOSGLOBAL+hi]=hcmsingle; // H } // first guess is neutrinos have U=P=S=0 parlist[UNUGLOBAL-FIRSTEOSGLOBAL]=0.0; parlist[PNUGLOBAL-FIRSTEOSGLOBAL]=0.0; parlist[SNUGLOBAL-FIRSTEOSGLOBAL]=0.0; parlist[IGLOBAL-FIRSTEOSGLOBAL]=i; parlist[JGLOBAL-FIRSTEOSGLOBAL]=j; parlist[KGLOBAL-FIRSTEOSGLOBAL]=k; store_EOS_parms(WHICHEOS,NUMEOSGLOBALS,GLOBALMAC(EOSextraglobal,i,j,k),parlist); ////////////////////////////////// // // Set other aspects of stellar model, such as rotational velocity // /////////////////////////////////// ptrgeom=&geom; get_geometry(i,j,k,CENT,ptrgeom); coord_ijk(i, j, k, CENT, X); bl_coord_ijk(i, j, k, CENT,V); dxdxprim_ijk(i, j, k, CENT,dxdxp); r=V[1]; th=V[2]; // if(i==1 && j==0 && k==0) dualfprintf(fail_file,"BANG1\n"); // PLOOP(pliter,pl) dualfprintf(fail_file,"i=%d j=%d k=%d prstellar[%d]=%21.15g\n",i,j,k,pl,prstellar[pl]); //////////////////////////// // assume in BL-coords // also adds up stellar 3-velocity to my additional atmosphere 3-velocity PLOOP(pliter,pl) pratm[pl]=prstellar[pl]; // default value (this copies densities and fields) set_phi_velocity_grb2(V,prstellar,pratm); // dualfprintf(fail_file,"prstellar[UU]=%21.15g pratm[UU]=%21.15g\n",prstellar[UU],pratm[UU]); // dualfprintf(fail_file,"%d %d :: star_rho=%g star_u=%g star_v1=%g star_v3=%g\n",i,j,pratm[RHO],pratm[UU],pratm[U1],pratm[U3]); //////////////////////////////////////////////////////////// // // Set primitives for different radial regions // //////////////////////////////////////////////////////////// // MBH is in length units if(fabs(r)<4.0*MBH){ // fabs(r) is because r can be less than 0 and if far from BH in negative sense, then don't want to still use this // chose 4MBH since then smoothly matches onto freely-falling frame from stellar model // if this close to BH, then set velocity in PRIMECOORDS since can't use BL coords inside horizon and dubiously set inside ergosphere // even if using VEL4 we can't use BL-coords inside horizon PLOOP(pliter,pl) przamo[pl]=0.0; set_zamo_velocity(WHICHVEL,ptrgeom, przamo); // in PRIMECOORDS/WHICHVEL pr[RHO]=pratm[RHO]; pr[UU]=pratm[UU]; for(pl=U1;pl<=U3;pl++) pr[pl]=przamo[pl]; for(pl=B1;pl<=B3;pl++) pr[pl]=pratm[pl]; // although really need vector potential+B3 or just B3 for 2D } else{ // GODMARK: at the moment the initial-value problem is setup only with 1 step iteration (essentially ignore star initially) // GODMARK: velocity can be quite bad if arbitrarily chosen and put in black hole that requires certain velocity near it. // so add in ZAMO observer in BL-coords here instead of afterwards // this gets BL-geometry in native BL spc coordinates (not PRIMECOORDS) gset(0,BLCOORDS,i,j,k,&geombl); przamobl[U1] = (geombl.gcon[GIND(0,1)])/(geombl.gcon[GIND(0,0)]) ; przamobl[U2] = (geombl.gcon[GIND(0,2)])/(geombl.gcon[GIND(0,0)]) ; przamobl[U3] = (geombl.gcon[GIND(0,3)])/(geombl.gcon[GIND(0,0)]) ; // can avoid doing below if using VEL4 // if(pratm[U1]<-0.1) pratm[U1]=-0.1; // near horizon time slows down so that 3-velocity actually goes to 0 for(pl=U1;pl<=U3;pl++) pratm[pl] += przamobl[pl]; // if(i==1 && j==0 && k==0) dualfprintf(fail_file,"BANG\n"); //PLOOP(pliter,pl) dualfprintf(fail_file,"i=%d j=%d k=%d prstellar[%d]=%21.15g pratm[%d]=%21.15g przamobl[%d]=%21.15g\n",i,j,k,pl,prstellar[pl],pl,pratm[pl],pl,przamobl[pl]); // convert BL-coordinate velocity to PRIMECOORDS // now conversion should be safe since have at least ZAMO + some small modification due to the star // GODMARK: converting to KSCOORDS since don't at the moment have TOV solution since haven't yet setup fluid! // *whichvel=VEL3; *whichvel=VEL4; // use 4-velocity so near BH velocity can be like in KS-coords and be -0.5 as in stellar model *whichcoord=BLCOORDS; if (bl2met2metp2v_gen(*whichvel,*whichcoord, WHICHVEL, KSCOORDS, pratm, i,j,k) >= 1) FAILSTATEMENT("init.readdata.c:get_data()", "bl2ks2ksp2v()", 1); /////////////////////// // // add up velocities in PRIMECOORDS // zamo is used in case near black hole so solution still good, where other term is not expected to account for black hole // zamo will be small if black hole starts off with small mass compared to self-gravity mass pr[RHO]=pratm[RHO]; pr[UU]=pratm[UU]; //for(pl=U1;pl<=U3;pl++) pr[pl]=przamo[pl]+pratm[pl]; // already accounted for ZAMO term in BL-coords above for(pl=U1;pl<=U3;pl++) pr[pl]=pratm[pl]; // These field components are overwritten by vector potential solution // If want B3, should provide A_r or A_\theta in init.c for(pl=B1;pl<=B3;pl++) pr[pl]=pratm[pl]; // although really need vector potential+B3 or just B3 for 2D // DEBUG pr[U3]=0.0;// DEBUG // DEBUG } // if(i==256 && j==0 && k==0) dualfprintf(fail_file,"BANG\n"); // PLOOP(pliter,pl) dualfprintf(fail_file,"i=%d j=%d k=%d prstellar[%d]=%21.15g przamo=%21.15g pratmpost=%21.15g\n",i,j,k,pl,prstellar[pl],przamo[pl],pratm[pl]); // dualfprintf(fail_file,"prstellar[UU]=%21.15g pratm[UU]=%21.15g pr[UU]=%21.15g\n",prstellar[UU],pratm[UU],pr[UU]); // assume same for now GODMARK (only non-field set so far anyways) PLOOP(pliter,pl){ pstag[pl]=pr[pl]; } ////////////////////////////////// // // Choose conversion of velocity // // assume already converted everything to PRIMECOORDS/WHICHVEL // use if setting in PRIMECOORDS *whichvel=WHICHVEL; *whichcoord=PRIMECOORDS; return(0); }