transform.h

Go to the documentation of this file.

#pragma once
#ifdef WEBCFACE_MESON
#include "webcface-config.h"
#else
#include "webcface/common/webcface-config.h"
#endif
#include "array_like.h"
#include <array>
#include <optional>
#include <cassert>
 
WEBCFACE_NS_BEGIN
 
class Transform;
 
class Point {
  protected:
    std::array<double, 3> pos_;
 
  public:
    Point(const std::array<double, 3> &pos = {0, 0, 0}) : pos_(pos) {}
    Point(const std::array<double, 2> &pos) : pos_({pos[0], pos[1], 0}) {}
    Point(double x, double y, double z = 0)
        : Point(std::array<double, 3>{x, y, z}) {}
    template <typename R,
              typename traits::ArrayLikeTrait<R>::ArrayLike = traits::TraitOk,
              typename traits::ArraySizeTrait<R, 3>::SizeMatchOrDynamic =
                  traits::TraitOk>
    Point(const R &pos) : pos_(traits::arrayLikeToArray<3>(pos)) {}
 
    std::array<double, 3> pos() const { return pos_; }
    std::array<double, 3> &pos() { return pos_; }
    double pos(std::size_t index) const { return pos_.at(index); }
    double &pos(std::size_t i) { return pos_.at(i); }
 
    Point operator+(const Point &other) const {
        return {
            pos_[0] + other.pos_[0],
            pos_[1] + other.pos_[1],
            pos_[2] + other.pos_[2],
        };
    }
    Point operator-() const { return {-pos_[0], -pos_[1], -pos_[2]}; }
    Point operator+() const { return *this; }
    Point operator-(const Point &other) const { return *this + (-other); }
    Point &operator+=(const Point &other) {
        pos_[0] += other.pos_[0];
        pos_[1] += other.pos_[1];
        pos_[2] += other.pos_[2];
        return *this;
    }
    Point &operator-=(const Point &other) { return *this += (-other); }
 
    Point operator*(double scalar) const {
        return {pos_[0] * scalar, pos_[1] * scalar, pos_[2] * scalar};
    }
    friend Point operator*(double scalar, const Point &point) {
        return point * scalar;
    }
    Point &operator*=(double scalar) {
        pos_[0] *= scalar;
        pos_[1] *= scalar;
        pos_[2] *= scalar;
        return *this;
    }
    Point operator/(double scalar) const {
        return {pos_[0] / scalar, pos_[1] / scalar, pos_[2] / scalar};
    }
    Point &operator/=(double scalar) {
        pos_[0] /= scalar;
        pos_[1] /= scalar;
        pos_[2] /= scalar;
        return *this;
    }
 
    WEBCFACE_DLL bool operator==(const Point &other) const;
    bool operator!=(const Point &other) const { return !(*this == other); }
 
    bool operator==(const Transform &) = delete;
    bool operator!=(const Transform &) = delete;
};
 
enum class AxisSequence {
    ZXZ,
    XYX,
    YZY,
    ZYZ,
    XZX,
    YXY,
    XYZ,
    YZX,
    ZXY,
    XZY,
    ZYX,
    YXZ,
};
 
struct AxisAngle {
    std::array<double, 3> axis;
    double angle;
    AxisAngle(const std::array<double, 3> &axis, double angle) noexcept
        : axis(axis), angle(angle) {}
 
    [[deprecated]]
    operator std::pair<std::array<double, 3>, double>() const noexcept {
        return {axis, angle};
    }
};
template <std::size_t I>
const auto &get(const AxisAngle &aa) {
    static_assert(I < 2);
    if constexpr (I == 0) {
        return aa.axis;
    } else {
        return aa.angle;
    }
}
 
class Rotation {
  protected:
    // rot_とrmat_のどちらかには必ず値が入っている。
    // 両方に値が入っているなら両方とも同じ回転を表現していなければならない。
    mutable std::optional<std::array<double, 3>> rot_;
    mutable std::optional<std::array<std::array<double, 3>, 3>> rmat_;
 
    Rotation(const std::optional<std::array<double, 3>> &rot,
             const std::optional<std::array<std::array<double, 3>, 3>> &rmat)
        : rot_(rot), rmat_(rmat) {}
 
  public:
    friend class Transform;
 
    static WEBCFACE_DLL std::array<std::array<double, 3>, 3>
    eulerToMatrix(const std::array<double, 3> &rot, AxisSequence axis);
    static WEBCFACE_DLL std::array<double, 3>
    matrixToEuler(const std::array<std::array<double, 3>, 3> &rmat,
                  AxisSequence axis);
 
