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.
46 lines
1.6 KiB
46 lines
1.6 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 constexpr auto sqrt_2 = 1.4142135623730951;
|
|
static constexpr auto sqrt_3 = 1.7320508075688772;
|
|
static constexpr auto infinity = std::numeric_limits<double>::infinity();
|
|
static const auto x_direction = Eigen::Vector3d{1.0, 0.0, 0.0};
|
|
|
|
inline auto to_vec4d_by_1(Eigen::Vector3d x)
|
|
{
|
|
return Eigen::Vector4d{x[0], x[1], x[2], 1.};
|
|
}
|
|
|
|
inline auto to_vec4d_by_0(Eigen::Vector3d x)
|
|
{
|
|
return Eigen::Vector4d{x[0], x[1], x[2], .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;
|
|
}
|
|
|
|
inline auto corrected_atan2(double y, double x)
|
|
{
|
|
auto v = atan2(y, x);
|
|
return v < 0 ? v + two_pi : v;
|
|
}
|