Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/GeometryToolbox.cpp @ 627:b7fd0471281c am-dev
fix CodeGeneration unit tests
author | Alain Mazy <alain@mazy.be> |
---|---|
date | Wed, 08 May 2019 10:51:41 +0200 |
parents | b70e9be013e4 |
children | 53cc787bd7bc |
rev | line source |
---|---|
0 | 1 /** |
2 * Stone of Orthanc | |
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics | |
4 * Department, University Hospital of Liege, Belgium | |
439 | 5 * Copyright (C) 2017-2019 Osimis S.A., Belgium |
0 | 6 * |
7 * This program is free software: you can redistribute it and/or | |
47 | 8 * modify it under the terms of the GNU Affero General Public License |
9 * as published by the Free Software Foundation, either version 3 of | |
10 * the License, or (at your option) any later version. | |
0 | 11 * |
12 * This program is distributed in the hope that it will be useful, but | |
13 * WITHOUT ANY WARRANTY; without even the implied warranty of | |
47 | 14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
15 * Affero General Public License for more details. | |
16 * | |
17 * You should have received a copy of the GNU Affero General Public License | |
0 | 18 * along with this program. If not, see <http://www.gnu.org/licenses/>. |
19 **/ | |
20 | |
21 | |
22 #include "GeometryToolbox.h" | |
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 | 26 |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
27 #include <cassert> |
0 | 28 |
29 namespace OrthancStone | |
30 { | |
31 namespace GeometryToolbox | |
32 { | |
33 void ProjectPointOntoPlane(Vector& result, | |
34 const Vector& point, | |
35 const Vector& planeNormal, | |
36 const Vector& planeOrigin) | |
37 { | |
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 | 40 { |
41 // Division by zero | |
42 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
43 } | |
44 | |
45 // Make sure the norm of the normal is 1 | |
46 Vector n; | |
47 n = planeNormal / norm; | |
48 | |
49 // Algebraic form of line–plane intersection, where the line passes | |
50 // through "point" along the direction "normal" (thus, l == n) | |
51 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form | |
52 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point; | |
53 } | |
54 | |
55 | |
56 | |
57 bool IsParallelOrOpposite(bool& isOpposite, | |
58 const Vector& u, | |
59 const Vector& v) | |
60 { | |
61 // The dot product of the two vectors gives the cosine of the angle | |
62 // between the vectors | |
63 // https://en.wikipedia.org/wiki/Dot_product | |
64 | |
65 double normU = boost::numeric::ublas::norm_2(u); | |
66 double normV = boost::numeric::ublas::norm_2(v); | |
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 | 70 { |
71 return false; | |
72 } | |
73 | |
74 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); | |
75 | |
76 // The angle must be zero, so the cosine must be almost equal to | |
77 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) | |
78 | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
79 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0)) |
0 | 80 { |
81 isOpposite = false; | |
82 return true; | |
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 | 85 { |
86 isOpposite = true; | |
87 return true; | |
88 } | |
89 else | |
90 { | |
91 return false; | |
92 } | |
93 } | |
94 | |
95 | |
96 bool IsParallel(const Vector& u, | |
97 const Vector& v) | |
98 { | |
99 bool isOpposite; | |
100 return (IsParallelOrOpposite(isOpposite, u, v) && | |
101 !isOpposite); | |
102 } | |
103 | |
104 | |
105 bool IntersectTwoPlanes(Vector& p, | |
106 Vector& direction, | |
107 const Vector& origin1, | |
108 const Vector& normal1, | |
109 const Vector& origin2, | |
110 const Vector& normal2) | |
111 { | |
112 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane | |
113 // Intersect Point" of: | |
114 // http://geomalgorithms.com/a05-_intersect-1.html | |
115 | |
116 // The direction of the line of intersection is orthogonal to the | |
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 | 119 |
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 | 122 { |
123 // The two planes are parallel or coincident | |
124 return false; | |
125 } | |
126 | |
127 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); | |
128 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); | |
129 Vector tmp = d2 * normal1 - d1 * normal2; | |
130 | |
158
a053ca7fa5c6
LinearAlgebra toolbox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
157
diff
changeset
|
131 LinearAlgebra::CrossProduct(p, tmp, direction); |
0 | 132 p /= norm; |
133 | |
134 return true; | |
135 } | |
136 | |
137 | |
138 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out) | |
139 double& y1, | |
140 double& x2, | |
141 double& y2, | |
142 const double ax, // Two points defining the line (in) | |
143 const double ay, | |
144 const double bx, | |
145 const double by, | |
146 const double& xmin, // Coordinates of the rectangle (in) | |
147 const double& ymin, | |
148 const double& xmax, | |
149 const double& ymax) | |
150 { | |
151 // This is Skala algorithm for rectangles, "A new approach to line | |
152 // and line segment clipping in homogeneous coordinates" | |
153 // (2005). This is a direct, non-optimized translation of Algorithm | |
154 // 2 in the paper. | |
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 | 172 |
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 | 190 |
191 // Create the coordinates of the rectangle | |
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 | 197 |
198 // Move to homogoneous coordinates in 2D | |
199 Vector p; | |
200 | |
201 { | |
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 | 206 } |
207 | |
208 uint8_t c = 0; | |
209 | |
210 for (unsigned int k = 0; k < 4; k++) | |
211 { | |
212 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0) | |
213 { | |
214 c |= (1 << k); | |
215 } | |
216 } | |
217 | |
218 assert(c < 16); | |
219 | |
220 uint8_t i = tab1[c]; | |
221 uint8_t j = tab2[c]; | |
222 | |
223 if (i == 255 || j == 255) | |
224 { | |
225 return false; // No intersection | |
226 } | |
227 else | |
228 { | |
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 | 234 |
235 // Go back to non-homogeneous coordinates | |
236 x1 = a[0] / a[2]; | |
237 y1 = a[1] / a[2]; | |
238 x2 = b[0] / b[2]; | |
239 y2 = b[1] / b[2]; | |
240 | |
241 return true; | |
242 } | |
243 } | |
32 | 244 |
245 | |
246 void GetPixelSpacing(double& spacingX, | |
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 | 249 { |
250 Vector v; | |
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 | 253 { |
254 if (v.size() != 2 || | |
255 v[0] <= 0 || | |
256 v[1] <= 0) | |
257 { | |
258 LOG(ERROR) << "Bad value for PixelSpacing tag"; | |
259 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat); | |
260 } | |
261 else | |
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 | 266 } |
267 } | |
268 else | |
269 { | |
270 // The "PixelSpacing" is of type 1C: It could be absent, use | |
271 // default value in such a case | |
272 spacingX = 1; | |
273 spacingY = 1; | |
274 } | |
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 | 391 |
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 | 477 } |
478 } |