CatUtil.h

#pragma once

#include <cmath>
#include <string>
#include <iostream>

#pragma region Constants

#define EXECUTION_FREQ__HZ  120
#define EXECUTION_TIME__SEC (1.0/EXECUTION_FREQ__HZ)
//#define USE_SPACE_MOUSE

#pragma endregion

#define print_err(libName, message) std::cerr << "[ERROR] " << libName << "(" << __LINE__ << "): " << message << std::endl;
#define print_msg(libName, message) std::cerr << "[INFO] "  << libName << ": " << message << std::endl;

typedef 
enum CAT_RESULT
    {
        CAT_RESULT_OK    = 0,
        CAT_RESULT_ERROR = ( CAT_RESULT_OK + 1 ), 
        CAT_RESULT_FILE_NOT_FOUND = ( CAT_RESULT_OK + 2 ), 
        CAT_RESULT_ABORT = ( CAT_RESULT_OK + 3 )
    }   CAT_RESULT;

inline void invertTransform(double inM[16], double outM[16])
{
        //translation from in matrix
        double x = inM[12];
        double y = inM[13];
        double z = inM[14];


        //transpose rotation matrix and insert

        outM[0] = inM[0];
        outM[4] = inM[1];
        outM[8] = inM[2];
        //0
        outM[1] = inM[4];
        outM[5] = inM[5];
        outM[9] = inM[6];
        //0
        outM[2] = inM[8];
        outM[6] = inM[9];
        outM[10] = inM[10];
        //0

        //calculate inv rotation * translation as translation component
        outM[12] = -(outM[0] * x + outM[1] * y + outM[2] * z);
        outM[13] = -(outM[4] * x + outM[5] * y + outM[6] * z);
        outM[14] = -(outM[8] * x + outM[9] * y + outM[10] * z);

        //set homogenous values
        outM[3] = 0;
        outM[7] = 0;
        outM[11] = 0;
        outM[15] = 1;
}


inline void multiplyTransforms(double l[16], double r[16], double outM[16])
{
    outM[0]  = l[0]*r[0]  + l[1]*r[4]  + l[2]*r[8]   + l[12]*r[3];
    outM[1]  = l[0]*r[1]  + l[1]*r[5]  + l[2]*r[9]   + l[12]*r[7];
    outM[2]  = l[0]*r[2]  + l[1]*r[6]  + l[2]*r[10]  + l[12]*r[11];
    outM[12] = l[0]*r[12] + l[1]*r[13] + l[2]*r[14]  + l[12]*r[15];
    outM[4]  = l[4]*r[0]  + l[5]*r[4]  + l[6]*r[8]   + l[13]*r[3];
    outM[5]  = l[4]*r[1]  + l[5]*r[5]  + l[6]*r[9]   + l[13]*r[7];
    outM[6]  = l[4]*r[2]  + l[5]*r[6]  + l[6]*r[10]  + l[13]*r[11];
    outM[13] = l[4]*r[12] + l[5]*r[13] + l[6]*r[14]  + l[13]*r[15];
    outM[8]  = l[8]*r[0]  + l[9]*r[4]  + l[10]*r[8]  + l[14]*r[3];
    outM[9]  = l[8]*r[1]  + l[9]*r[5]  + l[10]*r[9]  + l[14]*r[7];
    outM[10] = l[8]*r[2]  + l[9]*r[6]  + l[10]*r[10] + l[14]*r[11];
    outM[14] = l[8]*r[12] + l[9]*r[13] + l[10]*r[14] + l[14]*r[15];
    outM[3]  = l[3]*r[0]  + l[7]*r[4]  + l[11]*r[8]  + l[15]*r[3];
    outM[7]  = l[3]*r[1]  + l[7]*r[5]  + l[11]*r[9]  + l[15]*r[7];
    outM[11] = l[3]*r[2]  + l[7]*r[6]  + l[11]*r[10] + l[15]*r[11];
    outM[15] = l[3]*r[12] + l[7]*r[13] + l[11]*r[14] + l[15]*r[15]; 
}


// Makes a 3 X 4 into a homogenous 4x4
//
//  if the input matrix is:
//
//  0 1 2| 9           
//  3 4 5| 10    indices 9, 10, 11 are translation
//  5 6 7| 11    0 - 7 is row major rotation matrix
//
// the output matrix becomes:
//
// 0  1  2| 12
// 4  5  6| 13    indices 3, 7, 11 are set to 0
// 8  9 10| 14    indices 12, 13, 14 are translation
// 3  7 11  15
inline void extendTransform(double inM[12], double outM[16])
{
    int col =-1;
    for(int i = 0; i < 12; i++)
    {
        if(i % 3 == 0)
        {
            col+=1;
        }
        outM[i+col] = inM[i];
    }

    //set homogenous values
    outM[3] = 0;
    outM[7] = 0;
    outM[11] = 0;
    outM[15] = 1;
}

inline void transposeRotation(double inM[12], double outM[12])
{
    outM[0] = inM[0];
    outM[1] = inM[3];
    outM[2] = inM[6];
    outM[3] = inM[1];
    outM[4] = inM[4];
    outM[5] = inM[7];
    outM[6] = inM[2];
    outM[7] = inM[5];
    outM[8] = inM[8];
    outM[9] = inM[9];
    outM[10] = inM[10];
    outM[11] = inM[11];
}
        
inline void transposeRotation4x4(double inM[16], double outM[16])
{
    outM[0] = inM[0];
    outM[1] = inM[4];
    outM[2] = inM[8];
        
    outM[4] = inM[1];
    outM[5] = inM[5];
    outM[6] = inM[9];
        
    outM[8] = inM[2];
    outM[9] = inM[6];
    outM[10] = inM[10];

    //this part of the matrix should be zero
    outM[3] = inM[11];
    outM[7] = inM[7];
    outM[11] = inM[3];

    //keep translation the same
    outM[12] = inM[12];
    outM[13] = inM[13];
    outM[14] = inM[14];
    outM[15] = inM[15];
}

//makes a 4x4 homogenous transform into a 3x4 row major matrix
//
//  if the input matrix is:
//
// 0  1  2| 12
// 4  5  6| 13    where 3 7 11 are 0
// 8  9 10| 14    0 - 10 is row major rotation matrix
// 3  7 11  15    and 12, 13, 14 are translation
    
// the output matrix becomes
//  0 1 2| 9           
//  3 4 5| 10       ( 9, 10, 11 are the translation)
//  5 6 7| 11        0 - 7 is row major rotation matrix
// 
//
inline void shrinkTransform(double inM[16], double outM[12])
{
    int col = -1;
    for(int i = 0; i < 12; i++)
    {
        if(i % 3 == 0)
        {
            col+=1;
        }
        outM[i] = inM[i+col];
    }
}

//Creates a quaternion and a position from a 3x4 column-major transformation matrix
//Output format: pos[3]+quat[4]
//  the quaternion is in the order of [u v w a]
//  In IPSI, quaternions are stored with scalar last.
//So, the quaternion Q = a + bi + cj + dk
//will be stored [b, c, d, a] (or, if you prefer, [x, y, z, w])
inline void rotationMatrixToQuaternion(double inM[12], double outM[7])
{
    outM[0] = inM[9];
    outM[1] = inM[10];
    outM[2] = inM[11];

    outM[6] = std::sqrt(1 + inM[0] + (inM[4]) + inM[8])/2;
    outM[3] = (inM[5]-inM[7])/(4*outM[6]);
    outM[4] = (inM[6]-inM[2])/(4*outM[6]);
    outM[5] = (inM[1]-inM[3])/(4*outM[6]);
}