<commit_before>/***************************************************************************
 *   Copyright (C) 2006 by FThauer FHammer   *
 *   f.thauer@web.de   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "startsplash.h"

#include "configfile.h"
#include "mainwindowimpl.h"

StartSplash::StartSplash(mainWindowImpl *parent, ConfigFile *c)
 : QSplashScreen(parent), myW(parent), myConfig(c)
{

	myAppDataPath = QString::fromUtf8(myConfig->readConfigString("AppDataDir").c_str());

	QFontDatabase::addApplicationFont (myAppDataPath +"fonts/c059013l.pfb");
	QFontDatabase::addApplicationFont (myAppDataPath +"fonts/andybold.ttf");

	logo = myAppDataPath+"gfx/gui/misc/logoChip3D.png";

	frameNo = 52;
	opacityCounter = 13;
	opacity = 1.0;

	QTimer *timer = new QTimer;
	connect(timer, SIGNAL(timeout()), this, SLOT(nextAnimationFrame()));
	timer->start(40);
}

StartSplash::~StartSplash()
{
}

void StartSplash::nextAnimationFrame() {

	++frameNo;
     	update();
 }

void StartSplash::paintEvent(QPaintEvent * /*event*/) {

	QPainter painter(this);

#ifdef _WIN32
        QFont welcomeFont;
        welcomeFont.setFamily("Times New Roman");
        welcomeFont.setPixelSize(42);
#else
#ifdef __APPLE__
        QFont welcomeFont;
        welcomeFont.setFamily("Andy MT Bold");
        welcomeFont.setPixelSize(42);
#else
        QFont welcomeFont;
        welcomeFont.setFamily("Century Schoolbook L");
        welcomeFont.setPixelSize(35);
#endif
#endif
#ifdef __APPLE__
        QFont haveFont;
        haveFont.setFamily("Andy MT Bold");
        haveFont.setPixelSize(36);
#else
        QFont haveFont;
        haveFont.setFamily("Andy MT");
        haveFont.setPixelSize(36);
#endif

	QFont versionFont;
	versionFont.setFamily("Nimbus Sans L");
	versionFont.setPixelSize(12);

	if(frameNo >= 52 && frameNo < 55) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);
	}

	if(frameNo >= 55 && frameNo < 57) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"W");

	}

	if(frameNo >= 57 && frameNo < 59) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"We");

	}

	if(frameNo >= 59 && frameNo < 61) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Wel");

	}

	if(frameNo >= 61 && frameNo < 63) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welc");

	}

	if(frameNo >= 63 && frameNo < 65) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welco");

	}

	if(frameNo >= 65 && frameNo < 67) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcom");

	}

	if(frameNo >= 67 && frameNo < 72) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome");

	}

	if(frameNo >= 72 && frameNo < 74) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome t");

	}

	if(frameNo >= 74 && frameNo < 79 ) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

	}

	if(frameNo >= 79 && frameNo < 91) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		--opacityCounter;
// 			std::cout << opacity << " " << opacityCounter << "\n";
		opacity = 1.0/opacityCounter;
// 		std::cout << opacity << "\n";
		painter.setOpacity(opacity);
		painter.drawPixmap(150,85,100,100,logo);

	}

	if(frameNo >= 91 && frameNo < 106) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		painter.drawPixmap(150,85,100,100,logo);

	}

	if(frameNo >= 106 && frameNo < 136) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		painter.drawPixmap(150,85,100,100,logo);

		painter.setFont(haveFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(3,190,390,40,4,"Have a lot of fun!");

	}

	if(frameNo >= 136 && frameNo < 190) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		painter.drawPixmap(150,85,100,100,logo);

		painter.setFont(haveFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(3,190,390,40,4,"Have a lot of fun! ;-)");

	}

	//even draw version number
	painter.setOpacity(1.0);
	painter.setFont(versionFont);
	painter.setPen(QColor(102,153,0));
	painter.drawText(295,230,100,20,4,"Version 0.6-beta");
	painter.setOpacity(opacity);

	if(frameNo >=190) {
		this->close();
// 		myW->label_logo->show();

	}
}

<commit_msg>font fixes<commit_after>/***************************************************************************
 *   Copyright (C) 2006 by FThauer FHammer   *
 *   f.thauer@web.de   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "startsplash.h"

#include "configfile.h"
#include "mainwindowimpl.h"

StartSplash::StartSplash(mainWindowImpl *parent, ConfigFile *c)
 : QSplashScreen(parent), myW(parent), myConfig(c)
{

	myAppDataPath = QString::fromUtf8(myConfig->readConfigString("AppDataDir").c_str());

	QFontDatabase::addApplicationFont (myAppDataPath +"fonts/c059013l.pfb");

	logo = myAppDataPath+"gfx/gui/misc/logoChip3D.png";

	frameNo = 52;
	opacityCounter = 13;
	opacity = 1.0;

	QTimer *timer = new QTimer;
	connect(timer, SIGNAL(timeout()), this, SLOT(nextAnimationFrame()));
	timer->start(40);
}

StartSplash::~StartSplash()
{
}

void StartSplash::nextAnimationFrame() {

	++frameNo;
     	update();
 }

void StartSplash::paintEvent(QPaintEvent * /*event*/) {

	QPainter painter(this);

#ifdef _WIN32
        QFont welcomeFont;
        welcomeFont.setFamily("Times New Roman");
        welcomeFont.setPixelSize(42);
#else
#ifdef __APPLE__
        QFont welcomeFont;
        welcomeFont.setFamily("Century Schoolbook L");
        welcomeFont.setPixelSize(42);
#else
        QFont welcomeFont;
        welcomeFont.setFamily("Century Schoolbook L");
        welcomeFont.setPixelSize(35);
#endif
#endif
#ifdef __APPLE__
        QFont haveFont;
        haveFont.setFamily("Century Schoolbook L");
        haveFont.setPixelSize(24);
#else
        QFont haveFont;
        haveFont.setFamily("Century Schoolbook L");
        haveFont.setPixelSize(24);
#endif

	QFont versionFont;
	versionFont.setFamily("Nimbus Sans L");
	versionFont.setPixelSize(12);

	if(frameNo >= 52 && frameNo < 55) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);
	}

	if(frameNo >= 55 && frameNo < 57) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"W");

	}

	if(frameNo >= 57 && frameNo < 59) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"We");

	}

	if(frameNo >= 59 && frameNo < 61) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Wel");

	}

	if(frameNo >= 61 && frameNo < 63) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welc");

	}

	if(frameNo >= 63 && frameNo < 65) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welco");

	}

	if(frameNo >= 65 && frameNo < 67) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcom");

	}

	if(frameNo >= 67 && frameNo < 72) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome");

	}

	if(frameNo >= 72 && frameNo < 74) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome t");

	}

	if(frameNo >= 74 && frameNo < 79 ) {

		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

	}

	if(frameNo >= 79 && frameNo < 91) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		--opacityCounter;
// 			std::cout << opacity << " " << opacityCounter << "\n";
		opacity = 1.0/opacityCounter;
// 		std::cout << opacity << "\n";
		painter.setOpacity(opacity);
		painter.drawPixmap(150,85,100,100,logo);

	}

	if(frameNo >= 91 && frameNo < 106) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		painter.drawPixmap(150,85,100,100,logo);

	}

	if(frameNo >= 106 && frameNo < 136) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		painter.drawPixmap(150,85,100,100,logo);

		painter.setFont(haveFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(3,190,390,40,4,"Have a lot of fun!");

	}

	if(frameNo >= 136 && frameNo < 190) {
//
		painter.setBrush(QColor(35,71,0));
		painter.drawRect(0,0,399,249);

		painter.setRenderHint(QPainter::Antialiasing, TRUE);
		painter.setFont(welcomeFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(48,27,300,45,4,"Welcome to");

		painter.drawPixmap(150,85,100,100,logo);

		painter.setFont(haveFont);
		painter.setPen(QColor(255,255,255));
		painter.drawText(3,190,390,40,4,"Have a lot of fun! ;-)");

	}

	//even draw version number
	painter.setOpacity(1.0);
	painter.setFont(versionFont);
	painter.setPen(QColor(102,153,0));
	painter.drawText(295,230,100,20,4,"Version 0.6-beta");
	painter.setOpacity(opacity);

	if(frameNo >=190) {
		this->close();
// 		myW->label_logo->show();

	}
}

<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>

#include "imu_filter_madgwick/imu_filter.h"
#include "imu_filter_madgwick/stateless_orientation.h"
#include "imu_filter_madgwick/ImuFilterMadgwickConfig.h"
#include "geometry_msgs/TransformStamped.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf2_msgs/TFMessage.h>

using namespace std;

// NOTE: adapted from http://wiki.ros.org/rosbag/Cookbook

/**
 * Inherits from message_filters::SimpleFilter<M>
 * to use protected signalMessage function
 */
template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
	  message_filters::SimpleFilter<M>::signalMessage(msg);
  }
};

class ImuFilterRosbag
{
  typedef sensor_msgs::Imu              ImuMsg;
  typedef sensor_msgs::MagneticField    MagMsg;
  typedef BagSubscriber<ImuMsg> 		ImuSubscriber;
  typedef BagSubscriber<MagMsg> 		MagSubscriber;
  typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> SyncPolicy;
  typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
  typedef imu_filter_madgwick::ImuFilterMadgwickConfig   FilterConfig;

  public:

  ImuFilterRosbag(rosbag::Bag* bag, const std::string& output_imu_topic, const std::string& world_frame,
		  	      const bool& stateless = false, const bool& publish_tf = false, const bool& reverse_tf = false,
				  const std::string& imu_frame = "imu_link", const std::string& fixed_frame = "base_link"){

	  	bag_ = bag;
	  	nb_msg_generated_ = 0;
	  	nb_tf_msg_generated_ = 0;
	  	stateless_ = stateless;
	  	output_imu_topic_ = output_imu_topic;
	  	publish_tf_ = publish_tf;
	  	reverse_tf_ = reverse_tf;
	  	imu_frame_ = imu_frame;
	  	fixed_frame_ = fixed_frame;

		// Set up fake subscribers to capture Imu and MagneticField messages
	  	imu_sub_.reset(new ImuSubscriber());
	  	mag_sub_.reset(new MagSubscriber());

	  	sync_.reset(new Synchronizer(
		  SyncPolicy(50), *imu_sub_, *mag_sub_));
		sync_->registerCallback(boost::bind(&ImuFilterRosbag::imuMagCallback, this, _1, _2));

		 if (world_frame == "ned") {
			world_frame_ = WorldFrame::NED;
		  } else if (world_frame == "nwu"){
			world_frame_ = WorldFrame::NWU;
		  } else if (world_frame == "enu"){
			world_frame_ = WorldFrame::ENU;
		  } else {
			//ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
			//ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
			world_frame_ = WorldFrame::ENU;
		  }
		  filter_.setWorldFrame(world_frame_);
    }

