Mercurial > hg > orthanc-stone

annotate Framework/Toolbox/GeometryToolbox.cpp @ 700:059e1fd05fd6 refactor-viewport-controller

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Introduced the ViewportController that sits between the application and the Scene2D to handle the trackers and measuring tools. This is a work in progress. The Scene2D is no longer an observable. Message sending is managed by the ViewportController. Move some refs to shared and weak to prevent lifetime issues.
author Benjamin Golinvaux <bgo@osimis.io>
date Sun, 19 May 2019 16:26:17 +0200
parents b70e9be013e4
children 53cc787bd7bc
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
1 /**
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
2 * Stone of Orthanc
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
4 * Department, University Hospital of Liege, Belgium
439
b70e9be013e4 preparing for 2019
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 363
diff changeset
5 * Copyright (C) 2017-2019 Osimis S.A., Belgium
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
6 *
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
7 * This program is free software: you can redistribute it and/or
47
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
8 * modify it under the terms of the GNU Affero General Public License
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
9 * as published by the Free Software Foundation, either version 3 of
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
10 * the License, or (at your option) any later version.
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
11 *
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
12 * This program is distributed in the hope that it will be useful, but
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
47
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
15 * Affero General Public License for more details.
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
16 *
28956ed68280 agpl license
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 40
diff changeset
17 * You should have received a copy of the GNU Affero General Public License
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
18 * along with this program. If not, see <http://www.gnu.org/licenses/>.
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
19 **/
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
20
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
21
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
22 #include "GeometryToolbox.h"
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
23
212
5412adf19980 resort to OrthancFramework
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 201
diff changeset
24 #include <Core/Logging.h>
5412adf19980 resort to OrthancFramework
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 201
diff changeset
25 #include <Core/OrthancException.h>
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
26
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
27 #include <cassert>
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
28
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
29 namespace OrthancStone
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
30 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
31 namespace GeometryToolbox
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
32 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
33 void ProjectPointOntoPlane(Vector& result,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
34 const Vector& point,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
35 const Vector& planeNormal,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
36 const Vector& planeOrigin)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
37 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
38 double norm = boost::numeric::ublas::norm_2(planeNormal);
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
39 if (LinearAlgebra::IsCloseToZero(norm))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
40 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
41 // Division by zero
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
42 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
43 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
44
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
45 // Make sure the norm of the normal is 1
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
46 Vector n;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
47 n = planeNormal / norm;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
48
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
49 // Algebraic form of line–plane intersection, where the line passes
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
50 // through "point" along the direction "normal" (thus, l == n)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
51 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
52 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
53 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
54
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
55
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
56
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
57 bool IsParallelOrOpposite(bool& isOpposite,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
58 const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
59 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
60 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
61 // The dot product of the two vectors gives the cosine of the angle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
62 // between the vectors
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
63 // https://en.wikipedia.org/wiki/Dot_product
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
64
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
65 double normU = boost::numeric::ublas::norm_2(u);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
66 double normV = boost::numeric::ublas::norm_2(v);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
67
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
68 if (LinearAlgebra::IsCloseToZero(normU) ||
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
69 LinearAlgebra::IsCloseToZero(normV))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
70 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
71 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
72 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
73
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
74 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
75
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
76 // The angle must be zero, so the cosine must be almost equal to
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
77 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
78
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
79 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
80 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
81 isOpposite = false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
82 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
83 }
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
84 else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
85 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
86 isOpposite = true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
87 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
88 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
89 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
90 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
91 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
92 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
93 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
94
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
95
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
96 bool IsParallel(const Vector& u,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
97 const Vector& v)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
98 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
99 bool isOpposite;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
100 return (IsParallelOrOpposite(isOpposite, u, v) &&
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
101 !isOpposite);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
102 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
103
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
104
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
105 bool IntersectTwoPlanes(Vector& p,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
106 Vector& direction,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
107 const Vector& origin1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
108 const Vector& normal1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
109 const Vector& origin2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
110 const Vector& normal2)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
111 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
112 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
113 // Intersect Point" of:
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
114 // http://geomalgorithms.com/a05-_intersect-1.html
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
115
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
116 // The direction of the line of intersection is orthogonal to the
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
117 // normal of both planes
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
118 LinearAlgebra::CrossProduct(direction, normal1, normal2);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
119
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
120 double norm = boost::numeric::ublas::norm_2(direction);
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
121 if (LinearAlgebra::IsCloseToZero(norm))
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
122 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
123 // The two planes are parallel or coincident
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
124 return false;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
125 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
126
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
127 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
128 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
129 Vector tmp = d2 * normal1 - d1 * normal2;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
130
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
131 LinearAlgebra::CrossProduct(p, tmp, direction);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
132 p /= norm;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
133
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
134 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
135 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
136
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
137
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
138 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
139 double& y1,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
140 double& x2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
141 double& y2,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
142 const double ax, // Two points defining the line (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
143 const double ay,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
144 const double bx,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
145 const double by,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
146 const double& xmin, // Coordinates of the rectangle (in)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
147 const double& ymin,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
148 const double& xmax,
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
149 const double& ymax)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
150 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
151 // This is Skala algorithm for rectangles, "A new approach to line
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
152 // and line segment clipping in homogeneous coordinates"
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
153 // (2005). This is a direct, non-optimized translation of Algorithm
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
154 // 2 in the paper.
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
155
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
156 static const uint8_t tab1[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
157 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
158 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
159 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
160 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
161 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
162 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
163 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
164 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
165 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
166 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
167 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
168 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
169 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
170 0,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
171 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
172
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
173
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
174 static const uint8_t tab2[16] = { 255 /* none */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
175 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
176 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
177 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
178 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
179 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
180 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
181 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
182 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
183 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
184 255 /* na */,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
185 2,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
186 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
187 1,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
188 3,
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
189 255 /* none */ };
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
190
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
191 // Create the coordinates of the rectangle
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
192 Vector x[4];
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
193 LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
194 LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
195 LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
196 LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
197
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
198 // Move to homogoneous coordinates in 2D
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
199 Vector p;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
200
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
201 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
202 Vector a, b;
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
203 LinearAlgebra::AssignVector(a, ax, ay, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
204 LinearAlgebra::AssignVector(b, bx, by, 1.0);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
205 LinearAlgebra::CrossProduct(p, a, b);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
206 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
207
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
208 uint8_t c = 0;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
209
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
210 for (unsigned int k = 0; k < 4; k++)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
211 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
212 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
213 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
214 c |= (1 << k);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
215 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
216 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
217
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
218 assert(c < 16);
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
219
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
220 uint8_t i = tab1[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
221 uint8_t j = tab2[c];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
222
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
223 if (i == 255 || j == 255)
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
224 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
225 return false; // No intersection
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
226 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
227 else
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
228 {
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
229 Vector a, b, e;
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
230 LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
231 LinearAlgebra::CrossProduct(a, p, e);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
232 LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]);
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
233 LinearAlgebra::CrossProduct(b, p, e);
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
234
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
235 // Go back to non-homogeneous coordinates
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
236 x1 = a[0] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
237 y1 = a[1] / a[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
238 x2 = b[0] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
239 y2 = b[1] / b[2];
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
240
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
241 return true;
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
242 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
243 }
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
244
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
245
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
246 void GetPixelSpacing(double& spacingX,
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
247 double& spacingY,
118
a4d0b6c82b29 using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 113
diff changeset
248 const Orthanc::DicomMap& dicom)
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
249 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
250 Vector v;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
251
158
a053ca7fa5c6 LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 157
diff changeset
252 if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING))
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
253 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
254 if (v.size() != 2 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
255 v[0] <= 0 ||
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
256 v[1] <= 0)
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
257 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
258 LOG(ERROR) << "Bad value for PixelSpacing tag";
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
259 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat);
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
260 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
261 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
262 {
363
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
263 // WARNING: X/Y are swapped (Y comes first)
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
264 spacingX = v[1];
54ae0577f5bb fix anisotropic pixel spacing
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 212
diff changeset
265 spacingY = v[0];
32
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
266 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
267 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
268 else
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
269 {
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
270 // The "PixelSpacing" is of type 1C: It could be absent, use
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
271 // default value in such a case
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
272 spacingX = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
273 spacingY = 1;
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
274 }
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 16
diff changeset
275 }
140
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
276
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
277
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
278 Matrix CreateRotationMatrixAlongX(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
279 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
280 // Rotate along X axis (R_x)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
281 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
282 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
283 r(0,0) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
284 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
285 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
286 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
287 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
288 r(1,2) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
289 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
290 r(2,1) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
291 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
292 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
293 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
294
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
295
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
296 Matrix CreateRotationMatrixAlongY(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
297 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
298 // Rotate along Y axis (R_y)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
299 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
300 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
301 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
302 r(0,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
303 r(0,2) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
304 r(1,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
305 r(1,1) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
306 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
307 r(2,0) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
308 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
309 r(2,2) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
310 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
311 }
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
312
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
313
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
314 Matrix CreateRotationMatrixAlongZ(double a)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
315 {
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
316 // Rotate along Z axis (R_z)
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
317 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
318 Matrix r(3, 3);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
319 r(0,0) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
320 r(0,1) = -sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
321 r(0,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
322 r(1,0) = sin(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
323 r(1,1) = cos(a);
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
324 r(1,2) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
325 r(2,0) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
326 r(2,1) = 0;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
327 r(2,2) = 1;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
328 return r;
2115530d3703 OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 135
diff changeset
329 }
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
330
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
331
165
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
332 Matrix CreateTranslationMatrix(double dx,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
333 double dy,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
334 double dz)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
335 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
336 Matrix m = LinearAlgebra::IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
337 m(0,3) = dx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
338 m(1,3) = dy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
339 m(2,3) = dz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
340 return m;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
341 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
342
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
343
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
344 Matrix CreateScalingMatrix(double sx,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
345 double sy,
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
346 double sz)
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
347 {
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
348 Matrix m = LinearAlgebra::IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
349 m(0,0) = sx;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
350 m(1,1) = sy;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
351 m(2,2) = sz;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
352 return m;
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
353 }
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
354
8d50e6be565d LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 158
diff changeset
355
151
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
356 bool IntersectPlaneAndSegment(Vector& p,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
357 const Vector& normal,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
358 double d,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
359 const Vector& edgeFrom,
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
360 const Vector& edgeTo)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
361 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
362 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
363
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
364 // Check for parallel line and plane
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
365 Vector direction = edgeTo - edgeFrom;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
366 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
367
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
368 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
369 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
370 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
371 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
372 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
373 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
374 // Compute intersection
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
375 double t = -(normal[0] * edgeFrom[0] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
376 normal[1] * edgeFrom[1] +
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
377 normal[2] * edgeFrom[2] + d) / denominator;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
378
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
379 if (t >= 0 && t <= 1)
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
380 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
381 // The intersection lies inside edge segment
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
382 p = edgeFrom + t * direction;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
383 return true;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
384 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
385 else
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
386 {
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
387 return false;
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
388 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
389 }
c5044bbfc303 CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 150
diff changeset
390 }
156
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
391
441cfe8e7440 fill matrix
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 151
diff changeset
392
157
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
393 bool IntersectPlaneAndLine(Vector& p,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
394 const Vector& normal,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
395 double d,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
396 const Vector& origin,
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
397 const Vector& direction)
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
398 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
399 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
400
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
401 // Check for parallel line and plane
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
402 double denominator = boost::numeric::ublas::inner_prod(direction, normal);
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
403
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
404 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
405 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
406 return false;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
407 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
408 else
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
409 {
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
410 // Compute intersection
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
411 double t = -(normal[0] * origin[0] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
412 normal[1] * origin[1] +
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
413 normal[2] * origin[2] + d) / denominator;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
414
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
415 p = origin + t * direction;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
416 return true;
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
417 }
2309e8d86efe IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 156
diff changeset
418 }
189
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
419
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
420
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
421 void AlignVectorsWithRotation(Matrix& r,
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
422 const Vector& a,
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
423 const Vector& b)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
424 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
425 // This is Rodrigues' rotation formula:
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
426 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
427
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
428 // Check also result A4.6 from "Multiple View Geometry in Computer
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
429 // Vision - 2nd edition" (p. 584)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
430
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
431 if (a.size() != 3 ||
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
432 b.size() != 3)
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
433 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
434 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
435 }
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
436
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
437 double aNorm = boost::numeric::ublas::norm_2(a);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
438 double bNorm = boost::numeric::ublas::norm_2(b);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
439
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
440 if (LinearAlgebra::IsCloseToZero(aNorm) ||
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
441 LinearAlgebra::IsCloseToZero(bNorm))
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
442 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
443 LOG(ERROR) << "Vector with zero norm";
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
444 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
445 }
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
446
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
447 Vector aUnit, bUnit;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
448 aUnit = a / aNorm;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
449 bUnit = b / bNorm;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
450
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
451 Vector v;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
452 LinearAlgebra::CrossProduct(v, aUnit, bUnit);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
453
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
454 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
455
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
456 if (LinearAlgebra::IsCloseToZero(1 + cosine))
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
457 {
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
458 // "a == -b": TODO
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
459 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
460 }
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
461
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
462 Matrix k;
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
463 LinearAlgebra::CreateSkewSymmetric(k, v);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
464
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
465 #if 0
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
466 double sine = boost::numeric::ublas::norm_2(v);
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
467
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
468 r = (boost::numeric::ublas::identity_matrix<double>(3) +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
469 sine * k +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
470 (1 - cosine) * boost::numeric::ublas::prod(k, k));
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
471 #else
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
472 r = (boost::numeric::ublas::identity_matrix<double>(3) +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
473 k +
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
474 boost::numeric::ublas::prod(k, k) / (1 + cosine));
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
475 #endif
964118e7e6de unit test for AlignVectorsWithRotation
Sebastien Jodogne <s.jodogne@gmail.com>
parents: 165
diff changeset
476 }
0
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
477 }
351ab0da0150 initial commit
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
diff changeset
478 }