#include "FbxGeneration.h"

using namespace std;
using namespace cv;

//模板函数：将string类型变量转换为常用的数值类型
template <class Type>
Type stringToNum(const string& str)
{
    istringstream iss(str);
    Type num;
    iss >> num;
    return num;
}

GenFbx::GenFbx() {}

int GenFbx::GetRotationMatrix(cv::String filename1, cv::String filename, int startline, vector<vector<vector<double>>>& RotationMatrixSequence, vector<Point3d>& skeletonPoints3D2) {
    string file1 = filename1 + "/skeletondata.txt";
    ifstream infile1;
    infile1.open(file1.data());
    //assert(infile.is_open());
    if (infile1.is_open() == false)
    {
        return -1;
    }
    string s1;
    vector<Vec3f> skeletonPoints1;
    while (getline(infile1, s1))
    {
        //cout << s << endl;
        string x, y, z;
        Vec3f skeletonPoint;
        for (int i = 0; i < s1.size(); ++i)
        {
            if (s1.at(i) == ' '&&x.size() == 0)
            {
                x = s1.substr(0, i);
                skeletonPoint[0] = stringToNum<float>(x);
            }
            else if (s1.at(i) == ' '&&x.size() != 0 && y.size() == 0)
            {
                y = s1.substr(x.size() + 1, i - (x.size() + 1));
                skeletonPoint[1] = stringToNum<float>(y);
            }
            else if (i == s1.size() - 1 && y.size() != 0 && z.size() == 0)
            {
                z = s1.substr(x.size() + y.size() + 2, s1.size() - (x.size() + y.size() + 2));
                skeletonPoint[2] = stringToNum<float>(z);
            }
        }
        skeletonPoints1.push_back(skeletonPoint);
    }
    infile1.close();
    if (skeletonPoints1.size() == 0)
    {
        return -1;
    }
    skeletonPoints1.erase(skeletonPoints1.begin(), skeletonPoints1.begin() + skeletonPoints1.size() - 20);

    string file = filename;//
    ifstream infile;
    infile.open(file.data());   //将文件流对象与文件连接起来 
    //assert(infile.is_open());   //若失败,则输出错误消息,并终止程序运行 
    if (!infile.is_open())
    {
        return -1;
    }
    RotationMatrixSequence.clear();
    string s;
    vector<Vec3f> skeletonPoints;
    while (getline(infile, s))
    {
        //cout << s << endl;
        string x, y, z;
        Vec3f skeletonPoint;
        for (int i = 0; i < s.size(); ++i)
        {
            if (s.at(i) == ' '&&x.size() == 0)
            {
                x = s.substr(0, i);
                skeletonPoint[0] = stringToNum<float>(x);
            }
            else if (s.at(i) == ' '&&x.size() != 0 && y.size() == 0)
            {
                y = s.substr(x.size() + 1, i - (x.size() + 1));
                skeletonPoint[1] = stringToNum<float>(y);
            }
            else if (i == s.size() - 1 && y.size() != 0 && z.size() == 0)
            {
                z = s.substr(x.size() + y.size() + 2, s.size() - (x.size() + y.size() + 2));
                skeletonPoint[2] = stringToNum<float>(z);
            }
        }
        skeletonPoints.push_back(skeletonPoint);
    }
    infile.close();             //关闭文件输入流 
    if (skeletonPoints.size() == 0)
    {
        return -1;
    }
    skeletonPoints.erase(skeletonPoints.begin(), skeletonPoints.begin() + startline);
    skeletonPoints3D2.clear();
    skeletonPoints3D2.insert(skeletonPoints3D2.begin(), skeletonPoints.begin(), skeletonPoints.begin()+20);
    skeletonPoints.insert(skeletonPoints.begin(), skeletonPoints1.begin(), skeletonPoints1.end());
    vector<vector<double>> RotationMatrix;
    vector<Point3d> skeletonPoints3D;
    vector<vector<Point3d>> skeletonPoints3DGroup;
    for (int i = 0; i < skeletonPoints.size(); ++i)
    {
        Point3d skeletonpoint(0, 0, 0);
        skeletonpoint.x = skeletonPoints[i][0];
        skeletonpoint.y = skeletonPoints[i][1];
        skeletonpoint.z = skeletonPoints[i][2];
        skeletonPoints3D.push_back(skeletonpoint);
        if ((i + 1) % 20 == 0 && i > 0)
        {
            skeletonPoints3DGroup.push_back(skeletonPoints3D);
            skeletonPoints3D.clear();
        }
    }
    for (int i = 0; i < skeletonPoints3DGroup.size() - 1; ++i)
    {
        CalculateAffineMatrix(skeletonPoints3DGroup[0], skeletonPoints3DGroup[i + 1], RotationMatrix);
        RotationMatrixSequence.push_back(RotationMatrix);
    }

    return 0;
}

int GenFbx::MainFuncK1(cv::String filename1, cv::String skeletonfile,char* fbxname, float timestep) {
    vector<vector<float>> Triangleinfo1;
    vector<Point3d> skeletonPoints3D1;
    vector<Point3d> skeletonPoints3D2;
    //insert the initial file name
    //cv::String filename1 = "T000";//生成初始蒙皮的文件夹,需要包含color,depth,mask和skeleton
    int result;
    result = GetControlPoints(filename1, Triangleinfo1, skeletonPoints3D1);
    if (result == -1)
    {
        return -1;
    }
    vector<vector<vector<double>>> RotationMatrixSequence;
    //insert the sequence file name
    //cv::String skeletonfile = "T113";//存放skeleton动画
    result = GetRotationMatrix(filename1, skeletonfile, 0, RotationMatrixSequence, skeletonPoints3D2);
    if (result == -1)
    {
        return -1;
    }
    FbxManager* lSdkManager = NULL;
    FbxScene* lScene = NULL;
    bool lResult;

    // Prepare the FBX SDK.
    InitializeSdkObjects(lSdkManager, lScene);

    // Create the scene.
    //lResult = CreateScene(lSdkManager, lScene, fbxname, Triangleinfo1, skeletonPoints3D1, skeletonPoints3D2, RotationMatrixSequence, timestep);
    FbxKinect20 *Kinect = new FbxKinect20();
    Kinect->m_TimeStep = timestep;
    lResult = Kinect->CreateScene(lSdkManager, lScene, fbxname, Triangleinfo1, skeletonPoints3D1, RotationMatrixSequence);

    if (lResult == false)
    {
        FBXSDK_printf("\n\nAn error occurred while creating the scene...\n");
        DestroySdkObjects(lSdkManager, lResult);
        return 0;
    }

    // Save the scene.
    lResult = SaveScene(lSdkManager, lScene, fbxname);

    if (lResult == false)
    {
        FBXSDK_printf("\n\nAn error occurred while saving the scene...\n");
        DestroySdkObjects(lSdkManager, lResult);
        return 0;
    }

    // Destroy all objects created by the FBX SDK.
    DestroySdkObjects(lSdkManager, lResult);
    return 0;
}

