Пример #1
0
System* Articulation::system() const
      {
      Measure* m = measure();
      return static_cast<System*>(m ? m->parent() : 0);
      }
Пример #2
0
double MeUltrasonic::distanceCm(uint16_t maxCm)
{
    long distance = measure(maxCm * 55 + 200);
    return (double)distance / 58.0;
}
Пример #3
0
int
MB12XX::probe()
{
	return measure();
}
Пример #4
0
System* Fermata::system() const
      {
      Measure* m = measure();
      return toSystem(m ? m->parent() : 0);
      }
Пример #5
0
int
main()
{
  try {
    unsigned int nsignal = 2; // Number of signal to filter
    unsigned int niter = 200;
    unsigned int size_state_vector = 2*nsignal;
    unsigned int size_measure_vector = 1*nsignal;
    //vpMeasureType measure_t = Velocity;
    vpMeasureType measure_t = Position;

    std::string filename = "/tmp/log.dat";
    std::ofstream flog(filename.c_str());

    vpLinearKalmanFilterInstantiation kalman;

    vpColVector sigma_measure(size_measure_vector);
    for (unsigned int signal=0; signal < nsignal; signal ++)
      sigma_measure = 0.000001;
    vpColVector sigma_state(size_state_vector);

    switch (measure_t) {
    case Velocity:
      for (unsigned int signal=0; signal < nsignal; signal ++) {
        sigma_state[2*signal] = 0.; // not used
        sigma_state[2*signal+1] = 0.000001;
      }
      break;
    case Position:
      for (unsigned int signal=0; signal < nsignal; signal ++) {
        sigma_state[2*signal] = 0.000001;
        sigma_state[2*signal+1] = 0; // not used
      }
      break;
    }

    vpColVector measure(size_measure_vector);

    for (unsigned int signal=0; signal < nsignal; signal ++) {
      measure[signal] = 3+2*signal;
    }

    kalman.verbose(true);

    vpLinearKalmanFilterInstantiation::vpStateModel model;
    double dt = 0.04; // Sampling period
    double rho = 0.5;
    double dummy = 0; // non used parameter
    switch (measure_t) {
    case Velocity:
      model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel;
      kalman.setStateModel(model);
      kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
      break;
    case Position:
      model = vpLinearKalmanFilterInstantiation::stateConstVel_MeasurePos;
      kalman.setStateModel(model);
      kalman.initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
      break;
    }

    for (unsigned int iter=0; iter <= niter; iter++) {
      std::cout << "-------- iter " << iter << " ------------" << std::endl;
      for (unsigned int signal=0; signal < nsignal; signal ++) {
        measure[signal] = 3+2*signal + 0.3*sin(vpMath::rad(360./niter*iter));
      }
      std::cout << "measure : " << measure.t() << std::endl;

      flog << measure.t();

      //    kalman.prediction();
      kalman.filter(measure);
      flog << kalman.Xest.t() << std::endl;

      std::cout << "Xest: " << kalman.Xest.t() << std::endl;
    }

    flog.close();
    return 0;
  }
  catch(vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
    return 1;
  }
}
Пример #6
0
Element* Jump::nextElement()
      {
      Segment* seg = measure()->last();
      return seg->firstElement(staffIdx());
      }
Пример #7
0
/*
 * synchronize all of the slaves
 */
