PhysicalComponent.hpp

Go to the documentation of this file.

#pragma once

 
#include "icarus/core/Component.hpp"
#include "janus/math/Quaternion.hpp"
#include <cmath>
 
namespace icarus {

template <typename Scalar> class PhysicalComponent : public Component<Scalar> {
  protected:
    Vec3<Scalar> body_position_ = Vec3<Scalar>::Zero();

    janus::Quaternion<Scalar> body_orientation_;

    bool has_body_attachment_ = false;

    void ReadAttachmentFromConfig() {
        const auto &config = this->GetConfig();
 
        // Read position (default to origin)
        body_position_ = this->read_param_vec3("body_position", Vec3<Scalar>::Zero());
 
        // Check for Euler angles first (more user-friendly format)
        if (config.template Has<std::vector<double>>("body_orientation_euler_zyx")) {
            auto euler_deg = config.template Get<std::vector<double>>("body_orientation_euler_zyx",
                                                                      {0.0, 0.0, 0.0});
 
            if (euler_deg.size() != 3) {
                throw ConfigError(
                    "body_orientation_euler_zyx must have 3 elements [yaw, pitch, roll]");
            }
 
            // Convert degrees to radians
            constexpr double deg2rad = M_PI / 180.0;
            double yaw_rad = euler_deg[0] * deg2rad;
            double pitch_rad = euler_deg[1] * deg2rad;
            double roll_rad = euler_deg[2] * deg2rad;
 
            // Build quaternion from ZYX Euler angles
            // Note: Janus from_euler() takes (roll, pitch, yaw) order for ZYX sequence
            body_orientation_ = janus::Quaternion<Scalar>::from_euler(
                static_cast<Scalar>(roll_rad), static_cast<Scalar>(pitch_rad),
                static_cast<Scalar>(yaw_rad));
        } else if (config.template Has<std::vector<double>>("body_orientation")) {
            // Quaternion format: [w, x, y, z]
            auto q =
                config.template Get<std::vector<double>>("body_orientation", {1.0, 0.0, 0.0, 0.0});
 
            if (q.size() != 4) {
                throw ConfigError("body_orientation must have 4 elements [w, x, y, z]");
            }
 
            body_orientation_ = janus::Quaternion<Scalar>{
                static_cast<Scalar>(q[0]), // w
                static_cast<Scalar>(q[1]), // x
                static_cast<Scalar>(q[2]), // y
                static_cast<Scalar>(q[3])  // z
            };
 
            // Normalize to ensure unit quaternion
            body_orientation_ = body_orientation_.normalized();
        }
        // else: keep default identity orientation
 
        has_body_attachment_ = true;
    }

    void SetBodyAttachment(const Vec3<Scalar> &position,
                           const janus::Quaternion<Scalar> &orientation) {
        body_position_ = position;
        body_orientation_ = orientation;
        has_body_attachment_ = true;
    }

    void SetBodyPosition(const Vec3<Scalar> &position) {
        body_position_ = position;
        has_body_attachment_ = true;
    }
 
  public:
    // ===== Override Base Class Virtual Methods =====
 
    [[nodiscard]] bool HasBodyAttachment() const override { return has_body_attachment_; }
 
    [[nodiscard]] Vec3<Scalar> GetBodyPosition() const override { return body_position_; }
 
    [[nodiscard]] janus::Quaternion<Scalar> GetBodyOrientation() const override {
        return body_orientation_;
    }
 
    // ===== Additional Accessors =====

    [[nodiscard]] janus::Quaternion<Scalar> GetComponentToBodyRotation() const {
        return body_orientation_.conjugate();
    }

    [[nodiscard]] Vec3<Scalar> TransformToBodyFrame(const Vec3<Scalar> &v_local) const {
        return body_orientation_.conjugate().rotate(v_local);
    }

    [[nodiscard]] Vec3<Scalar> TransformToLocalFrame(const Vec3<Scalar> &v_body) const {
        return body_orientation_.rotate(v_body);
    }
};
 
} // namespace icarus