//--------------------------------------------------------------------------------------
// Name: Update()
// Desc: Adds a new frame of positions, updates the coordiante system, and calculates
// left and right hand.
//--------------------------------------------------------------------------------------
VOID SpineRelativeCameraSpaceCoordinateSystem::Update( const NUI_SKELETON_FRAME* pRawSkeletonFrame, INT iSkeletonIndex, XMVECTOR vLeft, XMVECTOR vRight )
{
if ( pRawSkeletonFrame == NULL ) return;
if ( iSkeletonIndex < 0 || iSkeletonIndex >= NUI_SKELETON_COUNT ) return;
CONST XMVECTOR *pSkeletonPosition = &pRawSkeletonFrame->SkeletonData[iSkeletonIndex].SkeletonPositions[0];
if ( m_dwLastTrackingID != pRawSkeletonFrame->SkeletonData[iSkeletonIndex].dwTrackingID )
{
m_vAverageNormalToGravity = pRawSkeletonFrame->vNormalToGravity;
}
else
{
m_vAverageNormalToGravity = m_fSpineUpdateRate * m_vAverageNormalToGravity +
pRawSkeletonFrame->vNormalToGravity * ( 1.0f - m_fSpineUpdateRate );
}
#if 0
CHAR out[255];
sprintf_s( out, "x=%f,y=%f,z=%f,w=%f\n", pRawSkeletonFrame->vNormalToGravity.x, pRawSkeletonFrame->vNormalToGravity.y, pRawSkeletonFrame->vNormalToGravity.z, pRawSkeletonFrame->vNormalToGravity.w );
OutputDebugString( out );
#endif
m_matRotateToNormalToGravity = NuiTransformMatrixLevel( m_vAverageNormalToGravity );
XMVECTOR vSpineTilted = XMVector3Transform( pSkeletonPosition[NUI_SKELETON_POSITION_SPINE], m_matRotateToNormalToGravity );
XMVECTOR vHeadTilted = XMVector3Transform( pSkeletonPosition[NUI_SKELETON_POSITION_HEAD], m_matRotateToNormalToGravity );
m_vLeftHandRelative = XMVector3Transform( vLeft, m_matRotateToNormalToGravity );
m_vRightHandRelative = XMVector3Transform( vRight, m_matRotateToNormalToGravity );
FLOAT fSpineHeadLength = XMVectorGetY( vHeadTilted ) - XMVectorGetY( vSpineTilted );
if ( m_dwLastTrackingID != pRawSkeletonFrame->SkeletonData[iSkeletonIndex].dwTrackingID )
{
m_dwLastTrackingID = pRawSkeletonFrame->SkeletonData[iSkeletonIndex].dwTrackingID;
m_vAverageSpine = vSpineTilted;
m_fAverageSpineHeadLength = fSpineHeadLength;
}
else
{
m_vAverageSpine = m_vAverageSpine * m_fSpineUpdateRate +
vSpineTilted * ( 1.0f - m_fSpineUpdateRate );
m_fAverageSpineHeadLength = ATG::Lerp( fSpineHeadLength, m_fAverageSpineHeadLength, m_fBodySizeUpdateRate );
}
m_vEstiamtedPivotOffsetLeft = XMVectorSet( m_fAverageSpineHeadLength * 0.3f, m_fAverageSpineHeadLength * 0.1f, 0.0f, 0.0f );
m_vEstiamtedPivotOffsetRight = XMVectorSet( -m_fAverageSpineHeadLength * 0.3f, m_fAverageSpineHeadLength * 0.1f, 0.0f, 0.0f );
m_vRightHandRelative -= m_vAverageSpine;
m_vRightHandRelative += m_vEstiamtedPivotOffsetRight;
m_vLeftHandRelative -= m_vAverageSpine;
m_vLeftHandRelative += m_vEstiamtedPivotOffsetLeft;
static XMVECTOR vFlipZ = XMVectorSet( 1.0f, 1.0f, -1.0f, 1.0f );
m_vRightHandRelative *= vFlipZ;
m_vLeftHandRelative *= vFlipZ;
}
//--------------------------------------------------------------------------------------
// Applies tilt correction to the skeleton data data. Source and destination can be the same
//--------------------------------------------------------------------------------------
VOID ApplyTiltCorrectionInPlayerSpace( NUI_SKELETON_FRAME* pDstSkeleton, const NUI_SKELETON_FRAME* pSrcSkeleton )
{
assert( pDstSkeleton );
assert( pSrcSkeleton );
if ( !pDstSkeleton ||
!pSrcSkeleton )
{
return;
}
static const XMVECTOR vUp = XMVectorSet( 0.0f, 1.0f, 0.0f, 0.0f );
static XMVECTOR vAverageSpine[ NUI_SKELETON_COUNT ] = { XMVectorZero(), XMVectorZero(), XMVectorZero(),
XMVectorZero(), XMVectorZero(), XMVectorZero() };
static XMVECTOR vAverageNormalToGravity = pSrcSkeleton->vNormalToGravity;
// Get a valid up vector
XMVECTOR vNormToGrav = pSrcSkeleton->vNormalToGravity;
// Check for an invalid up vector (we will synthesize it from
// the floor plane if that data is present). If we can't get an up
// vector, we default to 0.0, 1.0, 0.0 instead.
if ( fabs(vNormToGrav.x) < FLT_EPSILON &&
fabs(vNormToGrav.y) < FLT_EPSILON &&
fabs(vNormToGrav.z) < FLT_EPSILON )
{
vNormToGrav = vUp;
}
// Calculate running average of vector
vAverageNormalToGravity = XMVectorLerp( vAverageNormalToGravity, vNormToGrav, 0.1f );
// Generate the leveling matrix and apply it to all points on any skeletons
// which are currently being tracked.
XMMATRIX matLevel = NuiTransformMatrixLevel( vAverageNormalToGravity );
for ( INT i = 0 ; i < NUI_SKELETON_COUNT; i++ )
{
const NUI_SKELETON_DATA* pSkeletonData = &pSrcSkeleton->SkeletonData[ i ];
XMVECTOR vSpine = pSkeletonData->SkeletonPositions[ NUI_SKELETON_POSITION_SPINE ];
if ( pSkeletonData->eTrackingState != NUI_SKELETON_TRACKED )
{
vAverageSpine[ i ] = XMVectorZero();
continue;
}
else
{
UINT uCompareResults;
XMVectorEqualR( &uCompareResults, XMVectorZero(), vAverageSpine[ i ] );
// if still set to zero, then start the running average
if( XMComparisonAllTrue( uCompareResults ) )
{
vAverageSpine[ i ] = vSpine;
}
}
// Running average of spine
vAverageSpine[ i ] = XMVectorLerp( vAverageSpine[ i ], vSpine, 0.1f );
XMFLOAT4 fAverageSpine;
XMStoreFloat4( &fAverageSpine, vAverageSpine[ i ] );
XMMATRIX matTranslateToOrigin = XMMatrixTranslation( -fAverageSpine.x, 0, -fAverageSpine.z );
XMMATRIX matTranslateFromOrigin = XMMatrixTranslation( fAverageSpine.x, 0, fAverageSpine.z );
XMMATRIX matTransformation = matTranslateToOrigin * matLevel * matTranslateFromOrigin;
for ( UINT j = 0; j < NUI_SKELETON_POSITION_COUNT; j++ )
{
pDstSkeleton->SkeletonData[ i ].SkeletonPositions[ j ] = XMVector3Transform( pSkeletonData->SkeletonPositions[ j ], matLevel );
}
}
}
