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.
77 lines
2.6 KiB
77 lines
2.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 two_div_pi = 2 / 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 convert_vec3d(vector3d x) { return Eigen::Vector3d{x.x, x.y, x.z}; }
|
|
|
|
inline auto convert_vec3d(Eigen::Vector3d x) { return vector3d{x[0], x[1], x[2]}; }
|
|
|
|
inline auto convert_mat3x4d(const matrix3x4d& x)
|
|
{
|
|
Eigen::AffineCompact3d res{};
|
|
std::move(&x.col0.x, &x.col3.z + 1, res.data());
|
|
return res;
|
|
}
|
|
|
|
inline auto convert_mat3x4d(const Eigen::AffineCompact3d& x)
|
|
{
|
|
matrix3x4d res{};
|
|
std::move(x.data(), x.data() + 3 * 4, &res.col0.x);
|
|
return res;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
inline auto accurate_sin_cos(double x, bool is_cosine) noexcept
|
|
{
|
|
if (x == 0) return is_cosine ? 1. : 0.;
|
|
|
|
int q;
|
|
const auto r = std::remquo(x, pi_div_2, &q);
|
|
q = (q + (is_cosine ? 1 : 0)) & 3;
|
|
|
|
switch (q) {
|
|
case 0: return std::copysign(std::sin(r), x);
|
|
case 1: return std::copysign(std::cos(r), x);
|
|
case 2: return std::copysign(-std::sin(r), x);
|
|
case 3: return std::copysign(-std::cos(r), x);
|
|
}
|
|
|
|
return .0;
|
|
}
|