text:
/* \author Radu Bogdan Rusu
 * adaptation Raphael Favier*/
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//简单类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//这是一个辅助教程，因此我们可以负担全局变量
	//创建可视化工具
pcl::visualization::PCLVisualizer* p;
//定义左右视点
int vp_1, vp_2;
//处理点云的方便的结构定义
struct PCD
{
	PointCloud::Ptr cloud;
	std::string f_name;
	PCD() : cloud(new PointCloud) {};
};
struct PCDComparator
{
	bool operator () (const PCD& p1, const PCD& p2)
	{
		return (p1.f_name < p2.f_name);
	}
};
//以< x, y, z, curvature >形式定义一个新的点
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
	using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
	MyPointRepresentation()
	{
		//定义尺寸值
		nr_dimensions_ = 4;
	}
	//覆盖copyToFloatArray方法来定义我们的特征矢量
	virtual void copyToFloatArray(const PointNormalT& p, float* out) const
	{
		// < x, y, z, curvature >
		out[0] = p.x;
		out[1] = p.y;
		out[2] = p.z;
		out[3] = p.curvature;
	}
};
////////////////////////////////////////////////////////////////////////////////
/** 在可视化窗口的第一视点显示源点云和目标点云
*
 */
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
	p->removePointCloud("vp1_target");
	p->removePointCloud("vp1_source");
	PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
	PointCloudColorHandlerCustom<PointT> src_h(cloud_source, 255, 0, 0);
	p->addPointCloud(cloud_target, tgt_h, "vp1_target", vp_1);
	p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);
	PCL_INFO("Press q to begin the registration.\n");
	p->spin();
}
////////////////////////////////////////////////////////////////////////////////
/**在可视化窗口的第二视点显示源点云和目标点云
 *
 */
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
	p->removePointCloud("source");
	p->removePointCloud("target");
	PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler(cloud_target, "curvature");
	if (!tgt_color_handler.isCapable())
		PCL_WARN("Cannot create curvature color handler!");
	PointCloudColorHandlerGenericField<PointNormalT> src_color_handler(cloud_source, "curvature");
	if (!src_color_handler.isCapable())
		PCL_WARN("Cannot create curvature color handler!");
	p->addPointCloud(cloud_target, tgt_color_handler, "target", vp_2);
	p->addPointCloud(cloud_source, src_color_handler, "source", vp_2);
	p->spinOnce();
}
////////////////////////////////////////////////////////////////////////////////
/**加载一组我们想要匹配在一起的PCD文件
  * 参数argc是参数的数量 (pass from main ())
  *参数 argv 实际的命令行参数 (pass from main ())
  *参数models点云数据集的合成矢量
  */
void loadData(int argc, char** argv, std::vector<PCD, Eigen::aligned_allocator<PCD> >& models)
{
	std::string extension(".pcd");
	//假定第一个参数是实际测试模型
	for (int i = 1; i < argc; i++)
	{
		std::string fname = std::string(argv[i]);
		// 至少需要5个字符长（因为.plot就有 5个字符）
		if (fname.size() <= extension.size())
			continue;
		std::transform(fname.begin(), fname.end(), fname.begin(), (int(*)(int))tolower);
		//检查参数是一个pcd文件
		if (fname.compare(fname.size() - extension.size(), extension.size(), extension) == 0)
		{
			//加载点云并保存在总体的模型列表中
			PCD m;
			m.f_name = argv[i];
			pcl::io::loadPCDFile(argv[i], *m.cloud);
			//从点云中移除NAN点
			std::vector<int> indices;
			pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices);
			models.push_back(m);
		}
	}
}
////////////////////////////////////////////////////////////////////////////////
/**匹配一对点云数据集并且返还结果
  *参数 cloud_src 是源点云
  *参数 cloud_src 是目标点云
  *参数output输出的配准结果的源点云
  *参数final_transform是在来源和目标之间的转换
  */
