TransformInterpolator::TransformInterpolator(const MATRIX &start, const MATRIX &end, unsigned int totalSteps)
: d(new TransformInterpolatorPrivate)
{
d->totalSteps = totalSteps;
d->currentStep = 0;
d->startTransform = start;
MatrixToQuaternion(d->startRotation, start);
setEnd(end);
}
bool DexMouseTracker::GetCurrentMarkerFrame( CodaFrame &frame ) {
POINT mouse_position;
GetCursorPos( &mouse_position );
RECT rect;
GetWindowRect( GetDesktopWindow(), &rect );
Vector3 position, rotated;
Vector3 x_dir, y_span, z_span;
Vector3 x_displacement, y_displacement, z_displacement;
double x, y, z;
Quaternion Ry, Rz, Q, nominalQ, midQ;
Matrix3x3 xform = {{-1.0, 0.0, 0.0},{0.0, 0.0, 1.0},{0.0, 1.0, 0.0}};
int mrk, id;
// Just set the target frame markers at their nominal fixed positions.
for ( mrk = 0; mrk < nFrameMarkers; mrk++ ) {
id = FrameMarkerID[mrk];
CopyVector( frame.marker[id].position, TargetFrameBody[mrk] );
}
// Shift the vertical bar markers to simulate being in the left position.
if ( IsDlgButtonChecked( dlg, IDC_LEFT ) ) {
frame.marker[DEX_NEGATIVE_BAR_MARKER].position[X] += 300.0;
frame.marker[DEX_POSITIVE_BAR_MARKER].position[X] += 300.0;
}
// Transform the marker positions of the target box and frame according
// to whether the system was upright or supine when it was aligned and
// according to whether the system is currently installed in the upright
// or supine configuration.
if ( ( IsDlgButtonChecked( dlg, IDC_SUPINE ) && TRACKER_ALIGNED_SUPINE != SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) ||
( IsDlgButtonChecked( dlg, IDC_SEATED ) && TRACKER_ALIGNED_UPRIGHT != SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) ) {
for ( mrk = 0; mrk < nFrameMarkers; mrk++ ) {
id = FrameMarkerID[mrk];
position[X] = - frame.marker[id].position[X];
position[Z] = frame.marker[id].position[Y];
position[Y] = frame.marker[id].position[Z];
CopyVector( frame.marker[id].position, position );
}
MatrixToQuaternion( nominalQ, xform );
}
else CopyQuaternion( nominalQ, nullQuaternion );
// Now set the visibility flag as a funciton of the GUI.
if ( IsDlgButtonChecked( dlg, IDC_BAR_OCCLUDED ) ) {
frame.marker[DEX_NEGATIVE_BAR_MARKER].visibility = false;
frame.marker[DEX_POSITIVE_BAR_MARKER].visibility = false;
}
else {
frame.marker[DEX_NEGATIVE_BAR_MARKER].visibility = true;
frame.marker[DEX_POSITIVE_BAR_MARKER].visibility = true;
}
if ( IsDlgButtonChecked( dlg, IDC_BOX_OCCLUDED ) ) {
frame.marker[DEX_NEGATIVE_BOX_MARKER].visibility = false;
frame.marker[DEX_POSITIVE_BOX_MARKER].visibility = false;
}
else {
frame.marker[DEX_NEGATIVE_BOX_MARKER].visibility = true;
frame.marker[DEX_POSITIVE_BOX_MARKER].visibility = true;
}
// Map mouse coordinates to world coordinates. The factors used here are empirical.
y = (double) ( mouse_position.y - rect.top ) / (double) ( rect.bottom - rect.top );
z = (double) (mouse_position.x - rect.right) / (double) ( rect.left - rect.right );
x = 0.0;
// By default, the orientation of the manipulandum is the nominal orientation.
CopyQuaternion( Q, nominalQ );
// Simulate wobbly movements of the manipulandum. This has not really been tested.
if ( IsDlgButtonChecked( dlg, IDC_CODA_WOBBLY ) ) {
// We will make the manipulandum rotate in a strange way as a function of the distance from 0.
// This is just so that we can test the routines that compute the manipulandum position and orientation.
double theta = y * 45.0;
double gamma = - z * 45.0;
SetQuaterniond( Ry, theta, iVector );
SetQuaterniond( Rz, gamma, jVector );
MultiplyQuaternions( midQ, Rz, nominalQ );
MultiplyQuaternions( Q, Ry, midQ );
// Make the movement a little bit in X as well so that we test the routines in 3D.
x = 0.0 + 5.0 * sin( y / 80.0);
}
// Map screen position of the mouse pointer to 3D position of the wrist and manipulandum.
// Top of the screen corresponds to the bottom of the bar and vice versa. It's inverted to protect the right hand rule.
// Right of the screen correponds to the nearest horizontal target and left corresponds to the farthest.
// The X position is set to be just to the right of the box.
SubtractVectors( y_span, frame.marker[DEX_POSITIVE_BAR_MARKER].position, frame.marker[DEX_NEGATIVE_BAR_MARKER].position );
SubtractVectors( x_dir, frame.marker[DEX_POSITIVE_BOX_MARKER].position, frame.marker[DEX_NEGATIVE_BOX_MARKER].position );
NormalizeVector( x_dir );
ComputeCrossProduct( z_span, x_dir, y_span );
ScaleVector( y_displacement, y_span, y );
ScaleVector( z_displacement, z_span, z );
ScaleVector( x_displacement, x_dir, x );
// Reference position is the bottom target on the vertical target bar.
CopyVector( position, frame.marker[DEX_NEGATIVE_BAR_MARKER].position );
// Place the manipulandum to the right of the box, even if the target bar is in the left position.
position[X] = frame.marker[DEX_NEGATIVE_BOX_MARKER].position[X];
// Shift the position in X if the is any wobble to it.
AddVectors( position, position, x_displacement );
// Shift the position in Y and Z according to the displacements computed from the mouse position.
AddVectors( position, position, y_displacement );
AddVectors( position, position, z_displacement );
frame.time = DexTimerElapsedTime( acquisitionTimer );
// Displace the manipulandum with the mouse and make it rotate.
for ( mrk = 0; mrk < nManipulandumMarkers; mrk++ ) {
id = ManipulandumMarkerID[mrk];
RotateVector( rotated, Q, ManipulandumBody[mrk] );
AddVectors( frame.marker[id].position, position, rotated );
frame.marker[id].visibility = true;
}
// Displace the wrist with the mouse, but don't rotate it.
for ( mrk = 0; mrk < nWristMarkers; mrk++ ) {
id = WristMarkerID[mrk];
AddVectors( frame.marker[id].position, position, WristBody[mrk] );
frame.marker[id].visibility = true;
}
// Output the position and orientation used to compute the simulated
// marker positions. This is for testing only.
fprintf( fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
frame.time,
position[X], position[Y], position[Z],
Q[X], Q[Y], Q[Z], Q[M] );
return( true );
}
void TransformInterpolator::setEnd(const MATRIX &end)
{
d->endTransform = end;
MatrixToQuaternion(d->endRotation, end);
}