void
synch(long mydelta)
{
	struct hosttbl *htp;
	int measure_status;
	struct timeval check, stop, wait;

	if (slvcount > 0) {
		if (trace)
			fprintf(fd, "measurements starting at %s\n", date());
		gettimeofday(&check, 0);
		for (htp = self.l_fwd; htp != &self; htp = htp->l_fwd) {
			if (htp->noanswer != 0) {
				measure_status = measure(500, 100,
							 htp->name,
							 &htp->addr,0);
			} else {
				measure_status = measure(3000, 100,
							 htp->name,
							 &htp->addr,0);
			}
			if (measure_status != GOOD) {
				/* The slave did not respond.  We have
				 * just wasted lots of time on it.
				 */
				htp->delta = HOSTDOWN;
				if (++htp->noanswer >= LOSTHOST) {
					if (trace) {
						fprintf(fd,
					"purging %s for not answering ICMP\n",
							htp->name);
						fflush(fd);
					}
					htp = remmach(htp);
				}
			} else {
				htp->delta = measure_delta;
			}
			gettimeofday(&stop, 0);
			timevalsub(&stop, &stop, &check);
			if (stop.tv_sec >= 1) {
				if (trace)
					fflush(fd);
				/*
				 * ack messages periodically
				 */
				wait.tv_sec = 0;
				wait.tv_usec = 0;
				if (0 != readmsg(TSP_TRACEON,ANYADDR,
						 &wait,0))
					traceon();
				gettimeofday(&check, 0);
			}
		}
		if (trace)
			fprintf(fd, "measurements finished at %s\n", date());
	}
	if (!(status & SLAVE)) {
		if (!dictate) {
			mydelta = networkdelta();
		} else {
			dictate--;
		}
	}
	if (trace && (mydelta != 0 || (status & SLAVE)))
		fprintf(fd,"local correction of %ld ms.\n", mydelta);
	correct(mydelta);
}
Пример #8
0
int
SF0X::probe()
{
	return measure();
}
Пример #9
0
void
SF0X::cycle()
{
	/* fds initialized? */
	if (_fd < 0) {
		/* open fd */
		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
	}

	/* collection phase? */
	if (_collect_phase) {

		/* perform collection */
		int collect_ret = collect();

		if (collect_ret == -EAGAIN) {
			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&SF0X::cycle_trampoline,
				   this,
				   USEC2TICK(1042 * 8));
			return;
		}

		if (OK != collect_ret) {

			/* we know the sensor needs about four seconds to initialize */
			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
				DEVICE_LOG("collection error #%u", _consecutive_fail_count);
			}

			_consecutive_fail_count++;

			/* restart the measurement state machine */
			start();
			return;

		} else {
			/* apparently success */
			_consecutive_fail_count = 0;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/*
		 * Is there a collect->measure gap?
		 */
		if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&SF0X::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL));

			return;
		}
	}

	/* measurement phase */
	if (OK != measure()) {
		DEVICE_LOG("measure error");
	}

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&SF0X::cycle_trampoline,
		   this,
		   USEC2TICK(SF0X_CONVERSION_INTERVAL));
}
Пример #10
0
/*-----------------------------進行subpixel Corner------------------------------*/
void	MainWindow::CornerProcess()
{

	measure(OpenPIV::Corner);
}
Пример #11
0
int64_t Timer::getMicroSeconds() const {
  return measure() - m_start;
}
Пример #12
0
/*-----------------------------進行subpixel Edge------------------------------*/
void	MainWindow::EdgeProcess()
{

	measure(OpenPIV::Edge);
}
Пример #13
0
/*-----------------------------進行Steer的Junction Detector------------------------------*/
void	MainWindow::SteerJunctionProcess()
{

	measure(OpenPIV::Steer);
}
Пример #14
0
/*-----------------------------進行FFT的Junction Detector------------------------------*/
void	MainWindow::FFTJunctionProcess()
{
	measure(OpenPIV::FFT);
}
Пример #15
0
int
LL40LS::probe()
{
	return measure();
}
Пример #16
0
long Me_UltrasonicSensor::distanceCm()
{
	long distance = measure();
	return ((distance/29)>>1);
}
Пример #17
0
int
MPU9250::init()
{
	int ret;

	/* do SPI init (and probe) first */
	ret = SPI::init();

	/* if probe/setup failed, bail now */
	if (ret != OK) {
		DEVICE_DEBUG("SPI setup failed");
		return ret;
	}

	ret = probe();

	if (ret != OK) {
		DEVICE_DEBUG("MPU9250 probe failed");
		return ret;
	}

	/* allocate basic report buffers */
	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));

	if (_accel_reports == nullptr) {
		goto out;
	}

	_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));

	if (_gyro_reports == nullptr) {
		goto out;
	}

	if (reset() != OK) {
		goto out;
	}

	/* Initialize offsets and scales */
	_accel_scale.x_offset = 0;
	_accel_scale.x_scale  = 1.0f;
	_accel_scale.y_offset = 0;
	_accel_scale.y_scale  = 1.0f;
	_accel_scale.z_offset = 0;
	_accel_scale.z_scale  = 1.0f;

	_gyro_scale.x_offset = 0;
	_gyro_scale.x_scale  = 1.0f;
	_gyro_scale.y_offset = 0;
	_gyro_scale.y_scale  = 1.0f;
	_gyro_scale.z_offset = 0;
	_gyro_scale.z_scale  = 1.0f;

	/* do CDev init for the gyro device node, keep it optional */
	ret = _gyro->init();

	/* if probe/setup failed, bail now */
	if (ret != OK) {
		DEVICE_DEBUG("gyro init failed");
		return ret;
	}

	/* do CDev init for the mag device node, keep it optional */
	ret = _mag->init();

	/* if probe/setup failed, bail now */
	if (ret != OK) {
		DEVICE_DEBUG("mag init failed");
		return ret;
	}

	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);

	measure();

	/* advertise sensor topic, measure manually to initialize valid report */
	struct accel_report arp;
	_accel_reports->get(&arp);

	/* measurement will have generated a report, publish */
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
					   &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);

	if (_accel_topic == nullptr) {
		warnx("ADVERT FAIL");
	}

	/* advertise sensor topic, measure manually to initialize valid report */
	struct gyro_report grp;
	_gyro_reports->get(&grp);

	_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
			     &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);

	if (_gyro->_gyro_topic == nullptr) {
		warnx("ADVERT FAIL");
	}