    virtual ~ImuFilterRosbag(){
    	// TODO: fix the following error at termination:
    	// terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >'
    	//  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
    	// Aborted
    	//
    	// see: https://github.com/ros/ros_comm/issues/318
    	//      http://answers.ros.org/question/143756/rostimer-leads-to-boostlock_error-at-process-cleanup/
    	imu_sub_.reset();
    	mag_sub_.reset();
    	sync_.reset();
    }

    void addImuMessage(sensor_msgs::Imu::ConstPtr imu){
		imu_sub_->newMessage(imu);
	}

	void addMagMessage(sensor_msgs::MagneticField::ConstPtr mag){
		mag_sub_->newMessage(mag);
	}

  private:

    rosbag::Bag* bag_;
    std::string output_imu_topic_;

	boost::shared_ptr<Synchronizer> sync_;
	boost::shared_ptr<ImuSubscriber> imu_sub_;
	boost::shared_ptr<MagSubscriber> mag_sub_;

    // **** paramaters
    WorldFrame::WorldFrame world_frame_;
    bool stateless_;
    geometry_msgs::Vector3 mag_bias_;
    double orientation_variance_;

    // **** state variables
    bool initialized_;
    ros::Time last_time_;
    int nb_msg_generated_;
    int nb_tf_msg_generated_;
    bool publish_tf_;
    bool reverse_tf_;
    std::string imu_frame_;
    std::string fixed_frame_;

    // **** filter implementation
    ImuFilter filter_;

    void reconfigure(FilterConfig& config){
      double gain, zeta;
      gain = config.gain;
      zeta = config.zeta;
      filter_.setAlgorithmGain(gain);
      filter_.setDriftBiasGain(zeta);
      printf("Imu filter gain set to %f", gain);
      printf("Gyro drift bias set to %f", zeta);
      mag_bias_.x = config.mag_bias_x;
      mag_bias_.y = config.mag_bias_y;
      mag_bias_.z = config.mag_bias_z;
      orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
      printf("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
    }

    // **** member functions
    void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
                        const MagMsg::ConstPtr& mag_msg){

	  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
	  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
	  const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;

	  ros::Time time = imu_msg_raw->header.stamp;

	  /*** Compensate for hard iron ***/
	  geometry_msgs::Vector3 mag_compensated;
	  mag_compensated.x = mag_fld.x - mag_bias_.x;
	  mag_compensated.y = mag_fld.y - mag_bias_.y;
	  mag_compensated.z = mag_fld.z - mag_bias_.z;

	  double roll = 0.0;
	  double pitch = 0.0;
	  double yaw = 0.0;

	  if (!initialized_ || stateless_)
	  {
		// wait for mag message without NaN / inf
		if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
		{
		  return;
		}

		geometry_msgs::Quaternion init_q;
		StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q);
		filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);

		last_time_ = time;
		initialized_ = true;
	  }

	  float dt = (time - last_time_).toSec();

	  last_time_ = time;

	  if (!stateless_)
		filter_.madgwickAHRSupdate(
		  ang_vel.x, ang_vel.y, ang_vel.z,
		  lin_acc.x, lin_acc.y, lin_acc.z,
		  mag_compensated.x, mag_compensated.y, mag_compensated.z,
		  dt);

	  publishFilteredMsg(imu_msg_raw);
	  if (publish_tf_)
	      publishTransform(imu_msg_raw);
    }

    void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw){

    	  double q0,q1,q2,q3;
    	  filter_.getOrientation(q0,q1,q2,q3);

    	  // create and publish filtered IMU message
    	  boost::shared_ptr<ImuMsg> imu_msg =
    	    boost::make_shared<ImuMsg>(*imu_msg_raw);

    	  imu_msg->orientation.w = q0;
    	  imu_msg->orientation.x = q1;
    	  imu_msg->orientation.y = q2;
    	  imu_msg->orientation.z = q3;

    	  imu_msg->orientation_covariance[0] = orientation_variance_;
    	  imu_msg->orientation_covariance[1] = 0.0;
    	  imu_msg->orientation_covariance[2] = 0.0;
    	  imu_msg->orientation_covariance[3] = 0.0;
    	  imu_msg->orientation_covariance[4] = orientation_variance_;
    	  imu_msg->orientation_covariance[5] = 0.0;
    	  imu_msg->orientation_covariance[6] = 0.0;
    	  imu_msg->orientation_covariance[7] = 0.0;
    	  imu_msg->orientation_covariance[8] = orientation_variance_;

    	  // Write msg in rosbag here
    	  bag_->write(output_imu_topic_, imu_msg->header.stamp, imu_msg);
    	  nb_msg_generated_++;

    	  if (nb_msg_generated_ % 1000 == 0){
    		  printf("Number of filtered imu messages generated: %d \n", nb_msg_generated_);
    	  }
    }

    void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw){

      double q0,q1,q2,q3;
      filter_.getOrientation(q0,q1,q2,q3);
      geometry_msgs::TransformStamped transform;
      transform.header.stamp = imu_msg_raw->header.stamp;
      if (reverse_tf_)
      {
        transform.header.frame_id = imu_frame_;
        transform.child_frame_id = fixed_frame_;
        transform.transform.rotation.w = q0;
        transform.transform.rotation.x = -q1;
        transform.transform.rotation.y = -q2;
        transform.transform.rotation.z = -q3;
      }
      else {
        transform.header.frame_id = fixed_frame_;
        transform.child_frame_id = imu_frame_;
        transform.transform.rotation.w = q0;
        transform.transform.rotation.x = q1;
        transform.transform.rotation.y = q2;
        transform.transform.rotation.z = q3;
      }

		// Write tf message to rosbag
		//tf::tfMessage tf_msg;
        tf2_msgs::TFMessage tf_msg;
      	tf_msg.transforms.push_back(transform);
		bag_->write("/tf", transform.header.stamp, tf_msg);
		nb_tf_msg_generated_++;

		if (nb_tf_msg_generated_ % 1000 == 0){
			  printf("Number of tf messages generated: %d \n", nb_tf_msg_generated_);
		}
    }

};

int main(int argc, char **argv){

	ros::Time::init();

	std::string input_rosbag = "input.bag";
	std::string output_rosbag = "output.bag";
	std::string output_imu_topic = "/imu/data";
	std::string input_imu_topic = "/imu/data_raw";
	std::string input_mag_topic = "/imu/mag";
	std::string world_frame = "nwu";
	std::string imu_frame = "imu_link";
	std::string fixed_frame = "base_link";
	bool stateless = false;
	bool publish_tf = false;
	bool reverse_tf = false;

	namespace po = boost::program_options;
	po::options_description desc("Allowed options");
	desc.add_options()
	("help,h", "describe arguments")
	("output,o", po::value(&output_rosbag), "set output rosbag file")
	("input,i", po::value(&input_rosbag), "set input rosbag file")
	("output-imu-topic,d", po::value(&output_imu_topic), "set topic of the output Imu messages")
	("input-imu-topic,m", po::value(&input_imu_topic), "set topic of the input Imu messages")
	("input-mag-topic,g", po::value(&input_mag_topic), "set topic of the input MagneticField messages")
	("world-frame,w", po::value(&world_frame), "set the world frame")
	("stateless,s", po::bool_switch(&stateless), "set topic of the input Imu messages")
	("publish-tf,t", po::bool_switch(&publish_tf), "set to publish tf messages")
	("reverse-tf,r", po::bool_switch(&reverse_tf), "set to reverse tf messages")
	("imu-frame,f", po::value(&imu_frame), "set the name of the imu frame")
	("fixed-frame,x", po::value(&fixed_frame), "set the name of the fixed frame");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	rosbag::Bag output(output_rosbag, rosbag::bagmode::Write);
	rosbag::Bag input(input_rosbag, rosbag::bagmode::Read);

	ImuFilterRosbag filter(&output, output_imu_topic, world_frame, stateless, publish_tf, reverse_tf, imu_frame, fixed_frame);

	int nb_imu_msg_processed = 0;
	int nb_mag_msg_processed = 0;
	int nb_total_msg_processed = 0;
	rosbag::View view(input);
	BOOST_FOREACH(rosbag::MessageInstance const m, view)
	{
		// Detect Imu messages from the given topic
		if (m.getTopic() == input_imu_topic || ("/" + m.getTopic() == input_imu_topic))
		{
		  sensor_msgs::Imu::ConstPtr imu = m.instantiate<sensor_msgs::Imu>();
		  if (imu != NULL){
			  filter.addImuMessage(imu);
			  nb_imu_msg_processed++;
		  }
		}

		// Detect MagneticField messages from the given topic
		if (m.getTopic() == input_mag_topic || ("/" + m.getTopic() == input_mag_topic))
		{
		  sensor_msgs::MagneticField::ConstPtr mag = m.instantiate<sensor_msgs::MagneticField>();
		  if (mag != NULL){
			  filter.addMagMessage(mag);
			  nb_mag_msg_processed++;
		  }
		}

		if (m.getTopic() != output_imu_topic){
			// Write every message to output bag
			output.write(m.getTopic(), m.getTime(), m, m.getConnectionHeader());
			nb_total_msg_processed++;
		}

		if (nb_total_msg_processed % 1000 == 0){
			printf("Number of imu messages processed: %d (total %d)\n", nb_imu_msg_processed, nb_total_msg_processed);
			printf("Number of mag messages processed: %d (total %d)\n", nb_mag_msg_processed, nb_total_msg_processed);
		}
	}

	output.close();
	input.close();

	return 0;
}
<commit_msg>Increased gain from 0 to 0.1 in offline mode<commit_after>#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>

