Fraxinus  17.12-rc4
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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 
33 #include "cxNavigationAlgorithms.h"
34 #include "cxLogger.h"
35 #include "cxBoundingBox3D.h"
36 
37 namespace cx
38 {
39 
41 {
42  e_line = e_line.normal();
43 
44  // project focus onto line, then use pythoagoras:
45  // dist^2 = |focus-pff|^2 + q^2
46  // where we want to find q, dist from p_ff to camera along line.
47  Vector3D p_ff = p_line + e_line * dot(focus-p_line, e_line);
48  double q_sq = distance*distance - dot(focus-p_ff, focus-p_ff);
49  if (q_sq<0)
50  {
51  // too close: revert to use point on p_ff - focus line.
52  Vector3D p_c = focus - (p_ff-focus).normal() * distance;
53  return p_c;
54  }
55  double q = sqrt(q_sq);
56 
57  Vector3D p_c = p_ff - q * e_line;
58  Vector3D p2_c = p_ff + q * e_line;
59 
60  if (similar((p_c-focus).length(), distance))
61  return p_c;
62  else if (similar((p2_c-focus).length(), distance))
63  {
64  return p2_c;
65  }
66  else
67  {
68  CX_LOG_CHANNEL_DEBUG("CA") << "find point failed - error in distance!!!!!!!!!!!!!!!!!";
69  return p_c;
70  }
71 }
72 
74 {
75  if (cross(vup, vpn).length() < 0.01)
76  {
77 // CX_LOG_CHANNEL_DEBUG("CA") << "warning , cross(vup_r, vpn_r)=" << cross(vup, vpn).length();
78  vup = vup_fallback;
79  }
80  else
81  {
82  Vector3D left = cross(vup, vpn).normal();
83  vup = cross(vpn, left).normal();
84  }
85 
86  return vup;
87 }
88 
90 {
91  if (similar(angle, 0.0))
92  return camera;
93 
94  // clean up input in order to have two orthogonal unit vectors
95  Vector3D i = (camera-focus).normal();
96  Vector3D j = vup;
97  Vector3D k = cross(i, j).normal();
98  j = cross(k, i).normal();
99 
100  // define a new space spanned by (x=vpn,y=vup), centered in focus
101  // rotate the angle in this space
102  // input space is A, focus-centered space is B
103  Transform3D aMb = createTransformIJC(i, j, focus);
104  Transform3D bMa = aMb.inv();
106  camera = (aMb*R*bMa).coord(camera);
107 
108  return camera;
109 }
110 
111 Vector3D NavigationAlgorithms::findCameraPosByZoomingToROI(double viewAngle_vertical, double viewAngle_horizontal, Vector3D focus, Vector3D vup, Vector3D vpn, const DoubleBoundingBox3D& bb)
112 {
113  Vector3D n_vertical = cross(vpn, vup).normal();
114  double dist_v = NavigationAlgorithms::findMaxCameraDistance(n_vertical, viewAngle_vertical, focus, vpn, bb);
115  Vector3D n_horizontal = vup;
116  double dist_h = NavigationAlgorithms::findMaxCameraDistance(n_horizontal, viewAngle_horizontal, focus, vpn, bb);
117  double dist = std::max(dist_v, dist_h);
118 
119  Vector3D camera_r_t = focus + vpn*dist;
120  return camera_r_t;
121 }
122 
124 {
125  std::vector<double> dists;
126  for (unsigned x=0; x<2; ++x)
127  for (unsigned y=0; y<2; ++y)
128  for (unsigned z=0; z<2; ++z)
129  {
130  Vector3D p = bb.corner(x,y,z);
131  double d = NavigationAlgorithms::findCameraDistanceKeepPointInViewOneAxis(n, viewAngle, focus, vpn, p);
132  dists.push_back(d);
133  }
134 
135  double maxDist = *std::max_element(dists.begin(), dists.end());
136  return maxDist;
137 }
138 
140 {
141  // project all items into plane n
142  Vector3D focus_p = focus - dot(focus, n)*n;
143  Vector3D p_p = p - dot(p, n)*n;
144  Vector3D vpn_p = (vpn - dot(vpn, n)*n).normal();
145 
146  // find distance in projection plane n
147  double d_p = NavigationAlgorithms::findCameraDistanceKeepPointInView(viewAngle, focus_p, vpn_p, p_p);
148 
149  // recalculate non-projected distance.
150  double cos_plane_angle = dot(vpn, vpn_p); // cosine(angle) between plane n and original vpn direction
151  d_p = d_p / cos_plane_angle;
152  return d_p;
153 }
154 
156 {
157  Vector3D pp = focus + vpn*dot(p-focus, vpn); // p projected onto the camera line defined by focus and vpn.
158  double beta = (p-pp).length() / tan(viewAngle/2); // distance from pp to camera
159  beta = fabs(beta);
160  double dist = beta + dot(pp-focus, vpn); // total distance from focus to camera
161  return dist;
162 }
163 
164 
165 }//namespace cx
166 
167 
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:62
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:128
double dot(const Vector3D &a, const Vector3D &b)
compute inner product (or dot product) of a and b.
Definition: cxVector3D.cpp:67
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:63
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.