Program Listing for File yaml.h

↰ Return to documentation for file (interfaces/yaml.h)

#pragma once

#include <yaml-cpp/yaml.h>

#include "lupnt/core/definitions.h"

namespace YAML {
  template <> struct convert<lupnt::Real> {
    static Node encode(const lupnt::Real& rhs) {
      Node node;
      node = static_cast<double>(rhs);
      return node;
    }

    static bool decode(const Node& node, lupnt::Real& rhs) {
      if (!node.IsScalar()) return false;
      rhs = node.as<double>();
      return true;
    }
  };

  template <typename Scalar, int Rows, int Cols> struct convert<Eigen::Matrix<Scalar, Rows, Cols>> {
    static Node encode(const Eigen::Matrix<Scalar, Rows, Cols>& mat) {
      Node node;
      for (int i = 0; i < mat.rows(); ++i) {
        Node row;
        for (int j = 0; j < mat.cols(); ++j) row.push_back(mat(i, j));
        node.push_back(row);
      }
      return node;
    }

    static bool decode(const Node& node, Eigen::Matrix<Scalar, Rows, Cols>& mat) {
      if (!node.IsSequence()) return false;

      // Handle vector, row or column, as a flat sequence.
      constexpr bool is_row_vector = (Rows == 1 && Cols != 1);
      constexpr bool is_col_vector = (Cols == 1 && Rows != 1);
      constexpr bool is_vector = (is_row_vector || is_col_vector);

      if (is_vector) {
        int vec_size = static_cast<int>(node.size());

        if (Rows == 1 && Cols == Eigen::Dynamic) {
          mat.derived().resize(1, vec_size);
        } else if (Cols == 1 && Rows == Eigen::Dynamic) {
          mat.derived().resize(vec_size, 1);
        } else if (Rows == 1 && Cols != Eigen::Dynamic && Cols != vec_size) {
          return false;
        } else if (Cols == 1 && Rows != Eigen::Dynamic && Rows != vec_size) {
          return false;
        }

        for (int i = 0; i < vec_size; ++i) {
          if (is_row_vector) {
            mat(0, i) = node[i].as<Scalar>();
          } else {
            mat(i, 0) = node[i].as<Scalar>();
          }
        }
        return true;
      }

      // Matrix case.
      if (Rows != Eigen::Dynamic && node.size() != static_cast<size_t>(Rows)) {
        return false;
      }

      int n_rows = static_cast<int>(node.size());
      int n_cols = 0;

      if (n_rows > 0) {
        if (!node[0].IsSequence()) return false;
        n_cols = static_cast<int>(node[0].size());
        if (Cols != Eigen::Dynamic && n_cols != Cols) return false;
      }

      if (Rows == Eigen::Dynamic && Cols == Eigen::Dynamic) {
        mat.derived().resize(n_rows, n_cols);
      } else if (Rows == Eigen::Dynamic) {
        mat.derived().resize(n_rows, mat.cols());
      } else if (Cols == Eigen::Dynamic) {
        mat.derived().resize(mat.rows(), n_cols);
      }

      for (int i = 0; i < n_rows; ++i) {
        const Node& row = node[i];
        if (!row.IsSequence()) return false;
        if (Cols != Eigen::Dynamic && static_cast<int>(row.size()) != Cols) return false;

        for (int j = 0; j < static_cast<int>(row.size()); ++j) {
          mat(i, j) = row[j].as<Scalar>();
        }
      }

      return true;
    }
  };
}  // namespace YAML