#include "imu_filter_madgwick/imu_filter.h"
#include "imu_filter_madgwick/stateless_orientation.h"
#include "imu_filter_madgwick/ImuFilterMadgwickConfig.h"
#include "geometry_msgs/TransformStamped.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf2_msgs/TFMessage.h>

using namespace std;

// NOTE: adapted from http://wiki.ros.org/rosbag/Cookbook

/**
 * Inherits from message_filters::SimpleFilter<M>
 * to use protected signalMessage function
 */
template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
	  message_filters::SimpleFilter<M>::signalMessage(msg);
  }
};

class ImuFilterRosbag
{
  typedef sensor_msgs::Imu              ImuMsg;
  typedef sensor_msgs::MagneticField    MagMsg;
  typedef BagSubscriber<ImuMsg> 		ImuSubscriber;
  typedef BagSubscriber<MagMsg> 		MagSubscriber;
  typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> SyncPolicy;
  typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
  typedef imu_filter_madgwick::ImuFilterMadgwickConfig   FilterConfig;

  public:

  ImuFilterRosbag(rosbag::Bag* bag, const std::string& output_imu_topic, const std::string& world_frame,
		  	      const bool& stateless = false, const bool& publish_tf = false, const bool& reverse_tf = false,
				  const std::string& imu_frame = "imu_link", const std::string& fixed_frame = "base_link"){

	  	bag_ = bag;
	  	nb_msg_generated_ = 0;
	  	nb_tf_msg_generated_ = 0;
	  	stateless_ = stateless;
	  	output_imu_topic_ = output_imu_topic;
	  	publish_tf_ = publish_tf;
	  	reverse_tf_ = reverse_tf;
	  	imu_frame_ = imu_frame;
	  	fixed_frame_ = fixed_frame;

		// Set up fake subscribers to capture Imu and MagneticField messages
	  	imu_sub_.reset(new ImuSubscriber());
	  	mag_sub_.reset(new MagSubscriber());

	  	sync_.reset(new Synchronizer(
		  SyncPolicy(50), *imu_sub_, *mag_sub_));
		sync_->registerCallback(boost::bind(&ImuFilterRosbag::imuMagCallback, this, _1, _2));

		 if (world_frame == "ned") {
			world_frame_ = WorldFrame::NED;
		  } else if (world_frame == "nwu"){
			world_frame_ = WorldFrame::NWU;
		  } else if (world_frame == "enu"){
			world_frame_ = WorldFrame::ENU;
		  } else {
			//ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
			//ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
			world_frame_ = WorldFrame::ENU;
		  }
		  filter_.setWorldFrame(world_frame_);

      filter_.setAlgorithmGain(0.1);
    }

    virtual ~ImuFilterRosbag(){
    	// TODO: fix the following error at termination:
    	// terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >'
    	//  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
    	// Aborted
    	//
    	// see: https://github.com/ros/ros_comm/issues/318
    	//      http://answers.ros.org/question/143756/rostimer-leads-to-boostlock_error-at-process-cleanup/
    	imu_sub_.reset();
    	mag_sub_.reset();
    	sync_.reset();
    }

    void addImuMessage(sensor_msgs::Imu::ConstPtr imu){
		imu_sub_->newMessage(imu);
	}

	void addMagMessage(sensor_msgs::MagneticField::ConstPtr mag){
		mag_sub_->newMessage(mag);
	}

  private:

    rosbag::Bag* bag_;
    std::string output_imu_topic_;

	boost::shared_ptr<Synchronizer> sync_;
	boost::shared_ptr<ImuSubscriber> imu_sub_;
	boost::shared_ptr<MagSubscriber> mag_sub_;

    // **** paramaters
    WorldFrame::WorldFrame world_frame_;
    bool stateless_;
    geometry_msgs::Vector3 mag_bias_;
    double orientation_variance_;

    // **** state variables
    bool initialized_;
    ros::Time last_time_;
    int nb_msg_generated_;
    int nb_tf_msg_generated_;
    bool publish_tf_;
    bool reverse_tf_;
    std::string imu_frame_;
    std::string fixed_frame_;

    // **** filter implementation
    ImuFilter filter_;

    void reconfigure(FilterConfig& config){
      double gain, zeta;
      gain = config.gain;
      zeta = config.zeta;
      filter_.setAlgorithmGain(gain);
      filter_.setDriftBiasGain(zeta);
      printf("Imu filter gain set to %f", gain);
      printf("Gyro drift bias set to %f", zeta);
      mag_bias_.x = config.mag_bias_x;
      mag_bias_.y = config.mag_bias_y;
      mag_bias_.z = config.mag_bias_z;
      orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
      printf("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
    }

    // **** member functions
    void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
                        const MagMsg::ConstPtr& mag_msg){

	  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
	  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
	  const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;

	  ros::Time time = imu_msg_raw->header.stamp;

	  /*** Compensate for hard iron ***/
	  geometry_msgs::Vector3 mag_compensated;
	  mag_compensated.x = mag_fld.x - mag_bias_.x;
	  mag_compensated.y = mag_fld.y - mag_bias_.y;
	  mag_compensated.z = mag_fld.z - mag_bias_.z;

	  double roll = 0.0;
	  double pitch = 0.0;
	  double yaw = 0.0;

	  if (!initialized_ || stateless_)
	  {
		// wait for mag message without NaN / inf
		if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
		{
		  return;
		}

		geometry_msgs::Quaternion init_q;
		StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q);
		filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);

		last_time_ = time;
		initialized_ = true;
	  }

	  float dt = (time - last_time_).toSec();

	  last_time_ = time;

	  if (!stateless_)
		filter_.madgwickAHRSupdate(
		  ang_vel.x, ang_vel.y, ang_vel.z,
		  lin_acc.x, lin_acc.y, lin_acc.z,
		  mag_compensated.x, mag_compensated.y, mag_compensated.z,
		  dt);

	  publishFilteredMsg(imu_msg_raw);
	  if (publish_tf_)
	      publishTransform(imu_msg_raw);
    }

    void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw){

    	  double q0,q1,q2,q3;
    	  filter_.getOrientation(q0,q1,q2,q3);

    	  // create and publish filtered IMU message
    	  boost::shared_ptr<ImuMsg> imu_msg =
    	    boost::make_shared<ImuMsg>(*imu_msg_raw);

    	  imu_msg->orientation.w = q0;
    	  imu_msg->orientation.x = q1;
    	  imu_msg->orientation.y = q2;
    	  imu_msg->orientation.z = q3;

    	  imu_msg->orientation_covariance[0] = orientation_variance_;
    	  imu_msg->orientation_covariance[1] = 0.0;
    	  imu_msg->orientation_covariance[2] = 0.0;
    	  imu_msg->orientation_covariance[3] = 0.0;
    	  imu_msg->orientation_covariance[4] = orientation_variance_;
    	  imu_msg->orientation_covariance[5] = 0.0;
    	  imu_msg->orientation_covariance[6] = 0.0;
    	  imu_msg->orientation_covariance[7] = 0.0;
    	  imu_msg->orientation_covariance[8] = orientation_variance_;

    	  // Write msg in rosbag here
    	  bag_->write(output_imu_topic_, imu_msg->header.stamp, imu_msg);
    	  nb_msg_generated_++;

    	  if (nb_msg_generated_ % 1000 == 0){
    		  printf("Number of filtered imu messages generated: %d \n", nb_msg_generated_);
    	  }
    }

    void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw){

      double q0,q1,q2,q3;
      filter_.getOrientation(q0,q1,q2,q3);
      geometry_msgs::TransformStamped transform;
      transform.header.stamp = imu_msg_raw->header.stamp;
      if (reverse_tf_)
      {
        transform.header.frame_id = imu_frame_;
        transform.child_frame_id = fixed_frame_;
        transform.transform.rotation.w = q0;
        transform.transform.rotation.x = -q1;
        transform.transform.rotation.y = -q2;
        transform.transform.rotation.z = -q3;
      }
      else {
        transform.header.frame_id = fixed_frame_;
        transform.child_frame_id = imu_frame_;
        transform.transform.rotation.w = q0;
        transform.transform.rotation.x = q1;
        transform.transform.rotation.y = q2;
        transform.transform.rotation.z = q3;
      }

		// Write tf message to rosbag
		//tf::tfMessage tf_msg;
        tf2_msgs::TFMessage tf_msg;
      	tf_msg.transforms.push_back(transform);
		bag_->write("/tf", transform.header.stamp, tf_msg);
		nb_tf_msg_generated_++;

		if (nb_tf_msg_generated_ % 1000 == 0){
			  printf("Number of tf messages generated: %d \n", nb_tf_msg_generated_);
		}
    }

};

