<commit_before>// Copyright (C) 2017 Jérôme Leclercq
// This file is part of the "Nazara Engine - Graphics module"
// For conditions of distribution and use, see copyright notice in Config.hpp

#include <Nazara/Graphics/CullingList.hpp>
#include <Nazara/Graphics/Debug.hpp>

namespace Nz
{
	template<typename T>
	CullingList<T>::~CullingList()
	{
		OnCullingListRelease(this);
	}

	template<typename T>
	std::size_t CullingList<T>::Cull(const Frustumf& frustum, bool* forceInvalidation)
	{
		m_results.clear();

		bool forcedInvalidation = false;

		std::size_t visibleHash = 0U;

		for (NoTestVisibilityEntry& entry : m_noTestList)
		{
			m_results.push_back(entry.renderable);
			Nz::HashCombine(visibleHash, entry.renderable);

			if (entry.forceInvalidation)
			{
				forcedInvalidation = true;
				entry.forceInvalidation = false;
			}
		}

		for (SphereVisibilityEntry& entry : m_sphereTestList)
		{
			if (frustum.Contains(entry.sphere))
			{
				m_results.push_back(entry.renderable);
				Nz::HashCombine(visibleHash, entry.renderable);

				if (entry.forceInvalidation)
				{
					forcedInvalidation = true;
					entry.forceInvalidation = false;
				}
			}
		}

		for (VolumeVisibilityEntry& entry : m_volumeTestList)
		{
			if (frustum.Contains(entry.volume))
			{
				m_results.push_back(entry.renderable);
				Nz::HashCombine(visibleHash, entry.renderable);

				if (entry.forceInvalidation)
				{
					forcedInvalidation = true;
					entry.forceInvalidation = false;
				}
			}
		}

		if (forceInvalidation)
			*forceInvalidation = forcedInvalidation;

		return visibleHash;
	}

	template<typename T>
	typename CullingList<T>::NoTestEntry CullingList<T>::RegisterNoTest(const T* renderable)
	{
		NoTestEntry entry(this, m_noTestList.size());
		m_noTestList.emplace_back(NoTestVisibilityEntry{&entry, renderable, false}); //< Address of entry will be updated when moving

		return entry;
	}

	template<typename T>
	typename CullingList<T>::SphereEntry CullingList<T>::RegisterSphereTest(const T* renderable)
	{
		SphereEntry entry(this, m_sphereTestList.size());
		m_sphereTestList.emplace_back(SphereVisibilityEntry{Nz::Spheref(), &entry, renderable, false}); //< Address of entry will be updated when moving

		return entry;
	}

	template<typename T>
	typename CullingList<T>::VolumeEntry CullingList<T>::RegisterVolumeTest(const T* renderable)
	{
		VolumeEntry entry(this, m_volumeTestList.size());
		m_volumeTestList.emplace_back(VolumeVisibilityEntry{Nz::BoundingVolumef(), &entry, renderable, false}); //< Address of entry will be updated when moving

		return entry;
	}

