generic.hpp

// Copyright (C) 2024 Avular Holding B.V. - All Rights Reserved
// You may use this code under the terms of the Avular
// Software End-User License Agreement.
//
// You should have received a copy of the Avular
// Software End-User License Agreement license with
// this file, or download it from: avular.com/eula
//
/*****************************************************************************
 * Created on: 2024 July 08
 * Author: Bram Tertoolen
 *
 * @file Generic message constructs that are used in multiple messages.
 ****************************************************************************/
#pragma once
 
#include <cassert>
#include <creos/robot_clock.hpp>
#include <nlohmann/json.hpp>
 
namespace creos_messages {
 
struct Point {
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
 
    auto operator<=>(const Point& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Point, x, y, z);
 
struct Vector3d {
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
 
    auto operator<=>(const Vector3d& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Vector3d, x, y, z);
 
struct Vector3f {
    float x = 0.0f;
    float y = 0.0f;
    float z = 0.0f;
 
    auto operator<=>(const Vector3f& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Vector3f, x, y, z);
 
struct Quaterniond {
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    double w = 1.0;
 
    auto operator<=>(const Quaterniond& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Quaterniond, x, y, z, w);
 
struct Quaternionf {
    float x = 0.0f;
    float y = 0.0f;
    float z = 0.0f;
    float w = 1.0f;
 
    auto operator<=>(const Quaternionf& other) const = default;
};
 
NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Quaternionf, x, y, z, w);
 
template <typename T, std::size_t n_rows, std::size_t n_cols>
class Matrix {
public:
    Matrix() = default;
 
    Matrix(const std::array<std::array<T, n_cols>, n_rows>& data) : data_(data) {}
 
    Matrix(const std::array<T, n_rows * n_cols>& array) {
        for (std::size_t i = 0; i < n_rows; ++i) {
            for (std::size_t j = 0; j < n_cols; ++j) {
                data_[i][j] = array[i * n_cols + j];
            }
        }
    }
 
    Matrix(std::initializer_list<T> init) {
        assert(init.size() == n_rows * n_cols);
        std::size_t i = 0;
        for (const auto& value : init) {
            data_[i / n_cols][i % n_cols] = value;
            ++i;
        }
    }
 
    Matrix(std::initializer_list<std::initializer_list<T>> init) {
        assert(init.size() == n_rows);
        std::size_t i = 0;
        for (const auto& row : init) {
            assert(row.size() == n_cols);
            std::copy(row.begin(), row.end(), data_[i].begin());
            ++i;
        }
    }
 
    template <typename U>
    Matrix(const std::array<U, n_rows * n_cols>& array) {
        for (std::size_t i = 0; i < n_rows; ++i) {
            for (std::size_t j = 0; j < n_cols; ++j) {
                data_[i][j] = static_cast<T>(array[i * n_cols + j]);
            }
        }
    }
 
    std::array<T, n_rows * n_cols> toArray() const {
        std::array<T, n_rows * n_cols> array;
        for (std::size_t i = 0; i < n_rows; ++i) {
            for (std::size_t j = 0; j < n_cols; ++j) {
                array[i * n_cols + j] = data_[i][j];
            }
        }
        return array;
    }
 
    template <typename U>
    std::array<U, n_rows * n_cols> transformToArray() const {
        std::array<U, n_rows * n_cols> array;
        for (std::size_t i = 0; i < n_rows; ++i) {
            for (std::size_t j = 0; j < n_cols; ++j) {
                array[i * n_cols + j] = static_cast<U>(data_[i][j]);
            }
        }
        return array;
    }
 
    std::array<T, n_cols>& operator[](std::size_t index) { return data_[index]; }
 
    const std::array<T, n_cols>& operator[](std::size_t index) const { return data_[index]; }
 
    auto operator<=>(const Matrix& other) const = default;
 
    auto begin() const { return data_.begin(); }
 
    auto end() const { return data_.end(); }
 
private:
    std::array<std::array<T, n_cols>, n_rows> data_;
};
 
// to_json function for Matrix
template <typename T, std::size_t n_rows, std::size_t n_cols>
void to_json(nlohmann::json& j, const Matrix<T, n_rows, n_cols>& matrix) {
    j = nlohmann::json::array();
    for (const auto& row : matrix) {
        j.push_back(row);
    }
}
 
// from_json function for Matrix
template <typename T, std::size_t n_rows, std::size_t n_cols>
void from_json(const nlohmann::json& j, Matrix<T, n_rows, n_cols>& matrix) {
    for (std::size_t i = 0; i < n_rows; ++i) {
        for (std::size_t k = 0; k < n_cols; ++k) {
            matrix[i][k] = j[i][k].get<T>();
        }
    }
}
 
template <std::size_t n_rows, std::size_t n_cols>
using Matrixd = Matrix<double, n_rows, n_cols>;
 
template <std::size_t n_rows, std::size_t n_cols>
using Matrixf = Matrix<float, n_rows, n_cols>;
 
}  // namespace creos_messages