void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f& final_transform, bool downsample = false)
{
	//
	//为了一致性和高速的下采样
	//注意：为了大数据集需要允许这项
	PointCloud::Ptr src(new PointCloud);
	PointCloud::Ptr tgt(new PointCloud);
	pcl::VoxelGrid<PointT> grid;
	if (downsample)
	{
		grid.setLeafSize(0.05, 0.05, 0.05);
		grid.setInputCloud(cloud_src);
		grid.filter(*src);
		grid.setInputCloud(cloud_tgt);
		grid.filter(*tgt);
	}
	else
	{
		src = cloud_src;
		tgt = cloud_tgt;
	}
	//计算曲面法线和曲率
	PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
	PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
	pcl::NormalEstimation<PointT, PointNormalT> norm_est;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	norm_est.setSearchMethod(tree);
	norm_est.setKSearch(30);
	norm_est.setInputCloud(src);
	norm_est.compute(*points_with_normals_src);
	pcl::copyPointCloud(*src, *points_with_normals_src);
	norm_est.setInputCloud(tgt);
	norm_est.compute(*points_with_normals_tgt);
	pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
	//
	//举例说明我们自定义点的表示（以上定义）
	MyPointRepresentation point_representation;
	//调整'curvature'尺寸权重以便使它和x, y, z平衡
	float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
	point_representation.setRescaleValues(alpha);
	//
	// 配准
	pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
	reg.setTransformationEpsilon(1e-6);
	//将两个对应关系之间的(src<->tgt)最大距离设置为10厘米
	//注意：根据你的数据集大小来调整
	reg.setMaxCorrespondenceDistance(0.1);
	//设置点表示
	reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation));
	reg.setInputCloud(points_with_normals_src);
	reg.setInputTarget(points_with_normals_tgt);
	//
	//在一个循环中运行相同的最优化并且使结果可视化
	Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
	PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
	reg.setMaximumIterations(2);
	for (int i = 0; i < 30; ++i)
	{
		PCL_INFO("Iteration Nr. %d.\n", i);
		//为了可视化的目的保存点云
		points_with_normals_src = reg_result;
		//估计
		reg.setInputCloud(points_with_normals_src);
		reg.align(*reg_result);
		//在每一个迭代之间累积转换
		Ti = reg.getFinalTransformation() * Ti;
		//如果这次转换和之前转换之间的差异小于阈值
		//则通过减小最大对应距离来改善程序
		if (fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
			reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
		prev = reg.getLastIncrementalTransformation();
		//可视化当前状态
		showCloudsRight(points_with_normals_tgt, points_with_normals_src);
	}
	//
  // 得到目标点云到源点云的变换
	targetToSource = Ti.inverse();
	//
	//把目标点云转换回源框架
	pcl::transformPointCloud(*cloud_tgt, *output, targetToSource);
	p->removePointCloud("source");
	p->removePointCloud("target");
	PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0);
	PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0);
	p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
	p->addPointCloud(cloud_src, cloud_src_h, "source", vp_2);
	PCL_INFO("Press q to continue the registration.\n");
	p->spin();
	p->removePointCloud("source");
	p->removePointCloud("target");
	//添加源点云到转换目标
	*output += *cloud_src;
	final_transform = targetToSource;
}
/* ---[ */
int main(int argc, char** argv)
{
	// 加载数据
	std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
	loadData(argc, argv, data);
	//检查用户输入
	if (data.empty())
	{
		PCL_ERROR("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
		PCL_ERROR("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
		PCL_INFO("Example: %s `rospack find pcl`/test/bun0.pcd `rospack find pcl`/test/bun4.pcd", argv[0]);
		return (-1);
	}
	PCL_INFO("Loaded %d datasets.", (int)data.size());
	//创建一个PCL可视化对象
	p = new pcl::visualization::PCLVisualizer(argc, argv, "Pairwise Incremental Registration example");
	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
	PointCloud::Ptr result(new PointCloud), source, target;
	Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;
	for (size_t i = 1; i < data.size(); ++i)
	{
		source = data[i - 1].cloud;
		target = data[i].cloud;
		//添加可视化数据
		showCloudsLeft(source, target);
		PointCloud::Ptr temp(new PointCloud);
		PCL_INFO("Aligning %s (%d) with %s (%d).\n", data[i - 1].f_name.c_str(), source->points.size(), data[i].f_name.c_str(), target->points.size());
		pairAlign(source, target, temp, pairTransform, true);
		//把当前的两两配对转换到全局变换
		pcl::transformPointCloud(*temp, *result, GlobalTransform);
		//update the global transform更新全局变换
		GlobalTransform = pairTransform * GlobalTransform;
		//保存配准对，转换到第一个点云框架中
		std::stringstream ss;
		ss << i << ".pcd";
		pcl::io::savePCDFile(ss.str(), *result, true);
	}

	system("pause");
	return (0);
}
/* ]--- */

ext: cpp


text:
/*
Copyright 2012, 2013 Rogier van Dalen.

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.
*/

/** \file
An outcome type that contains the output and the remaining input.
*/

#ifndef PARSE_LL_CORE_OUTCOME_EXPLICIT_HPP_INCLUDED
#define PARSE_LL_CORE_OUTCOME_EXPLICIT_HPP_INCLUDED

#include <utility>
#include <type_traits>

// GCC spurious warning from boost::optional with -O1 or higher.
// http://gcc.gnu.org/bugzilla/show_bug.cgi?id=47679
#if BOOST_WORKAROUND(__GNUC__, == 4) && (__GNUC_MINOR__ == 6)
// Retain -Wuninitialized in debug mode.
#ifdef __OPTIMIZE__
#pragma GCC diagnostic ignored "-Wuninitialized"
#endif
#endif

#include <boost/optional.hpp>

#include "utility/returns.hpp"

#include "./core.hpp"
#include "./successful.hpp"

namespace parse_ll {

/**
Outcome class that, if successful, keeps the output and the input directly in
memory.
Useful for eager parsers.
\todo Optimise using move constructors and in-place construction of the result.
\todo This (and outcomes in general) is not necessarily assignable, is it?
    Otherwise something must be done with reference types for Output.
*/
template <class Output, class Input> struct explicit_outcome {
    typedef successful <Output, Input> successful_outcome;
    boost::optional <successful_outcome> result;
public:
    /**
    Construct as successful.
    */
    template <class OtherOutput, class OtherInput>
    explicit_outcome (OtherOutput && output, OtherInput && rest)
    : result (successful_outcome (
        std::forward <OtherOutput> (output), std::forward <OtherInput> (rest)))
    {}

    /**
    Construct with explicit rest, but default-constructed output.
    This is useful because it generalises to when Output is void.
    */
    explicit explicit_outcome (Input const & rest)
    : result (successful_outcome (rest)) {}
    explicit explicit_outcome (Input & rest)
    : result (successful_outcome (rest)) {}
    explicit explicit_outcome (Input && rest)
    : result (successful_outcome (std::forward <Input> (rest))) {}

    /**
    \todo Use in-place construction of the optional?
    */
    template <class OtherOutcome>
        explicit_outcome (OtherOutcome && other)
    : result (success (other) ?
        boost::optional <successful_outcome> (
            successful_outcome (std::forward <OtherOutcome> (other)))
        : boost::optional <successful_outcome>()) {}

    // Specialisation of the above for the known-failed case.
    // Otherwise, the compiler tries (and fails) to generate a call to
    // output (other).
    explicit_outcome (failed const &) : result() {}
    explicit_outcome (failed &) : result() {}
    explicit_outcome (failed &&) : result() {}

    template <class Other> explicit_outcome & operator = (Other const &)
        = delete;
};

namespace operation {

    template <class Output, class Input>
        struct success <explicit_outcome <Output, Input>>
    {
        bool operator() (explicit_outcome <Output, Input> const & outcome) const
        { return bool (outcome.result); }
    };

    template <class Output, class Input>
        struct output <explicit_outcome <Output, Input>>
    {
        Output operator() (explicit_outcome <Output, Input> const & outcome)
            const
        { return ::parse_ll::output (outcome.result.get()); }

        Output operator() (explicit_outcome <Output, Input> const && outcome)
            const
        { return ::parse_ll::output (std::move (outcome.result.get())); }
    };
    template <class Input> struct output <explicit_outcome <void, Input>>
    { void operator() (explicit_outcome <void, Input> const &) const; };

    template <class Output, class Input>
        struct rest <explicit_outcome <Output, Input>>
    {
        Input operator() (explicit_outcome <Output, Input> const & outcome)
            const
        { return ::parse_ll::rest (outcome.result.get()); }
        Input operator() (explicit_outcome <Output, Input> && outcome) const
        { return ::parse_ll::rest (std::move (outcome.result.get())); }
    };

} // namespace operation

} // namespace parse_ll

#endif // PARSE_LL_CORE_OUTCOME_EXPLICIT_HPP_INCLUDED

ext: hpp


text:
/*

Copyright (c) 2015, Arvid Norberg
All rights reserved.

    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 3 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, see <http://www.gnu.org/licenses/>.

*/

#ifndef SIMULATOR_HPP_INCLUDED
#define SIMULATOR_HPP_INCLUDED

#include "simulator/push_warnings.hpp"

#include <boost/config.hpp>
#include <boost/asio/detail/config.hpp>
#include <boost/asio/basic_deadline_timer.hpp>
#include <boost/asio/ip/udp.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/write.hpp>
#include <boost/asio/read.hpp>
#include <boost/asio/io_context.hpp>
#include <boost/system/error_code.hpp>
#include <boost/asio/ip/network_v4.hpp>
#include <boost/asio/buffers_iterator.hpp>

#include <boost/asio/post.hpp>
#include <boost/asio/defer.hpp>
#include <boost/asio/dispatch.hpp>

#include <boost/optional.hpp>

#include "simulator/pop_warnings.hpp"

#include "simulator/chrono.hpp"
#include "simulator/sink_forwarder.hpp"
#include "simulator/function.hpp"
#include "simulator/noexcept_movable.hpp"
#include "simulator/mallocator.hpp"

#include <deque>
#include <mutex>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <set>
#include <vector>
#include <list>
#include <functional>

#ifndef IP_DONTFRAGMENT
#define IP_DONTFRAGMENT 1
#endif

namespace sim
{
	namespace aux
	{
		struct channel;
		struct packet;
		struct pcap;
	}

	// this represents a network route (a series of sinks to pass a packet
	// through)
	struct SIMULATOR_DECL route
	{
		friend route operator+(route lhs, route rhs)
		{ return std::move(lhs.append(std::move(rhs))); }

		std::shared_ptr<sink> next_hop() const { return hops.front(); }
		std::shared_ptr<sink> pop_front()
		{
			if (hops.empty()) return std::shared_ptr<sink>();
			std::shared_ptr<sink> ret(std::move(hops.front()));
			hops.erase(hops.begin());
			return ret;
		}
		void replace_last(std::shared_ptr<sink> s) { hops.back() = std::move(s); }
		void prepend(route const& r)
		{ hops.insert(hops.begin(), r.hops.begin(), r.hops.end()); }
		void prepend(std::shared_ptr<sink> s) { hops.insert(hops.begin(), std::move(s)); }
		route& append(route const& r)
		{ hops.insert(hops.end(), r.hops.begin(), r.hops.end()); return *this; }
		route& append(std::shared_ptr<sink> s) { hops.push_back(std::move(s)); return *this; }
		bool empty() const { return hops.empty(); }
		std::shared_ptr<sink> last() const
		{ return hops.back(); }

	private:
		std::deque<std::shared_ptr<sink>, aux::mallocator<std::shared_ptr<sink>>> hops;
	};

	void forward_packet(aux::packet p);

	struct simulation;
	struct configuration;
	struct queue;

	namespace asio
	{

	using boost::asio::buffer_size;
	using boost::asio::const_buffer;
	using boost::asio::mutable_buffer;
	using boost::asio::buffer;

	using boost::asio::post;
	using boost::asio::dispatch;
	using boost::asio::defer;

	struct io_context;

	struct io_executor
	{
		io_executor(io_context& ctx) : m_ctx(&ctx) {}
		io_context& context() const { return *m_ctx; }

		template <typename Handler, typename Allocator>
		void dispatch(Handler handler, Allocator const& a) const;

		template <typename Handler, typename Allocator>
		void post(Handler handler, Allocator const& a) const;

		template <typename Handler, typename Allocator>
		void defer(Handler handler, Allocator const& a) const;

		void on_work_finished() const {}
		void on_work_started() const {}

		bool running_in_this_thread() const { return true; }

		friend bool operator==(io_executor const& lhs, io_executor const& rhs)
		{ return lhs.m_ctx == rhs.m_ctx; }

		friend bool operator!=(io_executor const& lhs, io_executor const& rhs)
		{ return lhs.m_ctx != rhs.m_ctx; }

	private:
		io_context* m_ctx;
	};

	struct SIMULATOR_DECL high_resolution_timer
	{
		friend struct sim::simulation;

		using time_type = chrono::high_resolution_clock::time_point;
		using duration_type = chrono::high_resolution_clock::duration;

		using executor_type = io_executor;
		executor_type get_executor();

		explicit high_resolution_timer(io_context& io_context);
		high_resolution_timer(io_context& io_context,
			const time_type& expiry_time);
		high_resolution_timer(io_context& io_context,
			const duration_type& expiry_time);
		high_resolution_timer(high_resolution_timer&&) noexcept = default;
		high_resolution_timer& operator=(high_resolution_timer&&) noexcept = default;
		~high_resolution_timer();

		std::size_t cancel();
		std::size_t cancel_one();

		time_type expiry() const;
		std::size_t expires_at(const time_type& expiry_time);
		std::size_t expires_after(const duration_type& expiry_time);

		void wait();
		void wait(boost::system::error_code& ec);

		void async_wait(aux::function<void(boost::system::error_code const&)> handler);

	private:

		void fire(boost::system::error_code ec);

		time_type m_expiration_time;
		aux::function<void(boost::system::error_code const&)> m_handler;
		io_context* m_io_service;
		bool m_expired;
	};

	using waitable_timer = high_resolution_timer;

	namespace error = boost::asio::error;

	template <typename Protocol>
	struct socket_base
	{
		socket_base(io_context& ios) : m_io_service(ios) {}
		socket_base(socket_base&& s) = default;

		enum wait_type_t
		{
			wait_read, wait_write, wait_error
		};

		// io_control
		using reuse_address = boost::asio::socket_base::reuse_address;
		using executor_type = io_executor;
		executor_type get_executor();

		// socket options
		using send_buffer_size = boost::asio::socket_base::send_buffer_size;
		using receive_buffer_size = boost::asio::socket_base::receive_buffer_size;

		template <class Option>
		void set_option(Option const& opt, boost::system::error_code&)
		{
			Protocol const p = Protocol::v4();
			(void)p;
#ifdef IP_DONTFRAG
			if (opt.name(p) == IP_DONTFRAG)
				m_dont_fragment = *reinterpret_cast<int const*>(opt.data(p)) != 0;
#endif
#ifdef IP_DONTFRAGMENT
			if (opt.name(p) == IP_DONTFRAGMENT)
				m_dont_fragment = *reinterpret_cast<int const*>(opt.data(p)) != 0;
#endif
#ifdef IP_MTU_DISCOVER
			if (opt.name(p) == IP_MTU_DISCOVER)
				m_dont_fragment = *reinterpret_cast<int const*>(opt.data(p)) == IP_PMTUDISC_DO;
#endif
		}

		void set_option(receive_buffer_size const& op, boost::system::error_code&)
		{
			m_max_receive_queue_size = op.value();
		}

		void set_option(send_buffer_size const&, boost::system::error_code&)
		{
			// TODO: implement
		}

		void set_option(reuse_address const&, boost::system::error_code&)
		{
			// TODO: implement
		}

		typename Protocol::endpoint local_endpoint(boost::system::error_code& ec) const
		{
			if (!m_open)
			{
				ec = error::bad_descriptor;
				return typename Protocol::endpoint{};
			}

			return m_user_bound_to;
		}

		typename Protocol::endpoint local_endpoint() const
		{
			boost::system::error_code ec;
			auto const ret = local_endpoint(ec);
			if (ec) throw boost::system::system_error(ec);
			return ret;
		}

		typename Protocol::endpoint local_bound_to(boost::system::error_code& ec) const
		{
			if (!m_open)
			{
				ec = error::bad_descriptor;
				return typename Protocol::endpoint{};
			}

			return m_bound_to;
		}

		typename Protocol::endpoint local_bound_to() const
		{
			boost::system::error_code ec;
			auto const ret = local_bound_to(ec);
			if (ec) throw boost::system::system_error(ec);
			return ret;
		}

		template <class Option>
		void get_option(Option&, boost::system::error_code&) { }

		void get_option(receive_buffer_size& op, boost::system::error_code&)
		{
			op = m_max_receive_queue_size;
		}

		template <class IoControl>
		void io_control(IoControl const&, boost::system::error_code&) { }

		template <class IoControl>
		void io_control(IoControl const&) {}

		void non_blocking(bool b, boost::system::error_code&)
		{ m_non_blocking = b; }

		void non_blocking(bool b)
		{ m_non_blocking = b; }

		bool is_open() const
		{
			return m_open;
		}

		using message_flags = int;

		// internal interface

		route get_incoming_route();
		route get_outgoing_route();

	protected:

		io_context& m_io_service;

		typename Protocol::endpoint m_bound_to;

		// this is the interface the user requested to bind to (in order to
		// distinguish the concrete interface it was bound to and INADDR_ANY if
		// that was requested). We keep this separately to return it as the local
		// endpoint
		typename Protocol::endpoint m_user_bound_to;

		// this is an object implementing the sink interface, forwarding
		// packets to this socket. If this socket is destructed, this forwarder
		// is redirected to just drop packets. This is necessary since sinks
		// must be held by shared_ptr, and socket objects aren't.
		std::shared_ptr<aux::sink_forwarder> m_forwarder;

		// whether the socket is open or not
		bool m_open = false;

		// true if the socket is set to non-blocking mode
		bool m_non_blocking = false;

		// when true, the MTU limit is in effect
		bool m_dont_fragment = false;

		// the max size of the incoming queue. This is to emulate the send and
		// receive buffer. This should also depend on the bandwidth, to not
		// make the queue size not grow too long in time.
		int m_max_receive_queue_size = 64 * 1024;
	};

	namespace ip {

	using boost::asio::ip::address;
	using boost::asio::ip::address_v4;
	using boost::asio::ip::address_v6;

	using boost::asio::ip::make_address_v4;
	using boost::asio::ip::make_address_v6;
	using boost::asio::ip::make_address;

	using boost::asio::ip::make_network_v4;

	template<typename Protocol>
	struct basic_endpoint : boost::asio::ip::basic_endpoint<Protocol>
	{
		basic_endpoint(ip::address const& addr, unsigned short port)
			: boost::asio::ip::basic_endpoint<Protocol>(addr, port) {}
		basic_endpoint() : boost::asio::ip::basic_endpoint<Protocol>() {}
	};

	template <typename Protocol>
	struct basic_resolver_entry
	{
		using endpoint_type = typename Protocol::endpoint;
		using protocol_type = Protocol;

		basic_resolver_entry() {}
		basic_resolver_entry(
			endpoint_type const& ep
			, std::string const& host
			, std::string const& service)
			: m_endpoint(ep)
			, m_host_name(host)
			, m_service(service)
		{}

		endpoint_type endpoint() const { return m_endpoint; }
		std::string host_name() const { return m_host_name; }
		operator endpoint_type() const { return m_endpoint; }
		std::string service_name() const { return m_service; }

	private:
		endpoint_type m_endpoint;
		std::string m_host_name;
		std::string m_service;
	};

	template<typename Protocol>
	struct SIMULATOR_DECL basic_resolver
	{
		basic_resolver(io_context& ios);

		using protocol_type = Protocol;
		using results_type = std::vector<basic_resolver_entry<Protocol>, aux::mallocator<basic_resolver_entry<Protocol>>>;

		void cancel();

		void async_resolve(std::string hostname, char const* service
			, aux::function<void(boost::system::error_code const&,
				results_type)> handler);

		basic_resolver(basic_resolver&&) noexcept;
		basic_resolver& operator=(basic_resolver&&) noexcept;
		basic_resolver(basic_resolver const&) = delete;
		basic_resolver& operator=(basic_resolver const&) = delete;

		//TODO: add remaining members

	private:

		void on_lookup(boost::system::error_code const& ec);

		struct result_t
		{
			chrono::high_resolution_clock::time_point completion_time;
			boost::system::error_code err;
			results_type ips;
			aux::function<void(boost::system::error_code const&,
				results_type)> handler;

			result_t(
				chrono::high_resolution_clock::time_point ct
				, boost::system::error_code e
				, results_type ips_
				, aux::function<void(boost::system::error_code const&
					, results_type)> h)
				: completion_time(ct)
				, err(e)
				, ips(std::move(ips_))
				, handler(std::move(h))
			{}

			result_t(result_t&&) noexcept = default;
			result_t& operator=(result_t&&) = default;
			result_t(result_t const&) = delete;
			result_t& operator=(result_t const&) = delete;
		};

		io_context* m_ios;
		asio::high_resolution_timer m_timer;
		using queue_t = aux::noexcept_movable<std::vector<result_t>>;

		queue_t m_queue;
	};

	struct SIMULATOR_DECL udp
	{
		static udp v4() { return udp(AF_INET); }
		static udp v6() { return udp(AF_INET6); }

		using endpoint = basic_endpoint<udp>;

		struct SIMULATOR_DECL socket : socket_base<udp>, sink
		{
			using endpoint_type = ip::udp::endpoint;
			using protocol_type = ip::udp;
			using lowest_layer_type = socket;

			socket(io_context& ios);
			~socket() override;

			socket(socket const&) = delete;
			socket& operator=(socket const&) = delete;
			socket(socket&&);

			lowest_layer_type& lowest_layer() { return *this; }

			void bind(ip::udp::endpoint const& ep
				, boost::system::error_code& ec);
			void bind(ip::udp::endpoint const& ep);

			void close();
			void close(boost::system::error_code& ec);

			void cancel(boost::system::error_code& ec);
			void cancel();

			void open(udp protocol, boost::system::error_code& ec);
			void open(udp protocol);

			template<typename ConstBufferSequence>
			std::size_t send_to(ConstBufferSequence const& bufs
				, udp::endpoint const& destination
				, socket_base::message_flags flags
				, boost::system::error_code& ec)
			{
				std::vector<asio::const_buffer> b(buffer_sequence_begin(bufs)
					, buffer_sequence_end(bufs));
				abort_send_handlers();
				return send_to_impl(b, destination, flags, ec);
			}

			template<typename ConstBufferSequence>
			std::size_t send_to(ConstBufferSequence const& bufs
				, udp::endpoint const& destination)
			{
				std::vector<asio::const_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				abort_send_handlers();
				boost::system::error_code ec;
				std::size_t ret = send_to_impl(b, destination, 0, ec);
				if (ec) throw boost::system::system_error(ec);
				return ret;
			}

			void async_wait(socket_base::wait_type_t w
				, aux::function<void(boost::system::error_code const&)> handler);

			template <class BufferSequence>
			void async_receive(BufferSequence const& bufs
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				abort_recv_handlers();

				async_receive_from_impl(b, nullptr, 0, std::move(handler));
			}

			template <class BufferSequence>
			void async_receive_from(BufferSequence const& bufs
				, udp::endpoint& sender
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				abort_recv_handlers();

				async_receive_from_impl(b, &sender, 0, std::move(handler));
			}

			template <class BufferSequence>
			void async_receive_from(BufferSequence const& bufs
				, udp::endpoint& sender
				, socket_base::message_flags flags
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				abort_recv_handlers();

				async_receive_from_impl(b, &sender, flags, std::move(handler));
			}
/*
			void async_read_from(null_buffers const&
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler)
			{
				abort_recv_handlers();
				async_read_some_null_buffers_impl(std::move(handler));
			}
*/

			template <class BufferSequence>
			std::size_t receive_from(BufferSequence const& bufs
				, udp::endpoint& sender)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				assert(!b.empty());
				abort_recv_handlers();
				boost::system::error_code ec;
				std::size_t ret = receive_from_impl(b, &sender, 0, ec);
				if (ec) throw boost::system::system_error(ec);
				return ret;
			}

			template <class BufferSequence>
			std::size_t receive_from(BufferSequence const& bufs
				, udp::endpoint& sender
				, socket_base::message_flags)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				assert(!b.empty());
				abort_recv_handlers();
				boost::system::error_code ec;
				std::size_t ret = receive_from_impl(b, &sender, 0, ec);
				if (ec) throw boost::system::system_error(ec);
				return ret;
			}

			template <class BufferSequence>
			std::size_t receive_from(BufferSequence const& bufs
				, udp::endpoint& sender
				, socket_base::message_flags
				, boost::system::error_code& ec)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				assert(!b.empty());
				abort_recv_handlers();
				return receive_from_impl(b, &sender, 0, ec);
			}

			// TODO: support connect and remote_endpoint

			// internal interface

			// implements sink
			virtual void incoming_packet(aux::packet p) override final;
			virtual std::string label() const override final
			{ return m_bound_to.address().to_string(); }

			void async_receive_from_impl(std::vector<asio::mutable_buffer> const& bufs
				, udp::endpoint* sender
				, socket_base::message_flags flags
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler);

			std::size_t receive_from_impl(
				std::vector<asio::mutable_buffer> const& bufs
				, udp::endpoint* sender
				, socket_base::message_flags flags
				, boost::system::error_code& ec);

			void async_wait_receive_impl(
				udp::endpoint* sender
				, aux::function<void(boost::system::error_code const&)> handler);

		private:

			void maybe_wakeup_reader();
			void abort_send_handlers();
			void abort_recv_handlers();

			std::size_t send_to_impl(std::vector<asio::const_buffer> const& b
				, udp::endpoint const& dst, message_flags flags
				, boost::system::error_code& ec);

			// this is the next time we'll have an opportunity to send another
			// outgoing packet. This is used to implement the bandwidth constraints
			// of channels. This may be in the past, in which case it's OK to send
			// a packet immediately.
			chrono::high_resolution_clock::time_point m_next_send;

			// while we're blocked in an async_write_some operation, this is the
			// handler that should be called once we're done sending
			aux::function<void(boost::system::error_code const&, std::size_t)>
				m_send_handler;
			aux::function<void(boost::system::error_code const&)>
				m_wait_send_handler;

			// if we have an outstanding read on this socket, this is set to the
			// handler.
			aux::function<void(boost::system::error_code const&, std::size_t)>
				m_recv_handler;
			aux::function<void(boost::system::error_code const&)>
				m_wait_recv_handler;

			// if we have an outstanding read operation, this is the buffer to
			// receive into
			std::vector<asio::mutable_buffer> m_recv_buffer;

			// if we have an outstanding receive operation, this may point to an
			// endpoint to fill in the senders IP in
			udp::endpoint* m_recv_sender;

			asio::high_resolution_timer m_recv_timer;
			asio::high_resolution_timer m_send_timer;

			// this is the incoming queue of packets for each socket
			std::list<aux::packet, aux::mallocator<aux::packet>> m_incoming_queue;

			bool m_recv_null_buffers;

			// the number of bytes in the incoming packet queue
			int m_queue_size;

			// our address family
			bool m_is_v4;
		};

		using resolver = basic_resolver<udp>;

		int family() const { return m_family; }

		friend bool operator==(udp const& lhs, udp const& rhs)
		{ return lhs.m_family == rhs.m_family; }

		friend bool operator!=(udp const& lhs, udp const& rhs)
		{ return lhs.m_family != rhs.m_family; }

	private:
		// Construct with a specific family.
		explicit udp(int protocol_family)
			: m_family(protocol_family)
		{}

		int m_family;

	}; // udp

	struct SIMULATOR_DECL tcp
	{
		// temporary fix until the resolvers are implemented using our endpoint
		tcp(boost::asio::ip::tcp p) : m_family(p.family()) {}

		static tcp v4() { return tcp(AF_INET); }
		static tcp v6() { return tcp(AF_INET6); }

		int family() const { return m_family; }

		using endpoint = basic_endpoint<tcp>;

		struct SIMULATOR_DECL socket : socket_base<tcp>, sink
		{
			using endpoint_type = ip::tcp::endpoint;
			using protocol_type = ip::tcp;
			using lowest_layer_type = socket;

			explicit socket(io_context& ios);
			socket(socket const&) = delete;
			socket& operator=(socket const&) = delete;
			socket(socket&&);
			socket& operator=(socket&&);

			~socket() override;

			void close();
			void close(boost::system::error_code& ec);
			void open(tcp protocol, boost::system::error_code& ec);
			void open(tcp protocol);
			void bind(ip::tcp::endpoint const& ep
				, boost::system::error_code& ec);
			void bind(ip::tcp::endpoint const& ep);
			tcp::endpoint remote_endpoint(boost::system::error_code& ec) const;
			tcp::endpoint remote_endpoint() const;

			lowest_layer_type& lowest_layer() { return *this; }

			void async_connect(tcp::endpoint const& target
				, aux::function<void(boost::system::error_code const&)> h);

			template <class ConstBufferSequence>
			void async_write_some(ConstBufferSequence const& bufs
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler)
			{
				std::vector<asio::const_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				abort_send_handlers();
				async_write_some_impl(b, std::move(handler));
			}

			void async_wait(socket_base::wait_type_t const w
				, aux::function<void(boost::system::error_code const&)> handler)
			{
				if (w == socket_base::wait_type_t::wait_write)
				{
					abort_send_handlers();
					assert(false && "not supported yet");
				}
				else if (w == socket_base::wait_type_t::wait_read)
				{
					abort_recv_handlers();
					async_wait_read_impl(std::move(handler));
				}
			}

			template <class BufferSequence>
			std::size_t read_some(BufferSequence const& bufs
				, boost::system::error_code& ec)
			{
				assert(m_non_blocking && "blocking operations not supported");
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				return read_some_impl(b, ec);
			}

			template <class ConstBufferSequence>
			std::size_t write_some(ConstBufferSequence const& bufs
				, boost::system::error_code& ec)
			{
				assert(m_non_blocking && "blocking operations not supported");
				std::vector<asio::const_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				return write_some_impl(b, ec);
			}

			template <class BufferSequence>
			void async_read_some(BufferSequence const& bufs
				, aux::function<void(boost::system::error_code const&
					, std::size_t)> handler)
			{
				std::vector<asio::mutable_buffer> b(buffer_sequence_begin(bufs), buffer_sequence_end(bufs));
				abort_recv_handlers();

				async_read_some_impl(b, std::move(handler));
			}

			std::size_t available(boost::system::error_code & ec) const;
			std::size_t available() const;

			void cancel(boost::system::error_code& ec);
			void cancel();

			using socket_base::set_option;
			using socket_base::get_option;
			using socket_base::io_control;

			// private interface

			// implements sink
			virtual void incoming_packet(aux::packet p) override;
			virtual std::string label() const override final
			{ return m_bound_to.address().to_string(); }

			void internal_connect(tcp::endpoint const& bind_ip
				, std::shared_ptr<aux::channel> const& c
				, boost::system::error_code& ec);

			void abort_send_handlers();
			void abort_recv_handlers();

			virtual bool internal_is_listening();
		protected:

			void maybe_wakeup_reader();
			void maybe_wakeup_writer();

			void async_write_some_impl(std::vector<asio::const_buffer> const& bufs
				, aux::function<void(boost::system::error_code const&, std::size_t)> handler);
			void async_read_some_impl(std::vector<asio::mutable_buffer> const& bufs
				, aux::function<void(boost::system::error_code const&, std::size_t)> handler);
			void async_wait_read_impl(
				aux::function<void(boost::system::error_code const&)> handler);
			std::size_t write_some_impl(std::vector<asio::const_buffer> const& bufs
				, boost::system::error_code& ec);
			std::size_t read_some_impl(std::vector<asio::mutable_buffer> const& bufs
				, boost::system::error_code& ec);

			void send_packet(aux::packet p);

			// called when a packet is dropped
			void packet_dropped(aux::packet p);

			aux::function<void(boost::system::error_code const&)> m_connect_handler;

			asio::high_resolution_timer m_connect_timer;

			// the tcp "packet size" (segment size)
			// TODO: name this constant!
			int m_mss = 1475;

			// while we're blocked in an async_write_some operation, this is the
			// handler that should be called once we're done sending
			aux::function<void(boost::system::error_code const&, std::size_t)> m_send_handler;
			aux::function<void(boost::system::error_code const&)> m_wait_send_handler;

			std::vector<asio::const_buffer> m_send_buffer;

			// this is the incoming queue of packets for each socket
			std::list<aux::packet, aux::mallocator<aux::packet>> m_incoming_queue;

			// the number of bytes in the incoming packet queue
			int m_queue_size = 0;

			// if we have an outstanding read on this socket, this is set to the
			// handler.
			aux::function<void(boost::system::error_code const&, std::size_t)> m_recv_handler;
			aux::function<void(boost::system::error_code const&)> m_wait_recv_handler;

			// if we have an outstanding buffer to receive into, these are them
			std::vector<asio::mutable_buffer> m_recv_buffer;

			asio::high_resolution_timer m_recv_timer;

			// our address family
			bool m_is_v4 = true;

			// true if the currently outstanding read operation is for null_buffers
			bool m_recv_null_buffers = false;

			// true if the currenly outstanding write operation is for null_buffers
			bool m_send_null_buffers = false;

			// if this socket is connected to another endpoint, this object is
			// shared between both sockets and contain information and state about
			// the channel.
			std::shared_ptr<aux::channel> m_channel;

			std::uint64_t m_next_outgoing_seq = 0;
			std::uint64_t m_next_incoming_seq = 0;

			// the sequence number of the last dropped packet. We should only cut
			// the cwnd in half once per round-trip. If a whole window is lost, we
			// need to only halve it once
			std::uint64_t m_last_drop_seq = 0;

			// the current congestion window size (in bytes)
			int m_cwnd = m_mss * 2;

			// the number of bytes that have been sent but not ACKed yet
			int m_bytes_in_flight = 0;

			// reorder buffer for when packets are dropped
			std::map<std::uint64_t, aux::packet, std::less<std::uint64_t>, aux::mallocator<std::pair<std::uint64_t const, aux::packet>>> m_reorder_buffer;

			// the sizes of packets given their sequence number
			std::unordered_map<std::uint64_t, int, std::hash<std::uint64_t>, std::equal_to<std::uint64_t>, aux::mallocator<std::pair<std::uint64_t const, int>>> m_outstanding_packet_sizes;

			// packets to re-send (because they were dropped)
			std::list<aux::packet, aux::mallocator<aux::packet>> m_outgoing_packets;
		};

		struct SIMULATOR_DECL acceptor : socket
		{
			explicit acceptor(io_context& ios);
			acceptor(acceptor&&);
			~acceptor() override;

			void cancel(boost::system::error_code& ec);
			void cancel();

			void listen(int qs = -1);
			void listen(int qs, boost::system::error_code& ec);

			void async_accept(ip::tcp::socket& peer
				, aux::function<void(boost::system::error_code const&)> h);
			void async_accept(ip::tcp::socket& peer
				, ip::tcp::endpoint& peer_endpoint
				, aux::function<void(boost::system::error_code const&)> h);
			void async_accept(aux::function<void(boost::system::error_code const&, ip::tcp::socket peer)> h);

			void close(boost::system::error_code& ec);
			void close();

			// private interface

			// implements sink
			virtual void incoming_packet(aux::packet p) override final;
			virtual bool internal_is_listening() override final;

		private:
			// check the incoming connection queue to see if any connection in
			// there is ready to be accepted and delivered to the user
			void check_accept_queue();
			void do_check_accept_queue(boost::system::error_code const& ec);

			aux::function<void(boost::system::error_code const&)> m_accept_handler;
			aux::function<void(boost::system::error_code const&, ip::tcp::socket)> m_accept_handler2;

			// the number of half-open incoming connections this listen socket can
			// hold. If this is -1, this socket is not yet listening and incoming
			// connection attempts should be rejected.
			int m_queue_size_limit;

			// these are incoming connection attempts. Both half-open and
			// completely connected. When accepting a connection, this queue is
			// checked first before waiting for a connection attempt.
			using incoming_conns_t = std::vector<std::shared_ptr<aux::channel>, aux::mallocator<std::shared_ptr<aux::channel>>>;
			incoming_conns_t m_incoming_conns;

			// for new-style accept, allocate socket in here just to fail early
			boost::optional<ip::tcp::socket> m_new_socket;

			// the socket to accept a connection into
			tcp::socket* m_accept_into;

			// the endpoint to write the remote endpoint into when accepting
			tcp::endpoint* m_remote_endpoint;

			// non copyable
			acceptor(acceptor const&);
			acceptor& operator=(acceptor const&);
		};

		using resolver = basic_resolver<tcp>;

		friend bool operator==(tcp const& lhs, tcp const& rhs)
		{ return lhs.m_family == rhs.m_family; }

		friend bool operator!=(tcp const& lhs, tcp const& rhs)
		{ return lhs.m_family != rhs.m_family; }

	private:
		// Construct with a specific family.
		explicit tcp(int protocol_family)
			: m_family(protocol_family)
		{}

		int m_family;
	};

	extern template struct basic_resolver<udp>;
	extern template struct basic_resolver<tcp>;

	} // ip

	} // asio

	struct SIMULATOR_DECL simulation
	{
		// it calls fire() when a timer fires
		friend struct high_resolution_timer;

		simulation(configuration& config);
		~simulation();

		std::size_t run();

		std::size_t poll(boost::system::error_code& ec);
		std::size_t poll();

		std::size_t poll_one(boost::system::error_code& ec);
		std::size_t poll_one();

		void stop();
		bool stopped() const;
		void restart();
		// private interface

		void add_timer(asio::high_resolution_timer* t);
		void remove_timer(asio::high_resolution_timer* t);

		boost::asio::io_context& get_internal_service()
		{ return m_service; }

		asio::io_context& get_io_context()
		{ return *m_internal_ios; }

		asio::ip::tcp::endpoint bind_socket(asio::ip::tcp::socket* socket
			, asio::ip::tcp::endpoint ep
			, boost::system::error_code& ec);
		void unbind_socket(asio::ip::tcp::socket* socket
			, asio::ip::tcp::endpoint const& ep);
		void rebind_socket(asio::ip::tcp::socket* s, asio::ip::tcp::endpoint ep);

		asio::ip::udp::endpoint bind_udp_socket(asio::ip::udp::socket* socket
			, asio::ip::udp::endpoint ep
			, boost::system::error_code& ec);
		void unbind_udp_socket(asio::ip::udp::socket* socket
			, asio::ip::udp::endpoint const& ep);
		void rebind_udp_socket(asio::ip::udp::socket* socket, asio::ip::udp::endpoint ep);

		std::shared_ptr<aux::channel> internal_connect(asio::ip::tcp::socket* s
			, asio::ip::tcp::endpoint const& target, boost::system::error_code& ec);

		route find_udp_socket(
			asio::ip::udp::socket const& socket
			, asio::ip::udp::endpoint const& ep);

		configuration& config() const { return m_config; }

		void add_io_service(asio::io_context* ios);
		void remove_io_service(asio::io_context* ios);
		std::vector<asio::io_context*> get_all_io_services() const;

		aux::pcap* get_pcap() const { return m_pcap.get(); }
		void log_pcap(char const* filename);

	private:
		struct timer_compare
		{
			bool operator()(asio::high_resolution_timer const* lhs
				, asio::high_resolution_timer const* rhs) const
			{ return lhs->expiry() < rhs->expiry(); }
		};

		configuration& m_config;

		std::unique_ptr<aux::pcap> m_pcap;

		// all non-expired timers
		std::mutex m_timer_queue_mutex;
		using timer_queue_t = std::multiset<asio::high_resolution_timer*, timer_compare, aux::mallocator<asio::high_resolution_timer*>>;
		timer_queue_t m_timer_queue;

		// these are the io services that represent nodes on the network
		std::unordered_set<asio::io_context*, std::hash<asio::io_context*>, std::equal_to<asio::io_context*>, aux::mallocator<asio::io_context*>> m_nodes;

		using listen_sockets_t = std::map<asio::ip::tcp::endpoint, asio::ip::tcp::socket*>;
		using listen_socket_iter_t = listen_sockets_t::iterator;
		listen_sockets_t m_listen_sockets;

		using udp_sockets_t = std::map<asio::ip::udp::endpoint, asio::ip::udp::socket*>;
		using udp_socket_iter_t = udp_sockets_t::iterator;
		udp_sockets_t m_udp_sockets;

		// used for internal timers. this is a pimpl sine our io_context is
		// incomplete at this point
		std::unique_ptr<asio::io_context> m_internal_ios;

		// the next port to use for an outgoing connection, where the port is not
		// specified. We want this to be as unique as possible, to distinguish the
		// TCP streams.
		std::uint16_t m_next_bind_port = 2000;

		bool m_stopped = false;

		// underlying message queue
		boost::asio::io_context m_service;
	};

	namespace asio {

	using boost::asio::async_write;
	using boost::asio::async_read;

	// boost.asio compatible io_context class that simulates the network
	// and time.
	struct SIMULATOR_DECL io_context : boost::asio::execution_context
	{
		io_context(sim::simulation& sim);
		io_context(sim::simulation& sim, ip::address const& ip);
		io_context(sim::simulation& sim, std::vector<ip::address> const& ips);
		io_context(std::size_t threads_hint = 0);
		~io_context();

		// not copyable and non movable (it's not movable because we currently
		// keep pointers to the io_context instances in the simulator object)
		io_context(io_context const&) = delete;
		io_context(io_context&&) = delete;
		io_context& operator=(io_context const&) = delete;
		io_context& operator=(io_context&&) = delete;

		std::size_t run(boost::system::error_code& ec);
		std::size_t run();

		std::size_t poll(boost::system::error_code& ec);
		std::size_t poll();

		std::size_t poll_one(boost::system::error_code& ec);
		std::size_t poll_one();

		void stop();
		bool stopped() const;
		void restart();

		template <typename Handler, typename Allocator>
		void dispatch_impl(Handler handler, Allocator const& a)
		{ get_internal_service().get_executor().dispatch(std::move(handler), a); }

		template <typename Handler, typename Allocator>
		void post_impl(Handler handler, Allocator const& a)
		{ get_internal_service().get_executor().post(std::move(handler), a); }

		template <typename Handler, typename Allocator>
		void defer_impl(Handler handler, Allocator const& a)
		{ get_internal_service().get_executor().defer(std::move(handler), a); }

		// internal interface
		boost::asio::io_context& get_internal_service();

		void add_timer(high_resolution_timer* t);
		void remove_timer(high_resolution_timer* t);

		ip::tcp::endpoint bind_socket(ip::tcp::socket* socket, ip::tcp::endpoint ep
			, boost::system::error_code& ec);
		void unbind_socket(ip::tcp::socket* socket
			, ip::tcp::endpoint const& ep);
		void rebind_socket(asio::ip::tcp::socket* s, asio::ip::tcp::endpoint ep);

		ip::udp::endpoint bind_udp_socket(ip::udp::socket* socket, ip::udp::endpoint ep
			, boost::system::error_code& ec);
		void unbind_udp_socket(ip::udp::socket* socket
			, ip::udp::endpoint const& ep);
		void rebind_udp_socket(asio::ip::udp::socket* socket, asio::ip::udp::endpoint ep);

		std::shared_ptr<aux::channel> internal_connect(ip::tcp::socket* s
			, ip::tcp::endpoint const& target, boost::system::error_code& ec);

		route find_udp_socket(asio::ip::udp::socket const& socket
			, ip::udp::endpoint const& ep);

		route const& get_outgoing_route(ip::address ip) const
		{ return m_outgoing_route.find(ip)->second; }

		route const& get_incoming_route(ip::address ip) const
		{ return m_incoming_route.find(ip)->second; }

		int get_path_mtu(const asio::ip::address& source, const asio::ip::address& dest) const;
		std::vector<ip::address> const& get_ips() const { return m_ips; }

		sim::simulation& sim() { return m_sim; }

		using executor_type = io_executor;
		executor_type get_executor() { return executor_type(*this); }

	private:

		sim::simulation& m_sim;
		std::vector<ip::address> m_ips;

		// these are determined by the configuration. They may include NATs and
		// DSL modems (queues)
		std::map<ip::address, route> m_outgoing_route;
		std::map<ip::address, route> m_incoming_route;

		bool m_stopped = false;
	};

	template <typename Handler, typename Allocator>
	void io_executor::dispatch(Handler handler, Allocator const& a) const
	{ m_ctx->dispatch_impl(std::move(handler), a); }

	template <typename Handler, typename Allocator>
	void io_executor::post(Handler handler, Allocator const& a) const
	{ m_ctx->post_impl(std::move(handler), a); }

	template <typename Handler, typename Allocator>
	void io_executor::defer(Handler handler, Allocator const& a) const
	{ m_ctx->defer_impl(std::move(handler), a); }

	template <typename Protocol>
	io_executor
	socket_base<Protocol>::get_executor()
	{ return io_executor(m_io_service); }

	template <typename Protocol>
	route socket_base<Protocol>::get_incoming_route()
	{
		route ret = m_io_service.get_incoming_route(m_bound_to.address());
		assert(m_forwarder);
		ret.append(std::static_pointer_cast<sim::sink>(m_forwarder));
		return ret;
	}

	template <typename Protocol>
	route socket_base<Protocol>::get_outgoing_route()
	{
		return route(m_io_service.get_outgoing_route(m_bound_to.address()));
	}

	} // asio

	// user supplied configuration of the network to simulate
	struct SIMULATOR_DECL configuration
	{
		virtual ~configuration() {}

		// build the network
		virtual void build(simulation& sim) = 0;

		// return the hops on the network packets from src to dst need to traverse
		virtual route channel_route(asio::ip::address src
			, asio::ip::address dst) = 0;

		// return the hops an incoming packet to ep need to traverse before
		// reaching the socket (for instance a NAT)
		virtual route incoming_route(asio::ip::address ip) = 0;

		// return the hops an outgoing packet from ep need to traverse before
		// reaching the network (for instance a DSL modem)
		virtual route outgoing_route(asio::ip::address ip) = 0;

		// return the path MTU between the two IP addresses
		// For TCP sockets, this will be called once when the connection is
		// established. For UDP sockets it's called for every burst of packets
		// that are sent
		virtual int path_mtu(asio::ip::address ip1, asio::ip::address ip2) = 0;

		// called for every hostname lookup made by the client. ``reqyestor`` is
		// the node performing the lookup, ``hostname`` is the name being looked
		// up. Resolve the name into addresses and fill in ``result`` or set
		// ``ec`` if the hostname is not found or some other error occurs. The
		// return value is the latency of the lookup. The client's callback won't
		// be called until after waiting this long.
		virtual chrono::high_resolution_clock::duration hostname_lookup(
			asio::ip::address const& requestor
			, std::string hostname
			, std::vector<asio::ip::address>& result
			, boost::system::error_code& ec) = 0;

		virtual void clear() = 0;
	};

	struct SIMULATOR_DECL default_config : configuration
	{
		default_config() : m_sim(nullptr) {}

		void build(simulation& sim) override;
		route channel_route(asio::ip::address src, asio::ip::address dst) override;
		route incoming_route(asio::ip::address ip) override;
		route outgoing_route(asio::ip::address ip) override;
		int path_mtu(asio::ip::address ip1, asio::ip::address ip2) override;
		chrono::high_resolution_clock::duration hostname_lookup(
			asio::ip::address const& requestor
			, std::string hostname
			, std::vector<asio::ip::address>& result
			, boost::system::error_code& ec) override;

		void clear() override;
	protected:
		std::shared_ptr<queue> m_network;
		std::map<asio::ip::address, std::shared_ptr<queue>> m_incoming;
		std::map<asio::ip::address, std::shared_ptr<queue>> m_outgoing;
		simulation* m_sim;
	};

	namespace aux
	{
		/* the channel can be in the following states:
			1. handshake-1 - the initiating socket has sent SYN
			2. handshake-2 - the accepting connection has sent SYN+ACK
			3. handshake-3 - the initiating connection has received the SYN+ACK and
			                 considers the connection open, but the 3rd handshake
			                 message is still in flight.
			4. connected   - the accepting side has received the 3rd handshake
			                 packet and considers it open

			Whenever a connection attempt is made to a listening socket, as long as
			there is still space in the incoming socket queue, the accepting side
			will always respond immediately and complete the handshake, then wait
			until the user calls async_accept (which in this case would complete
			immediately).
		*/
		struct SIMULATOR_DECL channel
		{
			channel() {}
			// index 0 is the incoming route to the socket that initiated the connection.
			// index 1 may be empty while the connection is half-open
			route hops[2];

			// the actual endpoint of each end of the channel
			asio::ip::tcp::endpoint ep[2];

			// observable endpoint of each side of the channel. This is not how you
			// see yourself, just the other end
			asio::ip::tcp::endpoint visible_ep[2];

			// the number of bytes sent from respective direction
			// this is used to simulate the TCP sequence number, so it deliberately
			// is meant to wrap at 32 bits
			std::uint32_t bytes_sent[2];

			int remote_idx(asio::ip::tcp::endpoint const& self) const;
			int self_idx(asio::ip::tcp::endpoint const& self) const;
		};

	} // aux

	void SIMULATOR_DECL dump_network_graph(simulation const& s, const std::string& filename);
}

