MotionBuilder-SDK/cpp_ref/devices_2deviceskeleton_2ordeviceskeleton_hardware_8cxx-example.html

/***************************************************************************************

 Autodesk(R) Open Reality(R) Samples


 (C) 2009 Autodesk, Inc. and/or its licensors

 All rights reserved.


 AUTODESK SOFTWARE LICENSE AGREEMENT

 Autodesk, Inc. licenses this Software to you only upon the condition that

 you accept all of the terms contained in the Software License Agreement ("Agreement")

 that is embedded in or that is delivered with this Software. By selecting

 the "I ACCEPT" button at the end of the Agreement or by copying, installing,

 uploading, accessing or using all or any portion of the Software you agree

 to enter into the Agreement. A contract is then formed between Autodesk and

 either you personally, if you acquire the Software for yourself, or the company

 or other legal entity for which you are acquiring the software.


 AUTODESK, INC., MAKES NO WARRANTY, EITHER EXPRESS OR IMPLIED, INCLUDING BUT

 NOT LIMITED TO ANY IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR

 PURPOSE REGARDING THESE MATERIALS, AND MAKES SUCH MATERIALS AVAILABLE SOLELY ON AN

 "AS-IS" BASIS.


 IN NO EVENT SHALL AUTODESK, INC., BE LIABLE TO ANYONE FOR SPECIAL, COLLATERAL,

 INCIDENTAL, OR CONSEQUENTIAL DAMAGES IN CONNECTION WITH OR ARISING OUT OF PURCHASE

 OR USE OF THESE MATERIALS. THE SOLE AND EXCLUSIVE LIABILITY TO AUTODESK, INC.,

 REGARDLESS OF THE FORM OF ACTION, SHALL NOT EXCEED THE PURCHASE PRICE OF THE

 MATERIALS DESCRIBED HEREIN.


 Autodesk, Inc., reserves the right to revise and improve its products as it sees fit.


 Autodesk and Open Reality are registered trademarks or trademarks of Autodesk, Inc.,

 in the U.S.A. and/or other countries. All other brand names, product names, or

 trademarks belong to their respective holders.


 GOVERNMENT USE

 Use, duplication, or disclosure by the U.S. Government is subject to restrictions as

 set forth in FAR 12.212 (Commercial Computer Software-Restricted Rights) and

 DFAR 227.7202 (Rights in Technical Data and Computer Software), as applicable.

 Manufacturer is Autodesk, Inc., 10 Duke Street, Montreal, Quebec, Canada, H3C 2L7.

***************************************************************************************/


//--- Class declaration

#include "ordeviceskeleton_hardware.h"


/************************************************

 *  Constructor.

 ************************************************/

ORDeviceSkeletonHardware::ORDeviceSkeletonHardware() :

    mChannelCount( 0 ),

    mPassCount( 0 ),

    mNetworkUse( false ),

    mNetworkPort( sSKDataBuffer::PORTNUMBER ),

    mSocket( 0 )

{

    mNetworkAddress = "127.0.0.1";

}


/************************************************

 *  Destructor.

 ************************************************/

ORDeviceSkeletonHardware::~ORDeviceSkeletonHardware()

{

}


/************************************************

 *  Open device communications.

 ************************************************/

bool ORDeviceSkeletonHardware::Open()

{

    return true;

}


/************************************************

 *  Get device setup information.

 ************************************************/

bool ORDeviceSkeletonHardware::GetSetupInfo()

