std::vector<double> mitk::ClaronInterface::GetTipPosition(mitk::claronToolHandle c)
{
std::vector<double> returnValue;
double Position[3];
mtHandle t2m = Xform3D_New(); // tooltip to marker xform handle
mtHandle t2c = Xform3D_New(); // tooltip to camera xform handle
mtHandle m2c = Xform3D_New(); // marker to camera xform handle
//Get m2c
MTC( Marker_Marker2CameraXfGet (c, CurrCamera, m2c, &IdentifyingCamera) );
//Get t2m
MTC( Marker_Tooltip2MarkerXfGet (c, t2m ));
//Transform both to t2c
MTC(Xform3D_Concatenate(t2m,m2c,t2c));
//Get position
MTC( Xform3D_ShiftGet(t2c, Position) );
// Here we have to negate the X- and Y-coordinates because of a bug of the
// MTC-library.
returnValue.push_back(-Position[0]);
returnValue.push_back(-Position[1]);
returnValue.push_back(Position[2]);
return returnValue;
}
std::vector<double> mitk::ClaronInterface::GetTipQuaternions(claronToolHandle c)
{
std::vector<double> returnValue;
mtHandle t2m = Xform3D_New(); // tooltip to marker xform handle
mtHandle t2c = Xform3D_New(); // tooltip to camera xform handle
mtHandle m2c = Xform3D_New(); // marker to camera xform handle
//Get m2c
MTC( Marker_Marker2CameraXfGet (c, CurrCamera, m2c, &IdentifyingCamera) );
//Get t2m
MTC( Marker_Tooltip2MarkerXfGet (c, t2m ));
//Transform both to t2c
MTC(Xform3D_Concatenate(t2m,m2c,t2c));
//get the Claron-Quaternion
double Quarternions[4];
MTC( Xform3D_RotQuaternionsGet(t2c, Quarternions) );
mitk::Quaternion claronQuaternion;
//note: claron quarternion has different order than the mitk quarternion
claronQuaternion[3] = Quarternions[0];
claronQuaternion[0] = Quarternions[1];
claronQuaternion[1] = Quarternions[2];
claronQuaternion[2] = Quarternions[3];
// Here we have to make a -90�-turn around the Y-axis because of a bug of the
// MTC-library.
mitk::Quaternion minusNinetyDegreeY;
minusNinetyDegreeY[3] = sqrt(2.0)/2.0;
minusNinetyDegreeY[0] = 0;
minusNinetyDegreeY[1] = -1.0/(sqrt(2.0));
minusNinetyDegreeY[2] = 0;
//calculate the result...
mitk::Quaternion erg = (minusNinetyDegreeY*claronQuaternion);
returnValue.push_back(erg[3]);
returnValue.push_back(erg[0]);
returnValue.push_back(erg[1]);
returnValue.push_back(erg[2]);
return returnValue;
}
Xform3D* Xform3D::concatenate(Xform3D* subsequentXform)
{
Xform3D* concatXf = new Xform3D;
Xform3D_Concatenate(this->m_handle, subsequentXform->getHandle(), concatXf->getHandle());
return concatXf;
}
