MySources/utilities.hpp

#ifndef UTILITIES_H
#define UTILITIES_H

#include <Eigen/Dense>
#include <filesystem>
#include <fstream>
#include <regex>

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(), this->begin());
  }

  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 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(begin(), end(),
                  [&](const double &v) { squaredNorm += pow(v, 2); });
    return squaredNorm;
  }

  double norm() const { return sqrt(squaredNorm()); }

  bool isFinite() const {
    return std::any_of(begin(), end(), [](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));
  }
};

namespace Utilities {
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;
//}

} // namespace Utilities

#ifdef POLYSCOPE_DEFINED
#include "polyscope/curve_network.h"
#include "polyscope/pick.h"
#include "polyscope/polyscope.h"
#include <functional>

namespace PolyscopeInterface {
inline struct GlobalPolyscopeData
{
    std::vector<std::function<void()>> userCallbacks;
} globalPolyscopeData;

inline void mainCallback()
{
    ImGui::PushItemWidth(100); // Make ui elements 100 pixels wide,
                               // instead of full width. Must have
                               // matching PopItemWidth() below.

    for (std::function<void()> userCallback : globalPolyscopeData.userCallbacks) {
        userCallback();
    }

    ImGui::PopItemWidth();
}

inline void addUserCallback(const std::function<void()> &userCallback)
{
    globalPolyscopeData.userCallbacks.push_back(userCallback);
}

inline void deinitPolyscope()
{
    if (!polyscope::state::initialized) {
        return;
    }

    polyscope::shutdown();
}

inline void init()
{
    if (polyscope::state::initialized) {
        return;
    }
    polyscope::init();
    polyscope::options::groundPlaneEnabled = false;
    polyscope::view::upDir = polyscope::view::UpDir::ZUp;

    polyscope::state::userCallback = &mainCallback;
}
using PolyscopeLabel = std::string;
inline std::pair<PolyscopeLabel, size_t> getSelection()
{
    std::pair<polyscope::Structure *, size_t> selection = polyscope::pick::getSelection();
    if (selection.first == nullptr) {
        return std::make_pair(std::string(), 0);
    }
    return std::make_pair(selection.first->name, selection.second);
}

inline void registerWorldAxes()
{
    PolyscopeInterface::init();

    Eigen::MatrixX3d axesPositions(4, 3);
    axesPositions.row(0) = Eigen::Vector3d(0, 0, 0);
    axesPositions.row(1) = Eigen::Vector3d(polyscope::state::lengthScale, 0, 0);
    axesPositions.row(2) = Eigen::Vector3d(0, polyscope::state::lengthScale, 0);
    axesPositions.row(3) = Eigen::Vector3d(0, 0, polyscope::state::lengthScale);

    Eigen::MatrixX2i axesEdges(3, 2);
    axesEdges.row(0) = Eigen::Vector2i(0, 1);
    axesEdges.row(1) = Eigen::Vector2i(0, 2);
    axesEdges.row(2) = Eigen::Vector2i(0, 3);
    Eigen::MatrixX3d axesColors(3, 3);
    axesColors.row(0) = Eigen::Vector3d(1, 0, 0);
    axesColors.row(1) = Eigen::Vector3d(0, 1, 0);
    axesColors.row(2) = Eigen::Vector3d(0, 0, 1);

    const std::string worldAxesName = "World Axes";
    polyscope::registerCurveNetwork(worldAxesName, axesPositions, axesEdges);
    polyscope::getCurveNetwork(worldAxesName)->setRadius(0.0001, false);
    const std::string worldAxesColorName = worldAxesName + " Color";
    polyscope::getCurveNetwork(worldAxesName)
        ->addEdgeColorQuantity(worldAxesColorName, axesColors)
        ->setEnabled(true);
}
} // 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>
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();
}

#endif // UTILITIES_H