{

    //This should be read from hardware.

    //Here we used simple script to generate this data from skeleton:

/*

from pyfbsdk import *


lModels = FBModelList()

FBGetSelectedModels(lModels)


def SetupHierarchy(pModel, pParentIdx, pNextIndex):

    lIdx = pNextIndex

    pNextIndex += 1

    lT = FBVector3d()

    lR = FBVector3d()

    pModel.GetVector(lT,FBModelTransformationType.kModelTranslation)

    pModel.GetVector(lR,FBModelTransformationType.kModelRotation)

    print '//'+str(lIdx)+':'+pModel.Name

    print 'mChannel['+str(lIdx)+'].mName = "' + pModel.Name + '";'

    print 'mChannel['+str(lIdx)+'].mParentChannel = ' +  str(pParentIdx) + ';'

    print 'mChannel['+str(lIdx)+'].mT[0] = ' +  str(lT[0]) + ';'

    print 'mChannel['+str(lIdx)+'].mT[1] = ' +  str(lT[1]) + ';'

    print 'mChannel['+str(lIdx)+'].mT[2] = ' +  str(lT[2]) + ';'

    print 'mChannel['+str(lIdx)+'].mR[0] = ' +  str(lR[0]) + ';'

    print 'mChannel['+str(lIdx)+'].mR[1] = ' +  str(lR[1]) + ';'

    print 'mChannel['+str(lIdx)+'].mR[2] = ' +  str(lR[2]) + ';'

    print ''

    for lChild in pModel.Children:

        pNextIndex = SetupHierarchy(lChild,lIdx,pNextIndex)

    return pNextIndex


if len(lModels) == 1:

    SetupHierarchy(lModels[0],-1,0)

*/

    mChannelCount   = 30;

    //0:Hips

    mChannel[0].mName = "Hips";

    mChannel[0].mParentChannel = -1;

    mChannel[0].mT[0] = 5.52507944791e-15;

    mChannel[0].mT[1] = 90.2313919067;

    mChannel[0].mT[2] = 0.0;

    mChannel[0].mR[0] = 0.0;

    mChannel[0].mR[1] = 0.0;

    mChannel[0].mR[2] = 0.0;


    //1:LeftUpLeg

    mChannel[1].mName = "LeftUpLeg";

    mChannel[1].mParentChannel = 0;

    mChannel[1].mT[0] = 10.8919000626;

    mChannel[1].mT[1] = 90.2313919067;

    mChannel[1].mT[2] = 0.0;

    mChannel[1].mR[0] = 0.0;

    mChannel[1].mR[1] = 0.0;

    mChannel[1].mR[2] = 0.0;


    //2:LeftLeg

    mChannel[2].mName = "LeftLeg";

    mChannel[2].mParentChannel = 1;

    mChannel[2].mT[0] = 10.8919000626;

    mChannel[2].mT[1] = 46.1967926025;

    mChannel[2].mT[2] = 0.0;

    mChannel[2].mR[0] = 0.0;

    mChannel[2].mR[1] = 0.0;

    mChannel[2].mR[2] = 0.0;


    //3:LeftFoot

    mChannel[3].mName = "LeftFoot";

    mChannel[3].mParentChannel = 2;

    mChannel[3].mT[0] = 10.8919000626;

    mChannel[3].mT[1] = 4.74909210205;

    mChannel[3].mT[2] = 0.0;

    mChannel[3].mR[0] = 14.9999777807;

    mChannel[3].mR[1] = 0.0;

    mChannel[3].mR[2] = 0.0;


    //4:LeftToeBase

    mChannel[4].mName = "LeftToeBase";

    mChannel[4].mParentChannel = 3;

    mChannel[4].mT[0] = 10.8919000626;

    mChannel[4].mT[1] = 1.05947666894;

    mChannel[4].mT[2] = 13.7698536165;

    mChannel[4].mR[0] = 0.0;

    mChannel[4].mR[1] = 0.0;

    mChannel[4].mR[2] = 0.0;


    //5:L_Toe_End

    mChannel[5].mName = "L_Toe_End";

    mChannel[5].mParentChannel = 4;

    mChannel[5].mT[0] = 10.8919000626;

    mChannel[5].mT[1] = 1.05947666894;

    mChannel[5].mT[2] = 21.2698536165;

    mChannel[5].mR[0] = 0.0;

    mChannel[5].mR[1] = 0.0;

    mChannel[5].mR[2] = 0.0;


    //6:RightUpLeg

    mChannel[6].mName = "RightUpLeg";

    mChannel[6].mParentChannel = 0;

    mChannel[6].mT[0] = -10.8919000626;

    mChannel[6].mT[1] = 90.2313919067;

    mChannel[6].mT[2] = 0.0;

    mChannel[6].mR[0] = 0.0;

    mChannel[6].mR[1] = 0.0;

    mChannel[6].mR[2] = 0.0;


    //7:RightLeg

    mChannel[7].mName = "RightLeg";

    mChannel[7].mParentChannel = 6;

    mChannel[7].mT[0] = -10.8919000626;

    mChannel[7].mT[1] = 46.1967926025;

    mChannel[7].mT[2] = 0.0;

    mChannel[7].mR[0] = 0.0;

    mChannel[7].mR[1] = 0.0;

    mChannel[7].mR[2] = 0.0;


    //8:RightFoot

    mChannel[8].mName = "RightFoot";

    mChannel[8].mParentChannel = 7;

    mChannel[8].mT[0] = -10.8919000626;

    mChannel[8].mT[1] = 4.74909210205;

    mChannel[8].mT[2] = 0.0;

    mChannel[8].mR[0] = 14.9999777807;

    mChannel[8].mR[1] = 0.0;

    mChannel[8].mR[2] = 0.0;


    //9:RightToeBase

    mChannel[9].mName = "RightToeBase";

    mChannel[9].mParentChannel = 8;

    mChannel[9].mT[0] = -10.8919000626;

    mChannel[9].mT[1] = 1.05947666894;

    mChannel[9].mT[2] = 13.7698536165;

    mChannel[9].mR[0] = 0.0;

    mChannel[9].mR[1] = 0.0;

    mChannel[9].mR[2] = 0.0;


    //10:R_Toe_End

    mChannel[10].mName = "R_Toe_End";

    mChannel[10].mParentChannel = 9;

    mChannel[10].mT[0] = -10.8919000626;

    mChannel[10].mT[1] = 1.05947666894;

    mChannel[10].mT[2] = 21.2698536165;

    mChannel[10].mR[0] = 0.0;

    mChannel[10].mR[1] = 0.0;

    mChannel[10].mR[2] = 0.0;


    //11:Spine

    mChannel[11].mName = "Spine";

    mChannel[11].mParentChannel = 0;

    mChannel[11].mT[0] = 5.52507944791e-15;

    mChannel[11].mT[1] = 90.3313919082;

    mChannel[11].mT[2] = 0.0;

    mChannel[11].mR[0] = 0.0;

    mChannel[11].mR[1] = 0.0;

    mChannel[11].mR[2] = 0.0;


    //12:Spine1

    mChannel[12].mName = "Spine1";

    mChannel[12].mParentChannel = 11;

    mChannel[12].mT[0] = 5.52507944791e-15;

    mChannel[12].mT[1] = 109.028892137;

    mChannel[12].mT[2] = 0.0;

    mChannel[12].mR[0] = 0.0;

    mChannel[12].mR[1] = 0.0;

    mChannel[12].mR[2] = 0.0;


    //13:Neck

    mChannel[13].mName = "Neck";

    mChannel[13].mParentChannel = 12;

    mChannel[13].mT[0] = 5.52507944791e-15;

    mChannel[13].mT[1] = 133.325692751;

    mChannel[13].mT[2] = 0.0;

    mChannel[13].mR[0] = 0.0;

    mChannel[13].mR[1] = 0.0;

    mChannel[13].mR[2] = 0.0;


    //14:Head

    mChannel[14].mName = "Head";

    mChannel[14].mParentChannel = 13;

    mChannel[14].mT[0] = 5.52507944791e-15;

    mChannel[14].mT[1] = 145.887693025;

    mChannel[14].mT[2] = 0.0;

    mChannel[14].mR[0] = 0.0;

    mChannel[14].mR[1] = 0.0;

    mChannel[14].mR[2] = 0.0;


    //15:Head_End

    mChannel[15].mName = "Head_End";

    mChannel[15].mParentChannel = 14;

    mChannel[15].mT[0] = 5.52507944791e-15;

    mChannel[15].mT[1] = 157.387693025;

    mChannel[15].mT[2] = 0.0;

    mChannel[15].mR[0] = 0.0;

    mChannel[15].mR[1] = 0.0;

    mChannel[15].mR[2] = 0.0;


    //16:LeftShoulder

    mChannel[16].mName = "LeftShoulder";

    mChannel[16].mParentChannel = 12;

    mChannel[16].mT[0] = 5.52507944791e-15;

    mChannel[16].mT[1] = 133.325692751;

    mChannel[16].mT[2] = 0.0;

    mChannel[16].mR[0] = 0.0;

    mChannel[16].mR[1] = 0.0;

    mChannel[16].mR[2] = -90.0002104591;


    //17:LeftArm

    mChannel[17].mName = "LeftArm";

    mChannel[17].mParentChannel = 16;

    mChannel[17].mT[0] = 21.7077007292;

    mChannel[17].mT[1] = 133.325613014;

    mChannel[17].mT[2] = 0.0;

    mChannel[17].mR[0] = 0.0;

    mChannel[17].mR[1] = 0.0;

    mChannel[17].mR[2] = -90.0002104591;


    //18:LeftForeArm

    mChannel[18].mName = "LeftForeArm";

    mChannel[18].mParentChannel = 17;

    mChannel[18].mT[0] = 48.0103015896;

    mChannel[18].mT[1] = 133.325516399;

    mChannel[18].mT[2] = 0.0;

    mChannel[18].mR[0] = 0.0;

    mChannel[18].mR[1] = 0.0;

    mChannel[18].mR[2] = -90.0002104591;


    //19:LeftHand

    mChannel[19].mName = "LeftHand";

    mChannel[19].mParentChannel = 18;

    mChannel[19].mT[0] = 75.5387020106;

    mChannel[19].mT[1] = 133.325415281;

    mChannel[19].mT[2] = 0.0;

    mChannel[19].mR[0] = 0.0;

    mChannel[19].mR[1] = 0.0;

    mChannel[19].mR[2] = -90.0002104591;


    //20:LeftHandThumb

    mChannel[20].mName = "LeftHandThumb";

    mChannel[20].mParentChannel = 19;

    mChannel[20].mT[0] = 75.5387020106;

    mChannel[20].mT[1] = 133.325415281;

    mChannel[20].mT[2] = 0.0;

    mChannel[20].mR[0] = 0.0;

    mChannel[20].mR[1] = 0.0;

    mChannel[20].mR[2] = -90.0002104591;


    //21:L_Thumb_End

    mChannel[21].mName = "L_Thumb_End";

    mChannel[21].mParentChannel = 20;

    mChannel[21].mT[0] = 75.5387020106;

    mChannel[21].mT[1] = 133.325415281;

    mChannel[21].mT[2] = 10.0;

    mChannel[21].mR[0] = 0.0;

    mChannel[21].mR[1] = 0.0;

    mChannel[21].mR[2] = -90.0002104591;


    //22:L_Wrist_End

    mChannel[22].mName = "L_Wrist_End";

    mChannel[22].mParentChannel = 19;

    mChannel[22].mT[0] = 85.5387020105;

    mChannel[22].mT[1] = 133.325378549;

    mChannel[22].mT[2] = 0.0;

    mChannel[22].mR[0] = 0.0;

    mChannel[22].mR[1] = 0.0;

    mChannel[22].mR[2] = -90.0002104591;


    //23:RightShoulder

    mChannel[23].mName = "RightShoulder";

    mChannel[23].mParentChannel = 12;

    mChannel[23].mT[0] = 5.52507944791e-15;

    mChannel[23].mT[1] = 133.325692751;

    mChannel[23].mT[2] = 0.0;

    mChannel[23].mR[0] = 0.0;

    mChannel[23].mR[1] = 0.0;

    mChannel[23].mR[2] = 90.0002104591;


    //24:RightArm

    mChannel[24].mName = "RightArm";

    mChannel[24].mParentChannel = 23;

    mChannel[24].mT[0] = -21.7077007292;

    mChannel[24].mT[1] = 133.325613014;

    mChannel[24].mT[2] = 0.0;

    mChannel[24].mR[0] = 0.0;

    mChannel[24].mR[1] = 0.0;

    mChannel[24].mR[2] = 90.0002104591;


    //25:RightForeArm

    mChannel[25].mName = "RightForeArm";

    mChannel[25].mParentChannel = 24;

    mChannel[25].mT[0] = -48.0103015896;

    mChannel[25].mT[1] = 133.325516399;

    mChannel[25].mT[2] = 0.0;

    mChannel[25].mR[0] = 0.0;

    mChannel[25].mR[1] = 0.0;

    mChannel[25].mR[2] = 90.0002104591;


    //26:RightHand

    mChannel[26].mName = "RightHand";

    mChannel[26].mParentChannel = 25;

    mChannel[26].mT[0] = -75.5387020106;

    mChannel[26].mT[1] = 133.325415281;

    mChannel[26].mT[2] = 0.0;

    mChannel[26].mR[0] = 0.0;

    mChannel[26].mR[1] = 0.0;

    mChannel[26].mR[2] = 90.0002104591;


    //27:RightHandThumb

    mChannel[27].mName = "RightHandThumb";

    mChannel[27].mParentChannel = 26;

    mChannel[27].mT[0] = -75.5387020106;

    mChannel[27].mT[1] = 133.325415281;

    mChannel[27].mT[2] = 0.0;

    mChannel[27].mR[0] = 0.0;

    mChannel[27].mR[1] = 0.0;

    mChannel[27].mR[2] = 90.0002104591;


    //28:R_Thumb_End

    mChannel[28].mName = "R_Thumb_End";

    mChannel[28].mParentChannel = 27;

    mChannel[28].mT[0] = -75.5387020106;

    mChannel[28].mT[1] = 133.325415281;

    mChannel[28].mT[2] = 10.0;

    mChannel[28].mR[0] = 0.0;

    mChannel[28].mR[1] = 0.0;

    mChannel[28].mR[2] = 90.0002104591;


    //29:R_Wrist_End

    mChannel[29].mName = "R_Wrist_End";

    mChannel[29].mParentChannel = 26;

    mChannel[29].mT[0] = -89.2387018198;

    mChannel[29].mT[1] = 133.325364958;

    mChannel[29].mT[2] = 0.0;

    mChannel[29].mR[0] = 0.0;

    mChannel[29].mR[1] = 0.0;

    mChannel[29].mR[2] = 90.0002104591;


    for(int i = 0; i < mChannelCount; i++)

    {

        memcpy(mChannel[i].mDefaultT,mChannel[i].mT, sizeof(double)*3);

        memcpy(mChannel[i].mDefaultR,mChannel[i].mR, sizeof(double)*3);

    }


    mSkDataBuffer.SetupBuffer();


    return true;

}


