Need help with transformation in C++

[tony_isa]

Hi, I have this project that I have to write a program that read BVH (Biovision Heirarchy) file and transform in into XYZ coordinate. BVH file contain offset and rotation of joints and I need to use these information to calculate an global position for my project. Does anyone know an way to do it? My code is in the attachment. Any help is appreciate.

Here is my code


#include <cmath.h>
#include <math.h>
#include <stdlib.h>
#include <iostream>

#define PI 3.14159265
#define x 0
#define y 1
#define z 2

class CMatrixCalculator
{
public:
CMatrixCalculator(double rotation[3], double T_offset[3], double V_offset[3]); //Constructor
~CMatrixCalculator();


void matrixTransform();
void simplifyMatrix();
void printFormular();


private:
double Arotate[3]; // x = index 0; y = index 1; z = index = 2
double T_matrix[4][4];
double V_vector[4];
double TV[4];
};

CMatrixCalculator::CMatrixCalculator(double rotation[3], double T_offset[3], double V_offset[3])
{
//assign XYZ rotation to array
for(int i = 0; i < 3; i++)
{
Arotate = rotation;
}

//bottom most of rotation matrix
T_matrix[3][0] = 0;
T_matrix[3][1] = 0;
T_matrix[3][2] = 0;
T_matrix[3][3] = 1;
//Xt, Yt, Zt rightmost of rotation matrix
T_matrix[0][3] = T_offset[x];
T_matrix[1][3] = T_offset[y];
T_matrix[2][3] = T_offset[z];

V_vector[0] = V_offset[0];
V_vector[1] = V_offset[1];
V_vector[2] = V_offset[2];
V_vector[3] = 1;
}

//destructor
CMatrixCalculator::~CMatrixCalculator()
{}



void CMatrixCalculator::simplifyMatrix()
{

double cosX, cosY, cosZ, sinX, sinY, sinZ;
cosX = cos(Arotate[x]*PI/180);
cosY = cos(Arotate[y]*PI/180);
cosZ = cos(Arotate[z]*PI/180);
sinX = sin(Arotate[x]*PI/180);
sinY = sin(Arotate[y]*PI/180);
sinZ = sin(Arotate[z]*PI/180);


//RyRxRz
T_matrix[0][0] = cosY * cosX;
T_matrix[1][0] = sinX;
T_matrix[2][0] = -(sinY * cosX);

T_matrix[0][1] = (sinY * sinZ) - (cosY * sinX * cosZ);
T_matrix[1][1] = cosX * cosZ;
T_matrix[2][1] = (cosY * sinZ) + (sinY * sinX * cosZ);

T_matrix[0][2] = (sinY * cosZ) + (cosY * sinX * sinZ);
T_matrix[1][2] = -(cosX * sinZ);
T_matrix[2][2] = (cosY * cosZ) - (sinY * sinX * sinZ);



/*
//XYZ
T_matrix[0][0] = cosY * cosZ;
T_matrix[1][0] = sinZ * cosY;
T_matrix[2][0] = -sinY;

T_matrix[0][1] = -(sinZ * cosX) + (cosZ * sinY * sinX);
T_matrix[1][1] = (cosX * cosZ) + (sinX * sinY * sinZ);
T_matrix[2][1] = (cosY * sinX);

T_matrix[0][2] = (sinZ * sinX) + (cosZ * sinY * sinX);
T_matrix[1][2] = -(cosZ * sinX) + (sinZ * sinY * cosX);
T_matrix[2][2] = cosX * cosY;

*/
bool Breverse = true;
if(Breverse == true)
{
double tempMat[3][3];
tempMat[0][0] = T_matrix[0][0]; tempMat[0][1] = T_matrix[0][1]; tempMat[0][2] = T_matrix[0][2];
tempMat[1][0] = T_matrix[1][0]; tempMat[1][1] = T_matrix[1][1]; tempMat[1][2] = T_matrix[1][2];
tempMat[2][0] = T_matrix[2][0]; tempMat[2][1] = T_matrix[2][1]; tempMat[2][2] = T_matrix[2][2];

T_matrix[0][0] = tempMat[2][2]; T_matrix[0][1] = tempMat[2][1]; T_matrix[0][2] = tempMat[2][0];
T_matrix[1][0] = tempMat[1][2]; T_matrix[1][1] = tempMat[1][1]; T_matrix[1][2] = tempMat[1][0];
T_matrix[2][0] = tempMat[0][2]; T_matrix[2][1] = tempMat[0][1]; T_matrix[2][2] = tempMat[0][0];
}


TV[0] = (T_matrix[0][0] * V_vector[0] ) + (T_matrix[0][1] * V_vector[1]) + (T_matrix[0][2] * V_vector[2]) + (T_matrix[0][3] * V_vector[3]);
TV[1] = (T_matrix[1][0] * V_vector[0] ) + (T_matrix[1][1] * V_vector[1]) + (T_matrix[1][2] * V_vector[2]) + (T_matrix[1][3] * V_vector[3]);
TV[2] = (T_matrix[2][0] * V_vector[0] ) + (T_matrix[2][1] * V_vector[1]) + (T_matrix[2][2] * V_vector[2]) + (T_matrix[2][3] * V_vector[3]);
TV[3] = (T_matrix[3][0] * V_vector[0] ) + (T_matrix[3][1] * V_vector[1]) + (T_matrix[3][2] * V_vector[2]) + (T_matrix[3][3] * V_vector[3]);


}


void CMatrixCalculator::matrixTransform()
{

TV[0] = (T_matrix[0][0] * V_vector[0] ) + (T_matrix[0][1] * V_vector[1]) + (T_matrix[0][2] * V_vector[2]) + (T_matrix[0][3] * V_vector[3]);
TV[1] = (T_matrix[1][0] * V_vector[0] ) + (T_matrix[1][1] * V_vector[1]) + (T_matrix[1][2] * V_vector[2]) + (T_matrix[1][3] * V_vector[3]);
TV[2] = (T_matrix[2][0] * V_vector[0] ) + (T_matrix[2][1] * V_vector[1]) + (T_matrix[2][2] * V_vector[2]) + (T_matrix[2][3] * V_vector[3]);
TV[3] = (T_matrix[3][0] * V_vector[0] ) + (T_matrix[3][1] * V_vector[1]) + (T_matrix[3][2] * V_vector[2]) + (T_matrix[3][3] * V_vector[3]);

}

void CMatrixCalculator:rintFormular()
{

for(int i = 0; i < 4; i++){
for(int j = 0; j < 4; j++){
std::cout << "(" << i << ", " << j << ") ";
std::cout << T_matrix[j] << " ";
}
std::cout << " (T_" << i << ") " << V_vector << " = (TV_" << i << ") " << TV << std::endl;
}
}


int main()
{
double r1, r2, r3, o1, o2, o3, p1, p2, p3;

std::cout << "Enter rotation 1" << std::endl; std::cin >> r1;
std::cout << "Enter rotation 2" << std::endl; std::cin >> r2;
std::cout << "Enter rotation 3" << std::endl; std::cin >> r3;


double rotation[3] = {r1,r2,r3};
double offset[3] = {4.9, -5.8, -0.125}; //xyz
double Position[3] = {53, 39, -26};
CMatrixCalculator cmc(rotation, offset, Position);
cmc.simplifyMatrix();
cmc.matrixTransform();
cmc.printFormular();




}