<commit_before>#include "tests.h"

// Work efficient prefix sum

template <typename F, typename std::enable_if<std::is_floating_point<F>::value>::type* = nullptr>
bool check_sum(cl::sycl::buffer<F, 1>& data) {
	using namespace cl::sycl;

	auto d = data.get_access<access::read, access::host_buffer>();
	F sum = 0;
	for(size_t i = 0; i < data.get_count(); ++i) {
		sum += (F)i;
		auto diff = std::abs(sum - d[i]);
		if(diff > 0.01) {
			debug() << "wrong sum, should be" << sum << "- is" << d[i];
			return false;
		}
	}

	return true;
}

template <typename I, typename std::enable_if<std::is_integral<I>::value>::type* = nullptr>
bool check_sum(cl::sycl::buffer<I, 1>& data) {
	using namespace cl::sycl;

	auto d = data.get_access<access::read, access::host_buffer>();
	I sum = 0;
	for(size_t i = 0; i < data.get_count(); ++i) {
		sum += (I)i;
		if(d[i] != sum) {
			debug() << "wrong sum, should be" << sum << "- is" << d[i];
			return false;
		}
	}

	return true;
}

template <typename T>
struct prefix_sum_kernel {
protected:
	using mode = cl::sycl::access::mode;
	using target = cl::sycl::access::target;

	// Global memory
	cl::sycl::accessor<T> input;

	// Local memory
	cl::sycl::accessor<T, 1, mode::read_write, target::local> localBlock;

public:
	prefix_sum_kernel(cl::sycl::buffer<T>& data, size_t group_size)
		: input(data.get_access<mode::read_write>()), localBlock(group_size) {}

	void operator()(cl::sycl::nd_item<1> index) {
		using namespace cl::sycl;

		uint1 GID = 2 * index.get_global_id(0);
		uint1 LID = 2 * index.get_local_id(0);

		localBlock[LID] = input[GID];
		localBlock[LID + 1] = input[GID + 1];

		index.barrier(access::fence_space::local);

		uint1 N = 2 * index.get_local_size(0);
		uint1 first;
		uint1 second;

		LID /= 2;

		// Up-sweep
		uint1 offset = 1;
		SYCL_WHILE(offset < N)
			SYCL_BEGIN {
			SYCL_IF(LID % offset == 0)
			SYCL_BEGIN {
				first = 2 * LID + offset - 1;
				second = first + offset;
				localBlock[second] = localBlock[first] + localBlock[second];
			}
			SYCL_END

			index.barrier(access::fence_space::local);
			offset *= 2;
		}
		SYCL_END

		SYCL_IF(LID == 0)
		SYCL_THEN({
			localBlock[N - 1] = 0;
		})
		index.barrier(access::fence_space::local);

		vec<T, 1> tmp;

		// Down-sweep
		offset = N;
		SYCL_WHILE(offset > 0)
		SYCL_BEGIN {
			SYCL_IF(LID % offset == 0)
			SYCL_BEGIN {
				first = 2 * LID + offset - 1;
				second = first + offset;
				tmp = localBlock[second];
				localBlock[second] = localBlock[first] + tmp;
				localBlock[first] = tmp;
			}
			SYCL_END

			index.barrier(access::fence_space::local);
			offset /= 2;
		}
		SYCL_END

		LID *= 2;
		input[GID] += localBlock[LID];
		input[GID + 1] += localBlock[LID + 1];
	}
};

template <typename T>
struct prefix_sum_join_kernel {
protected:
	using mode = cl::sycl::access::mode;
	using target = cl::sycl::access::target;
	using buffer = cl::sycl::buffer<T>;

	// Global memory
	cl::sycl::accessor<T> data;
	cl::sycl::accessor<T, 1, mode::read> higher_level;

	// Local memory
	cl::sycl::accessor<T, 1, mode::read_write, target::local> localBlock;

public:
	prefix_sum_join_kernel(buffer& data, buffer& higher_level, size_t group_size)
		: data(data.get_access<mode::read_write>()),
		higher_level(higher_level.get_access<mode::read>())
		localBlock(group_size) {}

	void operator()(cl::sycl::nd_item<1> index) {
		using namespace cl::sycl;
		int1 GID = index.get_global_id(0);
		int1 N = 2 * index.get_local_size(0);
		SYCL_IF(GID >= N)
		SYCL_THEN({
			data[GID] += higher_level[GID / N - 1];
		})
	}
};

bool test9() {
	using namespace cl::sycl;

	{
		queue myQueue;

		const auto group_size = myQueue.get_device().get_info<CL_DEVICE_MAX_WORK_GROUP_SIZE>();
		const auto size = group_size * 2;
		size_t N = size;

		using type = double;
		buffer<type> data(size);

		// Init
		command_group(myQueue, [&]() {
			auto d = data.get_access<access::write>();

			parallel_for<>(range<1>(size), [=](id<1> index) {
				d[index] = index;
			});
		});

		command_group(myQueue, [&]() {
			// TODO: Extend for large arrays
			parallel_for<>(
				nd_range<1>(N / 2, group_size),
				prefix_sum_kernel<type>(data, group_size)
			);

			if(N <= group_size * 2) {
				return;
			}
		});

		return check_sum(data);
	}

	return true;
}
<commit_msg>Preparing for large arrays by allocating multiple buffers in test9.<commit_after>#include "tests.h"

// Work efficient prefix sum

template <typename F, typename std::enable_if<std::is_floating_point<F>::value>::type* = nullptr>
bool check_sum(cl::sycl::buffer<F, 1>& data) {
	using namespace cl::sycl;

	auto d = data.get_access<access::read, access::host_buffer>();
	F sum = 0;
	for(size_t i = 0; i < data.get_count(); ++i) {
		sum += (F)i;
		auto diff = std::abs(sum - d[i]);
		if(diff > 0.01) {
			debug() << "wrong sum, should be" << sum << "- is" << d[i];
			return false;
		}
	}

	return true;
}

template <typename I, typename std::enable_if<std::is_integral<I>::value>::type* = nullptr>
bool check_sum(cl::sycl::buffer<I, 1>& data) {
	using namespace cl::sycl;

	auto d = data.get_access<access::read, access::host_buffer>();
	I sum = 0;
	for(size_t i = 0; i < data.get_count(); ++i) {
		sum += (I)i;
		if(d[i] != sum) {
			debug() << "wrong sum, should be" << sum << "- is" << d[i];
			return false;
		}
	}

	return true;
}

template <typename T>
struct prefix_sum_kernel {
protected:
	using mode = cl::sycl::access::mode;
	using target = cl::sycl::access::target;

	// Global memory
	cl::sycl::accessor<T> input;

	// Local memory
	cl::sycl::accessor<T, 1, mode::read_write, target::local> localBlock;

public:
	prefix_sum_kernel(cl::sycl::buffer<T>& data, size_t group_size)
		: input(data.get_access<mode::read_write>()), localBlock(group_size) {}

	void operator()(cl::sycl::nd_item<1> index) {
		using namespace cl::sycl;

		uint1 GID = 2 * index.get_global_id(0);
		uint1 LID = 2 * index.get_local_id(0);

		localBlock[LID] = input[GID];
		localBlock[LID + 1] = input[GID + 1];

		index.barrier(access::fence_space::local);

		uint1 N = 2 * index.get_local_size(0);
		uint1 first;
		uint1 second;

		LID /= 2;

		// Up-sweep
		uint1 offset = 1;
		SYCL_WHILE(offset < N)
			SYCL_BEGIN {
			SYCL_IF(LID % offset == 0)
			SYCL_BEGIN {
				first = 2 * LID + offset - 1;
				second = first + offset;
				localBlock[second] = localBlock[first] + localBlock[second];
			}
			SYCL_END

			index.barrier(access::fence_space::local);
			offset *= 2;
		}
		SYCL_END

		SYCL_IF(LID == 0)
		SYCL_THEN({
			localBlock[N - 1] = 0;
		})
		index.barrier(access::fence_space::local);

		vec<T, 1> tmp;

		// Down-sweep
		offset = N;
		SYCL_WHILE(offset > 0)
		SYCL_BEGIN {
			SYCL_IF(LID % offset == 0)
			SYCL_BEGIN {
				first = 2 * LID + offset - 1;
				second = first + offset;
				tmp = localBlock[second];
				localBlock[second] = localBlock[first] + tmp;
				localBlock[first] = tmp;
			}
			SYCL_END

			index.barrier(access::fence_space::local);
			offset /= 2;
		}
		SYCL_END

		LID *= 2;
		input[GID] += localBlock[LID];
		input[GID + 1] += localBlock[LID + 1];
	}
};

template <typename T>
struct prefix_sum_join_kernel {
protected:
	using mode = cl::sycl::access::mode;
	using target = cl::sycl::access::target;
	using buffer = cl::sycl::buffer<T>;

	// Global memory
	cl::sycl::accessor<T> data;
	cl::sycl::accessor<T, 1, mode::read> higher_level;

	// Local memory
	cl::sycl::accessor<T, 1, mode::read_write, target::local> localBlock;

public:
	prefix_sum_join_kernel(buffer& data, buffer& higher_level, size_t group_size)
		: data(data.get_access<mode::read_write>()),
		higher_level(higher_level.get_access<mode::read>())
		localBlock(group_size) {}

	void operator()(cl::sycl::nd_item<1> index) {
		using namespace cl::sycl;
		int1 GID = index.get_global_id(0);
		int1 N = 2 * index.get_local_size(0);
		SYCL_IF(GID >= N)
		SYCL_THEN({
			data[GID] += higher_level[GID / N - 1];
		})
	}
};

bool test9() {
	using namespace cl::sycl;

	{
		queue myQueue;

		const auto group_size = myQueue.get_device().get_info<CL_DEVICE_MAX_WORK_GROUP_SIZE>();
		const auto size = group_size * 2;
		size_t N = size;
		using type = double;

		// Calculate required number of buffer levels
		N = size;
		size_t number_levels = 1;
		while(N > 0) {
			N /= 2 * group_size;
			++number_levels;
		}

		// Create buffers
		vector_class<buffer<type>> data;
		data.reserve(number_levels);
		N = size;
		for(size_t i = 0; i < number_levels; ++i) {
			data.emplace_back(N);
			N = std::max(N / (2 * group_size), group_size);
		}

		// Init
		command_group(myQueue, [&]() {
			auto d = data[0].get_access<access::write>();

			parallel_for<>(range<1>(size), [=](id<1> index) {
				d[index] = index;
			});
		});

		command_group(myQueue, [&]() {
			N = size;

			parallel_for<>(
				nd_range<1>(N / 2, group_size),
				prefix_sum_kernel<type>(data[0], group_size)
			);

			if(N <= group_size * 2) {
				return;
			}

			// TODO: Extend for large arrays
		});

		return check_sum(data[0]);
	}

	return true;
}
<|endoftext|>