/************************************************

 *  Close device communications.

 ************************************************/

bool ORDeviceSkeletonHardware::Close()

{

    // TODO: Close network connection to server

    if (mSocket) mTCP.CloseSocket(mSocket);

    mSocket = 0;

    return true;

}


/************************************************

 *  Take current DataBuffer time as a time offset

 ************************************************/

void ORDeviceSkeletonHardware::ResetPacketTimeOffset(FBTime &pTime)

{

    mPacketTimeOffset = mSkDataBuffer.mTime;

    mStartRecordingTime = pTime;

}


/************************************************

 *  Fetch a data packet from the device.

 ************************************************/

bool ORDeviceSkeletonHardware::FetchDataPacket(FBTime &pTime)

{

    if (mSocket)

    {

        int tmpByteCount = 0;

        if (mTCP.Read(mSocket, (char*)&mSkDataBuffer,sizeof(mSkDataBuffer), &tmpByteCount))

        {

            if (tmpByteCount!=sizeof(mSkDataBuffer))

            {

                return false;

            }

        } else

        {

            return false;

        }

    } else

    // Otherwise use a simulated source

    {

        mPassCount++;

        if(mPassCount % 2==0)

        {

            mLastSimulationTime = mSystem.SystemTime;

            return false; // Nothing else to read.

        }


        mLastSimulationTime.SetMilliSeconds(mLastSimulationTime.GetMilliSeconds()+(1000/sSKDataBuffer::SIM_FPS));


        // Simulate Skeleton

        mSkDataBuffer.Simulate((mLastSimulationTime.GetMilliSeconds())*1000000);


        // Copy data

        SkeletonNodeInfo* lIter;

        for(int i = 0; i < GetChannelCount(); i++)

        {

            lIter = &mChannel[i];

            memcpy(lIter->mT,mSkDataBuffer.mChannel[i].mT,sizeof(lIter->mT));

            memcpy(lIter->mR,mSkDataBuffer.mChannel[i].mR,sizeof(lIter->mR));

        }

    }


    // Update packet relative time

    pTime.SetMilliSeconds((mSkDataBuffer.mTime-mPacketTimeOffset)/1000000);

    pTime += mStartRecordingTime;

    return true;

}


/************************************************

 *  Start data streaming from device.

 ************************************************/

bool ORDeviceSkeletonHardware::StartDataStream()

{

    // TODO: ask server to start sending data


    if (mNetworkUse)

    {

        if (mTCP.CreateSocket(mSocket,kFBTCPIP_Stream ))

        {

            if (!mTCP.Connect(mSocket, mNetworkAddress, mNetworkPort))

            {

                if (mSocket) mTCP.CloseSocket(mSocket);

                mSocket = 0;

            }

        }

        return mSocket!=0;

    }

    return true;

}


/************************************************

 *  Stop data streaming from device.

 ************************************************/

bool ORDeviceSkeletonHardware::StopDataStream()

{

    // TODO: ask server to stop sending data

    if (mSocket) mTCP.CloseSocket(mSocket);

    mSocket = 0;

    return true;

}