#ifdef _MSC_VER
#pragma warning(pop)
#endif

#endif // SIMULATOR_HPP_INCLUDED

max_stars_repo_name: misaki-sxc/libtorrentSuzu

max_issues_repo_name: misaki-sxc/libtorrentSuzu

max_forks_repo_name: misaki-sxc/libtorrentSuzu


text: #include <boost/asio/windows/random_access_handle.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/* FortuneAlgorithm
 * Copyright (C) 2018 Pierre Vigier
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published
 * by the Free Software Foundation, either version 3 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

// STL
#include <boost/bind.hpp>
#include <chrono>
#include <iostream>
#include <math.h>
#include <random>
#include <string>
#include <vector>
// SFML
//#include <SFML/Graphics.hpp>
// My includes
#include "FortuneAlgorithm.h"

#include "ros/ros.h"
#include "std_msgs/String.h"
//#include "voronoi_alg/RobotPos.h"
#include "geometry_msgs/Polygon.h"
#include "geometry_msgs/Polygon.h"
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"

constexpr float WINDOW_WIDTH = 600.0f;
constexpr float WINDOW_HEIGHT = 600.0f;
constexpr float POINT_RADIUS = 0.005f;
constexpr float OFFSET = 1.0f;

std::vector<std::string> split_string_by_spaces(std::string inputstr)
{
    std::stringstream tmpss(inputstr);
    std::vector<std::string> out;
    std::string tmps;
    while (getline(tmpss, tmps, ' ')) {
        out.push_back(tmps);
    }
    return out;
}