    static WEBCFACE_DLL std::array<std::array<double, 3>, 3>
    quaternionToMatrix(const std::array<double, 4> &quat);
    static WEBCFACE_DLL std::array<double, 4>
    matrixToQuaternion(const std::array<std::array<double, 3>, 3> &rmat);
 
    static WEBCFACE_DLL std::array<double, 4>
    axisAngleToQuaternion(const std::array<double, 3> &axis, double angle);
    static WEBCFACE_DLL AxisAngle
    quaternionToAxisAngle(const std::array<double, 4> &quat);
 
    Rotation()
        : rot_({0, 0, 0}), rmat_({{
                               {{1, 0, 0}},
                               {{0, 1, 0}},
                               {{0, 0, 1}},
                           }}) {}
 
    friend Rotation rotFromEuler(const std::array<double, 3> &rot,
                                 AxisSequence axis);
    friend Rotation
    rotFromMatrix(const std::array<std::array<double, 3>, 3> &matrix);
 
    [[deprecated("use rotEuler() or rot2D()")]]
    std::array<double, 3> rot() const {
        return rotEuler();
    }
 
    [[deprecated("use rotEuler() or rot2D()")]]
    double rot(std::size_t index) const {
        return rotEuler().at(index);
    }
    [[deprecated("use rotEuler() or rot2D()")]]
    std::array<double, 3> &rot() {
        rotEuler();
        rmat_ = std::nullopt;
        return *rot_;
    }
    [[deprecated("use rotEuler() or rot2D()")]]
    double &rot(std::size_t index) {
        rotEuler();
        rmat_ = std::nullopt;
        return rot_->at(index);
    }
 
    double rot2D() const { return rotEuler()[0]; }
    std::array<double, 3>
    rotEuler(AxisSequence axis = AxisSequence::ZYX) const {
        if (axis == AxisSequence::ZYX) {
            if (!rot_) {
                assert(rmat_);
                rot_.emplace(matrixToEuler(*rmat_, AxisSequence::ZYX));
            }
            return *rot_;
        } else {
            return matrixToEuler(rotMatrix(), axis);
        }
    }
 
    const std::array<std::array<double, 3>, 3> &rotMatrix() const {
        if (!rmat_) {
            assert(rot_);
            rmat_.emplace(eulerToMatrix(*rot_, AxisSequence::ZYX));
        }
        return *rmat_;
    }
    double rotMatrix(std::size_t row, std::size_t col) const {
        return rotMatrix().at(row).at(col);
    }
 
    std::array<double, 4> rotQuat() const {
        return matrixToQuaternion(rotMatrix());
    }
    AxisAngle rotAxisAngle() const { return quaternionToAxisAngle(rotQuat()); }
 
    WEBCFACE_DLL Transform appliedTo(const Transform &target) const;
    WEBCFACE_DLL Transform operator*(const Transform &target) const;
    WEBCFACE_DLL Rotation appliedTo(const Rotation &target) const;
    WEBCFACE_DLL Rotation operator*(const Rotation &target) const;
    WEBCFACE_DLL Point appliedTo(const Point &target) const;
    WEBCFACE_DLL Point operator*(const Point &target) const;
 
    WEBCFACE_DLL Rotation inversed() const;
 
    Rotation &operator*=(const Rotation &target) {
        *this = *this * target;
        return *this;
    }
 
    WEBCFACE_DLL bool operator==(const Rotation &other) const;
    bool operator!=(const Rotation &other) const { return !(*this == other); }
 
    WEBCFACE_DLL bool operator==(const Transform &other) const;
    bool operator!=(const Transform &other) const { return !(*this == other); }
};
 
inline Rotation rotFromEuler(const std::array<double, 3> &rot,
                             AxisSequence axis = AxisSequence::ZYX) {
    if (axis == AxisSequence::ZYX) {
        return {rot, std::nullopt};
    } else {
        return {std::nullopt, Rotation::eulerToMatrix(rot, axis)};
    }
}
template <
    typename R, typename traits::ArrayLikeTrait<R>::ArrayLike = traits::TraitOk,
    typename traits::ArraySizeTrait<R, 3>::SizeMatchOrDynamic = traits::TraitOk>
