CustusX  2023.01.05-dev+develop.0da12
An IGT application
cxNavigationAlgorithms.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
11 
12 #include "cxNavigationAlgorithms.h"
13 #include "cxLogger.h"
14 #include "cxBoundingBox3D.h"
15 
16 namespace cx
17 {
18 
20 {
21  e_line = e_line.normal();
22 
23  // project focus onto line, then use pythoagoras:
24  // dist^2 = |focus-pff|^2 + q^2
25  // where we want to find q, dist from p_ff to camera along line.
26  Vector3D p_ff = p_line + e_line * dot(focus-p_line, e_line);
27  double q_sq = distance*distance - dot(focus-p_ff, focus-p_ff);
28  if (q_sq<0)
29  {
30  // too close: revert to use point on p_ff - focus line.
31  Vector3D p_c = focus - (p_ff-focus).normal() * distance;
32  return p_c;
33  }
34  double q = sqrt(q_sq);
35 
36  Vector3D p_c = p_ff - q * e_line;
37  Vector3D p2_c = p_ff + q * e_line;
38 
39  if (similar((p_c-focus).length(), distance))
40  return p_c;
41  else if (similar((p2_c-focus).length(), distance))
42  {
43  return p2_c;
44  }
45  else
46  {
47  CX_LOG_CHANNEL_DEBUG("CA") << "find point failed - error in distance!!!!!!!!!!!!!!!!!";
48  return p_c;
49  }
50 }
51 
53 {
54  if (cross(vup, vpn).length() < 0.01)
55  {
56 // CX_LOG_CHANNEL_DEBUG("CA") << "warning , cross(vup_r, vpn_r)=" << cross(vup, vpn).length();
57  vup = vup_fallback;
58  }
59  else
60  {
61  Vector3D left = cross(vup, vpn).normal();
62  vup = cross(vpn, left).normal();
63  }
64 
65  return vup;
66 }
67 
69 {
70  if (similar(angle, 0.0))
71  return camera;
72 
73  // clean up input in order to have two orthogonal unit vectors
74  Vector3D i = (camera-focus).normal();
75  Vector3D j = vup;
76  Vector3D k = cross(i, j).normal();
77  j = cross(k, i).normal();
78 
79  // define a new space spanned by (x=vpn,y=vup), centered in focus
80  // rotate the angle in this space
81  // input space is A, focus-centered space is B
82  Transform3D aMb = createTransformIJC(i, j, focus);
83  Transform3D bMa = aMb.inv();
85  camera = (aMb*R*bMa).coord(camera);
86 
87  return camera;
88 }
89 
90 Vector3D NavigationAlgorithms::findCameraPosByZoomingToROI(double viewAngle_vertical, double viewAngle_horizontal, Vector3D focus, Vector3D vup, Vector3D vpn, const DoubleBoundingBox3D& bb)
91 {
92  Vector3D n_vertical = cross(vpn, vup).normal();
93  double dist_v = NavigationAlgorithms::findMaxCameraDistance(n_vertical, viewAngle_vertical, focus, vpn, bb);
94  Vector3D n_horizontal = vup;
95  double dist_h = NavigationAlgorithms::findMaxCameraDistance(n_horizontal, viewAngle_horizontal, focus, vpn, bb);
96  double dist = std::max(dist_v, dist_h);
97 
98  Vector3D camera_r_t = focus + vpn*dist;
99  return camera_r_t;
100 }
101 
103 {
104  std::vector<double> dists;
105  for (unsigned x=0; x<2; ++x)
106  for (unsigned y=0; y<2; ++y)
107  for (unsigned z=0; z<2; ++z)
108  {
109  Vector3D p = bb.corner(x,y,z);
110  double d = NavigationAlgorithms::findCameraDistanceKeepPointInViewOneAxis(n, viewAngle, focus, vpn, p);
111  dists.push_back(d);
112  }
113 
114  double maxDist = *std::max_element(dists.begin(), dists.end());
115  return maxDist;
116 }
117 
119 {
120  // project all items into plane n
121  Vector3D focus_p = focus - dot(focus, n)*n;
122  Vector3D p_p = p - dot(p, n)*n;
123  Vector3D vpn_p = (vpn - dot(vpn, n)*n).normal();
124 
125  // find distance in projection plane n
126  double d_p = NavigationAlgorithms::findCameraDistanceKeepPointInView(viewAngle, focus_p, vpn_p, p_p);
127 
128  // recalculate non-projected distance.
129  double cos_plane_angle = dot(vpn, vpn_p); // cosine(angle) between plane n and original vpn direction
130  d_p = d_p / cos_plane_angle;
131  return d_p;
132 }
133 
135 {
136  Vector3D pp = focus + vpn*dot(p-focus, vpn); // p projected onto the camera line defined by focus and vpn.
137  double beta = (p-pp).length() / tan(viewAngle/2); // distance from pp to camera
138  beta = fabs(beta);
139  double dist = beta + dot(pp-focus, vpn); // total distance from focus to camera
140  return dist;
141 }
142 
143 
144 }//namespace cx
145 
146 
Vector3D corner(int x, int y, int z) const
PlainObject normal() const
static Vector3D findCameraPosOnLineFixedDistanceFromFocus(Vector3D p_line, Vector3D e_line, double distance, Vector3D focus)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
Transform3D createTransformIJC(const Vector3D &ivec, const Vector3D &jvec, const Vector3D &center)
static double findCameraDistanceKeepPointInView(double viewAngle, Vector3D focus, Vector3D vpn, Vector3D p)
#define CX_LOG_CHANNEL_DEBUG(channel)
Definition: cxLogger.h:107
double dot(const Vector3D &a, const Vector3D &b)
compute inner product (or dot product) of a and b.
Definition: cxVector3D.cpp:46
static double findCameraDistanceKeepPointInViewOneAxis(Vector3D n, double viewAngle, Vector3D focus, Vector3D vpn, Vector3D p)
Representation of a floating-point bounding box in 3D. The data are stored as {xmin,xmax,ymin,ymax,zmin,zmax}, in order to simplify communication with vtk.
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
RealScalar length() const
Transform3D createTransformRotateZ(const double angle)
static double findMaxCameraDistance(Vector3D n, double viewAngle, Vector3D focus, Vector3D vpn, const DoubleBoundingBox3D &bb)
static Vector3D elevateCamera(double angle, Vector3D camera, Vector3D focus, Vector3D vup)
static Vector3D orthogonalize_vup(Vector3D vup, Vector3D vpn, Vector3D vup_fallback)
static Vector3D findCameraPosByZoomingToROI(double viewAngle_vertical, double viewAngle_horizontal, Vector3D focus, Vector3D vup, Vector3D vpn, const DoubleBoundingBox3D &bb)
Namespace for all CustusX production code.