#include "FbxFunction.h"

FbxKinect20::FbxKinect20() {
    m_TimeStep = 0.5;
}

bool FbxKinect20::CreateScene(FbxManager* pSdkManager, FbxScene* pScene, char* pSampleFileName, vector<vector<float>> Triangleinfo, vector<Point3d> skeletonPoints3D, vector<vector<vector<double>>> RotationMatrixSequence)
{
    // create scene info
    FbxDocumentInfo* sceneInfo = FbxDocumentInfo::Create(pSdkManager, "SceneInfo");
    sceneInfo->mTitle = "Example scene";
    sceneInfo->mSubject = "Illustrates the creation and animation of a deformed cylinder.";
    sceneInfo->mAuthor = "ExportScene01.exe sample program.";
    sceneInfo->mRevision = "rev. 1.0";
    sceneInfo->mKeywords = "deformed cylinder";
    sceneInfo->mComment = "no particular comments required.";

    // that function is asking the scene for the sceneInfo.
    pScene->SetSceneInfo(sceneInfo);

    //FbxNode* lPatch = CreatePatch(pScene, "Patch");
    FbxNode* lSkeletonRoot = CreateSkeleton(pScene, "Skeleton", skeletonPoints3D);

    // Build the node tree.
    FbxNode* lRootNode = pScene->GetRootNode();
    //lRootNode->AddChild(lPatch);
    lRootNode->AddChild(lSkeletonRoot);


    FbxNode* lTriangle = CreateTriangle(pScene, "Triangle", Triangleinfo);
    SetTriangleDefaultPosition(lTriangle, skeletonPoints3D);

    // Build the node tree.
    //FbxNode* lRootNode = pScene->GetRootNode();
    lRootNode->AddChild(lTriangle);

    // Store poses
    LinkPatchToSkeleton(pScene, lTriangle, lSkeletonRoot, Triangleinfo, skeletonPoints3D);
    StoreBindPose(pScene, lTriangle);
    //StoreRestPose(pScene, lSkeletonRoot);

    // Animation
    // First animation stack.
    FbxString lAnimStackName = "jump";
    FbxAnimStack* lAnimStack = FbxAnimStack::Create(pScene, lAnimStackName);

    // The animation nodes can only exist on AnimLayers therefore it is mandatory to
    // add at least one AnimLayer to the AnimStack. And for the purpose of this example,
    // one layer is all we need.
    FbxAnimLayer* lAnimLayer = FbxAnimLayer::Create(pScene, "Base Layer");
    lAnimStack->AddMember(lAnimLayer);
    AnimateSkeleton(pScene, lSkeletonRoot, lAnimLayer, skeletonPoints3D, RotationMatrixSequence);

    return true;
}

bool  FbxKinect20::CreateSceneK2(FbxManager* pSdkManager, FbxScene* pScene, char* pSampleFileName, vector<vector<float>> Triangleinfo, vector<Point3d> skeletonPoints3D, vector<vector<vector<double>>> RotationMatrixSequence, std::vector<std::vector<float>> TriangleinfoColor)
{
    // create scene info
    FbxDocumentInfo* sceneInfo = FbxDocumentInfo::Create(pSdkManager, "SceneInfo");
    sceneInfo->mTitle = "Example scene";
    sceneInfo->mSubject = "Illustrates the creation and animation of a deformed cylinder.";
    sceneInfo->mAuthor = "ExportScene01.exe sample program.";
    sceneInfo->mRevision = "rev. 1.0";
    sceneInfo->mKeywords = "deformed cylinder";
    sceneInfo->mComment = "no particular comments required.";

    // that function is asking the scene for the sceneInfo.
    pScene->SetSceneInfo(sceneInfo);

    //FbxNode* lPatch = CreatePatch(pScene, "Patch");
    FbxNode* lSkeletonRoot = CreateSkeleton(pScene, "Skeleton", skeletonPoints3D);

    // Build the node tree.
    FbxNode* lRootNode = pScene->GetRootNode();
    //lRootNode->AddChild(lPatch);
    lRootNode->AddChild(lSkeletonRoot);


    FbxNode* lTriangle = CreateTriangleK2(pScene, "Triangle", Triangleinfo, TriangleinfoColor);
    SetTriangleDefaultPosition(lTriangle, skeletonPoints3D);

    // Build the node tree.
    //FbxNode* lRootNode = pScene->GetRootNode();
    lRootNode->AddChild(lTriangle);

    // Store poses
    LinkPatchToSkeleton(pScene, lTriangle, lSkeletonRoot, Triangleinfo, skeletonPoints3D);
    StoreBindPose(pScene, lTriangle);
    //StoreRestPose(pScene, lSkeletonRoot);

    // Animation
    // First animation stack.
    FbxString lAnimStackName = "jump";
    FbxAnimStack* lAnimStack = FbxAnimStack::Create(pScene, lAnimStackName);

    // The animation nodes can only exist on AnimLayers therefore it is mandatory to
    // add at least one AnimLayer to the AnimStack. And for the purpose of this example,
    // one layer is all we need.
    FbxAnimLayer* lAnimLayer = FbxAnimLayer::Create(pScene, "Base Layer");
    lAnimStack->AddMember(lAnimLayer);
    AnimateSkeleton(pScene, lSkeletonRoot, lAnimLayer, skeletonPoints3D, RotationMatrixSequence);

    return true;
}

FbxNode* FbxKinect20::CreateTriangle(FbxScene* pScene, const char* pName, vector<vector<float>> Triangleinfo)
{
    FbxMesh* lMesh = FbxMesh::Create(pScene, pName);

    // The vertices
    vector<FbxVector4> vertices;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        FbxVector4 vert1(Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]);
        FbxVector4 vert2(Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]);
        FbxVector4 vert3(Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]);
        vertices.push_back(vert1);
        vertices.push_back(vert2);
        vertices.push_back(vert3);
    }

    // Create control points.
    lMesh->InitControlPoints(vertices.size());
    FbxVector4* lControlPoints = lMesh->GetControlPoints();

    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        lControlPoints[i * 3 + 0] = vertices[i * 3 + 0];
        lControlPoints[i * 3 + 1] = vertices[i * 3 + 1];
        lControlPoints[i * 3 + 2] = vertices[i * 3 + 2];
    }

    // specify normals per control point.
    FbxVector4 lNormalP0(0.0, 0.0, 1.0);
    FbxGeometryElementNormal* lNormalElement = lMesh->CreateElementNormal();
    lNormalElement->SetMappingMode(FbxGeometryElement::eByControlPoint);
    lNormalElement->SetReferenceMode(FbxGeometryElement::eDirect);
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        lNormalElement->GetDirectArray().Add(lNormalP0);
        lNormalElement->GetDirectArray().Add(lNormalP0);
        lNormalElement->GetDirectArray().Add(lNormalP0);
    }

    // Set material mapping.
    FbxGeometryElementMaterial* lMaterialElement = lMesh->CreateElementMaterial();
    lMaterialElement->SetMappingMode(FbxGeometryElement::eByPolygon);
    lMaterialElement->SetReferenceMode(FbxGeometryElement::eIndexToDirect);

    // Create UV for Diffuse channel
    FbxGeometryElementUV* lUVDiffuseElement = lMesh->CreateElementUV(gDiffuseElementName);
    FBX_ASSERT(lUVDiffuseElement != NULL);
    lUVDiffuseElement->SetMappingMode(FbxGeometryElement::eByControlPoint);
    lUVDiffuseElement->SetReferenceMode(FbxGeometryElement::eDirect);

    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        double x, y;
        x = (Triangleinfo[i][0] * camera_fx / Triangleinfo[i][2] + camera_cx) / 640.0;
        y = (Triangleinfo[i][1] * camera_fy / Triangleinfo[i][2] + camera_cy) / 480.0;
        if (x > 1)
            x = 1;
        if (x < 0)
            x = 0;
        if (y > 1)
            y = 1;
        if (y < 0)
            y = 0;
        FbxVector2 lVectors0(x, y);
        x = (Triangleinfo[i][3] * camera_fx / Triangleinfo[i][5] + camera_cx) / 640.0;
        y = (Triangleinfo[i][4] * camera_fy / Triangleinfo[i][5] + camera_cy) / 480.0;
        if (x > 1)
            x = 1;
        if (x < 0)
            x = 0;
        if (y > 1)
            y = 1;
        if (y < 0)
            y = 0;
        FbxVector2 lVectors1(x, y);
        x = (Triangleinfo[i][6] * camera_fx / Triangleinfo[i][8] + camera_cx) / 640.0;
        y = (Triangleinfo[i][7] * camera_fy / Triangleinfo[i][8] + camera_cy) / 480.0;
        if (x > 1)
            x = 1;
        if (x < 0)
            x = 0;
        if (y > 1)
            y = 1;
        if (y < 0)
            y = 0;
        FbxVector2 lVectors2(x, y);
        lUVDiffuseElement->GetDirectArray().Add(lVectors0);
        lUVDiffuseElement->GetDirectArray().Add(lVectors1);
        lUVDiffuseElement->GetDirectArray().Add(lVectors2);
    }

    //FbxVector2 lVectors0, lVectors1, lVectors2, lVectors3;

    //// Create UV for Ambient channel
    //FbxGeometryElementUV* lUVAmbientElement = lMesh->CreateElementUV(gAmbientElementName);

    //lUVAmbientElement->SetMappingMode(FbxGeometryElement::eByPolygonVertex);
    //lUVAmbientElement->SetReferenceMode(FbxGeometryElement::eIndexToDirect);

    //lVectors0.Set(0, 0);
    //lVectors1.Set(1, 0);
    //lVectors2.Set(0, 0.418586879968643);
    //lVectors3.Set(1, 0.418586879968643);

    //lUVAmbientElement->GetDirectArray().Add(lVectors0);
    //lUVAmbientElement->GetDirectArray().Add(lVectors1);
    //lUVAmbientElement->GetDirectArray().Add(lVectors2);
    //lUVAmbientElement->GetDirectArray().Add(lVectors3);

    //// Create UV for Emissive channel
    //FbxGeometryElementUV* lUVEmissiveElement = lMesh->CreateElementUV(gEmissiveElementName);

    //lUVEmissiveElement->SetMappingMode(FbxGeometryElement::eByPolygonVertex);
    //lUVEmissiveElement->SetReferenceMode(FbxGeometryElement::eIndexToDirect);

    //lVectors0.Set(0.2343, 0);
    //lVectors1.Set(1, 0.555);
    //lVectors2.Set(0.333, 0.999);
    //lVectors3.Set(0.555, 0.666);

    //lUVEmissiveElement->GetDirectArray().Add(lVectors0);
    //lUVEmissiveElement->GetDirectArray().Add(lVectors1);
    //lUVEmissiveElement->GetDirectArray().Add(lVectors2);
    //lUVEmissiveElement->GetDirectArray().Add(lVectors3);

    //Now we have set the UVs as eIndexToDirect reference and in eByPolygonVertex  mapping mode
    //we must update the size of the index array.
    lUVDiffuseElement->GetIndexArray().SetCount(vertices.size());
    //lUVAmbientElement->GetIndexArray().SetCount(vertices.size());
    //lUVEmissiveElement->GetIndexArray().SetCount(vertices.size());

    // Create the triangle's polygon
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        //lMesh->BeginPolygon(-1, -1, false);
        lMesh->BeginPolygon(i);
        for (int j = 0; j < 3; ++j)
        {
            lMesh->AddPolygon(i * 3 + j); // Control point
            //Now we have to update the index array of the UVs for diffuse, ambient and emissive
            //lUVDiffuseElement->GetIndexArray().SetAt(i * 3 + j, j);
            //lUVAmbientElement->GetIndexArray().SetAt(i * 3 + j, j);
            //lUVEmissiveElement->GetIndexArray().SetAt(i * 3 + j, j);
        }
        lMesh->EndPolygon();
    }

    FbxNode* lNode = FbxNode::Create(pScene, pName);
    lNode->SetNodeAttribute(lMesh);
    lNode->SetShadingMode(FbxNode::eTextureShading);

    //CreateMaterials(pScene, lMesh, Triangleinfo);
    CreateTexture(pScene, lMesh);
    return lNode;
}

