tomoto 0.1.0

data/vendor/tomotopy/src/Utils/ThreadPool.hpp

@@ -0,0 +1,150 @@
+#pragma once
+
+/*
+A simple C++11 Thread Pool implementation(https://github.com/progschj/ThreadPool)
+modified by bab2min to have additional parameter threadId
+*/
+
+#include <vector>
+#include <queue>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <stdexcept>
+
+namespace tomoto
+{
+	class ThreadPool {
+	public:
+		ThreadPool(size_t threads = 0, size_t maxQueued = 0);
+		template<class F, class... Args>
+		auto enqueue(F&& f, Args&&... args)
+			->std::future<typename std::result_of<F(size_t, Args...)>::type>;
+		
+		template<class F, class... Args>
+		auto enqueueToAll(F&& f, Args&&... args)
+			->std::vector<std::future<typename std::result_of<F(size_t, Args...)>::type>>;
+
+		~ThreadPool();
+		
+		size_t getNumWorkers() const { return workers.size(); }
+		size_t getNumEnqued() const { return tasks.size(); }
+	private:
+		// need to keep track of threads so we can join them
+		std::vector< std::thread > workers;
+		// the task queue
+		std::queue< std::function<void(size_t)> > shared_task;
+		std::vector< std::queue< std::function<void(size_t)> > > tasks;
+		// synchronization
+		std::mutex queue_mutex;
+		std::condition_variable condition, inputCnd;
+		size_t maxQueued;
+		bool stop;
+	};
+
+
+	// the constructor just launches some amount of workers
+	inline ThreadPool::ThreadPool(size_t threads, size_t _maxQueued)
+		: tasks(threads), maxQueued(_maxQueued), stop(false)
+	{
+		for (size_t i = 0; i < threads; ++i)
+		{
+			workers.emplace_back([this, i]
+			{
+				while (1)
+				{
+					std::function<void(size_t)> task;
+
+					{
+						std::unique_lock<std::mutex> lock(this->queue_mutex);
+						this->condition.wait(lock,
+							[this, i] { return this->stop || !this->shared_task.empty() || !this->tasks[i].empty(); });
+						if (this->stop && this->shared_task.empty() && this->tasks[i].empty()) return;
+						if (this->tasks[i].empty())
+						{
+							task = std::move(this->shared_task.front());
+							this->shared_task.pop();
+						}
+						else
+						{
+							task = std::move(this->tasks[i].front());
+							this->tasks[i].pop();
+						}
+						
+						if (this->maxQueued) this->inputCnd.notify_all();
+					}
+
+					//std::cout << "Start #" << i << std::endl;
+					task(i);
+					//std::cout << "End #" << i << std::endl;
+				}
+			});
+		}
+	}
+
+	// add new work item to the pool
+	template<class F, class... Args>
+	auto ThreadPool::enqueue(F&& f, Args&&... args)
+		-> std::future<typename std::result_of<F(size_t, Args...)>::type>
+	{
+		using return_type = typename std::result_of<F(size_t, Args...)>::type;
+
+		auto task = std::make_shared< std::packaged_task<return_type(size_t)> >(
+			std::bind(std::forward<F>(f), std::placeholders::_1, std::forward<Args>(args)...));
+
+		std::future<return_type> res = task->get_future();
+		{
+			std::unique_lock<std::mutex> lock(queue_mutex);
+
+			// don't allow enqueueing after stopping the pool
+			if (stop) throw std::runtime_error("enqueue on stopped ThreadPool");
+			if (maxQueued && shared_task.size() >= maxQueued)
+			{
+				inputCnd.wait(lock, [&]() { return shared_task.size() < maxQueued; });
+			}
+			shared_task.emplace([task](size_t id) { (*task)(id); });
+		}
+		condition.notify_one();
+		return res;
+	}
+
+	template<class F, class... Args>
+	auto ThreadPool::enqueueToAll(F&& f, Args&&... args)
+		->std::vector<std::future<typename std::result_of<F(size_t, Args...)>::type> >
+	{
+		using return_type = typename std::result_of<F(size_t, Args...)>::type;
+
+		std::vector<std::future<return_type> > ret;
+		std::unique_lock<std::mutex> lock(queue_mutex);
+		for (size_t i = 0; i < workers.size(); ++i)
+		{
+			auto task = std::make_shared< std::packaged_task<return_type(size_t)> >(
+				std::bind(f, std::placeholders::_1, args...));
+
+			ret.emplace_back(task->get_future());
+
+			{
+				// don't allow enqueueing after stopping the pool
+				if (stop) throw std::runtime_error("enqueue on stopped ThreadPool");
+				tasks[i].emplace([task](size_t id) { (*task)(id); });
+			}
+		}
+		condition.notify_all();
+		return ret;
+	}
+
+	// the destructor joins all threads
+	inline ThreadPool::~ThreadPool()
+	{
+		{
+			std::unique_lock<std::mutex> lock(queue_mutex);
+			stop = true;
+		}
+		condition.notify_all();
+		for (std::thread &worker : workers)
+			worker.join();
+	}
+}