int main(int argc, char **argv){

	ros::Time::init();

	std::string input_rosbag = "input.bag";
	std::string output_rosbag = "output.bag";
	std::string output_imu_topic = "/imu/data";
	std::string input_imu_topic = "/imu/data_raw";
	std::string input_mag_topic = "/imu/mag";
	std::string world_frame = "nwu";
	std::string imu_frame = "imu_link";
	std::string fixed_frame = "base_link";
	bool stateless = false;
	bool publish_tf = false;
	bool reverse_tf = false;

	namespace po = boost::program_options;
	po::options_description desc("Allowed options");
	desc.add_options()
	("help,h", "describe arguments")
	("output,o", po::value(&output_rosbag), "set output rosbag file")
	("input,i", po::value(&input_rosbag), "set input rosbag file")
	("output-imu-topic,d", po::value(&output_imu_topic), "set topic of the output Imu messages")
	("input-imu-topic,m", po::value(&input_imu_topic), "set topic of the input Imu messages")
	("input-mag-topic,g", po::value(&input_mag_topic), "set topic of the input MagneticField messages")
	("world-frame,w", po::value(&world_frame), "set the world frame")
	("stateless,s", po::bool_switch(&stateless), "set topic of the input Imu messages")
	("publish-tf,t", po::bool_switch(&publish_tf), "set to publish tf messages")
	("reverse-tf,r", po::bool_switch(&reverse_tf), "set to reverse tf messages")
	("imu-frame,f", po::value(&imu_frame), "set the name of the imu frame")
	("fixed-frame,x", po::value(&fixed_frame), "set the name of the fixed frame");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	rosbag::Bag output(output_rosbag, rosbag::bagmode::Write);
	rosbag::Bag input(input_rosbag, rosbag::bagmode::Read);

	ImuFilterRosbag filter(&output, output_imu_topic, world_frame, stateless, publish_tf, reverse_tf, imu_frame, fixed_frame);

	int nb_imu_msg_processed = 0;
	int nb_mag_msg_processed = 0;
	int nb_total_msg_processed = 0;
	rosbag::View view(input);
	BOOST_FOREACH(rosbag::MessageInstance const m, view)
	{
		// Detect Imu messages from the given topic
		if (m.getTopic() == input_imu_topic || ("/" + m.getTopic() == input_imu_topic))
		{
		  sensor_msgs::Imu::ConstPtr imu = m.instantiate<sensor_msgs::Imu>();
		  if (imu != NULL){
			  filter.addImuMessage(imu);
			  nb_imu_msg_processed++;
		  }
		}

		// Detect MagneticField messages from the given topic
		if (m.getTopic() == input_mag_topic || ("/" + m.getTopic() == input_mag_topic))
		{
		  sensor_msgs::MagneticField::ConstPtr mag = m.instantiate<sensor_msgs::MagneticField>();
		  if (mag != NULL){
			  filter.addMagMessage(mag);
			  nb_mag_msg_processed++;
		  }
		}

		if (m.getTopic() != output_imu_topic){
			// Write every message to output bag
			output.write(m.getTopic(), m.getTime(), m, m.getConnectionHeader());
			nb_total_msg_processed++;
		}

		if (nb_total_msg_processed % 1000 == 0){
			printf("Number of imu messages processed: %d (total %d)\n", nb_imu_msg_processed, nb_total_msg_processed);
			printf("Number of mag messages processed: %d (total %d)\n", nb_mag_msg_processed, nb_total_msg_processed);
		}
	}

	output.close();
	input.close();

	return 0;
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: MasterDetailLinkDialog.cxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: kz $ $Date: 2008-03-05 17:09:07 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

 #include "precompiled_extensions.hxx"
 #include "MasterDetailLinkDialog.hxx"
 #include "formlinkdialog.hxx"

 extern "C" void SAL_CALL createRegistryInfo_MasterDetailLinkDialog()
{
    ::pcr::OAutoRegistration< ::pcr::MasterDetailLinkDialog > aAutoRegistration;
}

//............................................................................
namespace pcr
{
//............................................................................

    using namespace ::com::sun::star::uno;
    using namespace ::com::sun::star::lang;
    using namespace ::com::sun::star::beans;

    //====================================================================
    //= MasterDetailLinkDialog
    //====================================================================
    //---------------------------------------------------------------------
    MasterDetailLinkDialog::MasterDetailLinkDialog(const Reference< XComponentContext >& _rxContext )
        :OGenericUnoDialog( _rxContext )
    {
    }
//---------------------------------------------------------------------
    Sequence<sal_Int8> SAL_CALL MasterDetailLinkDialog::getImplementationId(  ) throw(RuntimeException)
    {
        static ::cppu::OImplementationId aId;
        return aId.getImplementationId();
    }

    //---------------------------------------------------------------------
    Reference< XInterface > SAL_CALL MasterDetailLinkDialog::Create( const Reference< XComponentContext >& _rxContext )
    {
        return *( new MasterDetailLinkDialog( _rxContext ) );
    }

    //---------------------------------------------------------------------
    ::rtl::OUString SAL_CALL MasterDetailLinkDialog::getImplementationName() throw(RuntimeException)
    {
        return getImplementationName_static();
    }

    //---------------------------------------------------------------------
    ::rtl::OUString MasterDetailLinkDialog::getImplementationName_static() throw(RuntimeException)
    {
        return ::rtl::OUString::createFromAscii("org.openoffice.comp.form.ui.MasterDetailLinkDialog");
    }

    //---------------------------------------------------------------------
    ::comphelper::StringSequence SAL_CALL MasterDetailLinkDialog::getSupportedServiceNames() throw(RuntimeException)
    {
        return getSupportedServiceNames_static();
    }

    //---------------------------------------------------------------------
    ::comphelper::StringSequence MasterDetailLinkDialog::getSupportedServiceNames_static() throw(RuntimeException)
    {
        ::comphelper::StringSequence aSupported(1);
        aSupported.getArray()[0] = ::rtl::OUString::createFromAscii("com.sun.star.form.MasterDetailLinkDialog");
        return aSupported;
    }

    //---------------------------------------------------------------------
    Reference<XPropertySetInfo>  SAL_CALL MasterDetailLinkDialog::getPropertySetInfo() throw(RuntimeException)
    {
        Reference<XPropertySetInfo>  xInfo( createPropertySetInfo( getInfoHelper() ) );
        return xInfo;
    }

    //---------------------------------------------------------------------
    ::cppu::IPropertyArrayHelper& MasterDetailLinkDialog::getInfoHelper()
    {
        return *const_cast<MasterDetailLinkDialog*>(this)->getArrayHelper();
    }

    //--------------------------------------------------------------------------
    ::cppu::IPropertyArrayHelper* MasterDetailLinkDialog::createArrayHelper( ) const
    {
        Sequence< Property > aProps;
        describeProperties(aProps);
        return new ::cppu::OPropertyArrayHelper(aProps);
    }

    //--------------------------------------------------------------------------
    Dialog* MasterDetailLinkDialog::createDialog(Window* _pParent)
    {
        return new FormLinkDialog(_pParent,m_xDetail,m_xMaster,Reference< XMultiServiceFactory >( m_xContext->getServiceManager(),UNO_QUERY)
            ,m_sExplanation,m_sDetailLabel,m_sMasterLabel);
    }
    //---------------------------------------------------------------------
    void MasterDetailLinkDialog::implInitialize(const Any& _rValue)
    {
        PropertyValue aProperty;
        if (_rValue >>= aProperty)
        {
            if (0 == aProperty.Name.compareToAscii("Detail"))
            {
                OSL_VERIFY( aProperty.Value >>= m_xDetail );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("Master"))
            {
                OSL_VERIFY( aProperty.Value >>= m_xMaster );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("Explanation"))
            {
                OSL_VERIFY( aProperty.Value >>= m_sExplanation );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("DetailLabel"))
            {
                OSL_VERIFY( aProperty.Value >>= m_sDetailLabel );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("MasterLabel"))
            {
                OSL_VERIFY( aProperty.Value >>= m_sMasterLabel );
                return;
            }
        }
        MasterDetailLinkDialog_DBase::implInitialize(_rValue);
    }

//............................................................................
}   // namespace pcr
//............................................................................
<commit_msg>INTEGRATION: CWS changefileheader (1.2.14); FILE MERGED 2008/03/31 12:31:39 rt 1.2.14.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: MasterDetailLinkDialog.cxx,v $
 * $Revision: 1.3 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

 #include "precompiled_extensions.hxx"
 #include "MasterDetailLinkDialog.hxx"
 #include "formlinkdialog.hxx"

 extern "C" void SAL_CALL createRegistryInfo_MasterDetailLinkDialog()
{
    ::pcr::OAutoRegistration< ::pcr::MasterDetailLinkDialog > aAutoRegistration;
}

//............................................................................
namespace pcr
{
//............................................................................

    using namespace ::com::sun::star::uno;
    using namespace ::com::sun::star::lang;
    using namespace ::com::sun::star::beans;

    //====================================================================
    //= MasterDetailLinkDialog
    //====================================================================
    //---------------------------------------------------------------------
    MasterDetailLinkDialog::MasterDetailLinkDialog(const Reference< XComponentContext >& _rxContext )
        :OGenericUnoDialog( _rxContext )
    {
    }
//---------------------------------------------------------------------
    Sequence<sal_Int8> SAL_CALL MasterDetailLinkDialog::getImplementationId(  ) throw(RuntimeException)
    {
        static ::cppu::OImplementationId aId;
        return aId.getImplementationId();
    }

    //---------------------------------------------------------------------
    Reference< XInterface > SAL_CALL MasterDetailLinkDialog::Create( const Reference< XComponentContext >& _rxContext )
    {
        return *( new MasterDetailLinkDialog( _rxContext ) );
    }

    //---------------------------------------------------------------------
    ::rtl::OUString SAL_CALL MasterDetailLinkDialog::getImplementationName() throw(RuntimeException)
    {
        return getImplementationName_static();
    }

    //---------------------------------------------------------------------
    ::rtl::OUString MasterDetailLinkDialog::getImplementationName_static() throw(RuntimeException)
    {
        return ::rtl::OUString::createFromAscii("org.openoffice.comp.form.ui.MasterDetailLinkDialog");
    }

    //---------------------------------------------------------------------
    ::comphelper::StringSequence SAL_CALL MasterDetailLinkDialog::getSupportedServiceNames() throw(RuntimeException)
    {
        return getSupportedServiceNames_static();
    }

    //---------------------------------------------------------------------
    ::comphelper::StringSequence MasterDetailLinkDialog::getSupportedServiceNames_static() throw(RuntimeException)
    {
        ::comphelper::StringSequence aSupported(1);
        aSupported.getArray()[0] = ::rtl::OUString::createFromAscii("com.sun.star.form.MasterDetailLinkDialog");
        return aSupported;
    }

    //---------------------------------------------------------------------
    Reference<XPropertySetInfo>  SAL_CALL MasterDetailLinkDialog::getPropertySetInfo() throw(RuntimeException)
    {
        Reference<XPropertySetInfo>  xInfo( createPropertySetInfo( getInfoHelper() ) );
        return xInfo;
    }

    //---------------------------------------------------------------------
    ::cppu::IPropertyArrayHelper& MasterDetailLinkDialog::getInfoHelper()
    {
        return *const_cast<MasterDetailLinkDialog*>(this)->getArrayHelper();
    }

    //--------------------------------------------------------------------------
    ::cppu::IPropertyArrayHelper* MasterDetailLinkDialog::createArrayHelper( ) const
    {
        Sequence< Property > aProps;
        describeProperties(aProps);
        return new ::cppu::OPropertyArrayHelper(aProps);
    }

    //--------------------------------------------------------------------------
    Dialog* MasterDetailLinkDialog::createDialog(Window* _pParent)
    {
        return new FormLinkDialog(_pParent,m_xDetail,m_xMaster,Reference< XMultiServiceFactory >( m_xContext->getServiceManager(),UNO_QUERY)
            ,m_sExplanation,m_sDetailLabel,m_sMasterLabel);
    }
    //---------------------------------------------------------------------
    void MasterDetailLinkDialog::implInitialize(const Any& _rValue)
    {
        PropertyValue aProperty;
        if (_rValue >>= aProperty)
        {
            if (0 == aProperty.Name.compareToAscii("Detail"))
            {
                OSL_VERIFY( aProperty.Value >>= m_xDetail );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("Master"))
            {
                OSL_VERIFY( aProperty.Value >>= m_xMaster );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("Explanation"))
            {
                OSL_VERIFY( aProperty.Value >>= m_sExplanation );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("DetailLabel"))
            {
                OSL_VERIFY( aProperty.Value >>= m_sDetailLabel );
                return;
            }
            else if (0 == aProperty.Name.compareToAscii("MasterLabel"))
            {
                OSL_VERIFY( aProperty.Value >>= m_sMasterLabel );
                return;
            }
        }
        MasterDetailLinkDialog_DBase::implInitialize(_rValue);
    }

//............................................................................
}   // namespace pcr
//............................................................................
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 Cloudius Systems, Ltd.
 */

#include "murmur3_partitioner.hh"
#include "utils/murmur_hash.hh"
#include "sstables/key.hh"
#include "utils/class_registrator.hh"

namespace dht {

inline
int64_t
murmur3_partitioner::normalize(int64_t in) {
    return in == std::numeric_limits<int64_t>::lowest()
            ? std::numeric_limits<int64_t>::max()
            : in;
}

token
murmur3_partitioner::get_token(bytes_view key) {
    if (key.empty()) {
        return minimum_token();
    }
    std::array<uint64_t, 2> hash;
    utils::murmur_hash::hash3_x64_128(key, 0, hash);
    return get_token(hash[0]);
}

token
murmur3_partitioner::get_token(uint64_t value) const {
    // We don't normalize() the value, since token includes an is-before-everything
    // indicator.
    // FIXME: will this require a repair when importing a database?
    auto t = net::hton(normalize(value));
    bytes b(bytes::initialized_later(), 8);
    std::copy_n(reinterpret_cast<int8_t*>(&t), 8, b.begin());
    return token{token::kind::key, std::move(b)};
}

token
murmur3_partitioner::get_token(const sstables::key_view& key) {
    return get_token(bytes_view(key));
}

token
murmur3_partitioner::get_token(const schema& s, partition_key_view key) {
    std::array<uint64_t, 2> hash;
    auto&& legacy = key.legacy_form(s);
    utils::murmur_hash::hash3_x64_128(legacy.begin(), legacy.size(), 0, hash);
    return get_token(hash[0]);
}

token murmur3_partitioner::get_random_token() {
    auto rand = dht::get_random_number<uint64_t>();
    return get_token(rand);
}

inline long long_token(const token& t) {

    if (t._data.size() != sizeof(long)) {
        throw runtime_exception(sprint("Invalid token. Should have size %ld, has size %ld\n", sizeof(long), t._data.size()));
    }

    auto ptr = t._data.begin();
    auto lp = reinterpret_cast<const long *>(ptr);
    return net::ntoh(*lp);
}

sstring murmur3_partitioner::to_sstring(const token& t) const {
    return ::to_sstring(long_token(t));
}

bool murmur3_partitioner::is_equal(const token& t1, const token& t2) {

    auto l1 = long_token(t1);
    auto l2 = long_token(t2);

    return l1 == l2;
}

bool murmur3_partitioner::is_less(const token& t1, const token& t2) {

    auto l1 = long_token(t1);
    auto l2 = long_token(t2);

    return l1 < l2;
}

token murmur3_partitioner::midpoint(const token& t1, const token& t2) const {
    auto l1 = long_token(t1);
    auto l2 = long_token(t2);
    // long_token is defined as signed, but the arithmetic works out the same
    // without invoking undefined behavior with a signed type.
    auto delta = (uint64_t(l2) - uint64_t(l1)) / 2;
    if (l1 > l2) {
        // wraparound
        delta += 0x8000'0000'0000'0000;
    }
    auto mid = uint64_t(l1) + delta;
    return get_token(mid);
}

std::map<token, float>
murmur3_partitioner::describe_ownership(const std::vector<token>& sorted_tokens) {
    abort();
}

data_type
murmur3_partitioner::get_token_validator() {
    abort();
}

unsigned
murmur3_partitioner::shard_of(const token& t) const {
    int64_t l = long_token(t);
    // treat l as a fraction between 0 and 1 and use 128-bit arithmetic to
    // divide that range evenly among shards:
    uint64_t adjusted = uint64_t(l) + uint64_t(std::numeric_limits<int64_t>::min());
    return (__int128(adjusted) * smp::count) >> 64;
}

using registry = class_registrator<i_partitioner, murmur3_partitioner>;
static registry registrator("org.apache.cassandra.dht.Murmur3Partitioner");
static registry registrator_short_name("Murmur3Partitioner");

}

<commit_msg>dht: fix load of misaligned address<commit_after>/*
 * Copyright (C) 2015 Cloudius Systems, Ltd.
 */

#include "murmur3_partitioner.hh"
#include "utils/murmur_hash.hh"
#include "sstables/key.hh"
#include "utils/class_registrator.hh"

namespace dht {

inline
int64_t
murmur3_partitioner::normalize(int64_t in) {
    return in == std::numeric_limits<int64_t>::lowest()
            ? std::numeric_limits<int64_t>::max()
            : in;
}

token
murmur3_partitioner::get_token(bytes_view key) {
    if (key.empty()) {
        return minimum_token();
    }
    std::array<uint64_t, 2> hash;
    utils::murmur_hash::hash3_x64_128(key, 0, hash);
    return get_token(hash[0]);
}

token
murmur3_partitioner::get_token(uint64_t value) const {
    // We don't normalize() the value, since token includes an is-before-everything
    // indicator.
    // FIXME: will this require a repair when importing a database?
    auto t = net::hton(normalize(value));
    bytes b(bytes::initialized_later(), 8);
    std::copy_n(reinterpret_cast<int8_t*>(&t), 8, b.begin());
    return token{token::kind::key, std::move(b)};
}

token
murmur3_partitioner::get_token(const sstables::key_view& key) {
    return get_token(bytes_view(key));
}

token
murmur3_partitioner::get_token(const schema& s, partition_key_view key) {
    std::array<uint64_t, 2> hash;
    auto&& legacy = key.legacy_form(s);
    utils::murmur_hash::hash3_x64_128(legacy.begin(), legacy.size(), 0, hash);
    return get_token(hash[0]);
}

token murmur3_partitioner::get_random_token() {
    auto rand = dht::get_random_number<uint64_t>();
    return get_token(rand);
}

inline long long_token(const token& t) {

    if (t._data.size() != sizeof(long)) {
        throw runtime_exception(sprint("Invalid token. Should have size %ld, has size %ld\n", sizeof(long), t._data.size()));
    }

    auto ptr = t._data.begin();
    auto lp = unaligned_cast<const long *>(ptr);
    return net::ntoh(*lp);
}

sstring murmur3_partitioner::to_sstring(const token& t) const {
    return ::to_sstring(long_token(t));
}

bool murmur3_partitioner::is_equal(const token& t1, const token& t2) {

    auto l1 = long_token(t1);
    auto l2 = long_token(t2);

    return l1 == l2;
}

bool murmur3_partitioner::is_less(const token& t1, const token& t2) {

    auto l1 = long_token(t1);
    auto l2 = long_token(t2);

    return l1 < l2;
}

token murmur3_partitioner::midpoint(const token& t1, const token& t2) const {
    auto l1 = long_token(t1);
    auto l2 = long_token(t2);
    // long_token is defined as signed, but the arithmetic works out the same
    // without invoking undefined behavior with a signed type.
    auto delta = (uint64_t(l2) - uint64_t(l1)) / 2;
    if (l1 > l2) {
        // wraparound
        delta += 0x8000'0000'0000'0000;
    }
    auto mid = uint64_t(l1) + delta;
    return get_token(mid);
}

std::map<token, float>
murmur3_partitioner::describe_ownership(const std::vector<token>& sorted_tokens) {
    abort();
}

data_type
murmur3_partitioner::get_token_validator() {
    abort();
}

unsigned
murmur3_partitioner::shard_of(const token& t) const {
    int64_t l = long_token(t);
    // treat l as a fraction between 0 and 1 and use 128-bit arithmetic to
    // divide that range evenly among shards:
    uint64_t adjusted = uint64_t(l) + uint64_t(std::numeric_limits<int64_t>::min());
    return (__int128(adjusted) * smp::count) >> 64;
}

using registry = class_registrator<i_partitioner, murmur3_partitioner>;
static registry registrator("org.apache.cassandra.dht.Murmur3Partitioner");
static registry registrator_short_name("Murmur3Partitioner");

}

<|endoftext|>


<commit_before>/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "drivers/SPI.h"
#include "platform/mbed_critical.h"

#if DEVICE_SPI

namespace mbed {

#if DEVICE_SPI_ASYNCH && TRANSACTION_QUEUE_SIZE_SPI
CircularBuffer<Transaction<SPI>, TRANSACTION_QUEUE_SIZE_SPI> SPI::_transaction_buffer;
#endif

SPI::SPI(PinName mosi, PinName miso, PinName sclk, PinName ssel) :
        _spi(),
#if DEVICE_SPI_ASYNCH
        _irq(this),
        _usage(DMA_USAGE_NEVER),
#endif
        _bits(8),
        _mode(0),
        _hz(1000000) {
    // No lock needed in the constructor

    spi_init(&_spi, mosi, miso, sclk, ssel);
    _acquire();
}

void SPI::format(int bits, int mode) {
    lock();
    _bits = bits;
    _mode = mode;
    spi_format(&_spi, _bits, _mode, 0);
    _owner = this;
    unlock();
}

void SPI::frequency(int hz) {
    lock();
    _hz = hz;
    spi_frequency(&_spi, _hz);
    _owner = this;
    unlock();
}

SPI* SPI::_owner = NULL;
SingletonPtr<PlatformMutex> SPI::_mutex;

// ignore the fact there are multiple physical spis, and always update if it wasnt us last
void SPI::aquire() {
    lock();
     if (_owner != this) {
        spi_format(&_spi, _bits, _mode, 0);
        spi_frequency(&_spi, _hz);
        _owner = this;
    }
    unlock();
}

// Note: Private function with no locking
void SPI::_acquire() {
     if (_owner != this) {
        spi_format(&_spi, _bits, _mode, 0);
        spi_frequency(&_spi, _hz);
        _owner = this;
    }
}

int SPI::write(int value) {
    lock();
    _acquire();
    int ret = spi_master_write(&_spi, value);
    unlock();
    return ret;
}

int SPI::write(const char *tx_buffer, int tx_length, char *rx_buffer, int rx_length) {
    lock();
    _acquire();
    int ret = spi_master_block_write(&_spi, tx_buffer, tx_length, rx_buffer, rx_length);
    unlock();
    return ret;
}

void SPI::lock() {
    _mutex->lock();
}

void SPI::unlock() {
    _mutex->unlock();
}

#if DEVICE_SPI_ASYNCH

int SPI::transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, unsigned char bit_width, const event_callback_t& callback, int event)
{
    if (spi_active(&_spi)) {
        return queue_transfer(tx_buffer, tx_length, rx_buffer, rx_length, bit_width, callback, event);
    }
    start_transfer(tx_buffer, tx_length, rx_buffer, rx_length, bit_width, callback, event);
    return 0;
}

void SPI::abort_transfer()
{
    spi_abort_asynch(&_spi);
#if TRANSACTION_QUEUE_SIZE_SPI
    dequeue_transaction();
#endif
}

void SPI::clear_transfer_buffer()
{
#if TRANSACTION_QUEUE_SIZE_SPI
    _transaction_buffer.reset();
#endif
}

void SPI::abort_all_transfers()
{
    clear_transfer_buffer();
    abort_transfer();
}

int SPI::set_dma_usage(DMAUsage usage)
{
    if (spi_active(&_spi)) {
        return -1;
    }
    _usage = usage;
    return  0;
}

int SPI::queue_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, unsigned char bit_width, const event_callback_t& callback, int event)
{
#if TRANSACTION_QUEUE_SIZE_SPI
    transaction_t t;

    t.tx_buffer = const_cast<void *>(tx_buffer);
    t.tx_length = tx_length;
    t.rx_buffer = rx_buffer;
    t.rx_length = rx_length;
    t.event = event;
    t.callback = callback;
    t.width = bit_width;
    Transaction<SPI> transaction(this, t);
    if (_transaction_buffer.full()) {
        return -1; // the buffer is full
    } else {
        core_util_critical_section_enter();
        _transaction_buffer.push(transaction);
        if (!spi_active(&_spi)) {
            dequeue_transaction();
        }
        core_util_critical_section_exit();
        return 0;
    }
#else
    return -1;
#endif
}