void rotateWithMat(float ax, float ay, float costh, float sinth, float* outx, float* outy)
{
    float newx, newy;
    newx = ax * costh - ay * sinth;
    newy = ax * sinth + ay * costh;
    *outx = newx;
    *outy = newy;
}
void cornersToRotangles(std::vector<std::vector<float> > corners, float* costh, float* sinth)
{
    float ax, ay, bx, by, newcth, newsth;
    ax = corners.at(0).at(0);
    ay = corners.at(0).at(1);
    bx = corners.at(1).at(0);
    by = corners.at(1).at(1);
    *costh = (bx - ax) / sqrt((by - ay) * (by - ay) + (bx - ax) * (bx - ax));
    *sinth = (by - ay) / sqrt((by - ay) * (by - ay) + (bx - ax) * (bx - ax));
}
std::vector<std::vector<float> > transformRobLocsAndCorners(std::vector<std::vector<float> > corners,
    std::vector<std::vector<float> > robLocs, float* xmin, float* xmax, float* ymin, float* ymax)
{
    //find the transformed robot locations:
    float costh, sinth, tfx, tfy;
    std::vector<float> tfLoc{ 0, 0 };
    cornersToRotangles(corners, &costh, &sinth);
    std::vector<std::vector<float> > transformedRobLocs;
    for (size_t i = 0; i < robLocs.size(); i++) {
        rotateWithMat(robLocs.at(i).at(0), robLocs.at(i).at(1), costh, sinth, &tfx, &tfy);
        tfLoc.at(0) = tfx;
        tfLoc.at(1) = tfy;
        transformedRobLocs.push_back(tfLoc);
    }
    //find the smallest and largest x and y value from the corner vector:
    float tmpXmin, tmpXmax, tmpYmin, tmpYmax;
    for (size_t i = 0; i < corners.size(); i++) {
        rotateWithMat(corners.at(i).at(0), corners.at(i).at(1), costh, sinth, &tfx, &tfy);
        if (i == 0) {
            tmpXmin = tfx;
            tmpXmax = tfx;
            tmpYmin = tfy;
            tmpYmax = tfy;
        }
        if (tfx < tmpXmin) {
            tmpXmin = tfx;
        }
        if (tfx > tmpXmax) {
            tmpXmax = tfx;
        }
        if (tfy < tmpYmin) {
            tmpYmin = tfy;
        }
        if (tfy > tmpYmax) {
            tmpYmax = tfy;
        }
    }
    *xmin = tmpXmin;
    *xmax = tmpXmax;
    *ymin = tmpYmin;
    *ymax = tmpYmax;
    return transformedRobLocs;
}
std::vector<std::vector<float> > transformTargetLocs(
    std::vector<std::vector<float> > targetLocs,
    std::vector<std::vector<float> > corners)
{
    float costh, sinth, tfx, tfy;
    std::vector<float> tfLoc{ 0, 0 };
    cornersToRotangles(corners, &costh, &sinth);
    std::vector<std::vector<float> > transformedTargetLocs;
    for (size_t i = 0; i < targetLocs.size(); i++) {
        rotateWithMat(targetLocs.at(i).at(0), targetLocs.at(i).at(1), costh, -sinth, &tfx, &tfy);
        tfLoc.at(0) = tfx;
        tfLoc.at(1) = tfy;
        transformedTargetLocs.push_back(tfLoc);
    }
    return transformedTargetLocs;
}
void computeCentroid(const std::vector<std::vector<float> > vertices, std::vector<float>* centroid)
{
    float centroidX = 0, centroidY = 0;
    float det = 0, tempDet = 0;
    unsigned int j = 0;
    unsigned int nVertices = (unsigned int)vertices.size();

    for (unsigned int i = 0; i < nVertices; i++) {
        // closed polygon
        if (i + 1 == nVertices)
            j = 0;
        else
            j = i + 1;

        // compute the determinant
        tempDet = vertices[i][0] * vertices[j][1] - vertices[j][0] * vertices[i][1];
        det += tempDet;

        centroidX += (vertices[i][0] + vertices[j][0]) * tempDet;
        centroidY += (vertices[i][1] + vertices[j][1]) * tempDet;

        //std::cout << "vertice x: "<< vertices[i][0] << ", vertice y: " << vertices[i][1] << '\n';
    }

    // divide by the total mass of the polygon
    centroidX /= 3 * det;
    centroidY /= 3 * det;

    std::vector<float> newCentroid{ centroidX, centroidY };
    *centroid = newCentroid;
}
void findAllCentroids(std::vector<std::vector<std::vector<float> > > polygons, std::vector<std::vector<float> >* centroids)
{
    std::vector<std::vector<float> > newCentroids;
    std::vector<float> newCentroid;
    for (size_t i = 0; i < polygons.size(); i++) {
        /*std::cout << "polygon " << i << ":" << '\n';
      for (size_t j = 0; j < polygons.at(i).size(); j++) {
        std::cout << "x: " << polygons.at(i).at(j).at(0) << ", y: " << polygons.at(i).at(j).at(1) << '\n';
      }*/
        computeCentroid(polygons.at(i), &newCentroid);
        newCentroids.push_back(newCentroid);
    }
    *centroids = newCentroids;
}
std::vector<Vector2> generatePoints(int nbPoints)
{
    uint64_t seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::cout << "seed: " << seed << '\n';
    std::default_random_engine generator(seed);
    std::uniform_real_distribution<double> distribution(0.0, 1.0);

    std::vector<Vector2> points;
    for (int i = 0; i < nbPoints; ++i)
        points.push_back(Vector2{ distribution(generator), distribution(generator) });

    return points;
}
float cross(std::vector<float> a, std::vector<float> b)
{
    //cross product: assumes that input vectors have z=0 and computes the z-value
    //of the cross product.
    return a.at(0) * b.at(1) - a.at(1) * b.at(0);
}
bool is_counterclockwise(std::vector<std::vector<float> > points)
{
    //gets a list of points along the edge, determines if the points are in
    //counterclockwise order or not.

    //vectors from first to second and first to third point
    std::vector<float> p12, p13;
    p12.push_back(points.at(1).at(0) - points.at(0).at(0));
    p12.push_back(points.at(1).at(1) - points.at(0).at(1));
    p13.push_back(points.at(2).at(0) - points.at(0).at(0));
    p13.push_back(points.at(2).at(1) - points.at(0).at(1));
    //if the cross product of these vectors is greater than zero, the point 3 must
    //be left of the line from point 1 to point 2.
    return cross(p12, p13) > 0;
}
void detectPolygon(VoronoiDiagram& diagram, std::vector<std::vector<std::vector<float> > >* polys)
{
    bool polygonFound = false;
    int polysFound = 0;
    std::vector<float> origvec{ 0, 0 }, destvec{ 0, 0 };
    std::vector<std::vector<float> > cornerPoints;
    std::vector<std::vector<std::vector<float> > > allPolys;
    for (std::size_t i = 0; i < diagram.getNbSites(); ++i) {
        const VoronoiDiagram::Site* site = diagram.getSite(i);
        Vector2 center = site->point;
        VoronoiDiagram::Face* face = site->face;
        VoronoiDiagram::HalfEdge* halfEdge = face->outerComponent;
        if (halfEdge == nullptr)
            continue;
        while (halfEdge->prev != nullptr) {
            halfEdge = halfEdge->prev;
            if (halfEdge == face->outerComponent)
                break;
        }
        VoronoiDiagram::HalfEdge* start = halfEdge;
        if (start->origin != nullptr) {
            origvec.at(0) = halfEdge->origin->point.x;
            origvec.at(1) = halfEdge->origin->point.y;
            cornerPoints.push_back(origvec);
        }
        while (halfEdge != nullptr) {
            if (halfEdge->origin != nullptr && halfEdge->destination != nullptr) {
                //Vector2 origin = (halfEdge->origin->point - center) * OFFSET + center;
                //Vector2 destination = (halfEdge->destination->point - center) * OFFSET + center;
                origvec.at(0) = halfEdge->origin->point.x;
                origvec.at(1) = halfEdge->origin->point.y;
                destvec.at(0) = halfEdge->destination->point.x;
                destvec.at(1) = halfEdge->destination->point.y;
                float diff_from_prev = (origvec.at(0) - cornerPoints.back().at(0)) * (origvec.at(0) - cornerPoints.back().at(0)) + (origvec.at(1) - cornerPoints.back().at(1)) * (origvec.at(1) - cornerPoints.back().at(1));
                if (diff_from_prev > 0.0001)
                    std::cout << "Edges in wrong order!" << '\n';
                cornerPoints.push_back(destvec);
            }
            halfEdge = halfEdge->next;
            if (halfEdge == start) {
                polygonFound = true;
                break;
            }
        }
        //std::cout << "distance from start to end: " << diff_from_start << '\n';
        //std::cout << "Number of edges in latest polygon: " << cornerPoints.size() << '\n';
        if (is_counterclockwise(cornerPoints)) {
            //std::cout << "current polygon is counterclockwise" << '\n';
        } else {
            std::cout << "current polygon is clockwise" << '\n';
        }
        allPolys.push_back(cornerPoints);
        cornerPoints.clear();
        if (polygonFound)
            polysFound++;
        polygonFound = false;
        //if (polygonFound)
        //  break;
    }
    //std::cout << "number of polygons found: " << polysFound << '\n';
    //std::cout << "latest origin: x: " << origvec.at(0) << ", y: " << origvec.at(1) << '\n';
    *polys = allPolys;
}
void countEdges(VoronoiDiagram& diagram)
{
    int edgenumber = 0;
    for (std::size_t i = 0; i < diagram.getNbSites(); ++i) {
        const VoronoiDiagram::Site* site = diagram.getSite(i);
        Vector2 center = site->point;
        VoronoiDiagram::Face* face = site->face;
        VoronoiDiagram::HalfEdge* halfEdge = face->outerComponent;
        if (halfEdge == nullptr)
            continue;
        while (halfEdge->prev != nullptr) {
            halfEdge = halfEdge->prev;
            if (halfEdge == face->outerComponent)
                break;
        }
        VoronoiDiagram::HalfEdge* start = halfEdge;
        while (halfEdge != nullptr) {
            if (halfEdge->origin != nullptr && halfEdge->destination != nullptr) {
                Vector2 origin = (halfEdge->origin->point - center) * OFFSET + center;
                Vector2 destination = (halfEdge->destination->point - center) * OFFSET + center;
                edgenumber++;
                //drawEdge(window, origin, destination, sf::Color::Red);
            }
            halfEdge = halfEdge->next;
            if (halfEdge == start)
                break;
        }
    }
    //std::cout << "edgenumber: " << edgenumber << std::endl;
}
std::vector<std::vector<float> > vector2toFloats(std::vector<Vector2> points)
{
    std::vector<float> p{ 0, 0 };
    std::vector<std::vector<float> > res;
    for (size_t i = 0; i < points.size(); i++) {
        p.at(0) = points.at(i).x;
        p.at(1) = points.at(i).y;
        res.push_back(p);
    }
    return res;
}
std::vector<Vector2> floatsToVector2(std::vector<std::vector<float> > points)
{
    Vector2 p;
    std::vector<Vector2> res;
    for (size_t i = 0; i < points.size(); i++) {
        p.x = points.at(i).at(0);
        p.y = points.at(i).at(1);
        res.push_back(p);
    }
    return res;
}
VoronoiDiagram generateRandomDiagram(
    std::vector<std::vector<float> >* generatedPoints,
    Vector2 bottomLeftCorner, Vector2 topRightCorner)
{
    /*
    // Generate points
    std::vector<Vector2> points = generatePoints(nbPoints);
    // Write generated points to generatedPoints:
    std::vector<std::vector<float>> tmp = vector2toFloats(points);
    *generatedPoints = tmp;*/
    std::vector<Vector2> points = floatsToVector2(*generatedPoints);

    // Construct diagram
    FortuneAlgorithm algorithm(points);
    auto start = std::chrono::steady_clock::now();
    algorithm.construct();
    auto duration = std::chrono::steady_clock::now() - start;
    //std::cout << "construction: " <<
    //std::chrono::duration_cast<std::chrono::milliseconds>(duration).count() <<
    //"ms" << '\n';

    // Bound the diagram
    start = std::chrono::steady_clock::now();
    algorithm.bound(Box{ bottomLeftCorner.x - 0.05, bottomLeftCorner.y - 0.05,
        topRightCorner.x + 0.05, topRightCorner.y + 0.05 }); // Take the bounding box slightly bigger than the intersection box
    duration = std::chrono::steady_clock::now() - start;
    //std::cout << "bounding: " <<
    //std::chrono::duration_cast<std::chrono::milliseconds>(duration).count() <<
    //"ms" << '\n';
    VoronoiDiagram diagram = algorithm.getDiagram();

    // Intersect the diagram with a box
    start = std::chrono::steady_clock::now();
    bool valid = diagram.intersect(Box{ bottomLeftCorner.x, bottomLeftCorner.y,
        topRightCorner.x, topRightCorner.y });
    duration = std::chrono::steady_clock::now() - start;
    //std::cout << "intersection: " <<
    //std::chrono::duration_cast<std::chrono::milliseconds>(duration).count() <<
    //"ms" << '\n';
    if (!valid)
        throw std::runtime_error("An error occured in the box intersection algorithm");

    return diagram;
}
void printPointsWithCentroids(std::vector<std::vector<float> > points, std::vector<std::vector<float> > centroids)
{
    //orig. and new x and y:
    float ox, oy, nx, ny;
    for (size_t i = 0; i < points.size(); i++) {
        ox = points.at(i).at(0);
        oy = points.at(i).at(1);
        nx = centroids.at(i).at(0);
        ny = centroids.at(i).at(1);
        std::cout << "old: <" << ox << "," << oy << ">, new: <" << nx << "," << ny << ">" << '\n';
    }
}
std::vector<std::vector<float> > colVecToRowVec(std::vector<std::vector<float> > colVec)
{
    std::vector<std::vector<float> > rowVec;
    std::vector<float> xRow, yRow;
    for (size_t i = 0; i < colVec.size(); i++) {
        xRow.push_back(colVec.at(i).at(0));
        yRow.push_back(colVec.at(i).at(1));
    }
    rowVec.push_back(xRow);
    rowVec.push_back(yRow);
    return rowVec;
}
std::vector<std::vector<float> > rowVecToColVec(std::vector<std::vector<float> > rowVec)
{
    std::vector<std::vector<float> > colVec;
    std::vector<float> col{ 0, 0 };
    for (size_t i = 0; i < rowVec.at(0).size(); i++) {
        col.at(0) = rowVec.at(0).at(i);
        col.at(1) = rowVec.at(1).at(i);
        colVec.push_back(col);
    }
    return colVec;
}
void cullPoints(float xmin, float xmax, float ymin, float ymax,
    std::vector<std::vector<float> > unculledPoints,
    std::vector<std::vector<float> >* generatedPoints,
    std::vector<std::vector<float> >* unrotatedTargs,
    std::vector<bool>* validPoints)
{

    std::vector<std::vector<float> > generatedPointsTmp, unrotatedTargsTmp;
    std::vector<bool> validPointsTmp;

    std::vector<float> center;
    center.push_back((xmin + xmax) / 2);
    center.push_back((ymin + ymax) / 2);

    for (size_t i = 0; i < unculledPoints.size(); i++) {
        unrotatedTargsTmp.push_back(center);
        if (unculledPoints.at(i).at(0) > xmax || unculledPoints.at(i).at(0) < xmin || unculledPoints.at(i).at(1) > ymax || unculledPoints.at(i).at(1) < ymin) {
            //if (unculledPoints.at(i).at(0)>xmax||unculledPoints.at(i).at(0)<xmin||
            //unculledPoints.at(i).at(1)>ymax||unculledPoints.at(i).at(1)<ymin) {
            validPointsTmp.push_back(0);
        } else {
            validPointsTmp.push_back(1);
            generatedPointsTmp.push_back(unculledPoints.at(i));
        }
    }
    *generatedPoints = generatedPointsTmp;
    *unrotatedTargs = unrotatedTargsTmp;
    *validPoints = validPointsTmp;
}