out:
	return ret;
}
Пример #18
0
long Me_UltrasonicSensor::distanceInch()
{
	long distance = measure();
	return ((distance/74)>>1);
}
Пример #19
0
int
LSM303D::init()
{
	int ret = ERROR;

	/* do SPI init (and probe) first */
	if (SPI::init() != OK) {
		warnx("SPI init failed");
		goto out;
	}

	/* allocate basic report buffers */
	_accel_reports = new RingBuffer(2, sizeof(accel_report));

	if (_accel_reports == nullptr)
		goto out;

	/* advertise accel topic */
	_mag_reports = new RingBuffer(2, sizeof(mag_report));

	if (_mag_reports == nullptr)
		goto out;

	reset();

	/* do CDev init for the mag device node */
	ret = _mag->init();
	if (ret != OK) {
		warnx("MAG init failed");
		goto out;
	}

	/* fill report structures */
	measure();

	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {

		/* advertise sensor topic, measure manually to initialize valid report */
		struct mag_report mrp;
		_mag_reports->get(&mrp);

		/* measurement will have generated a report, publish */
		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);

		if (_mag->_mag_topic < 0)
			debug("failed to create sensor_mag publication");

	}

	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);

	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {

		/* advertise sensor topic, measure manually to initialize valid report */
		struct accel_report arp;
		_accel_reports->get(&arp);

		/* measurement will have generated a report, publish */
		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);

		if (_accel_topic < 0)
			debug("failed to create sensor_accel publication");

	}

out:
	return ret;
}
Пример #20
0
int
SRF02::init()
{
	int ret = ERROR;

	/* do I2C init (and probe) first */
	if (I2C::init() != OK) {
		goto out;
	}

	/* allocate basic report buffers */
	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));

	_index_counter = SRF02_BASEADDR;	/* set temp sonar i2c address to base adress */
	set_address(_index_counter);		/* set I2c port to temp sonar i2c adress */

	if (_reports == nullptr) {
		goto out;
	}

	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);

	if (_class_instance == CLASS_DEVICE_PRIMARY) {
		/* get a publish handle on the range finder topic */
		struct distance_sensor_s ds_report = {};

		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
					 &_orb_class_instance, ORB_PRIO_LOW);

		if (_distance_sensor_topic == nullptr) {
			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
		}
	}

	// XXX we should find out why we need to wait 200 ms here
	usleep(200000);

	/* check for connected rangefinders on each i2c port:
	   We start from i2c base address (0x70 = 112) and count downwards
	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
		_index_counter = SRF02_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */
		set_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
		int ret2 = measure();

		if (ret2 == 0) { /* sonar is present -> store address_index in array */
			addr_ind.push_back(_index_counter);
			DEVICE_DEBUG("sonar added");
			_latest_sonar_measurements.push_back(200);
		}
	}

	_index_counter = SRF02_BASEADDR;
	set_address(_index_counter); /* set i2c port back to base adress for rest of driver */

	/* if only one sonar detected, no special timing is required between firing, so use default */
	if (addr_ind.size() == 1) {
		_cycling_rate = SRF02_CONVERSION_INTERVAL;

	} else {
		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
	}

	/* show the connected sonars in terminal */
	for (unsigned i = 0; i < addr_ind.size(); i++) {
		DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
	}

	DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size());

	ret = OK;
	/* sensor is ok, but we don't really know if it is within range */
	_sensor_ok = true;
out:
	return ret;
}
Пример #21
0
/*
 * Clockdiff computes the difference between the time of the machine on
 * which it is called and the time of the machines given as argument.
 * The time differences measured by clockdiff are obtained using a sequence
 * of ICMP TSTAMP messages which are returned to the sender by the IP module
 * in the remote machine.
 * In order to compare clocks of machines in different time zones, the time
 * is transmitted (as a 32-bit value) in milliseconds since midnight UT.
 * If a hosts uses a different time format, it should set the high order
 * bit of the 32-bit quantity it transmits.
 * However, VMS apparently transmits the time in milliseconds since midnight
 * local time (rather than GMT) without setting the high order bit.
 * Furthermore, it does not understand daylight-saving time.  This makes
 * clockdiff behaving inconsistently with hosts running VMS.
 *
 * In order to reduce the sensitivity to the variance of message transmission
 * time, clockdiff sends a sequence of messages.  Yet, measures between
 * two `distant' hosts can be affected by a small error. The error can,
 * however, be reduced by increasing the number of messages sent in each
 * measurement.
 */