void SPI::start_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, unsigned char bit_width, const event_callback_t& callback, int event)
{
    _acquire();
    _callback = callback;
    _irq.callback(&SPI::irq_handler_asynch);
    spi_master_transfer(&_spi, tx_buffer, tx_length, rx_buffer, rx_length, bit_width, _irq.entry(), event , _usage);
}

#if TRANSACTION_QUEUE_SIZE_SPI

void SPI::start_transaction(transaction_t *data)
{
    start_transfer(data->tx_buffer, data->tx_length, data->rx_buffer, data->rx_length, data->width, data->callback, data->event);
}

void SPI::dequeue_transaction()
{
    Transaction<SPI> t;
    if (_transaction_buffer.pop(t)) {
        SPI* obj = t.get_object();
        transaction_t* data = t.get_transaction();
        obj->start_transaction(data);
    }
}

#endif

void SPI::irq_handler_asynch(void)
{
    int event = spi_irq_handler_asynch(&_spi);
    if (_callback && (event & SPI_EVENT_ALL)) {
        _callback.call(event & SPI_EVENT_ALL);
    }
#if TRANSACTION_QUEUE_SIZE_SPI
    if (event & (SPI_EVENT_ALL | SPI_EVENT_INTERNAL_TRANSFER_COMPLETE)) {
        // SPI peripheral is free (event happend), dequeue transaction
        dequeue_transaction();
    }
#endif
}

