Mercurial > hg > orthanc-stone
annotate Framework/Toolbox/GeometryToolbox.cpp @ 157:2309e8d86efe wasm
IntersectPlaneAndLine
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Fri, 09 Feb 2018 16:00:29 +0100 |
parents | 441cfe8e7440 |
children | a053ca7fa5c6 |
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 | |
135
e2fe9352f240
upgrade to year 2018
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
118
diff
changeset
|
5 * Copyright (C) 2017-2018 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 | |
113
2eca030792aa
using the Orthanc Framework
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
47
diff
changeset
|
24 #include <Core/Logging.h> |
2eca030792aa
using the Orthanc Framework
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
47
diff
changeset
|
25 #include <Core/OrthancException.h> |
2eca030792aa
using the Orthanc Framework
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
47
diff
changeset
|
26 #include <Core/Toolbox.h> |
0 | 27 |
28 #include <stdio.h> | |
29 #include <boost/lexical_cast.hpp> | |
30 | |
31 namespace OrthancStone | |
32 { | |
33 namespace GeometryToolbox | |
34 { | |
35 void Print(const Vector& v) | |
36 { | |
37 for (size_t i = 0; i < v.size(); i++) | |
38 { | |
157
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
39 printf("%g\n", v[i]); |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
40 //printf("%8.2f\n", v[i]); |
0 | 41 } |
42 printf("\n"); | |
43 } | |
44 | |
45 | |
150 | 46 void Print(const Matrix& m) |
47 { | |
48 for (size_t i = 0; i < m.size1(); i++) | |
49 { | |
50 for (size_t j = 0; j < m.size2(); j++) | |
51 { | |
157
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
52 printf("%g ", m(i,j)); |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
53 //printf("%8.2f ", m(i,j)); |
150 | 54 } |
55 printf("\n"); | |
56 } | |
157
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
57 printf("\n"); |
150 | 58 } |
59 | |
60 | |
0 | 61 bool ParseVector(Vector& target, |
62 const std::string& value) | |
63 { | |
64 std::vector<std::string> items; | |
65 Orthanc::Toolbox::TokenizeString(items, value, '\\'); | |
66 | |
67 target.resize(items.size()); | |
68 | |
69 for (size_t i = 0; i < items.size(); i++) | |
70 { | |
71 try | |
72 { | |
73 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); | |
74 } | |
75 catch (boost::bad_lexical_cast&) | |
76 { | |
77 target.clear(); | |
78 return false; | |
79 } | |
80 } | |
81 | |
82 return true; | |
83 } | |
84 | |
85 | |
32 | 86 bool ParseVector(Vector& target, |
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
87 const Orthanc::DicomMap& dataset, |
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
88 const Orthanc::DicomTag& tag) |
32 | 89 { |
90 std::string value; | |
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
91 return (dataset.CopyToString(value, tag, false) && |
32 | 92 ParseVector(target, value)); |
93 } | |
94 | |
95 | |
0 | 96 void AssignVector(Vector& v, |
97 double v1, | |
98 double v2) | |
99 { | |
100 v.resize(2); | |
101 v[0] = v1; | |
102 v[1] = v2; | |
103 } | |
104 | |
105 | |
106 void AssignVector(Vector& v, | |
107 double v1, | |
108 double v2, | |
109 double v3) | |
110 { | |
111 v.resize(3); | |
112 v[0] = v1; | |
113 v[1] = v2; | |
114 v[2] = v3; | |
115 } | |
116 | |
117 | |
118 bool IsNear(double x, | |
119 double y) | |
120 { | |
121 // As most input is read as single-precision numbers, we take the | |
122 // epsilon machine for float32 into consideration to compare numbers | |
123 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon()); | |
124 } | |
125 | |
126 | |
127 void NormalizeVector(Vector& u) | |
128 { | |
129 double norm = boost::numeric::ublas::norm_2(u); | |
130 if (!IsCloseToZero(norm)) | |
131 { | |
132 u = u / norm; | |
133 } | |
134 } | |
135 | |
136 | |
137 void CrossProduct(Vector& result, | |
138 const Vector& u, | |
139 const Vector& v) | |
140 { | |
141 if (u.size() != 3 || | |
142 v.size() != 3) | |
143 { | |
144 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
145 } | |
146 | |
147 result.resize(3); | |
148 | |
149 result[0] = u[1] * v[2] - u[2] * v[1]; | |
150 result[1] = u[2] * v[0] - u[0] * v[2]; | |
151 result[2] = u[0] * v[1] - u[1] * v[0]; | |
152 } | |
153 | |
154 | |
155 void ProjectPointOntoPlane(Vector& result, | |
156 const Vector& point, | |
157 const Vector& planeNormal, | |
158 const Vector& planeOrigin) | |
159 { | |
160 double norm = boost::numeric::ublas::norm_2(planeNormal); | |
161 if (IsCloseToZero(norm)) | |
162 { | |
163 // Division by zero | |
164 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
165 } | |
166 | |
167 // Make sure the norm of the normal is 1 | |
168 Vector n; | |
169 n = planeNormal / norm; | |
170 | |
171 // Algebraic form of line–plane intersection, where the line passes | |
172 // through "point" along the direction "normal" (thus, l == n) | |
173 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form | |
174 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point; | |
175 } | |
176 | |
177 | |
178 | |
179 bool IsParallelOrOpposite(bool& isOpposite, | |
180 const Vector& u, | |
181 const Vector& v) | |
182 { | |
183 // The dot product of the two vectors gives the cosine of the angle | |
184 // between the vectors | |
185 // https://en.wikipedia.org/wiki/Dot_product | |
186 | |
187 double normU = boost::numeric::ublas::norm_2(u); | |
188 double normV = boost::numeric::ublas::norm_2(v); | |
189 | |
190 if (IsCloseToZero(normU) || | |
191 IsCloseToZero(normV)) | |
192 { | |
193 return false; | |
194 } | |
195 | |
196 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); | |
197 | |
198 // The angle must be zero, so the cosine must be almost equal to | |
199 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) | |
200 | |
201 if (IsCloseToZero(cosAngle - 1.0)) | |
202 { | |
203 isOpposite = false; | |
204 return true; | |
205 } | |
206 else if (IsCloseToZero(fabs(cosAngle) - 1.0)) | |
207 { | |
208 isOpposite = true; | |
209 return true; | |
210 } | |
211 else | |
212 { | |
213 return false; | |
214 } | |
215 } | |
216 | |
217 | |
218 bool IsParallel(const Vector& u, | |
219 const Vector& v) | |
220 { | |
221 bool isOpposite; | |
222 return (IsParallelOrOpposite(isOpposite, u, v) && | |
223 !isOpposite); | |
224 } | |
225 | |
226 | |
227 bool IntersectTwoPlanes(Vector& p, | |
228 Vector& direction, | |
229 const Vector& origin1, | |
230 const Vector& normal1, | |
231 const Vector& origin2, | |
232 const Vector& normal2) | |
233 { | |
234 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane | |
235 // Intersect Point" of: | |
236 // http://geomalgorithms.com/a05-_intersect-1.html | |
237 | |
238 // The direction of the line of intersection is orthogonal to the | |
239 // normal of both planes | |
240 CrossProduct(direction, normal1, normal2); | |
241 | |
242 double norm = boost::numeric::ublas::norm_2(direction); | |
243 if (IsCloseToZero(norm)) | |
244 { | |
245 // The two planes are parallel or coincident | |
246 return false; | |
247 } | |
248 | |
249 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); | |
250 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); | |
251 Vector tmp = d2 * normal1 - d1 * normal2; | |
252 | |
253 CrossProduct(p, tmp, direction); | |
254 p /= norm; | |
255 | |
256 return true; | |
257 } | |
258 | |
259 | |
260 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out) | |
261 double& y1, | |
262 double& x2, | |
263 double& y2, | |
264 const double ax, // Two points defining the line (in) | |
265 const double ay, | |
266 const double bx, | |
267 const double by, | |
268 const double& xmin, // Coordinates of the rectangle (in) | |
269 const double& ymin, | |
270 const double& xmax, | |
271 const double& ymax) | |
272 { | |
273 // This is Skala algorithm for rectangles, "A new approach to line | |
274 // and line segment clipping in homogeneous coordinates" | |
275 // (2005). This is a direct, non-optimized translation of Algorithm | |
276 // 2 in the paper. | |
277 | |
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
278 static const uint8_t tab1[16] = { 255 /* none */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
279 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
280 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
281 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
282 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
283 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
284 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
285 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
286 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
287 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
288 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
289 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
290 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
291 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
292 0, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
293 255 /* none */ }; |
0 | 294 |
295 | |
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
296 static const uint8_t tab2[16] = { 255 /* none */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
297 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
298 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
299 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
300 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
301 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
302 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
303 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
304 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
305 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
306 255 /* na */, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
307 2, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
308 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
309 1, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
310 3, |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
311 255 /* none */ }; |
0 | 312 |
313 // Create the coordinates of the rectangle | |
314 Vector x[4]; | |
315 AssignVector(x[0], xmin, ymin, 1.0); | |
316 AssignVector(x[1], xmax, ymin, 1.0); | |
317 AssignVector(x[2], xmax, ymax, 1.0); | |
318 AssignVector(x[3], xmin, ymax, 1.0); | |
319 | |
320 // Move to homogoneous coordinates in 2D | |
321 Vector p; | |
322 | |
323 { | |
324 Vector a, b; | |
325 AssignVector(a, ax, ay, 1.0); | |
326 AssignVector(b, bx, by, 1.0); | |
327 CrossProduct(p, a, b); | |
328 } | |
329 | |
330 uint8_t c = 0; | |
331 | |
332 for (unsigned int k = 0; k < 4; k++) | |
333 { | |
334 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0) | |
335 { | |
336 c |= (1 << k); | |
337 } | |
338 } | |
339 | |
340 assert(c < 16); | |
341 | |
342 uint8_t i = tab1[c]; | |
343 uint8_t j = tab2[c]; | |
344 | |
345 if (i == 255 || j == 255) | |
346 { | |
347 return false; // No intersection | |
348 } | |
349 else | |
350 { | |
351 Vector a, b, e; | |
352 CrossProduct(e, x[i], x[(i + 1) % 4]); | |
353 CrossProduct(a, p, e); | |
354 CrossProduct(e, x[j], x[(j + 1) % 4]); | |
355 CrossProduct(b, p, e); | |
356 | |
357 // Go back to non-homogeneous coordinates | |
358 x1 = a[0] / a[2]; | |
359 y1 = a[1] / a[2]; | |
360 x2 = b[0] / b[2]; | |
361 y2 = b[1] / b[2]; | |
362 | |
363 return true; | |
364 } | |
365 } | |
32 | 366 |
367 | |
368 void GetPixelSpacing(double& spacingX, | |
369 double& spacingY, | |
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
370 const Orthanc::DicomMap& dicom) |
32 | 371 { |
372 Vector v; | |
373 | |
118
a4d0b6c82b29
using Orthanc::DicomMap instead of OrthancPlugins::DicomDatasetReader
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
113
diff
changeset
|
374 if (ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) |
32 | 375 { |
376 if (v.size() != 2 || | |
377 v[0] <= 0 || | |
378 v[1] <= 0) | |
379 { | |
380 LOG(ERROR) << "Bad value for PixelSpacing tag"; | |
381 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat); | |
382 } | |
383 else | |
384 { | |
385 spacingX = v[0]; | |
386 spacingY = v[1]; | |
387 } | |
388 } | |
389 else | |
390 { | |
391 // The "PixelSpacing" is of type 1C: It could be absent, use | |
392 // default value in such a case | |
393 spacingX = 1; | |
394 spacingY = 1; | |
395 } | |
396 } | |
140
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
397 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
398 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
399 Matrix CreateRotationMatrixAlongX(double a) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
400 { |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
401 // Rotate along X axis (R_x) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
402 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
403 Matrix r(3, 3); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
404 r(0,0) = 1; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
405 r(0,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
406 r(0,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
407 r(1,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
408 r(1,1) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
409 r(1,2) = -sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
410 r(2,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
411 r(2,1) = sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
412 r(2,2) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
413 return r; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
414 } |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
415 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
416 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
417 Matrix CreateRotationMatrixAlongY(double a) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
418 { |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
419 // Rotate along Y axis (R_y) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
420 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
421 Matrix r(3, 3); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
422 r(0,0) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
423 r(0,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
424 r(0,2) = sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
425 r(1,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
426 r(1,1) = 1; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
427 r(1,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
428 r(2,0) = -sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
429 r(2,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
430 r(2,2) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
431 return r; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
432 } |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
433 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
434 |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
435 Matrix CreateRotationMatrixAlongZ(double a) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
436 { |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
437 // Rotate along Z axis (R_z) |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
438 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
439 Matrix r(3, 3); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
440 r(0,0) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
441 r(0,1) = -sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
442 r(0,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
443 r(1,0) = sin(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
444 r(1,1) = cos(a); |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
445 r(1,2) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
446 r(2,0) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
447 r(2,1) = 0; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
448 r(2,2) = 1; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
449 return r; |
2115530d3703
OrientedBoundingBox
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
135
diff
changeset
|
450 } |
151
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
451 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
452 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
453 bool IntersectPlaneAndSegment(Vector& p, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
454 const Vector& normal, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
455 double d, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
456 const Vector& edgeFrom, |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
457 const Vector& edgeTo) |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
458 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
459 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
460 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
461 // Check for parallel line and plane |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
462 Vector direction = edgeTo - edgeFrom; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
463 double denominator = boost::numeric::ublas::inner_prod(direction, normal); |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
464 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
465 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
466 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
467 return false; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
468 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
469 else |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
470 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
471 // Compute intersection |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
472 double t = -(normal[0] * edgeFrom[0] + |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
473 normal[1] * edgeFrom[1] + |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
474 normal[2] * edgeFrom[2] + d) / denominator; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
475 |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
476 if (t >= 0 && t <= 1) |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
477 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
478 // The intersection lies inside edge segment |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
479 p = edgeFrom + t * direction; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
480 return true; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
481 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
482 else |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
483 { |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
484 return false; |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
485 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
486 } |
c5044bbfc303
CoordinateSystem3D::IntersectSegment()
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
150
diff
changeset
|
487 } |
156 | 488 |
489 | |
157
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
490 bool IntersectPlaneAndLine(Vector& p, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
491 const Vector& normal, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
492 double d, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
493 const Vector& origin, |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
494 const Vector& direction) |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
495 { |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
496 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
497 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
498 // Check for parallel line and plane |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
499 double denominator = boost::numeric::ublas::inner_prod(direction, normal); |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
500 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
501 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
502 { |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
503 return false; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
504 } |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
505 else |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
506 { |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
507 // Compute intersection |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
508 double t = -(normal[0] * origin[0] + |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
509 normal[1] * origin[1] + |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
510 normal[2] * origin[2] + d) / denominator; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
511 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
512 p = origin + t * direction; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
513 return true; |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
514 } |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
515 } |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
516 |
2309e8d86efe
IntersectPlaneAndLine
Sebastien Jodogne <s.jodogne@gmail.com>
parents:
156
diff
changeset
|
517 |
156 | 518 void FillMatrix(Matrix& target, |
519 size_t rows, | |
520 size_t columns, | |
521 const double values[]) | |
522 { | |
523 target.resize(rows, columns); | |
524 | |
525 size_t index = 0; | |
526 | |
527 for (size_t y = 0; y < rows; y++) | |
528 { | |
529 for (size_t x = 0; x < columns; x++, index++) | |
530 { | |
531 target(y, x) = values[index]; | |
532 } | |
533 } | |
534 } | |
535 | |
536 | |
537 void FillVector(Vector& target, | |
538 size_t size, | |
539 const double values[]) | |
540 { | |
541 target.resize(size); | |
542 | |
543 for (size_t i = 0; i < size; i++) | |
544 { | |
545 target[i] = values[i]; | |
546 } | |
547 } | |
548 | |
549 | |
550 void Convert(Matrix& target, | |
551 const Vector& source) | |
552 { | |
553 const size_t n = source.size(); | |
554 | |
555 target.resize(n, 1); | |
556 | |
557 for (size_t i = 0; i < n; i++) | |
558 { | |
559 target(i, 0) = source[i]; | |
560 } | |
561 } | |
0 | 562 } |
563 } |