├── .gitignore ├── LICENSE ├── README.md ├── demo ├── CMakeLists.txt ├── README.md ├── custom_vector_demo.cpp └── euclidean_demo.cpp ├── src └── nigh │ ├── auto_strategy.hpp │ ├── cartesian_space.hpp │ ├── gnat.hpp │ ├── impl │ ├── atom.hpp │ ├── atomic_bs.hpp │ ├── bits.hpp │ ├── block_allocator.hpp │ ├── box_distance.hpp │ ├── cartesian_get.hpp │ ├── cartesian_space.hpp │ ├── compare_nth.hpp │ ├── constants.hpp │ ├── eigen_space.hpp │ ├── identity.hpp │ ├── k_centers.hpp │ ├── kdtree_batch │ │ ├── branch.hpp │ │ ├── clear.hpp │ │ ├── leaf.hpp │ │ ├── lp_branch.hpp │ │ ├── nearest.hpp │ │ ├── nearest_traversal.hpp │ │ ├── nearest_traversal_cartesian.hpp │ │ ├── nearest_traversal_scaled.hpp │ │ ├── nearest_traversal_so2.hpp │ │ ├── nearest_traversal_so3.hpp │ │ ├── nearest_traversals.hpp │ │ ├── node.hpp │ │ ├── so3_branch.hpp │ │ ├── so3_root.hpp │ │ ├── strategy.hpp │ │ ├── traversal.hpp │ │ ├── traversal_cartesian.hpp │ │ ├── traversal_lp.hpp │ │ ├── traversal_scaled.hpp │ │ ├── traversal_so2.hpp │ │ ├── traversal_so3.hpp │ │ ├── traversals.hpp │ │ └── types.hpp │ ├── kdtree_median │ │ ├── accum.hpp │ │ ├── accum_cartesian.hpp │ │ ├── accum_lp.hpp │ │ ├── accum_scaled.hpp │ │ ├── accum_so2.hpp │ │ ├── accum_so3.hpp │ │ ├── accums.hpp │ │ ├── builder.hpp │ │ ├── lp_branch.hpp │ │ ├── nearest.hpp │ │ ├── node.hpp │ │ ├── root.hpp │ │ ├── so3_branch.hpp │ │ ├── so3_root.hpp │ │ ├── strategy.hpp │ │ ├── traversal.hpp │ │ ├── traversal_cartesian.hpp │ │ ├── traversal_lp.hpp │ │ ├── traversal_scaled.hpp │ │ ├── traversal_so2.hpp │ │ ├── traversal_so3.hpp │ │ └── traversals.hpp │ ├── linear │ │ └── store.hpp │ ├── locked_nearest.hpp │ ├── lp_sum.hpp │ ├── metric_specializations.hpp │ ├── near_set.hpp │ ├── nearest_base.hpp │ ├── non_atomic.hpp │ ├── region.hpp │ ├── region_cartesian.hpp │ ├── region_lp.hpp │ ├── region_scaled.hpp │ ├── region_so2.hpp │ ├── region_so3.hpp │ ├── regions.hpp │ ├── root_get.hpp │ ├── scaled_space_base.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── so3_region.hpp │ └── space_base.hpp │ ├── kdtree_batch.hpp │ ├── kdtree_median.hpp │ ├── linear.hpp │ ├── lp_space.hpp │ ├── metric │ ├── cartesian.hpp │ ├── cartesian_state_element.hpp │ ├── lp.hpp │ ├── metric.hpp │ ├── scaled.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── space.hpp │ ├── space_cartesian.hpp │ ├── space_lp_array.hpp │ ├── space_lp_eigen.hpp │ ├── space_lp_scalar.hpp │ ├── space_lp_vector.hpp │ ├── space_scaled.hpp │ ├── space_so2_eigen.hpp │ ├── space_so2_rotation.hpp │ ├── space_so2_scalar.hpp │ ├── space_so3_eigen.hpp │ └── space_so3_quaternion.hpp │ ├── nigh_forward.hpp │ ├── scaled_space.hpp │ ├── se2_space.hpp │ ├── se3_space.hpp │ ├── so2_space.hpp │ └── so3_space.hpp └── test ├── .gitignore ├── README.md ├── auto_strategy_test.cpp ├── bench_template.hpp ├── block_allocator_test.cpp ├── box_sampler.hpp ├── concurrent_test_template.hpp ├── configure.sh ├── custom_vector_test.cpp ├── dup_test.cpp ├── fk100000_test.cpp ├── fkmap100000.txt ├── issue_1_test.cpp ├── lp_space_test.cpp ├── nearest_test_cases.cpp ├── plot.sh ├── sampler.hpp ├── sampler_cartesian.hpp ├── sampler_lp.hpp ├── sampler_scaled.hpp ├── sampler_so2.hpp ├── sampler_so3.hpp ├── scaled_se3_space_test.cpp ├── scaled_space_test.cpp ├── se2_space_test.cpp ├── se3_space_test.cpp ├── so2_dist_test.cpp ├── so2_median_test.cpp ├── so2_space_test.cpp ├── so3_space_test.cpp ├── test.hpp └── test_template.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2018 The University of North Carolina at Chapel Hill 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | -------------------------------------------------------------------------------- /demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 3.10 introduces C++ 17 support 2 | cmake_minimum_required (VERSION 3.10) 3 | project (nigh_demo) 4 | 5 | # Enable C++17 (required for Nigh) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | 9 | # Disable extensions to keep compatible with standards 10 | set(CMAKE_CXX_EXTENSIONS OFF) 11 | 12 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 13 | 14 | include_directories(../src) 15 | 16 | add_executable(euclidean_demo euclidean_demo.cpp) 17 | target_link_libraries(euclidean_demo Eigen3::Eigen) 18 | 19 | add_executable(custom_vector_demo custom_vector_demo.cpp) 20 | target_link_libraries(custom_vector_demo Eigen3::Eigen) 21 | -------------------------------------------------------------------------------- /demo/README.md: -------------------------------------------------------------------------------- 1 | This directory contains files demonstrating how to use Nigh. CMake is required to build the demo files. Eigen 3 and a C++ 17 capable compiler is required. 2 | 3 | To build: 4 | 5 | % mkdir build 6 | % cmake .. 7 | % make 8 | 9 | If the default compiler does not support C++ 17, use the following commands (assuming, for example, that the C++ 17 compiler is in the path and named 'clang++-6.0'. Substitute your compiler as appropriate): 10 | 11 | % mkdir build 12 | % CXX=clang++-6.0 cmake .. 13 | % make 14 | 15 | The following demo files are included: 16 | 17 | euclidean_demo.cpp 18 | ============= 19 | 20 | This demo generates random 2D points on a 1.0x1.0 plane, performs a single query, then writes the output to an image file. You can modify the run-time behavior of the command with the following options: 21 | 22 | -o OUT -- generates OUT (default euclidean_demo.svg) 23 | -k COUNT -- compute the k nearest neighbors (default is 1, unless -r is specified) 24 | -r RADIUS -- compute the nearest neighbors in radius 25 | -N COUNT -- generate COUNT random points (default is 100) 26 | 27 | After running this demo, open 'euclidean_demo.svg' in any modern browser to see the random points and the result of the query. Note: only minimal argument checks on the arguments are performed. 28 | 29 | 30 | custom\_vector\_demo.cpp 31 | ================ 32 | 33 | This demo shows how to use a custom vector type as the key type in a nearest neighbor structure. Out of the box Nigh supports Eigen3 vectors, vectors based upon std::vector and std::array. However if a project is using a custom vector type or a vector based upon a different linear algebra library, Nigh can support it. The only requirement is a template specialization. 34 | 35 | When run, this demo will output the 10 nearest neighbors of 100 random points in a Euclidean R^3 space using a custom vector type. 36 | -------------------------------------------------------------------------------- /src/nigh/cartesian_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_CARTESIAN_SPACE_HPP 38 | #define NIGH_METRIC_CARTESIAN_SPACE_HPP 39 | 40 | #include "metric/cartesian.hpp" 41 | #include "metric/cartesian_state_element.hpp" 42 | #include "metric/space_cartesian.hpp" 43 | #include 44 | 45 | namespace unc::robotics::nigh::impl { 46 | template 47 | struct cartesian_space_select; 48 | 49 | template 50 | struct cartesian_space_select> { 51 | using type = Space< 52 | std::tuple::Type...>, 53 | Cartesian::Metric...>>; 54 | }; 55 | } 56 | 57 | namespace unc::robotics::nigh::metric { 58 | template 59 | using CartesianSpace = typename impl::cartesian_space_select< 60 | std::tuple, std::index_sequence_for>::type; 61 | } 62 | 63 | namespace std { 64 | template 65 | decltype(auto) get(const unc::robotics::nigh::metric::CartesianSpace& space) { 66 | return space.template get(); 67 | } 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/nigh/impl/atom.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_ATOM_HPP 38 | #define NIGH_IMPL_ATOM_HPP 39 | 40 | #include "non_atomic.hpp" 41 | #include "atomic_bs.hpp" 42 | 43 | namespace unc::robotics::nigh::impl { 44 | 45 | // TODO: use std::hardware_destructive_interference_size instead of 64 (when available) 46 | constexpr std::size_t cache_line_size = 64; 47 | 48 | // atomic_selector selects between atomic and non-atomic 49 | // versions of the same type. In theory, with a good optimizer, 50 | // this allow code that does not need atomic guarantees to be 51 | // reordered for better (single-threaded) performance. 52 | template 53 | struct atomicity_selector; 54 | 55 | // Atomic specialization delegates to the atomic_selector choose 56 | // an atomic implementation. 57 | template 58 | struct atomicity_selector { 59 | using type = typename atomic_selector::type; 60 | }; 61 | 62 | // Non-atomic specialization returns a NonAtomic object that 63 | // mimics the std::atomic interface. 64 | template 65 | struct atomicity_selector { 66 | using type = NonAtomic; 67 | }; 68 | 69 | // Base atomic/non-atomic implementation selector. 70 | template 71 | using Atom = typename atomicity_selector::type; 72 | 73 | inline void relax_cpu() { 74 | #if defined(__x86_64__) || defined(__i386__) 75 | __asm__ __volatile__("rep;nop": : :"memory"); 76 | // alternate method: 77 | // asm("rep; nop" ::: "memory"); 78 | #endif 79 | } 80 | } 81 | 82 | #endif // NIGH_IMPL_ATOM_HPP 83 | -------------------------------------------------------------------------------- /src/nigh/impl/bits.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_BITS_HPP 38 | #define NIGH_IMPL_BITS_HPP 39 | 40 | namespace unc::robotics::nigh::impl { 41 | 42 | // TODO: provide alternate implementations when compiler does not 43 | // have the builtin. 44 | 45 | constexpr int clz(unsigned x) { 46 | return __builtin_clz(x); 47 | } 48 | 49 | constexpr int clz(unsigned long x) { 50 | return __builtin_clzl(x); 51 | } 52 | 53 | constexpr int clz(unsigned long long x) { 54 | return __builtin_clzll(x); 55 | } 56 | 57 | constexpr int popcount(unsigned x) { 58 | return __builtin_popcount(x); 59 | } 60 | 61 | constexpr int popcount(unsigned long x) { 62 | return __builtin_popcountl(x); 63 | } 64 | 65 | constexpr int popcount(unsigned long long x) { 66 | return __builtin_popcountll(x); 67 | } 68 | 69 | template 70 | constexpr std::enable_if_t::value, int> 71 | log2(T x) { 72 | return sizeof(x)*8 - 1 - clz(x); 73 | } 74 | } 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /src/nigh/impl/box_distance.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_BOX_DISTANCE_HPP 38 | #define NIGH_IMPL_BOX_DISTANCE_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::nigh::impl { 43 | template 44 | auto boxDistance( 45 | const Eigen::MatrixBase& min, 46 | const Eigen::MatrixBase& max, 47 | const Eigen::MatrixBase& key) 48 | { 49 | return (min - key).cwiseMax(key - max).cwiseMax(0).template lpNorm