int GenFbx::GetControlPoints(cv::String filename, vector<vector<float>>& Triangleinfo, vector<Point3d>& skeletonPoints3D) {
    //read data
    cv::String colorImgName = filename + "/colorImg0.bmp";
    cv::String depthImgName = filename + "/depthImg0.bmp";
    cv::String contourImgName = filename + "/maskImg0.bmp";
    Mat srcimg = imread(colorImgName, 1);
    Mat depthimg = imread(depthImgName, 0);
    Mat smoothimg, blurimg;
    Mat contourimg = imread(contourImgName, 0);
    //delete_jut(contourimg, smoothimg, 5, 5, 1);
    //imageblur(smoothimg, blurimg, Size(10, 10), 150);
    //contourimg = blurimg;
    Mat skeletonimg = Mat::zeros(contourimg.size(), CV_8UC1);
    string file = filename + "/skeletondata.txt";
    ifstream infile;
    infile.open(file.data());
    //assert(infile.is_open());
    if (srcimg.empty() || depthimg.empty() || contourimg.empty() || infile.is_open() == false)
    {
        return -1;
    }
    string s;
    vector<Vec3f> skeletonPoints;
    while (getline(infile, s))
    {
        //cout << s << endl;
        string x, y, z;
        Vec3f skeletonPoint;
        for (int i = 0; i < s.size(); ++i)
        {
            if (s.at(i) == ' '&&x.size() == 0)
            {
                x = s.substr(0, i);
                skeletonPoint[0] = stringToNum<float>(x);
            }
            else if (s.at(i) == ' '&&x.size() != 0 && y.size() == 0)
            {
                y = s.substr(x.size() + 1, i - (x.size() + 1));
                skeletonPoint[1] = stringToNum<float>(y);
            }
            else if (i == s.size() - 1 && y.size() != 0 && z.size() == 0)
            {
                z = s.substr(x.size() + y.size() + 2, s.size() - (x.size() + y.size() + 2));
                skeletonPoint[2] = stringToNum<float>(z);
            }
        }
        skeletonPoints.push_back(skeletonPoint);
    }
    infile.close();
    if (skeletonPoints.size() == 0)
    {
        return -1;
    }
    skeletonPoints.erase(skeletonPoints.begin(), skeletonPoints.begin() + skeletonPoints.size() - 20);
    //GaussianBlur(srcimg, srcimg, Size(3, 3), 0, 0, BORDER_DEFAULT);
    //Canny(srcimg, contourimg, 40, 20);
    //create mesh with human mask
    int colstep = 2;
    int rowstep = 2;
    vector<Point2f> contourPoints;
    for (int i = 0; i < contourimg.cols; i = i + colstep)
    {
        for (int j = 0; j < contourimg.rows; j = j + rowstep)
        {
            if (contourimg.at<uchar>(j, i) == 255)
            {
                contourPoints.push_back(cv::Point(i, j));
            }
        }
    }
    //Get skeleton points
    skeletonPoints3D.clear();
    for (int i = 0; i < skeletonPoints.size(); ++i)
    {
        Point3d skeletonPosition(0, 0, 0);
        skeletonPosition.x = skeletonPoints[i][0];
        skeletonPosition.y = skeletonPoints[i][1];
        skeletonPosition.z = skeletonPoints[i][2];
        skeletonPoints3D.push_back(skeletonPosition);
    }
    Mat PointCloud = Mat(contourimg.size(), CV_8UC1);
    PointCloud = 255;
    //Draw contour points
    for (int i = 0; i < contourPoints.size(); ++i)
    {
        //cout << contourPoints.at(i) << "  ";
        circle(PointCloud, contourPoints.at(i), 1, 0);
    }
    //imshow("src", srcimg);
    //imshow("depth", depthimg);
    //imshow("contour", contourimg);
    //imshow("PointCloud", PointCloud);
    //imshow("skeleton", skeletonimg);
    // Keep a copy around
    Mat img_orig = srcimg.clone();
    // Rectangle to be used with Subdiv2D
    cv::Size size = srcimg.size();
    cv::Rect rect(0, 0, size.width, size.height);
    // Create an instance of Subdiv2D
    Subdiv2D subdiv(rect);
    // Insert points into subdiv
    for (vector<Point2f>::iterator it = contourPoints.begin(); it != contourPoints.end(); it++)
    {
        subdiv.insert(*it);
        //// Show animation
        //Mat img_copy = img_orig.clone();
        //// Draw delaunay triangles
        //draw_delaunay(img_copy, subdiv, Scalar(255,255,255));
        //imshow("win_delaunay", img_copy);
        //waitKey(100);
    }
    // Draw delaunay triangles
    draw_delaunay(srcimg, subdiv, Scalar(255, 255, 255));
    // Draw points
    for (vector<Point2f>::iterator it = contourPoints.begin(); it != contourPoints.end(); it++)
    {
        draw_point(srcimg, *it, Scalar(0, 0, 255));
    }
    //Get the 3D coordinate of triangle
    vector<Vec6f> triangleList, triangleDepthList;
    subdiv.getTriangleList(triangleList);
    Triangleinfo.clear();
    for (int i = 0; i < triangleList.size(); ++i)
    {
        Vec6f triMseh = triangleList[i];
        vector<float> singleTriangle;
        float depthpixel1 = depthimg.at<uchar>(triMseh[1], triMseh[0])*1.6;
        singleTriangle.push_back((triMseh[0] - camera_cx)*depthpixel1 / camera_fx);
        singleTriangle.push_back(-(triMseh[1] - camera_cy)*depthpixel1 / camera_fy);
        singleTriangle.push_back(depthpixel1);
        float depthpixel2 = depthimg.at<uchar>(triMseh[3], triMseh[2])*1.6;
        singleTriangle.push_back((triMseh[2] - camera_cx)*depthpixel2 / camera_fx);
        singleTriangle.push_back(-(triMseh[3] - camera_cy)*depthpixel2 / camera_fy);
        singleTriangle.push_back(depthpixel2);
        float depthpixel3 = depthimg.at<uchar>(triMseh[5], triMseh[4])*1.6;
        singleTriangle.push_back((triMseh[4] - camera_cx)*depthpixel3 / camera_fx);//*camera_fx/
        singleTriangle.push_back(-(triMseh[5] - camera_cy)*depthpixel3 / camera_fy);
        singleTriangle.push_back(depthpixel3);
        if (abs(depthpixel1 - depthpixel2) >= colstep * 4 || abs(depthpixel1 - depthpixel3) >= colstep * 4 || abs(depthpixel2 - depthpixel3) >= colstep * 4 || DistancePoints(triMseh[0], triMseh[1], triMseh[2], triMseh[3]) >= colstep * 2 || DistancePoints(triMseh[0], triMseh[1], triMseh[4], triMseh[5]) >= colstep * 2 || DistancePoints(triMseh[4], triMseh[5], triMseh[2], triMseh[3]) >= colstep * 2)
            continue;
        Triangleinfo.push_back(singleTriangle);
    }
    // Allocate space for Voronoi Diagram
    Mat img_voronoi = Mat::zeros(srcimg.rows, srcimg.cols, CV_8UC3);
    // Draw Voronoi diagram
    draw_voronoi(img_voronoi, subdiv);
    // Show results.
    //imshow("delaunay", srcimg);
    //imshow("voronoi", img_voronoi);
    //imshow("srcori", img_orig);
    //waitKey(0);
    return 0;
}

