diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp index fbcb43762c..2bb9714591 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp @@ -1,315 +1,302 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include #define _USE_MATH_DEFINES #include #include BYTE MotionBuf[0x1FA400]; mitk::PolhemusInterface::PolhemusInterface() : m_continousTracking(false) { m_pdiDev = new CPDIdev(); m_numberOfTools = 0; } mitk::PolhemusInterface::~PolhemusInterface() { delete m_pdiDev; } bool mitk::PolhemusInterface::InitializeDevice() { m_pdiDev->ResetTracker(); m_pdiDev->ResetSAlignment(-1); m_pdiDev->Trace(TRUE, 7); m_continousTracking = false; return true; } bool mitk::PolhemusInterface::SetupDevice() { m_pdiDev->SetPnoBuffer(MotionBuf, 0x1FA400); m_pdiDev->SetMetric(true); //use cm instead of inches m_pdiDev->StartPipeExport(); CPDImdat pdiMDat; pdiMDat.Empty(); pdiMDat.Append(PDI_MODATA_FRAMECOUNT); pdiMDat.Append(PDI_MODATA_POS); pdiMDat.Append(PDI_MODATA_ORI); m_pdiDev->SetSDataList(-1, pdiMDat); CPDIbiterr cBE; m_pdiDev->GetBITErrs(cBE); if (!(cBE.IsClear())) { m_pdiDev->ClearBITErrs(); } return true; } bool mitk::PolhemusInterface::StartTracking() { - LPCTSTR szWindowClass = _T("PDIconsoleWinClass"); - HINSTANCE hInst = GetModuleHandle(0); - HWND hwnd = CreateWindowEx( - WS_EX_NOACTIVATE,//WS_EX_STATICEDGE, // - szWindowClass, - _T("MyWindowName"), - WS_POPUP, - 0, 0, 1, 1, - HWND_MESSAGE, - 0, - hInst, - 0); - m_continousTracking = true; - return m_pdiDev->StartContPno(hwnd); + return m_pdiDev->StartContPno(0); } bool mitk::PolhemusInterface::StopTracking() { m_continousTracking = false; m_pdiDev->StopContPno(); return true; } bool mitk::PolhemusInterface::Connect() { bool returnValue; //Initialize, and if it is not successful, return false. if (!InitializeDevice()) { returnValue = false; } //Connect else if (m_pdiDev->CnxReady()) { returnValue = true; } //If it is not successful, search for connections. else { CPDIser pdiSer; m_pdiDev->SetSerialIF(&pdiSer); ePiCommType eType = m_pdiDev->DiscoverCnx(); switch (eType) { case PI_CNX_USB: MITK_INFO << "USB Connection: " << m_pdiDev->GetLastResultStr(); break; case PI_CNX_SERIAL: MITK_INFO << "Serial Connection: " << m_pdiDev->GetLastResultStr(); break; default: MITK_INFO << "DiscoverCnx result: " << m_pdiDev->GetLastResultStr(); break; } //Setup device if (!SetupDevice()) { returnValue = false; } else { returnValue = m_pdiDev->CnxReady(); } } if (returnValue) { m_numberOfTools = this->GetNumberOfTools(); //On first startUp or if number of tools changed if (m_Hemispheres.empty() || m_Hemispheres.size() != m_numberOfTools) { //Set Hemisphere for all tools to default 1,0,0... mitk::Vector3D temp; mitk::FillVector3D(temp, 1, 0, 0); m_Hemispheres.clear(); m_Hemispheres.assign(m_numberOfTools, temp); this->SetHemisphere(-1, temp); } } return returnValue; } bool mitk::PolhemusInterface::Disconnect() { bool returnValue = true; //If Tracking is running, stop tracking first if (m_continousTracking) { this->StopTracking(); } returnValue = m_pdiDev->Disconnect(); MITK_INFO << "Disconnect: " << m_pdiDev->GetLastResultStr(); return returnValue; } std::vector mitk::PolhemusInterface::GetLastFrame() { PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); } std::vector returnValue = ParsePolhemusRawData(pBuf, dwSize); if (returnValue.empty()) { MITK_WARN << "Cannot parse data / no tools present"; } return returnValue; } unsigned int mitk::PolhemusInterface::GetNumberOfTools() { if (m_continousTracking) return GetLastFrame().size(); else return GetSingleFrame().size(); } std::vector mitk::PolhemusInterface::GetSingleFrame() { if (m_continousTracking) { MITK_WARN << "Cannot get a single frame when continuous tracking is on!"; return std::vector(); } PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev->ReadSinglePnoBuf(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); return std::vector(); } return ParsePolhemusRawData(pBuf, dwSize); } std::vector mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize) { std::vector returnValue; DWORD i = 0; while (i < dwSize) { BYTE ucSensor = pBuf[i + 2]; SHORT shSize = pBuf[i + 6]; // skip rest of header i += 8; PDWORD pFC = (PDWORD)(&pBuf[i]); PFLOAT pPno = (PFLOAT)(&pBuf[i + 4]); mitk::PolhemusInterface::trackingData currentTrackingData; currentTrackingData.id = ucSensor; currentTrackingData.pos[0] = pPno[0] * 10; //from cm to mm currentTrackingData.pos[1] = pPno[1] * 10; currentTrackingData.pos[2] = pPno[2] * 10; double azimuthAngle = pPno[3] / 180 * M_PI; //from degree to rad double elevationAngle = pPno[4] / 180 * M_PI; double rollAngle = pPno[5] / 180 * M_PI; vnl_quaternion eulerQuat(rollAngle, elevationAngle, azimuthAngle); currentTrackingData.rot = eulerQuat; returnValue.push_back(currentTrackingData); i += shSize; } return returnValue; } void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled) { //HemisphereTracking is switched on by SetSHemiTrack(-1). "-1" means for all sensors. //To switch heisphere tracking of, you need to set a hemisphere vector by calling SetSHemisphere(-1, { (float)1,0,0 }) if (_HeisphereTrackingEnabeled) { m_pdiDev->SetSHemiTrack(-1); } else if (!m_Hemispheres.empty()) { for (int i = 0; i < m_numberOfTools; ++i) SetHemisphere(i + 1, m_Hemispheres.at(i)); } else { //Default Hemisphere mitk::Vector3D temp; mitk::FillVector3D(temp, 1, 0, 0); m_Hemispheres.clear(); m_Hemispheres.assign(m_numberOfTools, temp); this->SetHemisphere(-1, temp); //MITK_INFO << m_pdiDev->GetLastResultStr(); } } void mitk::PolhemusInterface::ToggleHemisphere(int _tool) { BOOL _hemiTrack; //m_hemisphere indices start at 0, tool count starts at 1 //-1 == all tools if (_tool == -1) { for (int i = 0; i < m_numberOfTools; ++i) { //is hemiTrack on? m_pdiDev->GetSHemiTrack(i+1,_hemiTrack); m_Hemispheres.at(i)[0] = -m_Hemispheres.at(i)[0]; m_Hemispheres.at(i)[1] = -m_Hemispheres.at(i)[1]; m_Hemispheres.at(i)[2] = -m_Hemispheres.at(i)[2]; SetHemisphere(i+1, m_Hemispheres.at(i)); if (_hemiTrack) m_pdiDev->SetSHemiTrack(i+1); } } else { //is hemiTrack on? m_pdiDev->GetSHemiTrack(_tool, _hemiTrack); m_Hemispheres.at(_tool - 1)[0] = -m_Hemispheres.at(_tool - 1)[0]; m_Hemispheres.at(_tool - 1)[1] = -m_Hemispheres.at(_tool - 1)[1]; m_Hemispheres.at(_tool - 1)[2] = -m_Hemispheres.at(_tool - 1)[2]; SetHemisphere(_tool, m_Hemispheres.at(_tool - 1)); if (_hemiTrack) m_pdiDev->SetSHemiTrack(_tool); } } void mitk::PolhemusInterface::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { m_pdiDev->SetSHemisphere(_tool, { (float)_hemisphere[0], (float)_hemisphere[1], (float)_hemisphere[2] }); } void mitk::PolhemusInterface::PrintStatus() { MITK_INFO << "Polhemus status: " << this->m_pdiDev->CnxReady(); } \ No newline at end of file