diff --git a/CMakeLists.txt b/CMakeLists.txt index 6cec042..01ec585 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,7 +80,7 @@ endif() list(APPEND UNGAR_INCLUDE_DIRECTORIES $ $) -list(APPEND UNGAR_LINK_LIBRARIES Eigen3::Eigen hana) +list(APPEND UNGAR_LINK_LIBRARIES Eigen3::Eigen hana Boost::preprocessor) # Optional modules. if(UNGAR_ENABLE_LOGGING) diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt index f725161..4c750d6 100644 --- a/external/CMakeLists.txt +++ b/external/CMakeLists.txt @@ -48,6 +48,18 @@ elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) set(HANA_ROOT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hana/install) endif() +# Preprocessor. +if(DEFINED Preprocessor_ROOT) + set(PREPROCESSOR_ROOT_DIRECTORY ${Preprocessor_ROOT}) +elseif(NOT UNGAR_USE_SYSTEM_LIBRARIES) + set(USE_BUNDLED_PREPROCESSOR ON) + set(BUNDLED_PREPROCESSOR_FILENAME + ${CMAKE_CURRENT_LIST_DIR}/config/preprocessor/preprocessor-1.84.0-ungar.zip + ) + set(PREPROCESSOR_ROOT_DIRECTORY + ${CMAKE_CURRENT_BINARY_DIR}/preprocessor/install) +endif() + # GoogleTest. if(DEFINED GTest_ROOT) set(GTEST_ROOT_DIRECTORY ${GTest_ROOT}) @@ -161,6 +173,28 @@ else() message(STATUS "Hana found.") endif() +# ############################################################################## +# Add Preprocessor. +# ############################################################################## +message(STATUS "--------------------------------------------------") +if(USE_BUNDLED_PREPROCESSOR) + message(STATUS "Using bundled Preprocessor...") + download_external_project(config/preprocessor/CMakeLists.txt.in preprocessor) + find_package( + Preprocessor REQUIRED PATHS + ${PREPROCESSOR_ROOT_DIRECTORY}/lib/cmake/preprocessor NO_DEFAULT_PATH) +else() + message(STATUS "Using system-wide Preprocessor...") + find_package(Preprocessor REQUIRED) +endif() +set_target_properties(Boost::preprocessor PROPERTIES IMPORTED_GLOBAL TRUE) + +if(DEFINED PREPROCESSOR_ROOT_DIRECTORY) + message(STATUS "Preprocessor found at '${PREPROCESSOR_ROOT_DIRECTORY}'.") +else() + message(STATUS "Preprocessor found.") +endif() + # =========================================================================== # 2) TESTING if(UNGAR_BUILD_TESTS) diff --git a/external/config/preprocessor/CMakeLists.txt.in b/external/config/preprocessor/CMakeLists.txt.in new file mode 100644 index 0000000..09a3dde --- /dev/null +++ b/external/config/preprocessor/CMakeLists.txt.in @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.20) + +project(ExternalPreprocessor NONE) + +include(ExternalProject) + +if(NOT @Preprocessor_CMAKE_FLAGS@ STREQUAL "") + message( + STATUS "Preprocessor additional CMake flags: @Preprocessor_CMAKE_FLAGS@") +endif() + +ExternalProject_Add( + preprocessor-1.84.0-ungar + URL @BUNDLED_PREPROCESSOR_FILENAME@ + URL_HASH + SHA256=6c5a4bf474c1ee5355d426a0a890d96186361eb66b73666bb09dc4062270435f + CMAKE_CACHE_ARGS + -DCMAKE_CXX_STANDARD:STRING=20 + -DCMAKE_INSTALL_PREFIX:STRING=@CMAKE_CURRENT_BINARY_DIR@/preprocessor/install + -DBUILD_TESTING:BOOL=OFF + -DBOOST_PREPROCESSOR_INSTALL:BOOL=ON + @Preprocessor_CMAKE_FLAGS@ + UPDATE_DISCONNECTED 1 + DOWNLOAD_EXTRACT_TIMESTAMP 1) diff --git a/external/config/preprocessor/preprocessor-1.84.0-ungar.zip b/external/config/preprocessor/preprocessor-1.84.0-ungar.zip new file mode 100644 index 0000000..42057a2 Binary files /dev/null and b/external/config/preprocessor/preprocessor-1.84.0-ungar.zip differ diff --git a/include/ungar/mvariable.hpp b/include/ungar/mvariable.hpp new file mode 100644 index 0000000..52daa29 --- /dev/null +++ b/include/ungar/mvariable.hpp @@ -0,0 +1,362 @@ +/****************************************************************************** + * + * @file ungar/mvariable.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__MVARIABLE_HPP_ +#define _UNGAR__MVARIABLE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "ungar/utils/macros/for_each.hpp" +#include "ungar/utils/utils.hpp" + +namespace Ungar { + +enum class MVariableKind { LEAF, BRANCH, ARRAY }; +enum class MVariableSpace { EUCLIDEAN, UNIT_QUATERNION }; + +template +constexpr auto make_mvariable_array(index_t index) { + return [index](std::index_sequence<_IS...>) { + return std::array<_Var, _N>{(index + _Var::Size() * static_cast(_IS))...}; + } + (std::make_index_sequence<_N>()); +} + +#define UNGAR_LEAF_MVARIABLE(name, size) \ + inline constexpr struct name##_t { \ + private: \ + static constexpr auto _name = #name; \ + static constexpr auto _size = \ + ::Ungar::hana::if_(static_cast<::Ungar::index_t>(size) == ::Ungar::Utils::Q, \ + static_cast<::Ungar::index_t>(4), \ + static_cast<::Ungar::index_t>(size)); \ + static constexpr auto _kind = ::Ungar::MVariableKind::LEAF; \ + static constexpr auto _space = \ + ::Ungar::hana::if_(static_cast<::Ungar::index_t>(size) == ::Ungar::Utils::Q, \ + ::Ungar::MVariableSpace::UNIT_QUATERNION, \ + ::Ungar::MVariableSpace::EUCLIDEAN); \ + ::Ungar::index_t _index; \ + \ + public: \ + static constexpr const char* Name() { \ + return _name; \ + } \ + \ + static constexpr ::Ungar::index_t Size() { \ + return _size; \ + } \ + \ + static constexpr ::Ungar::MVariableSpace Space() { \ + return _space; \ + } \ + \ + constexpr ::Ungar::index_t Index() const { \ + return _index; \ + } \ + \ + constexpr name##_t(const ::Ungar::index_t _index) : _index{_index} { \ + } \ + \ + constexpr const auto& Get(auto _var, auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var)->get().Get(_args...); \ + } else { \ + return GetOpt(_var).value().get(); \ + } \ + } \ + \ + constexpr const auto& Get(auto _var, \ + std::convertible_to<::Ungar::index_t> auto i, \ + auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var, i)->get().Get(_args...); \ + } else { \ + return GetOpt(_var, i).value().get(); \ + } \ + } \ + \ + constexpr const auto& Get(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i1, \ + std::convertible_to<::Ungar::index_t> auto _i2, \ + auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var, _i1, _i2)->get().Get(_args...); \ + } else { \ + return GetOpt(_var, _i1, _i2).value().get(); \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var) const { \ + if constexpr (std::same_as) { \ + return ::Ungar::hana::just(std::cref(*this)); \ + } else { \ + return ::Ungar::hana::nothing; \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i) const { \ + return ::Ungar::hana::nothing; \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i1, \ + std::convertible_to<::Ungar::index_t> auto _i2) const { \ + return ::Ungar::hana::nothing; \ + } \ + \ + constexpr decltype(auto) GetOpt(auto... _args) const { \ + return ::Ungar::hana::nothing; \ + } \ + } name { \ + static_cast<::Ungar::index_t>(0) \ + } + +#define UNGAR_MVARIABLE_ARRAY(name, var, size) \ + inline constexpr struct name##_t { \ + private: \ + static constexpr auto _name = #name; \ + static constexpr auto _size = static_cast<::Ungar::index_t>(size) * var##_t::Size(); \ + static constexpr auto _kind = ::Ungar::MVariableKind::ARRAY; \ + static constexpr auto _space = ::Ungar::MVariableSpace::EUCLIDEAN; \ + ::Ungar::index_t _index; \ + ::std::array(size)> _data; \ + \ + public: \ + static constexpr const char* Name() { \ + return _name; \ + } \ + \ + static constexpr ::Ungar::index_t Size() { \ + return _size; \ + } \ + \ + static constexpr ::Ungar::MVariableSpace Space() { \ + return _space; \ + } \ + \ + constexpr ::Ungar::index_t Index() const { \ + return _index; \ + } \ + \ + constexpr name##_t(const ::Ungar::index_t _index) \ + : _index{_index}, \ + _data{make_mvariable_array(size)>(_index)} { \ + } \ + \ + constexpr decltype(auto) operator[](std::convertible_to<::Ungar::index_t> auto i) const { \ + return _data[static_cast<::std::size_t>(i)]; \ + } \ + \ + constexpr const auto& Get(auto _var, auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var)->get().Get(_args...); \ + } else { \ + return GetOpt(_var).value().get(); \ + } \ + } \ + \ + constexpr const auto& Get(auto _var, \ + std::convertible_to<::Ungar::index_t> auto i, \ + auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var, i)->get().Get(_args...); \ + } else { \ + return GetOpt(_var, i).value().get(); \ + } \ + } \ + \ + constexpr const auto& Get(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i1, \ + std::convertible_to<::Ungar::index_t> auto _i2, \ + auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var, _i1, _i2)->get().Get(_args...); \ + } else { \ + return GetOpt(_var, _i1, _i2).value().get(); \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var) const { \ + if constexpr (std::same_as) { \ + return ::Ungar::hana::just(std::cref(*this)); \ + } else { \ + return ::Ungar::hana::nothing; \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i) const { \ + if constexpr (std::same_as, \ + ::Ungar::hana::optional<>>) { \ + return ::Ungar::hana::nothing; \ + } else { \ + return _data[static_cast<::std::size_t>(_i)].GetOpt(_var); \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i1, \ + std::convertible_to<::Ungar::index_t> auto _i2) const { \ + if constexpr (std::same_as< \ + std::remove_cvref_t, \ + ::Ungar::hana::optional<>>) { \ + return ::Ungar::hana::nothing; \ + } else { \ + return _data[static_cast<::std::size_t>(_i1)].GetOpt(_var, _i2); \ + } \ + } \ + } name { \ + static_cast<::Ungar::index_t>(0) \ + } + +#define _UNGAR_BRANCH_MVARIABLE_HELPER_1_PRED(r, state) \ + BOOST_PP_NOT_EQUAL(BOOST_PP_SEQ_SIZE(state), 1) +#define _UNGAR_BRANCH_MVARIABLE_HELPER_1_OP(r, state) BOOST_PP_SEQ_POP_FRONT(state) +#define _UNGAR_BRANCH_MVARIABLE_HELPER_1(r, state) \ + , BOOST_PP_SEQ_ELEM(1, state) { \ + BOOST_PP_SEQ_ELEM(0, state).Index() + BOOST_PP_SEQ_ELEM(0, state).Size() \ + } +#define _UNGAR_BRANCH_MVARIABLE_HELPER_2(name) +name##_t::Size() +#define _UNGAR_BRANCH_MVARIABLE_HELPER_3(name) , name.GetOpt(_var) +#define _UNGAR_BRANCH_MVARIABLE_HELPER_4(name) , name.GetOpt(_var, _i) +#define _UNGAR_BRANCH_MVARIABLE_HELPER_5(name) , name.GetOpt(_var, _i1, _i2) +#define _UNGAR_BRANCH_MVARIABLE_HELPER_6(name) name##_t name; +#define UNGAR_BRANCH_MVARIABLE(name, firstSubVariableName, ...) \ + inline constexpr struct name##_t { \ + private: \ + static constexpr auto _name = #name; \ + static constexpr auto _size = firstSubVariableName##_t::Size() \ + UNGAR_FOR_EACH(_UNGAR_BRANCH_MVARIABLE_HELPER_2, __VA_ARGS__); \ + static constexpr auto _kind = ::Ungar::MVariableKind::BRANCH; \ + static constexpr auto _space = ::Ungar::MVariableSpace::EUCLIDEAN; \ + ::Ungar::index_t _index; \ + \ + public: \ + UNGAR_FOR_EACH(_UNGAR_BRANCH_MVARIABLE_HELPER_6, firstSubVariableName, __VA_ARGS__) \ + \ + constexpr name##_t(const ::Ungar::index_t _index) \ + : _index{_index}, \ + firstSubVariableName{_index} BOOST_PP_FOR( \ + (firstSubVariableName)BOOST_PP_VARIADIC_TO_SEQ(__VA_ARGS__), \ + _UNGAR_BRANCH_MVARIABLE_HELPER_1_PRED, \ + _UNGAR_BRANCH_MVARIABLE_HELPER_1_OP, \ + _UNGAR_BRANCH_MVARIABLE_HELPER_1) { \ + } \ + \ + static constexpr const char* Name() { \ + return _name; \ + } \ + \ + static constexpr ::Ungar::index_t Size() { \ + return _size; \ + } \ + \ + static constexpr ::Ungar::MVariableSpace Space() { \ + return _space; \ + } \ + \ + constexpr ::Ungar::index_t Index() const { \ + return _index; \ + } \ + \ + constexpr const auto& Get(auto _var, auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var)->get().Get(_args...); \ + } else { \ + return GetOpt(_var).value().get(); \ + } \ + } \ + \ + constexpr const auto& Get(auto _var, \ + std::convertible_to<::Ungar::index_t> auto i, \ + auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var, i)->get().Get(_args...); \ + } else { \ + return GetOpt(_var, i).value().get(); \ + } \ + } \ + \ + constexpr const auto& Get(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i1, \ + std::convertible_to<::Ungar::index_t> auto _i2, \ + auto... _args) const { \ + if constexpr (sizeof...(_args)) { \ + return GetOpt(_var, _i1, _i2)->get().Get(_args...); \ + } else { \ + return GetOpt(_var, _i1, _i2).value().get(); \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var) const { \ + if constexpr (std::same_as) { \ + return ::Ungar::hana::just(std::cref(*this)); \ + } else { \ + auto candidates = ::Ungar::hana::make_tuple(firstSubVariableName.GetOpt( \ + _var) UNGAR_FOR_EACH(_UNGAR_BRANCH_MVARIABLE_HELPER_3, __VA_ARGS__)); \ + static_assert(hana::count_if(candidates, ::Ungar::hana::is_just) <= \ + ::Ungar::hana::size_c<1UL>, \ + "To prevent ambiguous bypasses, a variable must appear at most " \ + "once as a sub-variable."); \ + return ::Ungar::hana::flatten(hana::find_if(candidates, ::Ungar::hana::is_just)); \ + } \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i) const { \ + auto candidates = ::Ungar::hana::make_tuple(firstSubVariableName.GetOpt( \ + _var, _i) UNGAR_FOR_EACH(_UNGAR_BRANCH_MVARIABLE_HELPER_4, __VA_ARGS__)); \ + static_assert( \ + hana::count_if(candidates, ::Ungar::hana::is_just) <= ::Ungar::hana::size_c<1UL>, \ + "To prevent ambiguous bypasses, a variable must appear at most once as " \ + "a sub-variable."); \ + return ::Ungar::hana::flatten(hana::find_if(candidates, ::Ungar::hana::is_just)); \ + } \ + \ + constexpr decltype(auto) GetOpt(auto _var, \ + std::convertible_to<::Ungar::index_t> auto _i1, \ + std::convertible_to<::Ungar::index_t> auto _i2) const { \ + auto candidates = ::Ungar::hana::make_tuple(firstSubVariableName.GetOpt( \ + _var, _i1, _i2) UNGAR_FOR_EACH(_UNGAR_BRANCH_MVARIABLE_HELPER_5, __VA_ARGS__)); \ + static_assert( \ + hana::count_if(candidates, ::Ungar::hana::is_just) <= ::Ungar::hana::size_c<1UL>, \ + "To prevent ambiguous bypasses, a variable must appear at most once as " \ + "a sub-variable."); \ + return ::Ungar::hana::flatten(hana::find_if(candidates, ::Ungar::hana::is_just)); \ + } \ + } name { \ + static_cast<::Ungar::index_t>(0) \ + } + +} // namespace Ungar + +#endif /* _UNGAR__MVARIABLE_HPP_ */ diff --git a/include/ungar/mvariable_lazy_map.hpp b/include/ungar/mvariable_lazy_map.hpp new file mode 100644 index 0000000..f643982 --- /dev/null +++ b/include/ungar/mvariable_lazy_map.hpp @@ -0,0 +1,363 @@ +/****************************************************************************** + * + * @file ungar/mvariable_lazy_map.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__MVARIABLE_LAZY_MAP_HPP_ +#define _UNGAR__MVARIABLE_LAZY_MAP_HPP_ + +#include "ungar/mvariable.hpp" +#include "ungar/utils/utils.hpp" + +namespace Ungar { + +/** + * @brief Class representing a lazy mapping between an m-variable and an underlying data container. + * + * The MVariableLazyMap class provides a lazy mapping between an m-variable and an underlying + * data container, such as an Eigen vector or an std::array. It allows efficient access to the + * data associated with the m-variable without explicitly copying or storing the data. + * + * @tparam _Scalar Type of the underlying data (e.g., real_t, ad_scalar_t). + * @tparam _Variable MVariable type representing the m-variable being mapped. + * @tparam _EnableMutableMembers Boolean constant indicating whether mutable members are + * enabled or not. + * + * @warning The size of the underlying data must be identical to the size of the m-variable being + * mapped. Incorrect size mapping may lead to unexpected behavior or runtime errors. + */ +template +class MVariableLazyMap { + private: + template + static constexpr auto UnderlyingSize(hana::basic_type<_Underlying>) { + if constexpr (Concepts::DenseMatrixExpression<_Underlying>) { + return std::remove_cvref_t<_Underlying>::RowsAtCompileTime; + } else { + return static_cast(std::ranges::size(_Underlying{})); + } + } + + public: + using ScalarType = _Scalar; + + /** + * @brief Default constructor (not intended for use). + * + * @warning Calling this constructor results in undefined behavior. + */ + constexpr MVariableLazyMap() : _data{nullptr}, _variable{} { + Unreachable(); + } + + /** + * @brief Construct m-variable lazy map with a constant underlying data container and a m-variable. + * + * This constructor creates a MVariableLazyMap with a given constant VectorX<_Scalar> as the + * underlying data and a MVariable object representing the m-variable being mapped. The sizes of + * the data container and the m-variable must match, and the m-variable must have an index of 0. The + * resulting map has only const member functions. + * + * @param[in] underlying VectorX<_Scalar> representing the underlying data container. + * @param[in] var MVariable object representing the m-variable being mapped. + * + * @warning The sizes of the underlying data and the MVariable must match, and the m-variable's index + * must be 0. Otherwise, calling this constructor results in an assertion failure. + */ + MVariableLazyMap(const VectorX<_Scalar>& underlying, const _Variable& var) + : _data{underlying.data()}, _variable{var} { + UNGAR_ASSERT(underlying.size() == var.Size() && !var.Index()); + } + + /** + * @brief Construct m-variable lazy map with an lvalue VectorX<_Scalar> and a m-variable. + * + * This constructor creates a m-variable lazy map with a given lvalue VectorX<_Scalar> as the + * underlying data and a MVariable object representing the m-variable being mapped. The sizes + * of the data container and the m-variable must match, and the m-variable must have an index of 0. + * The resulting map has both const and non-const member functions. + * + * @param[in] underlying Lvalue VectorX<_Scalar> representing the underlying data container. + * @param[in] var MVariable object representing the m-variable being mapped. + * + * @warning The sizes of the underlying data and the m-variable must match, and the m-variable's index + * must be 0. Otherwise, calling this constructor results in an assertion failure. + */ + MVariableLazyMap(VectorX<_Scalar>& underlying, const _Variable& var) + : _data{underlying.data()}, _variable{var} { + UNGAR_ASSERT(underlying.size() == var.Size() && !var.Index()); + } + + /** + * @brief Construct m-variable lazy map with a constant lvalue data container and a m-variable. + * + * This constructor creates a m-variable lazy map with a given constant lvalue data container, + * `_Underlying`, and a MVariable object representing the m-variable being mapped. The sizes of + * the data container and the m-variable must match, and the m-variable must have an index of 0. + * The resulting map has only const member functions. + * + * @tparam _Underlying Type of the constant lvalue data container. + * @param[in] underlying Constant lvalue data container. + * @param[in] var MVariable object representing the m-variable being mapped. + * + * @warning The sizes of the underlying data and the m-variable must match, and the m-variable's index + * must be 0. Otherwise, calling this constructor results in an assertion failure. + */ + template + constexpr MVariableLazyMap(const _Underlying& underlying, const _Variable& var) requires( + UnderlyingSize(hana::type_c<_Underlying>) == static_cast(_Variable::Size())) + : _data{std::ranges::data(underlying)}, _variable{var} { + UNGAR_ASSERT(!var.Index()); + } + + /** + * @brief Construct m-variable lazy map with a mutable lvalue data container and a m-variable. + * + * This constructor creates a m-variable lazy map with a given mutable lvalue data container, + * `_Underlying`, and a MVariable object representing the m-variable being mapped. The sizes of + * the data container and the m-variable must match, and the MVariable must have an index of 0. + * The resulting map has both const and non-const member functions. + * + * @tparam _Underlying Type of the mutable lvalue data container. + * @param[in] underlying Mutable lvalue data container. + * @param[in] var MVariable object representing the m-variable being mapped. + * + * @warning The sizes of the underlying data and the m-variable must match, and the m-variable's index + * must be 0. Otherwise, calling this constructor results in an assertion failure. + */ + template + constexpr MVariableLazyMap(_Underlying& underlying, const _Variable& var) requires( + UnderlyingSize(hana::type_c<_Underlying>) == static_cast(_Variable::Size())) + : _data{std::ranges::data(underlying)}, _variable{var} { + UNGAR_ASSERT(!var.Index()); + } + + /** + * @brief Get underlying data associated with the m-variable in the m-variable lazy map. + * + * This member function retrieves the underlying data associated with the m-variable stored in the + * m-variable lazy map. The data is returned as a read-only reference. See [1] for more details. + * + * @return Read-only Eigen::Map to the underlying data associated with the MVariable. + * + * @see [1] Flavio De Vincenti and Stelian Coros. "Ungar -- A C++ Framework for + * Real-Time Optimal Control Using Template Metaprogramming." 2023 IEEE/RSJ + * International Conference on Intelligent Robots and Systems (IROS) (2023). + */ + decltype(auto) Get() const { + return Get1(_variable); + } + + /** + * @brief Get underlying data associated with the m-variable in the m-variable lazy map. + * + * This member function retrieves the underlying data associated with the m-variable stored in the + * m-variable lazy map. + * + * @return Eigen::Map to the underlying data associated with the MVariable. + * + * @see MVariableLazyMap::Get. + */ + decltype(auto) Get() requires _EnableMutableMembers { + return Get1(_variable); + } + + /** + * @brief Get underlying data associated with sub-variable in the m-variable lazy map. + * + * This member function retrieves the underlying data associated with a sub-variable stored in the + * m-variable lazy map. The data is returned as a read-only reference. + * + * @param[in] args Arguments specifying the sub-variable. + * @return Read-only Eigen::Map to the underlying data associated with the specified MVariable. + * + * @see MVariableLazyMap::Get. + */ + decltype(auto) Get(auto&&... args) const { + return Get1(_variable.Get(std::forward(args)...)); + } + + /** + * @brief Get underlying data associated with sub-variable in the m-variable lazy map. + * + * This member function retrieves the underlying data associated with a sub-variable stored in the + * m-variable lazy map. + * + * @param[in] args Arguments specifying the sub-variable. + * @return Eigen::Map to the underlying data associated with the specified MVariable. + * + * @see MVariableLazyMap::Get. + */ + decltype(auto) Get(auto&&... args) requires _EnableMutableMembers { + return Get1(_variable.Get(std::forward(args)...)); + } + + /** + * @brief Get tuple of underlying data associated with multiple sub-variables in the m-variable lazy map. + * + * This member function retrieves a tuple of underlying data associated with multiple Variables stored + * in the m-variable lazy map. The data is returned as a tuple of read-only references. + * + * @param[in] vars Sub-variables to be retrieved simultaneously. + * @return Tuple of read-only Eigen::Map objects to the specified sub-variables. + * + * @see MVariableLazyMap::Get. + */ + decltype(auto) GetTuple(auto&&... vars) const { + return std::tuple(vars)))...>( + Get(std::forward(vars))...); + } + + /** + * @brief Get tuple of underlying data associated with multiple sub-variables in the m-variable lazy map. + * + * This member function retrieves a tuple of underlying data associated with multiple Variables stored + * in the m-variable lazy map. + * + * @param[in] vars Sub-variables to be retrieved simultaneously. + * @return Tuple of Eigen::Map objects to the specified sub-variables. + * + * @see MVariableLazyMap::Get. + */ + decltype(auto) GetTuple(auto&&... vars) requires _EnableMutableMembers { + return std::tuple(vars)))...>( + Get(std::forward(vars))...); + } + + /** + * @todo Remove (sub-variables should only be accessed using \c Get and \c \GetTuple + * member functions). + */ + decltype(auto) Get1(const auto& var) const { + using VariableType = std::remove_cvref_t; + if constexpr (VariableType::Size() == 1_idx) { + return GetImpl(var).get(); + } else { + return GetImpl(var); + } + } + + /** + * @todo Remove (sub-variables should only be accessed using \c Get and \c \GetTuple + * member functions). + */ + decltype(auto) Get1(const auto& var) { + using VariableType = std::remove_cvref_t; + if constexpr (VariableType::Size() == 1_idx) { + return GetImpl(var).get(); + } else { + return GetImpl(var); + } + } + + protected: + auto GetImpl(const auto& var) const { + using VariableType = std::remove_cvref_t; + if constexpr (VariableType::Space() == MVariableSpace::UNIT_QUATERNION) { + return Eigen::Map>{_data + var.Index()}; + } else { + constexpr auto size = VariableType::Size(); + if constexpr (size == 1_idx) { + return std::cref(*(_data + var.Index())); + } else if constexpr (size <= 32_idx) { + return Eigen::Map(size)>>{_data + + var.Index()}; + } else { + return Eigen::Map>{_data + var.Index(), size}; + } + } + } + + auto GetImpl(const auto& var) requires _EnableMutableMembers { + using VariableType = std::remove_cvref_t; + if constexpr (VariableType::Space() == MVariableSpace::UNIT_QUATERNION) { + return Eigen::Map>{const_cast<_Scalar*>(_data) + var.Index()}; + } else { + constexpr auto size = VariableType::Size(); + if constexpr (size == 1_idx) { + return std::ref(*(const_cast<_Scalar*>(_data) + var.Index())); + } else if constexpr (size <= 32_idx) { + return Eigen::Map(size)>>{ + const_cast<_Scalar*>(_data) + var.Index()}; + } else { + return Eigen::Map>{const_cast<_Scalar*>(_data) + var.Index(), + size}; + } + } + } + + private: + const _Scalar* _data; + _Variable _variable; +}; + +MVariableLazyMap(const auto& underlying, const auto& var) + -> MVariableLazyMap, + std::remove_cvref_t, + false>; + +MVariableLazyMap(auto& underlying, const auto& var) + -> MVariableLazyMap, + std::remove_cvref_t, + true>; + +/** + * @brief Create m-variable lazy map with a constant lvalue data container and a m-variable. + * + * This function creates a m-variable lazy map with a given constant lvalue data container, `underlying`, + * and a MVariable object representing the m-variable being mapped. The sizes of the data container + * and the m-variable must match, and the m-variable must have an index of 0. + * + * @param[in] underlying Constant lvalue data container. + * @param[in] var MVariable object representing the m-variable being mapped. + * @return MVariableLazyMap object with the specified data container and m-variable. + * + * @warning The sizes of the underlying data and the m-variable must match, and the m-variable's index + * must be 0. Otherwise, creating the m-variable lazy map results in an assertion failure. + */ +inline static auto MakeMVariableLazyMap(const auto& underlying, const auto& var) { + return MVariableLazyMap{underlying, var}; +} + +/** + * @brief Create m-variable lazy map with a mutable lvalue data container and a m-variable. + * + * This function creates a m-variable lazy map with a given mutable lvalue data container, `underlying`, + * and a MVariable object representing the m-variable being mapped. The sizes of the data container + * and the m-variable must match, and the m-variable must have an index of 0. + * + * @param[in] underlying The mutable lvalue data container. + * @param[in] var The MVariable object representing the m-variable being mapped. + * @return MVariableLazyMap object with the specified data container and MVariable. + * + * @warning The sizes of the underlying data and the m-variable must match, and the m-variable's index + * must be 0. Otherwise, creating the MVariableLazyMap will result in an assertion failure. + */ +inline static auto MakeMVariableLazyMap(auto& underlying, const auto& var) { + return MVariableLazyMap{underlying, var}; +} + +} // namespace Ungar + +#endif /* _UNGAR__MVARIABLE_LAZY_MAP_HPP_ */ diff --git a/include/ungar/utils/macros/for_each.hpp b/include/ungar/utils/macros/for_each.hpp new file mode 100644 index 0000000..036ed7f --- /dev/null +++ b/include/ungar/utils/macros/for_each.hpp @@ -0,0 +1,48 @@ +/****************************************************************************** + * + * @file ungar/utils/macros/for_each.hpp + * @author Flavio De Vincenti (flavio.devincenti@inf.ethz.ch) + * + * @section LICENSE + * ----------------------------------------------------------------------- + * + * Copyright 2023 Flavio De Vincenti + * + * ----------------------------------------------------------------------- + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an "AS + * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either + * express or implied. See the License for the specific language + * governing permissions and limitations under the License. + * + ******************************************************************************/ + +#ifndef _UNGAR__UTILS__MACROS__FOR_EACH_HPP_ +#define _UNGAR__UTILS__MACROS__FOR_EACH_HPP_ + +#define _UNGAR_PARENS () + +#define _UNGAR_EXPAND(...) \ + _UNGAR_EXPAND4(_UNGAR_EXPAND4(_UNGAR_EXPAND4(_UNGAR_EXPAND4(__VA_ARGS__)))) +#define _UNGAR_EXPAND4(...) \ + _UNGAR_EXPAND3(_UNGAR_EXPAND3(_UNGAR_EXPAND3(_UNGAR_EXPAND3(__VA_ARGS__)))) +#define _UNGAR_EXPAND3(...) \ + _UNGAR_EXPAND2(_UNGAR_EXPAND2(_UNGAR_EXPAND2(_UNGAR_EXPAND2(__VA_ARGS__)))) +#define _UNGAR_EXPAND2(...) \ + _UNGAR_EXPAND1(_UNGAR_EXPAND1(_UNGAR_EXPAND1(_UNGAR_EXPAND1(__VA_ARGS__)))) +#define _UNGAR_EXPAND1(...) __VA_ARGS__ + +#define UNGAR_FOR_EACH(macro, ...) \ + __VA_OPT__(_UNGAR_EXPAND(_UNGAR_FOR_EACH_IMPL_1(macro, __VA_ARGS__))) +#define _UNGAR_FOR_EACH_IMPL_1(macro, first, ...) \ + macro(first) __VA_OPT__(_UNGAR_FOR_EACH_IMPL_2 _UNGAR_PARENS(macro, __VA_ARGS__)) +#define _UNGAR_FOR_EACH_IMPL_2() _UNGAR_FOR_EACH_IMPL_1 + +#endif /* _UNGAR__UTILS__MACROS__FOR_EACH_HPP_ */ diff --git a/include/ungar/variable_lazy_map.hpp b/include/ungar/variable_lazy_map.hpp index 4878430..41192ee 100644 --- a/include/ungar/variable_lazy_map.hpp +++ b/include/ungar/variable_lazy_map.hpp @@ -43,8 +43,8 @@ namespace Ungar { * @tparam _EnableMutableMembers Boolean integral constant indicating whether mutable members are * enabled or not. * - * @warning The size of the underlying data must be identical to the size of the variable being. - * mapped. Incorrect type mapping may lead to unexpected behavior or runtime errors. + * @warning The size of the underlying data must be identical to the size of the variable being + * mapped. Incorrect size mapping may lead to unexpected behavior or runtime errors. */ template class VariableLazyMap { @@ -124,9 +124,8 @@ class VariableLazyMap { * must be 0. Otherwise, calling this constructor results in an assertion failure. */ template - constexpr VariableLazyMap(const _Underlying& underlying, const _Variable& var) - requires(UnderlyingSize(hana::type_c<_Underlying>) == - static_cast(_Variable::Size())) + constexpr VariableLazyMap(const _Underlying& underlying, const _Variable& var) requires( + UnderlyingSize(hana::type_c<_Underlying>) == static_cast(_Variable::Size())) : _data{std::ranges::data(underlying)}, _variable{var} { UNGAR_ASSERT(!var.Index()); } @@ -147,9 +146,8 @@ class VariableLazyMap { * must be 0. Otherwise, calling this constructor results in an assertion failure. */ template - constexpr VariableLazyMap(_Underlying& underlying, const _Variable& var) - requires(UnderlyingSize(hana::type_c<_Underlying>) == - static_cast(_Variable::Size())) + constexpr VariableLazyMap(_Underlying& underlying, const _Variable& var) requires( + UnderlyingSize(hana::type_c<_Underlying>) == static_cast(_Variable::Size())) : _data{std::ranges::data(underlying)}, _variable{var} { UNGAR_ASSERT(!var.Index()); } @@ -180,9 +178,7 @@ class VariableLazyMap { * * @see VariableLazyMap::Get. */ - decltype(auto) Get() - requires _EnableMutableMembers::value - { + decltype(auto) Get() requires _EnableMutableMembers::value { return Get1(_variable); } @@ -212,9 +208,7 @@ class VariableLazyMap { * * @see VariableLazyMap::Get. */ - decltype(auto) Get(auto&&... args) - requires _EnableMutableMembers::value - { + decltype(auto) Get(auto&&... args) requires _EnableMutableMembers::value { return Get1(_variable(std::forward(args)...)); } @@ -245,9 +239,7 @@ class VariableLazyMap { * * @see VariableLazyMap::Get. */ - decltype(auto) GetTuple(auto&&... vars) - requires _EnableMutableMembers::value - { + decltype(auto) GetTuple(auto&&... vars) requires _EnableMutableMembers::value { return std::tuple(vars)))...>( Get(std::forward(vars))...); } @@ -296,9 +288,7 @@ class VariableLazyMap { } } - auto GetImpl(const Concepts::Variable auto& var) - requires _EnableMutableMembers::value - { + auto GetImpl(const Concepts::Variable auto& var) requires _EnableMutableMembers::value { using VariableType = std::remove_cvref_t; if constexpr (VariableType::IsQuaternion()) { return Eigen::Map>{const_cast<_Scalar*>(_data) + var.Index()};