int GenFbx::MainFuncK2(cv::String filename1, cv::String skeletonfile, char* fbxname, float timestep)
{
    vector<vector<float>> Triangleinfo1;
    std::vector<std::vector<float>> TriangleinfoColor;
    vector<Point3d> skeletonPoints3D1;
    //insert the initial file name
    //String filename1 = "K004";
    int result;
    result = GetControlPointsK2(filename1, Triangleinfo1, TriangleinfoColor, skeletonPoints3D1);
    if (result == -1)
    {
        return -1;
    }
    vector<vector<vector<double>>> RotationMatrixSequence;
    int CapType;
    if (timestep <= 0.05)
    {
        CapType = 4;
    }
    else if (timestep > 0.05&&timestep < 0.1)
    {
        CapType = 2;
    }
    else
    {
        CapType = 1;
    }
    result = GetRotationMatrixK2(skeletonfile, 0, skeletonPoints3D1, CapType, RotationMatrixSequence);
    if (result == -1)
    {
        return -1;
    }
    FbxManager* lSdkManager = NULL;
    FbxScene* lScene = NULL;
    bool lResult;

    // Prepare the FBX SDK.
    InitializeSdkObjects(lSdkManager, lScene);

    // Create the scene.
    FbxKinect20 *Kinect2 = new FbxKinect20();
    Kinect2->m_TimeStep = timestep;
    lResult = Kinect2->CreateSceneK2(lSdkManager, lScene, fbxname, Triangleinfo1, skeletonPoints3D1, RotationMatrixSequence, TriangleinfoColor);

    if (lResult == false)
    {
        FBXSDK_printf("\n\nAn error occurred while creating the scene...\n");
        DestroySdkObjects(lSdkManager, lResult);
        return 0;
    }

    // Save the scene.
    lResult = SaveScene(lSdkManager, lScene, fbxname);

    if (lResult == false)
    {
        FBXSDK_printf("\n\nAn error occurred while saving the scene...\n");
        DestroySdkObjects(lSdkManager, lResult);
        return 0;
    }

    // Destroy all objects created by the FBX SDK.
    DestroySdkObjects(lSdkManager, lResult);
    return 0;
}

int GenFbx::MainFuncAK(cv::String filename1, cv::String skeletonfile, char* fbxname, float timestep) {
    vector<vector<float>> Triangleinfo1;
    std::vector<std::vector<float>> TriangleinfoColor;
    vector<Point3d> skeletonPoints3D1;
    //insert the initial file name
    //String filename1 = "K004";
    int result;
    result = GetControlPointsK2(filename1, Triangleinfo1, TriangleinfoColor, skeletonPoints3D1);
    if (result == -1)
    {
        return -1;
    }
    vector<vector<vector<double>>> RotationMatrixSequence;
    int CapType;
    if (timestep <= 0.05)
    {
        CapType = 4;
    }
    else if (timestep > 0.05&&timestep < 0.1)
    {
        CapType = 2;
    }
    else
    {
        CapType = 1;
    }
    result = GetRotationMatrixK2(skeletonfile, 0, skeletonPoints3D1, CapType, RotationMatrixSequence);
    if (result == -1)
    {
        return -1;
    }
    FbxManager* lSdkManager = NULL;
    FbxScene* lScene = NULL;
    bool lResult;

    // Prepare the FBX SDK.
    InitializeSdkObjects(lSdkManager, lScene);

    // Create the scene.
    FbxKinect20 *Kinect2 = new FbxKinect20();
    Kinect2->m_TimeStep = timestep;
    lResult = Kinect2->CreateSceneK2(lSdkManager, lScene, fbxname, Triangleinfo1, skeletonPoints3D1, RotationMatrixSequence, TriangleinfoColor);

    if (lResult == false)
    {
        FBXSDK_printf("\n\nAn error occurred while creating the scene...\n");
        DestroySdkObjects(lSdkManager, lResult);
        return 0;
    }

    // Save the scene.
    lResult = SaveScene(lSdkManager, lScene, fbxname);

    if (lResult == false)
    {
        FBXSDK_printf("\n\nAn error occurred while saving the scene...\n");
        DestroySdkObjects(lSdkManager, lResult);
        return 0;
    }

    // Destroy all objects created by the FBX SDK.
    DestroySdkObjects(lSdkManager, lResult);
    return 0;
}