FbxNode* FbxKinect20::CreateTriangleK2(FbxScene* pScene, const char* pName, vector<vector<float>> Triangleinfo, std::vector<std::vector<float>> TriangleinfoColor)
{
    FbxMesh* lMesh = FbxMesh::Create(pScene, pName);

    // The vertices
    vector<FbxVector4> vertices;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        FbxVector4 vert1(Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]);
        FbxVector4 vert2(Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]);
        FbxVector4 vert3(Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]);
        vertices.push_back(vert1);
        vertices.push_back(vert2);
        vertices.push_back(vert3);
    }

    // Create control points.
    lMesh->InitControlPoints(vertices.size());
    FbxVector4* lControlPoints = lMesh->GetControlPoints();

    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        lControlPoints[i * 3 + 0] = vertices[i * 3 + 0];
        lControlPoints[i * 3 + 1] = vertices[i * 3 + 1];
        lControlPoints[i * 3 + 2] = vertices[i * 3 + 2];
    }

    // specify normals per control point.
    FbxVector4 lNormalP0(0.0, 0.0, 1.0);
    FbxGeometryElementNormal* lNormalElement = lMesh->CreateElementNormal();
    lNormalElement->SetMappingMode(FbxGeometryElement::eByControlPoint);
    lNormalElement->SetReferenceMode(FbxGeometryElement::eDirect);
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        lNormalElement->GetDirectArray().Add(lNormalP0);
        lNormalElement->GetDirectArray().Add(lNormalP0);
        lNormalElement->GetDirectArray().Add(lNormalP0);
    }

    // Set material mapping.
    FbxGeometryElementMaterial* lMaterialElement = lMesh->CreateElementMaterial();
    lMaterialElement->SetMappingMode(FbxGeometryElement::eByPolygon);
    lMaterialElement->SetReferenceMode(FbxGeometryElement::eIndexToDirect);

    // Create UV for Diffuse channel
    FbxGeometryElementUV* lUVDiffuseElement = lMesh->CreateElementUV(gDiffuseElementName);
    FBX_ASSERT(lUVDiffuseElement != NULL);
    lUVDiffuseElement->SetMappingMode(FbxGeometryElement::eByControlPoint);
    lUVDiffuseElement->SetReferenceMode(FbxGeometryElement::eDirect);

    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        double x, y;
        x = TriangleinfoColor[i][0] / 1920.0;
        y = (1080.0 - TriangleinfoColor[i][1]) / 1080.0;
        if (x > 1)
            x = 1;
        if (x < 0)
            x = 0;
        if (y > 1)
            y = 1;
        if (y < 0)
            y = 0;
        FbxVector2 lVectors0(x, y);
        x = TriangleinfoColor[i][2] / 1920.0;
        y = (1080.0 - TriangleinfoColor[i][3]) / 1080.0;
        if (x > 1)
            x = 1;
        if (x < 0)
            x = 0;
        if (y > 1)
            y = 1;
        if (y < 0)
            y = 0;
        FbxVector2 lVectors1(x, y);
        x = TriangleinfoColor[i][4] / 1920.0;
        y = (1080.0 - TriangleinfoColor[i][5]) / 1080.0;
        if (x > 1)
            x = 1;
        if (x < 0)
            x = 0;
        if (y > 1)
            y = 1;
        if (y < 0)
            y = 0;
        FbxVector2 lVectors2(x, y);
        lUVDiffuseElement->GetDirectArray().Add(lVectors0);
        lUVDiffuseElement->GetDirectArray().Add(lVectors1);
        lUVDiffuseElement->GetDirectArray().Add(lVectors2);
    }

    //FbxVector2 lVectors0, lVectors1, lVectors2, lVectors3;

    //// Create UV for Ambient channel
    //FbxGeometryElementUV* lUVAmbientElement = lMesh->CreateElementUV(gAmbientElementName);

    //lUVAmbientElement->SetMappingMode(FbxGeometryElement::eByPolygonVertex);
    //lUVAmbientElement->SetReferenceMode(FbxGeometryElement::eIndexToDirect);

    //lVectors0.Set(0, 0);
    //lVectors1.Set(1, 0);
    //lVectors2.Set(0, 0.418586879968643);
    //lVectors3.Set(1, 0.418586879968643);

    //lUVAmbientElement->GetDirectArray().Add(lVectors0);
    //lUVAmbientElement->GetDirectArray().Add(lVectors1);
    //lUVAmbientElement->GetDirectArray().Add(lVectors2);
    //lUVAmbientElement->GetDirectArray().Add(lVectors3);

    //// Create UV for Emissive channel
    //FbxGeometryElementUV* lUVEmissiveElement = lMesh->CreateElementUV(gEmissiveElementName);

    //lUVEmissiveElement->SetMappingMode(FbxGeometryElement::eByPolygonVertex);
    //lUVEmissiveElement->SetReferenceMode(FbxGeometryElement::eIndexToDirect);

    //lVectors0.Set(0.2343, 0);
    //lVectors1.Set(1, 0.555);
    //lVectors2.Set(0.333, 0.999);
    //lVectors3.Set(0.555, 0.666);

    //lUVEmissiveElement->GetDirectArray().Add(lVectors0);
    //lUVEmissiveElement->GetDirectArray().Add(lVectors1);
    //lUVEmissiveElement->GetDirectArray().Add(lVectors2);
    //lUVEmissiveElement->GetDirectArray().Add(lVectors3);

    //Now we have set the UVs as eIndexToDirect reference and in eByPolygonVertex  mapping mode
    //we must update the size of the index array.
    lUVDiffuseElement->GetIndexArray().SetCount(vertices.size());
    //lUVAmbientElement->GetIndexArray().SetCount(vertices.size());
    //lUVEmissiveElement->GetIndexArray().SetCount(vertices.size());

    // Create the triangle's polygon
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        //lMesh->BeginPolygon(-1, -1, false);
        lMesh->BeginPolygon(i);
        for (int j = 0; j < 3; ++j)
        {
            lMesh->AddPolygon(i * 3 + j); // Control point
            //Now we have to update the index array of the UVs for diffuse, ambient and emissive
            //lUVDiffuseElement->GetIndexArray().SetAt(i * 3 + j, j);
            //lUVAmbientElement->GetIndexArray().SetAt(i * 3 + j, j);
            //lUVEmissiveElement->GetIndexArray().SetAt(i * 3 + j, j);
        }
        lMesh->EndPolygon();
    }

    FbxNode* lNode = FbxNode::Create(pScene, pName);
    lNode->SetNodeAttribute(lMesh);
    lNode->SetShadingMode(FbxNode::eTextureShading);

    //CreateMaterials(pScene, lMesh, Triangleinfo);
    CreateTexture(pScene, lMesh);
    return lNode;
}