class Voronoicbc {
public:
    Voronoicbc(const ros::NodeHandle& nh)
    {
      std::string nodenames;
      bool retsucceeded = nh.getParam("robot_names_set", nodenames);
      mnh = nh;
      nodenameVec = split_string_by_spaces(nodenames);
      for (size_t i = 0; i < nodenameVec.size(); i++) {
        robodom_x.push_back(float(i) / 10000);
        robodom_y.push_back(float(i) / 10000);
      }
      for (size_t i = 0; i < nodenameVec.size(); i++) {
        //robodom_subs.push_back(mnh.subscribe(nodenameVec.at(i)+"/odom", 1,
        //&boost::bind(Voronoicbc::robNameVecCb, _1, i), this));
        robodom_subs.push_back(mnh.subscribe(nodenameVec.at(i) + "/odom", 1,
          &Voronoicbc::robNameVecCb, this));

        //robcont_pubs.push_back(mnh.advertise<geometry_msgs::Twist>(
        robcont_pubs.push_back(mnh.advertise<geometry_msgs::Pose2D>(
          nodenameVec.at(i) + "/controller_target", 1));

        robreg_pubs.push_back(mnh.advertise<geometry_msgs::Polygon>(
          nodenameVec.at(i) + "/robot_region",1));

      }
      botLX = -1;
      botLY = -1;
      topRX = 1;
      topRY = 1;
      target_region_x = { -10, -10, 10, 10 };
      target_region_y = { -10, 10, 10, -10 };
      targreg_sub = mnh.subscribe("target_region", 1000, &Voronoicbc::robRegVecCb, this);
      ros::spin();
    };

private:
    //void robRegVecCb(const voronoi_alg::RobotPos::ConstPtr& msg){
    void robRegVecCb(const geometry_msgs::Polygon::ConstPtr& msg)
    {
        //botLX=msg->x.at(0);botLY=msg->y.at(0);topRX=msg->x.at(1);topRY=msg->y.at(1);
        //std::cout << "robRegVecCb called" << '\n';
        std::vector<float> xvals, yvals;
        for (size_t i = 0; i < 4; i++) {
            xvals.push_back(msg->points[i].x);
            yvals.push_back(msg->points[i].y);
        }
        //target_region_x = msg->x;
        //target_region_y = msg->y;
        target_region_x = xvals;
        target_region_y = yvals;
        //eli tässä funktiossa callback, ja robojen paikat vaan luetaan jostain... kait?
        //jos nyt toistaseks muokkais vaan tota robPosVecCb:tä.
        //rowVecToColVec
    }
    //void robNameVecCb(const geometry_msgs::Twist::ConstPtr& msg, int nodeNum){
    void robNameVecCb(const ros::MessageEvent<nav_msgs::Odometry const>& event)
    {
        //std::cout << "robNameVecCb called" << '\n';
        int nodeNum;
        const ros::M_string& header = event.getConnectionHeader();
        const nav_msgs::Odometry::ConstPtr& msg = event.getMessage();
        std::string curr_node_name = header.at("topic");
        for (size_t i = 0; i < nodenameVec.size(); i++) {
            if (curr_node_name.compare("/" + nodenameVec.at(i) + "/odom") == 0) {
                nodeNum = i;
            }
        }
        //std::cout << "node name read" << '\n';
        //std::cout << "node name: "<< curr_node_name << '\n';
        //std::cout << "node index: " << nodeNum << '\n';

        std::vector<std::vector<float> > posIn, posOut,
            targetAsRowVec{ target_region_x, target_region_y };
        //std::cout << "about to write odometry data:" << '\n';
        //std::cout << "length of robodom_x: " << robodom_x.size() << '\n';
        robodom_x.at(nodeNum) = msg->pose.pose.position.x;
        robodom_y.at(nodeNum) = msg->pose.pose.position.y;
        //std::cout << "odometry data written" << '\n';
        posIn.push_back(robodom_x);
        posIn.push_back(robodom_y);
        std::vector<std::vector<float> > targetAsColVec = rowVecToColVec(targetAsRowVec);
        //float costh, sinth;
        //cornersToRotangles(targetAsColVec, &costh, &sinth);
        //std::vector<std::vector<float>>
        float xmin, xmax, ymin, ymax;
        std::vector<std::vector<float> > unculledPoints = transformRobLocsAndCorners(targetAsColVec,
            rowVecToColVec(posIn), &xmin, &xmax, &ymin, &ymax);
        std::vector<std::vector<float> > generatedPoints;
        std::vector<bool> validPoints;
        std::vector<std::vector<float> > unrotatedTargs;
        cullPoints(xmin, xmax, ymin, ymax, unculledPoints, &generatedPoints, &unrotatedTargs, &validPoints);
        std::vector<std::vector<std::vector<float>>> polygons_out;
        //std::cout << "cullPoints worked" << '\n';
        if (generatedPoints.size() > 1) {
          for (size_t i = 0; i < posIn.at(0).size(); i++) {
            polygons_out.push_back(targetAsColVec);
          }
          std::vector<std::vector<float> > centroids;
          Vector2 bottomLeftCorner, topRightCorner;
          bottomLeftCorner.x = xmin;
          bottomLeftCorner.y = ymin;
          topRightCorner.x = xmax;
          topRightCorner.y = ymax;
          // Write generated points to generatedPoints:
          VoronoiDiagram diagram = generateRandomDiagram(&generatedPoints,
              bottomLeftCorner, topRightCorner);

          //count the edges:
          countEdges(diagram);
          //detect the polygons:
          std::vector<std::vector<std::vector<float>>> polygons;
          detectPolygon(diagram, &polygons);
          //detect the point centers:
          findAllCentroids(polygons, &centroids);
          //write the positions inside the target region to the unrotatedTargs:
          int currCentroidInd = 0;
          //std::cout << "polygons computed" << '\n';
          //std::cout << "size of polygons: " << polygons.size() << polygons.at(0).size() << polygons.at(0).at(0).size() << '\n';
          //std::cout << "size of polygons_out: " << polygons_out.size() << polygons_out.at(0).size() << polygons_out.at(0).at(0).size() << '\n';
          for (size_t i = 0; i < unrotatedTargs.size(); i++) {
              if (validPoints.at(i)) {
                //std::cout << "currCentroidInd: " << currCentroidInd << '\n';
                  unrotatedTargs.at(i) = centroids.at(currCentroidInd);
                  polygons_out.at(i) = transformTargetLocs(
                    polygons.at(currCentroidInd), targetAsColVec);
                  currCentroidInd++;
              }
          }
        }
        //std::cout << "polygons rotated" << '\n';

        posOut = transformTargetLocs(unrotatedTargs, targetAsColVec);
        geometry_msgs::Pose2D reply;
        for (size_t i = 0; i < posOut.size(); i++) {
            reply.x = posOut.at(i).at(0);
            reply.y = posOut.at(i).at(1);
            robcont_pubs.at(i).publish(reply);
        }

        std::vector<geometry_msgs::Polygon> region_replies;
        geometry_msgs::Polygon poly;
        geometry_msgs::Point32 currPoint;
        for (size_t i = 0; i < polygons_out.size(); i++) {
          region_replies.push_back(poly);
          for (size_t j = 0; j < polygons_out.at(i).size(); j++) {
            std::cout << "size of polygons_out.at(i).at(j): " << polygons_out.at(i).at(j).size() << '\n';
            currPoint.x = polygons_out.at(i).at(j).at(0);
            currPoint.y = polygons_out.at(i).at(j).at(1);
            region_replies.at(i).points.push_back(currPoint);
          }
          robreg_pubs.at(i).publish(region_replies.at(i));
        }
        std::cout << "polygons published" << '\n';

        //reply.x = posOut.at(0);reply.y = posOut.at(1);
        //targloc_pub.publish(reply);
        //std::string xcoord_as_str = std::to_string(reply.x.at(0));
        //ROS_INFO("First target x: [%s]", xcoord_as_str.c_str());
        //std::string ycoord_as_str = std::to_string(reply.y.at(0));
        //ROS_INFO("First target y: [%s]", ycoord_as_str.c_str());
        //std::string botlx_as_str = std::to_string(botLX);
        //ROS_INFO("Bottom left x: [%s]", botlx_as_str.c_str());
    }
    float recXcoord = 0, botLX, botLY, topRX, topRY;
    std::vector<float> target_region_x, target_region_y, robodom_x, robodom_y;
    ros::Publisher targloc_pub;
    std::vector<ros::Publisher> robcont_pubs, robreg_pubs;
    ros::Subscriber targloc_sub, targreg_sub;
    std::vector<ros::Subscriber> robodom_subs;
    ros::NodeHandle mnh;
    std::vector<std::string> nodenameVec;
    //std::vector<float> targ_x;
    //std::vector<float> targ_y;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "listener");
    /**
	* NodeHandle is the main access point to communications with the ROS system.
	* The first NodeHandle constructed will fully initialize this node, and the last
	* NodeHandle destructed will close down the node.
	*/
    ros::NodeHandle n;
    Voronoicbc instance(n);
    /*std::size_t nbPoints = 2;
    std::vector<std::vector<float>> centroids;
    // Generate points
    std::vector<Vector2> points = generatePoints(nbPoints);
		Vector2 bottomLeftCorner, topRightCorner;
		bottomLeftCorner.x=-1;bottomLeftCorner.y=-1;
		topRightCorner.x=1;topRightCorner.y=1;
    // Write generated points to generatedPoints:
    std::vector<std::vector<float>> generatedPoints = vector2toFloats(points);

    VoronoiDiagram diagram = generateRandomDiagram(&generatedPoints,
			bottomLeftCorner, topRightCorner);

    std::vector<std::vector<std::vector<float>>> polygons;

    //count the edges:
    countEdges(diagram);
    //detect the first polygon:
    detectPolygon(diagram, &polygons);
    //detect the point centers:
    findAllCentroids(polygons, &centroids);
    //print result:
    printPointsWithCentroids(generatedPoints,centroids);*/