inline Rotation rotFromEuler(const R &rot,
                             AxisSequence axis = AxisSequence::ZYX) {
    return rotFromEuler(traits::arrayLikeToArray<3>(rot), axis);
}
inline Rotation rotFromEuler(double angle1, double angle2, double angle3,
                             AxisSequence axis = AxisSequence::ZYX) {
    return rotFromEuler(std::array<double, 3>{angle1, angle2, angle3}, axis);
}
inline Rotation
rotFromMatrix(const std::array<std::array<double, 3>, 3> &matrix) {
    return {std::nullopt, matrix};
}
inline Rotation rotFromQuat(const std::array<double, 4> &quat) {
    return rotFromMatrix(Rotation::quaternionToMatrix(quat));
}
template <
    typename R, typename traits::ArrayLikeTrait<R>::ArrayLike = traits::TraitOk,
    typename traits::ArraySizeTrait<R, 4>::SizeMatchOrDynamic = traits::TraitOk>
inline Rotation rotFromQuat(const R &quat) {
    return rotFromQuat(traits::arrayLikeToArray<4>(quat));
}
inline Rotation rotFromQuat(double w, double x, double y, double z) {
    return rotFromQuat({w, x, y, z});
}
inline Rotation rotFromAxisAngle(const std::array<double, 3> &axis,
                                 double angle) {
    return rotFromQuat(Rotation::axisAngleToQuaternion(axis, angle));
}
template <
    typename R, typename traits::ArrayLikeTrait<R>::ArrayLike = traits::TraitOk,
    typename traits::ArraySizeTrait<R, 3>::SizeMatchOrDynamic = traits::TraitOk>
inline Rotation rotFromAxisAngle(const R &axis, double angle) {
    return rotFromAxisAngle(traits::arrayLikeToArray<3>(axis), angle);
}
 
inline Rotation rot2D(double rot) { return rotFromEuler(rot, 0, 0); }
inline Rotation rotX(double rot) { return rotFromEuler(0, 0, rot); }
inline Rotation rotY(double rot) { return rotFromEuler(0, rot, 0); }
inline Rotation rotZ(double rot) { return rotFromEuler(rot, 0, 0); }
 
class Transform : private Point, private Rotation {
  public:
    Transform() : Point(), Rotation() {}
    Transform(const Point &pos, const Rotation &rot)
        : Point(pos), Rotation(rot) {}
 
    Transform(const Rotation &rot) : Point(), Rotation(rot) {}
 
    [[deprecated("use Transform(pos, rotEuler(rot))")]]
    Transform(const Point &pos, const std::array<double, 3> &rot)
        : Transform(pos, Rotation{rot, std::nullopt}) {}
 
    Transform(const Point &pos, double rot)
        : Transform(pos, webcface::rot2D(rot)) {};
    [[deprecated("use Transform({x, y, z}, rotEuler(z, y, x))")]]
    Transform(double x, double y, double z, double z_angle, double y_angle,
              double x_angle)
        : Transform(std::array<double, 3>{x, y, z},
                    Rotation{std::array<double, 3>{z_angle, y_angle, x_angle},
                             std::nullopt}) {}
    [[deprecated("use translation(x, y) for translation-only transform")]]
    Transform(double x, double y)
        : Transform({x, y}, 0) {}
 
    friend class Point;
    friend class Rotation;
    using Point::pos;
    using Rotation::rot;
    using Rotation::rot2D;
    using Rotation::rotAxisAngle;
    using Rotation::rotEuler;
    using Rotation::rotMatrix;
    using Rotation::rotQuat;
 
    WEBCFACE_DLL Transform appliedTo(const Transform &target) const;
    Transform operator*(const Transform &target) const {
        return this->appliedTo(target);
    }
    Transform appliedTo(const Rotation &target) const {
        return this->appliedTo(Transform(target));
    }
    Transform operator*(const Rotation &target) const {
        return this->appliedTo(target);
    }
    Transform &operator*=(const Transform &target) {
        *this = *this * target;
        return *this;
    }
 
    WEBCFACE_DLL Point appliedTo(const Point &target) const;
    Point operator*(const Point &target) const {
        return this->appliedTo(target);
    }
 
    WEBCFACE_DLL Transform inversed() const;
 
    bool operator==(const Transform &other) const {
        return Point(*this) == Point(other) &&
               Rotation(*this) == Rotation(other);
    }
    bool operator!=(const Transform &other) const { return !(*this == other); }
 
    bool operator==(const Rotation &other) const {
        return *this == Transform(other);
    }
    bool operator!=(const Rotation &other) const { return !(*this == other); }
};
 
inline Transform identity() { return {}; }
inline Transform translation(const Point &pos) { return {pos, Rotation{}}; }
inline Transform translation(double x, double y, double z) {
    return {{x, y, z}, Rotation{}};
}
inline Transform translation(double x, double y) { return {{x, y}, 0}; }
 
WEBCFACE_NS_END