devices/devicemocap/ordevicemocap_hardware.cxx

devices/devicemocap/ordevicemocap_hardware.cxx
/***************************************************************************************
Autodesk(R) Open Reality(R) Samples
(C) 2013 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 "ordevicemocap_hardware.h"
#include "speech/speech.h"
static FBMatrix RotationMatrix4ToFBMatrix(Matrix4 pMatrix4);
static FBTVector PositionVector4ToFBTVector(Vector4 pVector4);
/************************************************
* Constructor.
************************************************/
ORDeviceMocapKinect::ORDeviceMocapKinect() :
mChannelCount( 0 ),
mNuiSensor(NULL),
m_hNextSkeletonEvent(INVALID_HANDLE_VALUE),
mOpened(false),
mKinectMocapJointsState(NULL),
mAverageSensorFloorOffset(0.0),
mSensorFloorOffsetSet(false),
mInitSuccessful(false),
mHasVoiceControl(false),
mSpeech(NULL)
{
}
/************************************************
* Destructor.
************************************************/
ORDeviceMocapKinect::~ORDeviceMocapKinect()
{
if(mSpeech)
{
mSpeech->Stop();
delete mSpeech;
}
if (mNuiSensor)
{
// There is a unresolved issue, Freezing due to that sensor won't shut down properly by NuiShutdown.
// This possibly be resolved by Register dummy device status callback by NuiSetDeviceStatusCallback, But didn't work.
//mNuiSensor->NuiShutdown();
}
if (m_hNextSkeletonEvent && (m_hNextSkeletonEvent != INVALID_HANDLE_VALUE))
{
CloseHandle(m_hNextSkeletonEvent);
}
if ( mNuiSensor )
{
mNuiSensor->Release();
mNuiSensor = NULL;
}
if (mKinectMocapJointsState)
{
delete mKinectMocapJointsState;
}
}
bool ORDeviceMocapKinect::Init()
{
__try
{
NuiInitialize( NUI_INITIALIZE_FLAG_USES_SKELETON );
mInitSuccessful = true;
}
__except( puts("in filter"), EXCEPTION_EXECUTE_HANDLER )
{
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_RUNTIME_DLL_MISSING);
mInitSuccessful = false;
return false;
}
return true;
}
/************************************************
* Open device communications.
************************************************/
bool ORDeviceMocapKinect::Open()
{
if( !mInitSuccessful )
return false;
bool lResult = false;
if (!mOpened)
{
int lSensorCount = 0;
INuiSensor * lNuiSensor;
HRESULT hr = NuiGetSensorCount(&lSensorCount);
if (FAILED(hr))
{
return false;
}
// Look at each Kinect sensor
for (int i = 0; i < lSensorCount; ++i)
{
// Create the sensor so we can check status, if we can't create it, move on to the next
hr = NuiCreateSensorByIndex(i, &lNuiSensor);
if (FAILED(hr))
{
continue;
}
// Get the status of the sensor, and if connected, then we can initialize it
hr = lNuiSensor->NuiStatus();
if( hr == S_OK )
{
mNuiSensor = lNuiSensor;
lResult = true;
break;
}
else
{
switch (hr)
{
case S_NUI_INITIALIZING:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_INITIALIZING);
break;
case E_NUI_NOTCONNECTED:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_NOTCONNECTED);
break;
case E_NUI_NOTGENUINE:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_NOTGENUINE);
break;
case E_NUI_NOTSUPPORTED:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_NOTSUPPORTED);
break;
case E_NUI_INSUFFICIENTBANDWIDTH:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_INSUFFICIENTBANDWIDTH);
break;
case E_NUI_NOTPOWERED:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_NOTPOWERED);
break;
case E_NUI_NOTREADY:
StatusHandle::One()->PopMessage(eSTATUS_SENSOR_NOTREADY);
break;
default:
break;
}
// This sensor wasn't OK, so release it since we're not using it
lNuiSensor->Release();
}
}
}
// Ask for voice control
if( lResult )
{
static bool gVoiceControl_NotAskAgain = false;
if( !gVoiceControl_NotAskAgain )
{
int lSelect = FBMessageBoxWithCheck("Mocap Device Info:",
"Activate Voice Control?",
"Yes", "No", NULL,
"Don't show this dialog", gVoiceControl_NotAskAgain
);
if(lSelect == 1)
mHasVoiceControl = true;
else
mHasVoiceControl = false;
}
}
return lResult;
}
// Change the camera angle to horizontal
bool ORDeviceMocapKinect::AdjustAngle()
{
if( mNuiSensor )
{
long lVerticalAngle = 0;
HRESULT hr = mNuiSensor->NuiCameraElevationGetAngle(&lVerticalAngle);
if( hr != S_OK )
{
return false;
}
if( lVerticalAngle != 0 )
{
hr = mNuiSensor->NuiCameraElevationSetAngle(0);
if( hr != S_OK )
{
return false;
}
}
}
return true;
}
/************************************************
* Start data streaming from device.
************************************************/
bool ORDeviceMocapKinect::StartDataStream()
{
if (!mOpened)
{
if (NULL != mNuiSensor)
{
// Initialize the Kinect and specify that we'll be using skeleton
HRESULT hr;
if( mHasVoiceControl )
hr = mNuiSensor->NuiInitialize( NUI_INITIALIZE_FLAG_USES_SKELETON | NUI_INITIALIZE_FLAG_USES_AUDIO );
else
hr = mNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_SKELETON);
if (SUCCEEDED(hr))
{
// Create an event that will be signaled when skeleton data is available
m_hNextSkeletonEvent = CreateEventW(NULL, TRUE, FALSE, NULL);
// Open a skeleton stream to receive skeleton data
hr = mNuiSensor->NuiSkeletonTrackingEnable(m_hNextSkeletonEvent, 0);
if ( SUCCEEDED(hr))
{
mOpened = true;
return true;
}
else
{
mNuiSensor->Release();
return false;
}
}
else
{
mNuiSensor->Release();
return false;
}
}
}
return false;
}
/************************************************
* Close device communications.
************************************************/
bool ORDeviceMocapKinect::Close()
{
if (NULL != mNuiSensor)
{
mNuiSensor->NuiShutdown();
mNuiSensor = NULL;
mOpened = false;
}
return true;
}
/************************************************
* Stop data streaming from device.
************************************************/
bool ORDeviceMocapKinect::StopDataStream()
{
if( mOpened )
{
if (NULL != mNuiSensor)
{
if( mSpeech && mSpeech->IsValid() )
mSpeech->Stop();
HRESULT hr = mNuiSensor->NuiSkeletonTrackingDisable();
if ( SUCCEEDED(hr))
{
mOpened = false;
return true;
}
}
}
return false;
}
/************************************************
* Get device setup information.
************************************************/
bool ORDeviceMocapKinect::GetSetupInfo()
{
//The initial skeleton hierarchy exactly the same as Kinect skeleton.
//Add two shoulder bones
mChannelCount = NUI_SKELETON_POSITION_COUNT + 2;
//0:Hips - NUI_SKELETON_POSITION_HIP_CENTER
mChannel[0].mName = "Hips";
mChannel[0].mParentChannel = -1;
mChannel[0].mT[0] = 0;
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:Spine - NUI_SKELETON_POSITION_SPINE
mChannel[1].mName = "Spine";
mChannel[1].mParentChannel = 0;
mChannel[1].mT[0] = 0;
mChannel[1].mT[1] = 109;
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:Spine 1 - NUI_SKELETON_POSITION_SHOULDER_CENTER
mChannel[2].mName = "Neck";
mChannel[2].mParentChannel = 1;
mChannel[2].mT[0] = 0;
mChannel[2].mT[1] = 133.325692751;
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:Head - NUI_SKELETON_POSITION_HEAD
mChannel[3].mName = "Head";
mChannel[3].mParentChannel = 2;
mChannel[3].mT[0] = 0;
mChannel[3].mT[1] = 145.887693025;
mChannel[3].mT[2] = 0.0;
mChannel[3].mR[0] = 0.0;
mChannel[3].mR[1] = 0.0;
mChannel[3].mR[2] = 0.0;
//20:LeftShoulder - NUI_SKELETON_POSITION_SHOULDER_LEFT
mChannel[20].mName = "LeftShoulder";
mChannel[20].mParentChannel = 1;
mChannel[20].mT[0] = 20;
mChannel[20].mT[1] = 133.325692751;
mChannel[20].mT[2] = 0.0;
mChannel[20].mR[0] = 0.0;
mChannel[20].mR[1] = 0.0;
mChannel[20].mR[2] = -125.0;
//4:LeftArm - NUI_SKELETON_POSITION_SHOULDER_LEFT
mChannel[4].mName = "LeftArm";
mChannel[4].mParentChannel = 20;
mChannel[4].mT[0] = 20;
mChannel[4].mT[1] = 133.325692751;
mChannel[4].mT[2] = 0.0;
mChannel[4].mR[0] = 0.0;
mChannel[4].mR[1] = 0.0;
mChannel[4].mR[2] = -90.0002104591;
//5:LeftForeArm - NUI_SKELETON_POSITION_ELBOW_LEFT
mChannel[5].mName = "LeftForeArm";
mChannel[5].mParentChannel = 4;
mChannel[5].mT[0] = 45;
mChannel[5].mT[1] = 133.325613014;
mChannel[5].mT[2] = 0.0;
mChannel[5].mR[0] = 0.0;
mChannel[5].mR[1] = 0.0;
mChannel[5].mR[2] = -90.0002104591;
//6:LeftHand - NUI_SKELETON_POSITION_WRIST_LEFT
mChannel[6].mName = "LeftHand";
mChannel[6].mParentChannel = 5;
mChannel[6].mT[0] = 70;
mChannel[6].mT[1] = 133.325516399;
mChannel[6].mT[2] = 0.0;
mChannel[6].mR[0] = 0.0;
mChannel[6].mR[1] = 0.0;
mChannel[6].mR[2] = -90.0002104591;
//7:LeftFingerBase - NUI_SKELETON_POSITION_HAND_LEFT
mChannel[7].mName = "LeftFingerBase";
mChannel[7].mParentChannel = 6;
mChannel[7].mT[0] = 75;
mChannel[7].mT[1] = 133.325415281;
mChannel[7].mT[2] = 0.0;
mChannel[7].mR[0] = 0.0;
mChannel[7].mR[1] = 0.0;
mChannel[7].mR[2] = -90.0002104591;
//21:RightShoulder - NUI_SKELETON_POSITION_SHOULDER_RIGHT
mChannel[21].mName = "RightShoulder";
mChannel[21].mParentChannel = 1;
mChannel[21].mT[0] = -20;
mChannel[21].mT[1] = 133.325692751;
mChannel[21].mT[2] = 0.0;
mChannel[21].mR[0] = 0.0;
mChannel[21].mR[1] = 0.0;
mChannel[21].mR[2] = 125.0;
//8:RightArm - NUI_SKELETON_POSITION_SHOULDER_RIGHT
mChannel[8].mName = "RightArm";
mChannel[8].mParentChannel = 21;
mChannel[8].mT[0] = -20;
mChannel[8].mT[1] = 133.325692751;
mChannel[8].mT[2] = 0.0;
mChannel[8].mR[0] = 0.0;
mChannel[8].mR[1] = 0.0;
mChannel[8].mR[2] = 90.0002104591;
//9:RightForeArm - NUI_SKELETON_POSITION_ELBOW_RIGHT
mChannel[9].mName = "RightForeArm";
mChannel[9].mParentChannel = 8;
mChannel[9].mT[0] = -45;
mChannel[9].mT[1] = 133.325613014;
mChannel[9].mT[2] = 0.0;
mChannel[9].mR[0] = 0.0;
mChannel[9].mR[1] = 0.0;
mChannel[9].mR[2] = 90.0002104591;
//10:RightHand - NUI_SKELETON_POSITION_WRIST_RIGHT
mChannel[10].mName = "RightHand";
mChannel[10].mParentChannel = 9;
mChannel[10].mT[0] = -70;
mChannel[10].mT[1] = 133.325516399;
mChannel[10].mT[2] = 0.0;
mChannel[10].mR[0] = 0.0;
mChannel[10].mR[1] = 0.0;
mChannel[10].mR[2] = 90.0002104591;
//11:RightFingerBase - NUI_SKELETON_POSITION_HAND_RIGHT
mChannel[11].mName = "RightFingerBase";
mChannel[11].mParentChannel = 10;
mChannel[11].mT[0] = -75.5387020106;
mChannel[11].mT[1] = 133.325415281;
mChannel[11].mT[2] = 0.0;
mChannel[11].mR[0] = 0.0;
mChannel[11].mR[1] = 0.0;
mChannel[11].mR[2] = 90.0002104591;
//12:LeftUpLeg - NUI_SKELETON_POSITION_HIP_LEFT
mChannel[12].mName = "LeftUpLeg";
mChannel[12].mParentChannel = 0;
mChannel[12].mT[0] = 10.8919000626;
mChannel[12].mT[1] = 90.2313919067;
mChannel[12].mT[2] = 0.0;
mChannel[12].mR[0] = 0.0;
mChannel[12].mR[1] = 0.0;
mChannel[12].mR[2] = -180.0;
//13:LeftLeg - NUI_SKELETON_POSITION_KNEE_LEFT
mChannel[13].mName = "LeftLeg";
mChannel[13].mParentChannel = 12;
mChannel[13].mT[0] = 10.8919000626;
mChannel[13].mT[1] = 46.1967926025;
mChannel[13].mT[2] = 0.0;
mChannel[13].mR[0] = 0.0;
mChannel[13].mR[1] = 0.0;
mChannel[13].mR[2] = -180.0;
//14:LeftFoot - NUI_SKELETON_POSITION_ANKLE_LEFT
mChannel[14].mName = "LeftFoot";
mChannel[14].mParentChannel = 13;
mChannel[14].mT[0] = 10.8919000626;
mChannel[14].mT[1] = 4.74909210205;
mChannel[14].mT[2] = 0.0;
mChannel[14].mR[0] = 33;
mChannel[14].mR[1] = 0.0;
mChannel[14].mR[2] = -180.0;
//15:LeftToeBase - NUI_SKELETON_POSITION_FOOT_LEFT
mChannel[15].mName = "LeftToeBase";
mChannel[15].mParentChannel = 14;
mChannel[15].mT[0] = 10.8919000626;
mChannel[15].mT[1] = 1.05947666894;
mChannel[15].mT[2] = 13.7698536165;
mChannel[15].mR[0] = 28;
mChannel[15].mR[1] = 0.0;
mChannel[15].mR[2] = -180.0;
//16:RightUpLeg - NUI_SKELETON_POSITION_HIP_RIGHT
mChannel[16].mName = "RightUpLeg";
mChannel[16].mParentChannel = 0;
mChannel[16].mT[0] = -10.8919000626;
mChannel[16].mT[1] = 90.2313919067;
mChannel[16].mT[2] = 0.0;
mChannel[16].mR[0] = 0.0;
mChannel[16].mR[1] = 0.0;
mChannel[16].mR[2] = 180.0;
//17:RightLeg - NUI_SKELETON_POSITION_KNEE_RIGHT
mChannel[17].mName = "RightLeg";
mChannel[17].mParentChannel = 16;
mChannel[17].mT[0] = -10.8919000626;
mChannel[17].mT[1] = 46.1967926025;
mChannel[17].mT[2] = 0.0;
mChannel[17].mR[0] = 0.0;
mChannel[17].mR[1] = 0.0;
mChannel[17].mR[2] = 180.0;
//18:RightFoot - NUI_SKELETON_POSITION_ANKLE_RIGHT
mChannel[18].mName = "RightFoot";
mChannel[18].mParentChannel = 17;
mChannel[18].mT[0] = -10.8919000626;
mChannel[18].mT[1] = 4.74909210205;
mChannel[18].mT[2] = 0.0;
mChannel[18].mR[0] = 33;
mChannel[18].mR[1] = 0.0;
mChannel[18].mR[2] = 180.0;
//19:RightToeBase - NUI_SKELETON_POSITION_FOOT_RIGHT
mChannel[19].mName = "RightToeBase";
mChannel[19].mParentChannel = 18;
mChannel[19].mT[0] = -10.8919000626;
mChannel[19].mT[1] = 1.05947666894;
mChannel[19].mT[2] = 13.7698536165;
mChannel[19].mR[0] = 28;
mChannel[19].mR[1] = 0.0;
mChannel[19].mR[2] = 180.0;
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);
}
mKinectMocapJointsState = new FBMocapJointsState(NUI_SKELETON_POSITION_COUNT);
return true;
}
void ORDeviceMocapKinect::GetSkeletonJointsOrder(FBBodyNodeId* &pSkeletonJointsOrder)
{
pSkeletonJointsOrder = new FBBodyNodeId[NUI_SKELETON_POSITION_COUNT];
pSkeletonJointsOrder[0] = kFBHipsNodeId;
pSkeletonJointsOrder[1] = kFBWaistNodeId;
pSkeletonJointsOrder[2] = kFBNeckNodeId;
pSkeletonJointsOrder[3] = kFBHeadNodeId;
pSkeletonJointsOrder[4] = kFBLeftShoulderNodeId;
pSkeletonJointsOrder[5] = kFBLeftElbowNodeId;
pSkeletonJointsOrder[6] = kFBLeftWristNodeId;
pSkeletonJointsOrder[7] = kFBLeftHandNodeId;
pSkeletonJointsOrder[8] = kFBRightShoulderNodeId;
pSkeletonJointsOrder[9] = kFBRightElbowNodeId;
pSkeletonJointsOrder[10] = kFBRightWristNodeId;
pSkeletonJointsOrder[11] = kFBRightHandNodeId;
pSkeletonJointsOrder[12] = kFBLeftHipNodeId;
pSkeletonJointsOrder[13] = kFBLeftKneeNodeId;
pSkeletonJointsOrder[14] = kFBLeftAnkleNodeId;
pSkeletonJointsOrder[15] = kFBLeftFootNodeId;
pSkeletonJointsOrder[16] = kFBRightHipNodeId;
pSkeletonJointsOrder[17] = kFBRightKneeNodeId;
pSkeletonJointsOrder[18] = kFBRightAnkleNodeId;
pSkeletonJointsOrder[19] = kFBRightFootNodeId;
}
FBString ORDeviceMocapKinect::GetHardWareName()
{
return FBString("Kinect");
}
double ORDeviceMocapKinect::GetAverageSensorFloorOffset()
{
return mAverageSensorFloorOffset;
}
void ORDeviceMocapKinect::SetSensorFloorOffsetSet()
{
mSensorFloorOffsetSet = true;
}
/************************************************
* Fetch one frame skeleton data from the Kinect.
************************************************/
bool ORDeviceMocapKinect::FetchMocapData(FBTime &pTime)
{
if ( !mNuiSensor)
{
return false;
}
// Wait for 0ms, just quickly test if the skeleton data is available
if ( WAIT_OBJECT_0 == WaitForSingleObject(m_hNextSkeletonEvent, 0))
{
NUI_SKELETON_FRAME skeletonFrame = {0};
HRESULT hr = mNuiSensor->NuiSkeletonGetNextFrame(0, &skeletonFrame);
if ( FAILED(hr) )
{
return false;
}
// smooth out the skeleton data
mNuiSensor->NuiTransformSmooth(&skeletonFrame, NULL);
for (int i = 0 ; i < NUI_SKELETON_COUNT; ++i)
{
NUI_SKELETON_DATA lSkeletonData = skeletonFrame.SkeletonData[i];
NUI_SKELETON_TRACKING_STATE lTrackingState = skeletonFrame.SkeletonData[i].eTrackingState;
if (NUI_SKELETON_TRACKED == lTrackingState)
{
mKinectMocapJointsState->mFloorClipPlaneW = skeletonFrame.vFloorClipPlane.w;
CalculateAverageSensorFloorOffset();
mKinectMocapJointsState->mSkeletonTrackingState = (FBSkeletonTrackingState) lTrackingState;
mKinectMocapJointsState->mSkeletonPosition = PositionVector4ToFBTVector(lSkeletonData.Position);
NUI_SKELETON_BONE_ORIENTATION boneOrientations[NUI_SKELETON_POSITION_COUNT];
NuiSkeletonCalculateBoneOrientations(&lSkeletonData, boneOrientations);
// Copy data
SkeletonNodeInfo* lIter;
for(int j = 0; j < NUI_SKELETON_POSITION_COUNT; j++)
{
lIter = &mChannel[j];
if ( lSkeletonData.eSkeletonPositionTrackingState[j] != NUI_SKELETON_POSITION_NOT_TRACKED )
{
lIter->mT[0] = lSkeletonData.SkeletonPositions[j].x * 100;
lIter->mT[1] = lSkeletonData.SkeletonPositions[j].y * 100;
lIter->mT[2] = lSkeletonData.SkeletonPositions[j].z * 100;
mKinectMocapJointsState->mSkeletonJointTrackingState[j] = (FBSkeletonJointTrackingState)lSkeletonData.eSkeletonPositionTrackingState[j];
mKinectMocapJointsState->mSkeletonJointsPositions[j] = PositionVector4ToFBTVector(lSkeletonData.SkeletonPositions[j]);
NUI_SKELETON_BONE_ORIENTATION & orientation = boneOrientations[j];
Matrix4 lLocalRotationMatrix4 = orientation.absoluteRotation.rotationMatrix;
FBMatrix lGlobalRotationMatrix = RotationMatrix4ToFBMatrix(lLocalRotationMatrix4);
mKinectMocapJointsState->mSkeletonJointsGlobalOrientations[j] = lGlobalRotationMatrix;
FBRVector lRVector;
FBMatrixToRotation(lRVector,lGlobalRotationMatrix);
lIter->mR[0] = lRVector[0];
lIter->mR[1] = lRVector[1];
lIter->mR[2] = lRVector[2];
}
else
{
mKinectMocapJointsState->mSkeletonJointTrackingState[j] = (FBSkeletonJointTrackingState)lSkeletonData.eSkeletonPositionTrackingState[j];
lIter->mT[0] = mKinectMocapJointsState->mSkeletonJointsPositions[j][0];
lIter->mT[1] = mKinectMocapJointsState->mSkeletonJointsPositions[j][1];
lIter->mT[2] = mKinectMocapJointsState->mSkeletonJointsPositions[j][2];
FBRVector lRVector;
FBMatrixToRotation(lRVector,mKinectMocapJointsState->mSkeletonJointsGlobalOrientations[j]);
lIter->mR[0] = lRVector[0];
lIter->mR[1] = lRVector[1];
lIter->mR[2] = lRVector[2];
}
}
}
else if (NUI_SKELETON_POSITION_ONLY == lTrackingState)
{
}
}
}
else
{
return false;
}
return true;
}
void ORDeviceMocapKinect::CalculateAverageSensorFloorOffset()
{
if (!mSensorFloorOffsetSet)
{
if ( mKinectMocapJointsState->mFloorClipPlaneW != 0.0)
{
if (0.0 == mAverageSensorFloorOffset)
{
mAverageSensorFloorOffset = mKinectMocapJointsState->mFloorClipPlaneW*100.0;
}
else
{
mAverageSensorFloorOffset = (mAverageSensorFloorOffset * 0.9) + (mKinectMocapJointsState->mFloorClipPlaneW*100.0 * 0.1);
}
}
}
}
bool ORDeviceMocapKinect::SetupSpeechConnection()
{
if( mHasVoiceControl && mNuiSensor )
{
if(!mSpeech)
mSpeech = new Speech();
HRESULT hr = mSpeech->CreateFirstConnected(mNuiSensor);
if (FAILED(hr))
{
mSpeech->Stop();
return false;
}
}
return true;
}
SpeechCommands ORDeviceMocapKinect::ProcessSpeech()
{
if( mHasVoiceControl && mSpeech && mSpeech->IsValid() )
{
return mSpeech->Process();
}
return eNoCommand;
}
static FBMatrix RotationMatrix4ToFBMatrix(Matrix4 pMatrix4)
{
FBMatrix lMatrix;
lMatrix(0,0) = pMatrix4.M11;
lMatrix(0,1) = pMatrix4.M12;
lMatrix(0,2) = pMatrix4.M13;
lMatrix(1,0) = pMatrix4.M21;
lMatrix(1,1) = pMatrix4.M22;
lMatrix(1,2) = pMatrix4.M23;
lMatrix(2,0) = pMatrix4.M31;
lMatrix(2,1) = pMatrix4.M32;
lMatrix(2,2) = pMatrix4.M33;
lMatrix(3,3) = pMatrix4.M44;
return lMatrix;
}
static FBTVector PositionVector4ToFBTVector(Vector4 pVector4)
{
return FBTVector(pVector4.x*100, pVector4.y*100, pVector4.z*100);
}
Please send us your comment about this page