CameraControl.cpp

#include "CameraControl.h"

#include <cmath>

#include "LogBuffer.h"
#include "XPLMUtilities.h"
#include "XPLMGraphics.h"
#include "XPLMPlanes.h"

namespace
{
constexpr float kDegreesToRadians = 0.01745329251994329576f;

struct Quaternionf
{
    float w{1.0f};
    float x{0.0f};
    float y{0.0f};
    float z{0.0f};
};

struct Vector3f
{
    float x{0.0f};
    float y{0.0f};
    float z{0.0f};
};

Quaternionf Multiply(const Quaternionf &a, const Quaternionf &b)
{
    return {
        a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
        a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
        a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
        a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w};
}

Quaternionf Normalize(const Quaternionf &q)
{
    const float magnitude = std::sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    if (magnitude <= 0.0f)
    {
        return {};
    }
    const float inv = 1.0f / magnitude;
    return {q.w * inv, q.x * inv, q.y * inv, q.z * inv};
}

Quaternionf AxisAngle(float axisX, float axisY, float axisZ, float angleRadians)
{
    const float halfAngle = angleRadians * 0.5f;
    const float sinHalf = std::sin(halfAngle);
    return Normalize({
        std::cos(halfAngle),
        axisX * sinHalf,
        axisY * sinHalf,
        axisZ * sinHalf});
}

Quaternionf EulerToQuaternion(float pitchDeg, float headingDeg, float rollDeg)
{
    const Quaternionf headingQuat = AxisAngle(0.0f, 1.0f, 0.0f, headingDeg * kDegreesToRadians);
    const Quaternionf pitchQuat = AxisAngle(1.0f, 0.0f, 0.0f, pitchDeg * kDegreesToRadians);
    const Quaternionf rollQuat = AxisAngle(0.0f, 0.0f, 1.0f, rollDeg * kDegreesToRadians);
    return Multiply(headingQuat, Multiply(pitchQuat, rollQuat));
}

Vector3f RotateVector(const Quaternionf &q, const Vector3f &v)
{
    const Quaternionf vec{0.0f, v.x, v.y, v.z};
    const Quaternionf qConjugate{q.w, -q.x, -q.y, -q.z};
    const Quaternionf rotated = Multiply(Multiply(q, vec), qConjugate);
    return {rotated.x, rotated.y, rotated.z};
}
} // namespace

