Progress on HATO handling in Matlab and tracking

parent e91a641c
......@@ -2273,10 +2273,10 @@ void set_sound_receiver_pose( int, mxArray *[], int nrhs, const mxArray *prhs[]
pConnection->pCoreInterface->SetSoundReceiverPose( isoundreceiverID, v3Pos, qOrient );
}
REGISTER_PUBLIC_FUNCTION( get_sound_receiver_real_world_torso_orientation, "Returns the real-world position and orientation (as quaterion) of the sound receiver's torso", "The orientation is meant to be relative to the real world sound receiver orientation." );
DECLARE_FUNCTION_REQUIRED_INARG( get_sound_receiver_real_world_torso_orientation, soundreceiverID, "integer-1x1", "Sound receiver ID" );
DECLARE_FUNCTION_OUTARG( get_sound_receiver_real_world_torso_orientation, view, "double-4", "Rotation quaternion [w,x,y,z]" );
void get_sound_receiver_real_world_torso_orientation( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
REGISTER_PUBLIC_FUNCTION( get_sound_receiver_real_world_head_above_torso_orientation, "Returns the real-world orientation (as quaterion) of the sound receiver's head over the torso", "The orientation is meant to be inversely relative to the real world sound receiver orientation." );
DECLARE_FUNCTION_REQUIRED_INARG( get_sound_receiver_real_world_head_above_torso_orientation, soundreceiverID, "integer-1x1", "Sound receiver ID" );
DECLARE_FUNCTION_OUTARG( get_sound_receiver_real_world_head_above_torso_orientation, view, "double-4", "Rotation quaternion [w,x,y,z]" );
void get_sound_receiver_real_world_head_above_torso_orientation( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
REQUIRE_INPUT_ARGS( 2 );
......@@ -2285,10 +2285,27 @@ void get_sound_receiver_real_world_torso_orientation( int nlhs, mxArray *plhs[],
int isoundreceiverID = matlabGetIntegerScalar( prhs[ 1 ], "soundreceiverID" );
VAQuat qOrient = pConnection->pCoreInterface->GetSoundReceiverRealWorldTorsoOrientation( isoundreceiverID );
VAQuat qOrient = pConnection->pCoreInterface->GetSoundReceiverRealWorldHeadAboveTorsoOrientation( isoundreceiverID );
plhs[ 0 ] = matlabCreateQuaternion( qOrient );
}
REGISTER_PUBLIC_FUNCTION( set_sound_receiver_real_world_head_above_torso_orientation, "Updates the real-world position and orientation (as view- and up vector) of the sound receiver's head", "Note: The view and up vector must be an orthonormal pair of vectors." );
DECLARE_FUNCTION_REQUIRED_INARG( set_sound_receiver_real_world_head_above_torso_orientation, soundreceiverID, "integer-1x1", "Sound receiver ID" );
DECLARE_FUNCTION_REQUIRED_INARG( set_sound_receiver_real_world_head_above_torso_orientation, pos, "double-4", "Rotation quaternion [w, x, y, z]" );
void set_sound_receiver_real_world_head_above_torso_orientation( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
REQUIRE_INPUT_ARGS( 3 );
ConnectionHandle hHandle = GetConnectionHandle( prhs[ 0 ] );
CVAMatlabConnection* pConnection = g_vpConnections[ hHandle ];
const int isoundreceiverID = matlabGetIntegerScalar( prhs[ 1 ], "soundreceiverID" );
VAQuat qOrient;
matlabGetQuaternion( prhs[ 2 ], "orient", qOrient );
pConnection->pCoreInterface->SetSoundReceiverRealWorldHeadAboveTorsoOrientation( isoundreceiverID, qOrient );
}
REGISTER_PUBLIC_FUNCTION( get_sound_receiver_real_world_head_position_orientation_view_up, "Returns the real-world position and orientation (as view- and up vector) of the sound receiver's head", "Note: The view and up vector must be an orthonormal pair of vectors. The parameter isoundreceiverID has been added for future versions and is currently unsupported. You can set it any value you like." );
DECLARE_FUNCTION_REQUIRED_INARG( get_sound_receiver_real_world_head_position_orientation_view_up, soundreceiverID, "integer-1x1", "Sound receiver ID" );
DECLARE_FUNCTION_OUTARG( get_sound_receiver_real_world_head_position_orientation_view_up, pos, "double-3", "Position vector [x,y,z] (unit: meters)" );
......
......@@ -60,32 +60,24 @@ void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
pVACore->SetSoundReceiverPosition( iTrackedReceiverID, VAVec3( vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] ) );
pVACore->SetSoundReceiverOrientationVU( iTrackedReceiverID, VAVec3( vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ] ), VAVec3( vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] ) );
}
catch( ... )
{
}
}
// HATO orientation
if( ( iTrackedReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedReceiverHATORigidBodyIndex > 0 ) )
{
try
{
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedReceiverHATORigidBodyIndex - 1 ] );
VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
// Convert absolute orientation to relative HATO orientation
VAQuat qOrient = pVACore->GetSoundReceiverOrientation( iTrackedReceiverID );
VistaQuaternion qHeadOrient( qOrient.comp );
VistaQuaternion qHATOOrient = qOrientRaw * qHeadOrient;
VistaQuaternion qHATOOrient = qOrientRaw * qOrientRigidBody;
VAQuat qFinalHATO = VAQuat( qHATOOrient[ Vista::X ], qHATOOrient[ Vista::Y ], qHATOOrient[ Vista::Z ], qHATOOrient[ Vista::W ] );
pVACore->SetSoundReceiverHeadAboveTorsoOrientation( iTrackedReceiverID, qFinalHATO );
}
}
catch( ... )
{
}
}
}
// Real-world sound receiver
......@@ -115,21 +107,21 @@ void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
VistaVector3D vPosRigidBody = vPosPivotPoint + vPosOffsetGlobalCoordinateSystem;
pVACore->SetSoundReceiverRealWorldPositionOrientationVU( iTrackedRealWorldSoundReceiverID, VAVec3( vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] ), VAVec3( vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ] ), VAVec3( vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] ) );
}
catch( ... )
{
}
}
// Real world receiver HATO orientation
if( ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex > 0 ) )
{
try
{
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldSoundReceiverHATORigidBodyIndex - 1 ] );
VAQuat qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
const VistaQuaternion qTorsoOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
const VistaQuaternion qTorsoOrient = qOrientRotation * qOrientRigidBody;
pVACore->SetSoundReceiverRealWorldTorsoOrientation( iTrackedReceiverID, qOrientRaw );
const VAQuat qFinalOrient( qTorsoOrient[ Vista::X ], qTorsoOrient[ Vista::Y ], qTorsoOrient[ Vista::Z ], qTorsoOrient[ Vista::W ] );
pVACore->SetSoundReceiverRealWorldHeadAboveTorsoOrientation( iTrackedReceiverID, qFinalOrient );
}
}
catch( ... )
{
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment