axis_order.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_IMU_AXIS_ORDER_HPP
#define M5_UNIT_IMU_IMU_AXIS_ORDER_HPP
 
#include <cstdint>
 
namespace m5 {
namespace imu {
 
enum class AxisPermutation : uint8_t {
    XYZ,  
    XZY,  
    YXZ,  
    YZX,  
    ZXY,  
    ZYX,  
};
 
struct AxisOrder {
    AxisPermutation permutation{AxisPermutation::XYZ};  
    bool invert_x{false};                               
    bool invert_y{false};                               
    bool invert_z{false};                               
 
    inline void apply(float (&v)[3]) const
    {
        const float src[3] = {v[0], v[1], v[2]};
        switch (permutation) {
            case AxisPermutation::XYZ:
                v[0] = src[0];
                v[1] = src[1];
                v[2] = src[2];
                break;
            case AxisPermutation::XZY:
                v[0] = src[0];
                v[1] = src[2];
                v[2] = src[1];
                break;
            case AxisPermutation::YXZ:
                v[0] = src[1];
                v[1] = src[0];
                v[2] = src[2];
                break;
            case AxisPermutation::YZX:
                v[0] = src[1];
                v[1] = src[2];
                v[2] = src[0];
                break;
            case AxisPermutation::ZXY:
                v[0] = src[2];
                v[1] = src[0];
                v[2] = src[1];
                break;
            case AxisPermutation::ZYX:
                v[0] = src[2];
                v[1] = src[1];
                v[2] = src[0];
                break;
        }
        if (invert_x) {
            v[0] = -v[0];
        }
        if (invert_y) {
            v[1] = -v[1];
        }
        if (invert_z) {
            v[2] = -v[2];
        }
    }
};
 
inline const uint8_t* axis_permutation_indices(const AxisPermutation p)
{
    static const uint8_t table[6][3] = {
        {0, 1, 2},  // XYZ
        {0, 2, 1},  // XZY
        {1, 0, 2},  // YXZ
        {1, 2, 0},  // YZX
        {2, 0, 1},  // ZXY
        {2, 1, 0},  // ZYX
    };
    return table[static_cast<uint8_t>(p)];
}
 
inline AxisPermutation permutation_from_indices(const uint8_t (&p)[3])
{
    const uint8_t code = static_cast<uint8_t>((p[0] << 4) | (p[1] << 2) | p[2]);
    switch (code) {
        case (0 << 4) | (1 << 2) | 2:
            return AxisPermutation::XYZ;
        case (0 << 4) | (2 << 2) | 1:
            return AxisPermutation::XZY;
        case (1 << 4) | (0 << 2) | 2:
            return AxisPermutation::YXZ;
        case (1 << 4) | (2 << 2) | 0:
            return AxisPermutation::YZX;
        case (2 << 4) | (0 << 2) | 1:
            return AxisPermutation::ZXY;
        case (2 << 4) | (1 << 2) | 0:
            return AxisPermutation::ZYX;
        default:
            return AxisPermutation::XYZ;
    }
}
 
inline AxisOrder compose(const AxisOrder& first, const AxisOrder& second)
{
    const uint8_t* pa   = axis_permutation_indices(first.permutation);
    const uint8_t* pb   = axis_permutation_indices(second.permutation);
    const bool inv_a[3] = {first.invert_x, first.invert_y, first.invert_z};
    const bool inv_b[3] = {second.invert_x, second.invert_y, second.invert_z};
 
    uint8_t pc[3]{};
    bool inv_c[3]{};
    for (int i = 0; i < 3; ++i) {
        pc[i]    = pa[pb[i]];
        inv_c[i] = inv_b[i] != inv_a[pb[i]];
    }
    AxisOrder result;
    result.permutation = permutation_from_indices(pc);
    result.invert_x    = inv_c[0];
    result.invert_y    = inv_c[1];
    result.invert_z    = inv_c[2];
    return result;
}
 
}  // namespace imu
}  // namespace m5
#endif