/**Call when viewport size or zoom has changed.
* Recompute camera zoom and reps requiring vpMs.
*/
void ViewWrapper2D::viewportChanged()
{
if (!mView->getRenderer()->IsActiveCameraCreated())
return;
mView->setZoomFactor(mZoom2D->getFactor());
double viewHeight = mView->getViewport_s().range()[1];
mView->getRenderer()->GetActiveCamera()->SetParallelScale(viewHeight / 2);
// Heuristic guess for a good clip depth. The point is to show 3D data clipped in 2D
// with a suitable thickness projected onto the plane.
double clipDepth = 2.0; // i.e. all 3D props rendered outside this range is not shown.
double length = clipDepth*10;
clipDepth = viewHeight/120 + 1.5;
mView->getRenderer()->GetActiveCamera()->SetPosition(0,0,length);
mView->getRenderer()->GetActiveCamera()->SetClippingRange(length-clipDepth, length+0.1);
mSliceProxy->setToolViewportHeight(viewHeight);
double anyplaneViewOffset = settings()->value("Navigation/anyplaneViewOffset").toDouble();
mSliceProxy->initializeFromPlane(mSliceProxy->getComputer().getPlaneType(), false, Vector3D(0, 0, 1), true, viewHeight, anyplaneViewOffset, true);
DoubleBoundingBox3D BB_vp = getViewport();
Transform3D vpMs = mView->get_vpMs();
DoubleBoundingBox3D BB_s = transform(vpMs.inv(), BB_vp);
PLANE_TYPE plane = mSliceProxy->getComputer().getPlaneType();
mToolRep2D->setViewportData(vpMs, BB_vp);
if (mSlicePlanes3DMarker)
{
mSlicePlanes3DMarker->getProxy()->setViewportData(plane, mSliceProxy, BB_s);
}
mViewFollower->setView(BB_s);
}
double ManualImage2ImageRegistrationWidget::getAccuracy(QString uid)
{
DataPtr fixedData = mServices->registration()->getFixedData();
if (!fixedData)
return 1000.0;
DataPtr movingData = mServices->registration()->getMovingData();
if (!movingData)
return 1000.0;
Landmark masterLandmark = fixedData->getLandmarks()->getLandmarks()[uid];
Landmark targetLandmark = movingData->getLandmarks()->getLandmarks()[uid];
if (masterLandmark.getUid().isEmpty() || targetLandmark.getUid().isEmpty())
return 1000.0;
Vector3D p_master_master = masterLandmark.getCoord();
Vector3D p_target_target = targetLandmark.getCoord();
Transform3D rMmaster = fixedData->get_rMd();
Transform3D rMtarget = movingData->get_rMd();
Vector3D p_target_r = rMtarget.coord(p_target_target);
Vector3D p_master_r = rMmaster.coord(p_master_master);
double targetPoint[3];
double masterPoint[3];
targetPoint[0] = p_target_r[0];
targetPoint[1] = p_target_r[1];
targetPoint[2] = p_target_r[2];
masterPoint[0] = p_master_r[0];
masterPoint[1] = p_master_r[1];
masterPoint[2] = p_master_r[2];
return (vtkMath::Distance2BetweenPoints(targetPoint, masterPoint));
}
Vector3D Frame3D::getEulerXYZ() const
{
Transform3D t;
t = mAngleAxis;
Vector3D ea = t.matrix().block<3, 3> (0, 0).eulerAngles(0, 1, 2);
return ea;
}
float Transform3D::norm(Transform3D T){
float pos_norm = arma::norm(T.translation());
UnitQuaternion q = UnitQuaternion(Rotation3D(T.submat(0,0,2,2)));
float angle = q.getAngle();
//TODO: how to weight these two?
return pos_norm + Rotation3D::norm(T.rotation());
}
void LandmarkRegistrationWidget::cellChangedSlot(int row, int column)
{
QTableWidgetItem* item = mLandmarkTableWidget->item(row, column);
QString uid = item->data(Qt::UserRole).toString();
if (column == 0)
{
QString name = item->text();
mServices->patient()->setLandmarkName(uid, name);
}
if (column == 1)
{
Qt::CheckState state = item->checkState();
mServices->patient()->setLandmarkActive(uid, state == Qt::Checked);
this->performRegistration(); // automatic when changing active state (Mantis #0000674)s
}
if (column == 2)
{
QString val = item->text();
// remove formatting stuff:
val = val.replace('(', " ");
val = val.replace(')', " ");
val = val.replace(',', " ");
Transform3D rMtarget = this->getTargetTransform();
Vector3D p_r = Vector3D::fromString(val);
Vector3D p_target = rMtarget.inv().coord(p_r);
this->setTargetLandmark(uid, p_target);
}
}
vtkPolyDataPtr polydataFromTransforms(TimedTransformMap transformMap_prMt, Transform3D rMpr)
{
vtkPolyDataPtr retval = vtkPolyDataPtr::New();
vtkPointsPtr points = vtkPointsPtr::New();
vtkCellArrayPtr lines = vtkCellArrayPtr::New();
points->Allocate(transformMap_prMt.size());
TimedTransformMap::iterator mapIter = transformMap_prMt.begin();
while(mapIter != transformMap_prMt.end())
{
Vector3D point_t = Vector3D(0,0,0);
Transform3D prMt = mapIter->second;
Transform3D rMt = rMpr * prMt;
Vector3D p = rMt.coord(point_t);
points->InsertNextPoint(p.begin());
++mapIter;
}
lines->Initialize();
std::vector<vtkIdType> ids(points->GetNumberOfPoints());
for (unsigned i=0; i<ids.size(); ++i)
ids[i] = i;
lines->InsertNextCell(ids.size(), &(*ids.begin()));
retval->SetPoints(points);
retval->SetLines(lines);
return retval;
}
std::vector<Vector3D> CustomMetric::getPointCloud() const
{
std::vector<Vector3D> retval;
DataPtr model = this->getModel();
std::vector<Transform3D> pos = this->calculateOrientations();
std::vector<Vector3D> cloud;
Transform3D rrMd;
if (model)
{
rrMd = model->get_rMd();
cloud = model->getPointCloud();
}
else
{
cloud.push_back(Vector3D::Zero());
rrMd = Transform3D::Identity();
}
for (unsigned i=0; i<pos.size(); ++i)
{
Transform3D rMd = pos[i] * rrMd;
for (unsigned j=0; j<cloud.size(); ++j)
{
Vector3D p_r = rMd.coord(cloud[j]);
retval.push_back(p_r);
}
}
return retval;
}
void CustomMetric::updateTexture(MeshPtr model, Transform3D rMrr)
{
if (!model)
return;
if (!this->getTextureFollowTool() || !model->hasTexture())
return;
// special case:
// Project tool position down to the model, then set that position as
// the texture x pos.
Transform3D rMt = mSpaceProvider->getActiveToolTipTransform(CoordinateSystem::reference());
Transform3D rMd = rMrr * model->get_rMd();
Vector3D t_r = rMt.coord(Vector3D::Zero());
Vector3D td_r = rMt.vector(Vector3D::UnitZ());
DoubleBoundingBox3D bb_d = model->boundingBox();
Vector3D bl = bb_d.bottomLeft();
Vector3D tr = bb_d.topRight();
Vector3D c = (bl+tr)/2;
Vector3D x_min_r(c[0], bl[1], c[2]);
Vector3D x_max_r(c[0], tr[1], c[2]);
x_min_r = rMd.coord(x_min_r);
x_max_r = rMd.coord(x_max_r);
double t_x = dot(t_r, td_r);
double bbmin_x = dot(x_min_r, td_r);
double bbmax_x = dot(x_max_r, td_r);
double range = bbmax_x-bbmin_x;
if (similar(range, 0.0))
range = 1.0E-6;
double s = (t_x-bbmin_x)/range;
model->getTextureData().getPositionY()->setValue(s);
}
void Rect3D::updatePosition(const DoubleBoundingBox3D bb, const Transform3D& M)
{
mPoints = vtkPointsPtr::New();
mPoints->InsertPoint(0, M.coord(bb.corner(0,0,0)).begin());
mPoints->InsertPoint(1, M.coord(bb.corner(0,1,0)).begin());
mPoints->InsertPoint(2, M.coord(bb.corner(1,1,0)).begin());
mPoints->InsertPoint(3, M.coord(bb.corner(1,0,0)).begin());
mPolyData->SetPoints(mPoints);
}
//compute the small iteration
VelocityScrew6D<> computeG(Transform3D<> Tin, Transform3D<> Tdes){
//compute positional difference
Vector3D<> dif=Tdes.P()-Tin.P();
//compute rotation difference
Rotation3D<> rot=Tdes.R()*inverse(Tin.R());
//assign values to deltau
VelocityScrew6D<> deltau(dif(0),dif(1),dif(2),(rot(2,1)-rot(1,2))/2,(rot(0,2)-rot(2,0))/2,(rot(1,0)-rot(0,1))/2);
return(deltau);
}
/**Create a transform to a space defined by an origin and two perpendicular unit vectors that
* for the x-y plane.
* The original space is A and the space defined by ijc are B
* The returned transform M_AB converts a point in B to A:
* p_A = M_AB * p_B
*/
Transform3D createTransformIJC(const Vector3D& ivec, const Vector3D& jvec, const Vector3D& center)
{
Transform3D t = Transform3D::Identity();
t.matrix().col(0).head(3) = ivec;
t.matrix().col(1).head(3) = jvec;
t.matrix().col(2).head(3) = cross(ivec, jvec);
t.matrix().col(3).head(3) = center;
return t;
}
Vector3D ViewFollower::findCenter_r_fromShift_s(Vector3D shift_s)
{
Transform3D sMr = mSliceProxy->get_sMr();
Vector3D c_s = sMr.coord(mDataManager->getCenter());
Vector3D newcenter_s = c_s + shift_s;
Vector3D newcenter_r = sMr.inv().coord(newcenter_s);
return newcenter_r;
}
/**Create from affine matrix
*
*/
Frame3D Frame3D::create(const Transform3D& T)
{
Frame3D retval;
Eigen::Matrix3d R = T.matrix().block<3, 3> (0, 0); // extract rotational part
retval.mAngleAxis = Eigen::AngleAxisd(R); // construct angle axis from R
retval.mPos = T.matrix().block<3, 1> (0, 3); // extract translational part
return retval;
}
Vector3D ViewFollower::findVirtualTooltip_s()
{
ToolPtr tool = mSliceProxy->getTool();
Transform3D sMr = mSliceProxy->get_sMr();
Transform3D rMpr = mDataManager->get_rMpr();
Transform3D prMt = tool->get_prMt();
Vector3D pt_s = sMr * rMpr * prMt.coord(Vector3D(0,0,tool->getTooltipOffset()));
pt_s[2] = 0; // project into plane
return pt_s;
}
void Correlator::addData(MocapStream::RigidBodyID id1, Transform3D T1, MocapStream::RigidBodyID id2, Transform3D T2){
//Generate key for the map of correlationStats
std::pair<MocapStream::RigidBodyID, MocapStream::RigidBodyID> key = {id1,id2};
//Check if this hypothesis has been eliminated
if(eliminatedHypotheses.count(key) != 0) return;
if(recordedStates.count(key) == 0){
//Initialise if key missing. (true => calculate cov)
recordedStates[key] = StreamPair();
} else {
// std::cout << "number of samples = " << recordedStates[key].first.size() << std::endl;
// std::cout << "diff1 = " << diff1 << std::endl;
// if( id1 == 1 && id2 == 18) std::cout << "T2 = " << T2 << std::endl;
// std::cout << "diff2 = " << diff2 << std::endl;
// std::cout << "T2 = " << T2 << std::endl;
//If we have no recorded states yet, or the new states differ significantly from the previous, add the new states
if( stateIsNew(T1, recordedStates[key].first)
|| stateIsNew(T2, recordedStates[key].second) )
{
//Check if bad sample (for the particular solver we are using):
Rotation3D R1 = T1.rotation();
UnitQuaternion q1(R1);
Rotation3D R2 = T2.rotation();
UnitQuaternion q2(R2);
if(q1.kW() == 0 || q2.kW() == 0) return;
//Check det
if(std::fabs(arma::det(R1) - 1) > 0.1){
std::cout << __FILE__ << " : Det R1 = " << arma::det(R1) << std::endl;
}
if(std::fabs(arma::det(R2) - 1) > 0.1){
std::cout << __FILE__ << " : Det R2 = " << arma::det(R2) << std::endl;
}
std::cout << "Recording Sample..." << std::endl;
if(recordedStates[key].first.size() >= number_of_samples){
recordedStates[key].first.erase(recordedStates[key].first.begin());
recordedStates[key].first.push_back(T1);
recordedStates[key].second.erase(recordedStates[key].second.begin());
recordedStates[key].second.push_back(T2);
//Now we are ready to compute
computableStreams.insert(key);
} else {
recordedStates[key].first.push_back(T1);
recordedStates[key].second.push_back(T2);
}
}
}
}
Vector3D ToolTipCalibrationCalculator::get_sampledPoint_ref()
{
CoordinateSystem csT = mSpaces->getT(mTool); //from
CoordinateSystem csRef = mSpaces->getT(mRef); //to
Transform3D refMt = mSpaces->get_toMfrom(csT, csRef);
Vector3D P_ref = refMt.coord(mP_t);
return P_ref;
}
/**called when transform is changed
* reset it in the prop.*/
void GeometricRep2D::transformChangedSlot()
{
if (!mSlicer || !mMesh)
return;
Transform3D rMs = mSlicer->get_sMr().inv();
Transform3D dMr = mMesh->get_rMd().inv();
Transform3D dMs = dMr * rMs;
mActor->SetUserMatrix(dMs.inv().getVtkMatrix());
}
void Environment3D::worldToPlanner(Transform3D &state) const
{
MPVec3 pPos = state.getPosition();
this->worldToPlanner(pPos);
MPQuaternion pRot = state.getRotation();
this->worldToPlanner(pRot);
state.setPosition(pPos);
state.setRotation(pRot);
}
void Environment3D::plannerToWorld(Transform3D &state) const
{
MPVec3 wPos = state.getPosition();
this->plannerToWorld(wPos);
MPQuaternion wRot = state.getRotation();
this->plannerToWorld(wRot);
state.setPosition(wPos);
state.setRotation(wRot);
}
bool Render::on_motion_notify_event(GdkEventMotion* event)
{
bool redraw=true;
Vector2f dragp(event->x, event->y);
Vector2f delta = m_downPoint - dragp;
double factor = 0.3;
Vector3d delta3f(-delta.x*factor, delta.y*factor, 0);
get_model()->setMeasuresPoint(Vector2d((10.+event->x)/(get_width()-20),
(10.+get_height()-event->y)/(get_height()-20)));
if (event->state & GDK_BUTTON1_MASK) { // move or rotate
if (event->state & GDK_SHIFT_MASK) { // move object
if (false);//delta3f.x<1 && delta3f.y<1) redraw=false;
else {
Shape *shape;
TreeObject *object;
if (!m_view->get_selected_stl(object, shape))
return true;
if (!object && !shape)
return true;
Transform3D *transf;
if (!shape)
transf = &object->transform3D;
else
transf = &shape->transform3D;
transf->move(delta3f);
m_downPoint = dragp;
//m_view->get_model()->CalcBoundingBoxAndCenter();
}
}
else { // rotate
m_arcBall->dragAccumulate(event->x, event->y, &m_transform);
}
if (redraw) queue_draw();
return true;
}
else {
if (event->state & GDK_BUTTON2_MASK) { // zoom
double factor = 1.0 + 0.01 * (delta.x - delta.y);
m_zoom *= factor;
}
else if (event->state & GDK_BUTTON3_MASK) { // pan
Matrix4f matrix;
memcpy(&matrix.m00, &m_transform.M[0], sizeof(Matrix4f));
Vector3f m_transl = matrix.getTranslation();
m_transl += delta3f;
matrix.setTranslation(m_transl);
memcpy(&m_transform.M[0], &matrix.m00, sizeof(Matrix4f));
}
m_downPoint = dragp;
if (redraw) queue_draw();
return true;
}
return Gtk::DrawingArea::on_motion_notify_event (event);
}
void ImGuiProperty(const Transform3D &transform3D) {
auto position = transform3D.position();
auto center = transform3D.center();
auto scale = transform3D.scale();
auto orientation = transform3D.orientation();
ImGui::InputFloat3("Position", &position[0]);
ImGui::InputFloat3("Center", ¢er[0]);
ImGui::InputFloat("Scale", &scale);
ImGui::InputFloat4("Orientation", &orientation[0]);
}
void ViewWrapper2D::setSlicePlanesProxy(SlicePlanesProxyPtr proxy)
{
mSlicePlanes3DMarker = SlicePlanes3DMarkerIn2DRep::New("uid");
PLANE_TYPE plane = mSliceProxy->getComputer().getPlaneType();
mSlicePlanes3DMarker->setProxy(plane, proxy);
DoubleBoundingBox3D BB_vp = getViewport();
Transform3D vpMs = mView->get_vpMs();
mSlicePlanes3DMarker->getProxy()->setViewportData(plane, mSliceProxy, transform(vpMs.inv(), BB_vp));
mView->addRep(mSlicePlanes3DMarker);
}
void Camera :: transform ( const Transform3D& tr )
{
pos = tr.transformPoint ( pos );
viewDir = tr.transformDir ( viewDir );
upDir = tr.transformDir ( upDir );
sideDir = tr.transformDir ( sideDir );
// now check for right/left handedness
rightHanded = ((upDir ^ viewDir) & sideDir) > EPS;
computeMatrix ();
}
bool similar(const Transform3D& a, const Transform3D& b, double tol)
{
boost::array<double, 16> m = a.flatten();
boost::array<double, 16> n = b.flatten();
for (int j = 0; j < 16; ++j)
{
if (!similar(n[j], m[j], tol))
{
return false;
}
}
return true;
}
void ViewWrapper2D::samplePoint(Vector3D click_vp)
{
if(!this->isAnyplane())
return;
Transform3D sMr = mSliceProxy->get_sMr();
Transform3D vpMs = mView->get_vpMs();
Vector3D p_s = vpMs.inv().coord(click_vp);
Vector3D p_r = sMr.inv().coord(p_s);
emit pointSampled(p_r);
}
double manhattanHeuristic(const Transform3D &start, const Transform3D &goal)
{
MPQuaternion sRot = start.getRotation();
MPQuaternion tRot = goal.getRotation();
MPVec3 difference = MPVec3Subtract(start.getPosition(), goal.getPosition());
float pitchDiff = sRot.x - tRot.x;
float yawDiff = sRot.y - tRot.y;
float rollDiff = sRot.z - tRot.z;
return std::abs(difference.x) + std::abs(difference.y) + std::abs(difference.z) + std::abs(pitchDiff) + std::abs(yawDiff) + std::abs(rollDiff);
}
Transform3D ToolTipCalibrationCalculator::get_sMt_new()
{
Transform3D sMt_old = mTool->getCalibration_sMt();
CoordinateSystem csT = mSpaces->getT(mTool); //to
CoordinateSystem csRef = mSpaces->getT(mRef); //from
Transform3D tMref = mSpaces->get_toMfrom(csRef, csT);
Vector3D delta_t = tMref.vector(this->get_delta_ref());
Transform3D T_delta_t = createTransformTranslate(delta_t);
return sMt_old * T_delta_t;
}
void PickerRep::toolHasChanged()
{
if (!mTool)
return;
Transform3D prMt = mTool->get_prMt();
Transform3D rMpr = mDataManager->get_rMpr();
Transform3D rMt = rMpr * prMt;
Vector3D p_r = rMt.coord(Vector3D(0, 0, mTool->getTooltipOffset()));
mPickedPoint = p_r;
if (mGraphicalPoint)
mGraphicalPoint->setValue(mPickedPoint);
this->setGlyphCenter(mPickedPoint);
}
Box::Box(const glm::vec3 & halfExtent, const Transform3D & transform)
{
m_p = transform.position();
auto x = transform.directionLocalToWorld(glm::vec3(halfExtent.x, 0.0f, 0.0f));
auto y = transform.directionLocalToWorld(glm::vec3(0.0f, halfExtent.y, 0.0f));
auto z = transform.directionLocalToWorld(glm::vec3(0.0f, 0.0f, halfExtent.z));
x = x != glm::vec3(0.0f) ? glm::normalize(x) : x;
y = y != glm::vec3(0.0f) ? glm::normalize(y) : y;
z = z != glm::vec3(0.0f) ? glm::normalize(z) : z;
m_halfExtent = halfExtent;
m_axes = glm::mat3(x, y, z);
}
void Model::PlaceOnPlatform(Shape *shape, TreeObject *object)
{
if (shape)
shape->PlaceOnPlatform();
else if(object) {
Transform3D * transf = &object->transform3D;
transf->move(Vector3f(0, 0, -transf->getTranslation().z()));
for (uint s = 0;s<object->shapes.size(); s++) {
object->shapes[s]->PlaceOnPlatform();
}
}
else return;
ModelChanged();
}