int GenFbx::GetRotationMatrixK2(String filename, int startline, vector<Point3d> skeletonPoints3D1, int kinectType, vector<vector<vector<double>>>& RotationMatrixGroup) {
    RotationMatrixGroup.clear();
    string file1;
    for (int i = filename.size()-1; i >= 0; --i)
    {
        if (filename[i] == '/')
        {
            file1 = filename.substr(0, i+1) + "skeletonquatAK.txt";
            break;
        }
    }

    ifstream infile1;
    infile1.open(file1.data());   //将文件流对象与文件连接起来 
    //assert(infile.is_open());   //若失败,则输出错误消息,并终止程序运行 
    if (infile1.is_open() == false)
    {
        return -1;
    }
    string s1;
    vector<Vec4f> skeletonQuats;
    while (getline(infile1, s1))
    {
        //cout << s << endl;
        string w, x, y, z;
        Vec4f skeletonQuat;
        for (int i = 0; i < s1.size(); ++i)
        {
            if (s1.at(i) == ' '&&w.size() == 0)
            {
                w = s1.substr(0, i);
                skeletonQuat[0] = stringToNum<float>(w);
            }
            else if (s1.at(i) == ' '&&w.size() != 0 && x.size() == 0)
            {
                x = s1.substr(w.size() + 1, i - (w.size() + 1));
                skeletonQuat[1] = stringToNum<float>(x);
            }
            else if (s1.at(i) == ' ' && x.size() != 0 && y.size() == 0)
            {
                y = s1.substr(w.size() + x.size() + 2, i - (w.size() + x.size() + 2));
                skeletonQuat[2] = stringToNum<float>(y);
            }
            else if (i == s1.size() - 1 && y.size() != 0 && z.size() == 0)
            {
                z = s1.substr(w.size() + x.size() + y.size() + 3, s1.size() - (w.size() + x.size() + y.size() + 3));
                skeletonQuat[3] = stringToNum<float>(z);
            }
        }
        skeletonQuats.push_back(skeletonQuat);
    }
    infile1.close();             //关闭文件输入流 
    skeletonQuats.erase(skeletonQuats.begin(), skeletonQuats.begin() + startline);
    vector<Vec4f> skeletonQuats3D;
    vector<vector<Vec4f>> skeletonQuats3DGroup;
    for (int i = 0; i < skeletonQuats.size(); ++i)
    {
        skeletonQuats3D.push_back(skeletonQuats[i]);
        if ((i + 1) % 20 == 0 && i > 0)//20点
        {
            skeletonQuats3DGroup.push_back(skeletonQuats3D);
            skeletonQuats3D.clear();
        }
    }

    string file = filename;
    ifstream infile;
    infile.open(file.data());   //将文件流对象与文件连接起来 
    //assert(infile.is_open());   //若失败,则输出错误消息,并终止程序运行 
    if (infile.is_open() == false)
    {
        return -1;
    }
    string s;
    vector<Vec3f> skeletonPoints;
    while (getline(infile, s))
    {
        //cout << s << endl;
        string x, y, z;
        Vec3f skeletonPoint;
        for (int i = 0; i < s.size(); ++i)
        {
            if (s.at(i) == ' '&&x.size() == 0)
            {
                x = s.substr(0, i);
                skeletonPoint[0] = stringToNum<float>(x);
            }
            else if (s.at(i) == ' '&&x.size() != 0 && y.size() == 0)
            {
                y = s.substr(x.size() + 1, i - (x.size() + 1));
                skeletonPoint[1] = stringToNum<float>(y);
            }
            else if (i == s.size() - 1 && y.size() != 0 && z.size() == 0)
            {
                z = s.substr(x.size() + y.size() + 2, s.size() - (x.size() + y.size() + 2));
                skeletonPoint[2] = stringToNum<float>(z);
            }
        }
        skeletonPoints.push_back(skeletonPoint);
    }
    infile.close();             //关闭文件输入流 
    skeletonPoints.erase(skeletonPoints.begin(), skeletonPoints.begin() + startline);
    vector<vector<double>> RotationMatrix;
    vector<Point3d> skeletonPoints3D;
    vector<vector<Point3d>> skeletonPoints3DGroup;
    for (int i = 0; i < skeletonPoints.size(); ++i)
    {
        Point3d skeletonpoint(0, 0, 0);
        skeletonpoint.x = skeletonPoints[i][0];
        skeletonpoint.y = skeletonPoints[i][1];
        skeletonpoint.z = skeletonPoints[i][2];
        skeletonPoints3D.push_back(skeletonpoint);
        if ((i + 1) % 20 == 0 && i > 0)//20点
        {
            skeletonPoints3DGroup.push_back(skeletonPoints3D);
            skeletonPoints3D.clear();
        }
    }
    if (kinectType == 2)
    {
        vector<vector<Point3d>> skeletonPoints3DGroupClone = skeletonPoints3DGroup;
        for (int i = 1; i < skeletonPoints3DGroup.size() - 1; ++i)
        {
            for (int j = 0; j < skeletonPoints3DGroup[0].size(); ++j)
            {
                skeletonPoints3DGroup[i][j].x = (skeletonPoints3DGroupClone[i - 1][j].x + skeletonPoints3DGroupClone[i][j].x + skeletonPoints3DGroupClone[i + 1][j].x) / 3;
                skeletonPoints3DGroup[i][j].y = (skeletonPoints3DGroupClone[i - 1][j].y + skeletonPoints3DGroupClone[i][j].y + skeletonPoints3DGroupClone[i + 1][j].y) / 3;
                skeletonPoints3DGroup[i][j].z = (skeletonPoints3DGroupClone[i - 1][j].z + skeletonPoints3DGroupClone[i][j].z + skeletonPoints3DGroupClone[i + 1][j].z) / 3;
            }
        }
    }
    else if(kinectType == 4)
    {
        vector<vector<Point3d>> skeletonPoints3DGroupClone = skeletonPoints3DGroup;
        for (int i = 2; i < skeletonPoints3DGroup.size() - 2; ++i)
        {
            for (int j = 0; j < skeletonPoints3DGroup[0].size(); ++j)
            {
                skeletonPoints3DGroup[i][j].x = (skeletonPoints3DGroupClone[i - 2][j].x + skeletonPoints3DGroupClone[i - 1][j].x + skeletonPoints3DGroupClone[i][j].x + skeletonPoints3DGroupClone[i + 1][j].x + skeletonPoints3DGroupClone[i + 2][j].x) / 5;
                skeletonPoints3DGroup[i][j].y = (skeletonPoints3DGroupClone[i - 2][j].y + skeletonPoints3DGroupClone[i - 1][j].y + skeletonPoints3DGroupClone[i][j].y + skeletonPoints3DGroupClone[i + 1][j].y + skeletonPoints3DGroupClone[i + 2][j].y) / 5;
                skeletonPoints3DGroup[i][j].z = (skeletonPoints3DGroupClone[i - 2][j].z + skeletonPoints3DGroupClone[i - 1][j].z + skeletonPoints3DGroupClone[i][j].z + skeletonPoints3DGroupClone[i + 1][j].z + skeletonPoints3DGroupClone[i + 2][j].z) / 5;
            }
        }
    }
    std::vector<float> Quat1(4), Quat2(4);
    Quat1[0] = skeletonQuats3DGroup[0][0][0];
    Quat1[1] = skeletonQuats3DGroup[0][0][1];
    Quat1[2] = skeletonQuats3DGroup[0][0][2];
    Quat1[3] = skeletonQuats3DGroup[0][0][3];
    Quat2[0] = skeletonQuats3DGroup[1][0][0];
    Quat2[1] = skeletonQuats3DGroup[1][0][1];
    Quat2[2] = skeletonQuats3DGroup[1][0][2];
    Quat2[3] = skeletonQuats3DGroup[1][0][3];
    vector<float> RotVecQuatRtF = GetRotationQuat(Quat1, Quat2);
    for (int i = 0; i < skeletonPoints3DGroup.size(); ++i)
    {
        CalculateAffineMatrixK2(skeletonPoints3D1, skeletonPoints3DGroup[i], skeletonQuats3DGroup[0], skeletonQuats3DGroup[i], RotationMatrix, RotVecQuatRtF);
        RotationMatrixGroup.push_back(RotationMatrix);
    }
    vector<vector<vector<double>>> RotationMatrixGroupClone = RotationMatrixGroup;
    for (int i = 1; i < RotationMatrixGroup.size()-1; ++i)
    {
        for (int j = 0; j < RotationMatrixGroup[0].size(); ++j)
        {
            for (int k = 0; k < RotationMatrixGroup[0][0].size(); ++k)
            {
                RotationMatrixGroup[i][j][k] = (RotationMatrixGroupClone[i - 1][j][k] + RotationMatrixGroupClone[i][j][k] + RotationMatrixGroupClone[i + 1][j][k])/3;
            }
        }
    }
    return 0;
}