(); 50 | } 51 | 52 | template 53 | auto boxDistance( 54 | const Eigen::MatrixBase& min, 55 | const Eigen::MatrixBase& max, 56 | const Eigen::ArrayBase& key) 57 | { 58 | return boxDistance(min, max, key.matrix()); 59 | } 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /src/nigh/impl/cartesian_get.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_CARTESIAN_GET_HPP 38 | #define NIGH_IMPL_CARTESIAN_GET_HPP 39 | 40 | namespace unc::robotics::nigh::impl { 41 | template 42 | struct CartesianGet { 43 | template 44 | static constexpr decltype(auto) part(T&& x) { 45 | return std::get(ParentGet::part(std::forward(x))); 46 | } 47 | }; 48 | } 49 | 50 | #endif 51 | 52 | -------------------------------------------------------------------------------- /src/nigh/impl/compare_nth.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifndef NIGH_IMPL_COMPARE_NTH_HPP 37 | #define NIGH_IMPL_COMPARE_NTH_HPP 38 | 39 | #include 40 | 41 | namespace unc::robotics::nigh::impl { 42 | template 43 | struct CompareNth { 44 | template 45 | bool operator() (const T& a, const T& b) const { 46 | return std::get(a) < std::get(b); 47 | } 48 | }; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/nigh/impl/constants.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_CONSTANTS_HPP 38 | #define NIGH_IMPL_CONSTANTS_HPP 39 | 40 | namespace unc::robotics::nigh::impl { 41 | template constexpr T PI = T( 42 | 3.14159265358979323846264338327950288419716939937510582097494459230781640628620L); 43 | template constexpr T PI_2 = T( 44 | 1.57079632679489661923132169163975144209858469968755291048747229615390820314310L); 45 | template constexpr T E = T( 46 | 2.71828182845904523536028747135266249775724709369995957496696762772407663035355L); 47 | template constexpr T SQRT2 = T( 48 | 1.41421356237309504880168872420969807856967187537694807317667973799073247846211L); 49 | template constexpr T SQRT1_2 = T( 50 | 0.70710678118654752440084436210484903928483593768847403658833986899536623923105L); 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/nigh/impl/eigen_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_EIGEN_SPACE_HPP 38 | #define NIGH_IMPL_EIGEN_SPACE_HPP 39 | 40 | #include "space_base.hpp" 41 | #include 42 | 43 | namespace unc::robotics::nigh::impl { 44 | template 45 | struct EigenSpace; 46 | 47 | template <> 48 | struct EigenSpace<1, 1> : SpaceBase<1> { 49 | using SpaceBase<1>::SpaceBase; 50 | }; 51 | 52 | template 53 | struct EigenSpace : SpaceBase { 54 | using SpaceBase::SpaceBase; 55 | }; 56 | 57 | template 58 | struct EigenSpace<1, c> : SpaceBase { 59 | using SpaceBase::SpaceBase; 60 | }; 61 | 62 | template <> 63 | struct EigenSpace : SpaceBase<-1> { 64 | using SpaceBase::SpaceBase; 65 | }; 66 | 67 | template <> 68 | struct EigenSpace<1, Eigen::Dynamic> : SpaceBase<-1> { 69 | using SpaceBase::SpaceBase; 70 | }; 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /src/nigh/impl/identity.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_IDENTITY_HPP 38 | #define NIGH_IMPL_IDENTITY_HPP 39 | 40 | namespace unc::robotics::nigh::impl { 41 | struct Identity { 42 | template 43 | constexpr auto operator()(T&& t) const noexcept -> decltype(std::forward(t)) { 44 | return std::forward(t); 45 | } 46 | }; 47 | } 48 | 49 | #endif 50 | 51 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/branch.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_BRANCH_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_BRANCH_HPP 39 | 40 | #include "node.hpp" 41 | #include "leaf.hpp" 42 | 43 | namespace unc::robotics::nigh::impl::kdtree_batch { 44 | 45 | template 46 | class Branch : public Node { 47 | using Base = Node; 48 | using Leaf = kdtree_batch::Leaf; 49 | 50 | public: 51 | Branch(Leaf* leaf, int axis) : Base(leaf->region(), axis) { 52 | // non-concurrent trees can delete the leaf once the 53 | // branch is created. 54 | delete leaf; 55 | } 56 | }; 57 | 58 | template 59 | class Branch : public Node { 60 | using Base = Node; 61 | using Leaf = kdtree_batch::Leaf; 62 | 63 | Leaf *leaf_; 64 | 65 | public: 66 | Branch(Leaf* leaf, int axis) : Base(leaf->region(), axis), leaf_(leaf) { 67 | // another thread may be concurrently traversing the leaf 68 | // (unlike the non-concurrent version), thus we cannot 69 | // safely delete the leaf now. Instead we keep a 70 | // reference to it for later cleanup. 71 | } 72 | 73 | ~Branch() { 74 | delete leaf_; 75 | } 76 | }; 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/clear.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_CLEAR_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_CLEAR_HPP 39 | 40 | #include "types.hpp" 41 | #include "traversal.hpp" 42 | 43 | namespace unc::robotics::nigh::impl::kdtree_batch { 44 | 45 | template 46 | class Clear { 47 | using Key = key_t; 48 | using Node = node_t; 49 | using Leaf = leaf_t; 50 | 51 | Tree& tree_; 52 | Traversal traversal_; 53 | 54 | public: 55 | Clear(Tree& tree) 56 | : tree_(tree) 57 | , traversal_(tree.metricSpace()) 58 | { 59 | } 60 | 61 | template 62 | void dealloc(T* ptr) { 63 | tree_.dealloc(ptr); 64 | } 65 | 66 | void operator() (Node *node) { 67 | if (!node->isLeaf()) { 68 | traversal_.clear(*this, tree_.metricSpace(), node, node->axis()); 69 | } else { 70 | Leaf *leaf = static_cast(node); 71 | dealloc(leaf); 72 | } 73 | } 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/lp_branch.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_LP_BRANCH_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_LP_BRANCH_HPP 39 | 40 | #include "branch.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_batch { 43 | template 44 | class LPBranch : public Branch { 45 | using Base = Branch; 46 | using Node = kdtree_batch::Node; 47 | using Leaf = kdtree_batch::Leaf; 48 | using Distance = typename Space::Distance; 49 | 50 | static constexpr bool concurrentWrites = std::is_same_v; 51 | 52 | Distance split_; 53 | std::array, 2> children_; 54 | 55 | public: 56 | LPBranch(Leaf *leaf, int axis, Distance split, Leaf *c0, Leaf *c1) 57 | : Base(leaf, axis) 58 | , split_(split) 59 | , children_{{c0, c1}} 60 | { 61 | } 62 | 63 | Distance split() const { return split_; } 64 | auto& child(int i) { return children_[i]; } 65 | const auto& child(int i) const { return children_[i]; } 66 | }; 67 | } 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/nearest_traversal.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_NEAREST_TRAVERSAL_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_NEAREST_TRAVERSAL_HPP 39 | 40 | #include "traversal.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_batch { 43 | template 44 | class NearestTraversal : public Traversal { 45 | using Base = Traversal; 46 | public: 47 | using Base::Base; 48 | }; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/nearest_traversals.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifdef NIGH_KDTREE_BATCH_HPP 37 | #ifdef NIGH_METRIC_CARTESIAN_HPP 38 | #include "nearest_traversal_cartesian.hpp" 39 | #endif 40 | 41 | #ifdef NIGH_METRIC_SCALED_HPP 42 | #include "nearest_traversal_scaled.hpp" 43 | #endif 44 | 45 | #ifdef NIGH_METRIC_SO2_HPP 46 | #include "nearest_traversal_so2.hpp" 47 | #endif 48 | 49 | #ifdef NIGH_METRIC_SO3_HPP 50 | #include "nearest_traversal_so3.hpp" 51 | #endif 52 | #endif 53 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/node.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_NODE_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_NODE_HPP 39 | 40 | #include "../region.hpp" 41 | #include "../regions.hpp" 42 | 43 | namespace unc::robotics::nigh::impl::kdtree_batch { 44 | template 45 | struct CartesianHelper; 46 | 47 | template 48 | class Node : Region { 49 | protected: 50 | using Key = typename Space::Type; 51 | using Metric = typename Space::Metric; 52 | using NodeRegion = Region; 53 | 54 | int axis_; 55 | 56 | // the cartesian helper needs to modify axis_; 57 | template 58 | friend struct CartesianHelper; 59 | 60 | public: 61 | static constexpr int kLeafAxis = -1; 62 | 63 | // Leaf 64 | template 65 | Node(const Space& space, const Traversal& traversal, const Key& q) 66 | : NodeRegion(space, traversal, q) 67 | , axis_(kLeafAxis) 68 | { 69 | } 70 | 71 | // Branch 72 | Node(const NodeRegion& region, int axis) 73 | : NodeRegion(region) 74 | , axis_(axis) 75 | { 76 | assert(axis >= 0); 77 | } 78 | 79 | constexpr int axis() const { return axis_; } 80 | constexpr bool isLeaf() const { return axis_ == kLeafAxis; } 81 | constexpr NodeRegion& region() { return *this; } 82 | constexpr const NodeRegion& region() const { return *this; } 83 | }; 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/so3_branch.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_SO3_BRANCH_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_SO3_BRANCH_HPP 39 | 40 | #include "branch.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_batch { 43 | template 44 | class SO3Branch : public Branch { 45 | using Base = Branch; 46 | using Node = kdtree_batch::Node; 47 | using Leaf = kdtree_batch::Leaf; 48 | using Distance = typename Space::Distance; 49 | using Split = Eigen::Matrix; 50 | 51 | static constexpr bool concurrentWrites = std::is_same_v; 52 | 53 | Split split_; 54 | std::array, 2> children_; 55 | 56 | public: 57 | SO3Branch(Leaf *leaf, int axis, const Split& split, Leaf *c0, Leaf *c1) 58 | : Base(leaf, axis) 59 | , split_(split) 60 | , children_{{c0, c1}} 61 | { 62 | } 63 | 64 | const Split& split() const { return split_; } 65 | auto& child(int i) { return children_[i]; } 66 | const auto& child(int i) const { return children_[i]; } 67 | }; 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/so3_root.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_SO3_ROOT_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_SO3_ROOT_HPP 39 | 40 | #include "branch.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_batch { 43 | template 44 | class SO3Root : public Branch { 45 | using Base = Branch; 46 | using Node = kdtree_batch::Node; 47 | using Leaf = kdtree_batch::Leaf; 48 | 49 | static constexpr bool concurrentWrites = std::is_same_v; 50 | 51 | std::array, 4> children_; 52 | 53 | public: 54 | SO3Root(Leaf *leaf, int axis, Leaf **c) 55 | : Base(leaf, axis) 56 | , children_{{c[0], c[1], c[2], c[3]}} 57 | { 58 | } 59 | 60 | auto& child(int i) { return children_[i]; } 61 | const auto& child(int i) const { return children_[i]; } 62 | }; 63 | } 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/strategy.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_STRATEGY_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_STRATEGY_HPP 39 | 40 | namespace unc::robotics::nigh { 41 | template 42 | struct KDTreeBatch {}; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/traversal.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_BATCH_TRAVERSAL_HPP 38 | #define NIGH_IMPL_KDTREE_BATCH_TRAVERSAL_HPP 39 | 40 | #include "../root_get.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_batch { 43 | template 44 | class Traversal; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_batch/traversals.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifdef NIGH_KDTREE_BATCH_HPP 37 | #ifdef NIGH_METRIC_CARTESIAN_HPP 38 | #include "traversal_cartesian.hpp" 39 | #endif 40 | 41 | #ifdef NIGH_METRIC_LP_HPP 42 | #include "traversal_lp.hpp" 43 | #endif 44 | 45 | #ifdef NIGH_METRIC_SCALED_HPP 46 | #include "traversal_scaled.hpp" 47 | #endif 48 | 49 | #ifdef NIGH_METRIC_SO2_HPP 50 | #include "traversal_so2.hpp" 51 | #endif 52 | 53 | #ifdef NIGH_METRIC_SO3_HPP 54 | #include "traversal_so3.hpp" 55 | #endif 56 | #endif 57 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/accum.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_ACCUM_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_ACCUM_HPP 39 | 40 | #include "../root_get.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_median { 43 | template 44 | class Accum; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/accum_scaled.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_ACCUM_SCALED_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_ACCUM_SCALED_HPP 39 | 40 | #include "accum.hpp" 41 | #include "node.hpp" 42 | 43 | namespace unc::robotics::nigh::impl::kdtree_median { 44 | 45 | template 46 | class Accum, Get> 47 | : Accum 48 | { 49 | using Base = Accum; 50 | using Metric = metric::Scaled; 51 | using Space = metric::Space; 52 | using Distance = typename Space::Distance; 53 | public: 54 | 55 | template 56 | Distance selectAxis( 57 | Builder& builder, const Space& space, unsigned *axis, 58 | Iter first, Iter last) 59 | { 60 | return Base::selectAxis(builder, space.space(), axis, first, last) * space.weight(); 61 | } 62 | 63 | template 64 | Node *partition( 65 | Builder& builder, const Space& space, unsigned axis, Iter first, Iter last) 66 | { 67 | return Base::partition(builder, space.space(), axis, first, last); 68 | } 69 | }; 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/accums.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifdef NIGH_KDTREE_MEDIAN_HPP 37 | #ifdef NIGH_METRIC_LP_HPP 38 | #include "accum_lp.hpp" 39 | #endif 40 | 41 | #ifdef NIGH_METRIC_SO2_HPP 42 | #include "accum_so2.hpp" 43 | #endif 44 | 45 | #ifdef NIGH_METRIC_SO3_HPP 46 | #include "accum_so3.hpp" 47 | #endif 48 | 49 | #ifdef NIGH_METRIC_SCALED_HPP 50 | #include "accum_scaled.hpp" 51 | #endif 52 | 53 | #ifdef NIGH_METRIC_CARTESIAN_HPP 54 | #include "accum_cartesian.hpp" 55 | #endif 56 | #endif 57 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/builder.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_BUILDER_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_BUILDER_HPP 39 | 40 | #include "node.hpp" 41 | #include "accum.hpp" 42 | #include "accums.hpp" 43 | 44 | namespace unc::robotics::nigh::impl::kdtree_median { 45 | template 46 | class Builder { 47 | using Key = typename Tree::Key; 48 | using Metric = typename Tree::Metric; 49 | 50 | Tree& tree_; 51 | 52 | Accum accum_; 53 | 54 | public: 55 | Builder(Tree& tree) 56 | : tree_(tree) 57 | { 58 | } 59 | 60 | template 61 | T* allocate(Args&& ... args) { 62 | return tree_.blocks_.template allocate(std::forward(args)...); 63 | } 64 | 65 | decltype(auto) getKey(const typename Tree::Type& t) { 66 | return tree_.getKey(t); 67 | } 68 | 69 | template 70 | Node* operator() (Iter first, Iter last) { 71 | if (std::distance(first, last) <= 1) 72 | return nullptr; 73 | 74 | // TODO: we would get better cache locality if we 75 | // accumulated the region bounds in a loop here, instead 76 | // of having each space iterate individually. The problem 77 | // is that SO(2) needs to sort the elements locally to 78 | // find a good partition. 79 | 80 | // Iter it = first; 81 | // accum_.init(tree_.metricSpace(), tree_.getKey(*it)); 82 | // while (++it != last) 83 | // accum_.grow(tree_.metricSpace(), tree_.getKey(*it)); 84 | 85 | unsigned axis; 86 | accum_.selectAxis(*this, tree_.metricSpace(), &axis, first, last); 87 | return accum_.partition(*this, tree_.metricSpace(), axis, first, last); 88 | } 89 | }; 90 | } 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/lp_branch.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_LP_BRANCH_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_LP_BRANCH_HPP 39 | 40 | #include "node.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_median { 43 | template 44 | class LPBranch : public Node { 45 | Distance split_; 46 | std::array children_; 47 | 48 | public: 49 | LPBranch(unsigned axis, Distance split, Node* c0, Node * c1) 50 | : Node(axis) 51 | , split_(split) 52 | , children_{{c0, c1}} 53 | { 54 | } 55 | 56 | Distance split() const { 57 | return split_; 58 | } 59 | 60 | auto& child(int i) { 61 | return children_[i]; 62 | } 63 | 64 | const auto& child(int i) const { 65 | return children_[i]; 66 | } 67 | }; 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/node.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_NODE_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_NODE_HPP 39 | 40 | namespace unc::robotics::nigh::impl::kdtree_median { 41 | class Node { 42 | unsigned axis_; 43 | 44 | public: 45 | inline Node(unsigned axis) 46 | : axis_(axis) 47 | { 48 | } 49 | 50 | inline unsigned axis() const { 51 | return axis_; 52 | } 53 | 54 | inline void updateAxis(unsigned zero) { 55 | axis_ += zero; 56 | } 57 | }; 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/root.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_ROOT_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_ROOT_HPP 39 | 40 | #include "node.hpp" 41 | #include 42 | 43 | namespace unc::robotics::nigh::impl::kdtree_median { 44 | template 45 | class Root { 46 | using Marker = typename Blocks::Marker; 47 | 48 | Marker mark_; 49 | Node *node_; 50 | 51 | public: 52 | // Roots are stored in a vector. We use std::vector's 53 | // resize() to shrink the list of roots, but we never increase 54 | // the size of the vector using resize(). resize() requires a 55 | // default constructor to increase the size of the roots, but 56 | // since we never increase the size, this constructor is 57 | // required, but should never be called. 58 | Root() { 59 | assert(false); 60 | } 61 | 62 | Root(Marker mark, Node *node) 63 | : mark_(mark) 64 | , node_(node) 65 | { 66 | } 67 | 68 | const Marker& mark() const { 69 | return mark_; 70 | } 71 | }; 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/so3_branch.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_SO3_BRANCH_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_SO3_BRANCH_HPP 39 | 40 | #include "node.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_median { 43 | template 44 | class SO3Branch : public Node { 45 | Eigen::Matrix split_; 46 | std::array children_; 47 | 48 | public: 49 | SO3Branch( 50 | unsigned axis, 51 | const Eigen::Matrix& split, 52 | Node *c0, 53 | Node *c1) 54 | : Node(axis) 55 | , split_(split) 56 | , children_{{c0, c1}} 57 | { 58 | } 59 | 60 | const Node* child(int i) const { 61 | return children_[i]; 62 | } 63 | 64 | const auto& split() const { 65 | return split_; 66 | } 67 | }; 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/so3_root.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_SO3_ROOT_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_SO3_ROOT_HPP 39 | 40 | #include "node.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_median { 43 | class SO3Root : public Node { 44 | std::array children_{{}}; 45 | std::array offsets_; 46 | 47 | public: 48 | inline SO3Root(unsigned axis) 49 | : Node(axis) 50 | { 51 | } 52 | 53 | inline auto& offset(int i) { 54 | return offsets_[i]; 55 | } 56 | 57 | inline std::size_t offset(int i) const { 58 | return offsets_[i]; 59 | } 60 | 61 | inline auto& child(int i) { 62 | return children_[i]; 63 | } 64 | 65 | inline const auto& child(int i) const { 66 | return children_[i]; 67 | } 68 | }; 69 | } 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/strategy.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_STRATEGY_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_STRATEGY_HPP 39 | 40 | namespace unc::robotics::nigh { 41 | template 42 | struct KDTreeMedian {}; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/traversal.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_HPP 39 | 40 | #include "../root_get.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_median { 43 | template 44 | class Traversal; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/traversal_scaled.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_SCALED_HPP 38 | #define NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_SCALED_HPP 39 | 40 | #include "traversal.hpp" 41 | 42 | namespace unc::robotics::nigh::impl::kdtree_median { 43 | template 44 | class Traversal, Get> 45 | : Traversal 46 | { 47 | using Base = Traversal; 48 | using Metric = metric::Scaled; 49 | using Space = metric::Space; 50 | using Distance = typename Space::Distance; 51 | 52 | Distance weight_; 53 | 54 | public: 55 | Traversal(const Space& space) 56 | : Base(space.space()) 57 | , weight_(space.weight()) 58 | { 59 | } 60 | 61 | Distance distToRegion() const { 62 | return Base::distToRegion() * weight_; 63 | } 64 | 65 | template 66 | void follow( 67 | Nearest& nearest, 68 | const Space& space, 69 | const Node *node, unsigned axis, 70 | const Key& key, 71 | Iter first, Iter last) 72 | { 73 | Base::follow(nearest, space.space(), node, axis, key, first, last); 74 | } 75 | }; 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /src/nigh/impl/kdtree_median/traversals.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifdef NIGH_KDTREE_MEDIAN_HPP 37 | #ifdef NIGH_METRIC_CARTESIAN_HPP 38 | #include "traversal_cartesian.hpp" 39 | #endif 40 | 41 | #ifdef NIGH_METRIC_LP_HPP 42 | #include "traversal_lp.hpp" 43 | #endif 44 | 45 | #ifdef NIGH_METRIC_SCALED_HPP 46 | #include "traversal_scaled.hpp" 47 | #endif 48 | 49 | #ifdef NIGH_METRIC_SO2_HPP 50 | #include "traversal_so2.hpp" 51 | #endif 52 | 53 | #ifdef NIGH_METRIC_SO3_HPP 54 | #include "traversal_so3.hpp" 55 | #endif 56 | #endif 57 | -------------------------------------------------------------------------------- /src/nigh/impl/metric_specializations.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include "kdtree_batch/nearest_traversals.hpp" 37 | #include "kdtree_batch/traversals.hpp" 38 | #include "kdtree_median/accums.hpp" 39 | #include "kdtree_median/traversals.hpp" 40 | #include "regions.hpp" 41 | -------------------------------------------------------------------------------- /src/nigh/impl/region.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_REGION_HPP 38 | #define NIGH_IMPL_REGION_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::nigh::impl { 43 | 44 | template 45 | class Region; 46 | // public: 47 | // Region() {} 48 | // template 49 | // Region(const State& q) {} 50 | 51 | // template 52 | // void grow(const State& q) {} 53 | // }; 54 | 55 | 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /src/nigh/impl/region_scaled.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_REGION_SCALED_HPP 38 | #define NIGH_IMPL_REGION_SCALED_HPP 39 | 40 | #include "region.hpp" 41 | 42 | namespace unc::robotics::nigh::impl { 43 | 44 | template 45 | class Region, Concurrency> 46 | : public Region 47 | { 48 | using Space = metric::Space>; 49 | using Base = Region; 50 | public: 51 | template 52 | Region(const Space& space, const Traversal& traversal, const Key& q) 53 | : Base(space.space(), traversal.traversal(), q) 54 | { 55 | } 56 | }; 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /src/nigh/impl/region_so2.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_REGION_SO2_HPP 38 | #define NIGH_IMPL_REGION_SO2_HPP 39 | 40 | #include "region.hpp" 41 | 42 | namespace unc::robotics::nigh::impl { 43 | 44 | template 45 | class Region, Concurrency> { 46 | using Space = metric::Space>; 47 | public: 48 | template 49 | Region(const Space&, const Traversal&, const Key& q) {} 50 | }; 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/nigh/impl/region_so3.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_REGION_SO3_HPP 38 | #define NIGH_IMPL_REGION_SO3_HPP 39 | 40 | #include "region.hpp" 41 | #include "so3.hpp" 42 | #include "constants.hpp" 43 | #include 44 | 45 | namespace unc::robotics::nigh::impl { 46 | 47 | // TODO: Concurrent version 48 | template 49 | class Region { 50 | using Space = metric::Space; 51 | public: 52 | template 53 | Region(const Space&, const Traversal&, const Key& q) { 54 | } 55 | }; 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /src/nigh/impl/regions.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifdef NIGH_IMPL_REGION_HPP 37 | #ifdef NIGH_METRIC_LP_HPP 38 | #include "region_lp.hpp" 39 | #endif 40 | 41 | #ifdef NIGH_METRIC_SO3_HPP 42 | #include "region_so3.hpp" 43 | #endif 44 | 45 | #ifdef NIGH_METRIC_SO2_HPP 46 | #include "region_so2.hpp" 47 | #endif 48 | 49 | #ifdef NIGH_METRIC_SCALED_HPP 50 | #include "region_scaled.hpp" 51 | #endif 52 | 53 | #ifdef NIGH_METRIC_CARTESIAN_HPP 54 | #include "region_cartesian.hpp" 55 | #endif 56 | #endif 57 | -------------------------------------------------------------------------------- /src/nigh/impl/root_get.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_ROOT_GET_HPP 38 | #define NIGH_IMPL_ROOT_GET_HPP 39 | 40 | namespace unc::robotics::nigh::impl { 41 | struct RootGet { 42 | template 43 | static constexpr T& part(T& x) { return x; } 44 | template 45 | static constexpr const T& part(const T& x) { return x; } 46 | }; 47 | } 48 | 49 | #endif 50 | 51 | -------------------------------------------------------------------------------- /src/nigh/impl/so3_region.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_IMPL_SO3_REGION_HPP 38 | #define NIGH_IMPL_SO3_REGION_HPP 39 | 40 | #include "region.hpp" 41 | #include "so3.hpp" 42 | #include "constants.hpp" 43 | #include 44 | 45 | namespace unc::robotics::nigh::impl { 46 | 47 | template 48 | class SO3Region { 49 | using Distance = typename metric::Space::Distance; 50 | 51 | Eigen::Matrix min_; 52 | Eigen::Matrix max_; 53 | 54 | public: 55 | SO3Region() = delete; 56 | SO3Region(const SO3Region& region) 57 | : min_(region.min_) 58 | , max_(region.max_) 59 | { 60 | } 61 | 62 | SO3Region(const Key& q, int vol) { 63 | for (int a=0 ; a<3 ; ++a) 64 | min_.col(a) = so3::project(q, vol, a); 65 | max_ = min_; 66 | } 67 | 68 | void grow(const Key& q, int vol) { 69 | for (int a=0 ; a<3 ; ++a) { 70 | Eigen::Matrix c = so3::project(q, vol, a); 71 | assert(-SQRT1_2 <= c[0] && c[0] <= SQRT1_2); 72 | assert(c[1] >= SQRT1_2); 73 | if (c[0] < min_(0, a)) 74 | min_.col(a) = c; 75 | if (c[0] > max_(0, a)) 76 | max_.col(a) = c; 77 | } 78 | } 79 | 80 | Distance selectAxis(unsigned *axis, int vol) const { 81 | assert(vol != -1); 82 | Eigen::Matrix dots = min_.cwiseProduct(max_).colwise().sum(); 83 | assert((dots.array() >= Distance(0)).all()); 84 | return std::acos(dots.minCoeff(axis)); 85 | } 86 | }; 87 | } 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /src/nigh/impl/space_base.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_IMPL_SPACE_BASE_HPP 38 | #define NIGH_METRIC_IMPL_SPACE_BASE_HPP 39 | 40 | #include 41 | #include 42 | 43 | namespace unc::robotics::nigh::impl { 44 | // This is equivalent to Eigen::Dynamic. It is used when the 45 | // state storage type is not fixed at compile time. 46 | constexpr int Dynamic = -1; 47 | 48 | template 49 | struct SpaceBase { 50 | static_assert(dim > 0, "dimensions must be positive or dynamic"); 51 | 52 | static constexpr bool dynamic = false; 53 | static constexpr int kDimensions = dim; 54 | 55 | constexpr unsigned dimensions() const { 56 | return dim; 57 | } 58 | }; 59 | 60 | template <> 61 | struct SpaceBase { 62 | static constexpr bool dynamic = true; 63 | static constexpr int kDimensions = Dynamic; 64 | private: 65 | unsigned dimensions_; 66 | public: 67 | explicit SpaceBase(unsigned dimensions) 68 | : dimensions_(dimensions) 69 | { 70 | // dimensions must be positive 71 | assert(dimensions > 0); 72 | 73 | // something likely went wrong if dimensions is this big... 74 | assert(dimensions < static_cast(std::numeric_limits::max())); 75 | } 76 | 77 | constexpr unsigned dimensions() const { 78 | return dimensions_; 79 | } 80 | }; 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /src/nigh/lp_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_LP_SPACE_HPP 38 | #define NIGH_METRIC_LP_SPACE_HPP 39 | 40 | #include "metric/lp.hpp" 41 | #include "metric/space.hpp" 42 | #include "metric/space_lp_eigen.hpp" 43 | 44 | namespace unc::robotics::nigh::metric { 45 | 46 | template 47 | using LPSpace = Space, LP

