diff --git a/Modules/IGT/IGTFilters/mitkNavigationDataPlayer.cpp b/Modules/IGT/IGTFilters/mitkNavigationDataPlayer.cpp index c3008185fb..53c4823896 100644 --- a/Modules/IGT/IGTFilters/mitkNavigationDataPlayer.cpp +++ b/Modules/IGT/IGTFilters/mitkNavigationDataPlayer.cpp @@ -1,550 +1,550 @@ /*=================================================================== 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 "mitkNavigationDataPlayer.h" #include -#include +#include #include //includes for exceptions #include "mitkIGTException.h" #include "mitkIGTIOException.h" mitk::NavigationDataPlayer::NavigationDataPlayer() : mitk::NavigationDataPlayerBase() { m_NumberOfOutputs = 0; m_Pause = false; m_Playing = false; m_Stream = NULL; m_PlayerMode = NormalFile; m_FileName = ""; m_FileVersion = 1; m_Playing = false; m_Pause = false; m_NumberOfOutputs = 0; m_StartPlayingTimeStamp = 0.0; m_PauseTimeStamp = 0.0; m_parentElement = NULL; m_currentNode = NULL; m_StreamEnd = false; m_StreamSetOutsideFromClass = false; //To get a start time - mitk::TimeStamp::GetInstance()->Start(this); + mitk::IGTTimeStamp::GetInstance()->Start(this); } mitk::NavigationDataPlayer::~NavigationDataPlayer() { StopPlaying(); delete m_parentElement; } void mitk::NavigationDataPlayer::GenerateData() { //Only produce new output if the player is started if (!m_Playing) //m_Playing==true means player has started { //The output is not valid anymore for (unsigned int index = 0; index < m_NumberOfOutputs; index++) { mitk::NavigationData* output = this->GetOutput(index); assert(output); mitk::NavigationData::Pointer nd = mitk::NavigationData::New(); mitk::NavigationData::PositionType position; mitk::NavigationData::OrientationType orientation(0.0,0.0,0.0,0.0); position.Fill(0.0); nd->SetPosition(position); nd->SetOrientation(orientation); nd->SetDataValid(false); output->Graft(nd); } return; } //first of all get current time - TimeStampType now = mitk::TimeStamp::GetInstance()->GetElapsed(); + TimeStampType now = mitk::IGTTimeStamp::GetInstance()->GetElapsed(); //now we make a little time arithmetic //to get the elapsed time since the start of the player TimeStampType timeSinceStart = now - m_StartPlayingTimeStamp; //init the vectors std::vector< NavigationData::Pointer > nextCandidates; std::vector< NavigationData::Pointer > lastCandidates; std::vector< NavigationData::TimeStampType > currentTimeOfData; for (unsigned int index=0; index < m_NumberOfOutputs; index++) { nextCandidates.push_back(m_NextToPlayNavigationData.at(index)); lastCandidates.push_back(m_NextToPlayNavigationData.at(index)); currentTimeOfData.push_back(timeSinceStart + m_StartTimeOfData.at(index)); } if (m_NextToPlayNavigationData.size() != m_NumberOfOutputs) { MITK_ERROR << "Mismatch in data"; return; } // Now we try to find next NavigationData in the stream: // This means we step through the stream of NavigationDatas until we find // a NavigationData which has a current timestamp (currentTimeOfData) greater // than the current playing time. Then we store the data in // m_NextToPlayNavigationData and take the last data (lastCandidates) for the // output of this filter. // // The loop will stop when a suitable NavigationData is found or we reach EOF. // The timestamps of each recorded NavigationData should be equal // therefore we take always the time from the first. while( nextCandidates[0]->GetTimeStamp() < currentTimeOfData[0]) { for (unsigned int index=0; index < m_NumberOfOutputs; index++) { lastCandidates[index] = nextCandidates.at(index); switch(m_FileVersion) // m_FileVersion indicates which XML encoding is used { case 1: nextCandidates[index] = ReadVersion1(); break; default: //this case should not happen! therefore return at this point MITK_ERROR << "File encoding format was not stored, aborting!"; return; break; } //check if the input stream delivered a correct NavigationData object for (unsigned int i = 0; i < m_NumberOfOutputs; i++) { if (nextCandidates.at(index).IsNull()) //Stops playing because there is now more nextCandidate, the file ended for all outputs { m_StreamEnd = true; StopPlaying(); return; //the case if no NavigationData is found, e.g. EOF, bad stream } } } } //Now lastCandidates stores the new output and nextCandidates is stored to the m_NextToPlay vector for (unsigned int index = 0; index < m_NumberOfOutputs; index++) { mitk::NavigationData* output = this->GetOutput(index); assert(output); output->Graft(lastCandidates.at(index)); m_NextToPlayNavigationData[index] = nextCandidates.at(index); } } void mitk::NavigationDataPlayer::UpdateOutputInformation() { this->Modified(); // make sure that we need to be updated Superclass::UpdateOutputInformation(); } void mitk::NavigationDataPlayer::InitPlayer() { if (m_Stream == NULL) { StreamInvalid("Playing not possible. Wrong file name or path?"); return; } if (!m_Stream->good()) { StreamInvalid("Playing not possible. Stream is not good!"); return; } //first get the file version m_FileVersion = GetFileVersion(m_Stream); //check if we have a valid version: m_FileVersion has to be always bigger than 1 for playing if (m_FileVersion < 1) { StreamInvalid("Playing not possible. Invalid file version!"); return; } if(m_NumberOfOutputs == 0) {m_NumberOfOutputs = GetNumberOfNavigationDatas(m_Stream);} //with the information about the tracked tool number we can generate the output if (m_NumberOfOutputs > 0) { //Generate the output only if there are changes to the amount of outputs //This happens when the player is stopped and start again with different file if (this->GetNumberOfOutputs() != m_NumberOfOutputs) {SetNumberOfOutputs(m_NumberOfOutputs);} //initialize the player with first data GetFirstData(); //set stream valid m_ErrorMessage = ""; m_StreamValid = true; } else { StreamInvalid("The input stream seems to have NavigationData incompatible format"); return; } } unsigned int mitk::NavigationDataPlayer::GetFileVersion(std::istream* stream) { if (stream==NULL) { MITK_ERROR << "No input stream set!"; mitkThrowException(mitk::IGTException)<<"No input stream set!"; } if (!stream->good()) { MITK_ERROR << "Stream is not good!"; mitkThrowException(mitk::IGTException)<<"Stream is not good!"; } int version = 1; TiXmlDeclaration* dec = new TiXmlDeclaration(); *stream >> *dec; if(strcmp(dec->Version(),"") == 0) { MITK_ERROR << "The input stream seems to have XML incompatible format"; mitkThrowException(mitk::IGTIOException) << "The input stream seems to have XML incompatible format"; } m_parentElement = new TiXmlElement(""); *stream >> *m_parentElement; //2nd line this is the file version std::string tempValue = m_parentElement->Value(); if(tempValue != "Version") { if(tempValue == "Data"){ m_parentElement->QueryIntAttribute("version",&version); } } else { m_parentElement->QueryIntAttribute("Ver",&version); } if (version > 0) return version; else return 0; } unsigned int mitk::NavigationDataPlayer::GetNumberOfNavigationDatas(std::istream* stream) { if (stream == NULL) { MITK_ERROR << "No input stream set!"; mitkThrowException(mitk::IGTException)<<"No input stream set!"; } if (!stream->good()) { MITK_ERROR << "Stream not good!"; mitkThrowException(mitk::IGTException)<<"Stream not good!"; } //If something has changed in a future version of the XML definition e.g. navigationcount or addional parameters //catch this here with a select case block (see GenerateData() method) int numberOfTools = 0; std::string tempValue = m_parentElement->Value(); if(tempValue == "Version"){ *stream >> *m_parentElement; } m_parentElement->QueryIntAttribute("ToolCount",&numberOfTools); if (numberOfTools > 0) return numberOfTools; return 0; } mitk::NavigationData::Pointer mitk::NavigationDataPlayer::ReadVersion1() { if (m_Stream == NULL) { m_Playing = false; MITK_ERROR << "Playing not possible. Wrong file name or path? "; mitkThrowException(mitk::IGTException) << "Playing not possible. Wrong file name or path? "; } if (!m_Stream->good()) { m_Playing = false; MITK_ERROR << "Playing not possible. Stream is not good!"; mitkThrowException(mitk::IGTException) << "Playing not possible. Stream is not good!"; } /*TiXmlElement* elem = new TiXmlElement(""); m_currentNode = m_parentElement->IterateChildren(m_currentNode); if(m_currentNode) { elem = m_currentNode->ToElement(); }*/ TiXmlElement* elem; m_currentNode = m_parentElement->IterateChildren(m_currentNode); bool delElem; if(m_currentNode) { elem = m_currentNode->ToElement(); if(elem==NULL) { mitkThrowException(mitk::IGTException) << "Cannot find element: Is this file damaged?"; } delElem = false; } else { elem = new TiXmlElement(""); delElem = true; } mitk::NavigationData::Pointer nd = this->ReadNavigationData(elem); if(delElem) delete elem; return nd; } void mitk::NavigationDataPlayer::StartPlaying() { if (m_Stream == NULL) { m_Playing = false; //Perhaps the SetStream method was not called so we do this when a FileName is set with SetStream(PlayerMode) if (m_FileName != "") { //The PlayerMode is initialized with LastSetStream //CreateStreamFromFilename also calls InitPlayer() try { CreateStreamFromFilename(); } catch(mitk::IGTIOException e) { MITK_ERROR << "Cannot create stream from filename, please check the stream"; throw e; //TODO replace by macro } catch(mitk::IGTException e2) { MITK_ERROR << "Cannot open the file, please check the file"; throw e2; //TODO replace by macro } } //now check again if (m_Stream == NULL) { StopPlaying(); MITK_ERROR << "Playing not possible. Wrong file name or path?"; mitkThrowException(mitk::IGTException) << "Playing not possible. Wrong file name or path?"; } } if (!m_Playing && m_Stream->good()) { m_Playing = true; - m_StartPlayingTimeStamp = mitk::TimeStamp::GetInstance()->GetElapsed(); + m_StartPlayingTimeStamp = mitk::IGTTimeStamp::GetInstance()->GetElapsed(); } else { MITK_ERROR << "Player already started or stream is not good!"; StopPlaying(); } } void mitk::NavigationDataPlayer::StopPlaying() { //re init all data!! for playing again with different data //only PlayerMode and FileName are not changed m_Pause = false; m_Playing = false; if (!m_StreamSetOutsideFromClass) {delete m_Stream;} m_Stream = NULL; m_FileVersion = 1; m_Playing = false; m_Pause = false; m_StartPlayingTimeStamp = 0.0; m_PauseTimeStamp = 0.0; m_NextToPlayNavigationData.clear(); m_StartTimeOfData.clear(); } void mitk::NavigationDataPlayer::GetFirstData() { //Here we read the first lines of input (dependend on the number of inputs) for (unsigned int index=0; index < m_NumberOfOutputs; index++) { //Here we init the vector for later use m_NextToPlayNavigationData.push_back(NULL); m_StartTimeOfData.push_back(0.0); mitk::NavigationData::Pointer nd = this->GetOutput(index); switch(m_FileVersion) { case 1: m_NextToPlayNavigationData[index] = ReadVersion1(); //check if there is valid data in it if (m_NextToPlayNavigationData[index].IsNull()) { m_StreamEnd = true; StopPlaying(); mitkThrowException(mitk::IGTIOException) << "XML File is corrupt or has no NavigationData."; } //Have a look it the output was set already without this check the pipline will disconnect after a start/stop cycle if (nd.IsNull()) {this->SetNthOutput(index, m_NextToPlayNavigationData[index]);} m_StartTimeOfData[index] = m_NextToPlayNavigationData[index]->GetTimeStamp(); break; default: //this case should not happen! therefore the return at this point return; break; } } } void mitk::NavigationDataPlayer::Pause() { //player runs and pause was called -> pause the player if(m_Playing && !m_Pause) { m_Playing = false; m_Pause = true; - m_PauseTimeStamp = mitk::TimeStamp::GetInstance()->GetElapsed(); + m_PauseTimeStamp = mitk::IGTTimeStamp::GetInstance()->GetElapsed(); } else { MITK_ERROR << "Player is either not started or already is paused" << std::endl; } } void mitk::NavigationDataPlayer::Resume() { //player is in pause mode -> play at the last position if(!m_Playing && m_Pause) { m_Playing = true; m_Pause = false; - mitk::NavigationData::TimeStampType now = mitk::TimeStamp::GetInstance()->GetElapsed(); + mitk::NavigationData::TimeStampType now = mitk::IGTTimeStamp::GetInstance()->GetElapsed(); // in this case m_StartPlayingTimeStamp is set to the total elapsed time with NO playback m_StartPlayingTimeStamp = now - (m_PauseTimeStamp - m_StartPlayingTimeStamp); } else { MITK_ERROR << "Player is not paused!" << std::endl; } } void mitk::NavigationDataPlayer::CreateStreamFromFilename() { m_Stream = NULL; if (!itksys::SystemTools::FileExists(m_FileName.c_str())) { mitkThrowException(mitk::IGTIOException) << "File does not exist!"; } switch(m_PlayerMode) { case NormalFile: m_Stream = new std::ifstream(m_FileName.c_str()); m_StreamSetOutsideFromClass = false; break; case ZipFile: m_Stream = NULL; MITK_ERROR << "Sorry no ZipFile support yet"; break; default: m_Stream = NULL; mitkThrowException(mitk::IGTException) << "The stream is NULL"; break; } this->Modified(); InitPlayer(); } void mitk::NavigationDataPlayer::SetStream( std::istream* stream ) { if ( (stream == NULL) || (!stream->good())) { // throw an exception for stream=NULL or it is not good mitkThrowException(mitk::IGTException) << "The stream is NULL or it is not good"; m_StreamEnd = true; return; } m_Stream = stream; m_StreamSetOutsideFromClass = true; this->Modified(); InitPlayer(); } bool mitk::NavigationDataPlayer::IsAtEnd() { return this->m_StreamEnd; } void mitk::NavigationDataPlayer::StreamInvalid(std::string message) { m_StreamEnd = true; StopPlaying(); m_ErrorMessage = message; m_StreamValid = false; mitkThrowException(mitk::IGTIOException) << "Invalid stream!"; } \ No newline at end of file diff --git a/Modules/IGT/IGTFilters/mitkTrackingDeviceSource.cpp b/Modules/IGT/IGTFilters/mitkTrackingDeviceSource.cpp index 674ef77ef2..3a068bb2f7 100644 --- a/Modules/IGT/IGTFilters/mitkTrackingDeviceSource.cpp +++ b/Modules/IGT/IGTFilters/mitkTrackingDeviceSource.cpp @@ -1,215 +1,215 @@ /*=================================================================== 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 "mitkTrackingDeviceSource.h" #include "mitkTrackingDevice.h" #include "mitkTrackingTool.h" -#include "mitkTimeStamp.h" +#include "mitkIGTTimeStamp.h" mitk::TrackingDeviceSource::TrackingDeviceSource() : mitk::NavigationDataSource(), m_TrackingDevice(NULL) { } mitk::TrackingDeviceSource::~TrackingDeviceSource() { if (m_TrackingDevice.IsNotNull()) { if (m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking) { this->StopTracking(); } if (m_TrackingDevice->GetState() == mitk::TrackingDevice::Ready) { this->Disconnect(); } m_TrackingDevice = NULL; } } void mitk::TrackingDeviceSource::GenerateData() { if (m_TrackingDevice.IsNull()) return; if (m_TrackingDevice->GetToolCount() < 1) return; if (this->GetNumberOfOutputs() != m_TrackingDevice->GetToolCount()) // mismatch between tools and outputs. What should we do? Were tools added to the tracking device after SetTrackingDevice() was called? { //check this: TODO: ////this might happen if a tool is plugged into an aurora during tracking. //this->CreateOutputs(); std::stringstream ss; ss << "mitk::TrackingDeviceSource: not enough outputs available for all tools. " << this->GetNumberOfOutputs() << " outputs available, but " << m_TrackingDevice->GetToolCount() << " tools available in the tracking device."; throw std::out_of_range(ss.str()); } /* update outputs with tracking data from tools */ unsigned int toolCount = m_TrackingDevice->GetToolCount(); for (unsigned int i = 0; i < toolCount; ++i) { mitk::NavigationData* nd = this->GetOutput(i); assert(nd); mitk::TrackingTool* t = m_TrackingDevice->GetTool(i); assert(t); if ((t->IsEnabled() == false) || (t->IsDataValid() == false)) { nd->SetDataValid(false); continue; } nd->SetDataValid(true); mitk::NavigationData::PositionType p; t->GetPosition(p); nd->SetPosition(p); mitk::NavigationData::OrientationType o; t->GetOrientation(o); nd->SetOrientation(o); nd->SetOrientationAccuracy(t->GetTrackingError()); nd->SetPositionAccuracy(t->GetTrackingError()); - nd->SetTimeStamp( mitk::TimeStamp::GetInstance()->GetElapsed() ); + nd->SetTimeStamp( mitk::IGTTimeStamp::GetInstance()->GetElapsed() ); } } void mitk::TrackingDeviceSource::SetTrackingDevice( mitk::TrackingDevice* td ) { MITK_DEBUG << "Setting TrackingDevice to " << td; if (this->m_TrackingDevice.GetPointer() != td) { this->m_TrackingDevice = td; this->CreateOutputs(); std::stringstream name; // create a human readable name for the source name << td->GetData().Model << " Tracking Source"; this->SetName(name.str()); } } void mitk::TrackingDeviceSource::CreateOutputs(){ //if outputs are set then delete them if (this->GetNumberOfOutputs() > 0) { for (unsigned int numOP = this->GetNumberOfOutputs(); numOP>0; numOP--) this->RemoveOutput(this->GetOutput(numOP)); this->Modified(); } //fill the outputs if a valid tracking device is set if (m_TrackingDevice.IsNull()) return; this->SetNumberOfOutputs(m_TrackingDevice->GetToolCount()); // create outputs for all tools unsigned int numberOfOutputs = this->GetNumberOfOutputs(); for (unsigned int idx = 0; idx < numberOfOutputs; ++idx) { if (this->GetOutput(idx) == NULL) { DataObjectPointer newOutput = this->MakeOutput(idx); static_cast(newOutput.GetPointer())->SetName(m_TrackingDevice->GetTool(idx)->GetToolName()); // set NavigationData name to ToolName this->SetNthOutput(idx, newOutput); this->Modified(); } } } void mitk::TrackingDeviceSource::Connect() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (this->IsConnected()) return; if (m_TrackingDevice->OpenConnection() == false) throw std::runtime_error(std::string("mitk::TrackingDeviceSource: Could not open connection to tracking device. Error: ") + m_TrackingDevice->GetErrorMessage()); /* NDI Aurora needs a connection to discover tools that are connected to it. Therefore we need to create outputs for these tools now */ //if (m_TrackingDevice->GetType() == mitk::NDIAurora) //this->CreateOutputs(); } void mitk::TrackingDeviceSource::StartTracking() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking) return; if (m_TrackingDevice->StartTracking() == false) throw std::runtime_error("mitk::TrackingDeviceSource: Could not start tracking"); } void mitk::TrackingDeviceSource::Disconnect() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (m_TrackingDevice->CloseConnection() == false) throw std::runtime_error("mitk::TrackingDeviceSource: Could not close connection to tracking device"); } void mitk::TrackingDeviceSource::StopTracking() { if (m_TrackingDevice.IsNull()) throw std::invalid_argument("mitk::TrackingDeviceSource: No tracking device set"); if (m_TrackingDevice->StopTracking() == false) throw std::runtime_error("mitk::TrackingDeviceSource: Could not stop tracking"); } void mitk::TrackingDeviceSource::UpdateOutputInformation() { if(this->GetTrackingDevice()->GetToolCount() != this->GetNumberOfOutputs()) this->CreateOutputs(); this->Modified(); // make sure that we need to be updated Superclass::UpdateOutputInformation(); } //unsigned int mitk::TrackingDeviceSource::GetToolCount() //{ // if (m_TrackingDevice) // return m_TrackingDevice->GetToolCount(); // return 0; //} bool mitk::TrackingDeviceSource::IsConnected() { if (m_TrackingDevice.IsNull()) return false; return (m_TrackingDevice->GetState() == mitk::TrackingDevice::Ready) || (m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking); } bool mitk::TrackingDeviceSource::IsTracking() { if (m_TrackingDevice.IsNull()) return false; return m_TrackingDevice->GetState() == mitk::TrackingDevice::Tracking; } diff --git a/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp b/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp index 4b2abf2990..04899e16cb 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp @@ -1,318 +1,318 @@ /*=================================================================== 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 "mitkVirtualTrackingDevice.h" +#include "mitkIGTTimeStamp.h" #include #include #include -#include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::VirtualTrackingDevice::VirtualTrackingDevice() : mitk::TrackingDevice(), m_AllTools(), m_ToolsMutex(NULL), m_MultiThreader(NULL), m_ThreadID(-1), m_RefreshRate(100), m_NumberOfControlPoints(20) { m_Data = mitk::DeviceDataVirtualTracker; m_Bounds[0] = m_Bounds[2] = m_Bounds[4] = -400.0; // initialize bounds to -400 ... +400 (mm) cube m_Bounds[1] = m_Bounds[3] = m_Bounds[5] = 400.0; m_ToolsMutex = itk::FastMutexLock::New(); } mitk::VirtualTrackingDevice::~VirtualTrackingDevice() { if (this->GetState() == Tracking) { this->StopTracking(); } if (this->GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1)) { m_MultiThreader->TerminateThread(m_ThreadID); m_MultiThreader = NULL; } m_AllTools.clear(); } mitk::TrackingTool* mitk::VirtualTrackingDevice::AddTool(const char* toolName) { //if (this->GetState() == Tracking) //{ // return NULL; //} mitk::VirtualTrackingTool::Pointer t = mitk::VirtualTrackingTool::New(); t->SetToolName(toolName); t->SetVelocity(0.1); this->InitializeSpline(t); MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex m_AllTools.push_back(t); return t; } bool mitk::VirtualTrackingDevice::StartTracking() { if (this->GetState() != Ready) return false; this->SetState(Tracking); // go to mode Tracking this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread - mitk::TimeStamp::GetInstance()->Start(this); + mitk::IGTTimeStamp::GetInstance()->Start(this); if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1)) m_MultiThreader->TerminateThread(m_ThreadID); if (m_MultiThreader.IsNull()) m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } bool mitk::VirtualTrackingDevice::StopTracking() { if (this->GetState() == Tracking) // Only if the object is in the correct state { m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling m_StopTracking = true; m_StopTrackingMutex->Unlock(); this->SetState(Ready); } - mitk::TimeStamp::GetInstance()->Stop(this); + mitk::IGTTimeStamp::GetInstance()->Stop(this); m_TrackingFinishedMutex->Lock(); return true; } unsigned int mitk::VirtualTrackingDevice::GetToolCount() const { MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex return static_cast(this->m_AllTools.size()); } mitk::TrackingTool* mitk::VirtualTrackingDevice::GetTool(unsigned int toolNumber) const { MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex if ( toolNumber < m_AllTools.size()) return this->m_AllTools.at(toolNumber); return NULL; } bool mitk::VirtualTrackingDevice::OpenConnection() { if (m_NumberOfControlPoints < 1) { this->SetErrorMessage("to few control points for spline interpolation"); return false; } srand(time(NULL)); //Init random number generator this->SetState(Ready); return true; } void mitk::VirtualTrackingDevice::InitializeSpline( mitk::VirtualTrackingTool* t ) { if (t == NULL) return; typedef mitk::VirtualTrackingTool::SplineType SplineType; /* create random control points */ SplineType::ControlPointListType controlPoints; controlPoints.reserve(m_NumberOfControlPoints + 1); controlPoints.push_back(this->GetRandomPoint()); // insert point 0 double length = 0.0; // estimate spline length by calculating line segments lengths for (unsigned int i = 1; i < m_NumberOfControlPoints - 1; ++i) // set points 1..n-2 { SplineType::ControlPointType pos; pos = this->GetRandomPoint(); length += controlPoints.at(i - 1).EuclideanDistanceTo(pos); controlPoints.push_back(pos); } controlPoints.push_back(controlPoints.at(0)); // close spline --> insert point last control point with same value as first control point length += controlPoints.at(controlPoints.size() - 2).EuclideanDistanceTo(controlPoints.at(controlPoints.size() - 1)); /* Create knot list. TODO: rethink knot list values and list size. Is there a better solution? */ SplineType::KnotListType knotList; knotList.push_back(0.0); for (unsigned int i = 1; i < controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1; ++i) knotList.push_back(i); knotList.push_back(controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1); t->GetSpline()->SetControlPoints(controlPoints); t->GetSpline()->SetKnots(knotList); t->SetSplineLength(length); } bool mitk::VirtualTrackingDevice::CloseConnection() { bool returnValue = true; if(this->GetState() == Setup) return true; this->SetState(Setup); return returnValue; } mitk::ScalarType mitk::VirtualTrackingDevice::GetSplineChordLength(unsigned int idx) { mitk::VirtualTrackingTool* t = this->GetInternalTool(idx); if (t != NULL) return t->GetSplineLength(); else throw std::invalid_argument("invalid index"); } void mitk::VirtualTrackingDevice::SetToolSpeed(unsigned int idx, mitk::ScalarType roundsPerSecond) { if (roundsPerSecond < 0.0001) throw std::invalid_argument("Minimum tool speed is 0.0001 rounds per second"); mitk::VirtualTrackingTool* t = this->GetInternalTool(idx); if (t != NULL) t->SetVelocity(roundsPerSecond); else throw std::invalid_argument("invalid index"); } mitk::VirtualTrackingTool* mitk::VirtualTrackingDevice::GetInternalTool(unsigned int idx) { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if (idx < m_AllTools.size()) return m_AllTools.at(idx); else return NULL; } void mitk::VirtualTrackingDevice::TrackTools() { try { bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ if (!localStopTracking) { m_TrackingFinishedMutex->Lock(); } this->m_StopTrackingMutex->Unlock(); mitk::ScalarType t = 0.0; while ((this->GetState() == Tracking) && (localStopTracking == false)) { //for (ToolContainer::iterator itAllTools = m_AllTools.begin(); itAllTools != m_AllTools.end(); itAllTools++) for (unsigned int i = 0; i < this->GetToolCount(); ++i) // use mutexed methods to access tool container { mitk::VirtualTrackingTool::Pointer currentTool = this->GetInternalTool(i); mitk::VirtualTrackingTool::SplineType::PointType pos; /* calculate tool position with spline interpolation */ pos = currentTool->GetSpline()->EvaluateSpline(t); mitk::Point3D mp; mitk::itk2vtk(pos, mp); // convert from SplineType::PointType to mitk::Point3D currentTool->SetPosition(mp); // Currently, a constant speed is used. TODO: use tool velocity setting t += 0.001; if (t >= 1.0) t = 0.0; mitk::Quaternion quat; /* fix quaternion rotation */ quat.x() = 1.0; quat.y() = 1.0; quat.z() = 1.0; quat.r() = 1.0; currentTool->SetOrientation(quat); // TODO: rotate once per cycle around a fixed rotation vector currentTool->SetTrackingError( 2 * (rand() / (RAND_MAX + 1.0))); // tracking error in 0 .. 2 Range currentTool->SetDataValid(true); currentTool->Modified(); } itksys::SystemTools::Delay(m_RefreshRate); /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } // tracking ends if we pass this line m_TrackingFinishedMutex->Unlock(); // transfer control back to main thread } catch(...) { m_TrackingFinishedMutex->Unlock(); this->StopTracking(); this->SetErrorMessage("Error while trying to track tools. Thread stopped."); } } ITK_THREAD_RETURN_TYPE mitk::VirtualTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == NULL) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == NULL) { return ITK_THREAD_RETURN_VALUE; } VirtualTrackingDevice *trackingDevice = static_cast(pInfo->UserData); if (trackingDevice != NULL) trackingDevice->TrackTools(); trackingDevice->m_ThreadID = -1; // reset thread ID because we end the thread here return ITK_THREAD_RETURN_VALUE; } mitk::VirtualTrackingDevice::ControlPointType mitk::VirtualTrackingDevice::GetRandomPoint() { ControlPointType pos; pos[0] = m_Bounds[0] + (m_Bounds[1] - m_Bounds[0]) * (rand() / (RAND_MAX + 1.0)); // X = xMin + xRange * (random number between 0 and 1) pos[1] = m_Bounds[2] + (m_Bounds[3] - m_Bounds[2]) * (rand() / (RAND_MAX + 1.0)); // Y pos[2] = m_Bounds[4] + (m_Bounds[5] - m_Bounds[4]) * (rand() / (RAND_MAX + 1.0)); // Z return pos; }