8 #ifndef GEOMETRY_MISC_H
9 #define GEOMETRY_MISC_H
12 #include <Eigen/Dense>
13 #include <geometry_msgs/Point.h>
14 #include <geometry_msgs/Quaternion.h>
21 using vec_t = Eigen::Matrix<double, dims, 1>;
23 using pt2_t = vec_t<2>;
24 using vec2_t = vec_t<2>;
25 using pt3_t = vec_t<3>;
26 using vec3_t = vec_t<3>;
28 using mat3_t = Eigen::Matrix3d;
30 using quat_t = Eigen::Quaterniond;
31 using anax_t = Eigen::AngleAxisd;
34 vec_t<dims + 1> toHomogenous(
const vec_t<dims>& vec)
36 const Eigen::Matrix<double, dims + 1, 1> ret((Eigen::Matrix<double, dims + 1, 1>() << vec, 1).finished());
59 const vec3_t rot_vec = rot*vec3_t::UnitX();
60 return std::atan2(rot_vec.y(), rot_vec.x());
79 double angleBetween(
const vec2_t& a,
const vec2_t& b);
93 double angleBetween(
const vec3_t& a,
const vec3_t& b);
112 anax_t angleaxisBetween(
const vec3_t& a,
const vec3_t& b,
const double tolerance = 1e-9);
129 quat_t quaternionBetween(
const vec3_t& a,
const vec3_t& b,
const double tolerance = 1e-9);
146 Eigen::Matrix3d rotationBetween(
const vec3_t& a,
const vec3_t& b,
const double tolerance = 1e-9);
159 double haversin(
const double angle);
172 double invHaversin(
const double value);
187 double solidAngle(
double a,
double b,
double c);
202 quat_t quaternionFromEuler(
double x,
double y,
double z);
211 quat_t quaternionFromEuler(
const Eigen::Vector3d& euler);
224 quat_t quaternionFromHeading(
const double heading);
243 double cross(
const vec2_t& a,
const vec2_t b);
257 double dist(
const vec2_t& a,
const vec2_t& b);
267 double dist(
const vec3_t& a,
const vec3_t& b);
282 double triangleArea(
const double a,
const double b,
const double c);
297 double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
312 template <
typename T>
315 const mat3_t matrot(rot);
316 return matrot*cov*matrot.transpose();