void FbxKinect20::Animate(FbxNode* pNode, FbxAnimLayer* pAnimLayer)
{
    FbxAnimCurve* lCurve = NULL;
    FbxTime lTime;
    int lKeyIndex = 0;

    lCurve = pNode->LclRotation.GetCurve(pAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();

        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(0.5);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 90.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(1.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 180.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(1.5);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 270.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(2.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 360.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lCurve->KeyModifyEnd();
    }
    lCurve = pNode->LclTranslation.GetCurve(pAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();

        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(2.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, -100.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(4.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lCurve->KeyModifyEnd();
    }
    lCurve = pNode->LclRotation.GetCurve(pAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();

        lTime.SetSecondDouble(2.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(2.5);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 90.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(3.5);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, -90.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(4.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lCurve->KeyModifyEnd();
    }
    lCurve = pNode->LclTranslation.GetCurve(pAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();

        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 75.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(2.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(4.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, -75.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lCurve->KeyModifyEnd();
    }

    // The upside down shape is at index 0 because it is the only one.
    // The cube has no shape so the function returns NULL is this case.
    //先通过MapShapeOnPyramid创建好要转化的形状，0-100为转化的过程，100为完全转化为shape中设定的形状
    FbxGeometry* lGeometry = (FbxGeometry*)pNode->GetNodeAttribute();
    lCurve = lGeometry->GetShapeChannel(0, 0, pAnimLayer, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();

        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(2.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 100.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lTime.SetSecondDouble(4.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        lCurve->KeyModifyEnd();
    }
}

// Create materials.
void FbxKinect20::CreateMaterials(FbxScene* pScene, FbxMesh* pMesh, vector<vector<float>> Triangles)
{
    //int i=0;
    for (int i = 0; i < Triangles.size(); i++)
    {
        FbxString lMaterialName = "material";
        FbxString lShadingName = "Phong";
        lMaterialName += i;
        FbxDouble3 lBlack(0.0, 0.0, 0.0);
        FbxDouble3 lRed(1.0, 0.0, 0.0);
        FbxDouble3 lColor;
        FbxSurfacePhong *lMaterial = FbxSurfacePhong::Create(pScene, lMaterialName.Buffer());


        // Generate triangle and secondary colors.
        lMaterial->Emissive.Set(lBlack);
        lMaterial->Ambient.Set(lRed);
        lColor = FbxDouble3(Triangles[i][11] / 255.0, Triangles[i][10] / 255.0, Triangles[i][9] / 255.0);
        lMaterial->Diffuse.Set(lColor);
        lMaterial->TransparencyFactor.Set(0.0);
        lMaterial->ShadingModel.Set(lShadingName);
        lMaterial->Shininess.Set(0.5);
        //get the node of mesh, add material for it.
        FbxNode* lNode = pMesh->GetNode();
        if (lNode)
            lNode->AddMaterial(lMaterial);
    }
}

void FbxKinect20::SetTriangleDefaultPosition(FbxNode* pTriangle, vector<Point3d> skeletonPoints3D)
{
    pTriangle->LclTranslation.Set(FbxVector4(0.0, 0.0, 0.0));
    pTriangle->LclRotation.Set(FbxVector4(0.0, 0.0, 0.0));
    pTriangle->LclScaling.Set(FbxVector4(1.0, 1.0, 1.0));
}

// Create texture.
void FbxKinect20::CreateTexture(FbxScene* pScene, FbxMesh* pMesh)
{
    // A texture need to be connected to a property on the material,
    // so let's use the material (if it exists) or create a new one
    FbxSurfacePhong* lMaterial = NULL;

    //get the node of mesh, add material for it.
    FbxNode* lNode = pMesh->GetNode();
    if (lNode)
    {
        lMaterial = lNode->GetSrcObject<FbxSurfacePhong>(0);
        if (lMaterial == NULL)
        {
            FbxString lMaterialName = "toto";
            FbxString lShadingName = "Phong";
            FbxDouble3 lBlack(0.0, 0.0, 0.0);
            FbxDouble3 lRed(1.0, 0.0, 0.0);
            FbxDouble3 lDiffuseColor(1.0, 1.0, 1.0);

            FbxLayer* lLayer = pMesh->GetLayer(0);

            // Create a layer element material to handle proper mapping.
            FbxLayerElementMaterial* lLayerElementMaterial = FbxLayerElementMaterial::Create(pMesh, lMaterialName.Buffer());

            // This allows us to control where the materials are mapped.  Using eAllSame
            // means that all faces/polygons of the mesh will be assigned the same material.
            lLayerElementMaterial->SetMappingMode(FbxLayerElement::eAllSame);
            lLayerElementMaterial->SetReferenceMode(FbxLayerElement::eIndexToDirect);

            // Save the material on the layer
            lLayer->SetMaterials(lLayerElementMaterial);

            // Add an index to the lLayerElementMaterial.  Since we have only one, and are using eAllSame mapping mode,
            // we only need to add one.
            lLayerElementMaterial->GetIndexArray().Add(0);

            lMaterial = FbxSurfacePhong::Create(pScene, lMaterialName.Buffer());

            // Generate primary and secondary colors.
            lMaterial->Emissive.Set(lBlack);
            lMaterial->Ambient.Set(lRed);
            lMaterial->AmbientFactor.Set(1.);
            // Add texture for diffuse channel
            lMaterial->Diffuse.Set(lDiffuseColor);
            lMaterial->DiffuseFactor.Set(1.0);
            lMaterial->TransparencyFactor.Set(1.0);
            lMaterial->ShadingModel.Set(lShadingName);
            lMaterial->Shininess.Set(0.5);
            lMaterial->Specular.Set(lBlack);
            lMaterial->SpecularFactor.Set(0.3);
            lNode->AddMaterial(lMaterial);
        }
    }

    FbxFileTexture* lTexture = FbxFileTexture::Create(pScene, "Diffuse Texture");

    // Set texture properties.
    lTexture->SetFileName("colorImg.png"); // Resource file is in current directory.
    lTexture->SetTextureUse(FbxTexture::eStandard);
    lTexture->SetMappingType(FbxTexture::eUV);
    lTexture->SetMaterialUse(FbxFileTexture::eModelMaterial);
    lTexture->SetSwapUV(false);
    lTexture->SetTranslation(0.0, 0.0);
    lTexture->SetScale(1.0, 1.0);
    lTexture->SetRotation(0.0, 0.0);
    lTexture->UVSet.Set(FbxString(gDiffuseElementName)); // Connect texture to the proper UV


    // don't forget to connect the texture to the corresponding property of the material
    if (lMaterial)
        lMaterial->Diffuse.ConnectSrcObject(lTexture);

    //lTexture = FbxFileTexture::Create(pScene, "Ambient Texture");

    //// Set texture properties.
    //lTexture->SetFileName("gradient.jpg"); // Resource file is in current directory.
    //lTexture->SetTextureUse(FbxTexture::eStandard);
    //lTexture->SetMappingType(FbxTexture::eUV);
    //lTexture->SetMaterialUse(FbxFileTexture::eModelMaterial);
    //lTexture->SetSwapUV(false);
    //lTexture->SetTranslation(0.0, 0.0);
    //lTexture->SetScale(1.0, 1.0);
    //lTexture->SetRotation(0.0, 0.0);
    //lTexture->UVSet.Set(FbxString(gAmbientElementName)); // Connect texture to the proper UV

    //// don't forget to connect the texture to the corresponding property of the material
    //if (lMaterial)
    //    lMaterial->Ambient.ConnectSrcObject(lTexture);

    //lTexture = FbxFileTexture::Create(pScene, "Emissive Texture");

    //// Set texture properties.
    //lTexture->SetFileName("spotty.jpg"); // Resource file is in current directory.
    //lTexture->SetTextureUse(FbxTexture::eStandard);
    //lTexture->SetMappingType(FbxTexture::eUV);
    //lTexture->SetMaterialUse(FbxFileTexture::eModelMaterial);
    //lTexture->SetSwapUV(false);
    //lTexture->SetTranslation(0.0, 0.0);
    //lTexture->SetScale(1.0, 1.0);
    //lTexture->SetRotation(0.0, 0.0);
    //lTexture->UVSet.Set(FbxString(gEmissiveElementName)); // Connect texture to the proper UV

    //// don't forget to connect the texture to the corresponding property of the material
    //if (lMaterial)
    //    lMaterial->Emissive.ConnectSrcObject(lTexture);
}

// Create patch. 
FbxNode* FbxKinect20::CreatePatch(FbxScene* pScene, const char* pName)
{
    FbxPatch* lPatch = FbxPatch::Create(pScene, pName);

    // Set patch properties.
    lPatch->InitControlPoints(8, FbxPatch::eBSpline, 7, FbxPatch::eBSpline);
    lPatch->SetStep(4, 4);//相邻控制点之间的划分数，越小分辨率越差；1*1即为控制点
    lPatch->SetClosed(true, false);

    FbxVector4* lVector4 = lPatch->GetControlPoints();//vector大小对应lPatch->InitControlPoints(8, FbxPatch::eBSpline, 7, FbxPatch::eBSpline);中的参数1和参数3
    int i;

    for (i = 0; i < 7; i++)//设置控点在空间中的位置
    {
        double lRadius = 10.0;
        double lSegmentLength = 20.0;
        lVector4[8 * i + 0].Set(lRadius, 0.0, (i - 3)*lSegmentLength);
        lVector4[8 * i + 1].Set(lRadius*sqrtf(2), -lRadius * sqrtf(2), (i - 3)*lSegmentLength);
        lVector4[8 * i + 2].Set(0.0, -lRadius, (i - 3)*lSegmentLength);
        lVector4[8 * i + 3].Set(-lRadius * sqrtf(2), -lRadius * sqrtf(2), (i - 3)*lSegmentLength);
        lVector4[8 * i + 4].Set(-lRadius, 0.0, (i - 3)*lSegmentLength);
        lVector4[8 * i + 5].Set(-lRadius * sqrtf(2), lRadius * sqrtf(2), (i - 3)*lSegmentLength);
        lVector4[8 * i + 6].Set(0.0, lRadius, (i - 3)*lSegmentLength);
        lVector4[8 * i + 7].Set(lRadius * sqrtf(2), lRadius * sqrtf(2), (i - 3)*lSegmentLength);
    }

    FbxNode* lNode = FbxNode::Create(pScene, pName);

    // Rotate the cylinder along the X axis so the axis
    // of the cylinder is the same as the bone axis (Y axis)竖起来
    FbxVector4 lR(-90.0, 0.0, 0.0);
    lNode->LclRotation.Set(lR);
    lNode->SetNodeAttribute(lPatch);

    return lNode;
}

// Create a skeleton.
FbxNode* FbxKinect20::CreateSkeleton(FbxScene* pScene, const char* pName, vector<Point3d> skeletonPoints3D)
{

    FbxString lRootName(pName);
    lRootName += "HipCenterRoot";
    FbxSkeleton* lHipCenterRootAttribute = FbxSkeleton::Create(pScene, pName);
    lHipCenterRootAttribute->SetSkeletonType(FbxSkeleton::eRoot);
    FbxNode* lHipCenterRoot = FbxNode::Create(pScene, lRootName.Buffer());
    lHipCenterRoot->SetNodeAttribute(lHipCenterRootAttribute);
    lHipCenterRoot->LclTranslation.Set(FbxVector4(skeletonPoints3D[0].x, skeletonPoints3D[0].y, skeletonPoints3D[0].z));

    FbxString lLimbNodeName1(pName);
    lLimbNodeName1 += "Spine";
    FbxSkeleton* lSpineAttribute1 = FbxSkeleton::Create(pScene, lLimbNodeName1);
    lSpineAttribute1->SetSkeletonType(FbxSkeleton::eLimbNode);
    lSpineAttribute1->Size.Set(1.0);
    FbxNode* lSpine = FbxNode::Create(pScene, lLimbNodeName1.Buffer());
    lSpine->SetNodeAttribute(lSpineAttribute1);
    lSpine->LclTranslation.Set(FbxVector4(skeletonPoints3D[1].x - skeletonPoints3D[0].x, skeletonPoints3D[1].y - skeletonPoints3D[0].y, skeletonPoints3D[1].z - skeletonPoints3D[0].z));
    lHipCenterRoot->AddChild(lSpine);

    FbxString lLimbNodeName2(pName);
    lLimbNodeName2 += "ShoulderCenter";
    FbxSkeleton* lShoulderCenterAttribute2 = FbxSkeleton::Create(pScene, lLimbNodeName2);
    lShoulderCenterAttribute2->SetSkeletonType(FbxSkeleton::eLimbNode);
    lShoulderCenterAttribute2->Size.Set(1.0);
    FbxNode* lShoulderCenter = FbxNode::Create(pScene, lLimbNodeName2.Buffer());
    lShoulderCenter->SetNodeAttribute(lShoulderCenterAttribute2);
    lShoulderCenter->LclTranslation.Set(FbxVector4(skeletonPoints3D[2].x - skeletonPoints3D[1].x, skeletonPoints3D[2].y - skeletonPoints3D[1].y, skeletonPoints3D[2].z - skeletonPoints3D[1].z));
    lSpine->AddChild(lShoulderCenter);

    FbxString lLimbNodeName3(pName);
    lLimbNodeName3 += "Head";
    FbxSkeleton* lHeadAttribute3 = FbxSkeleton::Create(pScene, lLimbNodeName3);
    lHeadAttribute3->SetSkeletonType(FbxSkeleton::eLimbNode);
    lHeadAttribute3->Size.Set(1.0);
    FbxNode* lHead = FbxNode::Create(pScene, lLimbNodeName3.Buffer());
    lHead->SetNodeAttribute(lHeadAttribute3);
    lHead->LclTranslation.Set(FbxVector4(skeletonPoints3D[3].x - skeletonPoints3D[2].x, skeletonPoints3D[3].y - skeletonPoints3D[2].y, skeletonPoints3D[3].z - skeletonPoints3D[2].z));
    lShoulderCenter->AddChild(lHead);

    FbxString lLimbNodeName4(pName);
    lLimbNodeName4 += "ShoulderLeft";
    FbxSkeleton* lShoulderLeftAttribute4 = FbxSkeleton::Create(pScene, lLimbNodeName4);
    lShoulderLeftAttribute4->SetSkeletonType(FbxSkeleton::eLimbNode);
    lShoulderLeftAttribute4->Size.Set(1.0);
    FbxNode* lShoulderLeft = FbxNode::Create(pScene, lLimbNodeName4.Buffer());
    lShoulderLeft->SetNodeAttribute(lShoulderLeftAttribute4);
    lShoulderLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[4].x - skeletonPoints3D[2].x, skeletonPoints3D[4].y - skeletonPoints3D[2].y, skeletonPoints3D[4].z - skeletonPoints3D[2].z));
    lShoulderCenter->AddChild(lShoulderLeft);

    FbxString lLimbNodeName5(pName);
    lLimbNodeName5 += "ElbowLeft";
    FbxSkeleton* lElbowLeftAttribute5 = FbxSkeleton::Create(pScene, lLimbNodeName5);
    lElbowLeftAttribute5->SetSkeletonType(FbxSkeleton::eLimbNode);
    lElbowLeftAttribute5->Size.Set(1.0);
    FbxNode* lElbowLeft = FbxNode::Create(pScene, lLimbNodeName5.Buffer());
    lElbowLeft->SetNodeAttribute(lElbowLeftAttribute5);
    lElbowLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[5].x - skeletonPoints3D[4].x, skeletonPoints3D[5].y - skeletonPoints3D[4].y, skeletonPoints3D[5].z - skeletonPoints3D[4].z));
    lShoulderLeft->AddChild(lElbowLeft);

    FbxString lLimbNodeName6(pName);
    lLimbNodeName6 += "WristLeft";
    FbxSkeleton* lWristLeftAttribute6 = FbxSkeleton::Create(pScene, lLimbNodeName6);
    lWristLeftAttribute6->SetSkeletonType(FbxSkeleton::eLimbNode);
    lWristLeftAttribute6->Size.Set(1.0);
    FbxNode* lWristLeft = FbxNode::Create(pScene, lLimbNodeName6.Buffer());
    lWristLeft->SetNodeAttribute(lWristLeftAttribute6);
    lWristLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[6].x - skeletonPoints3D[5].x, skeletonPoints3D[6].y - skeletonPoints3D[5].y, skeletonPoints3D[6].z - skeletonPoints3D[5].z));
    lElbowLeft->AddChild(lWristLeft);

    FbxString lLimbNodeName7(pName);
    lLimbNodeName7 += "HandLeft";
    FbxSkeleton* lHandLeftAttribute7 = FbxSkeleton::Create(pScene, lLimbNodeName7);
    lHandLeftAttribute7->SetSkeletonType(FbxSkeleton::eLimbNode);
    lHandLeftAttribute7->Size.Set(1.0);
    FbxNode* lHandLeft = FbxNode::Create(pScene, lLimbNodeName7.Buffer());
    lHandLeft->SetNodeAttribute(lHandLeftAttribute7);
    lHandLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[7].x - skeletonPoints3D[6].x, skeletonPoints3D[7].y - skeletonPoints3D[6].y, skeletonPoints3D[7].z - skeletonPoints3D[6].z));
    lWristLeft->AddChild(lHandLeft);

    FbxString lLimbNodeName8(pName);
    lLimbNodeName8 += "ShoulderRight";
    FbxSkeleton* lShoulderRightAttribute8 = FbxSkeleton::Create(pScene, lLimbNodeName8);
    lShoulderRightAttribute8->SetSkeletonType(FbxSkeleton::eLimbNode);
    lShoulderRightAttribute8->Size.Set(1.0);
    FbxNode* lShoulderRight = FbxNode::Create(pScene, lLimbNodeName8.Buffer());
    lShoulderRight->SetNodeAttribute(lShoulderRightAttribute8);
    lShoulderRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[8].x - skeletonPoints3D[2].x, skeletonPoints3D[8].y - skeletonPoints3D[2].y, skeletonPoints3D[8].z - skeletonPoints3D[2].z));
    lShoulderCenter->AddChild(lShoulderRight);

    FbxString lLimbNodeName9(pName);
    lLimbNodeName9 += "ElbowRight";
    FbxSkeleton* lElbowRightAttribute9 = FbxSkeleton::Create(pScene, lLimbNodeName9);
    lElbowRightAttribute9->SetSkeletonType(FbxSkeleton::eLimbNode);
    lElbowRightAttribute9->Size.Set(1.0);
    FbxNode* lElbowRight = FbxNode::Create(pScene, lLimbNodeName9.Buffer());
    lElbowRight->SetNodeAttribute(lElbowRightAttribute9);
    lElbowRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[9].x - skeletonPoints3D[8].x, skeletonPoints3D[9].y - skeletonPoints3D[8].y, skeletonPoints3D[9].z - skeletonPoints3D[8].z));
    lShoulderRight->AddChild(lElbowRight);

    FbxString lLimbNodeName10(pName);
    lLimbNodeName10 += "WristRight";
    FbxSkeleton* lWristRightAttribute10 = FbxSkeleton::Create(pScene, lLimbNodeName10);
    lWristRightAttribute10->SetSkeletonType(FbxSkeleton::eLimbNode);
    lWristRightAttribute10->Size.Set(1.0);
    FbxNode* lWristRight = FbxNode::Create(pScene, lLimbNodeName10.Buffer());
    lWristRight->SetNodeAttribute(lWristRightAttribute10);
    lWristRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[10].x - skeletonPoints3D[9].x, skeletonPoints3D[10].y - skeletonPoints3D[9].y, skeletonPoints3D[10].z - skeletonPoints3D[9].z));
    lElbowRight->AddChild(lWristRight);

    FbxString lLimbNodeName11(pName);
    lLimbNodeName11 += "HandRight";
    FbxSkeleton* lHandRightAttribute11 = FbxSkeleton::Create(pScene, lLimbNodeName11);
    lHandRightAttribute11->SetSkeletonType(FbxSkeleton::eLimbNode);
    lHandRightAttribute11->Size.Set(1.0);
    FbxNode* lHandRight = FbxNode::Create(pScene, lLimbNodeName11.Buffer());
    lHandRight->SetNodeAttribute(lHandRightAttribute11);
    lHandRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[11].x - skeletonPoints3D[10].x, skeletonPoints3D[11].y - skeletonPoints3D[10].y, skeletonPoints3D[11].z - skeletonPoints3D[10].z));
    lWristRight->AddChild(lHandRight);

    FbxString lLimbNodeName12(pName);
    lLimbNodeName12 += "HipLeft";
    FbxSkeleton* lHipLeftAttribute12 = FbxSkeleton::Create(pScene, lLimbNodeName12);
    lHipLeftAttribute12->SetSkeletonType(FbxSkeleton::eLimbNode);
    lHipLeftAttribute12->Size.Set(1.0);
    FbxNode* lHipLeft = FbxNode::Create(pScene, lLimbNodeName12.Buffer());
    lHipLeft->SetNodeAttribute(lHipLeftAttribute12);
    lHipLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[12].x - skeletonPoints3D[0].x, skeletonPoints3D[12].y - skeletonPoints3D[0].y, skeletonPoints3D[12].z - skeletonPoints3D[0].z));
    lHipCenterRoot->AddChild(lHipLeft);

    FbxString lLimbNodeName13(pName);
    lLimbNodeName13 += "KneeLeft";
    FbxSkeleton* lKneeLeftAttribute13 = FbxSkeleton::Create(pScene, lLimbNodeName13);
    lKneeLeftAttribute13->SetSkeletonType(FbxSkeleton::eLimbNode);
    lKneeLeftAttribute13->Size.Set(1.0);
    FbxNode* lKneeLeft = FbxNode::Create(pScene, lLimbNodeName13.Buffer());
    lKneeLeft->SetNodeAttribute(lKneeLeftAttribute13);
    lKneeLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[13].x - skeletonPoints3D[12].x, skeletonPoints3D[13].y - skeletonPoints3D[12].y, skeletonPoints3D[13].z - skeletonPoints3D[12].z));
    lHipLeft->AddChild(lKneeLeft);

    FbxString lLimbNodeName14(pName);
    lLimbNodeName14 += "AnkleLeft";
    FbxSkeleton* lAnkleLeftAttribute14 = FbxSkeleton::Create(pScene, lLimbNodeName14);
    lAnkleLeftAttribute14->SetSkeletonType(FbxSkeleton::eLimbNode);
    lAnkleLeftAttribute14->Size.Set(1.0);
    FbxNode* lAnkleLeft = FbxNode::Create(pScene, lLimbNodeName14.Buffer());
    lAnkleLeft->SetNodeAttribute(lAnkleLeftAttribute14);
    lAnkleLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[14].x - skeletonPoints3D[13].x, skeletonPoints3D[14].y - skeletonPoints3D[13].y, skeletonPoints3D[14].z - skeletonPoints3D[13].z));
    lKneeLeft->AddChild(lAnkleLeft);

    FbxString lLimbNodeName15(pName);
    lLimbNodeName15 += "FootLeft";
    FbxSkeleton* lFootLeftAttribute15 = FbxSkeleton::Create(pScene, lLimbNodeName15);
    lFootLeftAttribute15->SetSkeletonType(FbxSkeleton::eLimbNode);
    lFootLeftAttribute15->Size.Set(1.0);
    FbxNode* lFootLeft = FbxNode::Create(pScene, lLimbNodeName15.Buffer());
    lFootLeft->SetNodeAttribute(lFootLeftAttribute15);
    lFootLeft->LclTranslation.Set(FbxVector4(skeletonPoints3D[15].x - skeletonPoints3D[14].x, skeletonPoints3D[15].y - skeletonPoints3D[14].y, skeletonPoints3D[15].z - skeletonPoints3D[14].z));
    lAnkleLeft->AddChild(lFootLeft);

    FbxString lLimbNodeName16(pName);
    lLimbNodeName16 += "HipRight";
    FbxSkeleton* lHipRightAttribute16 = FbxSkeleton::Create(pScene, lLimbNodeName16);
    lHipRightAttribute16->SetSkeletonType(FbxSkeleton::eLimbNode);
    lHipRightAttribute16->Size.Set(1.0);
    FbxNode* lHipRight = FbxNode::Create(pScene, lLimbNodeName16.Buffer());
    lHipRight->SetNodeAttribute(lHipRightAttribute16);
    lHipRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[16].x - skeletonPoints3D[0].x, skeletonPoints3D[16].y - skeletonPoints3D[0].y, skeletonPoints3D[16].z - skeletonPoints3D[0].z));
    lHipCenterRoot->AddChild(lHipRight);

    FbxString lLimbNodeName17(pName);
    lLimbNodeName17 += "KneeRight";
    FbxSkeleton* lKneeRightAttribute17 = FbxSkeleton::Create(pScene, lLimbNodeName17);
    lKneeRightAttribute17->SetSkeletonType(FbxSkeleton::eLimbNode);
    lKneeRightAttribute17->Size.Set(1.0);
    FbxNode* lKneeRight = FbxNode::Create(pScene, lLimbNodeName17.Buffer());
    lKneeRight->SetNodeAttribute(lKneeRightAttribute17);
    lKneeRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[17].x - skeletonPoints3D[16].x, skeletonPoints3D[17].y - skeletonPoints3D[16].y, skeletonPoints3D[17].z - skeletonPoints3D[16].z));
    lHipRight->AddChild(lKneeRight);

    FbxString lLimbNodeName18(pName);
    lLimbNodeName18 += "AnkleRight";
    FbxSkeleton* lAnkleRightAttribute18 = FbxSkeleton::Create(pScene, lLimbNodeName18);
    lAnkleRightAttribute18->SetSkeletonType(FbxSkeleton::eLimbNode);
    lAnkleRightAttribute18->Size.Set(1.0);
    FbxNode* lAnkleRight = FbxNode::Create(pScene, lLimbNodeName18.Buffer());
    lAnkleRight->SetNodeAttribute(lAnkleRightAttribute18);
    lAnkleRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[18].x - skeletonPoints3D[17].x, skeletonPoints3D[18].y - skeletonPoints3D[17].y, skeletonPoints3D[18].z - skeletonPoints3D[17].z));
    lKneeRight->AddChild(lAnkleRight);

    FbxString lLimbNodeName19(pName);
    lLimbNodeName19 += "FootRight";
    FbxSkeleton* lFootRightAttribute19 = FbxSkeleton::Create(pScene, lLimbNodeName19);
    lFootRightAttribute19->SetSkeletonType(FbxSkeleton::eLimbNode);
    lFootRightAttribute19->Size.Set(1.0);
    FbxNode* lFootRight = FbxNode::Create(pScene, lLimbNodeName19.Buffer());
    lFootRight->SetNodeAttribute(lFootRightAttribute19);
    lFootRight->LclTranslation.Set(FbxVector4(skeletonPoints3D[19].x - skeletonPoints3D[18].x, skeletonPoints3D[19].y - skeletonPoints3D[18].y, skeletonPoints3D[19].z - skeletonPoints3D[18].z));
    lAnkleRight->AddChild(lFootRight);

    return lHipCenterRoot;
}