>; 48 | 49 | template 50 | using L2Space = LPSpace; 51 | 52 | template 53 | using L1Space = LPSpace; 54 | 55 | template 56 | using LInfSpace = LPSpace; 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /src/nigh/metric/cartesian_state_element.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_CARTESIAN_STATE_ELEMENT_HPP 38 | #define NIGH_METRIC_CARTESIAN_STATE_ELEMENT_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::nigh::metric { 43 | // Cartesian state handling. Out of the box, Nigh supports any types 'T' for which 44 | // 45 | // std::tuple_element 46 | // std::get(T&) 47 | // 48 | // are defined (given a valid `I`). Thus good candidates for a Cartesian state are: 49 | // 50 | // std::tuple<...> 51 | // std::pair 52 | // std::array. 53 | // 54 | // In order to support a different or custom type containing for a 55 | // cartesian state, there are two options. Option 1: specialize 56 | // std::tuple_element and overload std::get(), or Option 2: 57 | // specialize cartesian_state_element. 58 | 59 | template 60 | struct cartesian_state_element { 61 | using type = std::tuple_element_t; 62 | 63 | // std::get provides 4 versions (&, const&, &&, const&&), we 64 | // only need const&. 65 | static constexpr const type& get(const T& q) { 66 | return std::get(q); 67 | } 68 | 69 | // other libraries (e.g. mpt) may need this: 70 | static constexpr type& get(T& q) { 71 | return std::get(q); 72 | } 73 | }; 74 | 75 | template 76 | using cartesian_state_element_t = typename cartesian_state_element::type; 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/nigh/metric/lp.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_LP_HPP 38 | #define NIGH_METRIC_LP_HPP 39 | 40 | #include "metric.hpp" 41 | 42 | namespace unc::robotics::nigh::metric { 43 | template 44 | struct LP { 45 | static_assert(p > 0 || p == -1, "p must be positive or -1 for L^Inf"); 46 | }; 47 | 48 | using L1 = LP<1>; 49 | using L2 = LP<2>; 50 | using LInf = LP<-1>; 51 | 52 | template 53 | struct is_metric> : std::true_type {}; 54 | } 55 | 56 | #include "../impl/metric_specializations.hpp" 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /src/nigh/metric/metric.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_IMPL_IS_METRIC_HPP 38 | #define NIGH_METRIC_IMPL_IS_METRIC_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::nigh::metric { 43 | template 44 | struct is_metric : std::false_type {}; 45 | 46 | template 47 | constexpr bool is_metric_v = is_metric::value; 48 | } 49 | 50 | namespace unc::robotics::nigh { 51 | using namespace metric; 52 | } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /src/nigh/metric/scaled.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SCALED_HPP 38 | #define NIGH_METRIC_SCALED_HPP 39 | 40 | #include "metric.hpp" 41 | #include 42 | 43 | namespace unc::robotics::nigh::metric { 44 | 45 | template 46 | struct Scaled; 47 | 48 | template 49 | struct Scaled { 50 | static_assert(is_metric_v, "first template parameter to Scaled must be a metric"); 51 | 52 | using Metric = M; 53 | 54 | constexpr const Metric& metric() const { 55 | return *this; 56 | } 57 | }; 58 | 59 | template 60 | struct Scaled> : private M { 61 | using Metric = M; 62 | using Weight = std::ratio; 63 | 64 | static_assert(is_metric_v, "first template parameter to Scaled must be a metric"); 65 | static_assert(Weight::num > 0, "ratio must be positive"); 66 | 67 | constexpr const Metric& metric() const { return *this; } 68 | }; 69 | 70 | template 71 | using RatioScaled = Scaled>; 72 | 73 | template 74 | struct is_metric> : is_metric {}; 75 | } 76 | 77 | #include "../impl/metric_specializations.hpp" 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/nigh/metric/so2.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SO2_HPP 38 | #define NIGH_METRIC_SO2_HPP 39 | 40 | #include "metric.hpp" 41 | 42 | namespace unc::robotics::nigh::metric { 43 | template 44 | struct SO2 { 45 | static_assert(p > 0 || p == -1, "p must be positive or -1 for L^Inf"); 46 | }; 47 | 48 | template 49 | struct is_metric> : std::true_type {}; 50 | } 51 | 52 | #include "../impl/metric_specializations.hpp" 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /src/nigh/metric/so3.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SO3_HPP 38 | #define NIGH_METRIC_SO3_HPP 39 | 40 | #include "metric.hpp" 41 | 42 | namespace unc::robotics::nigh::metric { 43 | struct SO3 {}; 44 | 45 | template <> 46 | struct is_metric : std::true_type {}; 47 | } 48 | 49 | #include "../impl/metric_specializations.hpp" 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/nigh/metric/space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SPACE_HPP 38 | #define NIGH_METRIC_SPACE_HPP 39 | 40 | #include "metric.hpp" 41 | #include 42 | 43 | namespace unc::robotics::nigh::metric { 44 | // Space is a combination of storage type and metric. It is 45 | // essentially a metric space where Metric is the metric, and T is 46 | // the type of element composing the set. Thus, for example, R^3 47 | // with an L2 metric can be represented as Space. The space must be specialized for each combindation of 49 | // 'T' and 'Metric', and the specializion must provide a small 50 | // number of member methods to make the type work. 51 | template 52 | struct Space; 53 | 54 | template 55 | struct is_space : std::false_type {}; 56 | 57 | template 58 | struct is_space> : is_metric {}; 59 | 60 | template 61 | constexpr bool is_space_v = is_space::value; 62 | } 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /src/nigh/metric/space_cartesian.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_SPACE_CARTESIAN_HPP 38 | #define NIGH_SPACE_CARTESIAN_HPP 39 | 40 | #include "cartesian.hpp" 41 | #include "space.hpp" 42 | #include "../impl/cartesian_space.hpp" 43 | 44 | namespace unc::robotics::nigh::metric { 45 | template 46 | struct Space> 47 | : impl::CartesianSpace, std::index_sequence_for> 48 | { 49 | using impl::CartesianSpace, std::index_sequence_for>::CartesianSpace; 50 | }; 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/nigh/metric/space_lp_array.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_SPACE_ARRAY_LP_HPP 38 | #define NIGH_SPACE_ARRAY_LP_HPP 39 | 40 | #include "lp.hpp" 41 | #include "space.hpp" 42 | #include "../impl/space_base.hpp" 43 | #include "../impl/lp_sum.hpp" 44 | #include 45 | #include 46 | 47 | namespace unc::robotics::nigh::metric { 48 | 49 | template 50 | struct Space, metric::LP

