コード例 #1
0
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);
}
コード例 #2
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;
}
コード例 #3
0
ファイル: tSolve.cpp プロジェクト: dajochen/jrlib
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;
  }

}
コード例 #4
0
ファイル: k_standard.c プロジェクト: Drakey83/steamlink-sdk
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);
    }
}
コード例 #5
0
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);
     }
}
コード例 #6
0
ファイル: train_l2sgd.c プロジェクト: pprett/crfsuite
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);
}
コード例 #7
0
ファイル: Tuning_table.c プロジェクト: EdwardBetts/kunquat
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;
}
コード例 #8
0
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.;
	    }
	}
    }
}
コード例 #10
0
ファイル: s_ctan.c プロジェクト: alucas/glibc
__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;
}
コード例 #11
0
ファイル: s_fma.c プロジェクト: 0omega/platform_bionic
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));
}
コード例 #12
0
/**
 * 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;
      }
    }
  }
}
コード例 #13
0
/** 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()
コード例 #14
0
ファイル: s_fmal.c プロジェクト: michalsc/AROS
/*
 * 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));
}
コード例 #15
0
ファイル: PathCG.cpp プロジェクト: studiomobile/webcore
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);
}
コード例 #16
0
ファイル: ensobs.c プロジェクト: sakov/enkf-c
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;
}
コード例 #17
0
ファイル: train_l2sgd.c プロジェクト: pprett/crfsuite
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;
}
コード例 #18
0
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);
}
コード例 #19
0
ファイル: Tuning_table.c プロジェクト: EdwardBetts/kunquat
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, &cents))
            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, &cents))
            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;
}
コード例 #20
0
ファイル: sprintf.c プロジェクト: dycoon/mruby_dycoon
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;
}
コード例 #21
0
	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);


	}
コード例 #22
0
/* Check for a timesteps etc. which will actually finish. */
int mwCheckNormalPosNumEps(real n)
{
    return !isfinite(n) || n <= REAL_EPSILON;
}
コード例 #23
0
ファイル: mnehs_ms.c プロジェクト: Fahdben/imscript
// 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);
}
コード例 #24
0
/* Check for positive, real numbers that can be any size */
int mwCheckNormalPosNum(real n)
{
    return !isfinite(n) || n <= 0.0;
}
コード例 #25
0
ファイル: mc.c プロジェクト: mpmccode/mpmc
/* 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);
}
コード例 #26
0
ファイル: e_j1l.c プロジェクト: KubaKaszycki/kklibc
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;
}
コード例 #27
0
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);
}
コード例 #28
0
ファイル: e_j1l.c プロジェクト: KubaKaszycki/kklibc
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;
}
コード例 #29
0
ファイル: solver.c プロジェクト: ploki/yanapack
static inline yana_complex_t c_let(yana_complex_t l)
{
    assert( isfinite(creal(l)) );
    assert( isfinite(cimag(l)) );
    return l;
}
コード例 #30
0
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);


}