namespace vr_image_client
{

// Configure how camera targets should be interpreted relative to simulator datarefs.
CameraControl::CameraControl(bool use_pilot_orientation)
    : use_pilot_orientation_(use_pilot_orientation)
{
}

// Resolve the datarefs needed for camera offsets and pilot tracking.
bool CameraControl::InitializeDataRefs()
{
    WriteToPluginLog("[VR_for_Mac] InitializeDataRefs: Enter\n");
    
    fatal_error_ = false;
    datarefs_ready_ = false;

    // Load base-position datarefs when targets are expressed as offsets.
    local_x_ref_ = XPLMFindDataRef("sim/flightmodel/position/local_x");
    local_y_ref_ = XPLMFindDataRef("sim/flightmodel/position/local_y");
    local_z_ref_ = XPLMFindDataRef("sim/flightmodel/position/local_z");
    pitch_ref_ = XPLMFindDataRef("sim/flightmodel/position/theta");
    heading_ref_ = XPLMFindDataRef("sim/flightmodel/position/psi");
    roll_ref_ = XPLMFindDataRef("sim/flightmodel/position/phi");
    if (!local_x_ref_ || !local_y_ref_ || !local_z_ref_ || !pitch_ref_ || !heading_ref_ || !roll_ref_)
    {
        WriteToPluginLog("[VR_for_Mac] ERROR: Required camera datarefs unavailable; plugin disabled.\n");
        fatal_error_ = true;
        return false;
    }
    // Load pilot head tracking datarefs when following the VR pilot view.
    pilot_head_x_ref_ = XPLMFindDataRef("sim/graphics/view/pilots_head_x");
    pilot_head_y_ref_ = XPLMFindDataRef("sim/graphics/view/pilots_head_y");
    pilot_head_z_ref_ = XPLMFindDataRef("sim/graphics/view/pilots_head_z");
    // Load pilot head orientation datarefs (available even when not used for control).
    pilot_head_psi_ref_ = XPLMFindDataRef("sim/graphics/view/pilots_head_psi");
    pilot_head_the_ref_ = XPLMFindDataRef("sim/graphics/view/pilots_head_the");
    pilot_head_phi_ref_ = XPLMFindDataRef("sim/graphics/view/pilots_head_phi");

    const bool missing_position =
        pilot_head_x_ref_ == nullptr || pilot_head_y_ref_ == nullptr || pilot_head_z_ref_ == nullptr;
    const bool missing_orientation =
        use_pilot_orientation_ &&
        (pilot_head_psi_ref_ == nullptr || pilot_head_the_ref_ == nullptr || pilot_head_phi_ref_ == nullptr);
    if (missing_position || missing_orientation)
    {
        // Inform the operator when pilot tracking is only partially available.
        WriteToPluginLog("[VR_for_Mac] Warning: pilot head datarefs unavailable; pilot view following limited.\n");
    }

    groundspeed_ref_ = XPLMFindDataRef("sim/flightmodel/position/groundspeed");

    datarefs_ready_ = true;
    WriteToPluginLog("[VR_for_Mac] InitializeDataRefs: Complete\n");
    return true;
}

// Forget any previously captured baseline pose or target.
void CameraControl::ResetSession()
{
    base_pose_valid_ = false;
    has_last_applied_target_ = false;
}

void CameraControl::ClearPilotHeadPose()
{
    if (ShouldUsePilotPosition())
    {
        XPLMSetDataf(pilot_head_x_ref_, pilot_head_offset_[0]);
        XPLMSetDataf(pilot_head_y_ref_, pilot_head_offset_[1]);
        XPLMSetDataf(pilot_head_z_ref_, pilot_head_offset_[2]);
    }
    if (pilot_head_the_ref_ != nullptr && pilot_head_psi_ref_ != nullptr && pilot_head_phi_ref_ != nullptr)
    {
        XPLMSetDataf(pilot_head_the_ref_, 0.0f);
        XPLMSetDataf(pilot_head_psi_ref_, 0.0f);
        XPLMSetDataf(pilot_head_phi_ref_, 0.0f);
    }
}

// Capture the current camera pose to anchor subsequent offset-based targets.
void CameraControl::PrimeBasePose()
{
    CameraPose pose;
    if (UpdateBaseCameraPose(pose))
    {
        // Cache the baseline pose to reuse for offset targets.
        base_pose_ = pose;
        base_pose_valid_ = true;
    }
}

void CameraControl::SetPilotHeadBaselineOffset(const std::array<float, 3> &offset)
{
    pilot_head_offset_ = offset;
}

// Apply the target by writing directly to the pilot head offsets/orientation.
bool CameraControl::ApplyPilotHeadTarget(const CameraTarget &target)
{
    if (!IsOperational())
    {
        WriteToPluginLog("[VR_for_Mac]: ApplyPilotHeadTarget: not operational\n");
        return false;
    }

    if (!ShouldUsePilotPosition())
    {
        WriteToPluginLog("[VR_for_Mac] Pilot head position datarefs unavailable; cannot apply target.\n");
        return false;
    }

    XPLMSetDataf(pilot_head_x_ref_, target.position[0] + pilot_head_offset_[0]);
    XPLMSetDataf(pilot_head_y_ref_, target.position[1] + pilot_head_offset_[1]);
    XPLMSetDataf(pilot_head_z_ref_, target.position[2] + pilot_head_offset_[2]);

    if (pilot_head_the_ref_ != nullptr && pilot_head_psi_ref_ != nullptr && pilot_head_phi_ref_ != nullptr)
    {
        XPLMSetDataf(pilot_head_the_ref_, target.orientation[0]);
        XPLMSetDataf(pilot_head_psi_ref_, target.orientation[1]);
        XPLMSetDataf(pilot_head_phi_ref_, target.orientation[2]);
    }

    last_applied_target_ = target;
    has_last_applied_target_ = true;
    return true;
}

bool CameraControl::ApplyTarget(const CameraTarget &target, XPLMCameraPosition_t &out_camera)
{
    const bool applied = ApplyPilotHeadTarget(target);
    out_camera = {};
    return applied;
}

bool CameraControl::SampleBasePose(CameraPose &out_pose) const
{
    return UpdateBaseCameraPose(out_pose);
}

bool CameraControl::ReadRawData(
    CameraPose &aircraft_pose,
    std::array<float, 3> &pilot_position,
    bool &pilot_position_valid,
    std::array<float, 3> &pilot_orientation,
    bool &pilot_orientation_valid) const
{
    if (!IsOperational())
    {
        WriteToPluginLog("[VR_for_Mac]: ReadRawData: not operational\n");
        pilot_position_valid = false;
        pilot_orientation_valid = false;
        return false;
    }

    aircraft_pose.x = static_cast<float>(XPLMGetDatad(local_x_ref_));
    aircraft_pose.y = static_cast<float>(XPLMGetDatad(local_y_ref_));
    aircraft_pose.z = static_cast<float>(XPLMGetDatad(local_z_ref_));
    aircraft_pose.pitch = static_cast<float>(XPLMGetDataf(pitch_ref_));
    aircraft_pose.heading = static_cast<float>(XPLMGetDataf(heading_ref_));
    aircraft_pose.roll = static_cast<float>(XPLMGetDataf(roll_ref_));

    pilot_position_valid =
        pilot_head_x_ref_ != nullptr && pilot_head_y_ref_ != nullptr && pilot_head_z_ref_ != nullptr;
    if (pilot_position_valid)
    {
        pilot_position[0] = XPLMGetDataf(pilot_head_x_ref_);
        pilot_position[1] = XPLMGetDataf(pilot_head_y_ref_);
        pilot_position[2] = XPLMGetDataf(pilot_head_z_ref_);
    }
    else
    {
        pilot_position = {0.0f, 0.0f, 0.0f};
    }

    pilot_orientation_valid =
        pilot_head_the_ref_ != nullptr && pilot_head_psi_ref_ != nullptr && pilot_head_phi_ref_ != nullptr;
    if (pilot_orientation_valid)
    {
        pilot_orientation[0] = XPLMGetDataf(pilot_head_the_ref_);
        pilot_orientation[1] = XPLMGetDataf(pilot_head_psi_ref_);
        pilot_orientation[2] = XPLMGetDataf(pilot_head_phi_ref_);
    }
    else
    {
        pilot_orientation = {0.0f, 0.0f, 0.0f};
    }

    return true;
}

bool CameraControl::RotateAircraftHeading(float delta_heading_deg, float &out_new_heading)
{
    out_new_heading = 0.0f;
    if (!IsOperational())
    {
        WriteToPluginLog("[VR_for_Mac]: RotateAircraftHeading: not operational\n");
        return false;
    }

    CameraPose aircraft_pose{};
    if (!UpdateBaseCameraPose(aircraft_pose))
    {
        WriteToPluginLog("[VR_for_Mac]: RotateAircraftHeading: failed to read aircraft pose\n");
        return false;
    }

    double latitude = 0.0;
    double longitude = 0.0;
    double altitude = 0.0;
    XPLMLocalToWorld(
        static_cast<double>(aircraft_pose.x),
        static_cast<double>(aircraft_pose.y),
        static_cast<double>(aircraft_pose.z),
        &latitude,
        &longitude,
        &altitude);

    const float new_heading = NormalizeHeading(aircraft_pose.heading + delta_heading_deg);
    float speed = 0.0f;
    if (groundspeed_ref_ != nullptr)
    {
        speed = XPLMGetDataf(groundspeed_ref_);
    }

    XPLMPlaceUserAtLocation(latitude, longitude, static_cast<float>(altitude), new_heading, speed);

    base_pose_valid_ = false;
    has_last_applied_target_ = false;

    out_new_heading = new_heading;
    return true;
}

// Decide how many frames to wait before capturing after a camera move.

bool CameraControl::HasLastAppliedTarget() const
{
    // Signal whether we have previously applied a camera pose.
    return has_last_applied_target_;
}

// Update out_pose with the simulator camera position, respecting offsets and pilot view.
bool CameraControl::UpdateBaseCameraPose(CameraPose &out_pose) const
{
    if (!IsOperational())
    {
        WriteToPluginLog("[VR_for_Mac]: ApplyTarget: not operational\n");
        return false;
    }

    // Pull the aircraft pose from the detailed flight model datarefs.
    out_pose.x = static_cast<float>(XPLMGetDatad(local_x_ref_));
    out_pose.y = static_cast<float>(XPLMGetDatad(local_y_ref_));
    out_pose.z = static_cast<float>(XPLMGetDatad(local_z_ref_));
    out_pose.pitch = static_cast<float>(XPLMGetDataf(pitch_ref_));
    out_pose.heading = static_cast<float>(XPLMGetDataf(heading_ref_));
    out_pose.roll = static_cast<float>(XPLMGetDataf(roll_ref_));

    const Quaternionf aircraft_orientation =
        EulerToQuaternion(out_pose.pitch, out_pose.heading, out_pose.roll);

    if (ShouldUsePilotPosition())
    {
        // Offset by the tracked pilot head position when requested.
        const Vector3f pilot_local{
            XPLMGetDataf(pilot_head_x_ref_),
            XPLMGetDataf(pilot_head_y_ref_),
            XPLMGetDataf(pilot_head_z_ref_)};
        const Vector3f pilot_world = RotateVector(aircraft_orientation, pilot_local);
        out_pose.x += pilot_world.x;
        out_pose.y += pilot_world.y;
        out_pose.z += pilot_world.z;
    }
    if (ShouldUsePilotOrientation())
    {
        // Rotate according to the pilot head orientation when requested.
        out_pose.pitch += XPLMGetDataf(pilot_head_the_ref_);
        out_pose.heading = NormalizeHeading(out_pose.heading + XPLMGetDataf(pilot_head_psi_ref_));
        out_pose.roll += XPLMGetDataf(pilot_head_phi_ref_);
    }

    return true;
}

bool CameraControl::ShouldUsePilotPosition() const
{
    // Confirm that the positional datarefs are available before using them.
    return pilot_head_x_ref_ != nullptr &&
           pilot_head_y_ref_ != nullptr &&
           pilot_head_z_ref_ != nullptr;
}

bool CameraControl::ShouldUsePilotOrientation() const
{
    // Confirm that the orientation datarefs are available before using them.
    return use_pilot_orientation_ &&
           pilot_head_psi_ref_ != nullptr &&
           pilot_head_the_ref_ != nullptr &&
           pilot_head_phi_ref_ != nullptr;
}

bool CameraControl::IsOperational() const
{
    return datarefs_ready_ && !fatal_error_;
}

bool CameraControl::HasFatalError() const
{
    return fatal_error_;
}

} // namespace vr_image_client