#endif

} // namespace mbed

#endif
<commit_msg>Corrected handling of format/frequency<commit_after>/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "drivers/SPI.h"
#include "platform/mbed_critical.h"

#if DEVICE_SPI

namespace mbed {

#if DEVICE_SPI_ASYNCH && TRANSACTION_QUEUE_SIZE_SPI
CircularBuffer<Transaction<SPI>, TRANSACTION_QUEUE_SIZE_SPI> SPI::_transaction_buffer;
#endif

SPI::SPI(PinName mosi, PinName miso, PinName sclk, PinName ssel) :
        _spi(),
#if DEVICE_SPI_ASYNCH
        _irq(this),
        _usage(DMA_USAGE_NEVER),
#endif
        _bits(8),
        _mode(0),
        _hz(1000000) {
    // No lock needed in the constructor

    spi_init(&_spi, mosi, miso, sclk, ssel);
    _acquire();
}

void SPI::format(int bits, int mode) {
    lock();
    _bits = bits;
    _mode = mode;
    // If changing format while you are the owner than just
    // update format, but if owner is changed than even frequency should be
    // updated which is done by acquire.
    if (_owner == this) {
        spi_format(&_spi, _bits, _mode, 0);
    } else {
        _acquire();
    }
    unlock();
}

void SPI::frequency(int hz) {
    lock();
    _hz = hz;
    // If changing format while you are the owner than just
    // update frequency, but if owner is changed than even frequency should be
    // updated which is done by acquire.
    if (_owner == this) {
        spi_frequency(&_spi, _hz);
    } else {
        _acquire();
    }
    unlock();
}

SPI* SPI::_owner = NULL;
SingletonPtr<PlatformMutex> SPI::_mutex;

// ignore the fact there are multiple physical spis, and always update if it wasnt us last
void SPI::aquire() {
    lock();
     if (_owner != this) {
        spi_format(&_spi, _bits, _mode, 0);
        spi_frequency(&_spi, _hz);
        _owner = this;
    }
    unlock();
}

// Note: Private function with no locking
void SPI::_acquire() {
     if (_owner != this) {
        spi_format(&_spi, _bits, _mode, 0);
        spi_frequency(&_spi, _hz);
        _owner = this;
    }
}

int SPI::write(int value) {
    lock();
    _acquire();
    int ret = spi_master_write(&_spi, value);
    unlock();
    return ret;
}

int SPI::write(const char *tx_buffer, int tx_length, char *rx_buffer, int rx_length) {
    lock();
    _acquire();
    int ret = spi_master_block_write(&_spi, tx_buffer, tx_length, rx_buffer, rx_length);
    unlock();
    return ret;
}

void SPI::lock() {
    _mutex->lock();
}

void SPI::unlock() {
    _mutex->unlock();
}

#if DEVICE_SPI_ASYNCH

int SPI::transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, unsigned char bit_width, const event_callback_t& callback, int event)
{
    if (spi_active(&_spi)) {
        return queue_transfer(tx_buffer, tx_length, rx_buffer, rx_length, bit_width, callback, event);
    }
    start_transfer(tx_buffer, tx_length, rx_buffer, rx_length, bit_width, callback, event);
    return 0;
}

void SPI::abort_transfer()
{
    spi_abort_asynch(&_spi);
#if TRANSACTION_QUEUE_SIZE_SPI
    dequeue_transaction();
#endif
}

void SPI::clear_transfer_buffer()
{
#if TRANSACTION_QUEUE_SIZE_SPI
    _transaction_buffer.reset();
#endif
}

void SPI::abort_all_transfers()
{
    clear_transfer_buffer();
    abort_transfer();
}

int SPI::set_dma_usage(DMAUsage usage)
{
    if (spi_active(&_spi)) {
        return -1;
    }
    _usage = usage;
    return  0;
}

int SPI::queue_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, unsigned char bit_width, const event_callback_t& callback, int event)
{
#if TRANSACTION_QUEUE_SIZE_SPI
    transaction_t t;

    t.tx_buffer = const_cast<void *>(tx_buffer);
    t.tx_length = tx_length;
    t.rx_buffer = rx_buffer;
    t.rx_length = rx_length;
    t.event = event;
    t.callback = callback;
    t.width = bit_width;
    Transaction<SPI> transaction(this, t);
    if (_transaction_buffer.full()) {
        return -1; // the buffer is full
    } else {
        core_util_critical_section_enter();
        _transaction_buffer.push(transaction);
        if (!spi_active(&_spi)) {
            dequeue_transaction();
        }
        core_util_critical_section_exit();
        return 0;
    }
#else
    return -1;
#endif
}

void SPI::start_transfer(const void *tx_buffer, int tx_length, void *rx_buffer, int rx_length, unsigned char bit_width, const event_callback_t& callback, int event)
{
    _acquire();
    _callback = callback;
    _irq.callback(&SPI::irq_handler_asynch);
    spi_master_transfer(&_spi, tx_buffer, tx_length, rx_buffer, rx_length, bit_width, _irq.entry(), event , _usage);
}

#if TRANSACTION_QUEUE_SIZE_SPI

void SPI::start_transaction(transaction_t *data)
{
    start_transfer(data->tx_buffer, data->tx_length, data->rx_buffer, data->rx_length, data->width, data->callback, data->event);
}

void SPI::dequeue_transaction()
{
    Transaction<SPI> t;
    if (_transaction_buffer.pop(t)) {
        SPI* obj = t.get_object();
        transaction_t* data = t.get_transaction();
        obj->start_transaction(data);
    }
}