void
clockdiff(int argc, char *argv[])
{
	int measure_status;
	extern int measure(u_long, u_long, char *, struct sockaddr_in*, int);
	int avg_cnt;
	long avg;
	struct servent *sp;

	if (argc < 2)  {
		printf("usage: timedc clockdiff host ...\n");
		return;
	}

	if (gethostname(myname, sizeof(myname) - 1) < 0)
		err(1, "gethostname");

	/* get the address for the date ready */
	sp = getservbyname(DATE_PORT, DATE_PROTO);
	if (!sp) {
		warnx("%s/%s is an unknown service", DATE_PORT, DATE_PROTO);
		dayaddr.sin_port = 0;
	} else {
		dayaddr.sin_port = sp->s_port;
	}

	while (argc > 1) {
		argc--; argv++;
		hp = gethostbyname(*argv);
		if (hp == NULL) {
			warnx("%s: %s", *argv, hstrerror(h_errno));
			continue;
		}

		server.sin_family = hp->h_addrtype;
		bcopy(hp->h_addr, &server.sin_addr.s_addr, hp->h_length);
		for (avg_cnt = 0, avg = 0; avg_cnt < 16; avg_cnt++) {
			measure_status = measure(10000,100, *argv, &server, 1);
			if (measure_status != GOOD)
				break;
			avg += measure_delta;
		}
		if (measure_status == GOOD)
			measure_delta = avg/avg_cnt;

		switch (measure_status) {
		case HOSTDOWN:
			printf("%s is down\n", hp->h_name);
			continue;
		case NONSTDTIME:
			printf("%s transmitts a non-standard time format\n",
			       hp->h_name);
			continue;
		case UNREACHABLE:
			printf("%s is unreachable\n", hp->h_name);
			continue;
		}

		/*
		 * Try to get the date only after using ICMP timestamps to
		 * get the time.  This is because the date protocol
		 * is optional.
		 */
		if (dayaddr.sin_port != 0) {
			dayaddr.sin_family = hp->h_addrtype;
			bcopy(hp->h_addr, &dayaddr.sin_addr.s_addr,
			      hp->h_length);
			avg = daydiff(*argv);
			if (avg > SECDAY) {
				printf("time on %s is %ld days ahead %s\n",
				       hp->h_name, avg/SECDAY, myname);
				continue;
			} else if (avg < -SECDAY) {
				printf("time on %s is %ld days behind %s\n",
				       hp->h_name, -avg/SECDAY, myname);
				continue;
			}
		}

		if (measure_delta > 0) {
			printf("time on %s is %d ms. ahead of time on %s\n",
			       hp->h_name, measure_delta, myname);
		} else if (measure_delta == 0) {
			printf("%s and %s have the same time\n",
			       hp->h_name, myname);
		} else {
			printf("time on %s is %d ms. behind time on %s\n",
			       hp->h_name, -measure_delta, myname);
		}
	}
	return;
}
Пример #22
0
int
SRF02::probe()
{
	return measure();
}
Пример #23
0
int
FXOS8700CQ::init()
{
	int ret = PX4_ERROR;

	/* do SPI init (and probe) first */
	if (SPI::init() != OK) {
		PX4_ERR("SPI init failed");
		goto out;
	}

	/* allocate basic report buffers */
	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));

	if (_accel_reports == nullptr) {
		goto out;
	}

	_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));

	if (_mag_reports == nullptr) {
		goto out;
	}

	reset();

	/* do CDev init for the mag device node */
	ret = _mag->init();

	if (ret != OK) {
		PX4_ERR("MAG init failed");
		goto out;
	}

	/* fill report structures */
	measure();

	/* advertise sensor topic, measure manually to initialize valid report */
	struct mag_report mrp;
	_mag_reports->get(&mrp);

	/* measurement will have generated a report, publish */
	_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
					       &_mag->_mag_orb_class_instance, ORB_PRIO_LOW);

	if (_mag->_mag_topic == nullptr) {
		PX4_ERR("ADVERT ERR");
	}


	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);

	/* advertise sensor topic, measure manually to initialize valid report */
	struct accel_report arp;
	_accel_reports->get(&arp);

	/* measurement will have generated a report, publish */
	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
					   &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);

	if (_accel_topic == nullptr) {
		PX4_ERR("ADVERT ERR");
	}