> 51 | : impl::SpaceBase 52 | { 53 | using Type = std::array; 54 | using Distance = S; 55 | using Metric = metric::LP

; 56 | 57 | static bool isValid(const Type& a) { 58 | static constexpr bool (*isfinite)(S) = &std::isfinite; 59 | return std::all_of(a.begin(), a.end(), isfinite); 60 | } 61 | 62 | static Distance coeff(const Type& a, std::size_t i) { 63 | assert(i < n); 64 | return a[i]; 65 | } 66 | 67 | static Distance& coeff(Type& a, std::size_t i) { 68 | assert(i < n); 69 | return a[i]; 70 | } 71 | 72 | Distance distance(const Type& a, const Type& b) const { 73 | impl::LPSum sum(a[0] - b[0]); 74 | for (std::size_t i=1 ; i 45 | struct Space, std::enable_if_t>> 46 | : impl::SpaceBase<1> 47 | { 48 | using Type = S; 49 | using Distance = S; 50 | using Metric = metric::LP

; 51 | 52 | static bool isValid(const Type& a) { 53 | return std::isfinite(a); 54 | } 55 | 56 | static Distance coeff(const Type& a, std::size_t i) { 57 | assert(i == 0); 58 | return a; 59 | } 60 | 61 | static Distance& coeff(Type& a, std::size_t i) { 62 | assert(i == 0); 63 | return a; 64 | } 65 | 66 | Distance distance(const Type& a, const Type& b) const { 67 | return std::abs(a - b); 68 | } 69 | }; 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /src/nigh/metric/space_lp_vector.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_SPACE_VECTOR_LP_HPP 38 | #define NIGH_SPACE_VECTOR_LP_HPP 39 | 40 | #include "lp.hpp" 41 | #include "space.hpp" 42 | #include "../impl/lp_sum.hpp" 43 | #include 44 | 45 | namespace unc::robotics::nigh::metric { 46 | 47 | template 48 | struct Space<::std::vector, LP

> 49 | : impl::SpaceBase 50 | { 51 | private: 52 | using Base = impl::SpaceBase; 53 | public: 54 | using Type = std::vector; 55 | using Distance = S; 56 | using Metric = LP

; 57 | 58 | using Base::Base; 59 | 60 | static bool isValid(const Type& a) { 61 | static constexpr bool (*isfinite)(S) = &std::isfinite; 62 | return std::all_of(a.begin(), a.end(), isfinite); 63 | } 64 | 65 | static Distance coeff(const Type& a, std::size_t index) { 66 | assert(index < a.size()); 67 | return a[index]; 68 | } 69 | 70 | static Distance& coeff(Type& a, std::size_t index) { 71 | assert(index < a.size()); 72 | return a[index]; 73 | } 74 | 75 | Distance distance(const Type& a, const Type& b) const { 76 | unsigned n = Base::dimensions(); 77 | assert(a.size() == n && b.size() == n); 78 | impl::LPSum sum(a[0] - b[0]); 79 | for (unsigned i=1 ; i 46 | struct Space> 47 | : private impl::ScaledSpaceBase 48 | { 49 | private: 50 | using Base = impl::ScaledSpaceBase; 51 | 52 | public: 53 | using Type = S; 54 | using Metric = Scaled; 55 | using Distance = typename Base::Distance; 56 | 57 | using Base::Base; 58 | using Base::dimensions; 59 | using Base::isValid; 60 | using Base::weight; 61 | 62 | const Space space() const { 63 | return *this; 64 | } 65 | 66 | Distance distance(const Type& a, const Type& b) const { 67 | return Base::distance(a, b) * Base::weight(); 68 | } 69 | }; 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /src/nigh/metric/space_so2_eigen.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SPACE_SO2_EIGEN_HPP 38 | #define NIGH_METRIC_SPACE_SO2_EIGEN_HPP 39 | 40 | #include "so2.hpp" 41 | #include "space.hpp" 42 | #include "../impl/so2.hpp" 43 | #include "../impl/eigen_space.hpp" 44 | #include 45 | 46 | namespace unc::robotics::nigh::metric { 47 | template 48 | struct Space, SO2

> : impl::EigenSpace 49 | { 50 | using Type = Eigen::Matrix; 51 | using Distance = S; 52 | using Metric = metric::SO2

; 53 | 54 | using impl::EigenSpace::EigenSpace; 55 | 56 | static bool isValid(const Type& a) { 57 | return a.allFinite(); 58 | } 59 | 60 | static Distance coeff(const Type& a, std::size_t index) { 61 | assert((int)index < a.size()); 62 | return a[index]; 63 | } 64 | 65 | static Distance& coeff(Type& a, std::size_t index) { 66 | assert((int)index < a.size()); 67 | return a[index]; 68 | } 69 | 70 | Distance distance(const Type& a, const Type& b) const { 71 | return nigh::impl::so2::angularDistance

(a, b); 72 | } 73 | }; 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /src/nigh/metric/space_so2_rotation.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SPACE_SO2_EIGEN_HPP 38 | #define NIGH_METRIC_SPACE_SO2_EIGEN_HPP 39 | 40 | #include "space.hpp" 41 | #include 42 | 43 | namespace unc::robotics::nigh::metric { 44 | template 45 | struct Space, SO2

> : impl::SpaceBase<1> 46 | { 47 | using Type = Eigen::Rotation2D; 48 | using Distance = S; 49 | using Metric = SO2

; 50 | 51 | static bool isValid(const Type& a) { 52 | return std::isfinite(a.angle()); 53 | } 54 | 55 | static Distance coeff(const Type& a, std::size_t i) { 56 | assert(i == 0); 57 | return a.angle(); 58 | } 59 | 60 | static Distance& coeff(Type& a, std::size_t i) { 61 | assert(i == 0); 62 | return a.angle(); 63 | } 64 | 65 | Distance distance(const Type& a, const Type& b) { 66 | return angularDistance(a.angle(), b.angle()); 67 | } 68 | }; 69 | } 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /src/nigh/metric/space_so2_scalar.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SPACE_SO2_SCALAR_HPP 38 | #define NIGH_METRIC_SPACE_SO2_SCALAR_HPP 39 | 40 | #include "so2.hpp" 41 | #include "../impl/so2.hpp" 42 | #include "space.hpp" 43 | #include "../impl/space_base.hpp" 44 | 45 | namespace unc::robotics::nigh::metric { 46 | template 47 | struct Space, std::enable_if_t>> 48 | : impl::SpaceBase<1> 49 | { 50 | using Type = S; 51 | using Distance = S; 52 | using Metric = SO2

; 53 | 54 | static bool isValid(const Type& a) { 55 | return std::isfinite(a); 56 | } 57 | 58 | static Distance coeff(const Type& a, std::size_t i) { 59 | assert(i == 0); 60 | return a; 61 | } 62 | 63 | static Distance& coeff(Type& a, std::size_t i) { 64 | assert(i == 0); 65 | return a; 66 | } 67 | 68 | Distance distance(const Type& a, const Type& b) const { 69 | return nigh::impl::so2::angularDistance(a, b); 70 | } 71 | }; 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/nigh/metric/space_so3_eigen.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SPACE_SO3_EIGEN_HPP 38 | #define NIGH_METRIC_SPACE_SO3_EIGEN_HPP 39 | 40 | #include "space.hpp" 41 | #include 42 | 43 | namespace unc::robotics::nigh::metric { 44 | template 45 | struct Space, metric::SO3> : impl::SpaceBase<3> { 46 | static_assert( 47 | (r == 1 && (c == Eigen::Dynamic || c == 4)) || 48 | (c == 1 && (r == Eigen::Dynamic || r == 4)), 49 | "SO(3) metric requires a vector with 4 coefficients"); 50 | 51 | using Type = Eigen::Matrix; 52 | using Distance = S; 53 | using Metric = metric::SO3; 54 | 55 | static bool isValid(const Type& a) { 56 | return std::abs(a.squaredNorm() - 1) <= 4*4 * std::numeric_limits::epsilon(); 57 | } 58 | 59 | static unsigned maxAbsCoeff(const Type& a) { 60 | unsigned axis; 61 | a.cwiseAbs().maxCoeff(&axis); 62 | return axis; 63 | } 64 | 65 | static Distance coeff(const Type& a, std::size_t index) { 66 | assert(index < 4); 67 | return a[index]; 68 | } 69 | 70 | static Distance& coeff(Type& a, std::size_t index) { 71 | assert(index < 4); 72 | return a[index]; 73 | } 74 | 75 | Distance distance(const Type& a, const Type& b) const { 76 | Distance d = std::abs(a.dot(b)); 77 | return d >= 1 ? 0 : std::acos(d); 78 | } 79 | }; 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /src/nigh/metric/space_so3_quaternion.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SPACE_SO3_QUATERNION_HPP 38 | #define NIGH_METRIC_SPACE_SO3_QUATERNION_HPP 39 | 40 | #include "so3.hpp" 41 | #include "space.hpp" 42 | #include "../impl/space_base.hpp" 43 | #include 44 | 45 | namespace unc::robotics::nigh::metric { 46 | template 47 | struct Space, SO3> : impl::SpaceBase<3> { 48 | using Type = Eigen::Quaternion; 49 | using Distance = S; 50 | using Metric = SO3; 51 | 52 | static bool isValid(const Type& a) { 53 | return std::abs(a.coeffs().squaredNorm() - 1) <= 4*4 * std::numeric_limits::epsilon(); 54 | } 55 | 56 | static Distance& coeff(Type& a, std::size_t index) { 57 | assert(index < 4); 58 | return a.coeffs()[index]; 59 | } 60 | 61 | static Distance coeff(const Type& a, std::size_t index) { 62 | assert(index < 4); 63 | return a.coeffs()[index]; 64 | } 65 | 66 | static unsigned maxAbsCoeff(const Type& a) { 67 | unsigned axis; 68 | a.coeffs().cwiseAbs().maxCoeff(&axis); 69 | return axis; 70 | } 71 | 72 | Distance distance(const Type& a, const Type& b) const { 73 | Distance d = std::abs(a.coeffs().dot(b.coeffs())); 74 | return d >= 1 ? 0 : std::acos(d); 75 | } 76 | }; 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/nigh/nigh_forward.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_NIGH_FORWARD_HPP 38 | #define NIGH_NIGH_FORWARD_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::nigh { 43 | 44 | struct Concurrent {}; 45 | struct ConcurrentRead {}; 46 | struct NoThreadSafety {}; 47 | 48 | template < 49 | typename T, 50 | typename Space, 51 | typename KeyFn, 52 | typename Concurrency, 53 | typename Strategy, 54 | typename Allocator = std::allocator> 55 | class Nigh; 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /src/nigh/scaled_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SCALED_SPACE_HPP 38 | #define NIGH_METRIC_SCALED_SPACE_HPP 39 | 40 | #include "metric/space_scaled.hpp" 41 | #include "metric/scaled.hpp" 42 | 43 | namespace unc::robotics::nigh::impl { 44 | template 45 | struct scaled_space_selector; 46 | 47 | template 48 | struct scaled_space_selector, Weight> { 49 | using type = Space>; 50 | }; 51 | } 52 | 53 | namespace unc::robotics::nigh::metric { 54 | template 55 | using ScaledSpace = typename impl::scaled_space_selector::type; 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /src/nigh/se2_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SE2_SPACE_HPP 38 | #define NIGH_METRIC_SE2_SPACE_HPP 39 | 40 | #include "lp_space.hpp" 41 | #include "so2_space.hpp" 42 | #include "cartesian_space.hpp" 43 | 44 | namespace unc::robotics::nigh::metric { 45 | template 46 | using SE2Space = CartesianSpace, SO2Space>; 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/nigh/se3_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SE3_HPP 38 | #define NIGH_METRIC_SE3_HPP 39 | 40 | #include "lp_space.hpp" 41 | #include "so3_space.hpp" 42 | #include "scaled_space.hpp" 43 | #include "cartesian_space.hpp" 44 | 45 | namespace unc::robotics::nigh::impl { 46 | template 47 | struct se3_space_selector { 48 | static_assert(so3weight >= 1 && l2weight >= 1, "weights must be positive"); 49 | 50 | using type = metric::CartesianSpace< 51 | metric::ScaledSpace, std::ratio>, 52 | metric::ScaledSpace, std::ratio>>; 53 | }; 54 | 55 | template 56 | struct se3_space_selector { 57 | using type = metric::CartesianSpace< 58 | metric::ScaledSpace, std::ratio>, 59 | metric::L2Space>; 60 | }; 61 | 62 | template 63 | struct se3_space_selector { 64 | using type = metric::CartesianSpace< 65 | metric::SO3Space, 66 | metric::ScaledSpace, std::ratio>>; 67 | }; 68 | 69 | template 70 | struct se3_space_selector { 71 | using type = metric::CartesianSpace, L2Space>; 72 | }; 73 | } 74 | 75 | namespace unc::robotics::nigh::metric { 76 | template 77 | using SE3Space = typename impl::se3_space_selector::type; 78 | 79 | template 80 | using ScaledSE3Space = metric::CartesianSpace< 81 | ScaledSpace, Weight>, 82 | L2Space>; 83 | } 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /src/nigh/so2_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SO2_SPACE_HPP 38 | #define NIGH_METRIC_SO2_SPACE_HPP 39 | 40 | #include "metric/space_so2_scalar.hpp" 41 | #include "metric/space_so2_eigen.hpp" 42 | 43 | namespace unc::robotics::nigh::metric { 44 | template 45 | using SO2LPSpace = Space, SO2

>; 46 | 47 | template 48 | using SO2Space = Space>; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/nigh/so3_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_METRIC_SO3_SPACE_HPP 38 | #define NIGH_METRIC_SO3_SPACE_HPP 39 | 40 | #include "metric/space_so3_quaternion.hpp" 41 | 42 | namespace unc::robotics::nigh::metric { 43 | template 44 | using SO3Space = Space, SO3>; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /test/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | build.ninja 3 | generated 4 | -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | # Unit Test for MPT 2 | 3 | Running the unit tests requires [ninja](https://ninja-build.org/). Nigh requires [Eigen 3](http://eigen.tuxfamily.org). 4 | 5 | % ./configure.sh 6 | % ninja test 7 | 8 | Generating the benchmark plots requires gnuplot. 9 | 10 | % ninja plot 11 | 12 | Test classes are in `*_test.cpp` files. The `./configure.sh` script finds files that match this pattern and generates the build rules to run the tests. Whenever a new test is added the script will have to be run again. 13 | 14 | The `./configure.sh` script generates tests to exercise template variants. These tests will appear in the `generated` folder after the script is run. 15 | 16 | Some `./configure.sh` script behaviors can be overridden with environment variables. To set the C++ compiler, use the `CXX` environment variable. To set the flags the compile will use, set the `CFLAGS` environment variable. Here are a few examples: 17 | 18 | To generate a build file that will use clang++ and compile in debug symbols: 19 | 20 | % CXX=clang++ CFLAGS=-g ./configure.sh 21 | 22 | To generate a build file that will use g++ using the default compiler flags: 23 | 24 | % CXX=g++ ./configure.sh 25 | 26 | Some compilers require an additional library to support atomic operations on long double. For those, add `-latomic` to the list of libraries. This situtation will be made apparent by the compiler link messages similar to "`undefined reference to __atomic_store_16`" 27 | 28 | % CXX=g++ LIBS=-latomic ./configure.sh 29 | 30 | -------------------------------------------------------------------------------- /test/auto_strategy_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include 39 | #include "test.hpp" 40 | 41 | namespace nigh_test { 42 | class NonMetricSpace; 43 | } 44 | 45 | TEST(strategies) { 46 | using namespace unc::robotics::nigh; 47 | 48 | EXPECT(( 49 | std::is_same_v< 50 | auto_strategy_t, Concurrent, true>, 51 | KDTreeBatch<>>)) == true; 52 | 53 | EXPECT(( 54 | std::is_same_v< 55 | auto_strategy_t, NoThreadSafety, true>, 56 | KDTreeBatch<>>)) == true; 57 | 58 | EXPECT(( 59 | std::is_same_v< 60 | auto_strategy_t, ConcurrentRead, true>, 61 | KDTreeBatch<>>)) == true; 62 | 63 | EXPECT(( 64 | std::is_same_v< 65 | auto_strategy_t, Concurrent, false>, 66 | KDTreeBatch<>>)) == true; 67 | 68 | EXPECT(( 69 | std::is_same_v< 70 | auto_strategy_t, NoThreadSafety, false>, 71 | KDTreeMedian<>>)) == true; 72 | 73 | EXPECT(( 74 | std::is_same_v< 75 | auto_strategy_t, ConcurrentRead, false>, 76 | KDTreeMedian<>>)) == true; 77 | 78 | EXPECT(( 79 | std::is_same_v< 80 | auto_strategy_t, 81 | Linear>)) == true; 82 | 83 | EXPECT(( 84 | std::is_same_v< 85 | auto_strategy_t, 86 | GNAT<>>)) == true; 87 | 88 | EXPECT(( 89 | std::is_same_v< 90 | auto_strategy_t, 91 | GNAT<>>)) == true; 92 | } 93 | -------------------------------------------------------------------------------- /test/dup_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include "test.hpp" 37 | #include 38 | #include 39 | #include 40 | 41 | using namespace unc::robotics::nigh; 42 | 43 | template 44 | void dupTest(Args&& ... args) { 45 | using State = typename Space::Type; 46 | using Scalar = typename Space::Distance; 47 | 48 | struct Node { 49 | State state_; 50 | std::size_t index_; 51 | }; 52 | struct KeyFn { 53 | const State& operator() (const Node& n) const { 54 | return n.state_; 55 | } 56 | }; 57 | 58 | State q{std::forward(args)...}; 59 | Space space; 60 | 61 | Nigh nn(space); 62 | 63 | for (std::size_t i=0 ; i<100 ; ++i) 64 | nn.insert(Node{q, i}); 65 | 66 | std::vector> nbh; 67 | nn.nearest(nbh, q, 100, 1e-9); 68 | EXPECT(nbh.size()) == 100; 69 | EXPECT(nbh[0].first) == 0.0; 70 | EXPECT(nbh[99].first) == 0.0; 71 | } 72 | 73 | 74 | TEST(batch_l2_dups) { 75 | dupTest, KDTreeBatch<>>(1.0, 2.0, 3.0); 76 | } 77 | 78 | TEST(batch_so3_dups) { 79 | Eigen::Quaternion q; 80 | q.coeffs() << 1.0, 0.0, 0.0, 0.0; 81 | dupTest, KDTreeBatch<>>(q); 82 | } 83 | -------------------------------------------------------------------------------- /test/lp_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance_l2) { 40 | using namespace unc::robotics::nigh; 41 | using Space = metric::L2Space; 42 | 43 | EXPECT((std::is_same_v)) == true; 44 | 45 | Space space; 46 | EXPECT(space.distance(Eigen::Vector3d(1,2,3), Eigen::Vector3d(1,1,1))) 47 | == std::sqrt(0.0 + 1.0 + 2.0*2.0); 48 | } 49 | 50 | TEST(distance_l1) { 51 | using namespace unc::robotics::nigh; 52 | metric::L1Space space; 53 | EXPECT(space.distance(Eigen::Vector3d(1,2,3), Eigen::Vector3d(1,1,1))) 54 | == 0.0 + 1.0 + 2.0; 55 | } 56 | 57 | TEST(distance_linf) { 58 | using namespace unc::robotics::nigh; 59 | metric::LInfSpace space; 60 | EXPECT(space.distance(Eigen::Vector3d(1,2,3), Eigen::Vector3d(1,1,1))) 61 | == 2.0; 62 | } 63 | -------------------------------------------------------------------------------- /test/plot.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | file="$1" 4 | title="$2" 5 | shift 2 6 | ( 7 | # set scale to 2 or hidpi screens on term qt 8 | echo "scale=1" 9 | #echo "set term qt size 1024*scale,768*scale persist raise" 10 | 11 | echo "set term svg size 640,480 dynamic" 12 | echo "set output '$file'" 13 | 14 | echo "set title '$title'" 15 | echo "set ylabel 'us/search'" 16 | echo "set xlabel 'data structure size'" 17 | echo "set logscale x" 18 | echo "set key top left" 19 | echo "set format x '10^{%T}'" 20 | 21 | echo "ymax = 0" 22 | for file in $@ ; do 23 | title=$(sed -n '/^# Strategy = / { s/.*= // ; p ; }' "$file") 24 | if [[ $title != Linear ]] ; then 25 | echo "stats '$file' u 1:4 nooutput" 26 | echo "ymax = STATS_max_y > ymax ? STATS_max_y : ymax" 27 | fi 28 | done 29 | echo "set yrange [0:ymax]" 30 | 31 | sep="plot " 32 | for file in $@ ; do 33 | title=$(sed -n '/^# Strategy = / { s/.*= // ; p ; }' "$file") 34 | echo -n "$sep '$file' using 1:4 w lp lw scale title '$title'" 35 | sep=", " 36 | done 37 | echo "" 38 | 39 | ) | /usr/bin/env gnuplot 40 | -------------------------------------------------------------------------------- /test/sampler.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_TEST_SAMPLER_HPP 38 | #define NIGH_TEST_SAMPLER_HPP 39 | 40 | namespace nigh_test { 41 | template 42 | struct Sampler; 43 | } 44 | 45 | #endif 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /test/sampler_cartesian.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_TEST_IMPL_SAMPLER_CARTESIAN_HPP 38 | #define NIGH_TEST_IMPL_SAMPLER_CARTESIAN_HPP 39 | 40 | #include "sampler.hpp" 41 | #include 42 | #include 43 | 44 | namespace nigh_test { 45 | using namespace unc::robotics::nigh::metric; 46 | 47 | template 48 | struct CartesianSampler; 49 | 50 | template 51 | using cartesian_sampler_element_t = Sampler< 52 | cartesian_state_element_t, 53 | cartesian_element_t>; 54 | 55 | template 56 | struct CartesianSampler> 57 | : std::tuple...> 58 | { 59 | static_assert(sizeof...(I) > 0, "empty cartesian metric"); 60 | using Base = std::tuple...>; 61 | 62 | CartesianSampler(const Space& space) 63 | : Base(cartesian_sampler_element_t(space.template get())...) 64 | { 65 | } 66 | 67 | template 68 | State operator() (RNG& rng){ 69 | State q; 70 | ((std::get(q) = std::get(*this)(rng)), ...); 71 | return q; 72 | } 73 | }; 74 | 75 | template 76 | struct Sampler> 77 | : CartesianSampler, std::index_sequence_for> 78 | { 79 | using Base = CartesianSampler, std::index_sequence_for>; 80 | using Base::Base; 81 | }; 82 | 83 | } 84 | 85 | #endif 86 | 87 | -------------------------------------------------------------------------------- /test/sampler_lp.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_TEST_IMPL_SAMPLER_LP_HPP 38 | #define NIGH_TEST_IMPL_SAMPLER_LP_HPP 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include "sampler.hpp" 47 | #include "box_sampler.hpp" 48 | 49 | namespace nigh_test { 50 | using namespace unc::robotics::nigh; 51 | 52 | template 53 | struct Sampler> 54 | : BoxSampler>::kDimensions> 55 | { 56 | using Metric = metric::LP

; 57 | using Space = metric::Space; 58 | 59 | Sampler(const Space& space) 60 | : BoxSampler(space.dimensions(), -5, 5) 61 | { 62 | } 63 | }; 64 | } 65 | 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /test/sampler_scaled.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_TEST_SAMPLER_SCALED_HPP 38 | #define NIGH_TEST_SAMPLER_SCALED_HPP 39 | 40 | #include "sampler.hpp" 41 | #include 42 | 43 | namespace nigh_test { 44 | using namespace unc::robotics::nigh::metric; 45 | 46 | template 47 | struct Sampler> 48 | : Sampler 49 | { 50 | Sampler(const Space>& space) 51 | : Sampler(space.space()) 52 | { 53 | } 54 | }; 55 | } 56 | 57 | #endif 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /test/sampler_so2.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_TEST_IMPL_SAMPLER_SO2_HPP 38 | #define NIGH_TEST_IMPL_SAMPLER_SO2_HPP 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include "sampler.hpp" 46 | #include "box_sampler.hpp" 47 | 48 | namespace nigh_test { 49 | using namespace unc::robotics::nigh; 50 | using namespace unc::robotics::nigh::metric; 51 | 52 | template 53 | struct Sampler> 54 | : BoxSampler>::kDimensions> 55 | { 56 | using Metric = SO2

; 57 | using Space = metric::Space; 58 | using Scalar = typename Space::Distance; 59 | static constexpr Scalar PI = unc::robotics::nigh::impl::PI; 60 | 61 | Sampler(const Space& space) 62 | : BoxSampler(space.dimensions(), -PI, PI) 63 | { 64 | } 65 | }; 66 | } 67 | 68 | #endif 69 | 70 | -------------------------------------------------------------------------------- /test/sampler_so3.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef NIGH_TEST_IMPL_SAMPLER_SO3_HPP 38 | #define NIGH_TEST_IMPL_SAMPLER_SO3_HPP 39 | 40 | #include "sampler.hpp" 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace nigh_test { 48 | using namespace unc::robotics::nigh::metric; 49 | 50 | template 51 | struct Sampler { 52 | using Scalar = typename State::Scalar; 53 | using Metric = SO3; 54 | 55 | Sampler(const Space&) { 56 | } 57 | 58 | template 59 | State operator() (RNG& rng) { 60 | using namespace unc::robotics::nigh::impl; 61 | std::uniform_real_distribution dist01(0, 1); 62 | std::uniform_real_distribution dist2pi(0, 2*PI); 63 | Scalar a = dist01(rng); 64 | Scalar b = dist2pi(rng); 65 | Scalar c = dist2pi(rng); 66 | 67 | return State( 68 | std::sqrt(1-a)*std::sin(b), 69 | std::sqrt(1-a)*std::cos(b), 70 | std::sqrt(a)*std::sin(c), 71 | std::sqrt(a)*std::cos(c)); 72 | } 73 | }; 74 | } 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /test/scaled_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include "test.hpp" 39 | 40 | TEST(distance_ratio_scaled_l2) { 41 | using namespace unc::robotics::nigh; 42 | using Weight = std::ratio<7,2>; 43 | using Space = metric::ScaledSpace, Weight>; 44 | 45 | EXPECT((std::is_same_v)) == true; 46 | 47 | Space space; 48 | EXPECT(space.distance(Eigen::Vector3d(1,2,3), Eigen::Vector3d(1,1,1))) 49 | == std::sqrt(0.0 + 1.0 + 2.0*2.0) * 7 / 2; 50 | } 51 | 52 | TEST(distance_scalar_scaled_l2) { 53 | using namespace unc::robotics::nigh; 54 | using Space = metric::ScaledSpace>; 55 | 56 | EXPECT((std::is_same_v)) == true; 57 | 58 | Space space(12.345); 59 | EXPECT(space.distance(Eigen::Vector3d(1,2,3), Eigen::Vector3d(1,1,1))) 60 | == std::sqrt(0.0 + 1.0 + 2.0*2.0) * 12.345; 61 | } 62 | -------------------------------------------------------------------------------- /test/se2_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance_se2) { 40 | using namespace unc::robotics::nigh; 41 | using Space = metric::SE2Space; 42 | 43 | using State = std::tuple; 44 | EXPECT((std::is_same_v)) == true; 45 | 46 | State a{Eigen::Vector2d(1.0, 2.0), 3.0}; 47 | State b{Eigen::Vector2d(-1.0,5.0), -3.0}; 48 | 49 | Space space; 50 | EXPECT(space.distance(a, b)) == std::sqrt(4.0 + 9.0) + (impl::PI * 2 - 6); 51 | } 52 | 53 | -------------------------------------------------------------------------------- /test/se3_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance_se3) { 40 | using namespace unc::robotics::nigh; 41 | using Space = metric::SE3Space; 42 | 43 | using State = std::tuple; 44 | 45 | EXPECT((std::is_same_v)) == true; 46 | 47 | Eigen::Quaterniond a0, b0; 48 | 49 | a0 = Eigen::AngleAxisd(2.0, Eigen::Vector3d(1, 2, 3).normalized()); 50 | b0 = Eigen::AngleAxisd(2.0 - 2*0.987654321, Eigen::Vector3d(1, 2, 3).normalized()); 51 | 52 | State a{a0, Eigen::Vector3d(1,1,1)}; 53 | State b{b0, Eigen::Vector3d(1,2,3)}; 54 | 55 | Space space; 56 | EXPECT(std::abs(space.distance(a, b) - (0.987654321 + std::sqrt(5.0)))) < 1e-15; 57 | } 58 | 59 | -------------------------------------------------------------------------------- /test/so2_dist_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(ccwDist) { 40 | using namespace unc::robotics::nigh::impl::so2; 41 | EXPECT(ccwDist(0.0, 0.0)) == 0.0; 42 | } 43 | -------------------------------------------------------------------------------- /test/so2_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance_so2_l1) { 40 | using namespace unc::robotics::nigh; 41 | using Space = metric::SO2LPSpace; 42 | 43 | EXPECT((std::is_same_v)) == true; 44 | 45 | Space space; 46 | EXPECT(space.distance(Eigen::Vector3d(1,2,3 + 2*impl::PI), Eigen::Vector3d(1,1,1))) 47 | == 0.0 + 1.0 + 2.0; 48 | } 49 | 50 | -------------------------------------------------------------------------------- /test/so3_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance_so3) { 40 | using namespace unc::robotics::nigh; 41 | using Space = metric::SO3Space; 42 | 43 | EXPECT((std::is_same_v)) == true; 44 | 45 | Eigen::Quaterniond a, b; 46 | 47 | a = Eigen::AngleAxisd(2.0, Eigen::Vector3d(1, 2, 3).normalized()); 48 | b = Eigen::AngleAxisd(2.0 - 2*0.987654321, Eigen::Vector3d(1, 2, 3).normalized()); 49 | 50 | Space space; 51 | EXPECT(std::abs(space.distance(a, b) - 0.987654321)) < 1e-15; 52 | } 53 | 54 | --------------------------------------------------------------------------------