// Set the influence of the skeleton segments over the cylinder.
// The link mode is FbxCluster::eTotalOne which means the total
// of the weights assigned to a given control point must equal 1.
void FbxKinect20::LinkPatchToSkeleton(FbxScene* pScene, FbxNode* pPatch, FbxNode* pSkeletonRoot, vector<vector<float>> Triangleinfo, vector<Point3d> skeletonPoints)
{
    //int i, j;
    FbxAMatrix lXMatrix;

    FbxNode* lHipCenterRoot = pSkeletonRoot;
    FbxNode* lSpine = lHipCenterRoot->GetChild(0);
    FbxNode* lShoulderCenter = lSpine->GetChild(0);
    FbxNode* lHead = lShoulderCenter->GetChild(0);
    FbxNode* lShoulderLeft = lShoulderCenter->GetChild(1);
    FbxNode* lElbowLeft = lShoulderLeft->GetChild(0);
    FbxNode* lWristLeft = lElbowLeft->GetChild(0);
    FbxNode* lHandLeft = lWristLeft->GetChild(0);
    FbxNode* lShoulderRight = lShoulderCenter->GetChild(2);
    FbxNode* lElbowRight = lShoulderRight->GetChild(0);
    FbxNode* lWristRight = lElbowRight->GetChild(0);
    FbxNode* lHandRight = lWristRight->GetChild(0);
    FbxNode* lHipLeft = lHipCenterRoot->GetChild(1);
    FbxNode* lKneeLeft = lHipLeft->GetChild(0);
    FbxNode* lAnkleLeft = lKneeLeft->GetChild(0);
    FbxNode* lFootLeft = lAnkleLeft->GetChild(0);
    FbxNode* lHipRight = lHipCenterRoot->GetChild(2);
    FbxNode* lKneeRight = lHipRight->GetChild(0);
    FbxNode* lAnkleRight = lKneeRight->GetChild(0);
    FbxNode* lFootRight = lAnkleRight->GetChild(0);

    float threshold1, threshold2, threshold3, threshold4, threshold5, threshold6;
    threshold1 = 30;
    threshold2 = 20;
    threshold3 = 15;
    threshold4 = 10;
    threshold5 = 5;
    threshold6 = 3;

    vector<vector<int>> ControlPointInfo(Triangleinfo.size(), vector<int>(3, 0));

    // Spine.
    FbxCluster *lClusterToSpine = FbxCluster::Create(pScene, "");
    lClusterToSpine->SetLink(lSpine);
    lClusterToSpine->SetLinkMode(FbxCluster::eNormalize);
    int skeletonIndex = 1;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        //if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        //{
        //    lClusterToSpine->AddControlPointIndex(i * 3 + 0, 0.5);
        //    lClusterToSpine->AddControlPointIndex(i * 3 + 1, 0.5);
        //    lClusterToSpine->AddControlPointIndex(i * 3 + 2, 0.5);
        //}
        //if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        //{
        //    lClusterToSpine->AddControlPointIndex(i * 3 + 0, 1.0);
        //    lClusterToSpine->AddControlPointIndex(i * 3 + 1, 1.0);
        //    lClusterToSpine->AddControlPointIndex(i * 3 + 2, 1.0);
        //}
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][0], Triangleinfo[i][1]) < 60 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][3], Triangleinfo[i][4]) < 60 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][6], Triangleinfo[i][7]) < 60 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][0], Triangleinfo[i][1]) < 30 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][3], Triangleinfo[i][4]) < 30 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][6], Triangleinfo[i][7]) < 30 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[0].x, skeletonPoints[0].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold1)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 0, 0.25);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold1)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 1, 0.25);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold1)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 2, 0.25);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold2)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold2)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold2)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToSpine->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Shoulder Center.
    FbxCluster *lClusterToShoulderCenter = FbxCluster::Create(pScene, "");
    lClusterToShoulderCenter->SetLink(lShoulderCenter);
    lClusterToShoulderCenter->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 2;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][0], Triangleinfo[i][1]) < 60 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][3], Triangleinfo[i][4]) < 60 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][6], Triangleinfo[i][7]) < 60 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][0], Triangleinfo[i][1]) < 30 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][3], Triangleinfo[i][4]) < 30 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][6], Triangleinfo[i][7]) < 30 && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[1].x, skeletonPoints[1].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold1)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 0.25);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold1)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 0.25);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold1)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 0.25);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold2)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold2)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold2)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[1].x, skeletonPoints[1].y, skeletonPoints[1].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Shoulder Right.
    FbxCluster *lClusterToShoulderRight = FbxCluster::Create(pScene, "");
    lClusterToShoulderRight->SetLink(lShoulderRight);
    lClusterToShoulderRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 8;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4]))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4]))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[9].x, skeletonPoints[9].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[9].x, skeletonPoints[9].y, skeletonPoints[9].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[9].x, skeletonPoints[9].y, skeletonPoints[9].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[9].x, skeletonPoints[9].y, skeletonPoints[9].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[9].x, skeletonPoints[9].y, skeletonPoints[9].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[9].x, skeletonPoints[9].y, skeletonPoints[9].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[9].x, skeletonPoints[9].y, skeletonPoints[9].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Elbow Right.
    FbxCluster *lClusterToElbowRight = FbxCluster::Create(pScene, "");
    lClusterToElbowRight->SetLink(lElbowRight);
    lClusterToElbowRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 9;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4]))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4]))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1], 5))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4], 5))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][6], Triangleinfo[i][7], 5))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[10].x, skeletonPoints[10].y, skeletonPoints[10].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[10].x, skeletonPoints[10].y, skeletonPoints[10].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[10].x, skeletonPoints[10].y, skeletonPoints[10].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[10].x, skeletonPoints[10].y, skeletonPoints[10].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[10].x, skeletonPoints[10].y, skeletonPoints[10].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[10].x, skeletonPoints[10].y, skeletonPoints[10].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Wrist Right.
    FbxCluster *lClusterToWristRight = FbxCluster::Create(pScene, "");
    lClusterToWristRight->SetLink(lWristRight);
    lClusterToWristRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 10;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4]))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4]))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1], 2))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4], 2))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][6], Triangleinfo[i][7], 2))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[11].x, skeletonPoints[11].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[11].x, skeletonPoints[11].y, skeletonPoints[11].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[11].x, skeletonPoints[11].y, skeletonPoints[11].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[11].x, skeletonPoints[11].y, skeletonPoints[11].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[11].x, skeletonPoints[11].y, skeletonPoints[11].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold6)
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[11].x, skeletonPoints[11].y, skeletonPoints[11].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold6)
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[11].x, skeletonPoints[11].y, skeletonPoints[11].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold6)
        {
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Hand Right.
    FbxCluster *lClusterToHandRight = FbxCluster::Create(pScene, "");
    lClusterToHandRight->SetLink(lHandRight);
    lClusterToHandRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 11;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        //if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        //{
        //    lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 0.5);
        //    ControlPointInfo[i][0] = 1;
        //}
        //if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        //{
        //    lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 0.5);
        //    ControlPointInfo[i][1] = 1;
        //}
        //if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        //{
        //    lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 0.5);
        //    ControlPointInfo[i][2] = 1;
        //}
        //if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        //{
        //    lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 1.0);
        //    ControlPointInfo[i][0] = 1;
        //}
        //if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        //{
        //    lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 1.0);
        //    ControlPointInfo[i][1] = 1;
        //}
        //if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[10].x, skeletonPoints[10].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        //{
        //    lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 1.0);
        //    ControlPointInfo[i][2] = 1;
        //}
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold6)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold6)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold6)
        {
            lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Head.
    FbxCluster *lClusterToHead = FbxCluster::Create(pScene, "");
    lClusterToHead->SetLink(lHead);
    lClusterToHead->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 3;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /* if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         {
             lClusterToHead->AddControlPointIndex(i * 3 + 0, 0.5);
             lClusterToHead->AddControlPointIndex(i * 3 + 1, 0.5);
             lClusterToHead->AddControlPointIndex(i * 3 + 2, 0.5);
         }
         if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         {
             lClusterToHead->AddControlPointIndex(i * 3 + 0, 1.0);
             lClusterToHead->AddControlPointIndex(i * 3 + 1, 1.0);
             lClusterToHead->AddControlPointIndex(i * 3 + 2, 1.0);
         }*/
         //if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         //{
         //    lClusterToHead->AddControlPointIndex(i * 3 + 0, 0.5);
         //    ControlPointInfo[i][0] = 1;
         //}
         //if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         //{
         //    lClusterToHead->AddControlPointIndex(i * 3 + 1, 0.5);
         //    ControlPointInfo[i][1] = 1;
         //}
         //if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         //{
         //    lClusterToHead->AddControlPointIndex(i * 3 + 2, 0.5);
         //    ControlPointInfo[i][2] = 1;
         //}
         //if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         //{
         //    lClusterToHead->AddControlPointIndex(i * 3 + 0, 1.0);
         //    ControlPointInfo[i][0] = 1;
         //}
         //if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         //{
         //    lClusterToHead->AddControlPointIndex(i * 3 + 1, 1.0);
         //    ControlPointInfo[i][1] = 1;
         //}
         //if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         //{
         //    lClusterToHead->AddControlPointIndex(i * 3 + 2, 1.0);
         //    ControlPointInfo[i][2] = 1;
         //}
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold2)
        {
            lClusterToHead->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold2)
        {
            lClusterToHead->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold2)
        {
            lClusterToHead->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToHead->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToHead->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToHead->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Shoulder Left.
    FbxCluster *lClusterToShoulderLeft = FbxCluster::Create(pScene, "");
    lClusterToShoulderLeft->SetLink(lShoulderLeft);
    lClusterToShoulderLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 4;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[5].x, skeletonPoints[5].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[5].x, skeletonPoints[5].y, skeletonPoints[5].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[5].x, skeletonPoints[5].y, skeletonPoints[5].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[5].x, skeletonPoints[5].y, skeletonPoints[5].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[5].x, skeletonPoints[5].y, skeletonPoints[5].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[5].x, skeletonPoints[5].y, skeletonPoints[5].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[5].x, skeletonPoints[5].y, skeletonPoints[5].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Elbow Left.
    FbxCluster *lClusterToElbowLeft = FbxCluster::Create(pScene, "");
    lClusterToElbowLeft->SetLink(lElbowLeft);
    lClusterToElbowLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 5;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1], 5))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4], 5))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7], 5))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[6].x, skeletonPoints[6].y, skeletonPoints[6].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[6].x, skeletonPoints[6].y, skeletonPoints[6].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[6].x, skeletonPoints[6].y, skeletonPoints[6].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[6].x, skeletonPoints[6].y, skeletonPoints[6].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[6].x, skeletonPoints[6].y, skeletonPoints[6].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[6].x, skeletonPoints[6].y, skeletonPoints[6].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Wrist Left.
    FbxCluster *lClusterToWristLeft = FbxCluster::Create(pScene, "");
    lClusterToWristLeft->SetLink(lWristLeft);
    lClusterToWristLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 6;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1], 2))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4], 2))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][6], Triangleinfo[i][7], 2))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[7].x, skeletonPoints[7].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[7].x, skeletonPoints[7].y, skeletonPoints[7].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[7].x, skeletonPoints[7].y, skeletonPoints[7].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[7].x, skeletonPoints[7].y, skeletonPoints[7].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[7].x, skeletonPoints[7].y, skeletonPoints[7].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold6)
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[7].x, skeletonPoints[7].y, skeletonPoints[7].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold6)
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[7].x, skeletonPoints[7].y, skeletonPoints[7].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold6)
        {
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Hand Left.
    FbxCluster *lClusterToHandLeft = FbxCluster::Create(pScene, "");
    lClusterToHandLeft->SetLink(lHandLeft);
    lClusterToHandLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 7;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /* if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
         {
             lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 0.5);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
         {
             lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 0.5);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
         {
             lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 0.5);
             ControlPointInfo[i][2] = 1;
         }
         if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
         {
             lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 1.0);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
         {
             lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 1.0);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[6].x, skeletonPoints[6].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
         {
             lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 1.0);
             ControlPointInfo[i][2] = 1;
         }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold6)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold6)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold6)
        {
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Hip Center.
    FbxCluster *lClusterToHipCenter = FbxCluster::Create(pScene, "");
    lClusterToHipCenter->SetLink(lHipCenterRoot);
    lClusterToHipCenter->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 0;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /* if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         {
             lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 0.5);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         {
             lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 0.5);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
         {
             lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 0.5);
             ControlPointInfo[i][2] = 1;
         }
         if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         {
             lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 1.0);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         {
             lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 1.0);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
         {
             lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 1.0);
             ControlPointInfo[i][2] = 1;
         }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold1)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 0.25);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold1)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 0.25);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold1)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 0.25);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold2)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold2)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold2)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Hip Right.
    FbxCluster *lClusterToHipRight = FbxCluster::Create(pScene, "");
    lClusterToHipRight->SetLink(lHipRight);
    lClusterToHipRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 16;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[17].x, skeletonPoints[17].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[17].x, skeletonPoints[17].y, skeletonPoints[17].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[17].x, skeletonPoints[17].y, skeletonPoints[17].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[17].x, skeletonPoints[17].y, skeletonPoints[17].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[17].x, skeletonPoints[17].y, skeletonPoints[17].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[17].x, skeletonPoints[17].y, skeletonPoints[17].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[17].x, skeletonPoints[17].y, skeletonPoints[17].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Knee Right.
    FbxCluster *lClusterToKneeRight = FbxCluster::Create(pScene, "");
    lClusterToKneeRight->SetLink(lKneeRight);
    lClusterToKneeRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 17;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /* if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
         {
             lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 0.5);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
         {
             lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 0.5);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
         {
             lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 0.5);
             ControlPointInfo[i][2] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
         {
             lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 1.0);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
         {
             lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 1.0);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
         {
             lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 1.0);
             ControlPointInfo[i][2] = 1;
         }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[18].x, skeletonPoints[18].y, skeletonPoints[18].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[18].x, skeletonPoints[18].y, skeletonPoints[18].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[18].x, skeletonPoints[18].y, skeletonPoints[18].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[18].x, skeletonPoints[18].y, skeletonPoints[18].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[18].x, skeletonPoints[18].y, skeletonPoints[18].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[18].x, skeletonPoints[18].y, skeletonPoints[18].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Ankle Right.
    FbxCluster *lClusterToAnkleRight = FbxCluster::Create(pScene, "");
    lClusterToAnkleRight->SetLink(lAnkleRight);
    lClusterToAnkleRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 18;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /* if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
         {
             lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 0.5);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
         {
             lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 0.5);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
         {
             lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 0.5);
             ControlPointInfo[i][2] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
         {
             lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 1.0);
             ControlPointInfo[i][0] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
         {
             lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 1.0);
             ControlPointInfo[i][1] = 1;
         }
         if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[19].x, skeletonPoints[19].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
         {
             lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 1.0);
             ControlPointInfo[i][2] = 1;
         }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[19].x, skeletonPoints[19].y, skeletonPoints[19].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[19].x, skeletonPoints[19].y, skeletonPoints[19].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[19].x, skeletonPoints[19].y, skeletonPoints[19].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[19].x, skeletonPoints[19].y, skeletonPoints[19].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[19].x, skeletonPoints[19].y, skeletonPoints[19].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[19].x, skeletonPoints[19].y, skeletonPoints[19].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Foot Right.
    FbxCluster *lClusterToFootRight = FbxCluster::Create(pScene, "");
    lClusterToFootRight->SetLink(lFootRight);
    lClusterToFootRight->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 19;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][3], Triangleinfo[i][4], 5))
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[18].x, skeletonPoints[18].y, Triangleinfo[i][6], Triangleinfo[i][7], 5))
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Hip Left.
    FbxCluster *lClusterToHipLeft = FbxCluster::Create(pScene, "");
    lClusterToHipLeft->SetLink(lHipLeft);
    lClusterToHipLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 12;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        //{
        //    lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 0.5);
        //    ControlPointInfo[i][0] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        //{
        //    lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 0.5);
        //    ControlPointInfo[i][1] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        //{
        //    lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        //    ControlPointInfo[i][2] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        //{
        //    lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 1.0);
        //    ControlPointInfo[i][0] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        //{
        //    lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 1.0);
        //    ControlPointInfo[i][1] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[13].x, skeletonPoints[13].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        //{
        //    lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        //    ControlPointInfo[i][2] = 1;
        //}
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[13].x, skeletonPoints[13].y, skeletonPoints[13].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[13].x, skeletonPoints[13].y, skeletonPoints[13].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[13].x, skeletonPoints[13].y, skeletonPoints[13].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[13].x, skeletonPoints[13].y, skeletonPoints[13].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[13].x, skeletonPoints[13].y, skeletonPoints[13].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[13].x, skeletonPoints[13].y, skeletonPoints[13].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Knee Left.
    FbxCluster *lClusterToKneeLeft = FbxCluster::Create(pScene, "");
    lClusterToKneeLeft->SetLink(lKneeLeft);
    lClusterToKneeLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 13;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        //{
        //    lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 0.5);
        //    ControlPointInfo[i][0] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        //{
        //    lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 0.5);
        //    ControlPointInfo[i][1] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        //{
        //    lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        //    ControlPointInfo[i][2] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        //{
        //    lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 1.0);
        //    ControlPointInfo[i][0] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        //{
        //    lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 1.0);
        //    ControlPointInfo[i][1] = 1;
        //}
        //if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        //{
        //    lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        //    ControlPointInfo[i][2] = 1;
        //}
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[14].x, skeletonPoints[14].y, skeletonPoints[14].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[14].x, skeletonPoints[14].y, skeletonPoints[14].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[14].x, skeletonPoints[14].y, skeletonPoints[14].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[14].x, skeletonPoints[14].y, skeletonPoints[14].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[14].x, skeletonPoints[14].y, skeletonPoints[14].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[14].x, skeletonPoints[14].y, skeletonPoints[14].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Ankle Left.
    FbxCluster *lClusterToAnkleLeft = FbxCluster::Create(pScene, "");
    lClusterToAnkleLeft->SetLink(lAnkleLeft);
    lClusterToAnkleLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 14;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLinesL(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1]) && PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4])&& PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][6], Triangleinfo[i][7]))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1], 10))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4], 10))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold3&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][6], Triangleinfo[i][7], 10))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][6], Triangleinfo[i][7]) < threshold4&&PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[15].x, skeletonPoints[15].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[15].x, skeletonPoints[15].y, skeletonPoints[15].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold3)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[15].x, skeletonPoints[15].y, skeletonPoints[15].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold3)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[15].x, skeletonPoints[15].y, skeletonPoints[15].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold3)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[15].x, skeletonPoints[15].y, skeletonPoints[15].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[15].x, skeletonPoints[15].y, skeletonPoints[15].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[15].x, skeletonPoints[15].y, skeletonPoints[15].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }

    // Foot Left.
    FbxCluster *lClusterToFootLeft = FbxCluster::Create(pScene, "");
    lClusterToFootLeft->SetLink(lFootLeft);
    lClusterToFootLeft->SetLinkMode(FbxCluster::eNormalize);
    skeletonIndex = 15;
    for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1)
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 2, 0.5);
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2)
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 2, 1.0);
        }*/
        /*if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1], 0))
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4], 0))
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold1 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7], 0))
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePoints(Triangleinfo[i][0], Triangleinfo[i][1], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][0], Triangleinfo[i][1], 5))
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePoints(Triangleinfo[i][3], Triangleinfo[i][4], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][3], Triangleinfo[i][4], 5))
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePoints(Triangleinfo[i][6], Triangleinfo[i][7], skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y) < threshold2 && !PointBetweenLines(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[14].x, skeletonPoints[14].y, Triangleinfo[i][6], Triangleinfo[i][7], 5))
        {
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }*/
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold4)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 0.5);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold4)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 0.5);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold4)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 0.5);
            ControlPointInfo[i][2] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2]) < threshold5)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            ControlPointInfo[i][0] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5]) < threshold5)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            ControlPointInfo[i][1] = 1;
        }
        if (DistancePointToLine3D(skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, skeletonPoints[skeletonIndex].x, skeletonPoints[skeletonIndex].y, skeletonPoints[skeletonIndex].z, Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8]) < threshold5)
        {
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            ControlPointInfo[i][2] = 1;
        }
    }
    vector<Point2f> skeletonJoints;
    Point2f Joint(0, 0);
    Joint.x = (skeletonPoints[1].x + skeletonPoints[0].x) / 2.0;
    Joint.y = (skeletonPoints[1].y + skeletonPoints[0].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[2].x + skeletonPoints[1].x) / 2.0;
    Joint.y = (skeletonPoints[2].y + skeletonPoints[1].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[8].x + skeletonPoints[9].x) / 2.0;
    Joint.y = (skeletonPoints[8].y + skeletonPoints[9].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[9].x + skeletonPoints[10].x) / 2.0;
    Joint.y = (skeletonPoints[9].y + skeletonPoints[10].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[10].x;
    Joint.y = skeletonPoints[10].y;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[11].x;
    Joint.y = skeletonPoints[11].y;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[3].x;
    Joint.y = skeletonPoints[3].y;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[4].x + skeletonPoints[5].x) / 2.0;
    Joint.y = (skeletonPoints[4].y + skeletonPoints[5].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[5].x + skeletonPoints[6].x) / 2.0;
    Joint.y = (skeletonPoints[5].y + skeletonPoints[6].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[6].x;
    Joint.y = skeletonPoints[6].y;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[7].x;
    Joint.y = skeletonPoints[7].y;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[0].x;
    Joint.y = skeletonPoints[0].y;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[16].x + skeletonPoints[17].x) / 2.0;
    Joint.y = (skeletonPoints[16].y + skeletonPoints[17].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[17].x + skeletonPoints[18].x) / 2.0;
    Joint.y = (skeletonPoints[17].y + skeletonPoints[18].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[18].x;
    Joint.y = skeletonPoints[18].y;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[19].x;
    Joint.y = skeletonPoints[19].y;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[12].x + skeletonPoints[13].x) / 2.0;
    Joint.y = (skeletonPoints[12].y + skeletonPoints[13].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = (skeletonPoints[13].x + skeletonPoints[14].x) / 2.0;
    Joint.y = (skeletonPoints[13].y + skeletonPoints[14].y) / 2.0;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[14].x;
    Joint.y = skeletonPoints[14].y;
    skeletonJoints.push_back(Joint);
    Joint.x = skeletonPoints[15].x;
    Joint.y = skeletonPoints[15].x;
    skeletonJoints.push_back(Joint);
    int count = 0;
    int count1 = 0;
    for (int i = 0; i < ControlPointInfo.size(); ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            if (ControlPointInfo[i][j] == 0)
            {
                count++;
            }
            if (ControlPointInfo[i][j] == 1)
            {
                count1++;
            }
        }
    }

    for (int i = 0; i < ControlPointInfo.size(); ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            if (ControlPointInfo[i][j] == 0)
            {
                float mindis = 10000;
                int index = 0;
                for (int k = 0; k < skeletonJoints.size(); ++k)
                {
                    float dis = DistancePoints(Triangleinfo[i][j * 3], Triangleinfo[i][j * 3 + 1], skeletonJoints[k].x, skeletonJoints[k].y);
                    if (mindis > dis)
                    {
                        mindis = dis;
                        index = k;
                    }
                }
                switch (index)
                {
                case 0:
                    lClusterToSpine->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 1:
                    lClusterToShoulderCenter->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 2:
                    lClusterToShoulderRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 3:
                    lClusterToElbowRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 4:
                    lClusterToWristRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 5:
                    lClusterToHandRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 6:
                    lClusterToHead->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 7:
                    lClusterToShoulderLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 8:
                    lClusterToElbowLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 9:
                    lClusterToWristLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 10:
                    lClusterToHandLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 11:
                    lClusterToHipCenter->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 12:
                    lClusterToHipRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 13:
                    lClusterToKneeRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 14:
                    lClusterToAnkleRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 15:
                    lClusterToFootRight->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 16:
                    lClusterToHipLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 17:
                    lClusterToKneeLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 18:
                    lClusterToAnkleLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                case 19:
                    lClusterToFootLeft->AddControlPointIndex(i * 3 + j, 0.5);
                    break;
                default:
                    break;
                }
            }
        }
    }
    /*for (int i = 0; i < Triangleinfo.size(); ++i)
    {
        float mindis = DistancePoints3D(Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2], skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z)+ DistancePoints3D(Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5], skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z)+ DistancePoints3D(Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8], skeletonPoints[0].x, skeletonPoints[0].y, skeletonPoints[0].z);
        int minindex = 0;
        for (int j = 1; j < skeletonPoints.size(); ++j)
        {
            float dis = DistancePoints3D(Triangleinfo[i][0], Triangleinfo[i][1], Triangleinfo[i][2], skeletonPoints[j].x, skeletonPoints[j].y, skeletonPoints[j].z) + DistancePoints3D(Triangleinfo[i][3], Triangleinfo[i][4], Triangleinfo[i][5], skeletonPoints[j].x, skeletonPoints[j].y, skeletonPoints[j].z) + DistancePoints3D(Triangleinfo[i][6], Triangleinfo[i][7], Triangleinfo[i][8], skeletonPoints[j].x, skeletonPoints[j].y, skeletonPoints[j].z);
            if (mindis > dis)
            {
                mindis = dis;
                minindex = j;
            }
        }
        switch (minindex)
        {
        case 0:
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHipCenter->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 1:
            lClusterToSpine->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToSpine->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToSpine->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 2:
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToShoulderCenter->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 3:
            lClusterToHead->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHead->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHead->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 4:
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToShoulderLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 5:
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToElbowLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 6:
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToWristLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 7:
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHandLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 8:
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToShoulderRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 9:
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToElbowRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 10:
            lClusterToWristRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToWristRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToWristRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 11:
            lClusterToHandRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHandRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHandRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 12:
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHipLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 13:
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToKneeLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 14:
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToAnkleLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 15:
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToFootLeft->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 16:
            lClusterToHipRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToHipRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToHipRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 17:
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToKneeRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 18:
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToAnkleRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        case 19:
            lClusterToFootRight->AddControlPointIndex(i * 3 + 0, 1.0);
            lClusterToFootRight->AddControlPointIndex(i * 3 + 1, 1.0);
            lClusterToFootRight->AddControlPointIndex(i * 3 + 2, 1.0);
            break;
        default:
            break;
        }
    }*/

    // Now we have the Patch and the skeleton correctly positioned,
    // set the Transform and TransformLink matrix accordingly.
    FbxScene* lScene = pPatch->GetScene();
    if (lScene) lXMatrix = pPatch->EvaluateGlobalTransform();
    //Transform matrix
    lClusterToSpine->SetTransformMatrix(lXMatrix);
    lClusterToShoulderCenter->SetTransformMatrix(lXMatrix);
    lClusterToShoulderRight->SetTransformMatrix(lXMatrix);
    lClusterToElbowRight->SetTransformMatrix(lXMatrix);
    lClusterToWristRight->SetTransformMatrix(lXMatrix);
    lClusterToHandRight->SetTransformMatrix(lXMatrix);
    lClusterToHead->SetTransformMatrix(lXMatrix);
    lClusterToShoulderLeft->SetTransformMatrix(lXMatrix);
    lClusterToElbowLeft->SetTransformMatrix(lXMatrix);
    lClusterToWristLeft->SetTransformMatrix(lXMatrix);
    lClusterToHandLeft->SetTransformMatrix(lXMatrix);
    lClusterToHipCenter->SetTransformMatrix(lXMatrix);
    lClusterToHipRight->SetTransformMatrix(lXMatrix);
    lClusterToKneeRight->SetTransformMatrix(lXMatrix);
    lClusterToAnkleRight->SetTransformMatrix(lXMatrix);
    lClusterToFootRight->SetTransformMatrix(lXMatrix);
    lClusterToHipLeft->SetTransformMatrix(lXMatrix);
    lClusterToKneeLeft->SetTransformMatrix(lXMatrix);
    lClusterToAnkleLeft->SetTransformMatrix(lXMatrix);
    lClusterToFootLeft->SetTransformMatrix(lXMatrix);

    //TransformLink matrix
    if (lScene) lXMatrix = lSpine->EvaluateGlobalTransform();
    lClusterToSpine->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lShoulderCenter->EvaluateGlobalTransform();
    lClusterToShoulderCenter->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lShoulderRight->EvaluateGlobalTransform();
    lClusterToShoulderRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lElbowRight->EvaluateGlobalTransform();
    lClusterToElbowRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lWristRight->EvaluateGlobalTransform();
    lClusterToWristRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lHandRight->EvaluateGlobalTransform();
    lClusterToHandRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lHead->EvaluateGlobalTransform();
    lClusterToHead->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lShoulderLeft->EvaluateGlobalTransform();
    lClusterToShoulderLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lElbowLeft->EvaluateGlobalTransform();
    lClusterToElbowLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lWristLeft->EvaluateGlobalTransform();
    lClusterToWristLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lHandLeft->EvaluateGlobalTransform();
    lClusterToHandLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lHipCenterRoot->EvaluateGlobalTransform();
    lClusterToHipCenter->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lHipRight->EvaluateGlobalTransform();
    lClusterToHipRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lKneeRight->EvaluateGlobalTransform();
    lClusterToKneeRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lAnkleRight->EvaluateGlobalTransform();
    lClusterToAnkleRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lFootRight->EvaluateGlobalTransform();
    lClusterToFootRight->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lHipLeft->EvaluateGlobalTransform();
    lClusterToHipLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lKneeLeft->EvaluateGlobalTransform();
    lClusterToKneeLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lAnkleLeft->EvaluateGlobalTransform();
    lClusterToAnkleLeft->SetTransformLinkMatrix(lXMatrix);

    if (lScene) lXMatrix = lFootLeft->EvaluateGlobalTransform();
    lClusterToFootLeft->SetTransformLinkMatrix(lXMatrix);

    // Add the clusters to the patch by creating a skin and adding those clusters to that skin.
    // After add that skin.

    FbxGeometry* lPatchAttribute = (FbxGeometry*)pPatch->GetNodeAttribute();
    FbxSkin* lSkin = FbxSkin::Create(pScene, "");
    lSkin->AddCluster(lClusterToSpine);
    lSkin->AddCluster(lClusterToShoulderCenter);
    lSkin->AddCluster(lClusterToShoulderRight);
    lSkin->AddCluster(lClusterToElbowRight);
    lSkin->AddCluster(lClusterToWristRight);
    lSkin->AddCluster(lClusterToHandRight);
    lSkin->AddCluster(lClusterToHead);
    lSkin->AddCluster(lClusterToShoulderLeft);
    lSkin->AddCluster(lClusterToElbowLeft);
    lSkin->AddCluster(lClusterToWristLeft);
    lSkin->AddCluster(lClusterToHandLeft);
    lSkin->AddCluster(lClusterToHipCenter);
    lSkin->AddCluster(lClusterToHipRight);
    lSkin->AddCluster(lClusterToKneeRight);
    lSkin->AddCluster(lClusterToAnkleRight);
    lSkin->AddCluster(lClusterToFootRight);
    lSkin->AddCluster(lClusterToHipLeft);
    lSkin->AddCluster(lClusterToKneeLeft);
    lSkin->AddCluster(lClusterToAnkleLeft);
    lSkin->AddCluster(lClusterToFootLeft);
    lPatchAttribute->AddDeformer(lSkin);

}