#endif

void SPI::irq_handler_asynch(void)
{
    int event = spi_irq_handler_asynch(&_spi);
    if (_callback && (event & SPI_EVENT_ALL)) {
        _callback.call(event & SPI_EVENT_ALL);
    }
#if TRANSACTION_QUEUE_SIZE_SPI
    if (event & (SPI_EVENT_ALL | SPI_EVENT_INTERNAL_TRANSFER_COMPLETE)) {
        // SPI peripheral is free (event happend), dequeue transaction
        dequeue_transaction();
    }
#endif
}

#endif

} // namespace mbed

#endif
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the examples of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the either Technology Preview License Agreement or the
** Beta Release License Agreement.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain
** additional rights. These rights are described in the Nokia Qt LGPL
** Exception version 1.0, included in the file LGPL_EXCEPTION.txt in this
** package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3.0 as published by the Free Software
** Foundation and appearing in the file LICENSE.GPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU General Public License version 3.0 requirements will be
** met: http://www.gnu.org/copyleft/gpl.html.
**
** If you are unsure which license is appropriate for your use, please
** contact the sales department at http://qt.nokia.com/contact.
** $QT_END_LICENSE$
**
****************************************************************************/

#include "customshadereffect.h"
#include <QGLShaderProgram>

static char const colorizeShaderCode[] =
    "uniform lowp vec4 effectColor;\n"
    "mediump vec4 customShader(lowp sampler2D imageTexture, highp vec2 textureCoords) {\n"
    "    vec4 src = texture2D(imageTexture, textureCoords);\n"
    "    float gray = dot(src.rgb, vec3(0.212671, 0.715160, 0.072169));\n"
    "    vec4 colorize = 1.0-((1.0-gray)*(1.0-effectColor));\n"
    "    return vec4(colorize.rgb, src.a);\n"
    "}";

CustomShaderEffect::CustomShaderEffect()
    : QGraphicsShaderEffect(),
      color(Qt::red)
{
    setPixelShaderFragment(colorizeShaderCode);
}

void CustomShaderEffect::setEffectColor(const QColor& c)
{
    color = c;
    setUniformsDirty();
}

void CustomShaderEffect::setUniforms(QGLShaderProgram *program)
{
    program->setUniformValue("effectColor", color);
}
<commit_msg>Fixed custom shader example to correctly use premultiplied colors.<commit_after>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the examples of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the either Technology Preview License Agreement or the
** Beta Release License Agreement.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain
** additional rights. These rights are described in the Nokia Qt LGPL
** Exception version 1.0, included in the file LGPL_EXCEPTION.txt in this
** package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3.0 as published by the Free Software
** Foundation and appearing in the file LICENSE.GPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU General Public License version 3.0 requirements will be
** met: http://www.gnu.org/copyleft/gpl.html.
**
** If you are unsure which license is appropriate for your use, please
** contact the sales department at http://qt.nokia.com/contact.
** $QT_END_LICENSE$
**
****************************************************************************/

#include "customshadereffect.h"
#include <QGLShaderProgram>

static char const colorizeShaderCode[] =
    "uniform lowp vec4 effectColor;\n"
    "mediump vec4 customShader(lowp sampler2D imageTexture, highp vec2 textureCoords) {\n"
    "    vec4 src = texture2D(imageTexture, textureCoords);\n"
    "    float gray = dot(src.rgb, vec3(0.212671, 0.715160, 0.072169));\n"
    "    vec4 colorize = 1.0-((1.0-gray)*(1.0-effectColor));\n"
    "    return vec4(colorize.rgb * src.a, src.a);\n"
    "}";

CustomShaderEffect::CustomShaderEffect()
    : QGraphicsShaderEffect(),
      color(Qt::red)
{
    setPixelShaderFragment(colorizeShaderCode);
}

void CustomShaderEffect::setEffectColor(const QColor& c)
{
    color = c;
    setUniformsDirty();
}

void CustomShaderEffect::setUniforms(QGLShaderProgram *program)
{
    program->setUniformValue("effectColor", color);
}
<|endoftext|>


<commit_before>#include <iostream>
#include <vector>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/features/normal_3d.h>

