diff --git a/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp b/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp index 4ff8e3cbce..058ea627a9 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkVirtualTrackingDevice.cpp @@ -1,316 +1,319 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Language: C++ Date: $Date: $ Version: $Revision: $ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #include "mitkVirtualTrackingDevice.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_Type = VirtualTracker; 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); 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); 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) MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope + 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; }