Quaternion.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "Quaternion.h"
 
#include <string>
#include <iostream>
#include <vector>
 
#include <SpiceUsr.h>
#include <SpiceZfc.h>
#include <SpiceZmc.h>
 
#include "IException.h"
#include "IString.h"
#include "NaifStatus.h"
 
namespace Isis {
  Quaternion::Quaternion() {
    p_quaternion.resize(4);
 
    for(int i = 0;  i < 4;  i++) {
      p_quaternion[i] = 0.;
    }
  }
 
  Quaternion::Quaternion(const std::vector<double> rotation) {
    p_quaternion.resize(4);
    Set(rotation);
 
  }
 
  Quaternion::~Quaternion() {}
 
  void Quaternion::Set(std::vector<double> rotation) {
 
    if(rotation.size() == 9) {        // Matrix initialization
      NaifStatus::CheckErrors();
      m2q_c(&rotation[0], &p_quaternion[0]);
      NaifStatus::CheckErrors();
    }
    else if(rotation.size() == 4) {   //quaternion initialization
      p_quaternion = rotation;
    }
    else {
      std::string msg = "Input vector of unexpected size for matrix or quaternion";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
  }
 
 
  std::vector<double> Quaternion::ToMatrix() {
    std::vector<double> matrix(9);
    q2m_c(&p_quaternion[0], (SpiceDouble( *)[3]) &matrix[0]);
    return matrix;
  }
 
 
  Quaternion &Quaternion::operator=(const Quaternion &quat) {
    p_quaternion = quat.p_quaternion;
    return *this;
  }
 
 
  Quaternion &Quaternion::operator*=(const Quaternion &quat) {
    std::vector<double> qout(4);
 
    qxq_c((SpiceDouble *) & (this->p_quaternion[0]),
          (SpiceDouble *) & (quat.p_quaternion[0]),
          (SpiceDouble *) & (qout[0]));
    this->p_quaternion[0] = qout[0];
    this->p_quaternion[1] = qout[1];
    this->p_quaternion[2] = qout[2];
    this->p_quaternion[3] = qout[3];
    return *this;
  }
 
 
  Quaternion Quaternion::operator*(const Quaternion &quat) const {
    Isis::Quaternion qout = *this;
    qout *= quat;
 
    return qout;
  }
 
 
 
  Quaternion Quaternion::operator*(const double &scalar) {
    Isis::Quaternion qout = *this;
 
    double scalar2 = scalar * scalar;
    double unitizer =  1 + qout.p_quaternion[0] * qout.p_quaternion[0] * (scalar2 - 1);
 
    if(unitizer > 0.) {
      unitizer  =  sqrt(unitizer);
    }
    else {
      std::string msg = "Unable to make quaternion a unit quaternion";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    qout.p_quaternion[0] = (qout.p_quaternion[0] * scalar) / unitizer;
 
    for(int i = 1;  i < 4;  i++) {
      qout.p_quaternion[i] /= unitizer;
    }
    Polish(qout);
 
    return qout;
  }
 
  Quaternion Quaternion::Conjugate() {
    Quaternion qout;
    qout.p_quaternion[0] = p_quaternion[0];
 
    for(int i = 1;  i < 4;  i++) {
      qout.p_quaternion[i] = -p_quaternion[i];
    }
    return qout;
  }
 
 
 
  std::vector<double> Quaternion::Qxv(const std::vector<double> &vin) {
    if(vin.size() != 3) {
      std::string msg = "Unexpected vector size -- 3 expected";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    Quaternion qvin;
    qvin.p_quaternion[0] = 0.;
    qvin.p_quaternion[1] = vin[0];
    qvin.p_quaternion[2] = vin[1];
    qvin.p_quaternion[3] = vin[2];
 
    Quaternion qvout(p_quaternion);
    Quaternion conj(p_quaternion);
    qvout *= qvin;
    qvout *= conj.Conjugate();
    std::vector<double> vout(qvout.p_quaternion.begin() + 1, qvout.p_quaternion.end());
 
    return vout;
  }
 
 
  void Quaternion::Polish(Quaternion &quat) {
 
    if(quat.p_quaternion[0] < 0) {
      quat.p_quaternion[0] = -quat.p_quaternion[0];
      quat.p_quaternion[1] = -quat.p_quaternion[1];
      quat.p_quaternion[2] = -quat.p_quaternion[2];
      quat.p_quaternion[3] = -quat.p_quaternion[3];
    }
 
  }
 
 
 
  std::vector<double> Quaternion::ToAngles(int axis3, int axis2, int axis1) {
    std::vector<double> rotationMatrix = ToMatrix();
    SpiceDouble ang1, ang2, ang3;
    NaifStatus::CheckErrors();
    m2eul_c((SpiceDouble *) &rotationMatrix[0], axis3, axis2, axis1,
            &ang3, &ang2, &ang1);
    NaifStatus::CheckErrors();
    std::vector<double> angles;
    angles.push_back(ang1);
    angles.push_back(ang2);
    angles.push_back(ang3);
    return angles;
  }
 
}
// end namespace isis