// Create animation stack.
void FbxKinect20::AnimateSkeleton(FbxScene* pScene, FbxNode* pSkeletonRoot, FbxAnimLayer* lAnimLayer, vector<Point3d> skeletonPoints3D, vector<vector<vector<double>>> RotationMatrixSequence)
{
    FbxTime lTime;
    int lKeyIndex = 0;

    FbxNode* lHipCenter = pSkeletonRoot;
    FbxNode* lSpine = lHipCenter->GetChild(0);
    FbxNode* lShoulderCenter = lSpine->GetChild(0);
    FbxNode* lHead = lShoulderCenter->GetChild(0);
    FbxNode* lShoulderLeft = lShoulderCenter->GetChild(1);
    FbxNode* lElbowLeft = lShoulderLeft->GetChild(0);
    FbxNode* lWristLeft = lElbowLeft->GetChild(0);
    FbxNode* lHandLeft = lWristLeft->GetChild(0);
    FbxNode* lShoulderRight = lShoulderCenter->GetChild(2);
    FbxNode* lElbowRight = lShoulderRight->GetChild(0);
    FbxNode* lWristRight = lElbowRight->GetChild(0);
    FbxNode* lHandRight = lWristRight->GetChild(0);
    FbxNode* lHipLeft = lHipCenter->GetChild(1);
    FbxNode* lKneeLeft = lHipLeft->GetChild(0);
    FbxNode* lAnkleLeft = lKneeLeft->GetChild(0);
    FbxNode* lFootLeft = lAnkleLeft->GetChild(0);
    FbxNode* lHipRight = lHipCenter->GetChild(2);
    FbxNode* lKneeRight = lHipRight->GetChild(0);
    FbxNode* lAnkleRight = lKneeRight->GetChild(0);
    FbxNode* lFootRight = lAnkleRight->GetChild(0);

    FbxAnimCurve* lCurve;
    lCurve = lHipCenter->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][0]- RotationMatrixSequence[0][0][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipCenter->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            if (i == 0)
            {
                lTime.SetSecondDouble(m_TimeStep*(i + 1));
                lKeyIndex = lCurve->KeyAdd(lTime);
                lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[1][0][1] - RotationMatrixSequence[1][0][1]);
                lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
            }
            else
            {
                lTime.SetSecondDouble(m_TimeStep*(i + 1));
                lKeyIndex = lCurve->KeyAdd(lTime);
                lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][1] - RotationMatrixSequence[1][0][1]);
                lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
            }
        }
    }
    lCurve = lHipCenter->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][2] - RotationMatrixSequence[0][0][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipCenter->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[0].x - skeletonPoints3D[15].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][0][3] - RotationMatrixSequence[i - 1][0][3]) < positionthreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i-1][0][3]);
            //    RotationMatrixSequence[i][0][3] = RotationMatrixSequence[i - 1][0][3];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][3]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][3] - skeletonPoints3D[15].x);//RotationMatrixSequence[i][0][3] - skeletonPoints3D[15].x
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipCenter->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[0].y - skeletonPoints3D[15].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][0][4] - RotationMatrixSequence[i - 1][0][4]) < positionthreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][0][4]);
            //    RotationMatrixSequence[i][0][4] = RotationMatrixSequence[i - 1][0][4];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][4]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][4] - skeletonPoints3D[15].y);//RotationMatrixSequence[i][0][4] - skeletonPoints3D[15].y
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipCenter->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[0].z - skeletonPoints3D[15].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][0][5] - RotationMatrixSequence[i - 1][0][5]) < positionthreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][0][5]);
            //    RotationMatrixSequence[i][0][5] = RotationMatrixSequence[i - 1][0][5];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][5]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][0][5] - skeletonPoints3D[15].z);//RotationMatrixSequence[i][0][5] - skeletonPoints3D[15].z
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lSpine->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][1][0] - RotationMatrixSequence[i - 1][1][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i-1][1][0]);
            //    RotationMatrixSequence[i][1][0] = RotationMatrixSequence[i - 1][1][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lSpine->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][1][1] - RotationMatrixSequence[i - 1][1][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][1][1]);
            //    RotationMatrixSequence[i][1][1] = RotationMatrixSequence[i - 1][1][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lSpine->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][1][2] - RotationMatrixSequence[i - 1][1][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][1][2]);
            //    RotationMatrixSequence[i][1][2] = RotationMatrixSequence[i - 1][1][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lSpine->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[1].x- skeletonPoints3D[0].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lSpine->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[1].y- skeletonPoints3D[0].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lSpine->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[1].z- skeletonPoints3D[0].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][1][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lShoulderCenter->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderCenter->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            if (i == 0)
            {
                lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[1][2][1]- RotationMatrixSequence[1][2][1]);
            }
            else
            {
                lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][1] - RotationMatrixSequence[1][2][1]);
            }
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderCenter->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][2][2] - RotationMatrixSequence[i - 1][2][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][2][2]);
            //    RotationMatrixSequence[i][2][2] = RotationMatrixSequence[i - 1][2][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lShoulderCenter->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[2].x - skeletonPoints3D[1].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderCenter->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[2].y - skeletonPoints3D[1].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderCenter->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[2].z - skeletonPoints3D[1].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][2][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lHead->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][3][0] - RotationMatrixSequence[i - 1][3][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][3][0]);
            //    RotationMatrixSequence[i][3][0] = RotationMatrixSequence[i - 1][3][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHead->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][3][1] - RotationMatrixSequence[i - 1][3][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][3][1]);
            //    RotationMatrixSequence[i][3][1] = RotationMatrixSequence[i - 1][3][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHead->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][3][2] - RotationMatrixSequence[i - 1][3][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][3][2]);
            //    RotationMatrixSequence[i][3][2] = RotationMatrixSequence[i - 1][3][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lHead->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[3].x - skeletonPoints3D[2].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHead->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[3].y - skeletonPoints3D[2].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHead->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[3].z - skeletonPoints3D[2].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][3][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lShoulderLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][5][0] - RotationMatrixSequence[i - 1][5][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][5][0]);
            //    RotationMatrixSequence[i][5][0] = RotationMatrixSequence[i - 1][5][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][5][1] - RotationMatrixSequence[i - 1][5][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][5][1]);
            //    RotationMatrixSequence[i][5][1] = RotationMatrixSequence[i - 1][5][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][5][2] - RotationMatrixSequence[i - 1][5][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][5][2]);
            //    RotationMatrixSequence[i][5][2] = RotationMatrixSequence[i - 1][5][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lShoulderLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[4].x - skeletonPoints3D[2].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][4][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[4].y - skeletonPoints3D[2].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][4][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[4].z - skeletonPoints3D[2].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][4][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lElbowLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][6][0] - RotationMatrixSequence[i - 1][6][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][6][0]);
            //    RotationMatrixSequence[i][6][0] = RotationMatrixSequence[i - 1][6][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][6][1] - RotationMatrixSequence[i - 1][6][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][6][1]);
            //    RotationMatrixSequence[i][6][1] = RotationMatrixSequence[i - 1][6][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][6][2] - RotationMatrixSequence[i - 1][6][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][6][2]);
            //    RotationMatrixSequence[i][6][2] = RotationMatrixSequence[i - 1][6][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lElbowLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[5].x - skeletonPoints3D[4].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[5].y - skeletonPoints3D[4].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[5].z - skeletonPoints3D[4].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][5][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lWristLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][7][0] - RotationMatrixSequence[i - 1][7][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][7][0]);
            //    RotationMatrixSequence[i][7][0] = RotationMatrixSequence[i - 1][7][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][7][1] - RotationMatrixSequence[i - 1][7][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][7][1]);
            //    RotationMatrixSequence[i][7][1] = RotationMatrixSequence[i - 1][7][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][7][2] - RotationMatrixSequence[i - 1][7][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][7][2]);
            //    RotationMatrixSequence[i][7][2] = RotationMatrixSequence[i - 1][7][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lWristLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[6].x - skeletonPoints3D[5].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[6].y - skeletonPoints3D[5].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[6].z - skeletonPoints3D[5].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][6][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lHandLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0.0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHandLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0.0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHandLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0.0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lHandLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[7].x - skeletonPoints3D[6].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHandLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[7].y - skeletonPoints3D[6].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHandLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[7].z - skeletonPoints3D[6].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][7][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/

    lCurve = lShoulderRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][9][0] - RotationMatrixSequence[i - 1][9][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][9][0]);
            //    RotationMatrixSequence[i][9][0] = RotationMatrixSequence[i - 1][9][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][9][1] - RotationMatrixSequence[i - 1][9][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][9][1]);
            //    RotationMatrixSequence[i][9][1] = RotationMatrixSequence[i - 1][9][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lShoulderRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][9][2] - RotationMatrixSequence[i - 1][9][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][9][2]);
            //    RotationMatrixSequence[i][9][2] = RotationMatrixSequence[i - 1][9][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /* lCurve = lShoulderRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[8].x - skeletonPoints3D[2].x);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][8][3]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }
     lCurve = lShoulderRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[8].y - skeletonPoints3D[2].y);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][8][4]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }
     lCurve = lShoulderRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[8].z - skeletonPoints3D[2].z);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][8][5]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }*/
    lCurve = lElbowRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][10][0] - RotationMatrixSequence[i - 1][10][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][10][0]);
            //    RotationMatrixSequence[i][10][0] = RotationMatrixSequence[i - 1][10][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][10][1] - RotationMatrixSequence[i - 1][10][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][10][1]);
            //    RotationMatrixSequence[i][10][1] = RotationMatrixSequence[i - 1][10][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][10][2] - RotationMatrixSequence[i - 1][10][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][10][2]);
            //    RotationMatrixSequence[i][10][2] = RotationMatrixSequence[i - 1][10][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lElbowRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[9].x - skeletonPoints3D[8].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[9].y - skeletonPoints3D[8].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lElbowRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[9].z - skeletonPoints3D[8].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][9][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lWristRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][11][0] - RotationMatrixSequence[i - 1][11][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][11][0]);
            //    RotationMatrixSequence[i][11][0] = RotationMatrixSequence[i - 1][11][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][11][1] - RotationMatrixSequence[i - 1][11][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][11][1]);
            //    RotationMatrixSequence[i][11][1] = RotationMatrixSequence[i - 1][11][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][11][2] - RotationMatrixSequence[i - 1][11][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][11][2]);
            //    RotationMatrixSequence[i][11][2] = RotationMatrixSequence[i - 1][11][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lWristRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[10].x - skeletonPoints3D[9].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[10].y - skeletonPoints3D[9].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lWristRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[10].z - skeletonPoints3D[9].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][10][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lHandRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0.0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHandRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0.0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHandRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0.0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /* lCurve = lHandRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[11].x - skeletonPoints3D[10].x);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][3]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }
     lCurve = lHandRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[11].y - skeletonPoints3D[10].y);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][4]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }
     lCurve = lHandRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[11].z - skeletonPoints3D[10].z);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][11][5]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }*/

    lCurve = lHipLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][13][0] - RotationMatrixSequence[i - 1][13][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][13][0]);
            //    RotationMatrixSequence[i][13][0] = RotationMatrixSequence[i - 1][13][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][13][1] - RotationMatrixSequence[i - 1][13][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][13][1]);
            //    RotationMatrixSequence[i][13][1] = RotationMatrixSequence[i - 1][13][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][13][2] - RotationMatrixSequence[i - 1][13][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][13][2]);
            //    RotationMatrixSequence[i][13][2] = RotationMatrixSequence[i - 1][13][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[12].x - skeletonPoints3D[0].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][12][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[12].y - skeletonPoints3D[0].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][12][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[12].z - skeletonPoints3D[0].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][12][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][14][0] - RotationMatrixSequence[i - 1][14][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][14][0]);
            //    RotationMatrixSequence[i][14][0] = RotationMatrixSequence[i - 1][14][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][14][1] - RotationMatrixSequence[i - 1][14][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][14][1]);
            //    RotationMatrixSequence[i][14][1] = RotationMatrixSequence[i - 1][14][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][14][2] - RotationMatrixSequence[i - 1][14][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][14][2]);
            //    RotationMatrixSequence[i][14][2] = RotationMatrixSequence[i - 1][14][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    //lCurve = lKneeLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[13].x - skeletonPoints3D[12].x);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][3]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    //lCurve = lKneeLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[13].y - skeletonPoints3D[12].y);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][4]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    //lCurve = lKneeLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[13].z - skeletonPoints3D[12].z);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][13][5]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    lCurve = lAnkleLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][15][0] - RotationMatrixSequence[i - 1][15][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][15][0]);
            //    RotationMatrixSequence[i][15][0] = RotationMatrixSequence[i - 1][15][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lAnkleLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][15][1] - RotationMatrixSequence[i - 1][15][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][15][1]);
            //    RotationMatrixSequence[i][15][1] = RotationMatrixSequence[i - 1][15][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lAnkleLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][15][2] - RotationMatrixSequence[i - 1][15][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][15][2]);
            //    RotationMatrixSequence[i][15][2] = RotationMatrixSequence[i - 1][15][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    //lCurve = lAnkleLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[14].x - skeletonPoints3D[13].x);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][3]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    //lCurve = lAnkleLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[14].y - skeletonPoints3D[13].y);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][4]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    //lCurve = lAnkleLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[14].z - skeletonPoints3D[13].z);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][14][5]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    lCurve = lFootLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lFootLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lFootLeft->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    //lCurve = lFootLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[15].x - skeletonPoints3D[14].x);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][3]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    //lCurve = lFootLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[15].y - skeletonPoints3D[14].y);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][4]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    //lCurve = lFootLeft->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    //if (lCurve)
    //{
    //    lCurve->KeyModifyBegin();
    //    lTime.SetSecondDouble(0.0);
    //    lKeyIndex = lCurve->KeyAdd(lTime);
    //    lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[15].z - skeletonPoints3D[14].z);
    //    lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //
    //    for (int i = 0; i < RotationMatrixSequence.size(); ++i)
    //    {
    //        lTime.SetSecondDouble(m_TimeStep*(i + 1));
    //        lKeyIndex = lCurve->KeyAdd(lTime);
    //        lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][15][5]);
    //        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
    //    }
    //}
    lCurve = lHipRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][17][0] - RotationMatrixSequence[i - 1][17][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][17][0]);
            //    RotationMatrixSequence[i][17][0] = RotationMatrixSequence[i - 1][17][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][17][1] - RotationMatrixSequence[i - 1][17][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][17][1]);
            //    RotationMatrixSequence[i][17][1] = RotationMatrixSequence[i - 1][17][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][17][2] - RotationMatrixSequence[i - 1][17][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][17][2]);
            //    RotationMatrixSequence[i][17][2] = RotationMatrixSequence[i - 1][17][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[16].x - skeletonPoints3D[0].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][16][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[16].y - skeletonPoints3D[0].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][16][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lHipRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[16].z - skeletonPoints3D[0].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][16][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][18][0] - RotationMatrixSequence[i - 1][18][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][18][0]);
            //    RotationMatrixSequence[i][18][0] = RotationMatrixSequence[i - 1][18][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][18][1] - RotationMatrixSequence[i - 1][18][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][18][1]);
            //    RotationMatrixSequence[i][18][1] = RotationMatrixSequence[i - 1][18][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][18][2] - RotationMatrixSequence[i - 1][18][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][18][2]);
            //    RotationMatrixSequence[i][18][2] = RotationMatrixSequence[i - 1][18][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lKneeRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[17].x - skeletonPoints3D[16].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[17].y - skeletonPoints3D[16].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lKneeRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[17].z - skeletonPoints3D[16].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][17][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lAnkleRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][19][0] - RotationMatrixSequence[i - 1][19][0]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][19][0]);
            //    RotationMatrixSequence[i][19][0] = RotationMatrixSequence[i - 1][19][0];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][0]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][0]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lAnkleRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][19][1] - RotationMatrixSequence[i - 1][19][1]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][19][1]);
            //    RotationMatrixSequence[i][19][1] = RotationMatrixSequence[i - 1][19][1];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][1]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][1]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lAnkleRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            //if (i != 0 && fabs(RotationMatrixSequence[i][19][2] - RotationMatrixSequence[i - 1][19][2]) < anglethreshold)
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i - 1][19][2]);
            //    RotationMatrixSequence[i][19][2] = RotationMatrixSequence[i - 1][19][2];
            //}
            //else
            //{
            //    lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][2]);
            //}
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][2]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /*lCurve = lAnkleRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[18].x - skeletonPoints3D[17].x);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][3]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lAnkleRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[18].y - skeletonPoints3D[17].y);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][4]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lAnkleRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[18].z - skeletonPoints3D[17].z);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][18][5]);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }*/
    lCurve = lFootRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lFootRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    lCurve = lFootRight->LclRotation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
    if (lCurve)
    {
        lCurve->KeyModifyBegin();
        lTime.SetSecondDouble(0.0);
        lKeyIndex = lCurve->KeyAdd(lTime);
        lCurve->KeySetValue(lKeyIndex, 0.0);
        lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

        for (int i = 0; i < RotationMatrixSequence.size(); ++i)
        {
            lTime.SetSecondDouble(m_TimeStep*(i + 1));
            lKeyIndex = lCurve->KeyAdd(lTime);
            lCurve->KeySetValue(lKeyIndex, 0);
            lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
        }
    }
    /* lCurve = lFootRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_X, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[19].x - skeletonPoints3D[18].x);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][3]);
         }
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
     }
     lCurve = lFootRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Y, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[19].y - skeletonPoints3D[18].y);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][4]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }
     lCurve = lFootRight->LclTranslation.GetCurve(lAnimLayer, FBXSDK_CURVENODE_COMPONENT_Z, true);
     if (lCurve)
     {
         lCurve->KeyModifyBegin();
         lTime.SetSecondDouble(0.0);
         lKeyIndex = lCurve->KeyAdd(lTime);
         lCurve->KeySetValue(lKeyIndex, skeletonPoints3D[19].z - skeletonPoints3D[18].z);
         lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);

         for (int i = 0; i < RotationMatrixSequence.size(); ++i)
         {
             lTime.SetSecondDouble(m_TimeStep*(i + 1));
             lKeyIndex = lCurve->KeyAdd(lTime);
             lCurve->KeySetValue(lKeyIndex, RotationMatrixSequence[i][19][5]);
             lCurve->KeySetInterpolation(lKeyIndex, FbxAnimCurveDef::eInterpolationCubic);
         }
     }*/
}