    return 0;
}

max_stars_repo_name: Marsunpaisti/RoboProject

max_issues_repo_name: Marsunpaisti/RoboProject

max_forks_repo_name: Marsunpaisti/RoboProject


text:
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <QDebug>
#include "shortestpath.h"

using namespace boost;

bool shortestPath(size_t nodeNum,
    const std::vector<std::pair<size_t, size_t>> &edges,
    const std::vector<int> &weights,
    size_t start,
    size_t stop,
    std::vector<size_t> *path)
{
    typedef adjacency_list < listS, vecS, undirectedS,
        no_property, property < edge_weight_t, int > > graph_t;
    typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
    typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
    typedef std::pair<size_t, size_t> Edge;

    size_t edgeNum = edges.size();
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    graph_t g(nodeNum);
    property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
    for (std::size_t j = 0; j < edgeNum; ++j) {
        edge_descriptor e; bool inserted;
        tie(e, inserted) = add_edge(edges[j].first, edges[j].second, g);
        weightmap[e] = weights[j];
    }
#else
    graph_t g(edges.data(), edges.data() + edgeNum, weights.data(), nodeNum);
    property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
#endif

    std::vector<vertex_descriptor> p(num_vertices(g));
    std::vector<size_t> d(num_vertices(g));
    vertex_descriptor s = vertex(start, g);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    // VC++ has trouble with the named parameters mechanism
    property_map<graph_t, vertex_index_t>::type indexmap = get(vertex_index, g);
    dijkstra_shortest_paths(g, s, &p[0], &d[0], weightmap, indexmap,
                          std::less<size_t>(), closed_plus<size_t>(),
                          (std::numeric_limits<size_t>::max)(), 0,
                          default_dijkstra_visitor());
#else
    dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));