<commit_before>#include "pacman_vision/tracker.h"
#include "pacman_vision/utility.h"

///////////////////
//Tracker Class//
///////////////////

//Constructor
Tracker::Tracker(ros::NodeHandle &n)
{
  this->scene.reset(new PTC);
  this->model.reset(new PTC);
  this->nh = ros::NodeHandle (n, "tracker");
  this->queue_ptr.reset(new ros::CallbackQueue);
  this->nh.setCallbackQueue(&(*this->queue_ptr));
  this->srv_track_object = nh.advertiseService("track_object", &Tracker::cb_track_object, this);
  this->rviz_marker_pub = nh.advertise<visualization_msgs::Marker>("tracked_object", 1);
  ce.reset( new pcl::registration::CorrespondenceEstimation<PTT, PTT, float>);
  crd.reset( new pcl::registration::CorrespondenceRejectorDistance);
  cro2o.reset( new pcl::registration::CorrespondenceRejectorOneToOne);
  teDQ.reset(new pcl::registration::TransformationEstimationDualQuaternion<PTT,PTT,float>);
 // crsc.reset( new pcl::registration::CorrespondenceRejectorSampleConsensus<PTT>);

  started = false;
  downsample = true;
  leaf = 0.02f;
  factor = 1.1f;
  rej_distance = 0.025f;
}
Tracker::~Tracker()
{
  this->nh.shutdown();
}

//tracker step
void Tracker::track()
{
  Eigen::Matrix4f inv_trans;
  PTC::Ptr target (new PTC);
  PTC::Ptr tmp (new PTC);
  inv_trans = transform.inverse();
  //filter
  pcl::transformPointCloud(*scene, *tmp, inv_trans);
  pass.setInputCloud(tmp);
  pass.setFilterFieldName("x");
  pass.setFilterLimits(factor*x1, factor*x2);
  pass.filter(*tmp);
  pass.setInputCloud(tmp);
  pass.setFilterFieldName("y");
  pass.setFilterLimits(factor*y1, factor*y2);
  pass.filter(*tmp);
  pass.setInputCloud(tmp);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(factor*z1, factor*z2);
  pass.filter(*tmp);
  if(downsample)
  {
    PTC t;
    vg.setInputCloud (tmp);
    vg.setLeafSize(leaf, leaf, leaf);
    vg.filter (t);
    pcl::transformPointCloud(t, *target, transform);
  }
  else
    pcl::transformPointCloud(*tmp, *target, transform);
  //align
  pcl::Correspondences corr, filt;
  ce->setInputTarget(target);
  ce->determineCorrespondences(corr);
  crd->setInputTarget<PTT>(target);
  crd->setMaximumDistance(rej_distance);
  //cro2o->getRemainingCorrespondences(corr, o2o);
  crd->getRemainingCorrespondences(corr, filt);
  double actual_corr_ratio = filt.size()/corr.size();
  //crsc->setInputTarget(target);
  icp.setInputTarget(target);
  icp.align(*tmp, transform);
  ROS_INFO("corr: %d, filt: %d, fitness: %g, actual: %g", (int)corr.size(), (int)filt.size(), fitness, icp.getFitnessScore(rej_distance));
  this->transform = icp.getFinalTransformation();
  //TODO adjust distance and factor according to fitness and rej.size

}

bool Tracker::cb_track_object(pacman_vision_comm::track_object::Request& req, pacman_vision_comm::track_object::Response& res)
{
  if (req.name.empty())
  {
    ROS_ERROR("[Tracker][%s] You need to provide the name of the object you want to track!", __func__);
    return false;
  }
  std::string models_path (ros::package::getPath("asus_scanner_models"));
  int j = -1;
  for (int i=0; i<names.size(); ++i)
  {
    if (names[i].compare(req.name) == 0)
    {
      j = i;
      break;
    }
  }
  if (j == -1)
  {
    ROS_ERROR("[Tracker][%s] Cannot find %s from the pool of already estimated objects, check spelling or run an estimation first!", __func__, req.name.c_str());
    return false;
  }
  name = (req.name);
  std::vector<std::string> vst;
  boost::split(vst, name, boost::is_any_of("_"), boost::token_compress_on);
  id = vst.at(0);
  transform = estimations[j];
  boost::filesystem::path model_path (models_path + "/" + id + "/" + id + ".pcd");
  PTC::Ptr tmp (new PTC);
  if (boost::filesystem::exists(model_path) && boost::filesystem::is_regular_file(model_path))
  {
    if (pcl::io::loadPCDFile(model_path.c_str(), *tmp))
    {
      ROS_ERROR("[Tracker][%s] Error loading model %s",__func__, model_path.c_str());
      return false;
    }
  }
  else
  {
    ROS_ERROR("[Tracker][%s] Request model (%s) does not exists in asus_scanner_models package",__func__, model_path.stem().c_str());
    return false;
  }
  pcl::VoxelGrid<PTT> vg;
  vg.setInputCloud (tmp);
  vg.setLeafSize(leaf, leaf, leaf);
  vg.filter (*model);
  //Get the minimum and maximum values on each of the 3 (x-y-z) dimensions of model
  std::vector<float> xvec,yvec,zvec;
  for (int i=0; i<model->points.size(); ++i)
  {
    xvec.push_back(model->points[i].x);
    yvec.push_back(model->points[i].y);
    zvec.push_back(model->points[i].z);
  }
  x1 = *std::min_element(xvec.begin(), xvec.end());
  y1 = *std::min_element(yvec.begin(), yvec.end());
  z1 = *std::min_element(zvec.begin(), zvec.end());
  x2 = *std::max_element(xvec.begin(), xvec.end());
  y2 = *std::max_element(yvec.begin(), yvec.end());
  z2 = *std::max_element(zvec.begin(), zvec.end());
  //init icps
  icp.setUseReciprocalCorrespondences(false);
  icp.setMaximumIterations(50);
  icp.setTransformationEpsilon(1e-9);
  icp.setEuclideanFitnessEpsilon(1e-9);
  ce->setInputSource(model);
  icp.setCorrespondenceEstimation(ce);
  crd->setInputSource<PTT>(model);
  crd->setMaximumDistance(rej_distance);
  //crsc->setInputSource(model);
  //crsc->setInlierThreshold(0.02);
  //crsc->setMaximumIterations(5);
  //crsc->setRefineModel(true);
  //icp.addCorrespondenceRejector(cro2o);
  icp.addCorrespondenceRejector(crd);
  //icp.addCorrespondenceRejector(crsc);
  icp.setInputSource(model);
  //do one step of icp
  ce->setInputTarget(scene);
  crd->setInputTarget<PTT>(scene);
  pcl::Correspondences corr, filt;
  ce->determineCorrespondences(corr);
  crd->getRemainingCorrespondences(corr, filt);
  corr_ratio = filt.size()/corr.size();
  icp.setInputTarget(scene);
  icp.setTransformationEstimation(teDQ);
  icp.align(*tmp, transform);
  fitness = icp.getFitnessScore(rej_distance); //save fitness of correct alignment
  //init rviz marker
  marker.header.frame_id = "/camera_rgb_optical_frame";
  marker.ns = std::string(id + "_tracked").c_str();
  marker.id = 0;
  marker.scale.x=1;
  marker.scale.y=1;
  marker.scale.z=1;
  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
  std::string mesh_path ("package://asus_scanner_models/" + id + "/" + id + ".stl");
  marker.mesh_resource = mesh_path.c_str();
  marker.action = visualization_msgs::Marker::ADD;
  marker.color.r = 1.0f;
  marker.color.g = 0.0f;
  marker.color.b = 0.3f;
  marker.color.a = 1.0f;
  marker.lifetime = ros::Duration(1);
  //we are ready to start
  started = true;
  return true;
}

void Tracker::broadcast_tracked_object()
{
  geometry_msgs::Pose pose;
  tf::Transform trans;
  fromEigen(transform, pose, trans);
  marker.header.stamp = ros::Time();
  marker.pose = pose;
  rviz_marker_pub.publish(marker);
  tf_broadcaster.sendTransform(tf::StampedTransform(trans, ros::Time::now(), "/camera_rgb_optical_frame", std::string(name + "_tracked").c_str()));
}

void Tracker::spin_once()
{
  //process this module callbacks
  this->queue_ptr->callAvailable(ros::WallDuration(0));
}

<commit_msg>updated tracker with partial adaptiveness, still missing transform perturbance<commit_after>#include "pacman_vision/tracker.h"
#include "pacman_vision/utility.h"

///////////////////
//Tracker Class//
///////////////////

//Constructor
Tracker::Tracker(ros::NodeHandle &n)
{
  this->scene.reset(new PTC);
  this->model.reset(new PTC);
  this->nh = ros::NodeHandle (n, "tracker");
  this->queue_ptr.reset(new ros::CallbackQueue);
  this->nh.setCallbackQueue(&(*this->queue_ptr));
  this->srv_track_object = nh.advertiseService("track_object", &Tracker::cb_track_object, this);
  this->rviz_marker_pub = nh.advertise<visualization_msgs::Marker>("tracked_object", 1);
  ce.reset( new pcl::registration::CorrespondenceEstimation<PTT, PTT, float>);
  crd.reset( new pcl::registration::CorrespondenceRejectorDistance);
  cro2o.reset( new pcl::registration::CorrespondenceRejectorOneToOne);
  teDQ.reset(new pcl::registration::TransformationEstimationDualQuaternion<PTT,PTT,float>);
 // crsc.reset( new pcl::registration::CorrespondenceRejectorSampleConsensus<PTT>);

  started = false;
  downsample = true;
  leaf = 0.02f;
  factor = 1.1f;
  rej_distance = 0.025f;
}
Tracker::~Tracker()
{
  this->nh.shutdown();
}

