inertia.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#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 > Scalar(0)) {
        ++positive;
      } else if (elem < Scalar(0)) {
        ++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 > Scalar(0)) {
        ++positive;
      } else if (elem < Scalar(0)) {
        ++negative;
      } else {
        ++zero;
      }
    }
  }
 
  bool operator==(const Inertia&) const = default;
};
 
}  // namespace slp