#endif

    auto current = stop;
    while (current != start) {
        path->push_back(current);
        size_t next = p[current];
        if (next == current)
            return false;
        current = next;
    }
    path->push_back(current);

    return true;
}


text: #include <boost/accumulators/statistics/covariance.hpp>


text:
//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Deliberately contains some unicode characters:
//
// boost-no-inspect
//
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/centered_continued_fraction.hpp>
#include <boost/multiprecision/mpfr.hpp>

using boost::math::constants::root_two;
using boost::math::constants::phi;
using boost::math::constants::pi;
using boost::math::constants::e;
using boost::math::constants::zeta_three;
using boost::math::tools::centered_continued_fraction;
using boost::multiprecision::mpfr_float;

int main()
{
    using Real = mpfr_float;
    int p = 10000;
    mpfr_float::default_precision(p);
    auto phi_cfrac = centered_continued_fraction(phi<Real>());
    std::cout << "φ ≈ " << phi_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) << phi_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto pi_cfrac = centered_continued_fraction(pi<Real>());
    std::cout << "π ≈ " << pi_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) << pi_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto rt_cfrac = centered_continued_fraction(root_two<Real>());
    std::cout << "√2 ≈ " << rt_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) <<  rt_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto e_cfrac = centered_continued_fraction(e<Real>());
    std::cout << "e ≈ " << e_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) <<  e_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto z_cfrac = centered_continued_fraction(zeta_three<Real>());
    std::cout << "ζ(3) ≈ " << z_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) <<  z_cfrac.khinchin_geometric_mean() << "\n\n\n";

    // http://jeremiebourdon.free.fr/data/Khintchine.pdf
    std::cout << "The expected Khinchin mean for a random centered continued fraction is 5.45451724454\n";
}

max_stars_repo_name: oleg-alexandrov/math

max_issues_repo_name: oleg-alexandrov/math

max_forks_repo_name: oleg-alexandrov/math


text:
//
// Copyright (c) 2019 The C++ Alliance (https://cppalliance.org)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/CPPAlliance/tls
//

#ifndef BOOST_TLS_STREAM_HPP
#define BOOST_TLS_STREAM_HPP

#include <boost/tls/detail/config.hpp>

namespace boost {
namespace tls {

template<class NextLayer>
class stream
{
public:
};

BOOST_TLS_DECL
int
forty_two() noexcept;

} // tls
} // boost

#ifdef BOOST_TLS_HEADER_ONLY
#include <boost/tls/impl/stream.ipp>
#endif

#endif

max_stars_repo_name: CPPAlliance/tls

max_issues_repo_name: CPPAlliance/tls

max_forks_repo_name: CPPAlliance/tls