int GenFbx::GetControlPointsK2(String filename, vector<vector<float>>& Triangleinfo, std::vector<std::vector<float>> &TriangleinfoColor, vector<Point3d>& skeletonPoints3D) {
    Triangleinfo.clear();
    TriangleinfoColor.clear();
    //read data
    String colorImgName =  filename + "/colorImg.png";
    String depthImgName =  filename + "/depthImg.png";
    String contourImgName = filename + "/maskImg.png";
    Mat srcimg = imread(colorImgName, 1);
    Mat depthimg = imread(depthImgName, 0);
    Mat smoothimg, blurimg;
    Mat contourimg = imread(contourImgName, 1);
    //delete_jut(contourimg, smoothimg, 5, 5, 1);
    //imageblur(smoothimg, blurimg, Size(10, 10), 150);
    //contourimg = blurimg;
    Mat skeletonimg = Mat::zeros(contourimg.size(), CV_8UC1);
    string file = filename + "/firstskeletonpoints.txt";
    ifstream infile;
    infile.open(file.data());
    //assert(infile.is_open());
    if (srcimg.empty() || depthimg.empty() || contourimg.empty() || infile.is_open() == false)
    {
        return -1;
    }
    string s;
    vector<Vec3f> skeletonPoints;
    while (getline(infile, s))
    {
        //cout << s << endl;
        string x, y, z;
        Vec3f skeletonPoint;
        for (int i = 0; i < s.size(); ++i)
        {
            if (s.at(i) == ' '&&x.size() == 0)
            {
                x = s.substr(0, i);
                skeletonPoint[0] = stringToNum<float>(x);
            }
            else if (s.at(i) == ' '&&x.size() != 0 && y.size() == 0)
            {
                y = s.substr(x.size() + 1, i - (x.size() + 1));
                skeletonPoint[1] = stringToNum<float>(y);
            }
            else if (i == s.size() - 1 && y.size() != 0 && z.size() == 0)
            {
                z = s.substr(x.size() + y.size() + 2, s.size() - (x.size() + y.size() + 2));
                skeletonPoint[2] = stringToNum<float>(z);
            }
        }
        skeletonPoints.push_back(skeletonPoint);
    }
    infile.close();
    skeletonPoints.erase(skeletonPoints.begin(), skeletonPoints.begin() + skeletonPoints.size() - 20);//20点

    string meshFile =  filename + "/depthtocolor.txt";
    ifstream inmeshFile;
    inmeshFile.open(meshFile.data());   //将文件流对象与文件连接起来 
    assert(inmeshFile.is_open());   //若失败,则输出错误消息,并终止程序运行 
    string s1;
    std::vector<Vec4i> depthToColor;
    while (getline(inmeshFile, s1))
    {
        string xd, yd, xc, yc;
        Vec4i depthToColorPoint;
        for (int i = 0; i < s1.size(); ++i)
        {
            if (s1.at(i) == ' '&&xd.size() == 0)
            {
                xd = s1.substr(0, i);
                depthToColorPoint[0] = stringToNum<int>(xd);
            }
            else if (s1.at(i) == ' '&&xd.size() != 0 && yd.size() == 0)
            {
                yd = s1.substr(xd.size() + 1, i - (xd.size() + 1));
                depthToColorPoint[1] = stringToNum<int>(yd);
            }
            else if (s1.at(i) == ' ' && yd.size() != 0 && xc.size() == 0)
            {
                xc = s1.substr(xd.size() + yd.size() + 2, i - (xd.size() + yd.size() + 2));
                depthToColorPoint[2] = stringToNum<int>(xc);
            }
            else if (i == s1.size() - 1 && xc.size() != 0 && yc.size() == 0)
            {
                yc = s1.substr(xd.size() + yd.size() + xc.size() + 3, s1.size() - (xd.size() + yd.size() + xc.size() + 3));
                depthToColorPoint[3] = stringToNum<int>(yc);
            }
        }
        depthToColor.push_back(depthToColorPoint);
    }
    inmeshFile.close();             //关闭文件输入流 

    string depthFile = filename + "/depthtocamera.txt";
    ifstream indepthFile;
    indepthFile.open(depthFile.data());   //将文件流对象与文件连接起来 
    assert(indepthFile.is_open());   //若失败,则输出错误消息,并终止程序运行 
    string s2;
    std::vector<std::vector<double>> depthToCamera;
    while (getline(indepthFile, s2))
    {
        string xd, yd, xc, yc, zc;
        std::vector<double>  depthToCameraPoint(5, 0);
        for (int i = 0; i < s2.size(); ++i)
        {
            if (s2.at(i) == ' '&&xd.size() == 0)
            {
                xd = s2.substr(0, i);
                depthToCameraPoint[0] = stringToNum<double>(xd);
            }
            else if (s2.at(i) == ' '&&xd.size() != 0 && yd.size() == 0)
            {
                yd = s2.substr(xd.size() + 1, i - (xd.size() + 1));
                depthToCameraPoint[1] = stringToNum<double>(yd);
            }
            else if (s2.at(i) == ' ' && yd.size() != 0 && xc.size() == 0)
            {
                xc = s2.substr(xd.size() + yd.size() + 2, i - (xd.size() + yd.size() + 2));
                depthToCameraPoint[2] = stringToNum<double>(xc);
            }
            else if (s2.at(i) == ' ' && xc.size() != 0 && yc.size() == 0)
            {
                yc = s2.substr(xd.size() + yd.size() + xc.size() + 3, i - (xd.size() + yd.size() + xc.size() + 3));
                depthToCameraPoint[3] = stringToNum<double>(yc);
            }
            else if (i == s2.size() - 1 && yc.size() != 0 && zc.size() == 0)
            {
                zc = s2.substr(xd.size() + yd.size() + xc.size() + yc.size() + 4, s2.size() - (xd.size() + yd.size() + xc.size() + yc.size() + 4));
                depthToCameraPoint[4] = stringToNum<double>(zc);
            }
        }
        depthToCamera.push_back(depthToCameraPoint);
    }
    indepthFile.close();             //关闭文件输入流 

    if (srcimg.empty() || depthimg.empty())
    {
        return -1;
    }

    //create mesh with human mask
    int colstep = 2;
    int rowstep = 2;
    vector<Point2f> contourPoints;
    for (int i = 0; i < contourimg.cols; i = i + colstep)
    {
        for (int j = 0; j < contourimg.rows; j = j + rowstep)
        {
            if (contourimg.at<Vec3b>(j, i) != Vec3b(0, 0, 0))
            {
                contourPoints.push_back(Point(i, j));
            }
        }
    }
    //Get skeleton points
    skeletonPoints3D.clear();
    for (int i = 0; i < skeletonPoints.size(); ++i)
    {
        Point3d skeletonPosition(0, 0, 0);
        skeletonPosition.x = skeletonPoints[i][0];
        skeletonPosition.y = skeletonPoints[i][1];
        skeletonPosition.z = skeletonPoints[i][2];
        skeletonPoints3D.push_back(skeletonPosition);
    }
    Mat PointCloud = Mat(contourimg.size(), CV_8UC1);
    PointCloud = 255;
    //Draw contour points
    for (int i = 0; i < contourPoints.size(); ++i)
    {
        //cout << contourPoints.at(i) << "  ";
        circle(PointCloud, contourPoints.at(i), 1, 0);
    }
    // Keep a copy around
    Mat img_orig = srcimg.clone();
    // Rectangle to be used with Subdiv2D
    Size size = srcimg.size();
    Rect rect(0, 0, size.width, size.height);
    // Create an instance of Subdiv2D
    Subdiv2D subdiv(rect);
    // Insert points into subdiv
    for (vector<Point2f>::iterator it = contourPoints.begin(); it != contourPoints.end(); it++)
    {
        subdiv.insert(*it);
    }
    // Draw delaunay triangles
    draw_delaunay(srcimg, subdiv, Scalar(255, 255, 255));
    // Draw points
    for (vector<Point2f>::iterator it = contourPoints.begin(); it != contourPoints.end(); it++)
    {
        draw_point(srcimg, *it, Scalar(0, 0, 255));
    }
    //Get the 3D coordinate of triangle
    vector<Vec6f> triangleList, triangleDepthList;
    subdiv.getTriangleList(triangleList);
    Triangleinfo.clear();
    for (int i = 0; i < triangleList.size(); ++i)
    {
        Vec6f triMseh = triangleList[i];
        vector<float> singleTriangle;
        std::vector<float> singleTriangleColor;
        singleTriangle.push_back(depthToCamera[triMseh[1] * 512 + triMseh[0]][2]);
        singleTriangle.push_back(depthToCamera[triMseh[1] * 512 + triMseh[0]][3]);
        singleTriangle.push_back(depthToCamera[triMseh[1] * 512 + triMseh[0]][4]);
        singleTriangle.push_back(depthToCamera[triMseh[3] * 512 + triMseh[2]][2]);
        singleTriangle.push_back(depthToCamera[triMseh[3] * 512 + triMseh[2]][3]);
        singleTriangle.push_back(depthToCamera[triMseh[3] * 512 + triMseh[2]][4]);
        singleTriangle.push_back(depthToCamera[triMseh[5] * 512 + triMseh[4]][2]);//*camera_fx/
        singleTriangle.push_back(depthToCamera[triMseh[5] * 512 + triMseh[4]][3]);
        singleTriangle.push_back(depthToCamera[triMseh[5] * 512 + triMseh[4]][4]);
        singleTriangleColor.push_back(depthToColor[triMseh[1] * 512 + triMseh[0]][2]);
        singleTriangleColor.push_back(depthToColor[triMseh[1] * 512 + triMseh[0]][3]);
        singleTriangleColor.push_back(depthToColor[triMseh[3] * 512 + triMseh[2]][2]);
        singleTriangleColor.push_back(depthToColor[triMseh[3] * 512 + triMseh[2]][3]);
        singleTriangleColor.push_back(depthToColor[triMseh[5] * 512 + triMseh[4]][2]);
        singleTriangleColor.push_back(depthToColor[triMseh[5] * 512 + triMseh[4]][3]);
        if (singleTriangle[0] == -1 || singleTriangle[3] == -1 || singleTriangle[6] == -1)
            continue;
        if (DistancePoints3D(singleTriangle[0], singleTriangle[1], singleTriangle[2], singleTriangle[3], singleTriangle[4], singleTriangle[5]) >= 10 || DistancePoints3D(singleTriangle[0], singleTriangle[1], singleTriangle[2], singleTriangle[6], singleTriangle[7], singleTriangle[8]) >= 10 || DistancePoints3D(singleTriangle[6], singleTriangle[7], singleTriangle[8], singleTriangle[3], singleTriangle[4], singleTriangle[5]) >= 10)
            continue;
        if (singleTriangleColor[0] == -1 || singleTriangleColor[2] == -1 || singleTriangleColor[4] == -1)
            continue;
        Triangleinfo.push_back(singleTriangle);
        TriangleinfoColor.push_back(singleTriangleColor);
    }
    // Allocate space for Voronoi Diagram
    //Mat img_voronoi = Mat::zeros(srcimg.rows, srcimg.cols, CV_8UC3);
    // Draw Voronoi diagram
    //draw_voronoi(img_voronoi, subdiv);
    return 0;
}