data/vendor/tomotopy/src/Utils/Trie.hpp

@@ -0,0 +1,220 @@
+#pragma once
+#include <map>
+#include <unordered_map>
+#include <deque>
+#include <functional>
+#include <iterator>
+
+namespace tomoto
+{
+	template<class _Map>
+	class ConstAccess : public _Map
+	{
+	public:
+		auto operator[](typename _Map::key_type key) const -> typename _Map::mapped_type
+		{
+			auto it = this->find(key);
+			if (it == this->end()) return {};
+			else return it->second;
+		}
+
+		auto operator[](typename _Map::key_type key) -> typename _Map::mapped_type&
+		{
+			auto it = this->find(key);
+			if (it == this->end()) return this->emplace(key, typename _Map::mapped_type{}).first->second;
+			else return it->second;
+		}
+	};
+
+	template<class _Key, class _Value, class _KeyStore = ConstAccess<std::map<_Key, int32_t>>, class _Trie = void>
+	struct Trie
+	{
+		using Node = typename std::conditional<std::is_same<_Trie, void>::value, Trie, _Trie>::type;
+		_KeyStore next = {};
+		int32_t fail = 0;
+		_Value val = {};
+
+		Trie() {}
+		~Trie() {}
+
+		Node* getNext(_Key i) const
+		{
+			return next[i] ? (Node*)this + next[i] : nullptr;
+		}
+
+		Node* getFail() const
+		{
+			return fail ? (Node*)this + fail : nullptr;
+		}
+
+		template<typename _TyIter, typename _FnAlloc>
+		void build(_TyIter first, _TyIter last, const _Value& _val, _FnAlloc&& alloc)
+		{
+			if (first == last)
+			{
+				if (!val) val = _val;
+				return;
+			}
+
+			auto v = *first;
+			if (!getNext(v))
+			{
+				next[v] = alloc() - this;
+			}
+			getNext(v)->build(++first, last, _val, alloc);
+		}
+
+		template<typename _TyIter>
+		Node* findNode(_TyIter begin, _TyIter end)
+		{
+			if (begin == end) return this;
+			auto n = getNext(*begin);
+			if (n) return n->findNode(++begin, end);
+			return nullptr;
+		}
+
+		template<class _Func>
+		void traverse(_Func&& func)
+		{
+			if (val)
+			{
+				if (func(val)) return;
+			}
+			for (auto& p : next)
+			{
+				if (getNext(p.first))
+				{
+					getNext(p.first)->traverse(std::forward<_Func>(func));
+				}
+			}
+			return;
+		}
+
+		template<typename _Fn>
+		void traverse_with_keys(_Fn&& fn, std::vector<_Key>& rkeys)
+		{
+			fn((Node*)this, rkeys);
+
+			for (auto& p : next)
+			{
+				if (p.first)
+				{
+					rkeys.emplace_back(p.first);
+					getNext(p.first)->traverse_with_keys(fn, rkeys);
+					rkeys.pop_back();
+				}
+			}
+		}
+
+		template<class _Iterator>
+		std::pair<_Value*, size_t> findMaximumMatch(_Iterator begin, _Iterator end, size_t idxCnt = 0)
+		{
+			if (begin == end) return std::make_pair(val ? &val : nullptr, idxCnt);
+			auto n = getNext(*begin);
+			if (n)
+			{
+				auto v = n->findMaximumMatch(++begin, end, idxCnt + 1);
+				if (v.first) return v;
+			}
+			return std::make_pair(val ? &val : nullptr, idxCnt);
+		}
+
+		Node* findFail(_Key i) const
+		{
+			if (!fail) // if this is Root
+			{
+				return (Node*)this;
+			}
+			else
+			{
+				if (getFail()->getNext(i)) // if 'i' node exists
+				{
+					return getFail()->getNext(i);
+				}
+				else // or loop for failure of this
+				{
+					return getFail()->findFail(i);
+				}
+			}
+		}
+
+		void fillFail()
+		{
+			std::deque<Node*> dq;
+			for (dq.emplace_back(this); !dq.empty(); dq.pop_front())
+			{
+				auto p = dq.front();
+				for (auto&& kv : p->next)
+				{
+					auto i = kv.first;
+					if (!p->getNext(i)) continue;
+					p->getNext(i)->fail = p->findFail(i) - p->getNext(i);
+					dq.emplace_back(p->getNext(i));
+
+					if (!p->val)
+					{
+						for (auto n = p; n->fail; n = n->getFail())
+						{
+							if (n->val)
+							{
+								p->val = (_Value)-1;
+								break;
+							}
+						}
+					}
+				}
+			}
+		}
+	};
+
+	template<class _Key, class _Value, class _KeyStore = ConstAccess<std::map<_Key, int32_t>>>
+	struct TrieEx : public Trie<_Key, _Value, _KeyStore, TrieEx<_Key, _Value, _KeyStore>>
+	{
+		int32_t parent = 0;
+
+		template<typename _TyIter, typename _FnAlloc>
+		void build(_TyIter first, _TyIter last, const _Value& _val, _FnAlloc&& alloc)
+		{
+			if (first == last)
+			{
+				if (!this->val) this->val = _val;
+				return;
+			}
+
+			auto v = *first;
+			if (!getNext(v))
+			{
+				this->next[v] = alloc() - this;
+				this->getNext(v)->parent = -this->next[v];
+			}
+			this->getNext(v)->build(++first, last, _val, alloc);
+		}
+
+		template<typename _FnAlloc>
+		TrieEx* makeNext(const _Key& k, _FnAlloc&& alloc)
+		{
+			if (!this->next[k])
+			{
+				this->next[k] = alloc() - this;
+				this->getNext(k)->parent = -this->next[k];
+				auto f = this->getFail();
+				if (f)
+				{
+					f = f->makeNext(k, std::forward<_FnAlloc>(alloc));
+					this->getNext(k)->fail = f - this->getNext(k);
+				}
+				else
+				{
+					this->getNext(k)->fail = this - this->getNext(k);
+				}
+			}
+			return this + this->next[k];
+		}
+
+		TrieEx* getParent() const
+		{
+			if (!parent) return nullptr;
+			return (TrieEx*)this + parent;
+		}
+	};
+}

