inertia.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <format>
#include <limits>
 
#include <Eigen/Core>
 
namespace slp {
 
class Inertia {
 public:
  int positive = 0;
  int negative = 0;
  int zero = 0;
 
  constexpr Inertia() = default;
 
  constexpr Inertia(int positive, int negative, int zero)
      : positive{positive}, negative{negative}, zero{zero} {}
 
  template <typename Scalar>
  explicit Inertia(const Eigen::Vector<Scalar, Eigen::Dynamic>& D) {
    for (const auto& elem : D) {
      if (elem > std::numeric_limits<Scalar>::epsilon()) {
        ++positive;
      } else if (elem < -std::numeric_limits<Scalar>::epsilon()) {
        ++negative;
      } else {
        ++zero;
      }
    }
  }
 
  template <typename Scalar>
  explicit Inertia(
      const Eigen::Diagonal<
          const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>& D) {
    for (const auto& elem : D) {
      if (elem > std::numeric_limits<Scalar>::epsilon()) {
        ++positive;
      } else if (elem < -std::numeric_limits<Scalar>::epsilon()) {
        ++negative;
      } else {
        ++zero;
      }
    }
  }
 
  bool operator==(const Inertia&) const = default;
};
 
}  // namespace slp
 
template <>
struct std::formatter<slp::Inertia> {
  constexpr auto parse(std::format_parse_context& ctx) {
    return m_underlying.parse(ctx);
  }
 
  template <typename FmtContext>
  auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
    auto out = ctx.out();
 
    out = std::format_to(out, "(");
    out = m_underlying.format(inertia.positive, ctx);
    out = std::format_to(out, ", ");
    out = m_underlying.format(inertia.negative, ctx);
    out = std::format_to(out, ", ");
    out = m_underlying.format(inertia.zero, ctx);
    return std::format_to(out, ")");
  }
 
 private:
  std::formatter<int> m_underlying;
};