quaternionStateData.cpp

/*
 ISC License

 Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder

 Permission to use, copy, modify, and/or distribute this software for any
 purpose with or without fee is hereby granted, provided that the above
 copyright notice and this permission notice appear in all copies.

 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

 */

#include "quaternionStateData.h"

#include <cmath>
#include <stdexcept>

QuaternionStateData::QuaternionStateData(std::string inName,
                                         const Eigen::MatrixXd& newState)
    : StateData(std::move(inName), newState)
{
    // state holds (w,x,y,z); deriv is the body angular velocity (3x1).
    this->state.resize(4, 1);
    this->state.setZero();
    this->state(0) = 1.0;  // identity quaternion
    this->stateDeriv.resize(3, 1);
    this->stateDeriv.setZero();
}

std::unique_ptr<StateData> QuaternionStateData::clone() const
{
    return std::unique_ptr<StateData>();
}

void QuaternionStateData::propagateState(double dt, std::vector<double> pseudoStep)
{
    // Equivalent to mju_quatIntegrate: q <- q * exp(0.5 * dt * omega), normalize.
    double wx = this->stateDeriv(0) * dt;
    double wy = this->stateDeriv(1) * dt;
    double wz = this->stateDeriv(2) * dt;
    double angle = std::sqrt(wx * wx + wy * wy + wz * wz);

    double dq_w, dq_x, dq_y, dq_z;
    if (angle < 1e-12) {
        // Small-angle limit avoids division by zero; sin(a/2)/a -> 1/2.
        dq_w = 1.0;
        dq_x = 0.5 * wx;
        dq_y = 0.5 * wy;
        dq_z = 0.5 * wz;
    } else {
        double half  = 0.5 * angle;
        double scale = std::sin(half) / angle;
        dq_w = std::cos(half);
        dq_x = scale * wx;
        dq_y = scale * wy;
        dq_z = scale * wz;
    }

    double qw = this->state(0);
    double qx = this->state(1);
    double qy = this->state(2);
    double qz = this->state(3);

    // Hamilton product q' = q ⊗ dq (body-frame angular velocity convention).
    double nw = qw * dq_w - qx * dq_x - qy * dq_y - qz * dq_z;
    double nx = qw * dq_x + qx * dq_w + qy * dq_z - qz * dq_y;
    double ny = qw * dq_y - qx * dq_z + qy * dq_w + qz * dq_x;
    double nz = qw * dq_z + qx * dq_y - qy * dq_x + qz * dq_w;

    double norm = std::sqrt(nw * nw + nx * nx + ny * ny + nz * nz);
    if (norm > 0.0) {
        double inv = 1.0 / norm;
        nw *= inv; nx *= inv; ny *= inv; nz *= inv;
    } else {
        nw = 1.0; nx = 0.0; ny = 0.0; nz = 0.0;
    }

    this->state(0) = nw;
    this->state(1) = nx;
    this->state(2) = ny;
    this->state(3) = nz;

    if (getNumNoiseSources() > 0 && pseudoStep.size() == 0) {
        auto errorMsg = "State " + this->getName() + " has stochastic dynamics, but "
            + "the integrator tried to propagate it without pseudoSteps. Are you sure "
            + "you are using a stochastic integrator?";
        bskLogger.bskLog(BSK_ERROR, "%s", errorMsg.c_str());
        throw std::invalid_argument(errorMsg);
    }
}