	// Interface STD
	template<typename T>
	typename CullingList<T>::ResultContainer::iterator CullingList<T>::begin()
	{
		return m_results.begin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::begin() const
	{
		return m_results.begin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::cbegin() const
	{
		return m_results.cbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::cend() const
	{
		return m_results.cend();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::crbegin() const
	{
		return m_results.crbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::crend() const
	{
		return m_results.crend();
	}

	template<typename T>
	bool CullingList<T>::empty() const
	{
		return m_results.empty();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::iterator CullingList<T>::end()
	{
		return m_results.end();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::end() const
	{
		return m_results.end();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::reverse_iterator CullingList<T>::rbegin()
	{
		return m_results.rbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::rbegin() const
	{
		return m_results.rbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::reverse_iterator CullingList<T>::rend()
	{
		return m_results.rend();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::rend() const
	{
		return m_results.rend();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::size_type CullingList<T>::size() const
	{
		return m_results.size();
	}

	template<typename T>
	void CullingList<T>::NotifyForceInvalidation(CullTest type, std::size_t index)
	{
		switch (type)
		{
			case CullTest::NoTest:
			{
				m_noTestList[index].forceInvalidation = true;
				break;
			}

			case CullTest::Sphere:
			{
				m_sphereTestList[index].forceInvalidation = true;
				break;
			}

			case CullTest::Volume:
			{
				m_volumeTestList[index].forceInvalidation = true;
				break;
			}

			default:
				NazaraInternalError("Unhandled culltype");
				break;
		}
	}

	template<typename T>
	void CullingList<T>::NotifyMovement(CullTest type, std::size_t index, void* oldPtr, void* newPtr)
	{
		switch (type)
		{
			case CullTest::NoTest:
			{
				NoTestVisibilityEntry& entry = m_noTestList[index];
				NazaraAssert(entry.entry == oldPtr, "Invalid entry");

				entry.entry = static_cast<NoTestEntry*>(newPtr);
				break;
			}

			case CullTest::Sphere:
			{
				SphereVisibilityEntry& entry = m_sphereTestList[index];
				NazaraAssert(entry.entry == oldPtr, "Invalid sphere entry");

				entry.entry = static_cast<SphereEntry*>(newPtr);
				break;
			}

			case CullTest::Volume:
			{
				VolumeVisibilityEntry& entry = m_volumeTestList[index];
				NazaraAssert(entry.entry == oldPtr, "Invalid volume entry");

				entry.entry = static_cast<VolumeEntry*>(newPtr);
				break;
			}

			default:
				NazaraInternalError("Unhandled culltype");
				break;
		}
	}

	template<typename T>
	void CullingList<T>::NotifyRelease(CullTest type, std::size_t index)
	{
		switch (type)
		{
			case CullTest::NoTest:
			{
				m_noTestList[index] = std::move(m_noTestList.back());
				m_noTestList[index].entry->UpdateIndex(index);
				m_noTestList.pop_back();
				break;
			}

			case CullTest::Sphere:
			{
				m_sphereTestList[index] = std::move(m_sphereTestList.back());
				m_sphereTestList[index].entry->UpdateIndex(index);
				m_sphereTestList.pop_back();
				break;
			}

			case CullTest::Volume:
			{
				m_volumeTestList[index] = std::move(m_volumeTestList.back());
				m_volumeTestList[index].entry->UpdateIndex(index);
				m_volumeTestList.pop_back();
				break;
			}

			default:
				NazaraInternalError("Unhandled culltype");
				break;
		}
	}

	template<typename T>
	void CullingList<T>::NotifySphereUpdate(std::size_t index, const Spheref& sphere)
	{
		m_sphereTestList[index].sphere = sphere;
	}

	template<typename T>
	void CullingList<T>::NotifyVolumeUpdate(std::size_t index, const BoundingVolumef& boundingVolume)
	{
		m_volumeTestList[index].volume = boundingVolume;
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::Entry() :
	m_parent(nullptr)
	{
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::Entry(CullingList* parent, std::size_t index) :
	m_index(index),
	m_parent(parent)
	{
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::Entry(Entry&& entry) :
	m_index(entry.m_index),
	m_parent(entry.m_parent)
	{
		if (m_parent)
			m_parent->NotifyMovement(Type, m_index, &entry, this);

		entry.m_parent = nullptr;
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::~Entry()
	{
		if (m_parent)
			m_parent->NotifyRelease(Type, m_index);
	}

	template<typename T>
	template<CullTest Type>
	void CullingList<T>::Entry<Type>::ForceInvalidation()
	{
		m_parent->NotifyForceInvalidation(Type, m_index);
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>* CullingList<T>::Entry<Type>::GetParent() const
	{
		return m_parent;
	}

	template<typename T>
	template<CullTest Type>
	void CullingList<T>::Entry<Type>::UpdateIndex(std::size_t index)
	{
		m_index = index;
	}

	template<typename T>
	template<CullTest Type>
	#ifdef NAZARA_COMPILER_MSVC
	// MSVC bug
	typename CullingList<T>::Entry<Type>& CullingList<T>::Entry<Type>::operator=(Entry&& entry)
	#else
	typename CullingList<T>::template Entry<Type>& CullingList<T>::Entry<Type>::operator=(Entry&& entry)
	#endif
	{
		m_index = entry.m_index;
		m_parent = entry.m_parent;
		if (m_parent)
			m_parent->NotifyMovement(Type, m_index, &entry, this);

		entry.m_parent = nullptr;

		return *this;
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	CullingList<T>::NoTestEntry::NoTestEntry() :
	Entry<CullTest::NoTest>()
	{
	}

	template<typename T>
	CullingList<T>::NoTestEntry::NoTestEntry(CullingList* parent, std::size_t index) :
	Entry<CullTest::NoTest>(parent, index)
	{
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	CullingList<T>::SphereEntry::SphereEntry() :
	Entry<CullTest::Sphere>()
	{
	}

	template<typename T>
	CullingList<T>::SphereEntry::SphereEntry(CullingList* parent, std::size_t index) :
	Entry<CullTest::Sphere>(parent, index)
	{
	}

	template<typename T>
	void CullingList<T>::SphereEntry::UpdateSphere(const Spheref& sphere)
	{
		this->m_parent->NotifySphereUpdate(this->m_index, sphere);
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	CullingList<T>::VolumeEntry::VolumeEntry() :
	Entry<CullTest::Volume>()
	{
	}

	template<typename T>
	CullingList<T>::VolumeEntry::VolumeEntry(CullingList* parent, std::size_t index) :
	Entry<CullTest::Volume>(parent, index)
	{
	}

	template<typename T>
	void CullingList<T>::VolumeEntry::UpdateVolume(const BoundingVolumef& volume)
	{
		this->m_parent->NotifyVolumeUpdate(this->m_index, volume);
	}
}

#include <Nazara/Graphics/DebugOff.hpp>
<commit_msg>Fix warning<commit_after>// Copyright (C) 2017 Jérôme Leclercq
// This file is part of the "Nazara Engine - Graphics module"
// For conditions of distribution and use, see copyright notice in Config.hpp

#include <Nazara/Graphics/CullingList.hpp>
#include <Nazara/Graphics/Debug.hpp>

namespace Nz
{
	template<typename T>
	CullingList<T>::~CullingList()
	{
		OnCullingListRelease(this);
	}

	template<typename T>
	std::size_t CullingList<T>::Cull(const Frustumf& frustum, bool* forceInvalidation)
	{
		m_results.clear();

		bool forcedInvalidation = false;

		std::size_t visibleHash = 0U;

		for (NoTestVisibilityEntry& entry : m_noTestList)
		{
			m_results.push_back(entry.renderable);
			Nz::HashCombine(visibleHash, entry.renderable);

			if (entry.forceInvalidation)
			{
				forcedInvalidation = true;
				entry.forceInvalidation = false;
			}
		}

		for (SphereVisibilityEntry& entry : m_sphereTestList)
		{
			if (frustum.Contains(entry.sphere))
			{
				m_results.push_back(entry.renderable);
				Nz::HashCombine(visibleHash, entry.renderable);

				if (entry.forceInvalidation)
				{
					forcedInvalidation = true;
					entry.forceInvalidation = false;
				}
			}
		}

		for (VolumeVisibilityEntry& entry : m_volumeTestList)
		{
			if (frustum.Contains(entry.volume))
			{
				m_results.push_back(entry.renderable);
				Nz::HashCombine(visibleHash, entry.renderable);

				if (entry.forceInvalidation)
				{
					forcedInvalidation = true;
					entry.forceInvalidation = false;
				}
			}
		}

		if (forceInvalidation)
			*forceInvalidation = forcedInvalidation;

		return visibleHash;
	}

	template<typename T>
	typename CullingList<T>::NoTestEntry CullingList<T>::RegisterNoTest(const T* renderable)
	{
		NoTestEntry entry(this, m_noTestList.size());
		m_noTestList.emplace_back(NoTestVisibilityEntry{&entry, renderable, false}); //< Address of entry will be updated when moving

		return entry;
	}

	template<typename T>
	typename CullingList<T>::SphereEntry CullingList<T>::RegisterSphereTest(const T* renderable)
	{
		SphereEntry entry(this, m_sphereTestList.size());
		m_sphereTestList.emplace_back(SphereVisibilityEntry{Nz::Spheref(), &entry, renderable, false}); //< Address of entry will be updated when moving

		return entry;
	}

	template<typename T>
	typename CullingList<T>::VolumeEntry CullingList<T>::RegisterVolumeTest(const T* renderable)
	{
		VolumeEntry entry(this, m_volumeTestList.size());
		m_volumeTestList.emplace_back(VolumeVisibilityEntry{Nz::BoundingVolumef(), &entry, renderable, false}); //< Address of entry will be updated when moving

		return entry;
	}

	// Interface STD
	template<typename T>
	typename CullingList<T>::ResultContainer::iterator CullingList<T>::begin()
	{
		return m_results.begin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::begin() const
	{
		return m_results.begin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::cbegin() const
	{
		return m_results.cbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::cend() const
	{
		return m_results.cend();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::crbegin() const
	{
		return m_results.crbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::crend() const
	{
		return m_results.crend();
	}

	template<typename T>
	bool CullingList<T>::empty() const
	{
		return m_results.empty();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::iterator CullingList<T>::end()
	{
		return m_results.end();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_iterator CullingList<T>::end() const
	{
		return m_results.end();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::reverse_iterator CullingList<T>::rbegin()
	{
		return m_results.rbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::rbegin() const
	{
		return m_results.rbegin();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::reverse_iterator CullingList<T>::rend()
	{
		return m_results.rend();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::const_reverse_iterator CullingList<T>::rend() const
	{
		return m_results.rend();
	}

	template<typename T>
	typename CullingList<T>::ResultContainer::size_type CullingList<T>::size() const
	{
		return m_results.size();
	}

	template<typename T>
	void CullingList<T>::NotifyForceInvalidation(CullTest type, std::size_t index)
	{
		switch (type)
		{
			case CullTest::NoTest:
			{
				m_noTestList[index].forceInvalidation = true;
				break;
			}

			case CullTest::Sphere:
			{
				m_sphereTestList[index].forceInvalidation = true;
				break;
			}

			case CullTest::Volume:
			{
				m_volumeTestList[index].forceInvalidation = true;
				break;
			}

			default:
				NazaraInternalError("Unhandled culltype");
				break;
		}
	}

	template<typename T>
	void CullingList<T>::NotifyMovement(CullTest type, std::size_t index, void* /*oldPtr*/, void* newPtr)
	{
		switch (type)
		{
			case CullTest::NoTest:
			{
				NoTestVisibilityEntry& entry = m_noTestList[index];
				NazaraAssert(entry.entry == oldPtr, "Invalid entry");

				entry.entry = static_cast<NoTestEntry*>(newPtr);
				break;
			}

			case CullTest::Sphere:
			{
				SphereVisibilityEntry& entry = m_sphereTestList[index];
				NazaraAssert(entry.entry == oldPtr, "Invalid sphere entry");

				entry.entry = static_cast<SphereEntry*>(newPtr);
				break;
			}

			case CullTest::Volume:
			{
				VolumeVisibilityEntry& entry = m_volumeTestList[index];
				NazaraAssert(entry.entry == oldPtr, "Invalid volume entry");

				entry.entry = static_cast<VolumeEntry*>(newPtr);
				break;
			}

			default:
				NazaraInternalError("Unhandled culltype");
				break;
		}
	}

	template<typename T>
	void CullingList<T>::NotifyRelease(CullTest type, std::size_t index)
	{
		switch (type)
		{
			case CullTest::NoTest:
			{
				m_noTestList[index] = std::move(m_noTestList.back());
				m_noTestList[index].entry->UpdateIndex(index);
				m_noTestList.pop_back();
				break;
			}

			case CullTest::Sphere:
			{
				m_sphereTestList[index] = std::move(m_sphereTestList.back());
				m_sphereTestList[index].entry->UpdateIndex(index);
				m_sphereTestList.pop_back();
				break;
			}

			case CullTest::Volume:
			{
				m_volumeTestList[index] = std::move(m_volumeTestList.back());
				m_volumeTestList[index].entry->UpdateIndex(index);
				m_volumeTestList.pop_back();
				break;
			}

			default:
				NazaraInternalError("Unhandled culltype");
				break;
		}
	}

	template<typename T>
	void CullingList<T>::NotifySphereUpdate(std::size_t index, const Spheref& sphere)
	{
		m_sphereTestList[index].sphere = sphere;
	}

	template<typename T>
	void CullingList<T>::NotifyVolumeUpdate(std::size_t index, const BoundingVolumef& boundingVolume)
	{
		m_volumeTestList[index].volume = boundingVolume;
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::Entry() :
	m_parent(nullptr)
	{
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::Entry(CullingList* parent, std::size_t index) :
	m_index(index),
	m_parent(parent)
	{
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::Entry(Entry&& entry) :
	m_index(entry.m_index),
	m_parent(entry.m_parent)
	{
		if (m_parent)
			m_parent->NotifyMovement(Type, m_index, &entry, this);

		entry.m_parent = nullptr;
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>::Entry<Type>::~Entry()
	{
		if (m_parent)
			m_parent->NotifyRelease(Type, m_index);
	}

	template<typename T>
	template<CullTest Type>
	void CullingList<T>::Entry<Type>::ForceInvalidation()
	{
		m_parent->NotifyForceInvalidation(Type, m_index);
	}

	template<typename T>
	template<CullTest Type>
	CullingList<T>* CullingList<T>::Entry<Type>::GetParent() const
	{
		return m_parent;
	}

	template<typename T>
	template<CullTest Type>
	void CullingList<T>::Entry<Type>::UpdateIndex(std::size_t index)
	{
		m_index = index;
	}

	template<typename T>
	template<CullTest Type>
	#ifdef NAZARA_COMPILER_MSVC
	// MSVC bug
	typename CullingList<T>::Entry<Type>& CullingList<T>::Entry<Type>::operator=(Entry&& entry)
	#else
	typename CullingList<T>::template Entry<Type>& CullingList<T>::Entry<Type>::operator=(Entry&& entry)
	#endif
	{
		m_index = entry.m_index;
		m_parent = entry.m_parent;
		if (m_parent)
			m_parent->NotifyMovement(Type, m_index, &entry, this);

		entry.m_parent = nullptr;

		return *this;
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	CullingList<T>::NoTestEntry::NoTestEntry() :
	Entry<CullTest::NoTest>()
	{
	}

	template<typename T>
	CullingList<T>::NoTestEntry::NoTestEntry(CullingList* parent, std::size_t index) :
	Entry<CullTest::NoTest>(parent, index)
	{
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	CullingList<T>::SphereEntry::SphereEntry() :
	Entry<CullTest::Sphere>()
	{
	}

	template<typename T>
	CullingList<T>::SphereEntry::SphereEntry(CullingList* parent, std::size_t index) :
	Entry<CullTest::Sphere>(parent, index)
	{
	}

	template<typename T>
	void CullingList<T>::SphereEntry::UpdateSphere(const Spheref& sphere)
	{
		this->m_parent->NotifySphereUpdate(this->m_index, sphere);
	}

	//////////////////////////////////////////////////////////////////////////

	template<typename T>
	CullingList<T>::VolumeEntry::VolumeEntry() :
	Entry<CullTest::Volume>()
	{
	}

	template<typename T>
	CullingList<T>::VolumeEntry::VolumeEntry(CullingList* parent, std::size_t index) :
	Entry<CullTest::Volume>(parent, index)
	{
	}

	template<typename T>
	void CullingList<T>::VolumeEntry::UpdateVolume(const BoundingVolumef& volume)
	{
		this->m_parent->NotifyVolumeUpdate(this->m_index, volume);
	}
}

#include <Nazara/Graphics/DebugOff.hpp>
<|endoftext|>


<commit_before>//
// Copyright (c) 2013-2014 Christoph Malek
// See LICENSE for more information.
//

#ifndef RJ_CORE_GAME_HANDLER_HPP
#define RJ_CORE_GAME_HANDLER_HPP

#include "game.hpp"
#include "game_window.hpp"
#include "render.hpp"
#include <rectojump/game/background/background_manager.hpp>
#include <rectojump/game/camera.hpp>
#include <rectojump/game/editor/editor.hpp>
#include <rectojump/game/main_menu/main_menu.hpp>
#include <rectojump/game/popup_manager.hpp>
#include <rectojump/global/common.hpp>
#include <rectojump/global/config_settings.hpp>
#include <rectojump/shared/level_manager/level_manager.hpp>
#include <rectojump/shared/data_manager.hpp>
#include <rectojump/shared/data_store.hpp>

#include <mlk/containers/container_utl.h>
#include <mlk/tools/bitset.h>

namespace rj
{
	class game_handler
	{
	public:
		// type decls
		using data_store_type = data_store<sf::Texture>;

	private:
		game_window& m_game_window;
		data_manager& m_datamgr;
		level_manager& m_lvmgr;

		data_store_type m_datastore{m_datamgr.get_all_containing_as_map_as<sf::Texture>(".png")};
		camera m_default_camera;
		render<game_handler> m_render;
		background_manager m_backgroundmgr;
		game<game_handler> m_game;
		editor<game_handler, decltype(m_game)> m_editor;
		main_menu<game_handler> m_mainmenu;
		popup_manager<game_handler> m_popupmgr;
		debug_info<game_handler, decltype(m_game)> m_debug_info;

		mlk::ebitset<state, state::num> m_current_states;

	public:
		game_handler(game_window& gw, data_manager& dm, level_manager& lm) :
			m_game_window{gw},
			m_datamgr{dm},
			m_lvmgr{lm},
			m_default_camera{gw, {settings::get_window_size<vec2f>() / 2.f, settings::get_window_size<vec2f>()}},
			m_render{*this},
			m_backgroundmgr{m_render},
			m_game{*this},
			m_editor{*this},
			m_mainmenu{*this},
			m_popupmgr{*this},
			m_debug_info{*this}
		{this->init();}

		void load_level(const level_id& id)
		{
			auto& lv(m_lvmgr.get_level(id));
			if(!lv.is_valid())
			{
				m_popupmgr.create_popup<popup_type::error>("Failed to load level: not a valid level");
				return;
			}

			m_backgroundmgr.clear();

			this->deactivate_state(state::main_menu);
			this->activate_state(state::game);

			m_game.load_level(lv);
		}

		template<typename... Args>
		void render_object(Args&&... args)
		{m_game_window.draw(std::forward<Args>(args)...);}

		template<state active_state, typename On_Input, typename... Input_Type_Args>
		void add_input(On_Input&& func, Input_Type_Args&&... keys_btns)
		{
			static_assert(sizeof...(keys_btns) > 0, "at least one key/button/wheel argument needed");
			add_input_helper<(sizeof...(keys_btns) > 1), typename std::tuple_element<0, std::tuple<Input_Type_Args...>>::type, active_state, On_Input, Input_Type_Args...>
			{*this, std::forward<On_Input>(func), std::forward<Input_Type_Args>(keys_btns)...};
		}

		// getters
		auto get_render() noexcept
		-> decltype(m_render)&
		{return m_render;}

		auto get_gamewindow() noexcept
		-> decltype(m_game_window)&
		{return m_game_window;}

		auto get_game() noexcept
		-> decltype(m_game)&
		{return m_game;}

		auto get_backgroundmgr() noexcept
		-> decltype(m_backgroundmgr)&
		{return m_backgroundmgr;}

		auto get_mainmenu() noexcept
		-> decltype(m_mainmenu)&
		{return m_mainmenu;}

		auto get_datastore() noexcept
		-> decltype(m_datastore)&
		{return m_datastore;}

		auto get_datamgr() noexcept
		-> decltype(m_datamgr)&
		{return m_datamgr;}

		auto get_levelmgr() noexcept
		-> decltype(m_lvmgr)&
		{return m_lvmgr;}

		auto get_debuginfo() noexcept
		-> decltype(m_debug_info)&
		{return m_debug_info;}

		auto get_popupmgr() noexcept
		-> decltype(m_popupmgr)&
		{return m_popupmgr;}

	private:
		void init()
		{
			m_current_states |= state::main_menu;

			m_game_window.get_updater().on_update += [this](dur duration){this->update(duration);};
			m_game_window.get_updater().on_render += [this]{this->render();};

			settings::on_changed() +=
			[this]{m_game_window.set_size(settings::get_window_size());};

			m_mainmenu.get_menu_start()->get_items().on_event("editor",
			[this]
			{
				this->deactivate_state(state::main_menu);
				this->activate_state(state::editor);
				m_editor.on_activate();
			});

			this->init_pointers();
			this->init_input();
		}

		void init_pointers() noexcept
		{
			m_game.set_levelmgr(&m_lvmgr);
		}

		void init_input()
		{
			// global input
			on_keys_pressed(key::LShift, key::F) +=
			[this]
			{
				m_game_window.toggle_fullscreen();
				rj::settings::set_fullscreen(m_game_window.get_fullscreen());
			};

			on_keys_pressed(key::LShift, key::T) +=
			[this]{m_game_window.toggle_titlebar();};

			on_keys_pressed(key::LShift, key::D) +=
			[this]{this->toggle_state(state::debug_info);};

			on_keys_pressed(key::LShift, key::Q) +=
			[this]{m_game_window.stop();};

			// window input
			on_key_pressed(key::Escape) +=
			[this]{if(!this->is_active(state::main_menu)) return; m_game_window.stop();};

			// game input
			on_key_pressed(key::P) +=
			[this]
			{
				if(this->is_active(state::game))
				{
					m_current_states.remove(state::game);
					m_current_states |= state::game_menu;
					return;
				}
				else if(this->is_active(state::game_menu))
				{
					m_current_states.remove(state::game_menu);
					m_current_states |= state::game;
					return;
				}
			};

			on_key_pressed(key::D) +=
			[this]{if(!this->is_active(state::game)) return; m_game.get_world().c_player();};
		}

		void activate_state(state s)
		{m_current_states |= s;}

		void deactivate_state(state s)
		{m_current_states.remove(s);}

		void toggle_state(state s)
		{m_current_states.toggle(s);}

		bool is_active(state s) const
		{return m_current_states & s;}

		void set_only_state(state s)
		{
			m_current_states.remove_all();
			m_current_states |= s;
		}

		void update(dur duration)
		{
			m_backgroundmgr.update(duration);

			if(this->is_active(state::game))
				m_game.update(duration);

			else if(this->is_active(state::editor))
			{
				m_editor.update(duration);
				m_game.get_world().get_entityhandler().update(duration);
			}

			else if(this->is_active(state::main_menu))
				m_mainmenu.update(duration);

			if(this->is_active(state::debug_info))
				m_debug_info.update(duration);

			m_popupmgr.update(duration);
		}

		void render()
		{
			m_default_camera.set_changes();
			m_backgroundmgr.render();

			m_default_camera.set_changes();
			// render game when in game menu
			if(this->is_active(state::game) || this->is_active(state::game_menu))
				m_game.render();

			else if(this->is_active(state::editor))
				m_editor.render();

			else if(this->is_active(state::main_menu))
				m_mainmenu.render();

			m_default_camera.set_changes();
			if(this->is_active(state::debug_info))
				m_debug_info.render();

			m_popupmgr.render();
		}

		// multi_args: true, Input_Type = key
		template<bool multi_args, typename Input_Type, state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... keys)
			{
				on_keys_pressed(std::forward<Input_Type_Args>(keys)...) +=
				[&gh, func]{if(gh.is_active(s)) func();};
			}
		};

		// multi_args: false, Input_Type = key
		template<typename Input_Type, state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper<false, Input_Type, s, On_Input, Input_Type_Args...>
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... k)
			{
				on_key_pressed(std::forward<Input_Type_Args>(k)...) +=
				[&gh, func]{if(gh.is_active(s)) func();};
			}
		};

		// multi_args: false, Input_Type = btn
		template<state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper<false, btn, s, On_Input, Input_Type_Args...>
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... b)
			{
				on_btn_pressed(std::forward<Input_Type_Args>(b)...) +=
				[&gh, func](const vec2f& pos){if(gh.is_active(s)) func(pos);};
			}
		};

		// multi_args: false, Input_Type = wheel
		template<state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper<false, wheel, s, On_Input, Input_Type_Args...>
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... w)
			{
				on_mousewheel(std::forward<Input_Type_Args>(w)...) +=
				[&gh, func](const vec2f& pos){if(gh.is_active(s)) func(pos);};
			}
		};
	};
}

#endif // RJ_CORE_GAME_HANDLER_HPP
<commit_msg>game_handler: adapted to error_handler,<commit_after>//
// Copyright (c) 2013-2014 Christoph Malek
// See LICENSE for more information.
//

#ifndef RJ_CORE_GAME_HANDLER_HPP
#define RJ_CORE_GAME_HANDLER_HPP

#include "game.hpp"
#include "game_window.hpp"
#include "render.hpp"
#include <rectojump/game/background/background_manager.hpp>
#include <rectojump/game/camera.hpp>
#include <rectojump/game/editor/editor.hpp>
#include <rectojump/game/main_menu/main_menu.hpp>
#include <rectojump/game/popup_manager.hpp>
#include <rectojump/global/common.hpp>
#include <rectojump/global/config_settings.hpp>
#include <rectojump/shared/level_manager/level_manager.hpp>
#include <rectojump/shared/data_manager.hpp>
#include <rectojump/shared/data_store.hpp>
#include <rectojump/shared/error_handler.hpp>

#include <mlk/containers/container_utl.h>
#include <mlk/tools/bitset.h>

namespace rj
{
	class game_handler
	{
	public:
		// type decls
		using data_store_type = data_store<sf::Texture, sf::Font>;

	private:
		game_window& m_game_window;
		error_handler& m_errorhandler;
		data_manager& m_datamgr;
		level_manager& m_lvmgr;

		data_store_type m_datastore{m_datamgr.get_all_containing_as_map_as<sf::Texture>(".png"), m_datamgr.get_all_containing_as_map_as<sf::Font>(".otf")};
		camera m_default_camera;
		render<game_handler> m_render;
		background_manager m_backgroundmgr;
		game<game_handler> m_game;
		editor<game_handler, decltype(m_game)> m_editor;
		main_menu<game_handler> m_mainmenu;
		popup_manager<game_handler> m_popupmgr;
		debug_info<game_handler, decltype(m_game)> m_debug_info;

		mlk::ebitset<state, state::num> m_current_states;

	public:
		game_handler(game_window& gw, error_handler& eh, data_manager& dm, level_manager& lm) :
			m_game_window{gw},
			m_errorhandler{eh},
			m_datamgr{dm},
			m_lvmgr{lm},
			m_default_camera{gw, {settings::get_window_size<vec2f>() / 2.f, settings::get_window_size<vec2f>()}},
			m_render{*this},
			m_backgroundmgr{m_render},
			m_game{*this},
			m_editor{*this},
			m_mainmenu{*this},
			m_popupmgr{*this},
			m_debug_info{*this}
		{this->init();}

		void load_level(const level_id& id)
		{
			auto& lv(m_lvmgr.get_level(id));
			if(!lv.is_valid())
			{
				m_popupmgr.create_popup<popup_type::error>("Failed to load level: not a valid level");
				return;
			}

			m_backgroundmgr.clear();

			this->deactivate_state(state::main_menu);
			this->activate_state(state::game);

			m_game.load_level(lv);
		}

		template<typename... Args>
		void render_object(Args&&... args)
		{m_game_window.draw(std::forward<Args>(args)...);}

		template<state active_state, typename On_Input, typename... Input_Type_Args>
		void add_input(On_Input&& func, Input_Type_Args&&... keys_btns)
		{
			static_assert(sizeof...(keys_btns) > 0, "at least one key/button/wheel argument needed");
			add_input_helper<(sizeof...(keys_btns) > 1), typename std::tuple_element<0, std::tuple<Input_Type_Args...>>::type, active_state, On_Input, Input_Type_Args...>
			{*this, std::forward<On_Input>(func), std::forward<Input_Type_Args>(keys_btns)...};
		}

		// getters
		auto get_render() noexcept
		-> decltype(m_render)&
		{return m_render;}

		auto get_gamewindow() noexcept
		-> decltype(m_game_window)&
		{return m_game_window;}

		auto get_game() noexcept
		-> decltype(m_game)&
		{return m_game;}

		auto get_backgroundmgr() noexcept
		-> decltype(m_backgroundmgr)&
		{return m_backgroundmgr;}

		auto get_mainmenu() noexcept
		-> decltype(m_mainmenu)&
		{return m_mainmenu;}

		auto get_datastore() noexcept
		-> decltype(m_datastore)&
		{return m_datastore;}

		auto get_datamgr() noexcept
		-> decltype(m_datamgr)&
		{return m_datamgr;}

		auto get_levelmgr() noexcept
		-> decltype(m_lvmgr)&
		{return m_lvmgr;}

		auto get_debuginfo() noexcept
		-> decltype(m_debug_info)&
		{return m_debug_info;}

		auto get_popupmgr() noexcept
		-> decltype(m_popupmgr)&
		{return m_popupmgr;}

	private:
		void init()
		{
			m_current_states |= state::main_menu;

			m_game_window.get_updater().on_update += [this](dur duration){this->update(duration);};
			m_game_window.get_updater().on_render += [this]{this->render();};

			settings::on_changed() +=
			[this]{m_game_window.set_size(settings::get_window_size());};

			m_mainmenu.get_menu_start()->get_items().on_event("editor",
			[this]
			{
				this->deactivate_state(state::main_menu);
				this->activate_state(state::editor);
				m_editor.on_activate();
			});

			this->init_errors();
			this->init_pointers();
			this->init_input();
		}

		void init_errors() noexcept
		{
			auto& font(m_datastore.get<sf::Font>("Fipps-Regular.otf"));

			// add the error instances
			m_errorhandler.create_error_instance(errors::cl_nullptr_access,
			"Something went wrong during program execution.\nPlease consider to make a bugreport at:\nhttps://github.com/Malekblubb/rectojump\n(Please attach 'log.log' and 'error.log'.)",
			font,
			[this]{this->set_only_state(state::error);});

			// TODO: add homepage here
			m_errorhandler.create_error_instance(errors::cl_data, "Some data wasn't loaded, can't run the game.\nPlease download a full release at: ",
			font,
			[this]{this->set_only_state(state::error);});

			// checking font
			if(!m_datamgr.exists_id("Fipps-Regular.otf"))
				mlk::exit_with("main font ('Fipps-Regular.otf') not loaded", mlk::lerr(errors::cl_data)["rj::game_handler"]);

			// checking data
			if(!m_datamgr.exists_ids({"debug_font.png", "Fipps-Regular.otf", "arrow.png",
									  "editor_item_rect.png", "editor_item_triangle.png", "editor_item_triangles4.png",
									  "menu_side.png"}))
				mlk::lerr(errors::cl_data);
		}

		void init_pointers() noexcept
		{
			m_game.set_levelmgr(&m_lvmgr);
		}

		void init_input()
		{
			// global input
			on_keys_pressed(key::LShift, key::F) +=
			[this]
			{
				m_game_window.toggle_fullscreen();
				rj::settings::set_fullscreen(m_game_window.get_fullscreen());
			};

			on_keys_pressed(key::LShift, key::T) +=
			[this]{m_game_window.toggle_titlebar();};

			on_keys_pressed(key::LShift, key::D) +=
			[this]{this->toggle_state(state::debug_info);};

			on_keys_pressed(key::LShift, key::Q) +=
			[this]{m_game_window.stop();};

			// window input
			on_key_pressed(key::Escape) +=
			[this]{if(!this->is_active(state::main_menu)) return; m_game_window.stop();};

			// game input
			on_key_pressed(key::P) +=
			[this]
			{
				if(this->is_active(state::game))
				{
					m_current_states.remove(state::game);
					m_current_states |= state::game_menu;
					return;
				}
				else if(this->is_active(state::game_menu))
				{
					m_current_states.remove(state::game_menu);
					m_current_states |= state::game;
					return;
				}
			};

			on_key_pressed(key::D) +=
			[this]{if(!this->is_active(state::game)) return; m_game.get_world().c_player();};
		}

		void activate_state(state s)
		{m_current_states |= s;}

		void deactivate_state(state s)
		{m_current_states.remove(s);}

		void toggle_state(state s)
		{m_current_states.toggle(s);}

		bool is_active(state s) const
		{return m_current_states & s;}

		void set_only_state(state s)
		{
			m_current_states.remove_all();
			m_current_states |= s;
		}

		void update(dur duration)
		{
			if(this->is_active(state::error))
				return;

			m_backgroundmgr.update(duration);

			if(this->is_active(state::game))
				m_game.update(duration);

			else if(this->is_active(state::editor))
			{
				m_editor.update(duration);
				m_game.get_world().get_entityhandler().update(duration);
			}

			else if(this->is_active(state::main_menu))
				m_mainmenu.update(duration);

			if(this->is_active(state::debug_info))
				m_debug_info.update(duration);

			m_popupmgr.update(duration);
		}

		void render()
		{
			if(this->is_active(state::error))
			{
				m_render(m_errorhandler.get_current_error_instance().error_text);
				return;
			}

			m_default_camera.set_changes();
			m_backgroundmgr.render();

			m_default_camera.set_changes();
			// render game when in game menu
			if(this->is_active(state::game) || this->is_active(state::game_menu))
				m_game.render();

			else if(this->is_active(state::editor))
				m_editor.render();

			else if(this->is_active(state::main_menu))
				m_mainmenu.render();

			m_default_camera.set_changes();
			if(this->is_active(state::debug_info))
				m_debug_info.render();

			m_popupmgr.render();
		}

		// multi_args: true, Input_Type = key
		template<bool multi_args, typename Input_Type, state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... keys)
			{
				on_keys_pressed(std::forward<Input_Type_Args>(keys)...) +=
				[&gh, func]{if(gh.is_active(s)) func();};
			}
		};

		// multi_args: false, Input_Type = key
		template<typename Input_Type, state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper<false, Input_Type, s, On_Input, Input_Type_Args...>
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... k)
			{
				on_key_pressed(std::forward<Input_Type_Args>(k)...) +=
				[&gh, func]{if(gh.is_active(s)) func();};
			}
		};

		// multi_args: false, Input_Type = btn
		template<state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper<false, btn, s, On_Input, Input_Type_Args...>
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... b)
			{
				on_btn_pressed(std::forward<Input_Type_Args>(b)...) +=
				[&gh, func](const vec2f& pos){if(gh.is_active(s)) func(pos);};
			}
		};

		// multi_args: false, Input_Type = wheel
		template<state s, typename On_Input, typename... Input_Type_Args>
		struct add_input_helper<false, wheel, s, On_Input, Input_Type_Args...>
		{
			add_input_helper(const game_handler& gh, On_Input&& func, Input_Type_Args&&... w)
			{
				on_mousewheel(std::forward<Input_Type_Args>(w)...) +=
				[&gh, func](const vec2f& pos){if(gh.is_active(s)) func(pos);};
			}
		};
	};
}

#endif // RJ_CORE_GAME_HANDLER_HPP
<|endoftext|>


<commit_before>#ifndef SIMDIFY_COMMON_DEFERRED_NOT_HPP
#define SIMDIFY_COMMON_DEFERRED_NOT_HPP

#include "../util/inline.hpp"
#include <type_traits>

namespace simd {
    namespace expr {
        template <typename T>
        struct dont_assign_to_deferred_not {
            template <typename T2>
            struct false_when_instantiated : std::integral_constant<bool, false> {};

            static_assert(false_when_instantiated<T>::value,
                "Hello! You are trying to assign to a varible of type deferred_not<T>. "
                "This is definitely not something you want to do. "
                "What probably happened is that you used *auto* to store a calculation result, "
                "but the type of the result is not what you expect. "
                "To solve the problem, don't use *auto* on that particular line.");
        };

        template <typename T>
        struct deferred_not {
            using vector_t = typename T::vector_t;
            using scalar_t = typename T::scalar_t;
            using mask_t = typename T::mask_t;
            using storage_t = typename T::storage_t;
            enum : std::size_t { width = T::width };

            SIMDIFY_INL constexpr explicit deferred_not(const T& r) : neg(r) {}

            template <typename Rhs>
            void operator=(const Rhs& r) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL typename T::mask_t mask() const { return ~neg.mask(); }
            SIMDIFY_INL typename T::scalar_t first_element() const { return ~neg.first_element(); }

            SIMDIFY_INL const T reduce(typename T::binary_op_t f) {
                T pos(~neg);
                return pos.reduce(f);
            }

            SIMDIFY_INL const vector_t data() const {
                T pos(~neg);
                return pos.data();
            }

            template <typename Rhs>
            void aligned_load(const Rhs& r) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL void aligned_store(scalar_t* r) const {
                T pos(~neg);
                pos.aligned_store(r);
            }

            template <typename Rhs>
            void unaligned_load(const Rhs& r) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL void unaligned_store(scalar_t* r) const {
                T pos(~neg);
                pos.unaligned_store(r);
            }

            // data
            const T neg;
        };

        template<typename T>
        SIMDIFY_INL constexpr const T operator~(const deferred_not<T>& l) {
            return l.neg;
        }
        template<typename T>
        SIMDIFY_INL const T operator&(const deferred_not<T>& l, const T& r) {
            return andnot(r, l.neg);
        }
        template<typename T>
        SIMDIFY_INL const T operator&(const T& l, const deferred_not<T>& r) {
            return andnot(l, r.neg);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator&(const deferred_not<T>& l, const deferred_not<T>& r) {
            return deferred_not<T>(l.neg | r.neg);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator|(const deferred_not<T>& l, const T& r) {
            return deferred_not<T>(andnot(l.neg, r));
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator|(const T& l, const deferred_not<T>& r) {
            return deferred_not<T>(andnot(r.neg, l));
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator|(const deferred_not<T>& l, const deferred_not<T>& r) {
            return deferred_not<T>(l.neg & r.neg);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator^(const deferred_not<T>& l, const T& r) {
            return deferred_not<T>(l.neg ^ r);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator^(const T& l, const deferred_not<T>& r) {
            return deferred_not<T>(l ^ r.neg);
        }
        template<typename T>
        SIMDIFY_INL const T operator^(const deferred_not<T>& l, const deferred_not<T>& r) {
            return l.neg ^ r.neg;
        }
        template<typename T>
        SIMDIFY_INL bool any(const deferred_not<T>& l) {
            return !l.neg.mask().all();
        }
        template<typename T>
        SIMDIFY_INL bool all(const deferred_not<T>& l) {
            return !l.neg.mask().any();
        }
        template <typename T, typename Rhs>
        SIMDIFY_INL void operator&=(const deferred_not<T>& l, const Rhs& r) {
            dont_assign_to_deferred_not<Rhs> fail;
        }
        template <typename T, typename Rhs>
        SIMDIFY_INL void operator|=(const deferred_not<T>& l, const Rhs& r) {
            dont_assign_to_deferred_not<Rhs> fail;
        }
        template <typename T, typename Rhs>
        SIMDIFY_INL void operator^=(const deferred_not<T>& l, const Rhs& r) {
            dont_assign_to_deferred_not<Rhs> fail;
        }
    }
}

#endif // SIMDIFY_COMMON_DEFERRED_NOT_HPP
<commit_msg>Add interleaved_store to deferred_not, fix reduce<commit_after>#ifndef SIMDIFY_COMMON_DEFERRED_NOT_HPP
#define SIMDIFY_COMMON_DEFERRED_NOT_HPP

#include "../util/inline.hpp"
#include <type_traits>

namespace simd {
    namespace expr {
        template <typename T>
        struct dont_assign_to_deferred_not {
            template <typename T2>
            struct false_when_instantiated : std::integral_constant<bool, false> {};

            static_assert(false_when_instantiated<T>::value,
                "Hello! You are trying to assign to a varible of type deferred_not<T>. "
                "This is definitely not something you want to do. "
                "What probably happened is that you used *auto* to store a calculation result, "
                "but the type of the result is not what you expect. "
                "To solve the problem, don't use *auto* on that particular line.");
        };

        template <typename T>
        struct deferred_not {
            using vector_t = typename T::vector_t;
            using scalar_t = typename T::scalar_t;
            using mask_t = typename T::mask_t;
            using storage_t = typename T::storage_t;
            using binary_op_t = typename T::binary_op_t;
            enum : std::size_t { width = T::width };

            SIMDIFY_INL constexpr explicit deferred_not(const T& r) : neg(r) {}

            template <typename Rhs>
            void operator=(const Rhs& r) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL typename T::mask_t mask() const { return ~neg.mask(); }
            SIMDIFY_INL typename T::scalar_t first_element() const { return ~neg.first_element(); }

            SIMDIFY_INL const vector_t data() const {
                T pos(~neg);
                return pos.data();
            }

            template <typename Rhs>
            void aligned_load(const Rhs& r) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL void aligned_store(scalar_t* r) const {
                T pos(~neg);
                pos.aligned_store(r);
            }

            template <typename Rhs>
            void unaligned_load(const Rhs& r) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL void unaligned_store(scalar_t* r) const {
                T pos(~neg);
                pos.unaligned_store(r);
            }

            template <typename Rhs>
            void interleaved_load(const Rhs& r, std::size_t step) {
                dont_assign_to_deferred_not<Rhs> fail;
            }

            SIMDIFY_INL void interleaved_store(scalar_t* r, std::size_t step) const {
                T pos(~neg);
                pos.interleaved_store(r, step);
            }

            SIMDIFY_INL const T reduce(binary_op_t f) const {
                T pos(~neg);
                return pos.reduce(f);
            }

            // data
            const T neg;
        };

        template<typename T>
        SIMDIFY_INL constexpr const T operator~(const deferred_not<T>& l) {
            return l.neg;
        }
        template<typename T>
        SIMDIFY_INL const T operator&(const deferred_not<T>& l, const T& r) {
            return andnot(r, l.neg);
        }
        template<typename T>
        SIMDIFY_INL const T operator&(const T& l, const deferred_not<T>& r) {
            return andnot(l, r.neg);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator&(const deferred_not<T>& l, const deferred_not<T>& r) {
            return deferred_not<T>(l.neg | r.neg);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator|(const deferred_not<T>& l, const T& r) {
            return deferred_not<T>(andnot(l.neg, r));
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator|(const T& l, const deferred_not<T>& r) {
            return deferred_not<T>(andnot(r.neg, l));
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator|(const deferred_not<T>& l, const deferred_not<T>& r) {
            return deferred_not<T>(l.neg & r.neg);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator^(const deferred_not<T>& l, const T& r) {
            return deferred_not<T>(l.neg ^ r);
        }
        template<typename T>
        SIMDIFY_INL const deferred_not<T> operator^(const T& l, const deferred_not<T>& r) {
            return deferred_not<T>(l ^ r.neg);
        }
        template<typename T>
        SIMDIFY_INL const T operator^(const deferred_not<T>& l, const deferred_not<T>& r) {
            return l.neg ^ r.neg;
        }
        template<typename T>
        SIMDIFY_INL bool any(const deferred_not<T>& l) {
            return !l.neg.mask().all();
        }
        template<typename T>
        SIMDIFY_INL bool all(const deferred_not<T>& l) {
            return !l.neg.mask().any();
        }
        template <typename T, typename Rhs>
        SIMDIFY_INL void operator&=(const deferred_not<T>& l, const Rhs& r) {
            dont_assign_to_deferred_not<Rhs> fail;
        }
        template <typename T, typename Rhs>
        SIMDIFY_INL void operator|=(const deferred_not<T>& l, const Rhs& r) {
            dont_assign_to_deferred_not<Rhs> fail;
        }
        template <typename T, typename Rhs>
        SIMDIFY_INL void operator^=(const deferred_not<T>& l, const Rhs& r) {
            dont_assign_to_deferred_not<Rhs> fail;
        }
    }
}

#endif // SIMDIFY_COMMON_DEFERRED_NOT_HPP
<|endoftext|>


<commit_before>//============================================================================
// include/vsmc/utility/rdtscp_counter.hpp
//----------------------------------------------------------------------------
//
//                         vSMC: Scalable Monte Carlo
//
// This file is distribured under the 2-clauses BSD License.
// See LICENSE for details.
//============================================================================

#ifndef VSMC_UTILITY_RDTSCP_COUNTER_HPP
#define VSMC_UTILITY_RDTSCP_COUNTER_HPP

#include <vsmc/internal/common.hpp>

#ifdef _MSC_VER
#include <intrin.h>
#endif

namespace vsmc {

/// \brief CPU clock cycle counter using RDTSCP
/// \ingroup RDTSCP
class RDTSCPCounter
{
    public :

    typedef unsigned VSMC_INTRINSIC_INT64 result_type;

    RDTSCPCounter () : elapsed_(0), start_(0), start_id_(0), running_(false)
    {reset();}

    /// \brief If the counter is running
    ///
    /// \details
    /// If `start()` has been called and no `stop()` call since, then it is
    /// running, otherwise it is stoped.
    bool running () const {return false;}

    /// \brief Start the counter, no effect if already started
    ///
    /// \return `true` if it is started by this call, and the `elapsed` cycle
    /// count will be incremented next time `stop()` is called. The increment
    /// will be relative to the time point of this call. `false` if it is
    /// already started earlier.
    bool start ()
    {
	if (running_)
	    return false;

	running_ = true;
	start_ = now(&start_id_);

	return true;
    }

    /// \brief Stop the counter, no effect if already stopped
    ///
    /// \return `true` if it is stoped by this call, and the `elapsed` cycle
    /// count has been incremented. `false` in one of the following situations.
    /// In all these situations, the `elapsed` cycle count will not be
    /// incremented.
    /// - It already stopped or wasn't started before.
    /// - The logical processor has changed since the last successful `start()`
    /// call. The user is repsonsible for assure threads affinity.
    /// - The cycle count appears to decrease. This is most likely in the case
    /// that the TSC MSR has been reset.
    bool stop ()
    {
	if (!running_)
	    return false;

	unsigned stop_id = 0;
	result_type stop = now(&stop_id);
	bool same_id = stop_id == start_id_;
	if (same_id)
	    elapsed_ += stop - start_;
	running_ = false;

	return same_id;
    }

    /// \brief Stop and reset the `elapsed` cycle count to zero
    void reset ()
    {
	start();
	elapsed_ = 0;
	running_ = false;
    }

    /// \brief Return the accumulated elapsed cycle count
    result_type elapsed () const {return elapsed_;}

    private :

    result_type elapsed_;
    result_type start_;
    unsigned start_id_;
    bool running_;

    result_type now (unsigned *aux)
    {
#ifdef _MSC_VER
        return __rdtscp(aux)
#else // _MSC_VER
        unsigned eax = 0x00;
        unsigned edx = 0x00;
        unsigned ecx = 0x00;
        __asm__(
                "rdtscp\n"
                : "=a" (eax), "=d" (edx), "=c" (ecx)
               );
        *aux = ecx;

        return (static_cast<result_type>(edx) << 32) +
            static_cast<result_type>(eax);
#endif // _MSC_VER
    }
}; // class RDTSCPCounter

} // namespace vsmc

#endif // VSMC_UTILITY_RDTSCP_COUNTER_HPP
<commit_msg>rename the returned value as cycles<commit_after>//============================================================================
// include/vsmc/utility/rdtscp_counter.hpp
//----------------------------------------------------------------------------
//
//                         vSMC: Scalable Monte Carlo
//
// This file is distribured under the 2-clauses BSD License.
// See LICENSE for details.
//============================================================================

#ifndef VSMC_UTILITY_RDTSCP_COUNTER_HPP
#define VSMC_UTILITY_RDTSCP_COUNTER_HPP

#include <vsmc/internal/common.hpp>

#ifdef _MSC_VER
#include <intrin.h>
#endif

namespace vsmc {

/// \brief CPU clock cycle counter using RDTSCP
/// \ingroup RDTSCP
class RDTSCPCounter
{
    public :

    typedef unsigned VSMC_INTRINSIC_INT64 result_type;

    RDTSCPCounter () : elapsed_(0), start_(0), start_id_(0), running_(false)
    {reset();}

    /// \brief If the counter is running
    ///
    /// \details
    /// If `start()` has been called and no `stop()` call since, then it is
    /// running, otherwise it is stoped.
    bool running () const {return false;}

    /// \brief Start the counter, no effect if already started
    ///
    /// \return `true` if it is started by this call, and the `elapsed` cycle
    /// count will be incremented next time `stop()` is called. The increment
    /// will be relative to the time point of this call. `false` if it is
    /// already started earlier.
    bool start ()
    {
	if (running_)
	    return false;

	running_ = true;
	start_ = now(&start_id_);

	return true;
    }

    /// \brief Stop the counter, no effect if already stopped
    ///
    /// \return `true` if it is stoped by this call, and the `elapsed` cycle
    /// count has been incremented. `false` in one of the following situations.
    /// In all these situations, the `elapsed` cycle count will not be
    /// incremented.
    /// - It already stopped or wasn't started before.
    /// - The logical processor has changed since the last successful `start()`
    /// call. The user is repsonsible for assure threads affinity.
    /// - The cycle count appears to decrease. This is most likely in the case
    /// that the TSC MSR has been reset.
    bool stop ()
    {
	if (!running_)
	    return false;

	unsigned stop_id = 0;
	result_type stop = now(&stop_id);
	bool same_id = stop_id == start_id_;
	if (same_id)
	    elapsed_ += stop - start_;
	running_ = false;

	return same_id;
    }

    /// \brief Stop and reset the `elapsed` cycle count to zero
    void reset ()
    {
	start();
	elapsed_ = 0;
	running_ = false;
    }

    /// \brief Return the accumulated elapsed cycle count
    result_type cycles () const {return elapsed_;}

    private :

    result_type elapsed_;
    result_type start_;
    unsigned start_id_;
    bool running_;

    result_type now (unsigned *aux)
    {
#ifdef _MSC_VER
        return __rdtscp(aux)
#else // _MSC_VER
        unsigned eax = 0x00;
        unsigned edx = 0x00;
        unsigned ecx = 0x00;
        __asm__(
                "rdtscp\n"
                : "=a" (eax), "=d" (edx), "=c" (ecx)
               );
        *aux = ecx;

        return (static_cast<result_type>(edx) << 32) +
            static_cast<result_type>(eax);
#endif // _MSC_VER
    }
}; // class RDTSCPCounter

} // namespace vsmc

#endif // VSMC_UTILITY_RDTSCP_COUNTER_HPP
<|endoftext|>


<commit_before>#ifndef MINIASCAPE_RANDOM_STATE_GENERATOR
#define MINIASCAPE_RANDOM_STATE_GENERATOR
#include "util/meta_function.hpp"
#include <random>

namespace miniascape
{

class RandomNumberGenerator
{
  public:
    RandomNumberGenerator(const unsigned int seed): seed_(seed), mt_(seed){}
    ~RandomNumberGenerator() = default;

    template<typename T>
    T uniform_int(const T begin, const T end)
    {
        return (std::uniform_int_distribution<T>(begin, end))(this->mt_);
    }

    template<typename T>
    T uniform_real(const T begin, const T end)
    {
        return (std::uniform_real_distribution<T>(begin, end))(this->mt_);
    }

    unsigned int seed() const {return seed_;}

  private:
    const unsigned int seed_;
    std::mt19937 mt_;
};

template<typename T_state>
class RandomStateGenerator{};

}

#endif /* MINIASCAPE_RANDOM_STATE_GENERATOR */
<commit_msg>remove file include<commit_after>#ifndef MINIASCAPE_RANDOM_STATE_GENERATOR
#define MINIASCAPE_RANDOM_STATE_GENERATOR
#include <random>

namespace miniascape
{

class RandomNumberGenerator
{
  public:
    RandomNumberGenerator(const unsigned int seed): seed_(seed), mt_(seed){}
    ~RandomNumberGenerator() = default;

    template<typename T>
    T uniform_int(const T begin, const T end)
    {
        return (std::uniform_int_distribution<T>(begin, end))(this->mt_);
    }

    template<typename T>
    T uniform_real(const T begin, const T end)
    {
        return (std::uniform_real_distribution<T>(begin, end))(this->mt_);
    }

    unsigned int seed() const {return seed_;}

  private:
    const unsigned int seed_;
    std::mt19937 mt_;
};

template<typename T_state>
class RandomStateGenerator{};

}

#endif /* MINIASCAPE_RANDOM_STATE_GENERATOR */
<|endoftext|>


<commit_before>#include "../../base/SRC_FIRST.hpp"

#include "../../testing/testing.hpp"

#include "../../base/math.hpp"
#include "../../base/macros.hpp"

#include "../../indexer/mercator.hpp"

UNIT_TEST(MercatorTestGrid)
{
  double const eps = 0.0000001;
  for (int lat = -85; lat <= 85; ++lat)
  {
    for (int lon = -180; lon <= 180; ++lon)
    {
      double const x = MercatorBounds::LonToX(lon);
      double const y = MercatorBounds::LatToY(lat);
      double const lat1 = MercatorBounds::YToLat(y);
      double const lon1 = MercatorBounds::XToLon(x);

      // Normal assumption for any projection.
      TEST_ALMOST_EQUAL(static_cast<double>(lat), lat1, ());
      TEST_ALMOST_EQUAL(static_cast<double>(lon), lon1, ());

      // x is actually lon unmodified.
      TEST_ALMOST_EQUAL(x, static_cast<double>(lon), ());

      if (lat == 0)
      {
        // TODO: Investigate, how to make Mercator transform more precise.
        // Error is to large for TEST_ALMOST_EQUAL(y, 0.0, ());
        TEST_LESS(fabs(y), eps, (lat, y, lat1));
      }
    }
  }
}

UNIT_TEST(MercatorTest)
{
  double const eps = 0.0000001;
  double lon = 63.45421;
  double x = MercatorBounds::LonToX(lon);
  double lon1 = MercatorBounds::XToLon(x);
  TEST_LESS(fabs(lon - lon1), eps, ("Too big round error"));
  double lat = 34.28754;
  double y = MercatorBounds::LatToY(lat);
  double lat1 = MercatorBounds::YToLat(y);
  TEST_LESS(fabs(lat - lat1), eps, ("Too big round error"));
  TEST_LESS(fabs(MercatorBounds::maxX - MercatorBounds::maxY), eps, ("Non-square maxX and maxY"));
  TEST_LESS(fabs(MercatorBounds::minX - MercatorBounds::minY), eps, ("Non-square minX and minY"));
}

UNIT_TEST(ErrorToRadius)
{
  double const points[] = { -85.0, -45.0, -10.0, -1.0, -0.003, 0.0, 0.003, 1.0, 10.0, 45.0, 85.0 };

  double const error1 = 1.0;    // 1 metre
  double const error10 = 10.0;  // 10 metres

  for (size_t i = 0; i < ARRAY_SIZE(points); ++i)
  {
    for (size_t j = 0; j < ARRAY_SIZE(points); ++j)
    {
      double const lon = points[i];
      double const lat = points[j];
      m2::PointD const mercPoint(MercatorBounds::LonToX(lon), MercatorBounds::LatToY(lat));

      m2::RectD radius1 = MercatorBounds::MetresToXY(lon, lat, error1);
      TEST(radius1.IsPointInside(mercPoint), (lat, lon));

      m2::RectD radius10 = MercatorBounds::MetresToXY(lon, lat, error10);
      TEST(radius10.IsPointInside(mercPoint), (lat, lon));

      m2::RectD radius10Added = radius10;
      radius10Added.Add(radius1);
      TEST_EQUAL(radius10, radius10Added, (lat, lon));

      TEST(radius10.IsPointInside(radius1.LeftTop()), (lat, lon));
      TEST(radius10.IsPointInside(radius1.LeftBottom()), (lat, lon));
      TEST(radius10.IsPointInside(radius1.RightTop()), (lat, lon));
      TEST(radius10.IsPointInside(radius1.RightBottom()), (lat, lon));
    }
  }
}
<commit_msg>Some code style fixes for tests naming.<commit_after>#include "../../base/SRC_FIRST.hpp"

#include "../../testing/testing.hpp"

#include "../../indexer/mercator.hpp"

#include "../../base/math.hpp"
#include "../../base/macros.hpp"
#include "../../base/logging.hpp"

UNIT_TEST(Mercator_Grid)
{
  double const eps = 0.0000001;
  for (int lat = -85; lat <= 85; ++lat)
  {
    for (int lon = -180; lon <= 180; ++lon)
    {
      double const x = MercatorBounds::LonToX(lon);
      double const y = MercatorBounds::LatToY(lat);
      double const lat1 = MercatorBounds::YToLat(y);
      double const lon1 = MercatorBounds::XToLon(x);

      // Normal assumption for any projection.
      TEST_ALMOST_EQUAL(static_cast<double>(lat), lat1, ());
      TEST_ALMOST_EQUAL(static_cast<double>(lon), lon1, ());

      // x is actually lon unmodified.
      TEST_ALMOST_EQUAL(x, static_cast<double>(lon), ());

      if (lat == 0)
      {
        // TODO: Investigate, how to make Mercator transform more precise.
        // Error is to large for TEST_ALMOST_EQUAL(y, 0.0, ());
        TEST_LESS(fabs(y), eps, (lat, y, lat1));
      }
    }
  }
}

UNIT_TEST(Mercator_DirectInferseF)
{
  double const eps = 0.0000001;
  double lon = 63.45421;
  double x = MercatorBounds::LonToX(lon);
  double lon1 = MercatorBounds::XToLon(x);
  TEST_LESS(fabs(lon - lon1), eps, ("Too big round error"));
  double lat = 34.28754;
  double y = MercatorBounds::LatToY(lat);
  double lat1 = MercatorBounds::YToLat(y);
  TEST_LESS(fabs(lat - lat1), eps, ("Too big round error"));
  TEST_LESS(fabs(MercatorBounds::maxX - MercatorBounds::maxY), eps, ("Non-square maxX and maxY"));
  TEST_LESS(fabs(MercatorBounds::minX - MercatorBounds::minY), eps, ("Non-square minX and minY"));
}

UNIT_TEST(Mercator_ErrorToRadius)
{
  double const points[] = { -85.0, -45.0, -10.0, -1.0, -0.003, 0.0, 0.003, 1.0, 10.0, 45.0, 85.0 };

  double const error1 = 1.0;    // 1 metre
  double const error10 = 10.0;  // 10 metres

  for (size_t i = 0; i < ARRAY_SIZE(points); ++i)
  {
    for (size_t j = 0; j < ARRAY_SIZE(points); ++j)
    {
      double const lon = points[i];
      double const lat = points[j];
      m2::PointD const mercPoint(MercatorBounds::LonToX(lon), MercatorBounds::LatToY(lat));

      m2::RectD radius1 = MercatorBounds::MetresToXY(lon, lat, error1);
      TEST(radius1.IsPointInside(mercPoint), (lat, lon));

      m2::RectD radius10 = MercatorBounds::MetresToXY(lon, lat, error10);
      TEST(radius10.IsPointInside(mercPoint), (lat, lon));

      m2::RectD radius10Added = radius10;
      radius10Added.Add(radius1);
      TEST_EQUAL(radius10, radius10Added, (lat, lon));

      TEST(radius10.IsPointInside(radius1.LeftTop()), (lat, lon));
      TEST(radius10.IsPointInside(radius1.LeftBottom()), (lat, lon));
      TEST(radius10.IsPointInside(radius1.RightTop()), (lat, lon));
      TEST(radius10.IsPointInside(radius1.RightBottom()), (lat, lon));
    }
  }
}

UNIT_TEST(Mercator_Sample1)
{
  LOG(LINFO, (MercatorBounds::XToLon(27.531491200000001385),
              MercatorBounds::YToLat(64.392864299248202542)));
}
<|endoftext|>


<commit_before>#include <ruby.h>
#include <set>
#include <algorithm>

using namespace std;

static VALUE cValueSet;
static ID id_new;

typedef std::set<VALUE> ValueSet;
static ValueSet& get_wrapped_set(VALUE self)
{
    ValueSet* object = 0;
    Data_Get_Struct(self, ValueSet, object);
    return *object;
}

static void value_set_mark(ValueSet const* set) { std::for_each(set->begin(), set->end(), rb_gc_mark); }
static void value_set_free(ValueSet const* set) { delete set; }
static VALUE value_set_alloc(VALUE klass)
{
    ValueSet* cxx_set = new ValueSet;
    return Data_Wrap_Struct(klass, value_set_mark, value_set_free, cxx_set);
}
/* call-seq:
 *  set.empty?			    => true or false
 *
 * Checks if this set is empty
 */
static VALUE value_set_empty_p(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    return set.empty() ? Qtrue : Qfalse;
}

/* call-seq:
 *  set.size			    => size
 *
 * Returns this set size
 */
static VALUE value_set_size(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    return INT2NUM(set.size());
}

/* call-seq:
 *  set.each { |obj| ... }	    => set
 *
 */
static VALUE value_set_each(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    for (ValueSet::iterator it = set.begin(); it != set.end();)
    {
	// Increment before calling yield() so that
	// the current element can be deleted safely
	ValueSet::iterator this_it = it++;
	rb_yield(*this_it);
    }
    return self;
}

/* call-seq:
 *  set.delete_if { |obj| ... }		=> set
 *
 * Deletes all objects for which the block returns true
 */
static VALUE value_set_delete_if(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    for (ValueSet::iterator it = set.begin(); it != set.end();)
    {
	// Increment before calling yield() so that
	// the current element can be deleted safely
	ValueSet::iterator this_it = it++;
	bool do_delete = RTEST(rb_yield(*this_it));
	if (do_delete)
	    set.erase(this_it);
    }
    return self;
}

/* call-seq:
 *  set.include?(value)	    => true or false
 *
 * Checks if +value+ is in +set+
 */
static VALUE value_set_include_p(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    return self.find(vother) == self.end() ? Qfalse : Qtrue;
}

/* call-seq:
 *  set.to_value_set		    => set
 */
static VALUE value_set_to_value_set(VALUE self) { return self; }

/* call-seq:
 *  set.dup => other_set
 *
 * Duplicates this set, without duplicating the pointed-to objects
 */
static VALUE value_set_dup(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    for (ValueSet::const_iterator it = self.begin(); it != self.end(); ++it)
	result.insert(result.end(), *it);

    return vresult;
}

/* call-seq:
 *  set.include_all?(other)		=> true or false
 *
 * Checks if all elements of +other+ are in +set+
 */
static VALUE value_set_include_all_p(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);
    return std::includes(self.begin(), self.end(), other.begin(), other.end()) ? Qtrue : Qfalse;
}

/* call-seq:
 *  set.union(other)		=> union_set
 *  set | other			=> union_set
 *
 * Computes the union of +set+ and +other+. This operation is O(N + M)
 * is +other+ is a ValueSet
 */
static VALUE value_set_union(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    std::set_union(self.begin(), self.end(), other.begin(), other.end(),
	    std::inserter(result, result.end()));
    return vresult;
}

/* call-seq:
 *  set.merge(other)		=> set
 *
 * Merges the elements of +other+ into +self+. If +other+ is a ValueSet, the operation is O(N + M)
 */
static VALUE value_set_merge(VALUE vself, VALUE vother)
{
    ValueSet& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    self.insert(other.begin(), other.end());
    return vself;
}

/* call-seq:
 *   set.intersection(other)	=> intersection_set
 *   set & other		=> intersection_set
 *
 * Computes the intersection of +set+ and +other+. This operation
 * is O(N + M) if +other+ is a ValueSet
 */
static VALUE value_set_intersection(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    std::set_intersection(self.begin(), self.end(), other.begin(), other.end(),
	    std::inserter(result, result.end()));
    return vresult;
}

/* call-seq:
 *  set.intersects?(other)	=> true or false
 *
 * Returns true if there is elements in +set+ that are also in +other
 */
static VALUE value_set_intersects(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    ValueSet::const_iterator
	self_it   = self.begin(),
	self_end  = self.end(),
	other_it  = other.begin(),
	other_end = other.end();

    while(self_it != self_end && other_it != other_end)
    {
	if (*self_it < *other_it)
	    ++self_it;
	else if (*other_it < *self_it)
	    ++other_it;
	else
	    return Qtrue;
    }
    return Qfalse;
}

/* call-seq:
 *   set.difference(other)	=> difference_set
 *   set - other		=> difference_set
 *
 * Computes the set of all elements of +set+ not in +other+. This operation
 * is O(N + M) if +other+ is a ValueSet
 */
static VALUE value_set_difference(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    std::set_difference(self.begin(), self.end(), other.begin(), other.end(),
	    std::inserter(result, result.end()));
    return vresult;
}

/* call-seq:
 *  set.insert(value)		=> true or false
 *
 * Inserts +value+ into +set+. Returns true if the value did not exist
 * in the set yet (it has actually been inserted), and false otherwise.
 * This operation is O(log N)
 */
static VALUE value_set_insert(VALUE vself, VALUE v)
{
    ValueSet& self  = get_wrapped_set(vself);
    bool exists = self.insert(v).second;
    return exists ? Qtrue : Qfalse;
}
/* call-seq:
 *  set.delete(value)		=> true or false
 *
 * Removes +value+ from +set+. Returns true if the value did exist
 * in the set yet (it has actually been removed), and false otherwise.
 */
static VALUE value_set_delete(VALUE vself, VALUE v)
{
    ValueSet& self  = get_wrapped_set(vself);
    size_t count = self.erase(v);
    return count > 0 ? Qtrue : Qfalse;
}

/* call-seq:
 *  set == other		=> true or false
 *
 * Equality
 */
static VALUE value_set_equal(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	return Qfalse;
    ValueSet const& other = get_wrapped_set(vother);
    return (self == other) ? Qtrue : Qfalse;
}

/* call-seq:
 *  set.clear			=> set
 *
 * Remove all elements of this set
 */
static VALUE value_set_clear(VALUE self)
{
    get_wrapped_set(self).clear();
    return self;
}

/* call-seq:
 *  set.initialize_copy(other)  => set
 *
 * Initializes +set+ with the values in +other+. Needed by #dup
 */
static VALUE value_set_initialize_copy(VALUE vself, VALUE vother)
{
    ValueSet const& other = get_wrapped_set(vother);
    set<VALUE> new_set(other.begin(), other.end());
    get_wrapped_set(vself).swap(new_set);
    return vself;
}

/* call-seq:
 *  to_value_set    => value_set
 *
 * Converts this array into a ValueSet object
 */
static VALUE array_to_value_set(VALUE self)
{
    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);

    long size  = rb_ary_size(self);
    for (int i = 0; i < size; ++i)
	result.insert(rb_ary_entry(self, i));

    return vresult;
}

#ifndef RUBINIUS
static VALUE enumerable_to_value_set_i(VALUE i, VALUE* memo)
{
    ValueSet& result = *reinterpret_cast<ValueSet*>(memo);
    result.insert(i);
    return Qnil;
}

/* call-seq:
 *  enum.to_value_set		=> value_set
 *
 * Builds a ValueSet object from this enumerable
 */
static VALUE enumerable_to_value_set(VALUE self)
{
    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);

    rb_iterate(rb_each, self, RUBY_METHOD_FUNC(enumerable_to_value_set_i), reinterpret_cast<VALUE>(&result));
    return vresult;
}
#endif

/*
 * Document-class: ValueSet
 *
 * ValueSet is a wrapper around the C++ set<> template. set<> is an ordered container,
 * for which union(), intersection() and difference() is done in linear time. For performance
 * reasons, in the case of ValueSet, the values are ordered by their VALUE, which roughly is
 * their object_id.
 */

extern "C" void Init_value_set()
{
#ifndef RUBINIUS
    rb_define_method(rb_mEnumerable, "to_value_set", RUBY_METHOD_FUNC(enumerable_to_value_set), 0);
#endif
    rb_define_method(rb_cArray, "to_value_set", RUBY_METHOD_FUNC(array_to_value_set), 0);

    cValueSet = rb_define_class("ValueSet", rb_cObject);
    id_new = rb_intern("new");
    rb_define_alloc_func(cValueSet, value_set_alloc);
    rb_define_method(cValueSet, "each", RUBY_METHOD_FUNC(value_set_each), 0);
    rb_define_method(cValueSet, "include?", RUBY_METHOD_FUNC(value_set_include_p), 1);
    rb_define_method(cValueSet, "include_all?", RUBY_METHOD_FUNC(value_set_include_all_p), 1);
    rb_define_method(cValueSet, "union", RUBY_METHOD_FUNC(value_set_union), 1);
    rb_define_method(cValueSet, "intersection", RUBY_METHOD_FUNC(value_set_intersection), 1);
    rb_define_method(cValueSet, "intersects?", RUBY_METHOD_FUNC(value_set_intersects), 1);
    rb_define_method(cValueSet, "difference", RUBY_METHOD_FUNC(value_set_difference), 1);
    rb_define_method(cValueSet, "insert", RUBY_METHOD_FUNC(value_set_insert), 1);
    rb_define_method(cValueSet, "merge", RUBY_METHOD_FUNC(value_set_merge), 1);
    rb_define_method(cValueSet, "delete", RUBY_METHOD_FUNC(value_set_delete), 1);
    rb_define_method(cValueSet, "==", RUBY_METHOD_FUNC(value_set_equal), 1);
    rb_define_method(cValueSet, "to_value_set", RUBY_METHOD_FUNC(value_set_to_value_set), 0);
    rb_define_method(cValueSet, "dup", RUBY_METHOD_FUNC(value_set_dup), 0);
    rb_define_method(cValueSet, "empty?", RUBY_METHOD_FUNC(value_set_empty_p), 0);
    rb_define_method(cValueSet, "size", RUBY_METHOD_FUNC(value_set_size), 0);
    rb_define_method(cValueSet, "clear", RUBY_METHOD_FUNC(value_set_clear), 0);
    rb_define_method(cValueSet, "initialize_copy", RUBY_METHOD_FUNC(value_set_initialize_copy), 1);
    rb_define_method(cValueSet, "delete_if", RUBY_METHOD_FUNC(value_set_delete_if), 0);
}

<commit_msg>rb_ary_size is not universally defined. Use RARRAY_LEN<commit_after>#include <ruby.h>
#include <set>
#include <algorithm>

using namespace std;

static VALUE cValueSet;
static ID id_new;

typedef std::set<VALUE> ValueSet;
static ValueSet& get_wrapped_set(VALUE self)
{
    ValueSet* object = 0;
    Data_Get_Struct(self, ValueSet, object);
    return *object;
}

static void value_set_mark(ValueSet const* set) { std::for_each(set->begin(), set->end(), rb_gc_mark); }
static void value_set_free(ValueSet const* set) { delete set; }
static VALUE value_set_alloc(VALUE klass)
{
    ValueSet* cxx_set = new ValueSet;
    return Data_Wrap_Struct(klass, value_set_mark, value_set_free, cxx_set);
}
/* call-seq:
 *  set.empty?			    => true or false
 *
 * Checks if this set is empty
 */
static VALUE value_set_empty_p(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    return set.empty() ? Qtrue : Qfalse;
}

/* call-seq:
 *  set.size			    => size
 *
 * Returns this set size
 */
static VALUE value_set_size(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    return INT2NUM(set.size());
}

/* call-seq:
 *  set.each { |obj| ... }	    => set
 *
 */
static VALUE value_set_each(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    for (ValueSet::iterator it = set.begin(); it != set.end();)
    {
	// Increment before calling yield() so that
	// the current element can be deleted safely
	ValueSet::iterator this_it = it++;
	rb_yield(*this_it);
    }
    return self;
}

/* call-seq:
 *  set.delete_if { |obj| ... }		=> set
 *
 * Deletes all objects for which the block returns true
 */
static VALUE value_set_delete_if(VALUE self)
{
    ValueSet& set = get_wrapped_set(self);
    for (ValueSet::iterator it = set.begin(); it != set.end();)
    {
	// Increment before calling yield() so that
	// the current element can be deleted safely
	ValueSet::iterator this_it = it++;
	bool do_delete = RTEST(rb_yield(*this_it));
	if (do_delete)
	    set.erase(this_it);
    }
    return self;
}

/* call-seq:
 *  set.include?(value)	    => true or false
 *
 * Checks if +value+ is in +set+
 */
static VALUE value_set_include_p(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    return self.find(vother) == self.end() ? Qfalse : Qtrue;
}

/* call-seq:
 *  set.to_value_set		    => set
 */
static VALUE value_set_to_value_set(VALUE self) { return self; }

/* call-seq:
 *  set.dup => other_set
 *
 * Duplicates this set, without duplicating the pointed-to objects
 */
static VALUE value_set_dup(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    for (ValueSet::const_iterator it = self.begin(); it != self.end(); ++it)
	result.insert(result.end(), *it);

    return vresult;
}

/* call-seq:
 *  set.include_all?(other)		=> true or false
 *
 * Checks if all elements of +other+ are in +set+
 */
static VALUE value_set_include_all_p(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);
    return std::includes(self.begin(), self.end(), other.begin(), other.end()) ? Qtrue : Qfalse;
}

/* call-seq:
 *  set.union(other)		=> union_set
 *  set | other			=> union_set
 *
 * Computes the union of +set+ and +other+. This operation is O(N + M)
 * is +other+ is a ValueSet
 */
static VALUE value_set_union(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    std::set_union(self.begin(), self.end(), other.begin(), other.end(),
	    std::inserter(result, result.end()));
    return vresult;
}

/* call-seq:
 *  set.merge(other)		=> set
 *
 * Merges the elements of +other+ into +self+. If +other+ is a ValueSet, the operation is O(N + M)
 */
static VALUE value_set_merge(VALUE vself, VALUE vother)
{
    ValueSet& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    self.insert(other.begin(), other.end());
    return vself;
}

/* call-seq:
 *   set.intersection(other)	=> intersection_set
 *   set & other		=> intersection_set
 *
 * Computes the intersection of +set+ and +other+. This operation
 * is O(N + M) if +other+ is a ValueSet
 */
static VALUE value_set_intersection(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    std::set_intersection(self.begin(), self.end(), other.begin(), other.end(),
	    std::inserter(result, result.end()));
    return vresult;
}

/* call-seq:
 *  set.intersects?(other)	=> true or false
 *
 * Returns true if there is elements in +set+ that are also in +other
 */
static VALUE value_set_intersects(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    ValueSet::const_iterator
	self_it   = self.begin(),
	self_end  = self.end(),
	other_it  = other.begin(),
	other_end = other.end();

    while(self_it != self_end && other_it != other_end)
    {
	if (*self_it < *other_it)
	    ++self_it;
	else if (*other_it < *self_it)
	    ++other_it;
	else
	    return Qtrue;
    }
    return Qfalse;
}

/* call-seq:
 *   set.difference(other)	=> difference_set
 *   set - other		=> difference_set
 *
 * Computes the set of all elements of +set+ not in +other+. This operation
 * is O(N + M) if +other+ is a ValueSet
 */
static VALUE value_set_difference(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	rb_raise(rb_eArgError, "expected a ValueSet");
    ValueSet const& other = get_wrapped_set(vother);

    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);
    std::set_difference(self.begin(), self.end(), other.begin(), other.end(),
	    std::inserter(result, result.end()));
    return vresult;
}

/* call-seq:
 *  set.insert(value)		=> true or false
 *
 * Inserts +value+ into +set+. Returns true if the value did not exist
 * in the set yet (it has actually been inserted), and false otherwise.
 * This operation is O(log N)
 */
static VALUE value_set_insert(VALUE vself, VALUE v)
{
    ValueSet& self  = get_wrapped_set(vself);
    bool exists = self.insert(v).second;
    return exists ? Qtrue : Qfalse;
}
/* call-seq:
 *  set.delete(value)		=> true or false
 *
 * Removes +value+ from +set+. Returns true if the value did exist
 * in the set yet (it has actually been removed), and false otherwise.
 */
static VALUE value_set_delete(VALUE vself, VALUE v)
{
    ValueSet& self  = get_wrapped_set(vself);
    size_t count = self.erase(v);
    return count > 0 ? Qtrue : Qfalse;
}

/* call-seq:
 *  set == other		=> true or false
 *
 * Equality
 */
static VALUE value_set_equal(VALUE vself, VALUE vother)
{
    ValueSet const& self  = get_wrapped_set(vself);
    if (!RTEST(rb_obj_is_kind_of(vother, cValueSet)))
	return Qfalse;
    ValueSet const& other = get_wrapped_set(vother);
    return (self == other) ? Qtrue : Qfalse;
}

/* call-seq:
 *  set.clear			=> set
 *
 * Remove all elements of this set
 */
static VALUE value_set_clear(VALUE self)
{
    get_wrapped_set(self).clear();
    return self;
}

/* call-seq:
 *  set.initialize_copy(other)  => set
 *
 * Initializes +set+ with the values in +other+. Needed by #dup
 */
static VALUE value_set_initialize_copy(VALUE vself, VALUE vother)
{
    ValueSet const& other = get_wrapped_set(vother);
    set<VALUE> new_set(other.begin(), other.end());
    get_wrapped_set(vself).swap(new_set);
    return vself;
}

/* call-seq:
 *  to_value_set    => value_set
 *
 * Converts this array into a ValueSet object
 */
static VALUE array_to_value_set(VALUE self)
{
    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);

    long size  = RARRAY_LEN(self);
    for (int i = 0; i < size; ++i)
	result.insert(rb_ary_entry(self, i));

    return vresult;
}

#ifndef RUBINIUS
static VALUE enumerable_to_value_set_i(VALUE i, VALUE* memo)
{
    ValueSet& result = *reinterpret_cast<ValueSet*>(memo);
    result.insert(i);
    return Qnil;
}

/* call-seq:
 *  enum.to_value_set		=> value_set
 *
 * Builds a ValueSet object from this enumerable
 */
static VALUE enumerable_to_value_set(VALUE self)
{
    VALUE vresult = rb_funcall2(cValueSet, id_new, 0, NULL);
    ValueSet& result = get_wrapped_set(vresult);

    rb_iterate(rb_each, self, RUBY_METHOD_FUNC(enumerable_to_value_set_i), reinterpret_cast<VALUE>(&result));
    return vresult;
}
#endif

/*
 * Document-class: ValueSet
 *
 * ValueSet is a wrapper around the C++ set<> template. set<> is an ordered container,
 * for which union(), intersection() and difference() is done in linear time. For performance
 * reasons, in the case of ValueSet, the values are ordered by their VALUE, which roughly is
 * their object_id.
 */

extern "C" void Init_value_set()
{
#ifndef RUBINIUS
    rb_define_method(rb_mEnumerable, "to_value_set", RUBY_METHOD_FUNC(enumerable_to_value_set), 0);
#endif
    rb_define_method(rb_cArray, "to_value_set", RUBY_METHOD_FUNC(array_to_value_set), 0);

    cValueSet = rb_define_class("ValueSet", rb_cObject);
    id_new = rb_intern("new");
    rb_define_alloc_func(cValueSet, value_set_alloc);
    rb_define_method(cValueSet, "each", RUBY_METHOD_FUNC(value_set_each), 0);
    rb_define_method(cValueSet, "include?", RUBY_METHOD_FUNC(value_set_include_p), 1);
    rb_define_method(cValueSet, "include_all?", RUBY_METHOD_FUNC(value_set_include_all_p), 1);
    rb_define_method(cValueSet, "union", RUBY_METHOD_FUNC(value_set_union), 1);
    rb_define_method(cValueSet, "intersection", RUBY_METHOD_FUNC(value_set_intersection), 1);
    rb_define_method(cValueSet, "intersects?", RUBY_METHOD_FUNC(value_set_intersects), 1);
    rb_define_method(cValueSet, "difference", RUBY_METHOD_FUNC(value_set_difference), 1);
    rb_define_method(cValueSet, "insert", RUBY_METHOD_FUNC(value_set_insert), 1);
    rb_define_method(cValueSet, "merge", RUBY_METHOD_FUNC(value_set_merge), 1);
    rb_define_method(cValueSet, "delete", RUBY_METHOD_FUNC(value_set_delete), 1);
    rb_define_method(cValueSet, "==", RUBY_METHOD_FUNC(value_set_equal), 1);
    rb_define_method(cValueSet, "to_value_set", RUBY_METHOD_FUNC(value_set_to_value_set), 0);
    rb_define_method(cValueSet, "dup", RUBY_METHOD_FUNC(value_set_dup), 0);
    rb_define_method(cValueSet, "empty?", RUBY_METHOD_FUNC(value_set_empty_p), 0);
    rb_define_method(cValueSet, "size", RUBY_METHOD_FUNC(value_set_size), 0);
    rb_define_method(cValueSet, "clear", RUBY_METHOD_FUNC(value_set_clear), 0);
    rb_define_method(cValueSet, "initialize_copy", RUBY_METHOD_FUNC(value_set_initialize_copy), 1);
    rb_define_method(cValueSet, "delete_if", RUBY_METHOD_FUNC(value_set_delete_if), 0);
}

<|endoftext|>


<commit_before>// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "examples_common.h"

#include <algorithm>
#include <array>
#include <cmath>

#include <franka/exception.h>
#include <franka/robot.h>

std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
                                const std::array<double, 7>& desired_values,
                                const std::array<double, 7>& last_desired_values) {
  std::array<double, 7> limited_values{};
  for (size_t i = 0; i < 7; i++) {
    double desired_difference = (desired_values[i] - last_desired_values[i]) / 1e-3;
    limited_values[i] =
        last_desired_values[i] +
        std::max(std::min(desired_difference, max_derivatives[i]), -max_derivatives[i]) * 1e-3;
  }
  return limited_values;
}

MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
    : q_goal_(q_goal.data()) {
  dq_max_ *= speed_factor;
  ddq_max_start_ *= speed_factor;
  ddq_max_goal_ *= speed_factor;
}

bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_d->cwiseSign().cast<int>();
  Vector7d t_d = t_2_sync_ - t_1_sync_;
  Vector7d delta_t_2_sync_ = t_f_sync_ - t_2_sync_;
  std::array<bool, 7> joint_motion_finished{};

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
      (*delta_q_d)[i] = 0;
      joint_motion_finished[i] = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3) * dq_max_sync_[i] * sign_delta_q[i] *
                          (0.5 * t - t_1_sync_[i]) * std::pow(t, 3);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        (*delta_q_d)[i] = delta_q_[i] +
                          0.5 *
                              (1.0 / std::pow(delta_t_2_sync_[i], 3) *
                                   (t - t_1_sync_[i] - 2 * delta_t_2_sync_[i] - t_d[i]) *
                                   std::pow((t - t_1_sync_[i] - t_d[i]), 3) +
                               (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync_[i] - 2.0 * t_d[i])) *
                              dq_max_sync_[i] * sign_delta_q[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        joint_motion_finished[i] = true;
      }
    }
  }
  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
                     [](bool x) { return x; });
}

void MotionGenerator::calculateSynchronizedValues() {
  Vector7d dq_max_reach(dq_max_);
  Vector7d t_f = Vector7d::Zero();
  Vector7d delta_t_2 = Vector7d::Zero();
  Vector7d t_1 = Vector7d::Zero();
  Vector7d delta_t_2_sync = Vector7d::Zero();
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }
  double max_t_f = t_f.maxCoeff();
  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2 + delta_t_2_sync[i] / 2 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
    }
  }
}

franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
                                                   franka::Duration time_step) {
  if (time_ == 0.0) {
    q_start_ = Vector7d(robot_state.q_d.data());
    delta_q_ = q_goal_ - q_start_;
    calculateSynchronizedValues();
  }

  time_ += time_step.toSec();

  Vector7d delta_q_d;
  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);

  std::array<double, 7> joint_positions;
  Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
  franka::JointPositions output(joint_positions);
  output.motion_finished = motion_finished;
  return output;
}
<commit_msg>add time_step before check<commit_after>// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "examples_common.h"

#include <algorithm>
#include <array>
#include <cmath>

#include <franka/exception.h>
#include <franka/robot.h>

std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
                                const std::array<double, 7>& desired_values,
                                const std::array<double, 7>& last_desired_values) {
  std::array<double, 7> limited_values{};
  for (size_t i = 0; i < 7; i++) {
    double desired_difference = (desired_values[i] - last_desired_values[i]) / 1e-3;
    limited_values[i] =
        last_desired_values[i] +
        std::max(std::min(desired_difference, max_derivatives[i]), -max_derivatives[i]) * 1e-3;
  }
  return limited_values;
}

MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
    : q_goal_(q_goal.data()) {
  dq_max_ *= speed_factor;
  ddq_max_start_ *= speed_factor;
  ddq_max_goal_ *= speed_factor;
}

bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_d->cwiseSign().cast<int>();
  Vector7d t_d = t_2_sync_ - t_1_sync_;
  Vector7d delta_t_2_sync_ = t_f_sync_ - t_2_sync_;
  std::array<bool, 7> joint_motion_finished{};

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
      (*delta_q_d)[i] = 0;
      joint_motion_finished[i] = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3) * dq_max_sync_[i] * sign_delta_q[i] *
                          (0.5 * t - t_1_sync_[i]) * std::pow(t, 3);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        (*delta_q_d)[i] = delta_q_[i] +
                          0.5 *
                              (1.0 / std::pow(delta_t_2_sync_[i], 3) *
                                   (t - t_1_sync_[i] - 2 * delta_t_2_sync_[i] - t_d[i]) *
                                   std::pow((t - t_1_sync_[i] - t_d[i]), 3) +
                               (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync_[i] - 2.0 * t_d[i])) *
                              dq_max_sync_[i] * sign_delta_q[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        joint_motion_finished[i] = true;
      }
    }
  }
  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
                     [](bool x) { return x; });
}

void MotionGenerator::calculateSynchronizedValues() {
  Vector7d dq_max_reach(dq_max_);
  Vector7d t_f = Vector7d::Zero();
  Vector7d delta_t_2 = Vector7d::Zero();
  Vector7d t_1 = Vector7d::Zero();
  Vector7d delta_t_2_sync = Vector7d::Zero();
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }
  double max_t_f = t_f.maxCoeff();
  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2 + delta_t_2_sync[i] / 2 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
    }
  }
}

franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
                                                   franka::Duration time_step) {
  time_ += time_step.toSec();

  if (time_ == 0.0) {
    q_start_ = Vector7d(robot_state.q_d.data());
    delta_q_ = q_goal_ - q_start_;
    calculateSynchronizedValues();
  }

  Vector7d delta_q_d;
  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);

  std::array<double, 7> joint_positions;
  Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
  franka::JointPositions output(joint_positions);
  output.motion_finished = motion_finished;
  return output;
}
<|endoftext|>