68 lines
2.0 KiB
C++
68 lines
2.0 KiB
C++
#ifndef CMVR_ES_CARTESIAN_MOTION_MATH_H
|
|
#define CMVR_ES_CARTESIAN_MOTION_MATH_H
|
|
|
|
#include <Eigen/Core>
|
|
#include <Eigen/Geometry>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
namespace cmvr::device::cartesian_motion {
|
|
|
|
inline double clamp(const double value, const double lo, const double hi)
|
|
{
|
|
return std::max(lo, std::min(hi, value));
|
|
}
|
|
|
|
inline Eigen::VectorXd toEigenVector(const std::vector<double>& values)
|
|
{
|
|
if (values.empty()) {
|
|
return {};
|
|
}
|
|
return Eigen::Map<const Eigen::VectorXd>(values.data(),
|
|
static_cast<Eigen::Index>(values.size()));
|
|
}
|
|
|
|
inline std::vector<double> toStdVector(const Eigen::VectorXd& values)
|
|
{
|
|
return {values.data(), values.data() + values.size()};
|
|
}
|
|
|
|
inline double directionDeviationDeg(const Eigen::Vector3d& desired,
|
|
const Eigen::Vector3d& actual)
|
|
{
|
|
const double desired_norm = desired.norm();
|
|
const double actual_norm = actual.norm();
|
|
if (desired_norm <= 1e-9 || actual_norm <= 1e-9) {
|
|
return 0.0;
|
|
}
|
|
const double direction_cos =
|
|
clamp(desired.dot(actual) / (desired_norm * actual_norm), -1.0, 1.0);
|
|
constexpr double rad_to_deg = 180.0 / 3.14159265358979323846;
|
|
return std::acos(direction_cos) * rad_to_deg;
|
|
}
|
|
|
|
inline double lateralDistanceToLine(const Eigen::Vector3d& start,
|
|
const Eigen::Vector3d& direction,
|
|
const Eigen::Vector3d& point)
|
|
{
|
|
const Eigen::Vector3d delta = point - start;
|
|
const Eigen::Vector3d lateral = delta - delta.dot(direction) * direction;
|
|
return lateral.norm();
|
|
}
|
|
|
|
inline Eigen::Vector3d rotationVector(const Eigen::Matrix3d& rotation)
|
|
{
|
|
Eigen::AngleAxisd angle_axis(rotation);
|
|
const double angle = angle_axis.angle();
|
|
if (std::abs(angle) <= 1e-9) {
|
|
return Eigen::Vector3d::Zero();
|
|
}
|
|
return angle_axis.axis() * angle;
|
|
}
|
|
|
|
} // namespace cmvr::device::cartesian_motion
|
|
|
|
#endif // CMVR_ES_CARTESIAN_MOTION_MATH_H
|