8#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar) \
9 using VectorXs = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>; \
10 using MatrixXs = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>; \
11 using VectorOfVectors = std::vector<VectorXs>; \
12 using VectorRef = Eigen::Ref<VectorXs>; \
13 using MatrixRef = Eigen::Ref<MatrixXs>; \
14 using ConstVectorRef = Eigen::Ref<const VectorXs>; \
15 using ConstMatrixRef = Eigen::Ref<const MatrixXs>; \
16 using Vector3s = Eigen::Matrix<Scalar, 3, 1>; \
17 using Vector6s = Eigen::Matrix<Scalar, 6, 1>; \
18 using Matrix3Xs = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>; \
19 using Matrix6Xs = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>; \
20 using Matrix6s = Eigen::Matrix<Scalar, 6, 6>
22#define ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar) \
23 ALIGATOR_DYNAMIC_TYPEDEFS(Scalar); \
24 using RowMatrixXs = typename Eigen::Transpose<MatrixXs>::PlainObject; \
25 using RowMatrixRef = Eigen::Ref<RowMatrixXs>; \
26 using ConstRowMatrixRef = Eigen::Ref<const RowMatrixXs>
28#ifdef ALIGATOR_EIGEN_CHECK_MALLOC
31#define ALIGATOR_EIGEN_ALLOW_MALLOC(allowed) \
32 ::Eigen::internal::set_is_malloc_allowed(allowed)
37#define ALIGATOR_NOMALLOC_SCOPED \
38 const ::aligator::internal::scoped_nomalloc ___aligator_nomalloc_zone {}
40#define ALIGATOR_NOMALLOC_RESTORE \
41 ALIGATOR_EIGEN_ALLOW_MALLOC(::aligator::internal::get_cached_malloc_status())
43#define ALIGATOR_EIGEN_ALLOW_MALLOC(allowed)
44#define ALIGATOR_NOMALLOC_SCOPED
45#define ALIGATOR_NOMALLOC_RESTORE
49#define ALIGATOR_NOMALLOC_BEGIN ALIGATOR_EIGEN_ALLOW_MALLOC(false)
51#define ALIGATOR_NOMALLOC_END ALIGATOR_EIGEN_ALLOW_MALLOC(true)
59template <
template <
class>
class Base,
typename Derived>
struct is_tpl_base_of {
60 static constexpr std::false_type
f(
const void *);
61 template <
typename OtherDerived>
62 static constexpr std::true_type
f(
const Base<OtherDerived> *);
64 decltype(
f(std::declval<std::remove_reference_t<Derived> *>()))
::value;
67template <
template <
class>
class Base,
typename Derived>
71struct is_eigen : std::bool_constant<is_tpl_base_of_v<Eigen::EigenBase, T>> {};
81template <
typename T,
typename T2 =
void>
88#ifdef ALIGATOR_EIGEN_CHECK_MALLOC
90thread_local static bool g_cached_malloc_status =
true;
92inline void set_malloc_status(
bool status) { g_cached_malloc_status = status; }
94inline void save_malloc_status() {
96#ifdef ALIGATOR_EIGEN_CHECK_MALLOC
97 ::Eigen::internal::is_malloc_allowed()
104inline bool get_cached_malloc_status() {
return g_cached_malloc_status; }
106struct scoped_nomalloc {
107 scoped_nomalloc(
const scoped_nomalloc &) =
delete;
108 scoped_nomalloc(scoped_nomalloc &&) =
delete;
110 save_malloc_status();
132 const std::string &text,
136 int i = int(text.length()) - 1;
138 if (text[
size_t(i)] !=
'\n')
142 return mat.derived().format(ft);
148template <
typename MatType>
149typename MatType::Scalar
infty_norm(
const Eigen::MatrixBase<MatType> &z) {
150 if (z.rows() == 0 || z.cols() == 0) {
153 return z.template lpNorm<Eigen::Infinity>();
157template <
typename MatType>
158typename MatType::Scalar
infty_norm(
const std::vector<MatType> &z) {
159 const std::size_t n = z.size();
160 typename MatType::Scalar out = 0.;
161 for (std::size_t i = 0; i < n; i++) {
168template <
typename Scalar>
inline bool check_scalar(
const Scalar value) {
169 return std::isnan(value) || std::isinf(value);
174 const std::size_t n = xs.size();
175 for (std::size_t i = 0; i < n; i++) {
182template <typename T, typename = std::enable_if_t<std::is_scalar<T>::value>>
184 static_assert(std::is_scalar<T>::value,
"Parameter T should be scalar.");
188template <
typename MatrixType>
190 return (x.hasNaN() || (!x.allFinite()));
196template <
typename Scalar>
198 const Scalar prec = std::numeric_limits<Scalar>::epsilon()) {
199 return std::abs(a - b) < prec * (1 + std::max(std::abs(a), std::abs(b)));
202template <
typename T> T
sign(
const T &x) {
203 static_assert(std::is_scalar<T>::value,
"Parameter T should be scalar.");
204 return T((x > T(0)) - (x < T(0)));
208template <
typename Derived,
unsigned int UpLo = Eigen::Lower>
210 Derived &mat = matrix.const_cast_derived();
212 Eigen::SelfAdjointView<Derived, UpLo> view{mat};
216template <
typename T>
void setZero(std::vector<T> &mats) {
217 for (std::size_t i = 0; i < mats.size(); i++) {
223template <
typename TA,
typename AA,
typename TB,
typename BA,
typename OutType,
224 typename AOut,
typename Scalar>
226 const std::vector<TB, BA> &b,
227 std::vector<OutType, AOut> &c,
const Scalar alpha) {
228 assert(a.size() == b.size());
229 assert(a.size() == c.size());
230 const std::size_t N = a.size();
231 for (std::size_t i = 0; i < N; i++) {
232 c[i] = a[i] + alpha * b[i];
#define EIGEN_DEFAULT_IO_FORMAT
#define ALIGATOR_NOMALLOC_RESTORE
#define ALIGATOR_EIGEN_ALLOW_MALLOC(allowed)
void setZero(std::vector< T > &mats)
MatType::Scalar infty_norm(const Eigen::MatrixBase< MatType > &z)
void make_symmetric(const Eigen::MatrixBase< Derived > &matrix)
Symmetrize a matrix using its lower triangular part.
bool check_scalar(const Scalar value)
Check that a scalar is neither inf, nor NaN.
bool check_value(const std::vector< T > &xs)
Check if a std::vector of numerical objects has invalid values.
bool scalar_close(const Scalar a, const Scalar b, const Scalar prec=std::numeric_limits< Scalar >::epsilon())
Tests whether a and b are close, within absolute and relative precision prec.
void vectorMultiplyAdd(const std::vector< TA, AA > &a, const std::vector< TB, BA > &b, std::vector< OutType, AOut > &c, const Scalar alpha)
Compute zi = xi + alpha * yi for all i.
constexpr bool is_eigen_dense_v
constexpr bool is_eigen_v
constexpr bool is_tpl_base_of_v
std::enable_if_t< is_eigen_dense_v< T >, T2 > enable_if_eigen_dense_t
std::enable_if_t< is_eigen_v< std::decay_t< T > > > enable_if_eigen_t
auto eigenPrintWithPreamble(const Eigen::EigenBase< D > &mat, const std::string &text, Eigen::IOFormat ft=EIGEN_DEFAULT_IO_FORMAT)
constexpr bool is_eigen_matrix_v
static constexpr std::false_type f(const void *)
static constexpr bool value
static constexpr std::true_type f(const Base< OtherDerived > *)
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
ALIGATOR_DYNAMIC_TYPEDEFS(_Scalar)