//tracker step
void Tracker::track()
{
  Eigen::Matrix4f inv_trans;
  PTC::Ptr target (new PTC);
  PTC::Ptr tmp (new PTC);
  inv_trans = transform.inverse();
  //filter
  pcl::transformPointCloud(*scene, *tmp, inv_trans);
  pass.setInputCloud(tmp);
  pass.setFilterFieldName("x");
  pass.setFilterLimits(factor*x1, factor*x2);
  pass.filter(*tmp);
  pass.setInputCloud(tmp);
  pass.setFilterFieldName("y");
  pass.setFilterLimits(factor*y1, factor*y2);
  pass.filter(*tmp);
  pass.setInputCloud(tmp);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(factor*z1, factor*z2);
  pass.filter(*tmp);
  if(downsample)
  {
    PTC t;
    vg.setInputCloud (tmp);
    vg.setLeafSize(leaf, leaf, leaf);
    vg.filter (t);
    pcl::transformPointCloud(t, *target, transform);
  }
  else
    pcl::transformPointCloud(*tmp, *target, transform);
  //align
  pcl::Correspondences corr, filt;
  ce->setInputTarget(target);
  ce->determineCorrespondences(corr);
  crd->setInputTarget<PTT>(target);
  crd->setMaximumDistance(rej_distance);
  //cro2o->getRemainingCorrespondences(corr, o2o);
  crd->getRemainingCorrespondences(corr, filt);
  double actual_corr_ratio = filt.size()/corr.size();
  //crsc->setInputTarget(target);
  icp.setInputTarget(target);
  icp.align(*tmp, transform);
  ROS_INFO("corr: %d, filt: %d, fitness: %g, actual: %g", (int)corr.size(), (int)filt.size(), fitness, icp.getFitnessScore(rej_distance));
  this->transform = icp.getFinalTransformation();
  //TODO adjust distance and factor according to fitness and rej.size
  if (icp.getFitnessScore() > 0.0008) //something is probably wrong
  {
    rej_distance +=0.003;
    factor += 0.01;
    //create a disturbance
  }
  else if (icp.getFitnessScore() < 0.0003)
  {
    if(rej_distance > 0.01)
      rej_distance -=0.003;
    if(factor >= 1.0)
      factor -=0.01;
  }

}

bool Tracker::cb_track_object(pacman_vision_comm::track_object::Request& req, pacman_vision_comm::track_object::Response& res)
{
  if (req.name.empty())
  {
    ROS_ERROR("[Tracker][%s] You need to provide the name of the object you want to track!", __func__);
    return false;
  }
  std::string models_path (ros::package::getPath("asus_scanner_models"));
  int j = -1;
  for (int i=0; i<names.size(); ++i)
  {
    if (names[i].compare(req.name) == 0)
    {
      j = i;
      break;
    }
  }
  if (j == -1)
  {
    ROS_ERROR("[Tracker][%s] Cannot find %s from the pool of already estimated objects, check spelling or run an estimation first!", __func__, req.name.c_str());
    return false;
  }
  name = (req.name);
  std::vector<std::string> vst;
  boost::split(vst, name, boost::is_any_of("_"), boost::token_compress_on);
  id = vst.at(0);
  transform = estimations[j];
  boost::filesystem::path model_path (models_path + "/" + id + "/" + id + ".pcd");
  PTC::Ptr tmp (new PTC);
  if (boost::filesystem::exists(model_path) && boost::filesystem::is_regular_file(model_path))
  {
    if (pcl::io::loadPCDFile(model_path.c_str(), *tmp))
    {
      ROS_ERROR("[Tracker][%s] Error loading model %s",__func__, model_path.c_str());
      return false;
    }
  }
  else
  {
    ROS_ERROR("[Tracker][%s] Request model (%s) does not exists in asus_scanner_models package",__func__, model_path.stem().c_str());
    return false;
  }
  pcl::VoxelGrid<PTT> vg;
  vg.setInputCloud (tmp);
  vg.setLeafSize(leaf, leaf, leaf);
  vg.filter (*model);
  //Get the minimum and maximum values on each of the 3 (x-y-z) dimensions of model
  std::vector<float> xvec,yvec,zvec;
  for (int i=0; i<model->points.size(); ++i)
  {
    xvec.push_back(model->points[i].x);
    yvec.push_back(model->points[i].y);
    zvec.push_back(model->points[i].z);
  }
  x1 = *std::min_element(xvec.begin(), xvec.end());
  y1 = *std::min_element(yvec.begin(), yvec.end());
  z1 = *std::min_element(zvec.begin(), zvec.end());
  x2 = *std::max_element(xvec.begin(), xvec.end());
  y2 = *std::max_element(yvec.begin(), yvec.end());
  z2 = *std::max_element(zvec.begin(), zvec.end());
  //init icps
  icp.setUseReciprocalCorrespondences(false);
  icp.setMaximumIterations(50);
  icp.setTransformationEpsilon(1e-9);
  icp.setEuclideanFitnessEpsilon(1e-9);
  ce->setInputSource(model);
  icp.setCorrespondenceEstimation(ce);
  crd->setInputSource<PTT>(model);
  crd->setMaximumDistance(rej_distance);
  //crsc->setInputSource(model);
  //crsc->setInlierThreshold(0.02);
  //crsc->setMaximumIterations(5);
  //crsc->setRefineModel(true);
  //icp.addCorrespondenceRejector(cro2o);
  icp.addCorrespondenceRejector(crd);
  //icp.addCorrespondenceRejector(crsc);
  icp.setInputSource(model);
  //do one step of icp
  ce->setInputTarget(scene);
  crd->setInputTarget<PTT>(scene);
  pcl::Correspondences corr, filt;
  ce->determineCorrespondences(corr);
  crd->getRemainingCorrespondences(corr, filt);
  corr_ratio = filt.size()/corr.size();
  icp.setInputTarget(scene);
  icp.setTransformationEstimation(teDQ);
  icp.align(*tmp, transform);
  fitness = icp.getFitnessScore(rej_distance); //save fitness of correct alignment
  //init rviz marker
  marker.header.frame_id = "/camera_rgb_optical_frame";
  marker.ns = std::string(id + "_tracked").c_str();
  marker.id = 0;
  marker.scale.x=1;
  marker.scale.y=1;
  marker.scale.z=1;
  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
  std::string mesh_path ("package://asus_scanner_models/" + id + "/" + id + ".stl");
  marker.mesh_resource = mesh_path.c_str();
  marker.action = visualization_msgs::Marker::ADD;
  marker.color.r = 1.0f;
  marker.color.g = 0.0f;
  marker.color.b = 0.3f;
  marker.color.a = 1.0f;
  marker.lifetime = ros::Duration(1);
  //we are ready to start
  started = true;
  return true;
}

void Tracker::broadcast_tracked_object()
{
  geometry_msgs::Pose pose;
  tf::Transform trans;
  fromEigen(transform, pose, trans);
  marker.header.stamp = ros::Time();
  marker.pose = pose;
  rviz_marker_pub.publish(marker);
  tf_broadcaster.sendTransform(tf::StampedTransform(trans, ros::Time::now(), "/camera_rgb_optical_frame", std::string(name + "_tracked").c_str()));
}

void Tracker::spin_once()
{
  //process this module callbacks
  this->queue_ptr->callAvailable(ros::WallDuration(0));
}

<|endoftext|>


<commit_before>// Copyright (C) 2017 Elviss Strazdins
// This file is part of the Ouzel engine.

#include <cstdlib>
#include <cstdint>
#include "Application.h"
#include "Engine.h"
#include "CursorResource.h"

namespace ouzel
{
    ouzel::Application* sharedApplication = nullptr;

    Application::Application():
        screenSaverEnabled(false)
    {
        sharedApplication = this;
    }

    Application::Application(int aArgc, char* aArgv[]):
        argc(aArgc), argv(aArgv)
    {
        sharedApplication = this;

        for (int i = 0; i < argc; ++i)
        {
            args.push_back(argv[i]);
        }
    }

    Application::Application(const std::vector<std::string>& aArgs):
        args(aArgs)
    {
        sharedApplication = this;
    }

    Application::~Application()
    {
    }

    int Application::run()
    {
        return EXIT_SUCCESS;
    }

    void Application::exit()
    {
        active = false;

        if (sharedEngine)
        {
            sharedEngine->exit();
        }
    }

    void Application::execute(const std::function<void(void)>& func)
    {
        std::lock_guard<std::mutex> lock(executeMutex);

        executeQueue.push(func);
    }

    void Application::executeAll()
    {
        std::function<void(void)> func;

        for (;;)
        {
            {
                std::lock_guard<std::mutex> lock(executeMutex);

                if (executeQueue.empty())
                {
                    break;
                }

                func = executeQueue.front();
                executeQueue.pop();
            }

            if (func)
            {
                func();
            }
        }
    }

    bool Application::openURL(const std::string&)
    {
        return false;
    }

    void Application::setScreenSaverEnabled(bool newScreenSaverEnabled)
    {
        screenSaverEnabled = newScreenSaverEnabled;
    }

    void Application::setCurrentCursor(Cursor* cursor)
    {
        if (cursor)
        {
            currentCursor = cursor->getResource();
        }
        else
        {
            currentCursor = nullptr;
        }
    }

    CursorResource* Application::createCursorResource()
    {
        std::lock_guard<std::mutex> lock(resourceMutex);

        std::unique_ptr<CursorResource> cursorResource(new CursorResource());
        CursorResource* result = cursorResource.get();

        resources.push_back(std::move(cursorResource));

        return result;
    }

    void Application::deleteCursorResource(CursorResource* resource)
    {
        {
            std::lock_guard<std::mutex> lock(resourceMutex);

            std::vector<std::unique_ptr<CursorResource>>::iterator i = std::find_if(resources.begin(), resources.end(), [resource](const std::unique_ptr<CursorResource>& ptr) {
                return ptr.get() == resource;
            });

            if (i != resources.end())
            {
                resourceDeleteSet.push_back(std::move(*i));
                resources.erase(i);
            }

            if (resource == currentCursor) setCurrentCursor(nullptr);
        }

        execute([this] {
            std::lock_guard<std::mutex> lock(resourceMutex);
            resourceDeleteSet.clear();
        });
    }
}
<commit_msg>Add missing algorithm include<commit_after>// Copyright (C) 2017 Elviss Strazdins
// This file is part of the Ouzel engine.

#include <cstdlib>
#include <cstdint>
#include <algorithm>
#include "Application.h"
#include "Engine.h"
#include "CursorResource.h"

namespace ouzel
{
    ouzel::Application* sharedApplication = nullptr;

    Application::Application():
        screenSaverEnabled(false)
    {
        sharedApplication = this;
    }

    Application::Application(int aArgc, char* aArgv[]):
        argc(aArgc), argv(aArgv)
    {
        sharedApplication = this;

        for (int i = 0; i < argc; ++i)
        {
            args.push_back(argv[i]);
        }
    }

    Application::Application(const std::vector<std::string>& aArgs):
        args(aArgs)
    {
        sharedApplication = this;
    }

    Application::~Application()
    {
    }

    int Application::run()
    {
        return EXIT_SUCCESS;
    }

    void Application::exit()
    {
        active = false;

        if (sharedEngine)
        {
            sharedEngine->exit();
        }
    }

    void Application::execute(const std::function<void(void)>& func)
    {
        std::lock_guard<std::mutex> lock(executeMutex);

        executeQueue.push(func);
    }

    void Application::executeAll()
    {
        std::function<void(void)> func;

        for (;;)
        {
            {
                std::lock_guard<std::mutex> lock(executeMutex);

                if (executeQueue.empty())
                {
                    break;
                }

                func = executeQueue.front();
                executeQueue.pop();
            }

            if (func)
            {
                func();
            }
        }
    }

