cu2qu.cpp

#include <stan/math.hpp>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>

#include "util.hpp"
#include "polybasis.hpp"
#include "mechanics.hpp"
#include "stan_mech.hpp"

using namespace Eigen;
using namespace stan::math;

double tol = 1e-5;
double fdtol = 1e-3;

int main() {
  double delta = 0.000001;

  //Matrix<var, Dynamic, Dynamic> C = rus_namespace::mech_rotate_cu(C_, cu, NULL);

  //std::cout << C << std::endl << std::endl;

  Matrix<var, Dynamic, 1> ax(4);

  ax << 0.5, 0.3, 0.812403840463596, 0.7;
  
  auto quc = rus_namespace::ax2qu(ax);

  Matrix<var, Dynamic, 1> quref(4);

  quref << 0.9393727128473789,
    0.17144890372772567,
    0.1028693422366354,
    0.2785714956633554;

  for(int i = 0; i < 4; i++) {
    if(abs((quc(i) - quref(i)).val()) > tol) {
      std::cout << "Error in ax2qu: " << quc(i) << " " << quref(i) << std::endl;
      break;
    }
  }

  Matrix<var, Dynamic, 1> cu(3);

  //cu << 0.80057441, 0.66424955, 1.02499762;
  cu << -1.0, 0.347377, 0.83077;

  auto hoc = rus_namespace::cu2ho(cu);

  Matrix<var, Dynamic, 1> horef(3);

  //horef << 0.63906110825522655, 0.51124888660418488, 0.97338888025074799;
  horef << -0.9690716716510636, 0.27570153251968205, 0.7240356941520346;

  for(int i = 0; i < 3; i++) {
    if(abs((hoc(i) - horef(i)).val()) > tol) {
      std::cout << "Error in cu2ho: " << hoc(i) << " " << horef(i) << std::endl;
    }
  }

  Matrix<var, Dynamic, 1> ho(3);

  //ho << 0.63906110825522655, 0.51124888660418488, 0.97338888025074799;
  ho = hoc;
  
  auto axc = rus_namespace::ho2ax(ho);

  Matrix<var, Dynamic, 1> axref(4);

  axref << 0.50251890762960527, 0.40201512610368711, 0.76541399638273189, 2.941257811266673;
  axref << -0.7810678687834022, 0.22221432606586872, 0.5835698566969045, 2.8418006158281055;
  
  for(int i = 0; i < 4; i++) {
    if(abs((axc(i) - axref(i)).val()) > tol) {
      std::cout << "Error in ho2ax: " << axc(i) << " " << axref(i) << std::endl;
    }
  }

  Matrix<var, 10, 3> cus;
  cus << 0.0365739622831, -0.226290098197, -0.329154036209,
    0.487541525906, -0.791856937552, 0.215963714314,
    0.267899994218, -1.01593071206, -0.565162139556,
    0.844629849906, 0.961752566299, -0.344844829132,
    0.450985978658, -0.87604789862, -0.858579930772,
    0.959395587842, 0.415761604021, 1.0212149277,
    -0.0688902000762, 0.273645380922, -0.684056219264,
    -0.97085511876, -0.566495917975, 0.569062450152,
    0.501795139967, -0.362665343543, -0.0646142866082,
    0.847151187924, -0.818412438273, -0.110337669404;

  Matrix<var, 10, 4> qus;
  qus << 0.915897944685, 0.0293842205532, -0.202837753064, -0.345143982181,
    0.489637936311, 0.38596954052, -0.766112552333, 0.15605691998,
    0.118141100152, 0.172546552538, -0.893427768873, -0.39756406964,
    0.220876704473, 0.598228855031, 0.737529394354, -0.22222986049,
    0.365701781465, 0.296306301105, -0.634882888805, -0.612689562879,
    0.107603217264, 0.643122604223, 0.254159536936, 0.714295312428,
    0.625140923349, -0.0529439719051, 0.23370663484, -0.742816915952,
    0.204263962676, -0.83713017722, -0.357804972575, 0.359812314324,
    0.802193502486, 0.504323858044, -0.315594787781, -0.0504277770851,
    0.410161528794, 0.660525215454, -0.62433298446, -0.0753809297026;
  
  //0.80057441, 0.66424955, 1.02499762;
  //cu << -1.0, 0.347377, 0.83077;

  for(int k = 0; k < cus.rows(); k++) {
    cu = cus.row(k);
    quc = rus_namespace::cu2qu(cu, NULL);

    quref = qus.row(k);
    
    for(int i = 0; i < 4; i++) {
      if(abs((quc(i) - quref(i)).val()) > tol) {
        std::cout << "Error in cu2qu: " << quc(i) << " " << quref(i) << std::endl;
      }
    }
  }
}