// Store the Bind Pose
void FbxKinect20::StoreBindPose(FbxScene* pScene, FbxNode* pPatch)
{
    // In the bind pose, we must store all the link's global matrix at the time of the bind.
    // Plus, we must store all the parent(s) global matrix of a link, even if they are not
    // themselves deforming any model.

    // In this example, since there is only one model deformed, we don't need walk through 
    // the scene
    //

    // Now list the all the link involve in the patch deformation
    FbxArray<FbxNode*> lClusteredFbxNodes;
    int i, j;

    if (pPatch && pPatch->GetNodeAttribute())
    {
        int lSkinCount = 0;
        int lClusterCount = 0;
        switch (pPatch->GetNodeAttribute()->GetAttributeType())
        {
        default:
            break;
        case FbxNodeAttribute::eMesh:
        case FbxNodeAttribute::eNurbs:
        case FbxNodeAttribute::ePatch:

            lSkinCount = ((FbxGeometry*)pPatch->GetNodeAttribute())->GetDeformerCount(FbxDeformer::eSkin);
            //Go through all the skins and count them
            //then go through each skin and get their cluster count
            for (i = 0; i < lSkinCount; ++i)
            {
                FbxSkin *lSkin = (FbxSkin*)((FbxGeometry*)pPatch->GetNodeAttribute())->GetDeformer(i, FbxDeformer::eSkin);
                lClusterCount += lSkin->GetClusterCount();
            }
            break;
        }
        //if we found some clusters we must add the node
        if (lClusterCount)
        {
            //Again, go through all the skins get each cluster link and add them
            for (i = 0; i < lSkinCount; ++i)
            {
                FbxSkin *lSkin = (FbxSkin*)((FbxGeometry*)pPatch->GetNodeAttribute())->GetDeformer(i, FbxDeformer::eSkin);
                lClusterCount = lSkin->GetClusterCount();
                for (j = 0; j < lClusterCount; ++j)
                {
                    FbxNode* lClusterNode = lSkin->GetCluster(j)->GetLink();
                    AddNodeRecursively(lClusteredFbxNodes, lClusterNode);
                }

            }

            // Add the patch to the pose
            lClusteredFbxNodes.Add(pPatch);
        }
    }

    // Now create a bind pose with the link list
    if (lClusteredFbxNodes.GetCount())
    {
        // A pose must be named. Arbitrarily use the name of the patch node.
        FbxPose* lPose = FbxPose::Create(pScene, pPatch->GetName());

        // default pose type is rest pose, so we need to set the type as bind pose
        lPose->SetIsBindPose(true);

        for (i = 0; i < lClusteredFbxNodes.GetCount(); i++)
        {
            FbxNode*  lKFbxNode = lClusteredFbxNodes.GetAt(i);
            FbxMatrix lBindMatrix = lKFbxNode->EvaluateGlobalTransform();

            lPose->Add(lKFbxNode, lBindMatrix);
        }

        // Add the pose to the scene
        pScene->AddPose(lPose);
    }
}

