types.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_DATA_TYPES_H
39 #define FCL_DATA_TYPES_H
40 
41 #include <cstddef>
42 #include <cstdint>
43 #include <vector>
44 #include <map>
45 #include <memory>
46 #include <Eigen/Dense>
47 #include <Eigen/StdVector>
48 #include "fcl/export.h"
49 
50 namespace fcl
51 {
52 
53 typedef FCL_DEPRECATED double FCL_REAL;
54 typedef FCL_DEPRECATED std::int64_t FCL_INT64;
55 typedef FCL_DEPRECATED std::uint64_t FCL_UINT64;
56 typedef FCL_DEPRECATED std::int32_t FCL_INT32;
57 typedef FCL_DEPRECATED std::uint32_t FCL_UINT32;
58 
59 using int64 = std::int64_t;
60 using uint64 = std::uint64_t;
61 using int32 = std::int32_t;
62 using uint32 = std::uint32_t;
65 
66 template <typename S>
67 using Vector2 = Eigen::Matrix<S, 2, 1>;
68 
69 template <typename S>
70 using Vector3 = Eigen::Matrix<S, 3, 1>;
71 
72 template <typename S>
73 using Vector6 = Eigen::Matrix<S, 6, 1>;
74 
75 template <typename S>
76 using Vector7 = Eigen::Matrix<S, 7, 1>;
77 
78 template <typename S, int N>
79 using VectorN = Eigen::Matrix<S, N, 1>;
80 
81 template <typename S>
82 using VectorX = Eigen::Matrix<S, Eigen::Dynamic, 1>;
83 
84 template <typename S>
85 using Matrix3 = Eigen::Matrix<S, 3, 3>;
86 
87 template <typename S>
88 using Quaternion = Eigen::Quaternion<S>;
89 
90 template <typename S>
91 using Transform3 = Eigen::Transform<S, 3, Eigen::Isometry>;
92 
93 template <typename S>
94 using Translation3 = Eigen::Translation<S, 3>;
95 
96 template <typename S>
97 using AngleAxis = Eigen::AngleAxis<S>;
98 
99 // float types
101 template <int N>
109 
110 // double types
112 template <int N>
120 
121 template <typename _Tp>
122 using aligned_vector = std::vector<_Tp, Eigen::aligned_allocator<_Tp>>;
123 
124 template <typename _Key, typename _Tp, typename _Compare = std::less<_Key>>
125 using aligned_map = std::map<_Key, _Tp, _Compare,
126  Eigen::aligned_allocator<std::pair<const _Key, _Tp>>>;
127 
128 #if EIGEN_VERSION_AT_LEAST(3,2,9)
129 
130 template <typename _Tp, typename... _Args>
131 inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args)
132 {
133  typedef typename std::remove_const<_Tp>::type _Tp_nc;
134  return std::allocate_shared<_Tp>(Eigen::aligned_allocator<_Tp_nc>(),
135  std::forward<_Args>(__args)...);
136 }
137 
138 #else
139 
141 // Ref: https://bitbucket.org/eigen/eigen/commits/f5b7700
142 // C++11 compatible version is available since Eigen 3.2.9 so we use this copy
143 // for Eigen (< 3.2.9).
144 template <class T>
145 class FCL_EXPORT aligned_allocator_cpp11 : public std::allocator<T>
146 {
147 public:
148  typedef std::size_t size_type;
149  typedef std::ptrdiff_t difference_type;
150  typedef T* pointer;
151  typedef const T* const_pointer;
152  typedef T& reference;
153  typedef const T& const_reference;
154  typedef T value_type;
155 
156  template <class U>
157  struct rebind
158  {
160  };
161 
163  : std::allocator<T>() {}
164 
166  : std::allocator<T>(other) {}
167 
168  template <class U>
170  : std::allocator<T>(other) {}
171 
173 
174  pointer allocate(size_type num, const void* /*hint*/ = 0)
175  {
176  Eigen::internal::check_size_for_overflow<T>(num);
177  return static_cast<pointer>( Eigen::internal::aligned_malloc(num * sizeof(T)) );
178  }
179 
180  void deallocate(pointer p, size_type /*num*/)
181  {
182  Eigen::internal::aligned_free(p);
183  }
184 };
185 
186 template <typename _Tp, typename... _Args>
187 inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args)
188 {
189  typedef typename std::remove_const<_Tp>::type _Tp_nc;
190  return std::allocate_shared<_Tp>(aligned_allocator_cpp11<_Tp_nc>(),
191  std::forward<_Args>(__args)...);
192 }
193 
194 #endif
195 
196 } // namespace fcl
197 
198 #endif
fcl::aligned_allocator_cpp11::difference_type
std::ptrdiff_t difference_type
Definition: types.h:149
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::VectorN
Eigen::Matrix< S, N, 1 > VectorN
Definition: types.h:79
fcl::FCL_REAL
FCL_DEPRECATED double FCL_REAL
Definition: types.h:53
fcl::int64
std::int64_t int64
Definition: types.h:59
fcl::Quaternionf
Quaternion< float > Quaternionf
Definition: types.h:105
fcl::uint64
std::uint64_t uint64
Definition: types.h:60
fcl::aligned_allocator_cpp11::aligned_allocator_cpp11
aligned_allocator_cpp11()
Definition: types.h:162
fcl::Quaternion
Eigen::Quaternion< S > Quaternion
Definition: types.h:88
fcl::aligned_allocator_cpp11::~aligned_allocator_cpp11
~aligned_allocator_cpp11()
Definition: types.h:172
fcl::Matrix3f
Matrix3< float > Matrix3f
Definition: types.h:104
fcl::aligned_allocator_cpp11::const_reference
const typedef T & const_reference
Definition: types.h:153
fcl::uint32
std::uint32_t uint32
Definition: types.h:62
fcl::FCL_INT64
FCL_DEPRECATED std::int64_t FCL_INT64
Definition: types.h:54
fcl::VectorNf
VectorN< float, N > VectorNf
Definition: types.h:102
fcl::Transform3d
Transform3< double > Transform3d
Definition: types.h:117
fcl::aligned_allocator_cpp11
Aligned allocator that is compatible with c++11.
Definition: types.h:145
fcl::int32
std::int32_t int32
Definition: types.h:61
fcl::Vector6
Eigen::Matrix< S, 6, 1 > Vector6
Definition: types.h:73
fcl::aligned_allocator_cpp11::aligned_allocator_cpp11
aligned_allocator_cpp11(const aligned_allocator_cpp11< U > &other)
Definition: types.h:169
fcl::uintptr_t
std::uintptr_t uintptr_t
Definition: types.h:64
fcl::VectorX
Eigen::Matrix< S, Eigen::Dynamic, 1 > VectorX
Definition: types.h:82
fcl::aligned_allocator_cpp11::rebind::other
aligned_allocator_cpp11< U > other
Definition: types.h:159
fcl::Vector3d
Vector3< double > Vector3d
Definition: types.h:111
fcl::AngleAxis
Eigen::AngleAxis< S > AngleAxis
Definition: types.h:97
fcl::Vector2
Eigen::Matrix< S, 2, 1 > Vector2
Definition: types.h:67
fcl::FCL_UINT64
FCL_DEPRECATED std::uint64_t FCL_UINT64
Definition: types.h:55
fcl::AngleAxisd
AngleAxis< double > AngleAxisd
Definition: types.h:119
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Translation3f
Translation3< float > Translation3f
Definition: types.h:107
fcl::aligned_allocator_cpp11::rebind
Definition: types.h:157
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::AngleAxisf
AngleAxis< float > AngleAxisf
Definition: types.h:108
fcl::Translation3d
Translation3< double > Translation3d
Definition: types.h:118
fcl::aligned_allocator_cpp11::aligned_allocator_cpp11
aligned_allocator_cpp11(const aligned_allocator_cpp11 &other)
Definition: types.h:165
fcl::Transform3f
Transform3< float > Transform3f
Definition: types.h:106
fcl::VectorXf
VectorX< float > VectorXf
Definition: types.h:103
fcl::VectorNd
VectorN< double, N > VectorNd
Definition: types.h:113
fcl::FCL_INT32
FCL_DEPRECATED std::int32_t FCL_INT32
Definition: types.h:56
fcl::aligned_allocator_cpp11::size_type
std::size_t size_type
Definition: types.h:148
fcl::VectorXd
VectorX< double > VectorXd
Definition: types.h:114
fcl::make_aligned_shared
std::shared_ptr< _Tp > make_aligned_shared(_Args &&... __args)
Definition: types.h:187
fcl::Quaterniond
Quaternion< double > Quaterniond
Definition: types.h:116
fcl::aligned_allocator_cpp11::value_type
T value_type
Definition: types.h:154
fcl::Vector7
Eigen::Matrix< S, 7, 1 > Vector7
Definition: types.h:76
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
fcl::aligned_allocator_cpp11::allocate
pointer allocate(size_type num, const void *=0)
Definition: types.h:174
fcl::Vector3f
Vector3< float > Vector3f
Definition: types.h:100
fcl::intptr_t
std::intptr_t intptr_t
Definition: types.h:63
fcl::aligned_allocator_cpp11::pointer
T * pointer
Definition: types.h:150
fcl::aligned_allocator_cpp11::const_pointer
const typedef T * const_pointer
Definition: types.h:151
fcl::aligned_map
std::map< _Key, _Tp, _Compare, Eigen::aligned_allocator< std::pair< const _Key, _Tp > >> aligned_map
Definition: types.h:126
fcl::Matrix3d
Matrix3< double > Matrix3d
Definition: types.h:115
fcl::aligned_allocator_cpp11::deallocate
void deallocate(pointer p, size_type)
Definition: types.h:180
fcl::FCL_UINT32
FCL_DEPRECATED std::uint32_t FCL_UINT32
Definition: types.h:57
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::aligned_vector
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: types.h:122
fcl::aligned_allocator_cpp11::reference
T & reference
Definition: types.h:152


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49