int GenFbx::CalculateAffineMatrix(vector<Point3d> skeletonPoints3D1, vector<Point3d> skeletonPoints3D2, vector<vector<double>>& RotationMatrix) {
    RotationMatrix.clear();
    vector<double> RMatrix;
    vector<int> IndexMatrix;
    insertlink(IndexMatrix);
    RMatrix.push_back(0.0);
    RMatrix.push_back(0.0);
    RMatrix.push_back(0.0);
    RMatrix.push_back(skeletonPoints3D2[0].x);//
    RMatrix.push_back(skeletonPoints3D2[0].y);//
    RMatrix.push_back(skeletonPoints3D2[0].z);//
    RotationMatrix.push_back(RMatrix);
    vector<float> v1, v2;
    for (int i = 0; i < IndexMatrix.size() / 2; ++i)
    {
        int index1 = IndexMatrix[i * 2 + 0], index2 = IndexMatrix[i * 2 + 1];
        v1.clear();
        v2.clear();
        RMatrix.clear();
        v1.push_back(skeletonPoints3D1[index2].x - skeletonPoints3D1[index1].x);
        v1.push_back(skeletonPoints3D1[index2].y - skeletonPoints3D1[index1].y);
        v1.push_back(skeletonPoints3D1[index2].z - skeletonPoints3D1[index1].z);
        v2.push_back(skeletonPoints3D2[index2].x - skeletonPoints3D2[index1].x);
        v2.push_back(skeletonPoints3D2[index2].y - skeletonPoints3D2[index1].y);
        v2.push_back(skeletonPoints3D2[index2].z - skeletonPoints3D2[index1].z);
        //calculate the Eular Angle between parent and son nodes
        vector<float> RotVec = GetRotationVector(v1, v2);
        //rotation
        RMatrix.push_back(RotVec[0]);
        RMatrix.push_back(RotVec[1]);
        RMatrix.push_back(RotVec[2]);
        //position
        RMatrix.push_back(skeletonPoints3D2[index2].x - skeletonPoints3D2[index1].x);
        RMatrix.push_back(skeletonPoints3D2[index2].y - skeletonPoints3D2[index1].y);
        RMatrix.push_back(skeletonPoints3D2[index2].z - skeletonPoints3D2[index1].z);
        RotationMatrix.push_back(RMatrix);
    }
    //Minus parent node rotation
    vector<vector<double>> RotationMatrixClone = RotationMatrix;
    RotationMatrix[2][0] = RotationMatrix[2][0] - RotationMatrixClone[1][0];
    RotationMatrix[2][1] = RotationMatrix[2][1] - RotationMatrixClone[1][1];
    RotationMatrix[2][2] = RotationMatrix[2][2] - RotationMatrixClone[1][2];
    RotationMatrix[3][0] = RotationMatrix[3][0] - RotationMatrixClone[2][0];
    RotationMatrix[3][1] = RotationMatrix[3][1] - RotationMatrixClone[2][1];
    RotationMatrix[3][2] = RotationMatrix[3][2] - RotationMatrixClone[2][2];
    RotationMatrix[4][0] = RotationMatrix[4][0] - RotationMatrixClone[2][0];
    RotationMatrix[4][1] = RotationMatrix[4][1] - RotationMatrixClone[2][1];
    RotationMatrix[4][2] = RotationMatrix[4][2] - RotationMatrixClone[2][2];
    RotationMatrix[5][0] = RotationMatrix[5][0] - RotationMatrixClone[4][0];
    //RotationMatrix[5][1] = RotationMatrix[5][1] - RotationMatrixClone[4][1];
    RotationMatrix[5][2] = RotationMatrix[5][2] - RotationMatrixClone[4][2];
    RotationMatrix[6][0] = RotationMatrix[6][0] - RotationMatrixClone[5][0];
    RotationMatrix[6][1] = RotationMatrix[6][1] - RotationMatrixClone[5][1];
    RotationMatrix[6][2] = RotationMatrix[6][2] - RotationMatrixClone[5][2];
    RotationMatrix[7][0] = 0;
    RotationMatrix[7][1] = 0;
    RotationMatrix[7][2] = 0;
    RotationMatrix[8][0] = RotationMatrix[8][0] - RotationMatrixClone[2][0];
    RotationMatrix[8][1] = RotationMatrix[8][1] - RotationMatrixClone[2][1];
    RotationMatrix[8][2] = RotationMatrix[8][2] - RotationMatrixClone[2][2];
    RotationMatrix[9][0] = RotationMatrix[9][0] - RotationMatrixClone[8][0];
    //RotationMatrix[9][1] = RotationMatrix[9][1] - RotationMatrixClone[8][1];
    RotationMatrix[9][2] = RotationMatrix[9][2] - RotationMatrixClone[8][2];
    RotationMatrix[10][0] = RotationMatrix[10][0] - RotationMatrixClone[9][0];
    RotationMatrix[10][1] = RotationMatrix[10][1] - RotationMatrixClone[9][1];
    RotationMatrix[10][2] = RotationMatrix[10][2] - RotationMatrixClone[9][2];
    RotationMatrix[11][0] = 0;
    RotationMatrix[11][1] = 0;
    RotationMatrix[11][2] = 0;
    //RotationMatrix[13][0] = RotationMatrix[13][0]- RotationMatrixClone[12][0];
    //RotationMatrix[13][1] = RotationMatrix[13][1]- RotationMatrixClone[12][1];
    //RotationMatrix[13][2] = RotationMatrix[13][2]- RotationMatrixClone[12][2];
    RotationMatrix[14][0] = RotationMatrix[14][0] - RotationMatrixClone[13][0];
    //convert Z axis to Y axis while the angle between parent node and it is over 20
    if (fabs(RotationMatrixClone[14][0] - RotationMatrixClone[13][0]) > 20)
    {
        RotationMatrix[14][1] = RotationMatrix[14][2] - RotationMatrixClone[13][2];
        RotationMatrix[14][2] = RotationMatrix[14][2] - RotationMatrixClone[13][1];
    }
    else
    {
        RotationMatrix[14][1] = RotationMatrix[14][1] - RotationMatrixClone[13][1];
        RotationMatrix[14][2] = RotationMatrix[14][2] - RotationMatrixClone[13][2];
    }
    //RotationMatrix[14][1] = 0;
    RotationMatrix[14][2] = 0;//knee cannot rotate in z axis
    RotationMatrix[15][0] = 0;
    RotationMatrix[15][1] = 0;
    RotationMatrix[15][2] = 0;
    //RotationMatrix[17][0] = RotationMatrix[17][0] - RotationMatrixClone[16][0];
    //RotationMatrix[17][1] = RotationMatrix[17][1] - RotationMatrixClone[16][1];
    //RotationMatrix[17][2] = RotationMatrix[17][2] - RotationMatrixClone[16][2];
    RotationMatrix[18][0] = RotationMatrix[18][0] - RotationMatrixClone[17][0];
    //convert Z axis to Y axis while the angle between parent node and it is over 20
    if (fabs(RotationMatrixClone[18][0] - RotationMatrixClone[17][0]) > 20)
    {
        RotationMatrix[18][1] = RotationMatrix[18][2] - RotationMatrixClone[17][2];
        RotationMatrix[18][2] = RotationMatrix[18][2] - RotationMatrixClone[17][1];
    }
    else
    {
        RotationMatrix[18][1] = RotationMatrix[18][1] - RotationMatrixClone[17][1];
        RotationMatrix[18][2] = RotationMatrix[18][2] - RotationMatrixClone[17][2];
    }
    //RotationMatrix[18][1] = 0;
    RotationMatrix[18][2] = 0;//knee cannot rotate in z axis
    RotationMatrix[19][0] = 0;
    RotationMatrix[19][1] = 0;
    RotationMatrix[19][2] = 0;
    //prevent the arms and legs from abnormal behaviour because we cannot make these joints over 180 degrees
    if (RotationMatrixClone[14][0] - RotationMatrixClone[13][0] > 0)
    {
        RotationMatrix[14][0] = 0;
    }
    if (RotationMatrixClone[18][0] - RotationMatrixClone[17][0] > 0)
    {
        RotationMatrix[18][0] = 0;
    }
    if (RotationMatrixClone[6][1] - RotationMatrixClone[5][1] > 0)
    {
        RotationMatrix[6][1] = 0;
    }
    if (RotationMatrixClone[10][1] - RotationMatrixClone[9][1] < 0)
    {
        RotationMatrix[10][1] = 0;
    }
    return 0;
}