// Store a Rest Pose
void FbxKinect20::StoreRestPose(FbxScene* pScene, FbxNode* pSkeletonRoot)
{
    // This example show an arbitrary rest pose assignment.
    // This rest pose will set the bone rotation to the same value 
    // as time 1 second in the first stack of animation, but the 
    // position of the bone will be set elsewhere in the scene.
    FbxString     lNodeName;
    FbxNode*   lKFbxNode;
    FbxMatrix  lTransformMatrix;
    FbxVector4 lT, lR, lS(1.0, 1.0, 1.0);

    // Create the rest pose
    FbxPose* lPose = FbxPose::Create(pScene, "A Bind Pose");

    // Set the skeleton root node to the global position (10, 10, 10)
    // and global rotation of 45deg along the Z axis.
    lT.Set(10.0, 10.0, 10.0);
    lR.Set(0.0, 0.0, 45.0);

    lTransformMatrix.SetTRS(lT, lR, lS);

    // Add the skeleton root node to the pose
    lKFbxNode = pSkeletonRoot;
    lPose->Add(lKFbxNode, lTransformMatrix, false /*it's a global matrix*/);

    // Set the lLimbNode1 node to the local position of (0, 40, 0)
    // and local rotation of -90deg along the Z axis. This show that
    // you can mix local and global coordinates in a rest pose.
    lT.Set(0.0, 40.0, 0.0);
    lR.Set(0.0, 0.0, -90.0);

    lTransformMatrix.SetTRS(lT, lR, lS);

    // Add the skeleton second node to the pose
    lKFbxNode = lKFbxNode->GetChild(0);
    lPose->Add(lKFbxNode, lTransformMatrix, true /*it's a local matrix*/);

    // Set the lLimbNode2 node to the local position of (0, 40, 0)
    // and local rotation of 45deg along the Z axis.
    lT.Set(0.0, 40.0, 0.0);
    lR.Set(0.0, 0.0, 45.0);

    lTransformMatrix.SetTRS(lT, lR, lS);

    // Add the skeleton second node to the pose
    lKFbxNode = lKFbxNode->GetChild(0);
    lNodeName = lKFbxNode->GetName();
    lPose->Add(lKFbxNode, lTransformMatrix, true /*it's a local matrix*/);

    // Now add the pose to the scene
    pScene->AddPose(lPose);
}

// Add the specified node to the node array. Also, add recursively
// all the parent node of the specified node to the array.
void FbxKinect20::AddNodeRecursively(FbxArray<FbxNode*>& pNodeArray, FbxNode* pNode)
{
    if (pNode)
    {
        AddNodeRecursively(pNodeArray, pNode->GetParent());

        if (pNodeArray.Find(pNode) == -1)
        {
            // Node not in the list, add it
            pNodeArray.Add(pNode);
        }
    }
}