data/vendor/tomotopy/src/Utils/TruncMultiNormal.hpp

@@ -0,0 +1,94 @@
+#pragma once
+#include <numeric>
+#include "MultiNormalDistribution.hpp"
+#include "rtnorm.hpp"
+
+namespace tomoto
+{
+	namespace math
+	{
+		template<typename _Ty, typename _Out, typename _Rng>
+		_Out sampleFromTruncatedMultiNormal(
+			_Out ret,
+			const MultiNormalDistribution<_Ty>& multiNormal,
+			const Eigen::Matrix<_Ty, -1, 1>& lowerBound,
+			const Eigen::Matrix<_Ty, -1, 1>& upperBound,
+			_Rng& rng,
+			size_t iteration)
+		{
+			constexpr _Ty epsilon = 1e-6;
+			const size_t K = ret.size();
+			Eigen::Matrix<_Ty, -1, 1> bias = Eigen::Matrix<_Ty, -1, 1>::Zero(K), lowers, uppers;
+			auto& l = multiNormal.getCovL();
+			ret.setZero();
+
+			std::vector<size_t> ks(K);
+			std::iota(ks.begin(), ks.end(), 0);
+			for (size_t i = 0; i < iteration; ++i)
+			{
+				// shuffle sampling orders except during initialization
+				if (i) std::shuffle(ks.begin(), ks.end(), rng);
+				for (size_t kx = 0; kx < K; ++kx)
+				{
+					size_t k = ks[kx];
+					ret[k] = 0;
+					//bias = multiNormal.mean + l * ret;
+					//bias.tail(K - k) = multiNormal.mean.tail(K - k) + l.block(k, 0, K - k, K) * ret;
+					bias.tail(K - k) = multiNormal.mean.tail(K - k);
+					bias.tail(K - k).noalias() += l.block(k, 0, K - k, K) * ret;
+					lowers = (lowerBound - bias).tail(K - k).array() / l.col(k).tail(K - k).array();
+					uppers = (upperBound - bias).tail(K - k).array() / l.col(k).tail(K - k).array();
+					_Ty nLower = lowers[0], nUpper = uppers[0];
+					if (l(k, k) < 0) std::swap(nLower, nUpper);
+					if (i)
+					{
+						for (size_t j = 1; j < lowers.size(); ++j)
+						{
+							if (l.col(k)(j + k) > epsilon)
+							{
+								if (lowers[j] > nLower) nLower = lowers[j];
+								if (uppers[j] < nUpper) nUpper = uppers[j];
+							}
+							else if (l.col(k)(j + k) < -epsilon)
+							{
+								if (uppers[j] > nLower) nLower = uppers[j];
+								if (lowers[j] < nUpper) nUpper = lowers[j];
+							}
+						}
+					}
+					if (abs(nLower - nUpper) <= 1e-4) ret[k] = (nLower + nUpper) / 2;
+					else
+					{
+						ret[k] = rtnorm::rtnorm(rng, nLower, nUpper);
+					}
+				}
+			}
+			ret = l * ret;
+			ret += multiNormal.mean;
+			return ret;
+		}
+
+		template<typename _Ty, typename _Out, typename _Rng>
+		_Out sampleFromTruncatedMultiNormalRejection(
+			_Out ret,
+			const MultiNormalDistribution<_Ty>& multiNormal,
+			const Eigen::Matrix<_Ty, -1, 1>& lowerBound,
+			const Eigen::Matrix<_Ty, -1, 1>& upperBound,
+			_Rng& rng)
+		{
+			const size_t K = ret.size();
+			auto& l = multiNormal.getCovL();
+			std::normal_distribution<_Ty> normal{};
+			while (1)
+			{
+				for (size_t k = 0; k < K; ++k) ret[k] = normal(rng);
+				ret = l * ret;
+				ret += multiNormal.mean;
+				if ((lowerBound.array() <= ret.array()).all() && (ret.array() <= upperBound.array()).all())
+				{
+					return ret;
+				}
+			}
+		}
+	}
+}