410 lines
11 KiB
C++
Executable File
410 lines
11 KiB
C++
Executable File
#ifndef UTILITIES_H
|
|
#define UTILITIES_H
|
|
|
|
#include <Eigen/Dense>
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <chrono>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <iterator>
|
|
#include <numeric>
|
|
#include <regex>
|
|
#include <string_view>
|
|
|
|
#define GET_VARIABLE_NAME(Variable) (#Variable)
|
|
|
|
struct Vector6d : public std::array<double, 6> {
|
|
Vector6d() {
|
|
for (size_t i = 0; i < 6; i++) {
|
|
this->operator[](i) = 0;
|
|
}
|
|
}
|
|
|
|
Vector6d(const std::vector<double> &v) {
|
|
assert(v.size() == 6);
|
|
std::copy(v.begin(), v.end(), this->begin());
|
|
}
|
|
|
|
Vector6d(const double &d) {
|
|
for (size_t i = 0; i < 6; i++) {
|
|
this->operator[](i) = d;
|
|
}
|
|
}
|
|
|
|
Vector6d(const std::array<double, 6> &arr) : std::array<double, 6>(arr) {}
|
|
|
|
Vector6d(const std::initializer_list<double> &initList) {
|
|
std::copy(initList.begin(), initList.end(), std::begin(*this));
|
|
}
|
|
|
|
Vector6d operator*(const double &d) const {
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
result[i] = this->operator[](i) * d;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Vector6d operator*(const Vector6d &v) const {
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
result[i] = this->operator[](i) * v[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Vector6d operator/(const double &d) const {
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
result[i] = this->operator[](i) / d;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Vector6d operator/(const Vector6d &v) const
|
|
{
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
result[i] = this->operator[](i) / v[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Vector6d operator+(const Vector6d &v) const {
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
result[i] = this->operator[](i) + v[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Vector6d operator-(const Vector6d &v) const {
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
result[i] = this->operator[](i) - v[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Vector6d inverted() const {
|
|
Vector6d result;
|
|
for (size_t i = 0; i < 6; i++) {
|
|
assert(this->operator[](i) != 0);
|
|
result[i] = 1 / this->operator[](i);
|
|
}
|
|
return result;
|
|
}
|
|
bool isZero() const {
|
|
for (size_t i = 0; i < 6; i++) {
|
|
if (this->operator[](i) != 0)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
double squaredNorm() const {
|
|
double squaredNorm = 0;
|
|
std::for_each(this->begin(), std::end(*this),
|
|
[&](const double &v) { squaredNorm += pow(v, 2); });
|
|
return squaredNorm;
|
|
}
|
|
|
|
double norm() const { return sqrt(squaredNorm()); }
|
|
|
|
bool isFinite() const {
|
|
return std::any_of(std::begin(*this), std::end(*this), [](const double &v) {
|
|
if (!std::isfinite(v)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
});
|
|
}
|
|
|
|
Eigen::Vector3d getTranslation() const {
|
|
return Eigen::Vector3d(this->operator[](0), this->operator[](1),
|
|
this->operator[](2));
|
|
}
|
|
|
|
Eigen::Vector3d getRotation() const
|
|
{
|
|
return Eigen::Vector3d(this->operator[](3), this->operator[](4), this->operator[](5));
|
|
}
|
|
|
|
std::string toString() const
|
|
{
|
|
std::string s;
|
|
for (int i = 0; i < 6; i++) {
|
|
s.append(std::to_string(this->operator[](i)) + ",");
|
|
}
|
|
s.pop_back();
|
|
return s;
|
|
}
|
|
};
|
|
|
|
namespace Utilities {
|
|
template<typename T>
|
|
std::string to_string_with_precision(const T a_value, const int n = 2)
|
|
{
|
|
std::ostringstream out;
|
|
out.precision(n);
|
|
out << std::fixed << a_value;
|
|
return out.str();
|
|
}
|
|
|
|
inline bool compareNat(const std::string &a, const std::string &b)
|
|
{
|
|
if (a.empty())
|
|
return true;
|
|
if (b.empty())
|
|
return false;
|
|
if (std::isdigit(a[0]) && !std::isdigit(b[0]))
|
|
return true;
|
|
if (!std::isdigit(a[0]) && std::isdigit(b[0]))
|
|
return false;
|
|
if (!std::isdigit(a[0]) && !std::isdigit(b[0])) {
|
|
if (std::toupper(a[0]) == std::toupper(b[0]))
|
|
return compareNat(a.substr(1), b.substr(1));
|
|
return (std::toupper(a[0]) < std::toupper(b[0]));
|
|
}
|
|
|
|
// Both strings begin with digit --> parse both numbers
|
|
std::istringstream issa(a);
|
|
std::istringstream issb(b);
|
|
int ia, ib;
|
|
issa >> ia;
|
|
issb >> ib;
|
|
if (ia != ib)
|
|
return ia < ib;
|
|
|
|
// Numbers are the same --> remove numbers and recurse
|
|
std::string anew, bnew;
|
|
std::getline(issa, anew);
|
|
std::getline(issb, bnew);
|
|
return (compareNat(anew, bnew));
|
|
}
|
|
|
|
inline std::string_view leftTrimSpaces(const std::string_view& str)
|
|
{
|
|
std::string_view trimmedString=str;
|
|
const auto pos(str.find_first_not_of(" \t\n\r\f\v"));
|
|
trimmedString.remove_prefix(std::min(pos, trimmedString.length()));
|
|
return trimmedString;
|
|
}
|
|
|
|
inline std::string_view rightTrimSpaces(const std::string_view& str)
|
|
{
|
|
std::string_view trimmedString=str;
|
|
const auto pos(trimmedString.find_last_not_of(" \t\n\r\f\v"));
|
|
trimmedString.remove_suffix(std::min(trimmedString.length() - pos - 1,trimmedString.length()));
|
|
return trimmedString;
|
|
}
|
|
|
|
inline std::string_view trimLeftAndRightSpaces(std::string_view str)
|
|
{
|
|
std::string_view trimmedString=str;
|
|
trimmedString = leftTrimSpaces(trimmedString);
|
|
trimmedString = rightTrimSpaces(trimmedString);
|
|
return trimmedString;
|
|
}
|
|
|
|
template<typename InputIt>
|
|
inline void normalize(InputIt itBegin, InputIt itEnd)
|
|
{
|
|
const auto squaredSumOfElements = std::accumulate(itBegin,
|
|
itEnd,
|
|
0.0,
|
|
[](const auto &sum, const auto &el) {
|
|
return sum + el * el;
|
|
});
|
|
assert(squaredSumOfElements != 0);
|
|
std::transform(itBegin, itEnd, itBegin, [&](auto &element) {
|
|
return element / std::sqrt(squaredSumOfElements);
|
|
});
|
|
}
|
|
|
|
inline std::vector<std::string> split(const std::string& text, std::string delim)
|
|
{
|
|
std::vector<std::string> vec;
|
|
size_t pos = 0, prevPos = 0;
|
|
while (1) {
|
|
pos = text.find(delim, prevPos);
|
|
if (pos == std::string::npos) {
|
|
vec.push_back(text.substr(prevPos));
|
|
return vec;
|
|
}
|
|
|
|
vec.push_back(text.substr(prevPos, pos - prevPos));
|
|
prevPos = pos + delim.length();
|
|
}
|
|
}
|
|
|
|
inline std::string toString(const std::vector<std::vector<int>> &vv)
|
|
{
|
|
std::string s;
|
|
s.append("{");
|
|
for (const std::vector<int> &v : vv) {
|
|
s.append("{");
|
|
for (const int &i : v) {
|
|
s.append(std::to_string(i) + ",");
|
|
}
|
|
s.pop_back();
|
|
s.append("}");
|
|
}
|
|
s.append("}");
|
|
return s;
|
|
}
|
|
inline void parseIntegers(const std::string &str, std::vector<size_t> &result)
|
|
{
|
|
typedef std::regex_iterator<std::string::const_iterator> re_iterator;
|
|
typedef re_iterator::value_type re_iterated;
|
|
|
|
std::regex re("(\\d+)");
|
|
|
|
re_iterator rit(str.begin(), str.end(), re);
|
|
re_iterator rend;
|
|
|
|
std::transform(rit, rend, std::back_inserter(result), [](const re_iterated &it) {
|
|
return std::stoi(it[1]);
|
|
});
|
|
}
|
|
|
|
inline Eigen::MatrixXd toEigenMatrix(const std::vector<Vector6d> &v) {
|
|
Eigen::MatrixXd m(v.size(), 6);
|
|
|
|
for (size_t vi = 0; vi < v.size(); vi++) {
|
|
const Vector6d &vec = v[vi];
|
|
for (size_t i = 0; i < 6; i++) {
|
|
m(vi, i) = vec[i];
|
|
}
|
|
}
|
|
|
|
return m;
|
|
}
|
|
|
|
inline std::vector<Vector6d> fromEigenMatrix(const Eigen::MatrixXd &m)
|
|
{
|
|
std::vector<Vector6d> v(m.rows());
|
|
|
|
for (size_t vi = 0; vi < m.rows(); vi++) {
|
|
const Eigen::RowVectorXd &row = m.row(vi);
|
|
for (size_t i = 0; i < 6; i++) {
|
|
v[vi][i] = row(i);
|
|
}
|
|
}
|
|
|
|
return v;
|
|
}
|
|
// std::string convertToLowercase(const std::string &s) {
|
|
// std::string lowercase;
|
|
// std::transform(s.begin(), s.end(), lowercase.begin(),
|
|
// [](unsigned char c) { return std::tolower(c); });
|
|
// return lowercase;
|
|
//}
|
|
// bool hasExtension(const std::string &filename, const std::string &extension)
|
|
// {
|
|
// const std::filesystem::path path(filename);
|
|
// if (!path.has_extension()) {
|
|
// std::cerr << "Error: No file extension found in " << filename <<
|
|
// std::endl; return false;
|
|
// }
|
|
|
|
// const std::string detectedExtension = path.extension().string();
|
|
|
|
// if (convertToLowercase(detectedExtension) != convertToLowercase(extension))
|
|
// {
|
|
// std::cerr << "Error: detected extension is " + detectedExtension +
|
|
// " and not " + extension
|
|
// << std::endl;
|
|
// return false;
|
|
// }
|
|
// return true;
|
|
//}
|
|
|
|
inline std::filesystem::path getFilepathWithExtension(const std::filesystem::path &folderPath,
|
|
const std::string &extension)
|
|
{
|
|
assert(std::filesystem::exists(folderPath));
|
|
for (const std::filesystem::directory_entry &dirEntry :
|
|
std::filesystem::directory_iterator(folderPath)) {
|
|
if (dirEntry.is_regular_file() && std::filesystem::path(dirEntry).extension() == extension) {
|
|
return std::filesystem::path(dirEntry);
|
|
}
|
|
}
|
|
|
|
return "";
|
|
}
|
|
|
|
void createPlot(const std::string &xLabel,
|
|
const std::string &yLabel,
|
|
const std::vector<double> &x,
|
|
const std::vector<double> &y,
|
|
const std::vector<double> &markerSizes,
|
|
const std::vector<double> &c,
|
|
const std::string &saveTo = {});
|
|
|
|
} // namespace Utilities
|
|
|
|
#ifdef POLYSCOPE_DEFINED
|
|
|
|
namespace PolyscopeInterface {
|
|
inline struct GlobalPolyscopeData
|
|
{
|
|
std::vector<std::function<void()>> userCallbacks;
|
|
} globalPolyscopeData;
|
|
|
|
void mainCallback();
|
|
|
|
void addUserCallback(const std::function<void()> &userCallback);
|
|
|
|
void deinitPolyscope();
|
|
|
|
void init();
|
|
using PolyscopeLabel = std::string;
|
|
std::pair<PolyscopeLabel, size_t> getSelection();
|
|
|
|
void registerWorldAxes();
|
|
} // namespace PolyscopeInterface
|
|
|
|
#endif
|
|
|
|
// namespace ConfigurationFile {
|
|
|
|
//}
|
|
//} // namespace ConfigurationFile
|
|
template <typename T1, typename T2>
|
|
void constructInverseMap(const T1 &map, T2 &oppositeMap) {
|
|
assert(!map.empty());
|
|
oppositeMap.clear();
|
|
for (const auto &mapIt : map) {
|
|
oppositeMap[mapIt.second] = mapIt.first;
|
|
}
|
|
}
|
|
|
|
template <typename T> std::string toString(const T &v) {
|
|
return "(" + std::to_string(v[0]) + "," + std::to_string(v[1]) + "," +
|
|
std::to_string(v[2]) + ")";
|
|
}
|
|
|
|
template<typename T>
|
|
size_t computeHashUnordered(const std::vector<T> &v)
|
|
{
|
|
size_t hash = 0;
|
|
for (const auto &el : v) {
|
|
hash += std::hash<T>{}(el);
|
|
}
|
|
return hash;
|
|
}
|
|
|
|
inline size_t computeHashOrdered(const std::vector<int> &v)
|
|
{
|
|
std::string elementsString;
|
|
for (const auto &el : v) {
|
|
elementsString += std::to_string(el);
|
|
}
|
|
|
|
return std::hash<std::string>{}(elementsString);
|
|
}
|
|
|
|
#endif // UTILITIES_H
|