int
main (int argc, char** argv)
{
  std::string fileName = argv[1];
  std::cout << "Reading " << fileName << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file");
    return (-1);
  }

  std::cout << "Loaded " << cloud->points.size() << " points." << std::endl;

  // Compute the normals
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
  normalEstimation.setInputCloud (cloud);

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  normalEstimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>);

  normalEstimation.setRadiusSearch (0.03);

  normalEstimation.compute (*cloudWithNormals);

  // Setup the feature computation

  pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfhEstimation;
  // Provide the original point cloud (without normals)
  pfhEstimation.setInputCloud (cloud);
  // Provide the point cloud with normals
  pfhEstimation.setInputNormals(cloudWithNormals);

  // pfhEstimation.setInputWithNormals(cloud, cloudWithNormals); PFHEstimation does not have this function
  // Use the same KdTree from the normal estimation
  pfhEstimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::PFHSignature125>::Ptr pfhFeatures(new pcl::PointCloud<pcl::PFHSignature125>);

  pfhEstimation.setRadiusSearch (0.2);

  // Actually compute the spin images
  pfhEstimation.compute (*pfhFeatures);

  std::cout << "output points.size (): " << pfhFeatures->points.size () << std::endl;

  // Display and retrieve the shape context descriptor vector for the 0th point.
  pcl::PFHSignature125 descriptor = pfhFeatures->points[0];
  std::cout << descriptor << std::endl;

  return 0;
}
<commit_msg>examples/ added another license<commit_after>/*
 * Software License Agreement (BSD License)
 *
 * Point Cloud Library (PCL) - www.pointclouds.org
 * Copyright (c) 2009-2011, Willow Garage, Inc.
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * * Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 *   copyright notice, this list of conditions and the following
 *   disclaimer in the documentation and/or other materials provided
 *   with the distribution.
 * * Neither the name of Willow Garage, Inc. nor the names of its
 *   contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#include <iostream>
#include <vector>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/features/normal_3d.h>

int
main (int argc, char** argv)
{
  std::string fileName = argv[1];
  std::cout << "Reading " << fileName << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file");
    return (-1);
  }

  std::cout << "Loaded " << cloud->points.size () << " points." << std::endl;

  // Compute the normals
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
  normalEstimation.setInputCloud (cloud);

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  normalEstimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>);

  normalEstimation.setRadiusSearch (0.03);

  normalEstimation.compute (*cloudWithNormals);

  // Setup the feature computation

  pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfhEstimation;
  // Provide the original point cloud (without normals)
  pfhEstimation.setInputCloud (cloud);
  // Provide the point cloud with normals
  pfhEstimation.setInputNormals (cloudWithNormals);

  // pfhEstimation.setInputWithNormals (cloud, cloudWithNormals); PFHEstimation does not have this function
  // Use the same KdTree from the normal estimation
  pfhEstimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::PFHSignature125>::Ptr pfhFeatures (new pcl::PointCloud<pcl::PFHSignature125>);

  pfhEstimation.setRadiusSearch (0.2);

  // Actually compute the spin images
  pfhEstimation.compute (*pfhFeatures);

  std::cout << "output points.size (): " << pfhFeatures->points.size () << std::endl;

  // Display and retrieve the shape context descriptor vector for the 0th point.
  pcl::PFHSignature125 descriptor = pfhFeatures->points[0];
  std::cout << descriptor << std::endl;

  return 0;
}
<|endoftext|>


<commit_before>#include <igl/viewer/Viewer.h>
#include <igl/tetgen/tetrahedralize.h>
#include <igl/readOFF.h>

// Input polygon
Eigen::MatrixXd V;
Eigen::MatrixXi F;

// Tetrahedralized interior
Eigen::MatrixXd TV;
Eigen::MatrixXi TT;
Eigen::MatrixXi TF;

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  // Load a surface mesh
  igl::readOFF("../shared/fertility.off",V,F);

  // Tetrahedralize the interior
  igl::tetrahedralize(V,F,"pq1.414", TV,TT,TF);

  // Compute berycenters

  // Plot the generated mesh
  igl::Viewer viewer;
  viewer.set_mesh(V,F);
  viewer.launch();
}
<commit_msg>* finished tutorial example for tetgen<commit_after>#include <igl/viewer/Viewer.h>
#include <igl/tetgen/tetrahedralize.h>
#include <igl/readOFF.h>
#include <igl/barycenter.h>

// Input polygon
Eigen::MatrixXd V;
Eigen::MatrixXi F;
Eigen::MatrixXd B;

// Tetrahedralized interior
Eigen::MatrixXd TV;
Eigen::MatrixXi TT;
Eigen::MatrixXi TF;

// This function is called every time a keyboard button is pressed
bool key_down(igl::Viewer& viewer, unsigned char key, int modifier)
{
  using namespace std;
  using namespace Eigen;

  if (key >= '1' && key <= '9')
  {
    double t = double((key - '1')+1) / 9.0;

    VectorXd v = B.col(2).array() - B.col(2).minCoeff();
    v /= v.col(0).maxCoeff();

    vector<int> s;

    for (unsigned i=0; i<v.size();++i)
      if (v(i) < t)
        s.push_back(i);

    MatrixXd V_temp(s.size()*4,3);
    MatrixXi F_temp(s.size()*4,3);

    for (unsigned i=0; i<s.size();++i)
    {
      V_temp.row(i*4+0) = TV.row(TT(s[i],0));
      V_temp.row(i*4+1) = TV.row(TT(s[i],1));
      V_temp.row(i*4+2) = TV.row(TT(s[i],2));
      V_temp.row(i*4+3) = TV.row(TT(s[i],3));
      F_temp.row(i*4+0) << (i*4)+0, (i*4)+1, (i*4)+3;
      F_temp.row(i*4+1) << (i*4)+0, (i*4)+2, (i*4)+1;
      F_temp.row(i*4+2) << (i*4)+3, (i*4)+2, (i*4)+0;
      F_temp.row(i*4+3) << (i*4)+1, (i*4)+2, (i*4)+3;
    }

    viewer.clear_mesh();
    viewer.set_mesh(V_temp,F_temp);
    viewer.options.face_based = true;
  }

  return false;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  // Load a surface mesh
  igl::readOFF("../shared/fertility.off",V,F);

  // Tetrahedralize the interior
  igl::tetrahedralize(V,F,"pq1.414", TV,TT,TF);

  // Compute barycenters
  igl::barycenter(TV,TT,B);

  // Plot the generated mesh
  igl::Viewer viewer;
  viewer.callback_key_down = &key_down;
  key_down(viewer,'5',0);
  viewer.launch();
}
<|endoftext|>


<commit_before>#include <iostream>
#include <thread>
#include <chrono>
#include <random>
#include <time.h>
#include <assert.h>
#include "IntervalTree.h"
#define CATCH_CONFIG_RUNNER // Mark this as file as the test-runner for catch
#include "catch.hpp"        // Include the catch unit test framework

using namespace std;

typedef Interval<bool> interval;
typedef vector<interval> intervalVector;
typedef IntervalTree<bool> intervalTree;

TEST_CASE( "Empty tree" ) {
    IntervalTree<int> t;
    REQUIRE( t.findOverlapping(-1,1).size() == 0 );
}

TEST_CASE( "Singleton tree" ) {
    vector<Interval<double>> values{{1,3,5.5}};
    IntervalTree<double> t{values};

    SECTION ("Point query on left") {
	auto v = t.findOverlapping(1,1);
	REQUIRE( v.size() == 1);
	REQUIRE( v.front().start == 1 );
	REQUIRE( v.front().stop == 3 );
	REQUIRE( v.front().value == 5.5 );
    }
}

template<typename K>
K randKey(K floor, K ceiling) {
    K range = ceiling - floor;
    return floor + range * ((double) rand() / (double) (RAND_MAX + 1.0));
}

template<class T, typename K>
Interval<T,K> randomInterval(K maxStart, K maxLength, K maxStop, const T& value) {
    K start = randKey<K>(0, maxStart);
    K stop = min<K>(randKey<K>(start, start + maxLength), maxStop);
    return Interval<T,K>(start, stop, value);
}

int main(int argc, char**argv) {
    typedef vector<std::size_t> countsVector;

    // a simple sanity check
    intervalVector sanityIntervals;
    sanityIntervals.push_back(interval(60, 80, true));
    sanityIntervals.push_back(interval(20, 40, true));
    intervalTree sanityTree(sanityIntervals);

    intervalVector sanityResults;
    sanityTree.findOverlapping(30, 50, sanityResults);
    assert(sanityResults.size() == 1);
    sanityResults.clear();
    sanityTree.findContained(15, 45, sanityResults);
    assert(sanityResults.size() == 1);

    srand((unsigned)time(NULL));

    intervalVector intervals;
    intervalVector queries;

    // generate a test set of target intervals
    for (int i = 0; i < 10000; ++i) {
        intervals.push_back(randomInterval<bool, unsigned long>(100000, 1000, 100000 + 1, true));
    }
    // and queries
    for (int i = 0; i < 5000; ++i) {
        queries.push_back(randomInterval<bool, unsigned long>(100000, 1000, 100000 + 1, true));
    }

    typedef chrono::high_resolution_clock Clock;
    typedef chrono::milliseconds milliseconds;

    // using brute-force search
    countsVector bruteforcecounts;
    Clock::time_point t0 = Clock::now();
    for (intervalVector::iterator q = queries.begin(); q != queries.end(); ++q) {
        intervalVector results;
        for (intervalVector::iterator i = intervals.begin(); i != intervals.end(); ++i) {
            if (i->start >= q->start && i->stop <= q->stop) {
                results.push_back(*i);
            }
        }
        bruteforcecounts.push_back(results.size());
    }
    Clock::time_point t1 = Clock::now();
    milliseconds ms = chrono::duration_cast<milliseconds>(t1 - t0);
    cout << "brute force:\t" << ms.count() << "ms" << endl;

    // using the interval tree
    intervalTree tree = intervalTree(intervals);
    countsVector treecounts;
    t0 = Clock::now();
    for (intervalVector::iterator q = queries.begin(); q != queries.end(); ++q) {
        intervalVector results;
        tree.findContained(q->start, q->stop, results);
        treecounts.push_back(results.size());
    }
    t1 = Clock::now();
    ms = std::chrono::duration_cast<milliseconds>(t1 - t0);
    cout << "interval tree:\t" << ms.count() << "ms" << endl;

    // check that the same number of results are returned
    countsVector::iterator b = bruteforcecounts.begin();
    for (countsVector::iterator t = treecounts.begin(); t != treecounts.end(); ++t, ++b) {
        assert(*b == *t);
    }

    return Catch::Session().run( argc, argv );
}
<commit_msg>Check that identical intervals are both stored<commit_after>#include <iostream>
#include <thread>
#include <chrono>
#include <random>
#include <time.h>
#include <assert.h>
#include "IntervalTree.h"
#define CATCH_CONFIG_RUNNER // Mark this as file as the test-runner for catch
#include "catch.hpp"        // Include the catch unit test framework

using namespace std;

typedef Interval<bool> interval;
typedef vector<interval> intervalVector;
typedef IntervalTree<bool> intervalTree;

TEST_CASE( "Empty tree" ) {
    IntervalTree<int> t;
    REQUIRE( t.findOverlapping(-1,1).size() == 0 );
}

TEST_CASE( "Singleton tree" ) {
    vector<Interval<double>> values{{1,3,5.5}};
    IntervalTree<double> t{values};

    SECTION ("Point query on left") {
	auto v = t.findOverlapping(1,1);
	REQUIRE( v.size() == 1);
	REQUIRE( v.front().start == 1 );
	REQUIRE( v.front().stop == 3 );
	REQUIRE( v.front().value == 5.5 );
    }

    SECTION ("Point query in middle") {
	auto v = t.findOverlapping(2,2);
	REQUIRE( v.size() == 1);
	REQUIRE( v.front().start == 1 );
	REQUIRE( v.front().stop == 3 );
	REQUIRE( v.front().value == 5.5 );
    }

    SECTION ("Point query on right") {
	auto v = t.findOverlapping(3,3);
	REQUIRE( v.size() == 1);
	REQUIRE( v.front().start == 1 );
	REQUIRE( v.front().stop == 3 );
	REQUIRE( v.front().value == 5.5 );
    }

    SECTION ("Non-overlapping queries") {
	REQUIRE( t.findOverlapping(4,4).size() == 0);
	REQUIRE( t.findOverlapping(0,0).size() == 0);
    }
}

TEST_CASE( "Two identical intervals with different contents" ) {
    vector<Interval<double>> values{{5,10,10.5},{5,10,5.5}};
    IntervalTree<double> t{values};

    auto v = t.findOverlapping(6,6);
    REQUIRE( v.size() == 2);
    REQUIRE( v.front().start == 5 );
    REQUIRE( v.front().stop == 10 );
    REQUIRE( v.back().start == 5 );
    REQUIRE( v.back().stop == 10 );
    set<double> expected{5.5, 10.5};
    set<double> actual{v.front().value, v.back().value};
    REQUIRE( actual == expected);
}

template<typename K>
K randKey(K floor, K ceiling) {
    K range = ceiling - floor;
    return floor + range * ((double) rand() / (double) (RAND_MAX + 1.0));
}

template<class T, typename K>
Interval<T,K> randomInterval(K maxStart, K maxLength, K maxStop, const T& value) {
    K start = randKey<K>(0, maxStart);
    K stop = min<K>(randKey<K>(start, start + maxLength), maxStop);
    return Interval<T,K>(start, stop, value);
}

int main(int argc, char**argv) {
    typedef vector<std::size_t> countsVector;

    // a simple sanity check
    intervalVector sanityIntervals;
    sanityIntervals.push_back(interval(60, 80, true));
    sanityIntervals.push_back(interval(20, 40, true));
    intervalTree sanityTree(sanityIntervals);

    intervalVector sanityResults;
    sanityTree.findOverlapping(30, 50, sanityResults);
    assert(sanityResults.size() == 1);
    sanityResults.clear();
    sanityTree.findContained(15, 45, sanityResults);
    assert(sanityResults.size() == 1);

    srand((unsigned)time(NULL));

    intervalVector intervals;
    intervalVector queries;

    // generate a test set of target intervals
    for (int i = 0; i < 10000; ++i) {
        intervals.push_back(randomInterval<bool, unsigned long>(100000, 1000, 100000 + 1, true));
    }
    // and queries
    for (int i = 0; i < 5000; ++i) {
        queries.push_back(randomInterval<bool, unsigned long>(100000, 1000, 100000 + 1, true));
    }

    typedef chrono::high_resolution_clock Clock;
    typedef chrono::milliseconds milliseconds;

    // using brute-force search
    countsVector bruteforcecounts;
    Clock::time_point t0 = Clock::now();
    for (intervalVector::iterator q = queries.begin(); q != queries.end(); ++q) {
        intervalVector results;
        for (intervalVector::iterator i = intervals.begin(); i != intervals.end(); ++i) {
            if (i->start >= q->start && i->stop <= q->stop) {
                results.push_back(*i);
            }
        }
        bruteforcecounts.push_back(results.size());
    }
    Clock::time_point t1 = Clock::now();
    milliseconds ms = chrono::duration_cast<milliseconds>(t1 - t0);
    cout << "brute force:\t" << ms.count() << "ms" << endl;

    // using the interval tree
    intervalTree tree = intervalTree(intervals);
    countsVector treecounts;
    t0 = Clock::now();
    for (intervalVector::iterator q = queries.begin(); q != queries.end(); ++q) {
        intervalVector results;
        tree.findContained(q->start, q->stop, results);
        treecounts.push_back(results.size());
    }
    t1 = Clock::now();
    ms = std::chrono::duration_cast<milliseconds>(t1 - t0);
    cout << "interval tree:\t" << ms.count() << "ms" << endl;

    // check that the same number of results are returned
    countsVector::iterator b = bruteforcecounts.begin();
    for (countsVector::iterator t = treecounts.begin(); t != treecounts.end(); ++t, ++b) {
        assert(*b == *t);
    }

    return Catch::Session().run( argc, argv );
}
<|endoftext|>