void mitk::InternalTrackingTool::SetToolTip(mitk::Point3D toolTipPosition,
mitk::Quaternion orientation,
mitk::ScalarType eps)
{
if ( !Equal(m_ToolTip, toolTipPosition, eps) ||
!Equal(m_ToolTipRotation, orientation, eps) )
{
if( (toolTipPosition[0] == 0) &&
(toolTipPosition[1] == 0) &&
(toolTipPosition[2] == 0) &&
(orientation.x() == 0) &&
(orientation.y() == 0) &&
(orientation.z() == 0) &&
(orientation.r() == 1))
{
m_ToolTipSet = false;
}
else
{
m_ToolTipSet = true;
}
m_ToolTip = toolTipPosition;
m_ToolTipRotation = orientation;
this->Modified();
}
}
double mitk::StaticIGTHelperFunctions::GetAngleBetweenTwoQuaterions(mitk::Quaternion a, mitk::Quaternion b, itk::Vector<double,3> rotationVector)
{
double returnValue;
itk::Vector<double,3> point; //caution 5D-Tools: correct verctor along the tool axis is needed
point[0] = rotationVector[0];
point[1] = rotationVector[1];
point[2] = rotationVector[2];
//Quaternions used for rotations should alway be normalized, so just to be safe:
a.normalize();
b.normalize();
itk::Matrix<double,3,3> rotMatrixA;
for(int i=0; i<3; i++) for(int j=0; j<3; j++) rotMatrixA[i][j] = a.rotation_matrix_transpose().transpose()[i][j];
itk::Matrix<double,3,3> rotMatrixB;
for(int i=0; i<3; i++) for(int j=0; j<3; j++) rotMatrixB[i][j] = b.rotation_matrix_transpose().transpose()[i][j];
itk::Vector<double,3> pt1 = rotMatrixA * point;
itk::Vector<double,3> pt2 = rotMatrixB * point;
returnValue = (pt1[0]*pt2[0]+pt1[1]*pt2[1]+pt1[2]*pt2[2]) / ( sqrt(pow(pt1[0],2.0)+pow(pt1[1],2.0)+pow(pt1[2],2.0)) * sqrt(pow(pt2[0],2.0)+pow(pt2[1],2.0)+pow(pt2[2],2.0)));
returnValue = acos(returnValue) * 57.296; //57,296 = 180/Pi ; conversion to degrees
return returnValue;
}
bool runLoop()
{
bool success = true;
mitk::NavigationData::Pointer nd0;
mitk::NavigationData::Pointer nd1;
for(unsigned int i=0; i<player->GetNumberOfSnapshots();++i)
{
player->Update();
nd0 = player->GetOutput();
nd1 = player->GetOutput(1);
// test some values
if(nd0.IsNull() || nd1.IsNull()) return false;
if(i==0)
{
if (!(qTool0Snapshot0.as_vector() == nd0->GetOrientation().as_vector())) {success = false;}
}
else if(i==1)
{
if (!(tTool0Snapshot1 == nd0->GetPosition().GetVnlVector())) {success = false;}
else if (!(qTool1Snapshot1.as_vector() == nd1->GetOrientation().as_vector())) {success = false;}
}
else if(i==2) // should be repeated
{
if (!(tTool1Snapshot2 == nd1->GetPosition().GetVnlVector())) {success = false;}
}
}
return success;
}
/**
* Tests if NavigationData::GetInverse throws an error if the NavigationData has no inverse
* (e.g. after it is initialized, no rotation is stored -> the transformation cannot be inverted).
*/
static void TestInverseError()
{
// initialize empty NavigationData (quaternion is zeroed)
mitk::NavigationData::Pointer nd = mitk::NavigationData::New();
mitk::Quaternion quaternion;
quaternion.fill(0);
nd->SetOrientation(quaternion);
MITK_TEST_FOR_EXCEPTION(mitk::Exception&, nd->GetInverse());
}
void TestStandardWorkflow()
{
// create test values valid for the xml data above
tTool0Snapshot1[0] = -336.65;
tTool0Snapshot1[1] = 138.5;
tTool0Snapshot1[2]= -2061.07;
tTool1Snapshot2[0] = -56.93;
tTool1Snapshot2[1] = 233.79;
tTool1Snapshot2[2]= -2042.6;
vnl_vector_fixed<mitk::ScalarType,4> qVec;
qVec[0] = 0.0085;
qVec[1] = -0.0576;
qVec[2]= -0.0022;
qVec[3]= 0.9982;
qTool0Snapshot0 = mitk::Quaternion(qVec);
qVec[0] = 0.4683;
qVec[1] = 0.0188;
qVec[2]= -0.8805;
qVec[3]= 0.0696;
qTool1Snapshot1 = mitk::Quaternion(qVec);
//test SetXMLString()
player->SetXMLString(XML_STRING);
MITK_TEST_CONDITION_REQUIRED(player->GetNumberOfSnapshots() == 3,"Testing method SetXMLString with 3 navigation datas.");
MITK_TEST_CONDITION_REQUIRED(player->GetNumberOfIndexedOutputs() == 2,"Testing number of outputs");
//rest repeat
player->SetRepeat(true);
MITK_TEST_CONDITION_REQUIRED(runLoop(),"Testing first run.");
MITK_TEST_CONDITION_REQUIRED(runLoop(),"Testing second run."); //repeat is on should work a second time
// now test the go to snapshot function
player->GoToSnapshot(3);
mitk::NavigationData::Pointer nd1 = player->GetOutput(1);
MITK_TEST_CONDITION(tTool1Snapshot2 == nd1->GetPosition().GetVnlVector(),
"Testing GoToSnapshot() [1]");
player->GoToSnapshot(1);
mitk::NavigationData::Pointer nd0 = player->GetOutput();
MITK_TEST_CONDITION(qTool0Snapshot0.as_vector() == nd0->GetOrientation().as_vector(),
"Testing GoToSnapshot() [2]");
player->GoToSnapshot(3);
// and a third time
MITK_TEST_CONDITION_REQUIRED(runLoop(),"Tested if repeat works again.");
}
std::string mitk::NavigationToolWriter::ConvertQuaternionToString(mitk::Quaternion quat)
{
std::stringstream returnValue;
returnValue << quat.x() << ";" << quat.y() << ";" << quat.z() << ";" << quat.r();
return returnValue.str();
}