quaternion.H

Go to the documentation of this file.

#ifndef VS_QUATERNION_H
#define VS_QUATERNION_H
 
#include "src/core/vs/tensor.H"
#include "src/utilities/trig_ops.H"
 
#include <cmath>
 
#include "AMReX.H"
 
namespace kynema_sgf::vs {
 
using namespace amrex::literals;

struct Quaternion
{
    amrex::Real w{1.0_rt};
    amrex::Real x{0.0_rt};
    amrex::Real y{0.0_rt};
    amrex::Real z{0.0_rt};

    Quaternion& normalize();

    [[nodiscard]] Quaternion normalized() const;

    [[nodiscard]] Quaternion conjugate() const;
};
 
amrex::Real dot(const Quaternion& a, const Quaternion& b);

Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs);
 
AMREX_GPU_HOST_DEVICE AMREX_FORCE_INLINE Tensor tensor_unit(const Quaternion& q)
{
    return {
        1.0_rt - (2.0_rt * ((q.y * q.y) + (q.z * q.z))),
        2.0_rt * ((q.x * q.y) - (q.z * q.w)),
        2.0_rt * ((q.x * q.z) + (q.y * q.w)),
        2.0_rt * ((q.x * q.y) + (q.z * q.w)),
        1.0_rt - (2.0_rt * ((q.x * q.x) + (q.z * q.z))),
        2.0_rt * ((q.y * q.z) - (q.x * q.w)),
        2.0_rt * ((q.x * q.z) - (q.y * q.w)),
        2.0_rt * ((q.y * q.z) + (q.x * q.w)),
        1.0_rt - (2.0_rt * ((q.x * q.x) + (q.y * q.y)))};
}
 
Tensor tensor(const Quaternion& input);
 
AMREX_GPU_HOST_DEVICE AMREX_FORCE_INLINE Quaternion
from_axis_angle(const Vector& axis, const amrex::Real angle_degrees)
{
    const amrex::Real half_angle = -0.5_rt * utils::radians(angle_degrees);
    const amrex::Real axis_magnitude = mag(axis);
    const amrex::Real scale = std::sin(half_angle) / axis_magnitude;
    return {
        .w = std::cos(half_angle),
        .x = scale * axis.x(),
        .y = scale * axis.y(),
        .z = scale * axis.z()};
}

AMREX_GPU_HOST_DEVICE AMREX_FORCE_INLINE Tensor
quaternion(const Vector& axis, const amrex::Real angle_degrees)
{
    // Preserve the original operation sequence used by existing actuator and
    // sampling models. Although tensor_unit(from_axis_angle(...)) is
    // algebraically equivalent, it changes floating-point rounding and
    // therefore existing regression results.
    const amrex::Real angle = -1.0_rt * utils::radians(angle_degrees);
    const amrex::Real cval = std::cos(0.5_rt * angle);
    const amrex::Real sval = std::sin(0.5_rt * angle);
    const auto axis_magnitude = mag(axis);
    const amrex::Real q0 = cval;
    const amrex::Real q1 = sval * axis.x() / axis_magnitude;
    const amrex::Real q2 = sval * axis.y() / axis_magnitude;
    const amrex::Real q3 = sval * axis.z() / axis_magnitude;
 
    Tensor result;
    result.xx() = (q0 * q0) + (q1 * q1) - (q2 * q2) - (q3 * q3);
    result.xy() = 2.0_rt * ((q1 * q2) - (q0 * q3));
    result.xz() = 2.0_rt * ((q0 * q2) + (q1 * q3));
 
    result.yx() = 2.0_rt * ((q1 * q2) + (q0 * q3));
    result.yy() = (q0 * q0) - (q1 * q1) + (q2 * q2) - (q3 * q3);
    result.yz() = 2.0_rt * ((q2 * q3) - (q0 * q1));
 
    result.zx() = 2.0_rt * ((q1 * q3) - (q0 * q2));
    result.zy() = 2.0_rt * ((q0 * q1) + (q2 * q3));
    result.zz() = (q0 * q0) - (q1 * q1) - (q2 * q2) + (q3 * q3);
 
    return result;
}

Quaternion from_roll_pitch_yaw(const Vector& angles);
 
Quaternion slerp(Quaternion a, Quaternion b, amrex::Real fraction);
 
} // namespace kynema_sgf::vs
 
#endif