    bool Application::openURL(const std::string&)
    {
        return false;
    }

    void Application::setScreenSaverEnabled(bool newScreenSaverEnabled)
    {
        screenSaverEnabled = newScreenSaverEnabled;
    }

    void Application::setCurrentCursor(Cursor* cursor)
    {
        if (cursor)
        {
            currentCursor = cursor->getResource();
        }
        else
        {
            currentCursor = nullptr;
        }
    }

    CursorResource* Application::createCursorResource()
    {
        std::lock_guard<std::mutex> lock(resourceMutex);

        std::unique_ptr<CursorResource> cursorResource(new CursorResource());
        CursorResource* result = cursorResource.get();

        resources.push_back(std::move(cursorResource));

        return result;
    }

    void Application::deleteCursorResource(CursorResource* resource)
    {
        {
            std::lock_guard<std::mutex> lock(resourceMutex);

            std::vector<std::unique_ptr<CursorResource>>::iterator i = std::find_if(resources.begin(), resources.end(), [resource](const std::unique_ptr<CursorResource>& ptr) {
                return ptr.get() == resource;
            });

            if (i != resources.end())
            {
                resourceDeleteSet.push_back(std::move(*i));
                resources.erase(i);
            }

            if (resource == currentCursor) setCurrentCursor(nullptr);
        }

        execute([this] {
            std::lock_guard<std::mutex> lock(resourceMutex);
            resourceDeleteSet.clear();
        });
    }
}
<|endoftext|>


<commit_before>// This Source Code Form is licensed MPL-2.0: http://mozilla.org/MPL/2.0
#ifndef __AIDA_HH__
#define __AIDA_HH__

#include "aidacc/aida.hh"
#include <assert.h>

// == Macros ==
#define TEST_CPP_PASTE2(a,b)    a ## b                          // Indirection helper, required to expand macros like __LINE__
#define TEST_CPP_PASTE(a,b)     TEST_CPP_PASTE2 (a,b)           ///< Paste two macro arguments into one C symbol name
#define TEST_CPP_STRINGIFY2(s)  #s                              // Indirection helper, required to expand macros like __LINE__
#define TEST_CPP_STRINGIFY(s)   TEST_CPP_STRINGIFY2 (s)         ///< Convert macro argument into a C const char*.
#define TASSERT(cond)           assert (cond)
#define TCMP(a,op,b)            assert (a op b)

// == TestChain ==
class TestChain {
  std::string           name_;
  std::function<void()> func_;
  const TestChain      *const next_;
  static TestChain*&    global_test_chain() { static TestChain *gchain = NULL; return gchain; }
public:
  static int run       (int argc, const char **argv);
  /*ctor*/   TestChain (std::function<void()> tfunc, const std::string &tname);
};
#define TEST_ADD(func)  static const ::TestChain TEST_CPP_PASTE (__TESTS_TestChain__, __LINE__) (func, TEST_CPP_STRINGIFY (func))

// == Implementations ==
TestChain::TestChain (std::function<void()> tfunc, const std::string &tname) :
  name_ (tname), func_ (tfunc), next_ (global_test_chain())
{
  AIDA_ASSERT_RETURN (next_ == global_test_chain());
  global_test_chain() = this;
}

int
TestChain::run (int argc, const char **argv)
{
  if (argc >= 1 && argv)
    ; // printf ("  TEST     %s\n", argv[0]);
  for (const TestChain *t = global_test_chain(); t; t = t->next_)
    {
      fflush (stderr);
      printf ("  RUN…     %s\n", t->name_.c_str());
      fflush (stdout);
      t->func_();
      fflush (stderr);
      printf ("  PASS     %s\n", t->name_.c_str());
      fflush (stdout);
    }
  return 0;
}

#endif // __AIDA_HH__
<commit_msg>AIDACC: tests: execute tests in the order of declaration<commit_after>// This Source Code Form is licensed MPL-2.0: http://mozilla.org/MPL/2.0
#ifndef __AIDA_HH__
#define __AIDA_HH__

#include "aidacc/aida.hh"
#include <algorithm>
#include <cassert>

// == Macros ==
#define TEST_CPP_PASTE2(a,b)    a ## b                          // Indirection helper, required to expand macros like __LINE__
#define TEST_CPP_PASTE(a,b)     TEST_CPP_PASTE2 (a,b)           ///< Paste two macro arguments into one C symbol name
#define TEST_CPP_STRINGIFY2(s)  #s                              // Indirection helper, required to expand macros like __LINE__
#define TEST_CPP_STRINGIFY(s)   TEST_CPP_STRINGIFY2 (s)         ///< Convert macro argument into a C const char*.
#define TASSERT(cond)           assert (cond)
#define TCMP(a,op,b)            assert (a op b)

// == TestChain ==
class TestChain {
  std::string           name_;
  std::function<void()> func_;
  const TestChain      *const next_;
  static TestChain*&    global_test_chain() { static TestChain *gchain = NULL; return gchain; }
public:
  static int run       (int argc, const char **argv);
  /*ctor*/   TestChain (std::function<void()> tfunc, const std::string &tname);
};
#define TEST_ADD(func)  static const ::TestChain TEST_CPP_PASTE (__TESTS_TestChain__, __LINE__) (func, TEST_CPP_STRINGIFY (func))

// == Implementations ==
TestChain::TestChain (std::function<void()> tfunc, const std::string &tname) :
  name_ (tname), func_ (tfunc), next_ (global_test_chain())
{
  AIDA_ASSERT_RETURN (next_ == global_test_chain());
  global_test_chain() = this;
}

int
TestChain::run (int argc, const char **argv)
{
  if (argc >= 1 && argv)
    ; // printf ("  TEST     %s\n", argv[0]);
  std::vector<const TestChain*> tests;
  for (const TestChain *t = global_test_chain(); t; t = t->next_)
    tests.push_back (t);
  std::reverse (tests.begin(), tests.end());
  for (const TestChain *t : tests)
    {
      fflush (stderr);
      printf ("  RUN…     %s\n", t->name_.c_str());
      fflush (stdout);
      t->func_();
      fflush (stderr);
      printf ("  PASS     %s\n", t->name_.c_str());
      fflush (stdout);
    }
  return 0;
}

#endif // __AIDA_HH__
<|endoftext|>


<commit_before>/*
 * Mfrc.cpp
 *
 *  Created on: 18/12/2010
 *      Author: victor
 */

#include "engine/engine.h"
#include "Mfrc.h"

double Mfrc::log05 = log(0.5);

Mfrc::Mfrc() {
	noise = NULL;
	rand = NULL;
}

float Mfrc::getNoise(float x, float y, int i, int j) {
	float v39 = x * xFrequency;
	float v41 = y * yFrequency;

	double result = 0;

	switch (combination) {
	case 0:
	case 1:
		result = calculateCombination1(v39, v41);
		break;
	case 2:
		result = calculateCombination2(v39, v41);
		break;
	case 3:
		result = calculateCombination3(v39, v41);
		break;
	case 4:
		result = calculateCombination4(v39, v41);
		break;
	case 5:
		result = calculateCombination5(v39, v41);
		break;
	}

	if (bias) {
		result = pow(log(biasValue) / log05, result);
	}

	if (gainType) {
		if (result < 0.001) {
			result = 0;

			return result;
		}

		if (result > 0.999) {
			result = 1.0;

			return result;
		}

		double v40 = log(1.0 - gainValue) / log05;

		if (result < 0.5) {
			result = pow(v40, result + result) * 0.5;

			return result;
		}

		result = 1.0 - pow(v40, 1.0 - result + 1.0 - result) * 0.5;
	}

	return result;
}

void Mfrc::parseFromIffStream(engine::util::IffStream* iffStream) {
	uint32 version = iffStream->getNextFormType();

	iffStream->openForm(version);

	switch (version) {
	case '0001':
		parseFromIffStream(iffStream, Version<'0001'>());
		break;
	default:
		//System::out << "unknown MFRC version " << version << endl;
		break;
	}

	iffStream->closeForm(version);
}

void Mfrc::parseFromIffStream(engine::util::IffStream* iffStream, Version<'0001'>) {
	iffStream->openChunk('DATA');

	int seed = iffStream->getInt();
	setSeed(seed);
	////System::out << "setting seed to:" << seed << endl;
	bias = iffStream->getInt();
	////System::out << "setting bias to:" << bias << endl;

	biasValue = iffStream->getFloat();

	////System::out << "setting biasValue to:" << biasValue << endl;
	gainType = iffStream->getInt();

	//System::out << "setting gainType to:" << gainType << endl;
	gainValue = iffStream->getFloat();

	//System::out << "setting gainValue to:" << gainValue << endl;

	octaves = iffStream->getUnsignedInt();

	//System::out << "setting octaves to:" << octaves << endl;
	octavesParam = iffStream->getFloat();
	setAmplitude(iffStream->getFloat());
	//System::out << "setting amplitude to:" << amplitude << endl;

	xFrequency = iffStream->getFloat();
	yFrequency = iffStream->getFloat();
	xOffset = iffStream->getFloat();
	zOffset = iffStream->getFloat();
	combination = iffStream->getUnsignedInt();

	iffStream->closeChunk('DATA');
}

double Mfrc::calculateCombination1(float v39, float v41) {
	float v48 = 1, v47 = 1;
	double result = 0;

	float v42 = v41 + zOffset; // + 16 = z.offset
	float v36 = v39 + xOffset; // + 12 = x.offset
	float v15, v16, v33;
	double coord[2];

	for (int i = 0; i < octaves; ++i) {
		v15 = v42 * v48;
		v16 = v36 * v48;

		coord[0] = v16;
		coord[1] = v15;

		v33 = noise->noise2(coord) * v47 + v33;
		v48 = v48 * octavesParam; // + 24 octaves param
		v47 = v47 * amplitude; // + 28 amplitude
	}

	if (false) // v6 + 52 initialized to 0
		v33 = sin(v33 + v39);

	result = (v33 * offset32 + 1.0) * 0.5; //  v6 + 32 initialized to 1.0

	return result;
}