out:
	return ret;
}
Пример #24
0
void
SRF02::cycle()
{
	if (_collect_phase) {
		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
		set_address(_index_counter);

		/* perform collection */
		if (OK != collect()) {
			DEVICE_DEBUG("collection error");
			/* if error restart the measurement state machine */
			start();
			return;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/* change i2c adress to next sonar */
		_cycle_counter = _cycle_counter + 1;

		if (_cycle_counter >= addr_ind.size()) {
			_cycle_counter = 0;
		}

		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */

		if (_measure_ticks > USEC2TICK(_cycling_rate)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&SRF02::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(_cycling_rate));
			return;
		}
	}

	/* Measurement (firing) phase */

	/* ensure sonar i2c adress is still correct */
	_index_counter = addr_ind[_cycle_counter];
	set_address(_index_counter);

	/* Perform measurement */
	if (OK != measure()) {
		DEVICE_DEBUG("measure error sonar adress %d", _index_counter);
	}

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&SRF02::cycle_trampoline,
		   this,
		   USEC2TICK(_cycling_rate));

}
Пример #25
0
bool IntrEllipse2Ellipse2<Real>::Find ()
{
    mQuantity = 0;

    // Test for separation of oriented bounding boxes of ellipses.  This is
    // a fast-out attempt.
    Box2<Real> box0, box1;
    box0.Center = mEllipse0->Center;
    box0.Axis[0] = mEllipse0->Axis[0];
    box0.Axis[1] = mEllipse0->Axis[1];
    box0.Extent[0] = mEllipse0->Extent[0];
    box0.Extent[1] = mEllipse0->Extent[1];
    box1.Center = mEllipse1->Center;
    box1.Axis[0] = mEllipse1->Axis[0];
    box1.Axis[1] = mEllipse1->Axis[1];
    box1.Extent[0] = mEllipse1->Extent[0];
    box1.Extent[1] = mEllipse1->Extent[1];
    if(!IntrBox2Box2<Real>(box0, box1).Test())
    {
        // The boxes do not overlap, so neither do the ellipses.
        mIntersectionType = IT_EMPTY;
        return false;
    }

    // Compute the 4th-degree polynomial whose roots lead to intersections of
    // the ellipses, and then compute its roots.
    Polynomial1<Real> poly = GetQuartic(*mEllipse0, *mEllipse1);
    PolynomialRoots<Real> proots(Math<Real>::ZERO_TOLERANCE);
    proots.FindB(poly, DIGITS_ACCURACY);
    int yCount = proots.GetCount();
    const Real* yRoot = proots.GetRoots();
    if (yCount == 0)
    {
        mIntersectionType = IT_EMPTY;
        return false;
    }

    // TODO:  Adjust the comments.
    // Compute the coefficients of a polynomial in s = sin(angle) and
    // c = cos(angle) that relates ellipse0 to ellipse1
    // affinely transformed to a circle.  The polynomial is
    //   d0 + d1*c + d2*s + d3*c^2 + d4*c*s + d5*s^2 = 0
    // where c^2 + s^2 = 1.
    Vector2<Real> C0mC1 = mEllipse0->Center - mEllipse1->Center;
    Matrix2<Real> M1;
    mEllipse1->GetM(M1);
    Vector2<Real> M1C0mC1 = M1*C0mC1;
    Vector2<Real> M1A0 = M1*mEllipse0->Axis[0];
    Vector2<Real> M1A1 = M1*mEllipse0->Axis[1];
    Real coeff[6];
    coeff[0] = M1C0mC1.Dot(C0mC1) - (Real)1;
    coeff[1] = ((Real)2)*mEllipse0->Extent[0]*(M1A0.Dot(C0mC1));
    coeff[2] = ((Real)2)*mEllipse0->Extent[1]*(M1A1.Dot(C0mC1));
    coeff[3] = mEllipse0->Extent[0]*mEllipse0->Extent[0]*
        (M1A0.Dot(mEllipse0->Axis[0]));
    coeff[4] = ((Real)2)*mEllipse0->Extent[0]*mEllipse0->Extent[1]*
        (M1A0.Dot(mEllipse0->Axis[1]));
    coeff[5] = mEllipse0->Extent[1]*mEllipse0->Extent[1]*
        (M1A1.Dot(mEllipse0->Axis[1]));

    // Evaluate the quadratics, saving the values to test later for closeness
    // to zero and for root polishing.
    Real qp0[6], qp1[6];
    mEllipse0->ToCoefficients(qp0);
    mEllipse1->ToCoefficients(qp1);
    std::vector<Measurement> measure(8);  // store <x,y,sqrt(Q0^2+S1^2)>
    Vector2<Real> point;
    int i;
    for (int iy = 0; iy < yCount; ++iy)
    {
        point[1] = yRoot[iy];
        PolynomialRoots<Real> ar(Math<Real>::ZERO_TOLERANCE);
        Polynomial1<Real> apoly(2);
        apoly[0] = qp0[0] + point[1]*(qp0[2] + point[1]*qp0[5]);
        apoly[1] = qp0[1] + point[1]*qp0[4];
        apoly[2] = qp0[3];
        ar.FindB(apoly, DIGITS_ACCURACY);
        int xCount = ar.GetCount();
        const Real* xRoot = ar.GetRoots();
        for (int ix = 0; ix < xCount; ++ix)
        {
            point[0] = xRoot[ix];
            Real q0 = mEllipse0->Evaluate(point);
            Real q1 = mEllipse1->Evaluate(point);

            Real angle0;
            bool transverse = RefinePoint(coeff, point, q0, q1, angle0);

            i = ix + 2*iy;
            measure[i].Point = point;
            measure[i].Q0 = q0;
            measure[i].Q1 = q1;
            measure[i].Norm = Math<Real>::Sqrt(q0*q0 + q1*q1);
            measure[i].Angle0 = angle0;
            measure[i].Transverse = transverse;
        }
    }
    std::sort(measure.begin(), measure.end());

    for (i = 0; i < 8; ++i)
    {
        if (measure[i].Norm < Math<Real>::ZERO_TOLERANCE)
        {
            int j;
            Real adiff;
            for (j = 0; j < mQuantity; ++j)
            {
                adiff = measure[i].Angle0 - measure[j].Angle0;
                if (Math<Real>::FAbs(adiff) < Math<Real>::ZERO_TOLERANCE)
                {
                    break;
                }
            }
            if (j == mQuantity)
            {
                mPoint[mQuantity] = measure[i].Point;
                mTransverse[mQuantity] = measure[i].Transverse;
                if (++mQuantity == 4)
                {
                    break;
                }
            }
        }
    }

    if (mQuantity == 0)
    {
        mIntersectionType = IT_EMPTY;
        return false;
    }

    mIntersectionType = IT_POINT;
    return true;
}
Пример #26
0
int main () {

  try {
  
    // -------------------------------------------------------------
    // ---------------- set the cosmological parameters ------------
    // -------------------------------------------------------------

    const double OmegaM = 0.27;
    const double Omega_b = 0.046;
    const double Omega_nu = 0.;
    const double massless_neutrinos = 3.04;
    const int    massive_neutrinos = 0; 
    const double OmegaL = 1.-OmegaM;
    const double Omega_radiation = 0.;
    const double hh = 0.7;
    const double scalar_amp = 2.46e-9;
    const double n_s = 0.96;
    const double wa = 0.;
    const double w0 = -1.;   
  
    const cosmobl::cosmology::Cosmology cosmology {OmegaM, Omega_b, Omega_nu, massless_neutrinos, massive_neutrinos, OmegaL, Omega_radiation, hh, scalar_amp, n_s, w0, wa};
  
  
    // ----------------------------------------------------------
    // ---------------- read the input catalogue ----------------
    // ----------------------------------------------------------
  
    const string file_catalogue = cosmobl::par::DirLoc+"../input/cat.dat";

    const cosmobl::catalogue::Catalogue catalogue {cosmobl::catalogue::_Galaxy_, cosmobl::_observedCoordinates_, {file_catalogue}, cosmology};
  
  
    // --------------------------------------------------------------------------------------
    // ---------------- construct the random catalogue (with cubic geometry) ---------------- 
    // -------------------------------------------------------------------------------------- 

    const double N_R = 1.; // random/data ratio
 
    const cosmobl::catalogue::Catalogue random_catalogue {cosmobl::catalogue::_createRandom_box_, catalogue, N_R};
  
  
    // -----------------------------------------------------------------------------------------------
    // ---------------- measure the 2D two-point correlation in Cartesian coordinates ----------------
    // -----------------------------------------------------------------------------------------------

    // ----- output data ----- 

    const string dir_pairs = cosmobl::par::DirLoc+"../output/";
    const string dir_output = cosmobl::par::DirLoc+"../output/";

  
    // ----- measure the 2D correlation function in Cartesian coordinates, xi(rp,pi), and store the outputs ----- 
  
    const double rpMin = 1.;     // minimum separation in the first dimension
    const double rpMax = 50.;    // maximum separation in the first dimension 
    const int nbins_D1 = 50;     // number of bins in the first dimension
    const double shift_D1 = 0.5; // spatial shift used to set the bin centre in the first dimension
    const double piMin = 1.;     // minimum separation in the second dimension
    const double piMax = 50.;    // maximum separation in the second dimension 
    const int nbins_D2 = 50;     // number of bins in the second dimension
    const double shift_D2 = 0.5; // spatial shift used to set the bin centre in the second dimension
  
    // construct the object using a static factory
    const auto xi2DCart = cosmobl::twopt::TwoPointCorrelation::Create(cosmobl::twopt::_2D_Cartesian_, catalogue, random_catalogue, cosmobl::_linear_, rpMin, rpMax, nbins_D1, shift_D1, cosmobl::_linear_, piMin, piMax, nbins_D2, shift_D2);

    // measure the 2D correlation function and compute Poisson errors
    xi2DCart->measure(cosmobl::twopt::_Poisson_, dir_pairs);
    xi2DCart->write(dir_output, "xi_rp_pi_linlin.dat");

  }

  catch(cosmobl::glob::Exception &exc) { std::cerr << exc.what() << std::endl; }
  
  return 0;
}
Пример #27
0
double MeUltrasonic::distanceInch(uint16_t maxInch)
{
    long distance = measure(maxInch * 145 + 200);
    return (double)(distance / 148.0);
}
Пример #28
0
void run(Par *par)
{
  int i, itherm, nstep, istep, isamp, iblock, st;
  int nsamp = par->nsamp, nblock = par->nblock;
  char wfile[FNAMESIZE];
  Vec *pos, *vel, *force;
  double ekin, epot;
  double v0[NV], v1[NV], v2[NV];
  FILE *estream = NULL;

  // Initialize

  //if (par->alpha > 0.0) {
  printf("Seed for random number generator: %d.\n", par->seed);
  init_ran(par->seed);
  //}

  pos = malloc(par->n * sizeof(Vec));
  force = malloc(par->n * sizeof(Vec));
  vel = malloc(par->n * sizeof(Vec));

  // Read from file...
  if (par->readfile)
    st = read_conf(par->n, pos, vel, par->readfile);
  else {
    init_pos(par->n, &par->L, pos);
    set_temperature(par->n, par->t, vel);
  }

  // Get file name for writing to.
  get_filename(par, wfile);

  // Open file for writing energy results
  estream = fopen_wfile("efile/", wfile);
  if (!estream) return;

  measure(par->n, &par->L, pos, vel, &epot, &ekin);
  //printf("Energy = %g  (%g  %g)\n", epot + ekin, epot, ekin);

  for(i=0; i<par->ntherm; i++) {
    step(par, pos, vel, force);
  }
  //printf("rx ry vx vy = %g  %g  %g  %g\n", pos[0].x, pos[0].y, vel[0].x, vel[0].y);

  // Run and collect values
  printf("\nSimulate %d blocks x %d samples each: ", par->nblock, par->nsamp);
  fflush(stdout);

  
  // Initialize for measuring a histogram of particle distances for
  // distances up to 5.0 and bin size 0.02.
  // init_pcorr(par->n, 0.02, 5.0);
 
  init_pcorr();
  for (i = 0; i < NV; i++) v1[i] = v2[i] = 0.0;

  nstep = rint(1.0 / par->deltat);
  
  
  for (iblock = 0; iblock < nblock; iblock++) {

    for (i = 0; i < NV; i++) v0[i] = 0.0;
    for (isamp = 0; isamp < nsamp; isamp++) {
      for (istep = 0; istep < nstep; istep++) {
	      step(par, pos, vel, force);
      }

      

      measure(par->n, &par->L, pos, vel, &epot, &ekin);
      if (estream) fprintf(estream, "%d %g\n", isamp + nsamp * iblock, epot + ekin);
      v0[0] += epot;
      //v0[1] += ekin;
      //v0[2] += epot + ekin;
    }
    for (i = 0; i < NV; i++) {
      v0[i] /= nsamp;
      v1[i] += v0[i];
      v2[i] += v0[i] * v0[i];
    }

    printf("%d ", iblock + 1);	fflush(stdout);
  }
  
  printf("\n");
 
  
  if (estream) fclose(estream);

  for (i = 0; i < NV; i++) {
    v1[i] /= nblock;
    v2[i] /= nblock;
  }

  // Write configuration to the named file.
  write_conf(par->n, pos, vel, "conf/", wfile);

  measure_pcorr(par->n, &par->L, pos);
  // Write velocity correlation results to files.
  write_pcorr(par->n, wfile);
  // write_pcorr(par->n, wfile);

  // Print out some results
  printf("\n");
  print_standard_error("Potential E:  ", v1[0], v2[0], nblock);
  //print_standard_error("Kinetic  E:   ", v1[1], v2[1], nblock);
  //print_standard_error("Total energy: ", v1[2], v2[2], nblock);


  // From the virial theorem:  pressure = N * T + Virial / Dimensionality

  free(vel);
  free(pos);
  
}
Пример #29
0
void Rest::layout()
      {
      if (staff() && staff()->isTabStaff()) {
            // no rests for tablature
            _space.setLw(0.0);
            _space.setRw(0.0);
            return;
            }
      switch(durationType().type()) {
            case TDuration::V_64TH:
            case TDuration::V_32ND:
                  dotline = -3;
                  break;
            case TDuration::V_256TH:
            case TDuration::V_128TH:
                  dotline = -5;
                  break;
            default:
                  dotline = -1;
                  break;
            }
      qreal _spatium = spatium();
      int stepOffset     = 0;
      if (staff())
            stepOffset = staff()->staffType()->stepOffset();
      int line        = lrint(userOff().y() / _spatium); //  + ((staff()->lines()-1) * 2);
      int lineOffset  = 0;

      int lines = staff() ? staff()->lines() : 5;
      if (segment() && measure() && measure()->mstaff(staffIdx())->hasVoices) {
            // move rests in a multi voice context
            bool up = (voice() == 0) || (voice() == 2);       // TODO: use style values
            switch(durationType().type()) {
                  case TDuration::V_LONG:
                        lineOffset = up ? -3 : 5;
                        break;
                  case TDuration::V_BREVE:
                        lineOffset = up ? -3 : 5;
                        break;
                  case TDuration::V_MEASURE:
                        if (duration() >= Fraction(2, 1))    // breve symbol
                              lineOffset = up ? -3 : 5;
                        // fall through
                  case TDuration::V_WHOLE:
                        lineOffset = up ? -4 : 6;
                        break;
                  case TDuration::V_HALF:
                        lineOffset = up ? -4 : 4;
                        break;
                  case TDuration::V_QUARTER:
                        lineOffset = up ? -4 : 4;
                        break;
                  case TDuration::V_EIGHT:
                        lineOffset = up ? -4 : 4;
                        break;
                  case TDuration::V_16TH:
                        lineOffset = up ? -6 : 4;
                        break;
                  case TDuration::V_32ND:
                        lineOffset = up ? -6 : 6;
                        break;
                  case TDuration::V_64TH:
                        lineOffset = up ? -8 : 6;
                        break;
                  case TDuration::V_128TH:
                        lineOffset = up ? -8 : 8;
                        break;
                  case TDuration::V_256TH:             // not available
                        lineOffset = up ? -10 : 6;
                        break;
                  default:
                        break;
                  }
            }
      else {
            switch(durationType().type()) {
                  case TDuration::V_LONG:
                  case TDuration::V_BREVE:
                  case TDuration::V_MEASURE:
                  case TDuration::V_WHOLE:
                        if (lines == 1)
                              lineOffset = -2;
                        break;
                  case TDuration::V_HALF:
                  case TDuration::V_QUARTER:
                  case TDuration::V_EIGHT:
                  case TDuration::V_16TH:
                  case TDuration::V_32ND:
                  case TDuration::V_64TH:
                  case TDuration::V_128TH:
                  case TDuration::V_256TH:             // not available
                        if (lines == 1)
                              lineOffset = -4;
                        break;
                  default:
                        break;
                  }
            }

      int yo;
      _sym = getSymbol(durationType().type(), line + lineOffset/2, lines, &yo);
      layoutArticulations();
      rypos() = (qreal(yo) + qreal(lineOffset + stepOffset) * .5) * _spatium;

      Spatium rs;
      if (dots()) {
            rs = Spatium(score()->styleS(ST_dotNoteDistance)
               + dots() * score()->styleS(ST_dotDotDistance));
            }
      Segment* s = segment();
      if (s && s->measure() && s->measure()->multiMeasure()) {
            qreal _spatium = spatium();
            qreal h = _spatium * 6.5;
            qreal w = point(score()->styleS(ST_minMMRestWidth));
            setbbox(QRectF(-w * .5, -h + 2 * _spatium, w, h));
            }
      else {
            if (dots()) {
                  rs = Spatium(score()->styleS(ST_dotNoteDistance)
                     + dots() * score()->styleS(ST_dotDotDistance));
                  }
            setbbox(symbols[score()->symIdx()][_sym].bbox(magS()));
            }
      _space.setLw(0.0);
      _space.setRw(width() + point(rs));
      }
Пример #30
0
double sys_timer_t::measure_reset()
{
    measure();
    reset();
    return elapsed;
}