Need help with transformation in C++

Discussion in 'C Programming' started by tony_isa, Jul 11, 2008.

  1. tony_isa

    tony_isa

    Joined:
    Jul 11, 2008
    Messages:
    1
    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::printFormular()
    {

    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();




    }
    tony_isa, Jul 11, 2008
    #1
    1. Advertising

Want to reply to this thread or ask your own question?

It takes just 2 minutes to sign up (and it's free!). Just click the sign up button to choose a username and then you can ask your own questions on the forum.
Similar Threads
  1. KathyB
    Replies:
    0
    Views:
    362
    KathyB
    Jul 19, 2003
  2. =?Utf-8?B?c2VyZ2UgY2FsZGVyYXJh?=

    Help on XLS transformation ????

    =?Utf-8?B?c2VyZ2UgY2FsZGVyYXJh?=, Feb 10, 2006, in forum: ASP .Net
    Replies:
    0
    Views:
    1,432
    =?Utf-8?B?c2VyZ2UgY2FsZGVyYXJh?=
    Feb 10, 2006
  3. coincoin
    Replies:
    0
    Views:
    569
    coincoin
    Aug 5, 2003
  4. Mike Conmackie
    Replies:
    4
    Views:
    713
    Mike Conmackie
    Apr 29, 2004
  5. sommarlov@gmail.com

    Need help remake xsl transformation

    sommarlov@gmail.com, Jun 19, 2006, in forum: XML
    Replies:
    1
    Views:
    483
    Joe Kesselman
    Jun 22, 2006
Loading...

Share This Page