You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
36 lines
1.5 KiB
36 lines
1.5 KiB
#pragma once
|
|
|
|
#include "math_defs.h"
|
|
#include "eigen_alias.hpp"
|
|
|
|
static constexpr auto epsilon = std::numeric_limits<double>::epsilon() * 1e6;
|
|
static constexpr auto pi = 3.14159265358979323846;
|
|
static constexpr auto two_pi = pi * 2;
|
|
static constexpr auto pi_div_2 = pi / 2;
|
|
static constexpr auto inv_pi = 1. / pi;
|
|
static constexpr auto inv_two_pi = 1. / two_pi;
|
|
static const auto x_direction = Eigen::Vector3d{1.0, 0.0, 0.0};
|
|
|
|
inline void vec3d_conversion(const vector3d& src, Eigen::Ref<Eigen::Vector3d> dst)
|
|
{
|
|
dst = Eigen::Map<const Eigen::Vector3d>(&src.x);
|
|
}
|
|
|
|
inline void vec3d_conversion(vector3d&& src, Eigen::Ref<Eigen::Vector3d> dst) { std::move(&src.x, &src.x + 3, dst.data()); }
|
|
|
|
inline double sign(const double t) { return t >= 0.0 ? 1.0 : -1.0; }
|
|
|
|
// Eigen has a type Isometry which supports similar operations as this
|
|
// but we use AffineCompact to get lower storage cost
|
|
// so we have to implement a helper function to apply the operation of Isometry to AffineCompact
|
|
inline auto inversed_affine_transform(const Eigen::Transform<double, 3, Eigen::AffineCompact>& trs)
|
|
{
|
|
Eigen::Transform<double, 3, Eigen::AffineCompact> result;
|
|
|
|
auto linear_part = result.matrix().template topLeftCorner<3, 3>();
|
|
linear_part = trs.linear().transpose();
|
|
result.matrix().template topRightCorner<3, 1>() = -linear_part * trs.translation();
|
|
result.makeAffine();
|
|
|
|
return result;
|
|
}
|