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; }