utils.hpp

浏览该文件的文档.

 
#pragma once
 
#include <concepts>
#include <array>
#include <cstdint>
#include <cstring>
#include <chrono>
#include <cmath>
#include <numbers>
#include <type_traits>
 
#include "utils/concepts.hpp"
 
namespace roboctrl{
 
using fp32 = float;
using fp64 = double;
 
template<typename T>
constexpr T Pi = std::numbers::pi_v<T>;
 
constexpr auto Pi_f = Pi<fp32>;
 
 
template<typename T>
    requires std::is_arithmetic_v<T>
struct vector{
    T x;
    T y;
 
    T norm() const{
        return std::sqrt(x * x + y * y);
    }
 
    vector normalized()const{
        return *this / norm();
    }
};
 
using vectori = vector<int>;
using vectorf = vector<float>;
 
template<typename T>
vector<T> operator + (vector<T> a,vector<T> b){
    return {a.x+b.x,a.y+b.y};
    
}
 
template<typename T>
vector<T> operator - (vector<T> a,vector<T> b){
    return {a.x-b.x,a.y-b.y};
}
 
template<typename T>
vector<T> operator * (vector<T> a,T b){
    return {a.x * b,a.y * b};
}
 
template<typename T>
vector<T> operator / (vector<T> a,T b){
    return {a.x / b,a.y / b};
}
 
namespace utils{
 
constexpr inline fp32 rad_format(fp32 ang){
    fp32 ans = fmodf(ang + Pi_f, Pi_f * 2.f);
    return (ans < 0.f) ? ans + Pi_f : ans - Pi_f;
}
 
template<typename C>
concept byte_container =
    requires(C c) {
        { c.data() } -> std::convertible_to<const std::byte*>;
        { c.size() } -> std::convertible_to<std::size_t>;
    };
 
template<package T, byte_container C>
T from_bytes(const C& bytes) {
    if (bytes.size() != sizeof(T)) {
        throw std::invalid_argument("byte buffer size does not match destination type");
    }
    T t{};
    std::memcpy(&t, bytes.data(), sizeof(T));
    return t;
}
 
template<package T>
auto to_bytes(const T& t) -> std::array<std::byte, sizeof(T)> {
    std::array<std::byte, sizeof(T)> bytes{};
    std::memcpy(bytes.data(), &t, sizeof(T));
    return bytes;
}
 
template<package T, typename C>
requires requires(C& c) {
    { c.data() } -> std::convertible_to<std::byte*>;
    { c.size() } -> std::convertible_to<std::size_t>;
}
void to_bytes(const T& t, C& container) {
    if (container.size() < sizeof(T)) {
        throw std::invalid_argument("byte buffer is smaller than source type");
    }
    std::memcpy(container.data(), &t, sizeof(T));
}
inline auto now() {
    static const auto init_time = std::chrono::steady_clock::now();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now() - init_time);
}
 
inline constexpr uint16_t make_u16(uint16_t high, uint16_t low) noexcept {
    return (static_cast<uint16_t>(high) << 8) | static_cast<uint16_t>(low);
}
 
inline constexpr int16_t make_i16(int16_t high, int16_t low) noexcept {
    return (static_cast<int16_t>(high) << 8) | static_cast<int16_t>(low);
}
 
inline constexpr std::byte to_byte(std::integral auto v) noexcept{
    return static_cast<std::byte>(v);
}
 
template<typename T>
struct function_arg : function_arg<decltype(&std::remove_cvref_t<T>::operator())> {};
 
template<typename Ret, typename Arg>
struct function_arg<Ret(Arg)> { using type = Arg; };
 
template<typename Ret, typename Arg>
struct function_arg<Ret(*)(Arg)> : function_arg<Ret(Arg)> {};
 
template<typename Ret, typename Arg>
struct function_arg<Ret(Arg) noexcept> : function_arg<Ret(Arg)> {};
 
template<typename Ret, typename Arg>
struct function_arg<Ret(*)(Arg) noexcept> : function_arg<Ret(Arg)> {};
 
template<typename Ret, typename Class, typename Arg>
struct function_arg<Ret(Class::*)(Arg)> { using type = Arg; };
 
template<typename Ret, typename Class, typename Arg>
struct function_arg<Ret(Class::*)(Arg) const> { using type = Arg; };
 
template<typename Ret, typename Class, typename Arg>
struct function_arg<Ret(Class::*)(Arg) noexcept> : function_arg<Ret(Class::*)(Arg)> {};
 
template<typename Ret, typename Class, typename Arg>
struct function_arg<Ret(Class::*)(Arg) const noexcept> : function_arg<Ret(Class::*)(Arg) const> {};
 
template<typename Fn>
using function_arg_t = typename function_arg<std::remove_cvref_t<Fn>>::type;
 
}
}