double Mfrc::calculateCombination2(float v39, float v41) {
	float v34 = 0;
	float v43 = v41 + zOffset; // + 16 = z.offset
	float v19, v18, v20;
	float v48 = 1.0;
	float v47 = 1.0;
	double result = 0;
	double coord[2];

	for (int i = 0; i < octaves; ++i) {
		v19 = v43 * v48;
		v18 = v39 + xOffset; //  + 12 = x.offset
		v20 = v18 * v48;

		coord[0] = v20;
		coord[1] = v19;

		v34 = (1.0 - fabs(noise->noise2(coord))) * v47 + v34;
		v48 = v48 * octavesParam;  // + 24 octaves param
		v47 = v47 * amplitude; // + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}

double Mfrc::calculateCombination3(float v39, float v41) {
	float v48 = 1.0;
	float v47 = 1.0;
	double result = 0;
	float v34 = 0;
	float v44 = v41 + zOffset; // + 16 = z.offset
	float v23, v22, v24;

	double coord[2];

	for (int i = 0; i < octaves; ++i) {
		v23 = v44 * v48;
		v22 = v39 + xOffset; //  + 12 = x.offset
		v24 = v22 * v48;

		coord[0] = v24;
		coord[1] = v23;

		v34 = fabs(noise->noise2(coord)) * v47 + v34;
		v48 = v48 * octavesParam; // + 24 octaves param
		v47 = v47 * amplitude;  // + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}

double Mfrc::calculateCombination4(float v39, float v41) {
	float v34 = 0;
	float v27, v28, v26;
	float v45 = v41 + zOffset; // + 16 = z.offset
	float v37 = v39 + xOffset; //  + 12 = x.offset
	double coord[2];

	float v48 = 1.0;
	float v47 = 1.0;
	double result = 0;

	for (int i = 0; i < octaves; ++i) {
		v27 = v45 * v48;
		v28 = v37 * v48;

		coord[0] = v28;
		coord[1] = v27;

		v26 = noise->noise2(coord);
		if ( v26 >= 0.0 ) {
			if ( v26 > 1.0 )
				v26 = 1.0;
		} else {
			v26 = 0.0;
		}

		v34 = (1.0 - v26) * v47 + v34;
		v48 = v48 * octavesParam; // + 24 octaves param
		v47 = v47 * amplitude; // + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}

double Mfrc::calculateCombination5(float v39, float v41) {
	float v34 = 0;
	float v31, v32, v30;
	float v46 = v41 + zOffset; //  + 16 = z.offset
	float v38 = v39 + xOffset; //  + 12 = x.offset
	double coord[2];
	double result = 0;

	float v48 = 1.0;
	float v47 = 1.0;

	for (int i = 0; i < octaves; ++i) {
		v31 = v46 * v48;
		v32 = v38 * v48;

		coord[0] = v32;
		coord[1] = v31;

		v30 = noise->noise2(coord); //

		if ( v30 >= 0.0 ) {
			if (v30 > 1.0)
				v30 = 1.0;
		} else {
			v30 = 0.0;
		}

		v34 = v30 * v47 + v34;
		v48 = v48 * octavesParam; // v6 + 24 octaves param
		v47 = v47 * amplitude; // v6 + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}
<commit_msg>[fixed] Mfrc<commit_after>/*
 * Mfrc.cpp
 *
 *  Created on: 18/12/2010
 *      Author: victor
 */

#include "engine/engine.h"
#include "Mfrc.h"

double Mfrc::log05 = log(0.5);

Mfrc::Mfrc() {
	noise = NULL;
	rand = NULL;
}

float Mfrc::getNoise(float x, float y, int i, int j) {
	float v39 = x * xFrequency;
	float v41 = y * yFrequency;

	double result = 0;

	switch (combination) {
	case 0:
	case 1:
		result = calculateCombination1(v39, v41);
		break;
	case 2:
		result = calculateCombination2(v39, v41);
		break;
	case 3:
		result = calculateCombination3(v39, v41);
		break;
	case 4:
		result = calculateCombination4(v39, v41);
		break;
	case 5:
		result = calculateCombination5(v39, v41);
		break;
	}

	if (bias) {
		result = pow(log(biasValue) / log05, result);
	}

	if (gainType) {
		if (result < 0.001) {
			result = 0;

			return result;
		}

		if (result > 0.999) {
			result = 1.0;

			return result;
		}

		double v40 = log(1.0 - gainValue) / log05;

		if (result < 0.5) {
			result = pow(v40, result + result) * 0.5;

			return result;
		}

		result = 1.0 - pow(v40, 1.0 - result + 1.0 - result) * 0.5;
	}

	return result;
}

void Mfrc::parseFromIffStream(engine::util::IffStream* iffStream) {
	uint32 version = iffStream->getNextFormType();

	iffStream->openForm(version);

	switch (version) {
	case '0001':
		parseFromIffStream(iffStream, Version<'0001'>());
		break;
	default:
		//System::out << "unknown MFRC version " << version << endl;
		break;
	}

	iffStream->closeForm(version);
}

void Mfrc::parseFromIffStream(engine::util::IffStream* iffStream, Version<'0001'>) {
	iffStream->openChunk('DATA');

	int seed = iffStream->getInt();
	setSeed(seed);
	////System::out << "setting seed to:" << seed << endl;
	bias = iffStream->getInt();
	////System::out << "setting bias to:" << bias << endl;

	biasValue = iffStream->getFloat();

	////System::out << "setting biasValue to:" << biasValue << endl;
	gainType = iffStream->getInt();

	//System::out << "setting gainType to:" << gainType << endl;
	gainValue = iffStream->getFloat();

	//System::out << "setting gainValue to:" << gainValue << endl;

	octaves = iffStream->getUnsignedInt();

	//System::out << "setting octaves to:" << octaves << endl;
	octavesParam = iffStream->getFloat();
	setAmplitude(iffStream->getFloat());
	//System::out << "setting amplitude to:" << amplitude << endl;

	xFrequency = iffStream->getFloat();
	yFrequency = iffStream->getFloat();
	xOffset = iffStream->getFloat();
	zOffset = iffStream->getFloat();
	combination = iffStream->getUnsignedInt();

	iffStream->closeChunk('DATA');
}

double Mfrc::calculateCombination1(float v39, float v41) {
	float v48 = 1, v47 = 1;
	double result = 0;

	float v42 = v41 + zOffset; // + 16 = z.offset
	float v36 = v39 + xOffset; // + 12 = x.offset
	float v15, v16, v33 = 0;
	double coord[2];

	for (int i = 0; i < octaves; ++i) {
		v15 = v42 * v48;
		v16 = v36 * v48;

		coord[0] = v16;
		coord[1] = v15;

		v33 = noise->noise2(coord) * v47 + v33;
		v48 = v48 * octavesParam; // + 24 octaves param
		v47 = v47 * amplitude; // + 28 amplitude
	}

	if (false) // v6 + 52 initialized to 0
		v33 = sin(v33 + v39);

	result = (v33 * offset32 + 1.0) * 0.5; //  v6 + 32 initialized to 1.0

	return result;
}

double Mfrc::calculateCombination2(float v39, float v41) {
	float v34 = 0;
	float v43 = v41 + zOffset; // + 16 = z.offset
	float v19, v18, v20;
	float v48 = 1.0;
	float v47 = 1.0;
	double result = 0;
	double coord[2];

	for (int i = 0; i < octaves; ++i) {
		v19 = v43 * v48;
		v18 = v39 + xOffset; //  + 12 = x.offset
		v20 = v18 * v48;

		coord[0] = v20;
		coord[1] = v19;

		v34 = (1.0 - fabs(noise->noise2(coord))) * v47 + v34;
		v48 = v48 * octavesParam;  // + 24 octaves param
		v47 = v47 * amplitude; // + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}

double Mfrc::calculateCombination3(float v39, float v41) {
	float v48 = 1.0;
	float v47 = 1.0;
	double result = 0;
	float v34 = 0;
	float v44 = v41 + zOffset; // + 16 = z.offset
	float v23, v22, v24;

	double coord[2];

	for (int i = 0; i < octaves; ++i) {
		v23 = v44 * v48;
		v22 = v39 + xOffset; //  + 12 = x.offset
		v24 = v22 * v48;

		coord[0] = v24;
		coord[1] = v23;

		v34 = fabs(noise->noise2(coord)) * v47 + v34;
		v48 = v48 * octavesParam; // + 24 octaves param
		v47 = v47 * amplitude;  // + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}

double Mfrc::calculateCombination4(float v39, float v41) {
	float v34 = 0;
	float v27, v28, v26;
	float v45 = v41 + zOffset; // + 16 = z.offset
	float v37 = v39 + xOffset; //  + 12 = x.offset
	double coord[2];

	float v48 = 1.0;
	float v47 = 1.0;
	double result = 0;

	for (int i = 0; i < octaves; ++i) {
		v27 = v45 * v48;
		v28 = v37 * v48;

		coord[0] = v28;
		coord[1] = v27;

		v26 = noise->noise2(coord);
		if ( v26 >= 0.0 ) {
			if ( v26 > 1.0 )
				v26 = 1.0;
		} else {
			v26 = 0.0;
		}

		v34 = (1.0 - v26) * v47 + v34;
		v48 = v48 * octavesParam; // + 24 octaves param
		v47 = v47 * amplitude; // + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}

double Mfrc::calculateCombination5(float v39, float v41) {
	float v34 = 0;
	float v31, v32, v30;
	float v46 = v41 + zOffset; //  + 16 = z.offset
	float v38 = v39 + xOffset; //  + 12 = x.offset
	double coord[2];
	double result = 0;

	float v48 = 1.0;
	float v47 = 1.0;

	for (int i = 0; i < octaves; ++i) {
		v31 = v46 * v48;
		v32 = v38 * v48;

		coord[0] = v32;
		coord[1] = v31;

		v30 = noise->noise2(coord); //

		if ( v30 >= 0.0 ) {
			if (v30 > 1.0)
				v30 = 1.0;
		} else {
			v30 = 0.0;
		}

		v34 = v30 * v47 + v34;
		v48 = v48 * octavesParam; // v6 + 24 octaves param
		v47 = v47 * amplitude; // v6 + 28 amplitude
	}

	if (false) //v6 + 52 initialized to 0
		v34 = sin(v34 + v39);

	result = v34 * offset32; //

	return result;
}
<|endoftext|>


<commit_before>#pragma once

#include <atomic>

namespace BlindingTechnologies
{
	namespace BlindingEngine
	{
		inline namespace Version_1
		{
			namespace Private
			{
				class SpinLock final
				{
				public:
					// Constructors
					inline SpinLock(void)
						: atomicLock(false)
					{
						return;
					}

					SpinLock(SpinLock const &) = default;

					SpinLock(SpinLock &&) = default;

					// Assignment Operators
					SpinLock &operator=(SpinLock const &) = default;

					SpinLock &operator=(SpinLock &&) = default;

					// Destructor
					~SpinLock(void) = default;

					// Methods
					inline void lock(void)
					{
						while (atomicLock.exchange(true));
					}

					inline void lock(void) volatile
					{
						while (atomicLock.exchange(true));
					}

					inline void unlock(void)
					{
						atomicLock = false;
					}

					inline void unlock(void) volatile
					{
						atomicLock = false;
					}

					inline bool isLocked(void) const
					{
						return atomicLock;
					}

				private:
					// Attributes
					std::atomic_bool atomicLock;
				};
			}
		}
	}
}<commit_msg>Update SpinLock.hpp<commit_after>#pragma once

#include <atomic>

namespace BlindingTechnologies
{
	namespace BlindingEngine
	{
		inline namespace Version_1
		{
			namespace Private
			{
				class SpinLock final
				{
				public:
					// Constructors
					inline SpinLock(void)
						: atomicLock(false)
					{
						return;
					}

					SpinLock(SpinLock const &) = default;

					SpinLock(SpinLock &&) = default;

					// Assignment Operators
					SpinLock &operator=(SpinLock const &) = default;

					SpinLock &operator=(SpinLock &&) = default;

					// Destructor
					~SpinLock(void) = default;

					// Methods
					inline void lock(void)
					{
						while (atomicLock.exchange(true));
					}

					inline void unlock(void)
					{
						atomicLock = false;
					}

					inline bool isLocked(void) const
					{
						return atomicLock;
					}

				private:
					// Attributes
					std::atomic_bool atomicLock;
				};
			}
		}
	}
}<|endoftext|>


<commit_before>#include "stdafx.h"
#include "Game.h"
#include "EntityManager.h"
#include "RenderMap.h"
#include "PositionMap.h"
#include "CollisionMap.h"
#include "TextureManager.h"
#include <sstream>
#include <cstdlib>

/*
_______________________________________
---------------TODO LIST---------------
_______________________________________

1. Pan and zoom
2. User interface
3. player object
4. projectile object
5. game states
6. main menu
7. in-game menu
8. maps - static and procdually generated

_______________________________________
-----------------IDEAS-----------------
_______________________________________

gavity field view
-many vectors (drawn as lines) that show gravy field on map
-dotted line trails for objects

*/

void Game::start() {

	_mainWindow.create(sf::VideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, 32), "Title");

	gameLoop();

	_mainWindow.close();
}

sf::RenderWindow& Game::GetWindow() {
	return Game::_mainWindow;
}

RenderMap& Game::getRenderMap() {
	return Game::_renderMap;
}

PositionMap& Game::getPositionMap() {
	return _positionMap;
}

CollisionMap& Game::getCollisionMap() {
	return _collisionMap;
}

TextureManager& Game::getTextureManager() {
	return _textureManager;
}

EntityManager& Game::getEntityManager() {
	return _entityManager;
}

int Game::getScreenWidth() {
	return Game::_mainWindow.getSize().x;
}

int Game::getScreenHeight() {
	return Game::_mainWindow.getSize().y;
}

void Game::gameLoop() {

	for (int i = 0; i < 30; i++){
		int x = rand() % 1000;
		int y = rand() % 700;
		_entityManager.addPlanetEntity(sf::Vector2f(x, y), sf::Vector2f(x/100, y/100), 100, 10);
	}

	while (GetWindow().isOpen())
    {
        sf::Event event;
        while (GetWindow().pollEvent(event))
        {
           switch (event.type) {
				case sf::Event::Closed:
					GetWindow().close();
					break;
			}
        }

        GetWindow().clear();

		auto deltaTime = _clock.restart();
		_positionMap.updateGravity();
		_collisionMap.resolveCollisions();
		_positionMap.updateComponents(deltaTime.asSeconds());
		_renderMap.drawAll();

        GetWindow().display();

		//update title
		ostringstream ss;
		ss << 1/deltaTime.asSeconds();
		string s1(ss.str());
		ostringstream sss;
		sss << _entityManager.numEntities();
		string s2(sss.str());
		string s("Title " + s1 + "  " + s2);
		GetWindow().setTitle(s);
    }

}

sf::RenderWindow Game::_mainWindow;
sf::Clock Game::_clock;
EntityManager  Game::_entityManager;
RenderMap  Game::_renderMap;
PositionMap Game::_positionMap;
CollisionMap Game::_collisionMap;
TextureManager Game::_textureManager;<commit_msg>minor change<commit_after>#include "stdafx.h"
#include "Game.h"
#include "EntityManager.h"
#include "RenderMap.h"
#include "PositionMap.h"
#include "CollisionMap.h"
#include "TextureManager.h"
#include <sstream>
#include <cstdlib>

/*
_______________________________________
---------------TODO LIST---------------
_______________________________________

1. Pan and zoom
2. User interface
3. player object
4. projectile object
5. game states
6. main menu
7. in-game menu
8. maps - static and procdually generated

_______________________________________
-----------------IDEAS-----------------
_______________________________________

gavity field view
-many vectors (drawn as lines) that show gravy field on map
-dotted line trails for objects

*/

void Game::start() {

	_mainWindow.create(sf::VideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, 32), "Title");

	gameLoop();

	_mainWindow.close();
}

sf::RenderWindow& Game::GetWindow() {
	return Game::_mainWindow;
}

RenderMap& Game::getRenderMap() {
	return Game::_renderMap;
}

PositionMap& Game::getPositionMap() {
	return _positionMap;
}

CollisionMap& Game::getCollisionMap() {
	return _collisionMap;
}

TextureManager& Game::getTextureManager() {
	return _textureManager;
}

EntityManager& Game::getEntityManager() {
	return _entityManager;
}

int Game::getScreenWidth() {
	return Game::_mainWindow.getSize().x;
}

int Game::getScreenHeight() {
	return Game::_mainWindow.getSize().y;
}

void Game::gameLoop() {

	for (int i = 0; i < 30; i++){
		int x = rand() % 1000;
		int y = rand() % 700;
		_entityManager.addPlanetEntity(sf::Vector2f(x, y), sf::Vector2f(x/100, y/100), 500, 5);
	}

	while (GetWindow().isOpen())
    {
        sf::Event event;
        while (GetWindow().pollEvent(event))
        {
           switch (event.type) {
				case sf::Event::Closed:
					GetWindow().close();
					break;
			}
        }

        GetWindow().clear();

		auto deltaTime = _clock.restart();
		_positionMap.updateGravity();
		_collisionMap.resolveCollisions();
		_positionMap.updateComponents(deltaTime.asSeconds());
		_renderMap.drawAll();

        GetWindow().display();

		//update title
		ostringstream ss;
		ss << 1/deltaTime.asSeconds();
		string s1(ss.str());
		ostringstream sss;
		sss << _entityManager.numEntities();
		string s2(sss.str());
		string s("Title " + s1 + "  " + s2);
		GetWindow().setTitle(s);
    }

}

sf::RenderWindow Game::_mainWindow;
sf::Clock Game::_clock;
EntityManager  Game::_entityManager;
RenderMap  Game::_renderMap;
PositionMap Game::_positionMap;
CollisionMap Game::_collisionMap;
TextureManager Game::_textureManager;<|endoftext|>


<commit_before>namespace factor
{

inline static void memset_cell(void *dst, cell pattern, size_t size)
{
#ifdef __APPLE__
	#ifdef FACTOR_64
		memset_pattern8(dst,&pattern,size);
	#else
		memset_pattern4(dst,&pattern,size);
	#endif
#else
	if(pattern == 0)
		memset(dst,0,size);
	else
	{
		cell *start = (cell *)dst;
		cell *end = (cell *)((cell)dst + size);
		while(start < end)
		{
			*start = fill;
			start++;
		}
	}
#endif
}

vm_char *safe_strdup(const vm_char *str);
void print_string(const char *str);
void nl();
void print_cell(cell x);
void print_cell_hex(cell x);
void print_cell_hex_pad(cell x);
void print_fixnum(fixnum x);
cell read_cell_hex();

}
<commit_msg>vm: fix compile error on non-Mac OS X platforms<commit_after>namespace factor
{

inline static void memset_cell(void *dst, cell pattern, size_t size)
{
#ifdef __APPLE__
	#ifdef FACTOR_64
		memset_pattern8(dst,&pattern,size);
	#else
		memset_pattern4(dst,&pattern,size);
	#endif
#else
	if(pattern == 0)
		memset(dst,0,size);
	else
	{
		cell *start = (cell *)dst;
		cell *end = (cell *)((cell)dst + size);
		while(start < end)
		{
			*start = pattern;
			start++;
		}
	}
#endif
}

vm_char *safe_strdup(const vm_char *str);
void print_string(const char *str);
void nl();
void print_cell(cell x);
void print_cell_hex(cell x);
void print_cell_hex_pad(cell x);
void print_fixnum(fixnum x);
cell read_cell_hex();

}
<|endoftext|>


<commit_before>//============================================================================
// Name        : glogin.cpp
// Author      : Michael Gentili
// Version     :
// Copyright   : Copyright 2015
// Description : Hello World in C, Ansi-style
//============================================================================

#include <SDL.h>
#include <iostream>

using namespace std;

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

	if (SDL_Init(0) != 0) {
		cerr << "ERROR: SDL_Init() - " << SDL_GetError() << endl;
		return 1;
	}
	if (SDL_VideoInit("directfb") != 0) {
                cerr << "WARN: No directfb" << endl;
		if (SDL_VideoInit(NULL) != 0) {
			cerr << "ERROR: SDL_VideoInit() - " << SDL_GetError() << endl;
			return 1;
		}

	}

        SDL_DisplayMode mode;
        for (int d = 0; d < SDL_GetNumVideoDisplays(); d++) {
            for (int m = 0; m < SDL_GetNumDisplayModes(0); m++) {
                SDL_GetDisplayMode(d,m,&mode);
                cout << d << ":" << mode.w << "," << mode.h << " " << mode.format << endl;
                if ((mode.w == 640) && (mode.h == 480)) {
                    break;
                }
            }
        }

	SDL_Window * win = SDL_CreateWindow("glogin", SDL_WINDOWPOS_CENTERED,
			SDL_WINDOWPOS_CENTERED, mode.w, mode.h, SDL_WINDOW_FULLSCREEN);
	if (win == NULL) {
		cerr << "ERROR: SDL_CreateWindow() - " << SDL_GetError() << endl;
                SDL_VideoQuit();
                SDL_Quit();
		return 1;
	}
        if (SDL_SetWindowDisplayMode(win,&mode) != 0) {
		cerr << "ERROR: SDL_SetWindowDisplayMode() - " << SDL_GetError() << endl;
                SDL_VideoQuit();
                SDL_Quit();
		return 1;
        }
        if (SDL_SetWindowFullscreen(win,SDL_WINDOW_FULLSCREEN) != 0) {
		cerr << "ERROR: SDL_SetWindowFullscreen() - " << SDL_GetError() << endl;
                SDL_VideoQuit();
                SDL_Quit();
		return 1;
        }
	SDL_Renderer * rnd = SDL_CreateRenderer(win,-1,SDL_RENDERER_SOFTWARE);
	if (rnd == NULL) {
		cerr << "ERROR: SDL_CreateRenderer() - " << SDL_GetError() << endl;
                SDL_VideoQuit();
                SDL_Quit();
		return 1;
	}
	SDL_SetRenderDrawColor(rnd,100,0,0,255);
	SDL_RenderClear(rnd);
	SDL_RenderPresent(rnd);

	// Main loop
	bool isRunning = true;
	cout << "start" << endl;
	while (isRunning) {
		SDL_Event event;
		if (SDL_PollEvent(&event) && event.type == SDL_QUIT) {
			cout << "quit" << endl;
			isRunning = false;
		}
	}
	cout << "done" << endl;
	SDL_VideoQuit();
	SDL_Quit();
	return 0;
}
<commit_msg>Reformat with spaces not tabs<commit_after>//============================================================================
// Name        : glogin.cpp
// Author      : Michael Gentili
// Version     :
// Copyright   : Copyright 2015
// Description : Hello World in C, Ansi-style
//============================================================================

#include <SDL.h>
#include <iostream>

using namespace std;

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

    if (SDL_Init(0) != 0) {
        cerr << "ERROR: SDL_Init() - " << SDL_GetError() << endl;
        return 1;
    }
    if (SDL_VideoInit("directfb") != 0) {
        cerr << "WARN: No directfb" << endl;
        if (SDL_VideoInit(NULL) != 0) {
            cerr << "ERROR: SDL_VideoInit() - " << SDL_GetError() << endl;
            return 1;
        }

    }

    SDL_DisplayMode mode;
    for (int d = 0; d < SDL_GetNumVideoDisplays(); d++) {
        for (int m = 0; m < SDL_GetNumDisplayModes(0); m++) {
            SDL_GetDisplayMode(d, m, &mode);
            cout << d << ":" << mode.w << "," << mode.h << " " << mode.format
                    << endl;
            if ((mode.w == 640) && (mode.h == 480)) {
                break;
            }
        }
    }

    SDL_Window * win = SDL_CreateWindow("glogin", SDL_WINDOWPOS_CENTERED,
    SDL_WINDOWPOS_CENTERED, mode.w, mode.h, SDL_WINDOW_FULLSCREEN);
    if (win == NULL) {
        cerr << "ERROR: SDL_CreateWindow() - " << SDL_GetError() << endl;
        SDL_VideoQuit();
        SDL_Quit();
        return 1;
    }
    if (SDL_SetWindowDisplayMode(win, &mode) != 0) {
        cerr << "ERROR: SDL_SetWindowDisplayMode() - " << SDL_GetError()
                << endl;
        SDL_VideoQuit();
        SDL_Quit();
        return 1;
    }
    if (SDL_SetWindowFullscreen(win, SDL_WINDOW_FULLSCREEN) != 0) {
        cerr << "ERROR: SDL_SetWindowFullscreen() - " << SDL_GetError() << endl;
        SDL_VideoQuit();
        SDL_Quit();
        return 1;
    }
    SDL_Renderer * rnd = SDL_CreateRenderer(win, -1, SDL_RENDERER_SOFTWARE);
    if (rnd == NULL) {
        cerr << "ERROR: SDL_CreateRenderer() - " << SDL_GetError() << endl;
        SDL_VideoQuit();
        SDL_Quit();
        return 1;
    }
    SDL_SetRenderDrawColor(rnd, 100, 0, 0, 255);
    SDL_RenderClear(rnd);
    SDL_RenderPresent(rnd);

    // Main loop
    bool isRunning = true;
    cout << "start" << endl;
    while (isRunning) {
        SDL_Event event;
        if (SDL_PollEvent(&event) && event.type == SDL_QUIT) {
            cout << "quit" << endl;
            isRunning = false;
        }
    }
    cout << "done" << endl;
    SDL_VideoQuit();
    SDL_Quit();
    return 0;
}
<|endoftext|>


<commit_before>// stdlib includes
#include <iterator>
#include <sstream>
#include <algorithm>
#include <math.h>

// Utils-Jsonschema includes
#include <utils/jsonschema/QJsonSchemaChecker.h>

QJsonSchemaChecker::QJsonSchemaChecker()
{
	// empty
}

QJsonSchemaChecker::~QJsonSchemaChecker()
{
	// empty
}

bool QJsonSchemaChecker::setSchema(const QJsonObject & schema)
{
	_qSchema = schema;

	// TODO: check the schema

	return true;
}

bool QJsonSchemaChecker::validate(const QJsonObject & value)
{
	// initialize state
	_error = false;
	_messages.clear();
	_currentPath.clear();
	_currentPath.push_back("[root]");

	// validate
	validate(value, _qSchema);

	return !_error;
}

void QJsonSchemaChecker::validate(const QJsonValue & value, const QJsonObject &schema)
{
	// check the current json value
	for (QJsonObject::const_iterator i = schema.begin(); i != schema.end(); ++i)
	{
		QString attribute = i.key();
		const QJsonValue & attributeValue = *i;

		if (attribute == "type")
			checkType(value, attributeValue);
		else if (attribute == "properties")
		{
			if (value.isObject())
				checkProperties(value.toObject(), attributeValue.toObject());
			else
			{
				_error = true;
				setMessage("properties attribute is only valid for objects");
				continue;
			}
		}
		else if (attribute == "additionalProperties")
		{
			if (value.isObject())
			{
				// ignore the properties which are handled by the properties attribute (if present)
				QStringList ignoredProperties;
				if (schema.contains("properties")) {
					const QJsonObject & props = schema["properties"].toObject();
					ignoredProperties = props.keys();
				}

				checkAdditionalProperties(value.toObject(), attributeValue, ignoredProperties);
			}
			else
			{
				_error = true;
				setMessage("additional properties attribute is only valid for objects");
				continue;
			}
		}
		else if (attribute == "minimum")
			checkMinimum(value, attributeValue);
		else if (attribute == "maximum")
			checkMaximum(value, attributeValue);
		else if (attribute == "items")
		{
			if (value.isArray())
				checkItems(value, attributeValue.toObject());
			else
			{
				_error = true;
				setMessage("items only valid for arrays");
				continue;
			}
		}
		else if (attribute == "minItems")
			checkMinItems(value, attributeValue);
		else if (attribute == "maxItems")
			checkMaxItems(value, attributeValue);
		else if (attribute == "uniqueItems")
			checkUniqueItems(value, attributeValue);
		else if (attribute == "enum")
			checkEnum(value, attributeValue);
 		else if (attribute == "required")
 			; // nothing to do. value is present so always oke
 		else if (attribute == "id")
 			; // references have already been collected
 		else if (attribute == "title" || attribute == "description"  || attribute == "default" || attribute == "format"
			|| attribute == "defaultProperties" || attribute == "propertyOrder")
 			; // nothing to do.
		else
		{
			// no check function defined for this attribute
			setMessage(std::string("No check function defined for attribute ") + attribute.toStdString());
			continue;
		}
	}
}

void QJsonSchemaChecker::setMessage(const std::string & message)
{
	std::ostringstream oss;
	std::copy(_currentPath.begin(), _currentPath.end(), std::ostream_iterator<std::string>(oss, ""));
	oss << ": " << message;
	_messages.push_back(oss.str());
}

const std::list<std::string> & QJsonSchemaChecker::getMessages() const
{
	return _messages;
}

void QJsonSchemaChecker::checkType(const QJsonValue & value, const QJsonValue & schema)
{
	QString type = schema.toString();

	bool wrongType = false;
	if (type == "string")
		wrongType = !value.isString();
	else if (type == "number")
		wrongType = !value.isDouble();
	else if (type == "integer")
		wrongType = (rint(value.toDouble()) != value.toDouble());
	else if (type == "double")
		wrongType = !value.isDouble();
	else if (type == "boolean")
		wrongType = !value.isBool();
	else if (type == "object")
		wrongType = !value.isObject();
	else if (type == "array")
		wrongType = !value.isArray();
	else if (type == "null")
		wrongType = !value.isNull();
	else if (type == "enum")
		wrongType = !value.isString();
	else if (type == "any")
		wrongType = false;
//	else
//		assert(false);

	if (wrongType)
	{
		_error = true;
		setMessage(type.toStdString() + " expected");
	}
}

void QJsonSchemaChecker::checkProperties(const QJsonObject & value, const QJsonObject & schema)
{
	for (QJsonObject::const_iterator i = schema.begin(); i != schema.end(); ++i)
	{
		QString property = i.key();

		const QJsonValue  & propertyValue = i.value();

		_currentPath.push_back(std::string(".") + property.toStdString());
		QJsonObject::const_iterator required = propertyValue.toObject().find("required");

		if (value.contains(property))
		{
			validate(value[property], propertyValue.toObject());
		}
		else if (required != propertyValue.toObject().end() && required.value().toBool())
		{
			_error = true;
			setMessage("missing member");
		}
		_currentPath.pop_back();
	}
}

void QJsonSchemaChecker::checkAdditionalProperties(const QJsonObject & value, const QJsonValue & schema, const QStringList & ignoredProperties)
{
	for (QJsonObject::const_iterator i = value.begin(); i != value.end(); ++i)
	{
		QString property = i.key();
		if (std::find(ignoredProperties.begin(), ignoredProperties.end(), property) == ignoredProperties.end())
		{
			// property has no property definition. check against the definition for additional properties
			_currentPath.push_back(std::string(".") + property.toStdString());
			if (schema.isBool())
			{
				if (schema.toBool() == false)
				{
					_error = true;
					setMessage("no schema definition");
				}
			}
			else
			{
				validate(value[property].toObject(), schema.toObject());
			}
			_currentPath.pop_back();
		}
	}
}

void QJsonSchemaChecker::checkMinimum(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isDouble())
	{
		// only for numeric
		_error = true;
		setMessage("minimum check only for numeric fields");
		return;
	}

	if (value.toDouble() < schema.toDouble())
	{
		_error = true;
		std::ostringstream oss;
		oss << "value is too small (minimum=" << schema.toDouble() << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkMaximum(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isDouble())
	{
		// only for numeric
		_error = true;
		setMessage("maximum check only for numeric fields");
		return;
	}

	if (value.toDouble() > schema.toDouble())
	{
		_error = true;
		std::ostringstream oss;
		oss << "value is too large (maximum=" << schema.toDouble() << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkItems(const QJsonValue & value, const QJsonObject & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("items only valid for arrays");
		return;
	}

	QJsonArray jArray = value.toArray();
	for(int i = 0; i < jArray.size(); ++i)
	{
		// validate each item
		std::ostringstream oss;
		oss << "[" << i << "]";
		_currentPath.push_back(oss.str());
		validate(jArray[i].toObject(), schema);
		_currentPath.pop_back();
	}
}

void QJsonSchemaChecker::checkMinItems(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("minItems only valid for arrays");
		return;
	}

	int minimum = schema.toInt();

	QJsonArray jArray = value.toArray();
	if (static_cast<int>(jArray.size()) < minimum)
	{
		_error = true;
		std::ostringstream oss;
		oss << "array is too small (minimum=" << minimum << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkMaxItems(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("maxItems only valid for arrays");
		return;
	}

	int maximum = schema.toInt();

	QJsonArray jArray = value.toArray();
	if (static_cast<int>(jArray.size()) > maximum)
	{
		_error = true;
		std::ostringstream oss;
		oss << "array is too large (maximum=" << maximum << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkUniqueItems(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("uniqueItems only valid for arrays");
		return;
	}

	if (schema.toBool() == true)
	{
		// make sure no two items are identical

		QJsonArray jArray = value.toArray();
		for(int i = 0; i < jArray.size(); ++i)
		{
			for (int j = i+1; j < jArray.size(); ++j)
			{
				if (jArray[i] == jArray[j])
				{
					// found a value twice
					_error = true;
					setMessage("array must have unique values");
				}
			}
		}
	}
}

void QJsonSchemaChecker::checkEnum(const QJsonValue & value, const QJsonValue & schema)
{
	if (schema.isArray())
	{
		QJsonArray jArray = schema.toArray();
		for(int i = 0; i < jArray.size(); ++i)
		{
			if (jArray[i] == value)
			{
				// found enum value. done.
				return;
			}
		}
	}

	// nothing found
	_error = true;
	std::ostringstream oss;
	oss << "Unknown enum value (allowed values are: " << schema.toString().toStdString();
	QJsonDocument doc(schema.toArray());
	QString strJson(doc.toJson(QJsonDocument::Compact));
	oss << strJson.toStdString() << ")";
	setMessage(oss.str());
}
<commit_msg>hot fixing missing schema attribut append<commit_after>// stdlib includes
#include <iterator>
#include <sstream>
#include <algorithm>
#include <math.h>

// Utils-Jsonschema includes
#include <utils/jsonschema/QJsonSchemaChecker.h>

QJsonSchemaChecker::QJsonSchemaChecker()
{
	// empty
}

QJsonSchemaChecker::~QJsonSchemaChecker()
{
	// empty
}

bool QJsonSchemaChecker::setSchema(const QJsonObject & schema)
{
	_qSchema = schema;

	// TODO: check the schema

	return true;
}

bool QJsonSchemaChecker::validate(const QJsonObject & value)
{
	// initialize state
	_error = false;
	_messages.clear();
	_currentPath.clear();
	_currentPath.push_back("[root]");

	// validate
	validate(value, _qSchema);

	return !_error;
}

void QJsonSchemaChecker::validate(const QJsonValue & value, const QJsonObject &schema)
{
	// check the current json value
	for (QJsonObject::const_iterator i = schema.begin(); i != schema.end(); ++i)
	{
		QString attribute = i.key();
		const QJsonValue & attributeValue = *i;

		if (attribute == "type")
			checkType(value, attributeValue);
		else if (attribute == "properties")
		{
			if (value.isObject())
				checkProperties(value.toObject(), attributeValue.toObject());
			else
			{
				_error = true;
				setMessage("properties attribute is only valid for objects");
				continue;
			}
		}
		else if (attribute == "additionalProperties")
		{
			if (value.isObject())
			{
				// ignore the properties which are handled by the properties attribute (if present)
				QStringList ignoredProperties;
				if (schema.contains("properties")) {
					const QJsonObject & props = schema["properties"].toObject();
					ignoredProperties = props.keys();
				}

				checkAdditionalProperties(value.toObject(), attributeValue, ignoredProperties);
			}
			else
			{
				_error = true;
				setMessage("additional properties attribute is only valid for objects");
				continue;
			}
		}
		else if (attribute == "minimum")
			checkMinimum(value, attributeValue);
		else if (attribute == "maximum")
			checkMaximum(value, attributeValue);
		else if (attribute == "items")
		{
			if (value.isArray())
				checkItems(value, attributeValue.toObject());
			else
			{
				_error = true;
				setMessage("items only valid for arrays");
				continue;
			}
		}
		else if (attribute == "minItems")
			checkMinItems(value, attributeValue);
		else if (attribute == "maxItems")
			checkMaxItems(value, attributeValue);
		else if (attribute == "uniqueItems")
			checkUniqueItems(value, attributeValue);
		else if (attribute == "enum")
			checkEnum(value, attributeValue);
 		else if (attribute == "required")
 			; // nothing to do. value is present so always oke
 		else if (attribute == "id")
 			; // references have already been collected
 		else if (attribute == "title" || attribute == "description"  || attribute == "default" || attribute == "format"
			|| attribute == "defaultProperties" || attribute == "propertyOrder" || attribute == "append")
 			; // nothing to do.
		else
		{
			// no check function defined for this attribute
			setMessage(std::string("No check function defined for attribute ") + attribute.toStdString());
			continue;
		}
	}
}

void QJsonSchemaChecker::setMessage(const std::string & message)
{
	std::ostringstream oss;
	std::copy(_currentPath.begin(), _currentPath.end(), std::ostream_iterator<std::string>(oss, ""));
	oss << ": " << message;
	_messages.push_back(oss.str());
}

const std::list<std::string> & QJsonSchemaChecker::getMessages() const
{
	return _messages;
}

void QJsonSchemaChecker::checkType(const QJsonValue & value, const QJsonValue & schema)
{
	QString type = schema.toString();

	bool wrongType = false;
	if (type == "string")
		wrongType = !value.isString();
	else if (type == "number")
		wrongType = !value.isDouble();
	else if (type == "integer")
		wrongType = (rint(value.toDouble()) != value.toDouble());
	else if (type == "double")
		wrongType = !value.isDouble();
	else if (type == "boolean")
		wrongType = !value.isBool();
	else if (type == "object")
		wrongType = !value.isObject();
	else if (type == "array")
		wrongType = !value.isArray();
	else if (type == "null")
		wrongType = !value.isNull();
	else if (type == "enum")
		wrongType = !value.isString();
	else if (type == "any")
		wrongType = false;
//	else
//		assert(false);

	if (wrongType)
	{
		_error = true;
		setMessage(type.toStdString() + " expected");
	}
}

void QJsonSchemaChecker::checkProperties(const QJsonObject & value, const QJsonObject & schema)
{
	for (QJsonObject::const_iterator i = schema.begin(); i != schema.end(); ++i)
	{
		QString property = i.key();

		const QJsonValue  & propertyValue = i.value();

		_currentPath.push_back(std::string(".") + property.toStdString());
		QJsonObject::const_iterator required = propertyValue.toObject().find("required");

		if (value.contains(property))
		{
			validate(value[property], propertyValue.toObject());
		}
		else if (required != propertyValue.toObject().end() && required.value().toBool())
		{
			_error = true;
			setMessage("missing member");
		}
		_currentPath.pop_back();
	}
}

void QJsonSchemaChecker::checkAdditionalProperties(const QJsonObject & value, const QJsonValue & schema, const QStringList & ignoredProperties)
{
	for (QJsonObject::const_iterator i = value.begin(); i != value.end(); ++i)
	{
		QString property = i.key();
		if (std::find(ignoredProperties.begin(), ignoredProperties.end(), property) == ignoredProperties.end())
		{
			// property has no property definition. check against the definition for additional properties
			_currentPath.push_back(std::string(".") + property.toStdString());
			if (schema.isBool())
			{
				if (schema.toBool() == false)
				{
					_error = true;
					setMessage("no schema definition");
				}
			}
			else
			{
				validate(value[property].toObject(), schema.toObject());
			}
			_currentPath.pop_back();
		}
	}
}

void QJsonSchemaChecker::checkMinimum(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isDouble())
	{
		// only for numeric
		_error = true;
		setMessage("minimum check only for numeric fields");
		return;
	}

	if (value.toDouble() < schema.toDouble())
	{
		_error = true;
		std::ostringstream oss;
		oss << "value is too small (minimum=" << schema.toDouble() << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkMaximum(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isDouble())
	{
		// only for numeric
		_error = true;
		setMessage("maximum check only for numeric fields");
		return;
	}

	if (value.toDouble() > schema.toDouble())
	{
		_error = true;
		std::ostringstream oss;
		oss << "value is too large (maximum=" << schema.toDouble() << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkItems(const QJsonValue & value, const QJsonObject & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("items only valid for arrays");
		return;
	}

	QJsonArray jArray = value.toArray();
	for(int i = 0; i < jArray.size(); ++i)
	{
		// validate each item
		std::ostringstream oss;
		oss << "[" << i << "]";
		_currentPath.push_back(oss.str());
		validate(jArray[i].toObject(), schema);
		_currentPath.pop_back();
	}
}

void QJsonSchemaChecker::checkMinItems(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("minItems only valid for arrays");
		return;
	}

	int minimum = schema.toInt();

	QJsonArray jArray = value.toArray();
	if (static_cast<int>(jArray.size()) < minimum)
	{
		_error = true;
		std::ostringstream oss;
		oss << "array is too small (minimum=" << minimum << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkMaxItems(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("maxItems only valid for arrays");
		return;
	}

	int maximum = schema.toInt();

	QJsonArray jArray = value.toArray();
	if (static_cast<int>(jArray.size()) > maximum)
	{
		_error = true;
		std::ostringstream oss;
		oss << "array is too large (maximum=" << maximum << ")";
		setMessage(oss.str());
	}
}

void QJsonSchemaChecker::checkUniqueItems(const QJsonValue & value, const QJsonValue & schema)
{
	if (!value.isArray())
	{
		// only for arrays
		_error = true;
		setMessage("uniqueItems only valid for arrays");
		return;
	}

	if (schema.toBool() == true)
	{
		// make sure no two items are identical

		QJsonArray jArray = value.toArray();
		for(int i = 0; i < jArray.size(); ++i)
		{
			for (int j = i+1; j < jArray.size(); ++j)
			{
				if (jArray[i] == jArray[j])
				{
					// found a value twice
					_error = true;
					setMessage("array must have unique values");
				}
			}
		}
	}
}

void QJsonSchemaChecker::checkEnum(const QJsonValue & value, const QJsonValue & schema)
{
	if (schema.isArray())
	{
		QJsonArray jArray = schema.toArray();
		for(int i = 0; i < jArray.size(); ++i)
		{
			if (jArray[i] == value)
			{
				// found enum value. done.
				return;
			}
		}
	}

	// nothing found
	_error = true;
	std::ostringstream oss;
	oss << "Unknown enum value (allowed values are: " << schema.toString().toStdString();
	QJsonDocument doc(schema.toArray());
	QString strJson(doc.toJson(QJsonDocument::Compact));
	oss << strJson.toStdString() << ")";
	setMessage(oss.str());
}
<|endoftext|>