void torsoController::UpdateRobotStatus(const RobotStatus& st)
{
Quaternion targetQuat;
targetQuat[0] = st.headRotation[0];
targetQuat[1] = st.headRotation[3];
targetQuat[2] = st.headRotation[1];
targetQuat[3] = -st.headRotation[2];
m_headPos[0] = -st.headPos[2];
m_headPos[1] = -st.headPos[0];
m_headPos[2] = st.headPos[1];
//printf("HEAD POSITION = %f,%f,%f\n", m_headPos[0], m_headPos[1], m_headPos[2]);
float p[3];
p[0] = m_headPos[0] + m_offset[0];
p[1] = m_headPos[1] + m_offset[1];
p[2] = m_headPos[2] + m_offset[2];
if (st.connected)
threadStart = true;
else
threadStart = false;
ConvertToMatrix(targetQuat, p, targetRotMat);
}
xlw::MyMatrix xlw::XlfOper4::AsMatrix( const std::string& ErrorId, int * pxlret) const
{
MyMatrix output; // will be resized anyway
int xlret = ConvertToMatrix(output);
if (pxlret)
*pxlret=xlret;
else
ThrowOnError(xlret,ErrorId+" conversion to matrix failed");
return output;
}
/***********************
* DlgProc: Process the Dialog Box commands
* @author: Callan Moore
* @Parameter: _hWnd: Handle to the Dialog Box
* @Parameter: _uiMsg: The message ID being sent
* @Parameter: _wParam: Additional detail about the message being sent
* @Parameter: _lParam: Additional detail about the message being sent
* @return: BOOL: Boolean Result
********************/
BOOL CALLBACK DlgProc(HWND _hDlg, UINT _msg, WPARAM _wparam, LPARAM _lparam)
{
static HWND hEditBox = 0;
switch(_msg)
{
case(WM_INITDIALOG):
{
InitialSetup( _hDlg);
return (0);
}
break;
case(WM_COMMAND):
{
switch(LOWORD( _wparam))
{
case (IDC_QUAT_SLERP): // SLERP Button for calculating the SLERP
{
SLERP(_hDlg);
}
break;
case (IDC_CONVERT_A): // Convert Quaternion A to a Matrix
{
ConvertToMatrix(_hDlg, 'a');
}
break;
case (IDC_CONVERT_B): // Convert Quaternion B to a Matrix
{
ConvertToMatrix(_hDlg, 'b');
}
break;
case (IDC_CONVERT_SLERP): // Convert the Quaternion calculatedvia SLERP to a Matrix
{
ConvertToMatrix(_hDlg, 's');
}
break;
case (IDC_RESET): // Reset Button to reset the calculator back to default starting values
{
InitialSetup(_hDlg);
}
break;
default: break;
} // End Switch
}
break;
case(WM_CLOSE):
{
switch( MessageBox( _hDlg, L"Are you sure you would like to close the calculator?", L"Exit", MB_ICONQUESTION | MB_YESNO))
{
case (IDYES):
{
// Quit out of the whole application if dialog box is closed
PostQuitMessage(0);
return true;
}
break;
case (IDNO): // Do Nothing -> Fall Through
default: break;
} // End Switch
}
default:
{
break;
}
} // End Switch
return false;
}