int GenFbx::CalculateAffineMatrixK2(vector<Point3d> skeletonPoints3D1, vector<Point3d> skeletonPoints3D2, vector<Vec4f> skeletonQuats3D1, vector<Vec4f> skeletonQuats3D2, vector<vector<double>>& RotationMatrix, vector<float> RotVecQuatRtF) {
    RotationMatrix.clear();
    vector<double> RMatrix;
    vector<int> IndexMatrix;
    insertlink(IndexMatrix);
    std::vector<float> Quat1(4), Quat2(4);
    Quat1[0] = skeletonQuats3D1[0][0];
    Quat1[1] = skeletonQuats3D1[0][1];
    Quat1[2] = skeletonQuats3D1[0][2];
    Quat1[3] = skeletonQuats3D1[0][3];
    Quat2[0] = skeletonQuats3D2[0][0];
    Quat2[1] = skeletonQuats3D2[0][1];
    Quat2[2] = skeletonQuats3D2[0][2];
    Quat2[3] = skeletonQuats3D2[0][3];
    vector<float> RotVecQuatRt = GetRotationQuat(Quat1, Quat2);
    RMatrix.push_back(0.0);
    RMatrix.push_back(-RotVecQuatRt[1]);
    RMatrix.push_back(0.0);
    RMatrix.push_back(skeletonPoints3D2[0].x);//
    RMatrix.push_back(skeletonPoints3D2[0].y);//
    RMatrix.push_back(skeletonPoints3D2[0].z);//
    RotationMatrix.push_back(RMatrix);
    vector<float> v1, v2;
    for (int i = 0; i < IndexMatrix.size() / 2; ++i)
    {
        int index1 = IndexMatrix[i * 2 + 0], index2 = IndexMatrix[i * 2 + 1];
        v1.clear();
        v2.clear();
        RMatrix.clear();
        v1.push_back(skeletonPoints3D1[index2].x - skeletonPoints3D1[index1].x);
        v1.push_back(skeletonPoints3D1[index2].y - skeletonPoints3D1[index1].y);
        v1.push_back(skeletonPoints3D1[index2].z - skeletonPoints3D1[index1].z);
        v2.push_back(skeletonPoints3D2[index2].x - skeletonPoints3D2[index1].x);
        v2.push_back(skeletonPoints3D2[index2].y - skeletonPoints3D2[index1].y);
        v2.push_back(skeletonPoints3D2[index2].z - skeletonPoints3D2[index1].z);
        //calculate the Eular Angle between parent and son nodes
        vector<float> RotVec = GetRotationVector(v1, v2);
        if (i == 1)
        {
            std::vector<float> Quat1(4), Quat2(4);
            Quat1[0] = skeletonQuats3D1[i+1][0];
            Quat1[1] = skeletonQuats3D1[i+1][1];
            Quat1[2] = skeletonQuats3D1[i+1][2];
            Quat1[3] = skeletonQuats3D1[i+1][3];
            Quat2[0] = skeletonQuats3D2[i+1][0];
            Quat2[1] = skeletonQuats3D2[i+1][1];
            Quat2[2] = skeletonQuats3D2[i+1][2];
            Quat2[3] = skeletonQuats3D2[i+1][3];
            vector<float> RotVecQuatSC = GetRotationQuat(Quat1, Quat2);
            //rotation
            RMatrix.push_back(RotVec[0]);
            RMatrix.push_back(RotVec[1] - (RotVecQuatSC[1]- RotVecQuatRt[1]));
            RMatrix.push_back(RotVec[2]);
        }
        else
        {
            //rotation
            RMatrix.push_back(RotVec[0]);
            RMatrix.push_back(RotVec[1]);
            RMatrix.push_back(RotVec[2]);
        }
        //position
        RMatrix.push_back(skeletonPoints3D2[index2].x - skeletonPoints3D2[index1].x);
        RMatrix.push_back(skeletonPoints3D2[index2].y - skeletonPoints3D2[index1].y);
        RMatrix.push_back(skeletonPoints3D2[index2].z - skeletonPoints3D2[index1].z);
        RotationMatrix.push_back(RMatrix);
    }
    //Minus parent node rotation
    vector<vector<double>> RotationMatrixClone = RotationMatrix;
    RotationMatrix[2][0] = RotationMatrix[2][0] - RotationMatrixClone[1][0];
    RotationMatrix[2][1] = RotationMatrix[2][1] - RotationMatrixClone[1][1];
    RotationMatrix[2][2] = RotationMatrix[2][2] - RotationMatrixClone[1][2];
    //RotationMatrix[3][0] = RotationMatrix[3][0] - RotationMatrixClone[2][0];
    //RotationMatrix[3][1] = RotationMatrix[3][1] - RotationMatrixClone[2][1];
    //RotationMatrix[3][2] = RotationMatrix[3][2] - RotationMatrixClone[2][2];
    RotationMatrix[4][0] = RotationMatrix[4][0] - RotationMatrixClone[2][0];
    RotationMatrix[4][1] = RotationMatrix[4][1] - RotationMatrixClone[2][1];
    RotationMatrix[4][2] = RotationMatrix[4][2] - RotationMatrixClone[2][2];
    RotationMatrix[5][0] = RotationMatrix[5][0] - RotationMatrixClone[4][0] - RotationMatrixClone[2][0];
    //RotationMatrix[5][1] = RotationMatrix[5][1] - RotationMatrixClone[4][1];
    RotationMatrix[5][2] = RotationMatrix[5][2] - RotationMatrixClone[4][2];
    RotationMatrix[6][1] = RotationMatrix[6][1] - RotationMatrixClone[5][1];
    RotationMatrix[6][0] = RotationMatrix[6][0] - RotationMatrixClone[5][0];
    RotationMatrix[6][2] = RotationMatrix[6][2] - RotationMatrixClone[5][2];
    RotationMatrix[7][0] = 0;
    RotationMatrix[7][1] = 0;
    RotationMatrix[7][2] = 0;
    RotationMatrix[8][0] = RotationMatrix[8][0] - RotationMatrixClone[2][0];
    RotationMatrix[8][1] = RotationMatrix[8][1] - RotationMatrixClone[2][1];
    RotationMatrix[8][2] = RotationMatrix[8][2] - RotationMatrixClone[2][2];
    RotationMatrix[9][0] = RotationMatrix[9][0] - RotationMatrixClone[8][0] - RotationMatrixClone[2][0];
    //RotationMatrix[9][1] = RotationMatrix[9][1] - RotationMatrixClone[8][1];
    RotationMatrix[9][2] = RotationMatrix[9][2] - RotationMatrixClone[8][2];
    RotationMatrix[10][1] = RotationMatrix[10][1] - RotationMatrixClone[9][1];
    RotationMatrix[10][0] = RotationMatrix[10][0] - RotationMatrixClone[9][0];
    RotationMatrix[10][2] = RotationMatrix[10][2] - RotationMatrixClone[9][2];
    RotationMatrix[11][0] = 0;
    RotationMatrix[11][1] = 0;
    RotationMatrix[11][2] = 0;
    //RotationMatrix[13][0] = RotationMatrix[13][0]- RotationMatrixClone[12][0];
    //RotationMatrix[13][1] = RotationMatrix[13][1]- RotationMatrixClone[12][1];
    //RotationMatrix[13][2] = RotationMatrix[13][2]- RotationMatrixClone[12][2];
    RotationMatrix[14][0] = RotationMatrix[14][0] - RotationMatrixClone[13][0];
    //convert Z axis to Y axis while the angle between parent node and it is over 20
    if (fabs(RotationMatrixClone[14][0] - RotationMatrixClone[13][0]) > 20)
    {
        RotationMatrix[14][1] = RotationMatrix[14][2] - RotationMatrixClone[13][2];
        RotationMatrix[14][2] = RotationMatrix[14][2] - RotationMatrixClone[13][1];
    }
    else
    {
        RotationMatrix[14][1] = RotationMatrix[14][1] - RotationMatrixClone[13][1];
        RotationMatrix[14][2] = RotationMatrix[14][2] - RotationMatrixClone[13][2];
    }
    //RotationMatrix[14][1] = 0;
    RotationMatrix[14][2] = 0;//knee cannot rotate in z axis
    RotationMatrix[15][0] = RotationMatrix[15][0] - RotationMatrixClone[14][0];
    RotationMatrix[15][1] = RotationMatrix[15][1] - RotationMatrixClone[14][1];
    RotationMatrix[15][2] = RotationMatrix[15][2] - RotationMatrixClone[14][2];
    //RotationMatrix[17][0] = RotationMatrix[17][0] - RotationMatrixClone[16][0];
    //RotationMatrix[17][1] = RotationMatrix[17][1] - RotationMatrixClone[16][1];
    //RotationMatrix[17][2] = RotationMatrix[17][2] - RotationMatrixClone[16][2];
    RotationMatrix[18][0] = RotationMatrix[18][0] - RotationMatrixClone[17][0];
    //convert Z axis to Y axis while the angle between parent node and it is over 20
    if (fabs(RotationMatrixClone[18][0] - RotationMatrixClone[17][0]) > 20)
    {
        RotationMatrix[18][1] = RotationMatrix[18][2] - RotationMatrixClone[17][2];
        RotationMatrix[18][2] = RotationMatrix[18][2] - RotationMatrixClone[17][1];
    }
    else
    {
        RotationMatrix[18][1] = RotationMatrix[18][1] - RotationMatrixClone[17][1];
        RotationMatrix[18][2] = RotationMatrix[18][2] - RotationMatrixClone[17][2];
    }
    //RotationMatrix[18][1] = 0;
    RotationMatrix[18][2] = 0;//knee cannot rotate in z axis
    RotationMatrix[19][0] = RotationMatrix[19][0] - RotationMatrixClone[18][0];
    RotationMatrix[19][1] = RotationMatrix[19][1] - RotationMatrixClone[18][1];
    RotationMatrix[19][2] = RotationMatrix[19][2] - RotationMatrixClone[18][2];
    //prevent the arms and legs from abnormal behaviour because we cannot make these joints over 180 degrees
    if (RotationMatrixClone[14][0] - RotationMatrixClone[13][0] > 0)
    {
        RotationMatrix[14][0] = 0;
    }
    if (RotationMatrixClone[18][0] - RotationMatrixClone[17][0] > 0)
    {
        RotationMatrix[18][0] = 0;
    }
    if (RotationMatrixClone[6][1] - RotationMatrixClone[5][1] > -0)
    {
        RotationMatrix[6][1] = 0;
    }
    if (RotationMatrixClone[10][1] - RotationMatrixClone[9][1] < 0)
    {
        RotationMatrix[10][1] = 0;
    }
    return 0;
}

void GenFbx::insertlink(vector<int>& linkindex)
{
    linkindex.clear();
    int Index[38] = { 0,1,//0
        1,2,//1
        2,3,//2
        2,4,//3
        4,5,//4
        5,6,//5
        6,7,//6
        2,8,//7
        8,9,//8
        9,10,//9
        10,11,//10
        0,12,//11
        12,13,//12
        13,14,//13
        14,15,//14
        0,16,//15
        16,17,//16
        17,18,//17
        18,19,//18       
    };
    linkindex = std::vector<int>(Index, Index + 38);
}